431a9c00a5
On Linux and macOS, using Clang to compile OpenStarbound produces about 400 MB worth of warnings during the build, making the compiler output unreadable and slowing the build down considerably. 99% of the warnings were unqualified uses of std::move and std::forward, which are now all properly qualified. Fixed a few other minor warnings about non-virtual destructors and some uses of std::move preventing copy elision on temporary objects. Most remaining warnings are now unused parameters.
277 lines
8.7 KiB
C++
277 lines
8.7 KiB
C++
#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 = 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;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|