Added A* pathfinding

1 files changed, 170 insertions, 1 deletions

@@ -1,8 +1,16 @@
 #include "behaviour_system.h"
+#include <queue>
+#include <vector>
+#include <utility>
 #include <random>
 #include "game.h"
 #include "character_system.h"
 #include "direction.h"
+#include "transform_system.h"
+
+bool pathfindingOptionsContains(PathfindingOptions options, PathfindingOptions value) {
+  return (static_cast<int>(options) & static_cast<int>(value)) != 0;
+}
 
 void BehaviourSystem::tick(double dt) {
   if (game_.isGameplayPaused()) return;
@@ -11,7 +19,7 @@ void BehaviourSystem::tick(double dt) {
   while (timer_.step()) {
     for (int spriteId : game_.getSprites()) {
       Sprite& sprite = game_.getSprite(spriteId);
-      if (sprite.wander && !sprite.paused) {
+      if (!sprite.paused && sprite.behaviourType == BehaviourType::Wander) {
         // 75% chance of changing what's happening
         if (std::bernoulli_distribution(0.75)(game_.getRng())) {
           // 50% chance of choosing a direction or stopping
@@ -31,4 +39,165 @@ void BehaviourSystem::tick(double dt) {
       }
     }
   }
+
+  for (int spriteId : game_.getSprites()) {
+    Sprite& sprite = game_.getSprite(spriteId);
+    if (!sprite.paused && sprite.behaviourType == BehaviourType::Path) {
+      while (!sprite.path.empty() && sprite.path.front().endpoint == sprite.loc) {
+        sprite.path.pop_front();
+      }
+      if (sprite.path.empty()) {
+        game_.getSystem<CharacterSystem>().stopDirecting(spriteId);
+      } else {
+        if (sprite.characterState == CharacterState::Still || sprite.movementDir != sprite.path.front().dir) {
+          game_.getSystem<CharacterSystem>().moveInDirection(spriteId, sprite.path.front().dir);
+        }
+      }
+    }
+  }
+}
+
+void BehaviourSystem::directSpriteToLocation(int spriteId, vec2i pos, PathfindingOptions options) {
+  Sprite& sprite = game_.getSprite(spriteId);
+  sprite.behaviourType = BehaviourType::Path;
+  sprite.pathfindingDestination = pos;
+  sprite.cardinalDirectionsOnly = pathfindingOptionsContains(options, PathfindingOptions::CardinalDirectionsOnly);
+
+  createPath(spriteId);
+}
+
+bool BehaviourSystem::isFollowingPath(int spriteId) {
+  Sprite& sprite = game_.getSprite(spriteId);
+  return sprite.behaviourType == BehaviourType::Path && !sprite.path.empty();
+}
+
+struct PathNodeInfo {
+  int cheapestPathCost = INT_MAX;
+  int estimatedRemainingCost = INT_MAX;
+  vec2i previousPoint;
+  Direction dirFromPreviousPoint;
+};
+
+struct SearchNode {
+  vec2i point;
+  int estimatedCost = INT_MAX;
+  int tiebreaker; // this actually counts downward. wow
+
+  SearchNode(vec2i point, int estimatedCost) : point(point), estimatedCost(estimatedCost) {
+    static int tiebreakerCounter = 0;
+    tiebreaker = tiebreakerCounter--;
+  }
+
+  bool operator>(const SearchNode& rhs) const {
+    return std::tie(estimatedCost, tiebreaker) > std::tie(rhs.estimatedCost, rhs.tiebreaker);
+  }
+};
+
+int 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;
+  }
+}
+
+void BehaviourSystem::createPath(int spriteId) {
+  Sprite& sprite = game_.getSprite(spriteId);
+  sprite.path.clear();
+
+  const Map& map = game_.getMap();
+  vec2i mapBounds = map.getMapSize() * map.getTileSize();
+
+  // If it is not possible to reach the destination because of the parity (if
+  // the movement speed is above 1), then adjust the destination.
+  if (sprite.movementSpeed > 1) {
+    if ((sprite.loc.x() % sprite.movementSpeed) != (sprite.pathfindingDestination.x() % sprite.movementSpeed)) {
+      sprite.loc.x() = (sprite.loc.x() / sprite.movementSpeed) * sprite.movementSpeed + (sprite.pathfindingDestination.x() % sprite.movementSpeed);
+    }
+    if ((sprite.loc.y() % sprite.movementSpeed) != (sprite.pathfindingDestination.y() % sprite.movementSpeed)) {
+      sprite.loc.y() = (sprite.loc.y() / sprite.movementSpeed) * sprite.movementSpeed + (sprite.pathfindingDestination.y() % sprite.movementSpeed);
+    }
+  }
+
+  int initialCostGuess = estimateRemainingCost(sprite.loc, sprite.pathfindingDestination, sprite.movementSpeed, sprite.cardinalDirectionsOnly);
+
+  std::map<vec2i, PathNodeInfo> pathNodes;
+  pathNodes[sprite.loc] = PathNodeInfo{.cheapestPathCost = 0, .estimatedRemainingCost = initialCostGuess};
+
+  std::priority_queue<SearchNode, std::vector<SearchNode>, std::greater<SearchNode>> openSet;
+  openSet.emplace(sprite.loc, initialCostGuess);
+
+  while (!openSet.empty()) {
+    SearchNode searchNode = openSet.top();
+    openSet.pop();
+
+    if (searchNode.point == sprite.pathfindingDestination) {
+      // We're there!
+      break;
+    }
+
+    PathNodeInfo& nodeInfo = pathNodes[searchNode.point];
+    int newCost = nodeInfo.cheapestPathCost + 1;
+
+    static const std::vector<Direction> allDirections = {
+      Direction::down,
+      Direction::down_left,
+      Direction::left,
+      Direction::up_left,
+      Direction::up,
+      Direction::up_right,
+      Direction::right,
+      Direction::down_right };
+
+    static const std::vector<Direction> cardinalDirections = {
+      Direction::down,
+      Direction::left,
+      Direction::up,
+      Direction::right };
+
+    const std::vector<Direction>* directionList = sprite.cardinalDirectionsOnly ? &cardinalDirections : &allDirections;
+    for (Direction dir : *directionList) {
+      vec2i newPos = searchNode.point + unitVecInDirection(dir) * sprite.movementSpeed;
+
+      if (newPos.x() < 0 || newPos.y() < 0 || newPos.x() >= mapBounds.w() || newPos.y() >= mapBounds.h()) {
+        // The path can't go outside the map.
+        continue;
+      }
+
+      PathNodeInfo& neighborInfo = pathNodes[newPos];
+
+      if (neighborInfo.cheapestPathCost <= newCost) {
+        // There is already a faster path through this neighbor.
+        continue;
+      }
+
+      CollisionResult collision = game_.getSystem<TransformSystem>().checkCollision(spriteId, searchNode.point, newPos, dir);
+      if (collision.horiz.blocked || collision.vert.blocked) {
+        // There isn't actually an edge to this neighbor.
+        continue;
+      }
+
+      neighborInfo.cheapestPathCost = newCost;
+      neighborInfo.estimatedRemainingCost = estimateRemainingCost(newPos, sprite.pathfindingDestination, sprite.movementSpeed, sprite.cardinalDirectionsOnly);
+      neighborInfo.previousPoint = searchNode.point;
+      neighborInfo.dirFromPreviousPoint = dir;
+
+      openSet.emplace(newPos, neighborInfo.cheapestPathCost + neighborInfo.estimatedRemainingCost);
+    }
+  }
+
+  if (!pathNodes.count(sprite.pathfindingDestination)) {
+    // There was no path to the destination.
+    return;
+  }
+
+  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});
+    }
+    curPos = nodeInfo.previousPoint;
+  }
 }