Improve dynamic tree for movement

src/game/Maps/Map.cpp

@@ -3234,11 +3234,16 @@ bool Map::GetWalkHitPosition(GenericTransport* transport, float srcX, float srcY
         return false;
     }
 
+    if (t == 0)
+    {
+        return false;
+    }
+
     // We hit a wall - calculate new endposition
     if ((t < 1) && (t > 0))
     {
         for (int i = 0; i < 3; ++i)
-            endPosition[i] = point[i] + (endPosition[i] - point[i]) * hitNormal[i];
+            endPosition[i] = point[i] + (endPosition[i] - point[i]) * t;
     }
 
     if (dtStatusFailed(navMeshQuery->closestPointOnPoly(visited[visitedCount - 1], endPosition, endPosition, nullptr)))

src/game/Maps/PathFinder.cpp

@@ -1109,12 +1109,20 @@ void PathInfo::CutPathWithDynamicLoS()
     Vector3 out;
     // We have always keep at least 2 points (else, there is no mvt !)
     for (uint32 i = 1; i <= maxIndex; ++i)
-        if (m_sourceUnit->GetMap()->GetDynamicObjectHitPos(m_pathPoints[i - 1], m_pathPoints[i], out, -0.1f))
+    {
+        Vector3 start = m_pathPoints[i - 1];
+        Vector3 end = m_pathPoints[i];
+        start.z += 1.0f;
+        end.z += 1.0f;
+
+        if (m_sourceUnit->GetMap()->GetDynamicObjectHitPos(start, end, out, -0.1f))
         {
+            out.z -= 1.0f;
             m_pathPoints[i] = out;
             m_pathPoints.resize(i + 1);
             break;
         }
+    }
 }
 
 float PathInfo::Length() const

src/game/Spells/Spell.cpp

@@ -3217,22 +3217,8 @@ void Spell::SetTargetMap(SpellEffectIndex effIndex, uint32 targetMode, UnitList&
                 }
             }
 
-            GameObject const* const pDoor = m_caster->FindNearbyClosedDoor(dist);
-            bool const directionThroughDoor = pDoor ? pDoor->HasInArc(M_PI_F, src.x, src.y) != pDoor->HasInArc(M_PI_F, dest.x, dest.y) : false;
-
             if (m_caster->GetMap()->GetWalkHitPosition(m_caster->GetTransport(), src.x, src.y, src.z, dest.x, dest.y, dest.z, NAV_GROUND | NAV_WATER, 20.0f, false))
             {
-                // move back so we dont clip into a door
-                if (pDoor)
-                {
-                    if (directionThroughDoor)
-                        Geometry::Move2dPointTowards(src, dest, 3.0f);
-
-                    if (pDoor->HasInArc(M_PI_F, src.x, src.y) != pDoor->HasInArc(M_PI_F, dest.x, dest.y) ||
-                       !m_caster->IsWithinLOS(dest.x, dest.y, dest.z, true, 0.1f))
-                        dest = src;
-                }
-
                 // should never go backwards or sideways
                 if (!m_caster->HasInArc(M_PI_F / 2.0f, dest.x, dest.y))
                     dest = src;
@@ -3244,6 +3230,7 @@ void Spell::SetTargetMap(SpellEffectIndex effIndex, uint32 targetMode, UnitList&
             }
 
             m_targets.setDestination(dest.x, dest.y, dest.z);
+            break;
         }
         default:
             //sLog.Out(LOG_BASIC, LOG_LVL_ERROR, "SPELL: Unknown implicit target (%u) for spell ID %u", targetMode, m_spellInfo->Id);

src/game/vmap/BIH.h

@@ -141,7 +141,7 @@ class BIH
                         intervalMax = t2;
                     // intervalMax can only become smaller for other axis,
                     //  and intervalMin only larger respectively, so stop early
-                    if (intervalMax <= 0 || intervalMin >= maxDist)
+                    if (intervalMax < 0 || intervalMin >= maxDist)
                         return;
                 }
             }

src/game/vmap/BIHWrap.h

@@ -98,11 +98,11 @@ class BIHWrap
     }
 
     template<typename RayCallback>
-    void intersectRay(const Ray& r, RayCallback& intersectCallback, float& maxDist, bool ignoreM2Model)
+    void intersectRay(const Ray& r, RayCallback& intersectCallback, float& maxDist, bool stopAtFirst, bool ignoreM2Model)
     {
         balance();
         MDLCallback<RayCallback> temp_cb(intersectCallback, m_objects.getCArray(), m_objects.size());
-        m_tree.intersectRay(r, temp_cb, maxDist, true, ignoreM2Model);
+        m_tree.intersectRay(r, temp_cb, maxDist, stopAtFirst, ignoreM2Model);
     }
 
     template<typename IsectCallback>

src/game/vmap/DynamicTree.cpp

@@ -155,10 +155,13 @@ struct DynamicTreeIntersectionCallback
 {
     bool did_hit;
     DynamicTreeIntersectionCallback() : did_hit(false) {}
+    // Returns true if traversal shoul stop
     bool operator()(const G3D::Ray& r, GameObjectModel const& obj, float& distance, bool stopAtFirst, bool ignoreM2Model)
     {
-        did_hit = obj.intersectRay(r, distance, stopAtFirst, ignoreM2Model);
-        return did_hit;
+        const bool hit = obj.intersectRay(r, distance, stopAtFirst, ignoreM2Model);
+        if (hit)
+            did_hit = true;
+        return stopAtFirst && hit;
     }
     bool didHit() const { return did_hit; }
 };
@@ -208,11 +211,11 @@ If intersection is found within pMaxDist, sets pMaxDist to intersection distance
 Else, pMaxDist is not modified and returns false;
 */
 
-bool DynamicMapTree::getIntersectionTime(G3D::Ray const& ray, Vector3 const& endPos, float& pMaxDist) const
+bool DynamicMapTree::getIntersectionTime(G3D::Ray const& ray, Vector3 const& endPos, float& pMaxDist, bool stopAtFirst) const
 {
     float distance = pMaxDist;
     DynamicTreeIntersectionCallback callback;
-    impl.intersectRay(ray, callback, distance, endPos, false);
+    impl.intersectRay(ray, callback, distance, endPos, stopAtFirst, false); 
     if (callback.didHit())
         pMaxDist = distance;
     return callback.didHit();
@@ -251,7 +254,7 @@ bool DynamicMapTree::getObjectHitPos(Vector3 const& pPos1, Vector3 const& pPos2,
     Vector3 dir = (pPos2 - pPos1) / maxDist;            // direction with length of 1
     G3D::Ray ray(pPos1, dir);
     float dist = maxDist;
-    if (getIntersectionTime(ray, pPos2, dist))
+    if (getIntersectionTime(ray, pPos2, dist, false /*stopAtFirsthit*/)) 
     {
         pResultHitPos = pPos1 + dir * dist;
         if (pModifyDist < 0)
@@ -285,7 +288,7 @@ bool DynamicMapTree::isInLineOfSight(float x1, float y1, float z1, float x2, flo
 
     G3D::Ray r(v1, (v2 - v1) / maxDist);
     DynamicTreeIntersectionCallback callback;
-    impl.intersectRay(r, callback, maxDist, v2, ignoreM2Model);
+    impl.intersectRay(r, callback, maxDist, v2, true /*stopAtFirst*/, ignoreM2Model);
 
     return !callback.did_hit;
 }
@@ -295,7 +298,7 @@ float DynamicMapTree::getHeight(float x, float y, float z, float maxSearchDist)
     Vector3 v(x, y, z);
     G3D::Ray r(v, -Vector3::unitZ()); // down
     DynamicTreeIntersectionCallback callback;
-    impl.intersectZAllignedRay(r, callback, maxSearchDist);
+    impl.intersectZAllignedRay(r, callback, maxSearchDist, true /*stopatfirst*/); // TODO: stopatfirst should probably be false - need testing
 
     if (callback.didHit())
         return v.z - maxSearchDist;
@@ -309,7 +312,7 @@ GameObjectModel const* DynamicMapTree::getObjectHit(Vector3 const& pPos1, Vector
     G3D::Ray const ray(pPos1, dir);
 
     DynamicTreeIntersectionCallback_findCollisionObject callback;
-    impl.intersectRay(ray, callback, distance, pPos2, false);
+    impl.intersectRay(ray, callback, distance, pPos2, true /*stopAtFirst*/, false /*ignoreM2Model*/);
     if (callback.hitObj)
         return callback.hitObj;
     return nullptr;

src/game/vmap/DynamicTree.h

@@ -35,7 +35,7 @@ class DynamicMapTree
         ~DynamicMapTree();
 
         bool isInLineOfSight(float x1, float y1, float z1, float x2, float y2, float z2, bool ignoreM2Model) const;
-        bool getIntersectionTime(G3D::Ray const& ray, G3D::Vector3 const& endPos, float& maxDist) const;
+        bool getIntersectionTime(G3D::Ray const& ray, G3D::Vector3 const& endPos, float& maxDist, bool stopAtFirst) const;
         bool getObjectHitPos(G3D::Vector3 const& pPos1, G3D::Vector3 const& pPos2, G3D::Vector3& pResultHitPos, float pModifyDist) const;
         bool getObjectHitPos(float x1, float y1, float z1, float x2, float y2, float z2, float& rx, float& ry, float& rz, float pModifyDist) const;
         GameObjectModel const* getObjectHit(G3D::Vector3 const& pPos1, G3D::Vector3 const& pPos2) const;

src/game/vmap/RegularGrid.h

@@ -152,13 +152,13 @@ class RegularGrid2D
         }
 
         template<typename RayCallback>
-        void intersectRay(Ray const& ray, RayCallback& intersectCallback, float max_dist, bool ignoreM2Model)
+        void intersectRay(Ray const& ray, RayCallback& intersectCallback, float max_dist, bool stopAtfirst, bool ignoreM2Model)
         {
-            intersectRay(ray, intersectCallback, max_dist, ray.origin() + ray.direction() * max_dist, ignoreM2Model);
+            intersectRay(ray, intersectCallback, max_dist, ray.origin() + ray.direction() * max_dist, stopAtfirst, ignoreM2Model);
         }
 
         template<typename RayCallback>
-        void intersectRay(Ray const& ray, RayCallback& intersectCallback, float& max_dist, Vector3 const& end, bool ignoreM2Model)
+        void intersectRay(Ray const& ray, RayCallback& intersectCallback, float& max_dist, Vector3 const& end, bool stopAtFirst, bool ignoreM2Model)
         {
             Cell cell = Cell::ComputeCell(ray.origin().x, ray.origin().y);
             if (!cell.isValid())
@@ -169,7 +169,7 @@ class RegularGrid2D
             if (cell == last_cell)
             {
                 if (Node* node = nodes[cell.x][cell.y])
-                    node->intersectRay(ray, intersectCallback, max_dist, ignoreM2Model);
+                    node->intersectRay(ray, intersectCallback, max_dist, stopAtFirst, ignoreM2Model);
                 return;
             }
 
@@ -214,8 +214,10 @@ class RegularGrid2D
             {
                 if (Node* node = nodes[cell.x][cell.y])
                 {
-                    //float enterdist = max_dist;
-                    node->intersectRay(ray, intersectCallback, max_dist, ignoreM2Model);
+                    float const enter_dist = max_dist;
+                    node->intersectRay(ray, intersectCallback, max_dist, stopAtFirst, ignoreM2Model);
+                    if (stopAtFirst && max_dist < enter_dist)
+                        return;
                 }
                 if (cell == last_cell)
                     break;
@@ -246,13 +248,13 @@ class RegularGrid2D
 
         // Optimized verson of intersectRay function for rays with vertical directions
         template<typename RayCallback>
-        void intersectZAllignedRay(Ray const& ray, RayCallback& intersectCallback, float& max_dist)
+        void intersectZAllignedRay(Ray const& ray, RayCallback& intersectCallback, float& max_dist, bool stopAtfirst)
         {
             Cell cell = Cell::ComputeCell(ray.origin().x, ray.origin().y);
             if (!cell.isValid())
                 return;
             if (Node* node = nodes[cell.x][cell.y])
-                node->intersectRay(ray, intersectCallback, max_dist, false);
+                node->intersectRay(ray, intersectCallback, max_dist, stopAtfirst, false);
         }
 };