2 files changed, 39 insertions, 20 deletions

@@ -98,18 +98,19 @@ bool BehaviourSystem::isFollowingPath(int spriteId) {
 }
 
 struct PathNodeInfo {
-  int cheapestPathCost = INT_MAX;
-  int estimatedRemainingCost = INT_MAX;
+  double cheapestPathCost = DBL_MAX;
+  double estimatedRemainingCost = DBL_MAX;
   vec2i previousPoint;
   Direction dirFromPreviousPoint;
 };
 
 struct SearchNode {
   vec2i point;
-  int estimatedCost = INT_MAX;
+  Direction dirFromPreviousPoint;
+  double estimatedCost = DBL_MAX;
   int tiebreaker; // this actually counts downward. wow
 
-  SearchNode(vec2i point, int estimatedCost) : point(point), estimatedCost(estimatedCost) {
+  SearchNode(vec2i point, Direction dirFromPreviousPoint, double estimatedCost) : point(point), dirFromPreviousPoint(dirFromPreviousPoint), estimatedCost(estimatedCost) {
     static int tiebreakerCounter = 0;
     tiebreaker = tiebreakerCounter--;
   }
@@ -119,12 +120,14 @@ struct SearchNode {
   }
 };
 
-int estimateRemainingCost(vec2i current, vec2i dest, int movementSpeed, bool cardinalDirectionsOnly) {
+double estimateRemainingCost(vec2i current, vec2i dest, int movementSpeed, bool cardinalDirectionsOnly) {
   vec2i difference = dest - current;
   if (cardinalDirectionsOnly) {
     return (std::abs(difference.x()) + std::abs(difference.y())) / movementSpeed;
   } else {
-    return std::max(std::abs(difference.x()), std::abs(difference.y())) / movementSpeed;
+    int dx = std::abs(difference.x()) / movementSpeed;
+    int dy = std::abs(difference.y()) / movementSpeed;
+    return (dx + dy) + (1.4 - 2) * std::min(dx, dy);
   }
 }
 
@@ -146,13 +149,13 @@ void BehaviourSystem::createPath(int spriteId) {
     }
   }
 
-  int initialCostGuess = estimateRemainingCost(sprite.loc, sprite.pathfindingDestination, sprite.movementSpeed, sprite.cardinalDirectionsOnly);
+  double initialCostGuess = estimateRemainingCost(sprite.loc, sprite.pathfindingDestination, sprite.movementSpeed, sprite.cardinalDirectionsOnly);
 
-  std::map<vec2i, PathNodeInfo> pathNodes;
-  pathNodes[sprite.loc] = PathNodeInfo{.cheapestPathCost = 0, .estimatedRemainingCost = initialCostGuess};
+  std::map<vec2i, std::map<Direction, PathNodeInfo>> pathNodes;
+  pathNodes[sprite.loc][sprite.dir] = PathNodeInfo{.cheapestPathCost = 0, .estimatedRemainingCost = initialCostGuess};
 
   std::priority_queue<SearchNode, std::vector<SearchNode>, std::greater<SearchNode>> openSet;
-  openSet.emplace(sprite.loc, initialCostGuess);
+  openSet.emplace(sprite.loc, sprite.dir, initialCostGuess);
 
   while (!openSet.empty()) {
     SearchNode searchNode = openSet.top();
@@ -163,8 +166,7 @@ void BehaviourSystem::createPath(int spriteId) {
       break;
     }
 
-    PathNodeInfo& nodeInfo = pathNodes[searchNode.point];
-    int newCost = nodeInfo.cheapestPathCost + 1;
+    PathNodeInfo& nodeInfo = pathNodes[searchNode.point][searchNode.dirFromPreviousPoint];
 
     static const std::vector<Direction> allDirections = {
       Direction::down,
@@ -183,9 +185,17 @@ void BehaviourSystem::createPath(int spriteId) {
       Direction::right };
 
     std::vector<Direction> directionList = sprite.cardinalDirectionsOnly ? cardinalDirections : allDirections;
-    directionList.push_back(nodeInfo.dirFromPreviousPoint);
-
     for (Direction dir : directionList) {
+      double newCost = nodeInfo.cheapestPathCost;
+      if (dir != searchNode.dirFromPreviousPoint) {
+        newCost += 0.1;
+      }
+      if (isCardinalDirection(dir)) {
+        newCost += 1;
+      } else {
+        newCost += 1.4;
+      }
+
       vec2i newPos = searchNode.point + unitVecInDirection(dir) * sprite.movementSpeed;
 
       if (newPos.x() < 0 || newPos.y() < 0 || newPos.x() >= mapBounds.w() || newPos.y() >= mapBounds.h()) {
@@ -193,7 +203,7 @@ void BehaviourSystem::createPath(int spriteId) {
         continue;
       }
 
-      PathNodeInfo& neighborInfo = pathNodes[newPos];
+      PathNodeInfo& neighborInfo = pathNodes[newPos][dir];
 
       if (neighborInfo.cheapestPathCost <= newCost) {
         // There is already a faster path through this neighbor.
@@ -211,7 +221,7 @@ void BehaviourSystem::createPath(int spriteId) {
       neighborInfo.previousPoint = searchNode.point;
       neighborInfo.dirFromPreviousPoint = dir;
 
-      openSet.emplace(newPos, neighborInfo.cheapestPathCost + neighborInfo.estimatedRemainingCost);
+      openSet.emplace(newPos, dir, neighborInfo.cheapestPathCost + neighborInfo.estimatedRemainingCost);
     }
   }
 
@@ -222,10 +232,15 @@ void BehaviourSystem::createPath(int spriteId) {
 
   vec2i curPos = sprite.pathfindingDestination;
   while (curPos != sprite.loc) {
-    PathNodeInfo& nodeInfo = pathNodes[curPos];
-    if (sprite.path.empty() || sprite.path.front().dir != nodeInfo.dirFromPreviousPoint) {
-      sprite.path.push_front(PathfindingInstruction{.dir = nodeInfo.dirFromPreviousPoint, .endpoint = curPos});
+    const PathNodeInfo* nodeInfo = nullptr;
+    for (const auto& pni : pathNodes[curPos]) {
+      if (nodeInfo == nullptr || pni.second.cheapestPathCost < nodeInfo->cheapestPathCost) {
+        nodeInfo = &pni.second;
+      }
+    }
+    if (sprite.path.empty() || sprite.path.front().dir != nodeInfo->dirFromPreviousPoint) {
+      sprite.path.push_front(PathfindingInstruction{.dir = nodeInfo->dirFromPreviousPoint, .endpoint = curPos});
     }
-    curPos = nodeInfo.previousPoint;
+    curPos = nodeInfo->previousPoint;
   }
 }
@@ -120,4 +120,8 @@ inline Direction cardinalDirectionFacingPoint(vec2i point) {
   }
 }
 
+inline bool isCardinalDirection(Direction dir) {
+  return (dir == Direction::left || dir == Direction::right || dir == Direction::up || dir == Direction::down);
+}
+
 #endif /* end of include guard: DIRECTION_H_AB66A90E */