Ticket #4327: pathfinderFix.patch

source/simulation2/components/CCmpPathfinder_Vertex.cpp

@@ -592 +592 @@
     entity_pos_t pathfindClearance = clearance;
 
     // Convert each obstruction square into collision edges and search graph vertexes
+    // We also expand our range if vertices fall outside of it, so that all required terrain edges are added.
     for (size_t i = 0; i < squares.size(); ++i)
     {
         CFixedVector2D center(squares[i].x, squares[i].z);

source/simulation2/components/CCmpUnitMotion.cpp

@@ -217 +217 @@
          */
         PATHSTATE_FOLLOWING_REQUESTING_SHORT,
 
+        /*
+         * We ran out of waypoints and were not really close to our final destination
+         * There is a possibility the short-range pathfinder will find a better path
+         * since the long-range rasterization is less permissive
+         * So we'll request one final short path. If it fails, we fail.
+         */
+        PATHSTATE_ARRIVED_TRYING_SHORT,
+
         PATHSTATE_MAX
     };
     u8 m_PathState;
@@ -639 +647 @@
     void UpdateFinalGoal();
 
     /**
-     * Returns whether we are close enough to the target to assume it's a good enough
-     * position to stop.
+     * Returns if we are close enough to safely assume we've arrived.
+     */
+    bool IsAtDestination(const CFixedVector2D& from);
+
+    /**
+     * If we are close enough to stop, succeed the move.
      */
     bool ShouldConsiderOurselvesAtDestination(const CFixedVector2D& from);
 
@@ -741 +753 @@
 
         m_Moving = true;
     }
-    else if (m_PathState == PATHSTATE_WAITING_REQUESTING_SHORT || m_PathState == PATHSTATE_FOLLOWING_REQUESTING_SHORT)
+    else if (m_PathState == PATHSTATE_WAITING_REQUESTING_SHORT || m_PathState == PATHSTATE_FOLLOWING_REQUESTING_SHORT
+             || m_PathState == PATHSTATE_ARRIVED_TRYING_SHORT)
     {
         m_ShortPath = path;
-
         // If there's no waypoints then we couldn't get near the target
         if (m_ShortPath.m_Waypoints.empty())
         {
+            CmpPtr<ICmpPosition> cmpPosition(GetEntityHandle());
+
+            if (!cmpPosition || !cmpPosition->IsInWorld())
+                return;
+
+            CFixedVector2D pos = cmpPosition->GetPosition2D();
+
+            // we failed to improve on our path
+            if (m_PathState == PATHSTATE_ARRIVED_TRYING_SHORT)
+            {
+                StopMoving();
+                m_State = STATE_IDLE;
+                // two options: if we were just moving there, succeed: we got as close as we could.
+                // if we were targeting an entity, only succeed if we're in range
+                if (!m_TargetEntity || IsInTargetRange(m_TargetEntity, m_TargetMinRange, m_TargetMaxRange))
+                {
+                    MoveSucceeded();
+
+                    if (m_FacePointAfterMove)
+                        FaceTowardsPointFromPos(pos, m_FinalGoal.x, m_FinalGoal.z);
+                    return;
+                }
+                // we actually failed.
+                MoveFailed();
+                return;
+            }
+
             // If we're globally following a long path, try to remove the next waypoint, it might be obstructed
             // If not, and we are not in a formation, retry
             // unless we are close to our target and we don't have a target entity.
@@ -765 +804 @@
             CMessageMotionChanged msg(false, false);
             GetSimContext().GetComponentManager().PostMessage(GetEntityId(), msg);
 
-            CmpPtr<ICmpPosition> cmpPosition(GetEntityHandle());
-            if (!cmpPosition || !cmpPosition->IsInWorld())
-                return;
-
-            CFixedVector2D pos = cmpPosition->GetPosition2D();
-
             if (ShouldConsiderOurselvesAtDestination(pos))
                 return;
 
@@ -1011 +1044 @@
 
         if (m_PathState == PATHSTATE_FOLLOWING)
         {
-            // If we're not currently computing any new paths:
-            if (m_LongPath.m_Waypoints.empty() && m_ShortPath.m_Waypoints.empty())
-            {
-                if (IsFormationMember())
-                {
-                    // We've reached our assigned position. If the controller
-                    // is idle, send a notification in case it should disband,
-                    // otherwise continue following the formation next turn.
-                    CmpPtr<ICmpUnitMotion> cmpUnitMotion(GetSimContext(), m_TargetEntity);
-                    if (cmpUnitMotion && !cmpUnitMotion->IsMoving())
-                    {
-                        CmpPtr<ICmpObstruction> cmpObstruction(GetEntityHandle());
-                        if (cmpObstruction)
-                            cmpObstruction->SetMovingFlag(false);
-
-                        m_Moving = false;
-                        CMessageMotionChanged msg(false, false);
-                        GetSimContext().GetComponentManager().PostMessage(GetEntityId(), msg);
-                    }
-                }
-                else
-                {
-                    // check if target was reached in case of a moving target
-                    CmpPtr<ICmpUnitMotion> cmpUnitMotion(GetSimContext(), m_TargetEntity);
-                    if (cmpUnitMotion && cmpUnitMotion->IsMoving() &&
-                        MoveToTargetRange(m_TargetEntity, m_TargetMinRange, m_TargetMaxRange))
-                        return;
-
-                    // Not in formation, so just finish moving
-                    StopMoving();
-                    m_State = STATE_IDLE;
-                    MoveSucceeded();
-
-                    if (m_FacePointAfterMove)
-                        FaceTowardsPointFromPos(pos, m_FinalGoal.x, m_FinalGoal.z);
-                    // TODO: if the goal was a square building, we ought to point towards the
-                    // nearest point on the square, not towards its center
-                }
-            }
-
             // If we have a target entity, and we're not miles away from the end of
             // our current path, and the target moved enough, then recompute our
             // whole path
+            bool followingTarget = false;
             if (IsFormationMember())
-                CheckTargetMovement(pos, CHECK_TARGET_MOVEMENT_MIN_DELTA_FORMATION);
+                followingTarget = CheckTargetMovement(pos, CHECK_TARGET_MOVEMENT_MIN_DELTA_FORMATION);
             else
-                CheckTargetMovement(pos, CHECK_TARGET_MOVEMENT_MIN_DELTA);
+                followingTarget = CheckTargetMovement(pos, CHECK_TARGET_MOVEMENT_MIN_DELTA);
+
+            if (followingTarget)
+                return;
+
+            // If we are still going to compute a path, return.
+            if (!m_LongPath.m_Waypoints.empty() || !m_ShortPath.m_Waypoints.empty())
+             return;
+
+            // if we dont believe we're actually arrived, presumably the long-range pathfinder's path was a little bad
+            // probably because it over-rasterized. Retry a new long path, which will pop us in recovery mode if necessary.
+            if (!ShouldConsiderOurselvesAtDestination(pos))
+            {
+                m_PathState = PATHSTATE_ARRIVED_TRYING_SHORT;
+                RequestShortPath(pos, m_FinalGoal, true);
+                return;
+            }
         }
     }
     }
@@ -1213 +1223 @@
     m_FinalGoal.z = targetPos.Y;
 }
 
+bool CCmpUnitMotion::IsAtDestination(const CFixedVector2D& from)
+{
+    return ((m_TargetEntity != INVALID_ENTITY && IsInTargetRange(m_TargetEntity, m_TargetMinRange, m_TargetMaxRange))
+            || m_FinalGoal.DistanceToPoint(from) <= SHORT_PATH_GOAL_RADIUS);
+}
+
 bool CCmpUnitMotion::ShouldConsiderOurselvesAtDestination(const CFixedVector2D& from)
 {
-    if (m_TargetEntity != INVALID_ENTITY || m_FinalGoal.DistanceToPoint(from) > SHORT_PATH_GOAL_RADIUS)
+    if (!IsAtDestination(from))
         return false;
 
-    StopMoving();
-    MoveSucceeded();
+    if (IsFormationMember())
+    {
+        // We've reached our assigned position. If the controller
+        // is idle, send a notification in case it should disband,
+        // otherwise continue following the formation next turn.
+        CmpPtr<ICmpUnitMotion> cmpUnitMotion(GetSimContext(), m_TargetEntity);
+        if (cmpUnitMotion && !cmpUnitMotion->IsMoving())
+        {
+            CmpPtr<ICmpObstruction> cmpObstruction(GetEntityHandle());
+            if (cmpObstruction)
+                cmpObstruction->SetMovingFlag(false);
+
+            m_Moving = false;
+            CMessageMotionChanged msg(false, false);
+            GetSimContext().GetComponentManager().PostMessage(GetEntityId(), msg);
+        }
+    }
+    else
+    {
+        // check if target was reached in case of a moving target
+        CmpPtr<ICmpUnitMotion> cmpUnitMotion(GetSimContext(), m_TargetEntity);
+        if (cmpUnitMotion && cmpUnitMotion->IsMoving() &&
+            MoveToTargetRange(m_TargetEntity, m_TargetMinRange, m_TargetMaxRange))
+            return true;
 
-    if (m_FacePointAfterMove)
-        FaceTowardsPointFromPos(from, m_FinalGoal.x, m_FinalGoal.z);
+        // Not in formation, so just finish moving
+        StopMoving();
+        m_State = STATE_IDLE;
+        MoveSucceeded();
+
+        if (m_FacePointAfterMove)
+            FaceTowardsPointFromPos(from, m_FinalGoal.x, m_FinalGoal.z);
+        // TODO: if the goal was a square building, we ought to point towards the
+        // nearest point on the square, not towards its center
+    }
     return true;
 }