Fixed pathfinder.

1 files changed, 30 insertions, 6 deletions

diff --git a/src/map.cpp b/src/map.cpp
index cbebc41c..5063a754 100644
--- a/src/map.cpp
+++ b/src/map.cpp
@@ -328,6 +328,7 @@ Map::getWalk(int x, int y)
         return false;
     }
 
+    /*
     // Check for collision with a being
     Beings *beings = beingManager->getAll();
     for (BeingIterator i = beings->begin(); i != beings->end(); i++) {
@@ -336,6 +337,7 @@ Map::getWalk(int x, int y)
             return false;
         }
     }
+    */
 
     return true;
 }
@@ -383,6 +385,8 @@ Map::removeSprite(SpriteIterator iterator)
     mSprites.erase(iterator);
 }
 
+static int const basicCost = 100;
+
 Path
 Map::findPath(int startX, int startY, int destX, int destY)
 {
@@ -460,13 +464,27 @@ Map::findPath(int startX, int startY, int destX, int destY)
                     }
                 }
 
-                // Calculate G cost for this route, 10 for moving straight and
-                // 14 for moving diagonal
-                int Gcost = curr.tile->Gcost + ((dx == 0 || dy == 0) ? 10 : 14);
+                // Calculate G cost for this route, ~sqrt(2) for moving diagonal
+                int Gcost = curr.tile->Gcost +
+                    (dx == 0 || dy == 0 ? basicCost : basicCost * 362 / 256);
+
+                /* Demote an arbitrary direction to speed pathfinding by
+                   adding a defect (TODO: change depending on the desired
+                   visual effect, e.g. a cross-product defect toward
+                   destination).
+                   Important: as long as the total defect along any path is
+                   less than the basicCost, the pathfinder will still find one
+                   of the shortest paths! */
+                if (dx == 0 || dy == 0)
+                {
+                    // Demote horizontal and vertical directions, so that two
+                    // consecutive directions cannot have the same Fcost.
+                    ++Gcost;
+                }
 
                 // Skip if Gcost becomes too much
                 // Warning: probably not entirely accurate
-                if (Gcost > 200)
+                if (Gcost > 20 * basicCost)
                 {
                     continue;
                 }
@@ -474,8 +492,14 @@ Map::findPath(int startX, int startY, int destX, int destY)
                 if (newTile->whichList != mOnOpenList)
                 {
                     // Found a new tile (not on open nor on closed list)
-                    // Update Hcost of the new tile using Manhatten distance
-                    newTile->Hcost = 10 * (abs(x - destX) + abs(y - destY));
+
+                    /* Update Hcost of the new tile. The pathfinder does not
+                       work reliably if the heuristic cost is higher than the
+                       real cost. In particular, using Manhattan distance is
+                       forbidden here. */
+                    int dx = std::abs(x - destX), dy = std::abs(y - destY);
+                    newTile->Hcost = std::abs(dx - dy) * basicCost +
+                        std::min(dx, dy) * (basicCost * 362 / 256);
 
                     // Set the current tile as the parent of the new tile
                     newTile->parentX = curr.x;