Jump to content

  • Log In with Google      Sign In   
  • Create Account






A-star pathfinding

Posted by EWClay, 17 March 2013 · 647 views

Here's an implementation of A* in C++ which hopefully works and is quite sensible.

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<int> 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<ClosedSet::iterator, bool> 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<int>& 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<std::pair<float, int>> OpenQueue;
	typedef std::unordered_set<int> ClosedSet;
	typedef std::unordered_map<int, NodeInfo> NodeMap;

	OpenQueue openQueue;
	ClosedSet closedSet;
	NodeMap nodeMap;
};





This might be more efficient than my own implementation. What's the OpenQueue type?

std::priority_queue I believe is a heap.

April 2014 »

S M T W T F S
  12345
6789101112
13141516171819
202122 23 242526
27282930   

Recent Entries

Recent Comments

PARTNERS