The How of A*

(For the Why of A*, go here)

Here is a basic implementation of A*. Note that this isn’t necessarily an optimal solution, but it is a working one.

For my solution, I have two classes: Node and PathfindingAStar. The two classes are tightly coupled, with Pathfinding depending on Node to have certain properties in order to work. Node itself is the bridge between the pathfinding subsystem and the wider system.

---

Here is Node.h:

typedef struct
{
 unsigned int x;
 unsigned int y;
}NodePos;

class Node
{
public:
 Node(unsigned int p_x, unsigned int p_y);
 ~Node();
 NodePos getPosition();
 bool isSameNode(Node* p_node);
 void setNeighbourAt(Direction p_dir, Node* p_neighbour, double p_cost = 1.0);
 Node* getNeighbourAt(Direction p_dir);
 unsigned int getMovementCost(Direction p_dir);
 std::vector getAllAvailableDirections();
 bool isOccupied();
 void setOccupied(bool p_occupied);
 void setParent(Node* p_parent);
 Node* getParent();
 double getG();
 void setG(double p_distanceTravelled);
 double getH();
 void setH(double p_heuristic);
 double getF();
 unsigned int getX();
 unsigned int getY();
private:
 std::map m_movementCost;
 NodePos m_pos;
 std::map m_neighbours;
 Node *m_parent;
 bool m_occupied;
 double m_h;
 double m_g;
};

The Node caters for two purposes. Firstly, it holds information about the world itself that are vital for Pathfinding to operate. The position of the world, the neighbours of the node, the costs, and whether the node is occupied are all created and maintained by the game code. Neighbours cannot be populated upon creation because neighbours won’t have necessarily have been created themselves. Whether the node is currently occupied will be need to be maintained by the game code.

Secondly, the node holds information relevant to the cycle of the path. m_g and m_h are used by Pathfinding for the node’s distance from the start node and to the goal node. Similarly, m_parent is used to keep track of where the navigation came from.

As far as methods in Node.cpp go, there are two methods that are worth exploring, both to do with the available directions.

void Node::setNeighbourAt(Direction p_dir, Node* p_neighbour, double p_cost)
{
 m_neighbours[p_dir] = p_neighbour;
 m_movementCost[p_dir] = p_cost;
}
std::vector Node::getAllAvailableDirections()
{
 std::vector directions;
 directions.clear();
 std::map::iterator itr;
 std::pair pair;
 for(itr = m_movementCost.begin(); itr != m_movementCost.end(); itr++)
 {
  pair = *itr;
  directions.push_back(pair.first);
 }
 return directions;
}

The Pathfinding algorithm will need to know what directions are available to move in, and how much it will “cost” to move in that direction. An alternative would be to simply have a cost associated with a node, but this way gives more flexibility to the game code. So there are two lists, one for the cost of moving in a direction, and one for the neighbour Node in that direction.

Since directions are set when neighbours are set, there’s never any need to worry about the two maps going out of sync. So either map could have been iterated over to build a list for Pathfinding to use. This returned list could just as easily have been a private member of Node, generated as part of the setting of neighbours, and perhaps could be a point of efficiency to do so in the future.

---

Here is the source code for PathfindingAStar.h:

#include "Node.h"

class PathfindingAStar
{
public:
    static PathfindingAStar* getInstance();
    ~PathfindingAStar();
 std::list findPath(Node* p_startNode, Node* p_goalNode);
private:
 std::list finalisePath(Node* p_currentNode); 
 PathfindingAStar();
 static PathfindingAStar *singleton;
};

Pathfinding has one public method – to find the path. The rest is just for housekeeping. There only ever needs to be one instance of the class, so the Singleton pattern is implemented. There’s nothing particularly special about the class from an OO point of view, as all of the work is done in the single method search.

std::list PathfindingAStar::findPath(Node* p_startNode, Node* p_goalNode) 
{
 std::vector m_openList; //Nodes that are active
 std::vector m_closedList; //Nodes that have been used
 //Clear the list
 m_openList.clear();
 m_closedList.clear();
 //Reset the node to parent for the current cycle
 p_startNode->setParent(0);
 //and reset all values of f, g & h
 p_startNode->setG(0.0);
 double h = (double) abs(p_goalNode->getX() - p_startNode->getX());
 h += (double) abs(p_goalNode->getY() - p_startNode->getY());
 p_startNode->setH(h);
 m_openList.push_back(p_startNode);

 while (!m_openList.empty()) //If there are still nodes to explore
 {
  //Get the node with the smallest f value
  Node* currentNode = m_openList.front();
  //Remove node from the open list
  std::vector::iterator itr = m_openList.begin();
  m_openList.erase(itr);


  if (currentNode->isSameNode(p_goalNode)) 
  {
   std::list goalList = finalisePath(currentNode);
   //return out of the program
   return goalList;
  }

  std::vector neighbours = currentNode->getAllAvailableDirections();

  bool isFound;

  for (unsigned int i = 0; i < neighbours.size(); i++) {
   isFound = false;
   Node *n = currentNode->getNeighbourAt(neighbours[i]);
   if (n->isOccupied()) {
    continue;
   }
   //Make sure node isn't in open list
   for (unsigned int j = 0; j < m_openList.size(); j++) {
    Node *n2 = m_openList.at(j);
    if (n2 == n) 
    {
     double g = currentNode->getG() + currentNode->getMovementCost(neighbours[i]);
     if (g < n2->getG()) 
     {
      std::vector::iterator itr2 = m_openList.begin();
      m_openList.erase(itr2 + j);
      for (; itr2 != m_openList.end(); itr2++) 
      {
       Node* n2 = *itr2;
       if (n->getF() <= n2->getF()) 
       {
        break;
       }
      }
      m_openList.insert(itr2, n);

      n2->setG(g);
      n2->setParent(currentNode);
     }
     isFound = true;
     break;
    }
   }
   if (isFound)
    continue;
   for (unsigned int j = 0; j < m_closedList.size(); j++) 
   {
    Node *n2 = m_closedList.at(j);
    if (n2 == n) 
    {
     isFound = true;
     break;
    }
   }
   if (isFound)
    continue;

   n->setParent(currentNode);
   //work out g
   n->setG(currentNode->getG() + currentNode->getMovementCost(neighbours[i]));
   //work out h
   h = (double) abs(p_goalNode->getX() - n->getX());
   h += (double) abs(p_goalNode->getY() - n->getY());
   n->setH(h);

   //add to open list in order
   std::vector::iterator itr2 = m_openList.begin();
   for (; itr2 != m_openList.end(); itr2++) 
   {
    Node* n2 = *itr2;
    if (n->getF() <= n2->getF()) 
    {
     break;
    }
   }
   m_openList.insert(itr2, n);
  }

  m_closedList.push_back(currentNode);
 }
 std::list dummylist;
 return dummylist;
}

The first few lines of code are setting up the world. Since the start node has no parent, it’s set as 0. The start node is then pushed onto the frontier list. The reason there are two lists is so that once a node is fully explored, it never needs to be explored again. It subsequently shifts from the frontier list to the closed list.

The main algorithm is a while loop, which will run until either the goal Node is found, or the frontier list is exhausted. The Node at the front of the frontier list (sorted by f) is checked for whether it is in-fact the goal Node, in which case the algorithm constructs the list of Nodes and returns it. Otherwise, the Node goes through each available direction on the current Node, removes it from the frontier list, and adds it to the closed list.

For each neighbour Node, it needs to be checked whether it’s already a) on the frontier, or b) on the closed list. In either of these cases, the Node needs no further exploration except in one special case of a Node on the frontier. If the distance travelled to the Node in this iteration is less than the distance travelled to the Node, then the Node and frontier list need to be updated. i.e. we have found a shorter path to the same position. If the neighbour Node is not yet explored, its g and h are calculated (in this case, h is calculated using the Manhattan heuristic), its parent is set as the current Node, then it is put onto the frontier list sorted by f. Finally, if the frontier list is exhausted, this means there was no path from the start Node to the goal Node, so an empty list is created and returned. Thus the empty() function on list acts as the indicator of whether there was path successfully generated.

The code for building the path list is as follows:

std::list PathfindingAStar::finalisePath(Node* p_currentNode) 
{
 std::list goalList;
 while (p_currentNode != NULL) 
 {
  goalList.push_front(p_currentNode);
  p_currentNode = p_currentNode->getParent();
 }
 //don't need the first node
 goalList.pop_front();
 return goalList;
}

The code is fairly straight forward. Since each Node knows its parent, it’s simply a matter of traversing back to the first Node where the parent was set as NULL.

---

And there it is – a basic implementation of the A* algorithm. It’s by no means the most efficient possible version of the algorithm, nor does it do any of the C++ things one really ought to do when using the language. The important thing is that it works, and it’s reasonably clear how.