Jump to content

  • Log In with Google      Sign In   
  • Create Account

We need your help!

We need 1 more developer from Canada and 12 more from Australia to help us complete a research survey.

Support our site by taking a quick sponsored survey and win a chance at a $50 Amazon gift card. Click here to get started!

A-star pathfinding

Posted by EWClay, 17 March 2013 · 819 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
	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;

			// 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)

						if (nodeIndex == startIndex)

						nodeIndex = nodeMap[nodeIndex].parentNode;


				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));


	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.

September 2015 »

  123 4 5

Recent Entries

Recent Comments