Ticket #2725: AtGoal.patch

source/simulation2/components/CCmpPathfinder_Tile.cpp

@@ -32 +32 @@
 
 typedef PriorityQueueHeap<std::pair<u16, u16>, u32> PriorityQueue;
 
-#define PATHFIND_STATS 0
+#define PATHFIND_STATS 1
+#if PATHFIND_STATS
+#include "lib/timer.h"
+TIMER_ADD_CLIENT(tc_AtGoal);
+#endif
 
 #define USE_DIAGONAL_MOVEMENT 1
 
@@ -215 +219 @@
 #endif
 };
 
-static bool AtGoal(u16 i, u16 j, const ICmpPathfinder::Goal& goal)
+static bool AtGoal(u16 i, u16 j, const ICmpPathfinder::Goal& goal, const u32 h)
 {
+#if PATHFIND_STATS
+    TIMER_ACCRUE(tc_AtGoal); 
+#endif
     // Allow tiles slightly more than sqrt(2) from the actual goal,
     // i.e. adjacent diagonally to the target tile
-    fixed tolerance = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*3/2);
+    const fixed tolerance = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*3/2);
 
+    entity_pos_t margin = tolerance;
+
+    if (goal.type == ICmpPathfinder::Goal::SQUARE)
+        // diagonal line is always shorter than max(height, width) * 1.5 
+        margin += (goal.hh > goal.hw ? goal.hh : goal.hw) * 3  / entity_pos_t::FromInt(2 * TERRAIN_TILE_SIZE);
+    
+    if (h > g_CostPerTile * margin.ToFloat())
+        return false;
+
     entity_pos_t x, z;
     CCmpPathfinder::TileCenter(i, j, x, z);
     fixed dist = CCmpPathfinder::DistanceToGoal(CFixedVector2D(x, z), goal);
@@ -377 +393 @@
     NearestTile(goal.x, goal.z, state.iGoal, state.jGoal);
 
     // If we're already at the goal tile, then move directly to the exact goal coordinates
-    if (AtGoal(i0, j0, goal))
+    if (AtGoal(i0, j0, goal, 0))
     {
         Waypoint w = { goal.x, goal.z };
         path.m_Waypoints.push_back(w);
@@ -438 +454 @@
         state.tiles->get(i, j).SetStatusClosed();
 
         // If we've reached the destination, stop
-        if (AtGoal(i, j, goal))
+        if (AtGoal(i, j, goal, state.tiles->get(i, j).h))
         {
             state.iBest = i;
             state.jBest = j;