Core/Objects: Fix MovePositionToFirstCollision for non-flying units that aren't close to ground

Author: Shauren

src/server/game/Entities/Object/Object.cpp

@@ -3323,40 +3323,41 @@ void WorldObject::MovePosition(Position &pos, float dist, float angle)
 void WorldObject::MovePositionToFirstCollision(Position &pos, float dist, float angle)
 {
     angle += GetOrientation();
+    float cosAngle = std::cos(angle);
+    float sinAngle = std::sin(angle);
     float destx, desty, destz;
-    destx = pos.m_positionX + dist * std::cos(angle);
-    desty = pos.m_positionY + dist * std::sin(angle);
+    destx = pos.m_positionX + dist * cosAngle;
+    desty = pos.m_positionY + dist * sinAngle;
     destz = pos.m_positionZ;
 
     // Prevent invalid coordinates here, position is unchanged
     if (!Trinity::IsValidMapCoord(destx, desty))
     {
-        TC_LOG_FATAL("misc", "WorldObject::MovePositionToFirstCollision invalid coordinates X: {} and Y: {} were passed!", destx, desty);
+        TC_LOG_FATAL("misc", "WorldObject::MovePositionToFirstCollision invalid coordinates Src: {}, dist {}, angle {}, destX: {} destY: {} were passed!",
+            pos.ToString(), dist, angle, destx, desty);
         return;
     }
 
+    float halfHeight = GetCollisionHeight() * 0.5f;
+    bool col = false;
+
     // Use a detour raycast to get our first collision point
     PathGenerator path(this);
     path.SetUseRaycast(true);
     path.CalculatePath(destx, desty, destz, false);
 
     // Check for valid path types before we proceed
-    if (!(path.GetPathType() & PATHFIND_NOT_USING_PATH))
-        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;
-
-    // check static LOS
-    float halfHeight = GetCollisionHeight() * 0.5f;
-    bool col = false;
-
-    // Unit is flying, check for potential collision via vmaps
-    if (path.GetPathType() & PATHFIND_NOT_USING_PATH)
+    if (!(path.GetPathType() & (PATHFIND_NOPATH | PATHFIND_NOT_USING_PATH | PATHFIND_FARFROMPOLY_START)))
     {
+        G3D::Vector3 const& result = path.GetPath().back();
+        destx = result.x;
+        desty = result.y;
+        destz = result.z;
+    }
+    else
+    {
+        // check static LOS
+        // Unit is flying, check for potential collision via vmaps
         col = VMAP::VMapFactory::createOrGetVMapManager()->getObjectHitPos(GetMapId(),
             pos.m_positionX, pos.m_positionY, pos.m_positionZ + halfHeight,
             destx, desty, destz + halfHeight,
@@ -3367,9 +3368,8 @@ void WorldObject::MovePositionToFirstCollision(Position &pos, float dist, float
         // 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));
+            destx -= CONTACT_DISTANCE * cosAngle;
+            desty -= CONTACT_DISTANCE * sinAngle;
         }
     }
 
@@ -3384,18 +3384,16 @@ void WorldObject::MovePositionToFirstCollision(Position &pos, float dist, float
     // Collided with a gameobject, 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));
+        destx -= CONTACT_DISTANCE * cosAngle;
+        desty -= CONTACT_DISTANCE * sinAngle;
     }
 
     float groundZ = VMAP_INVALID_HEIGHT_VALUE;
     Trinity::NormalizeMapCoord(pos.m_positionX);
     Trinity::NormalizeMapCoord(pos.m_positionY);
     UpdateAllowedPositionZ(destx, desty, destz, &groundZ);
 
-    pos.SetOrientation(GetOrientation());
-    pos.Relocate(destx, desty, destz);
+    pos.Relocate(destx, desty, destz, GetOrientation());
 
     // position has no ground under it (or is too far away)
     if (groundZ <= INVALID_HEIGHT)