I implemented this A* algorithm in C++ for my Game Engineering II class in the Master of Entertainment Arts & Engineering program at University of Utah.

To fully understand the structs and classes the I am using in this code, check out my Game Engineering II final project series.

For my A* algorithm, I created several helper structs to use.

```// Helper struct
// =============================================
// the struct that stores a node's value
struct NodeValue {
NodeValue() :h(0.0f), g(0.0f) {}
NodeValue(float i_h, float i_g) :h(i_h), g(i_g) {}
float h; // the heuristic cost that estimates the cost of the cheapest path from this node to the goal.
float g; // the cost of path from the source node to this node.

float TotalVal() const {
return h + g;
}
};

// The struct that gets stored in the priority queue
struct AStarNode {
:m_navNode(i_thisNode), m_val(i_val)
{}

bool HasLowerPriorityThan(const AStarNode & r) const {
// Compares the total heuristic
return (m_val.TotalVal() > r.m_val.TotalVal());
}

NodeValue m_val;
};

// Determine priority (in the priority queue)
// Returns true if its first argument comes before its second argument in a weak ordering.
// But because the priority queue outputs largest elements first, the elements that "come before" are actually output last.
struct AStarNodeCompare {
bool operator()(const AStarNode & a, const AStarNode & b)
{
// Returns true if a should go to the "bottom", that is if a's totla heuristic is larger than b
return a.HasLowerPriorityThan(b);
}
};
```

I first intuition was to use a single priority_queue for priority sorting and also for priority/previous nodes lookup and updates. However, priority_queue does not natively allow iteration over the underlying container. Therefore, I thought about an approach like this that I found on stack overflow.

```template
S& Container(priority_queue& q) {
struct HackedQueue : private priority_queue {
static S& Container(priority_queue& q) {
return q.*&HackedQueue::c;
}
};
return HackedQueue::Container(q);
}
```

This is somewhat hacky and probably not a suggested approach since the action of privately subclassing priority_queue can be unsafe because priority_queue does not have a virtual destructor. Even though the code above is simply returning the underlying container.

Therefore, I chose to use two std::vectors with a std::priority_queue to implement the A* algorithm, the reason that I could easily use the std::vector is that each node in our graph will get assigned a unique id, which I can then use to access them very quickly and easily.

The priority queue is used to maintain a sorted order in priority among the nodes that I have visited along the path, whenever I update a node’s g value and its previous node, I update them inside the vectors and push the newer version of the node into the priority queue. Therefore, there are actually old/stale nodes inside the queue, which can be easily ignored and pop out with simple checks.

Comparing to my approach here, using a single queue or deque with manual priority maintaining can definitely reduce memory usage. However, every check and look up will take linear search time so it might not be worth it.

The actual A* algorithm is shown below.

```std::stack AStarSearch(cNavigationGraphManager* i_graphManager, unsigned int i_sourceNodeId, unsigned int i_targetNodeId)
{
// return value
std::stack ret;

// Store the actual value of the priority, keep updating, use this for value look up
std::vector nodeValues(i_graphManager->GetNodesCount(), NodeValue(std::numeric_limits::max(), std::numeric_limits::max()));

// Store the previous nodes that lead to node N, keep updating, use this for previous nodes look up
std::vector nodePrevs(i_graphManager->GetNodesCount(), nullptr);

// Use priority queue for our a* star nodes, store node* and priority value "at the time"
std::priority_queue nodesQueue;

// Initialize the source node
AStarNode sourceNode((*i_graphManager)[i_sourceNodeId],
NodeValue (NodeDistance((*i_graphManager)[i_targetNodeId], (*i_graphManager)[i_sourceNodeId]), 0.0f)
);
nodesQueue.emplace(sourceNode);
nodeValues[i_sourceNodeId].g = 0.0f;
nodeValues[i_sourceNodeId].h = sourceNode.m_val.h;
nodePrevs[i_sourceNodeId] = (*i_graphManager)[i_sourceNodeId];

// End condition: queue empty / queue top == target
while (!nodesQueue.empty()) {
AStarNode top = nodesQueue.top();
nodesQueue.pop();

// Check if popped node is destination
if (top.m_navNode == (*i_graphManager)[i_targetNodeId]) {
goto FoundTarget;
}
// check if this node is queue is old version (larger heuristic)
if (nodePrevs[top.m_navNode->GetId()] != nullptr && nodeValues[top.m_navNode->GetId()].g GetReachableNodes()) {
if (nodePrevs[reachableNode->GetId()] == nullptr) {
nodePrevs[reachableNode->GetId()] = top.m_navNode;

nodeValues[reachableNode->GetId()].g = nodeValues[top.m_navNode->GetId()].g + NodeDistance(reachableNode, top.m_navNode);// g value
nodeValues[reachableNode->GetId()].h = NodeDistance(reachableNode, (*i_graphManager)[i_targetNodeId]); // assign h value only the first time

nodesQueue.emplace(AStarNode(reachableNode, nodeValues[reachableNode->GetId()]));

if (reachableNode == (*i_graphManager)[i_targetNodeId]) {
goto FoundTarget;
}
} else {
if (nodePrevs[reachableNode->GetId()] != top.m_navNode &&
nodeValues[reachableNode->GetId()].g > nodeValues[top.m_navNode->GetId()].g + NodeDistance(reachableNode, top.m_navNode))
{
// update g value and prev node
nodeValues[reachableNode->GetId()].g = nodeValues[top.m_navNode->GetId()].g + NodeDistance(reachableNode, top.m_navNode);
nodePrevs[reachableNode->GetId()] = top.m_navNode;

// push the newer version into the queue
nodesQueue.emplace(AStarNode(reachableNode, nodeValues[reachableNode->GetId()]));
}
}
}
}
return ret; // Didn't find a path!
FoundTarget:
{
ret.push((*i_graphManager)[i_targetNodeId]);