Added A* pathfinding

10 files changed, 221 insertions, 9 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;
+  }
 }
@@ -3,6 +3,12 @@
 
 #include "system.h"
 #include "timer.h"
+#include "vector.h"
+
+enum class PathfindingOptions {
+  None                    = 0,
+  CardinalDirectionsOnly  = 1 << 0
+};
 
 class Game;
 
@@ -15,8 +21,14 @@ public:
 
   void tick(double dt) override;
 
+  void directSpriteToLocation(int spriteId, vec2i pos, PathfindingOptions options = PathfindingOptions::None);
+
+  bool isFollowingPath(int spriteId);
+
 private:
 
+  void createPath(int spriteId);
+
   Game& game_;
   Timer timer_ { 500 };
 };
@@ -150,7 +150,7 @@ void CharacterSystem::tick(double dt) {
         pLoc += (unitVecInDirection(sprite.movementDir) * speed);
 
         // Check collision.
-        CollisionResult collision = game_.getSystem<TransformSystem>().checkCollision(spriteId, pLoc, sprite.movementDir);
+        CollisionResult collision = game_.getSystem<TransformSystem>().checkCollision(spriteId, sprite.loc, pLoc, sprite.movementDir);
         bool blocked = collision.horiz.blocked || collision.vert.blocked;
 
         if (collision.horiz.blocked && !sprite.clipping) {
@@ -69,7 +69,7 @@ void Game::loadMap(std::string filename) {
     if (p.movementSpeed != -1) {
       getSystem<CharacterSystem>().initSprite(spriteId, p.movementSpeed);
       if (p.wander) {
-        getSprite(spriteId).wander = true;
+        getSprite(spriteId).behaviourType = BehaviourType::Wander;
       }
     }
     if (!p.enclosureZone.empty()) {
@@ -85,7 +85,7 @@ void InputSystem::tick(double dt) {
             if (sprite.controllable) {
               // Interacting with objects always uses Lucas's movement speed.
               vec2i checkLoc = sprite.loc + (unitVecInDirection(sprite.dir) * LUCAS_MOVEMENT_SPEED);
-              CollisionResult collision = game_.getSystem<TransformSystem>().checkCollision(spriteId, checkLoc, sprite.dir);
+              CollisionResult collision = game_.getSystem<TransformSystem>().checkCollision(spriteId, sprite.loc, checkLoc, sprite.dir);
 
               if (collision.horiz.blocked || collision.vert.blocked) {
                 inFrontOfSomething = true;
@@ -7,6 +7,7 @@
 #include "transform_system.h"
 #include "effect_system.h"
 #include "camera_system.h"
+#include "behaviour_system.h"
 #include "vector.h"
 
 ScriptSystem::ScriptSystem(Game& game) : game_(game) {
@@ -42,7 +43,8 @@ ScriptSystem::ScriptSystem(Game& game) : game_(game) {
     "clipping", &Sprite::clipping,
     "cantCrouch", &Sprite::cantCrouch,
     "bobsWhenNormal", &Sprite::bobsWhenNormal,
-    "animSlowdown", &Sprite::animSlowdown);
+    "animSlowdown", &Sprite::animSlowdown,
+    "enclosureZone", &Sprite::enclosureZone);
 
   engine_.new_usertype<MessageSystem>(
     "message",
@@ -93,6 +95,11 @@ ScriptSystem::ScriptSystem(Game& game) : game_(game) {
     "unlockCamera", &CameraSystem::unlockCamera,
     "setFollowingSprite", &CameraSystem::setFollowingSprite);
 
+  engine_.new_usertype<BehaviourSystem>(
+    "behaviour",
+    "directSpriteToLocation", &BehaviourSystem::directSpriteToLocation,
+    "isFollowingPath", &BehaviourSystem::isFollowingPath);
+
   engine_.new_usertype<Mixer>(
     "mixer",
     "playSound", &Mixer::playSound,
@@ -146,6 +153,12 @@ ScriptSystem::ScriptSystem(Game& game) : game_(game) {
     });
 
   engine_.set_function(
+    "behaviour",
+    [&] () -> BehaviourSystem& {
+      return game_.getSystem<BehaviourSystem>();
+    });
+
+  engine_.set_function(
     "mixer",
     [&] () -> Mixer& {
       return game_.getMixer();
@@ -40,12 +40,23 @@ enum class CharacterMedium {
   Water
 };
 
+enum class BehaviourType {
+  None,
+  Wander,
+  Path
+};
+
 struct Movement {
   vec2i pos;
   Direction dir;
   CharacterMedium medium;
 };
 
+struct PathfindingInstruction {
+  Direction dir;
+  vec2i endpoint;
+};
+
 class Sprite {
 public:
 
@@ -106,7 +117,10 @@ public:
   bool player = false;
 
   // Behaviour
-  bool wander = false;
+  BehaviourType behaviourType = BehaviourType::None;
+  vec2i pathfindingDestination;
+  bool cardinalDirectionsOnly = false;
+  std::deque<PathfindingInstruction> path;
 };
 
 #endif /* end of include guard: SPRITE_H_70503825 */
@@ -47,7 +47,7 @@ void TransformSystem::moveSprite(int spriteId, vec2i newLoc) {
   }
 }
 
-CollisionResult TransformSystem::checkCollision(int spriteId, vec2i newLoc, Direction dir) {
+CollisionResult TransformSystem::checkCollision(int spriteId, vec2i curLoc, vec2i newLoc, Direction dir) {
   CollisionResult result;
 
   Sprite& sprite = game_.getSprite(spriteId);
@@ -57,7 +57,7 @@ CollisionResult TransformSystem::checkCollision(int spriteId, vec2i newLoc, Dire
 
   vec2i mapBounds = map.getMapSize() * map.getTileSize();
 
-  vec2i oldColUL = sprite.loc + sprite.collisionOffset;
+  vec2i oldColUL = curLoc + sprite.collisionOffset;
   vec2i oldColDR = oldColUL + sprite.collisionSize;
   vec2i newColUL = newLoc + sprite.collisionOffset;
   vec2i newColDR = newColUL + sprite.collisionSize;
@@ -45,7 +45,7 @@ public:
     });
   }
 
-  CollisionResult checkCollision(int spriteId, vec2i newLoc, Direction dir);
+  CollisionResult checkCollision(int spriteId, vec2i curLoc, vec2i newLoc, Direction dir);
 
   CharacterMedium getMediumAtPosition(int spriteId, vec2i newLoc);
 
@@ -109,6 +109,10 @@ public:
     return (x() != other.x()) || (y() != other.y());
   }
 
+  bool operator<(const vec2& other) const {
+    return std::tie(x(), y()) < std::tie(other.x(), other.y());
+  }
+
 };
 
 using vec2i = vec2<int>;