Core/Objects: further improvements for MovePositionToFirstCollision (#24523)

* Core/Objects: further improvements for MovePositionToFirstCollision

* the PathGenerator will now normalize incomplete destinations as well
* normalize destination positions before launching detour raycasts to get better direction data
* exclude unwanted pathfinding results from further use to avoid unintended behaivior

* Core/PathFinding: Split PATHFIND_FARFROMPOLY into PATHFIND_FARFROMPOLY_START and PATHFIND_FARFROMPOLY_END for start and end position

Handle PATHFIND_FARFROMPOLY_END as valid in MovePositionToFirstCollision

Co-authored-by: jackpoz <giacomopoz@gmail.com>

3 files changed, 66 insertions, 17 deletions

diff --git a/src/server/game/Entities/Object/Object.cpp b/src/server/game/Entities/Object/Object.cpp
index 706d7368fb7..352fb8554a5 100644
--- a/src/server/game/Entities/Object/Object.cpp
+++ b/src/server/game/Entities/Object/Object.cpp
@@ -3273,16 +3273,37 @@ void WorldObject::MovePositionToFirstCollision(Position &pos, float dist, float
     // Use a detour raycast to get our first collision point
     PathGenerator path(this);
     path.CalculatePath(destx, desty, destz, false, true);
+
+    // We have a invalid path result. Skip further processing.
+    if (path.GetPathType() & ~(PATHFIND_NORMAL | PATHFIND_SHORTCUT | PATHFIND_INCOMPLETE | PATHFIND_FARFROMPOLY_END))
+        return;
+
     G3D::Vector3 result = path.GetPath().back();
     destx = result.x;
     desty = result.y;
     destz = result.z;
 
-    UpdateAllowedPositionZ(destx, desty, destz);
+    // Object is using a shortcut. Check static LOS
+    float halfHeight = GetCollisionHeight() * 0.5f;
+    if (path.GetPathType() & PATHFIND_SHORTCUT)
+    {
+        bool col = VMAP::VMapFactory::createOrGetVMapManager()->getObjectHitPos(GetMapId(),
+            pos.m_positionX, pos.m_positionY, pos.m_positionZ + halfHeight,
+            destx, desty, destz + halfHeight,
+            destx, desty, destz, -0.5f);
+
+        destz -= halfHeight;
 
+        // Collided with static LOS object, move back to collision point
+        if (col)
+        {
+            destx -= CONTACT_DISTANCE * std::cos(angle);
+            desty -= CONTACT_DISTANCE * std::sin(angle);
+            dist = std::sqrt((pos.m_positionX - destx) * (pos.m_positionX - destx) + (pos.m_positionY - desty) * (pos.m_positionY - desty));
+        }
+    }
 
     // check dynamic collision
-    float halfHeight = GetCollisionHeight() * 0.5f;
     bool col = GetMap()->getObjectHitPos(GetPhaseMask(),
         pos.m_positionX, pos.m_positionY, pos.m_positionZ + halfHeight,
         destx, desty, destz + halfHeight,
diff --git a/src/server/game/Movement/PathGenerator.cpp b/src/server/game/Movement/PathGenerator.cpp
index e9460c33679..5402cb8e33a 100644
--- a/src/server/game/Movement/PathGenerator.cpp
+++ b/src/server/game/Movement/PathGenerator.cpp
@@ -199,8 +199,9 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
     }
 
     // we may need a better number here
-    bool farFromPoly = (distToStartPoly > 7.0f || distToEndPoly > 7.0f);
-    if (farFromPoly)
+    bool startFarFromPoly = distToStartPoly > 7.0f;
+    bool endFarFromPoly = distToEndPoly > 7.0f;
+    if (startFarFromPoly || endFarFromPoly)
     {
         TC_LOG_DEBUG("maps.mmaps", "++ BuildPolyPath :: farFromPoly distToStartPoly=%.3f distToEndPoly=%.3f", distToStartPoly, distToEndPoly);
 
@@ -230,7 +231,13 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
         if (buildShotrcut)
         {
             BuildShortcut();
-            _type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH | PATHFIND_FARFROMPOLY);
+            _type = PathType(PATHFIND_NORMAL | PATHFIND_NOT_USING_PATH);
+
+            if (startFarFromPoly)
+                _type = PathType(_type | PATHFIND_FARFROMPOLY_START);
+            if (endFarFromPoly)
+                _type = PathType(_type | PATHFIND_FARFROMPOLY_END);
+
             return;
         }
         else
@@ -243,7 +250,12 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
                 SetActualEndPosition(G3D::Vector3(endPoint[2], endPoint[0], endPoint[1]));
             }
 
-            _type = PathType(PATHFIND_INCOMPLETE | PATHFIND_FARFROMPOLY);
+            _type = PathType(PATHFIND_INCOMPLETE);
+
+            if (startFarFromPoly)
+                _type = PathType(_type | PATHFIND_FARFROMPOLY_START);
+            if (endFarFromPoly)
+                _type = PathType(_type | PATHFIND_FARFROMPOLY_END);
         }
     }
 
@@ -257,7 +269,17 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
         _pathPolyRefs[0] = startPoly;
         _polyLength = 1;
 
-        _type = farFromPoly ? PathType(PATHFIND_INCOMPLETE | PATHFIND_FARFROMPOLY) : PATHFIND_NORMAL;
+        if (startFarFromPoly || endFarFromPoly)
+        {
+            _type = PathType(PATHFIND_INCOMPLETE);
+
+            if (startFarFromPoly)
+                _type = PathType(_type | PATHFIND_FARFROMPOLY_START);
+            if (endFarFromPoly)
+                _type = PathType(_type | PATHFIND_FARFROMPOLY_END);
+        }
+        else
+         _type = PATHFIND_NORMAL;
 
         BuildPointPath(startPoint, endPoint);
         return;
@@ -382,6 +404,7 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
                 dtVlerp(hitPos, startPoint, endPoint, hit);
                 _pathPoints[1] = G3D::Vector3(hitPos[2], hitPos[0], hitPos[1]);
 
+                NormalizePath();
                 _type = PATHFIND_INCOMPLETE;
                 return;
             }
@@ -453,6 +476,7 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
                 dtVlerp(hitPos, startPoint, endPoint, hit);
                 _pathPoints[1] = G3D::Vector3(hitPos[2], hitPos[0], hitPos[1]);
 
+                NormalizePath();
                 _type = PATHFIND_INCOMPLETE;
                 return;
             }
@@ -488,8 +512,10 @@ void PathGenerator::BuildPolyPath(G3D::Vector3 const& startPos, G3D::Vector3 con
     else
         _type = PATHFIND_INCOMPLETE;
 
-    if (farFromPoly)
-        _type = PathType(_type | PATHFIND_FARFROMPOLY);
+    if (startFarFromPoly)
+        _type = PathType(_type | PATHFIND_FARFROMPOLY_START);
+    if (endFarFromPoly)
+        _type = PathType(_type | PATHFIND_FARFROMPOLY_END);
 
     // generate the point-path out of our up-to-date poly-path
     BuildPointPath(startPoint, endPoint);
diff --git a/src/server/game/Movement/PathGenerator.h b/src/server/game/Movement/PathGenerator.h
index ce82da1f1c0..d739dd19872 100644
--- a/src/server/game/Movement/PathGenerator.h
+++ b/src/server/game/Movement/PathGenerator.h
@@ -41,14 +41,16 @@ class WorldObject;
 
 enum PathType
 {
-    PATHFIND_BLANK          = 0x00,   // path not built yet
-    PATHFIND_NORMAL         = 0x01,   // normal path
-    PATHFIND_SHORTCUT       = 0x02,   // travel through obstacles, terrain, air, etc (old behavior)
-    PATHFIND_INCOMPLETE     = 0x04,   // we have partial path to follow - getting closer to target
-    PATHFIND_NOPATH         = 0x08,   // no valid path at all or error in generating one
-    PATHFIND_NOT_USING_PATH = 0x10,   // used when we are either flying/swiming or on map w/o mmaps
-    PATHFIND_SHORT          = 0x20,   // path is longer or equal to its limited path length
-    PATHFIND_FARFROMPOLY    = 0x40,   // start of end positions are far from the mmap poligon
+    PATHFIND_BLANK             = 0x00,   // path not built yet
+    PATHFIND_NORMAL            = 0x01,   // normal path
+    PATHFIND_SHORTCUT          = 0x02,   // travel through obstacles, terrain, air, etc (old behavior)
+    PATHFIND_INCOMPLETE        = 0x04,   // we have partial path to follow - getting closer to target
+    PATHFIND_NOPATH            = 0x08,   // no valid path at all or error in generating one
+    PATHFIND_NOT_USING_PATH    = 0x10,   // used when we are either flying/swiming or on map w/o mmaps
+    PATHFIND_SHORT             = 0x20,   // path is longer or equal to its limited path length
+    PATHFIND_FARFROMPOLY_START = 0x40,   // start position is far from the mmap poligon
+    PATHFIND_FARFROMPOLY_END   = 0x80,   // end positions is far from the mmap poligon
+    PATHFIND_FARFROMPOLY       = PATHFIND_FARFROMPOLY_START | PATHFIND_FARFROMPOLY_END, // start or end positions are far from the mmap poligon
 };
 
 class TC_GAME_API PathGenerator