v1.4.4

source/core/StarAStar.hpp

@@ -0,0 +1,276 @@
+#ifndef STAR_A_STAR_HPP
+#define STAR_A_STAR_HPP
+
+#include <queue>
+
+#include "StarList.hpp"
+#include "StarMap.hpp"
+#include "StarSet.hpp"
+#include "StarLexicalCast.hpp"
+#include "StarMathCommon.hpp"
+#include "StarBlockAllocator.hpp"
+
+namespace Star {
+namespace AStar {
+
+  struct Score {
+    Score();
+
+    double gScore;
+    double hScore;
+    double fScore;
+  };
+
+  // 'Edge' should be implemented as a class with public fields compatible with
+  // these:
+  //    double cost;
+  //    Node source;
+  //    Node target;
+
+  template <class Edge>
+  using Path = List<Edge>;
+
+  template <class Edge, class Node>
+  class Search {
+  public:
+    typedef function<double(Node, Node)> HeuristicFunction;
+    typedef function<void(Node, List<Edge>& neighbors)> NeighborFunction;
+    typedef function<bool(Node)> GoalFunction;
+    typedef function<bool(Node, Node)> CompareFunction;
+    typedef function<bool(Edge)> ValidateEndFunction;
+
+    Search(HeuristicFunction heuristicCost,
+        NeighborFunction getAdjacent,
+        GoalFunction goalReached,
+        bool returnBestIfFailed = false,
+        // In returnBestIfFailed mode, validateEnd checks the end of the path
+        // is valid, e.g. not floating in the air.
+        Maybe<ValidateEndFunction> validateEnd = {},
+        Maybe<double> maxFScore = {},
+        Maybe<unsigned> maxNodesToSearch = {});
+
+    // Start a new exploration, resets result if it was found before.
+    void start(Node startNode, Node goalNode);
+    // Explore the given number of nodes in the search space.  If
+    // maxNodesToSearch is reached, or the search space is exhausted, will
+    // return
+    // false to signal failure.  On success, will return true.  If the given
+    // maxExploreNodes is exhausted before success or failure, will return
+    // nothing.
+    Maybe<bool> explore(Maybe<unsigned> maxExploreNodes = {});
+    // Returns the result if it was found.
+    Maybe<Path<Edge>> const& result() const;
+
+    // Convenience, equivalent to calling start, then explore({}) and returns
+    // result()
+    Maybe<Path<Edge>> const& findPath(Node startNode, Node goalNode);
+
+  private:
+    struct ScoredNode {
+      bool operator<(ScoredNode const& other) const {
+        return score.fScore > other.score.fScore;
+      }
+
+      Score score;
+      Node node;
+    };
+
+    struct NodeMeta {
+      Score score;
+      Maybe<Edge> cameFrom;
+    };
+
+    Path<Edge> reconstructPath(Node currentNode);
+
+    HeuristicFunction m_heuristicCost;
+    NeighborFunction m_getAdjacent;
+    GoalFunction m_goalReached;
+    bool m_returnBestIfFailed;
+    Maybe<ValidateEndFunction> m_validateEnd;
+    Maybe<double> m_maxFScore;
+    Maybe<unsigned> m_maxNodesToSearch;
+
+    Node m_goal;
+    Map<Node, NodeMeta, std::less<Node>, BlockAllocator<pair<Node const, NodeMeta>, 1024>> m_nodeMeta;
+    std::priority_queue<ScoredNode> m_openQueue;
+    Set<Node, std::less<Node>, BlockAllocator<Node, 1024>> m_openSet;
+    Set<Node, std::less<Node>, BlockAllocator<Node, 1024>> m_closedSet;
+    Maybe<ScoredNode> m_earlyExploration;
+
+    bool m_finished;
+    Maybe<Path<Edge>> m_result;
+  };
+
+  inline Score::Score() : gScore(highest<double>()), hScore(0), fScore(highest<double>()) {}
+
+  template <class Edge, class Node>
+  Search<Edge, Node>::Search(HeuristicFunction heuristicCost,
+      NeighborFunction getAdjacent,
+      GoalFunction goalReached,
+      bool returnBestIfFailed,
+      Maybe<ValidateEndFunction> validateEnd,
+      Maybe<double> maxFScore,
+      Maybe<unsigned> maxNodesToSearch)
+    : m_heuristicCost(heuristicCost),
+      m_getAdjacent(getAdjacent),
+      m_goalReached(goalReached),
+      m_returnBestIfFailed(returnBestIfFailed),
+      m_validateEnd(validateEnd),
+      m_maxFScore(maxFScore),
+      m_maxNodesToSearch(maxNodesToSearch) {}
+
+  template <class Edge, class Node>
+  void Search<Edge, Node>::start(Node startNode, Node goalNode) {
+    m_goal = move(goalNode);
+    m_nodeMeta.clear();
+    m_openQueue = std::priority_queue<ScoredNode>();
+    m_openSet.clear();
+    m_closedSet.clear();
+    m_earlyExploration = {};
+    m_finished = false;
+    m_result.reset();
+
+    Score startScore;
+    startScore.gScore = 0;
+    startScore.hScore = m_heuristicCost(startNode, m_goal);
+    startScore.fScore = startScore.hScore;
+    m_nodeMeta[startNode].score = startScore;
+
+    m_openSet.insert(startNode);
+    m_openQueue.push(ScoredNode{startScore, move(startNode)});
+  }
+
+  template <class Edge, class Node>
+  Maybe<bool> Search<Edge, Node>::explore(Maybe<unsigned> maxExploreNodes) {
+    if (m_finished)
+      return m_result.isValid();
+
+    List<Edge> neighbors;
+    while (true) {
+      if ((m_maxNodesToSearch && m_closedSet.size() > *m_maxNodesToSearch)
+          || (m_openQueue.empty() && !m_earlyExploration)) {
+        m_finished = true;
+        // Search failed. Either return the path to the closest node to the
+        // target,
+        // or return nothing.
+        if (m_returnBestIfFailed) {
+          double bestScore = highest<double>();
+          Maybe<Node> bestNode;
+          for (Node node : m_closedSet) {
+            NodeMeta const& nodeMeta = m_nodeMeta[node];
+            if (m_validateEnd && nodeMeta.cameFrom && !(*m_validateEnd)(*nodeMeta.cameFrom))
+              continue;
+            if (nodeMeta.score.hScore < bestScore) {
+              bestScore = nodeMeta.score.hScore;
+              bestNode = node;
+            }
+          }
+
+          if (bestNode)
+            m_result = reconstructPath(*bestNode);
+        }
+
+        return false;
+      }
+
+      if (maxExploreNodes) {
+        if (*maxExploreNodes == 0)
+          return {};
+        --*maxExploreNodes;
+      }
+
+      ScoredNode currentScoredNode;
+      if (m_earlyExploration) {
+        currentScoredNode = m_earlyExploration.take();
+      } else {
+        currentScoredNode = m_openQueue.top();
+        m_openQueue.pop();
+        if (!m_openSet.remove(currentScoredNode.node))
+          // Duplicate entry in the queue due to this node's score being
+          // updated.
+          // Just ignore this node; we've already searched it.
+          continue;
+      }
+
+      Node const& current = currentScoredNode.node;
+      Score const& currentScore = currentScoredNode.score;
+
+      if (m_goalReached(current)) {
+        m_finished = true;
+        m_result = reconstructPath(current);
+        return true;
+      }
+
+      m_closedSet.insert(current);
+
+      neighbors.clear();
+      m_getAdjacent(current, neighbors);
+
+      for (Edge const& edge : neighbors) {
+        if (m_closedSet.find(edge.target) != m_closedSet.end())
+          // We've already visited this node.
+          continue;
+
+        double newGScore = currentScore.gScore + edge.cost;
+        NodeMeta& targetMeta = m_nodeMeta[edge.target];
+        Score& targetScore = targetMeta.score;
+        if (m_openSet.find(edge.target) == m_openSet.end() || newGScore < targetScore.gScore) {
+          targetMeta.cameFrom = edge;
+          targetScore.gScore = newGScore;
+          targetScore.hScore = m_heuristicCost(edge.target, m_goal);
+          targetScore.fScore = targetScore.gScore + targetScore.hScore;
+
+          if (m_maxFScore && targetScore.fScore > *m_maxFScore)
+            continue;
+
+          // Early exploration optimization - no need to add things to the
+          // openQueue/openSet
+          // if they're at least as good as the current node.
+          if (targetScore.fScore <= currentScore.fScore) {
+            if (m_earlyExploration.isNothing()) {
+              m_earlyExploration = ScoredNode{targetScore, edge.target};
+              continue;
+            } else if (m_earlyExploration->score.fScore > targetScore.fScore) {
+              m_openSet.insert(m_earlyExploration->node);
+              m_openQueue.push(*m_earlyExploration);
+              m_earlyExploration = ScoredNode{targetScore, edge.target};
+              continue;
+            }
+          }
+          m_openSet.insert(edge.target);
+          m_openQueue.push(ScoredNode{targetScore, edge.target});
+        }
+      }
+    }
+  }
+
+  template <class Edge, class Node>
+  Maybe<Path<Edge>> const& Search<Edge, Node>::result() const {
+    return m_result;
+  }
+
+  template <class Edge, class Node>
+  Maybe<Path<Edge>> const& Search<Edge, Node>::findPath(Node startNode, Node goalNode) {
+    start(move(startNode), move(goalNode));
+    explore();
+    return result();
+  }
+
+  template <class Edge, class Node>
+  Path<Edge> Search<Edge, Node>::reconstructPath(Node currentNode) {
+    Path<Edge> res; // this will be backwards, we reverse it before returning it.
+    while (m_nodeMeta.find(currentNode) != m_nodeMeta.end()) {
+      Maybe<Edge> currentEdge = m_nodeMeta[currentNode].cameFrom;
+      if (currentEdge.isNothing())
+        break;
+      res.append(*currentEdge);
+      currentNode = currentEdge->source;
+    }
+    std::reverse(res.begin(), res.end());
+    return res;
+  }
+}
+
+}
+
+#endif