Note that
- It doesn't use a million templates, or any for that matter (except std containers).
- It doesn't do stupid things like trying to erase out of the middle of a queue.
- It assumes nothing about data structures so is highly generic.
- It doesn't come with its own memory allocator/hash table/queue/linked list. It's just A*.
Instructions: derive from it and implement the virtual functions. Then call FindPath(start, end). That's it!
class AStar{public: void FindPath(int startIndex, int endIndex) { // Add the nodes in the starting zone to the open list. std::vector connectedNodes; GetConnectedNodes(startIndex, connectedNodes); for (int nodeIndex : connectedNodes) { float f = GetDistance(startIndex, nodeIndex) + GetDistance(endIndex, nodeIndex); openQueue.emplace(OpenQueue::value_type(-f, nodeIndex)); nodeMap[nodeIndex] = NodeInfo(f, startIndex, GetData(startIndex)); } while (!openQueue.empty()) { // Get the first item in the queue and remove it. float currentLength = -openQueue.top().first; int currentIndex = openQueue.top().second; openQueue.pop(); // Add it to the closed list; std::pair r = closedSet.insert(currentIndex); if (r.second == true) // If it was already on the closed list, ignore it. { if (currentIndex == endIndex) // Found the final node. { int nodeIndex = currentIndex; // Output the path. while (true) { AddToPath(nodeIndex); if (nodeIndex == startIndex) break; nodeIndex = nodeMap[nodeIndex].parentNode; }; return; } connectedNodes.clear(); GetConnectedNodes(currentIndex, connectedNodes); float g = currentLength - GetDistance(currentIndex, endIndex); for (int newNodeIndex : connectedNodes) { if (closedSet.find(newNodeIndex) == closedSet.end()) { float f = g + GetDistance(currentIndex, newNodeIndex) + GetDistance(newNodeIndex, endIndex); auto it = nodeMap.find(newNodeIndex); if (it == nodeMap.end() || it->second.f > f) { // The node may be in the open set already. If so, this will add it again // with a lower f score and update the NodeMap. openQueue.push(OpenQueue::value_type(-f, newNodeIndex)); nodeMap[newNodeIndex] = NodeInfo(f, currentIndex, GetData(currentIndex)); } } } } } }protected: struct NodeInfo { NodeInfo() {} NodeInfo(float f, int parentNode, int data) : f(f), parentNode(parentNode), data(data) {} float f; int parentNode; int data; }; virtual void GetConnectedNodes(int nodeIndex, std::vector& nodes) = 0; virtual float GetDistance(int node1, int node2) = 0; virtual void AddToPath(int nodeIndex) = 0; virtual int GetData(int nodeIndex) = 0; typedef std::priority_queue> OpenQueue; typedef std::unordered_set ClosedSet; typedef std::unordered_map NodeMap; OpenQueue openQueue; ClosedSet closedSet; NodeMap nodeMap;};
This might be more efficient than my own implementation. What's the OpenQueue type?