274 lines
8.6 KiB
C++
274 lines
8.6 KiB
C++
#pragma once
|
|
|
|
#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 = std::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, std::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(std::move(startNode), std::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;
|
|
}
|
|
}
|
|
|
|
}
|