how to convert this code from Dijkstra to Astar? - c++

Question

So I have a project of which I want to switch to Astar due to speed reasons.
But C++ is not my strongest point. Could anyone help me (or tell me how to do the..) converting the algorythm from Dijkstra to Astar?
I found this Astar implementation:
http://code.google.com/p/a-star-algorithm-implementation/
But I don't know how to use it with my existing code.
Here is the graph file which got the algorithm:
#include "Graph.h"
#include <iostream>
#include <algorithm>
#include <stack>
Graph::Graph(void)
{
}
Graph::~Graph(void)
{
while(!mNodes.empty())
{
delete mNodes.back();
mNodes.pop_back();
}
}
void Graph::addNode(int name, bool exists, Node** NodeID )
{
Node* pStart = NULL;
mNodes.push_back(new Node(name,exists));
std::vector<Node*>::iterator itr;
itr = mNodes.begin()+mNodes.size()-1;
pStart = (*itr);
if(exists == true)pStart->DoesExist_yes();
*NodeID = pStart;
}
void Graph::connect_oneway(Node* pFirst, Node* pSecond, int moveCost)
{
if(pFirst != NULL && pSecond != NULL)
{
pFirst->createEdge(pSecond, moveCost);
}
}
#define MAX_NODES (32768)
#define MAX_CONNECTIONS (5)
#include <time.h>
int * Graph::findPath_r(Node* pStart, Node* pEnd)
{
int *arr = new int[MAX_NODES+2];
for (int i=0; i<MAX_NODES; i++)
arr[i] = -1;
arr[0] = 0;
if(pStart == pEnd)
{
return arr;
}
std::vector<Node*> openList;
openList.push_back(pStart);
Node* pCurrNode = NULL;
while(!openList.empty())
{
//Get best node from open list (lowest F value).
//Since we sort the list at the end of the previous loop we know
//the front node is the best
pCurrNode = openList.front();
//Exit if we're are the goal
if(pCurrNode == pEnd)
break;
//Remove the node from the open list and place it in the closed
openList.erase(openList.begin());
pCurrNode->setClosed(true); //We use a flag instead of a list for speed
//Test all of the edge nodes from the current node
std::vector<Edge*>* pEdges = pCurrNode->getEdges();
Node* pEdgeNode = NULL;
for(std::vector<Edge*>::iterator i = pEdges->begin(); i != pEdges->end(); ++i)
{
pEdgeNode = (*i)->pNode;
//If it's closed we've already analysed it
if(!pEdgeNode->getClosed() && pCurrNode->DoesExist() == true)
{
if(!inList(pEdgeNode,&openList))
{
openList.push_back(pEdgeNode);
pEdgeNode->setGCost(pCurrNode->getGCost()+(*i)->moveCost);
pEdgeNode->calcFCost();
pEdgeNode->setParent(pCurrNode);
}
else
{
//If this is a better node (lower G cost)
if(pEdgeNode->getGCost() > pCurrNode->getGCost()+(*i)->moveCost)
{
pEdgeNode->setGCost(pCurrNode->getGCost()+(*i)->moveCost);
pEdgeNode->calcFCost();
pEdgeNode->setParent(pCurrNode);
}
}
}
}
//Place the lowest F cost item in the open list at the top, so we can
//access it easily next iteration
std::sort(openList.begin(), openList.end(), Graph::compareNodes);
}
//Make sure we actually found a path
if(pEnd->getParent() != NULL)
{
//Output the path
//Use a stack because it is LIFO
std::stack<Node*> path;
while(pCurrNode != NULL)
{
path.push(pCurrNode);
pCurrNode = pCurrNode->getParent();
}
int counter = 0;
arr[1] = 0;
while(!path.empty())
{
arr[counter+2] = path.top()->getName();
counter++;
arr[1] += path.top()->getGCost();
path.pop();
}
arr[0] = counter;
return arr;
}
return arr;
}
bool Graph::inList(Node* pNode, std::vector<Node*>* pList)
{
for(std::vector<Node*>::iterator i = pList->begin(); i != pList->end(); ++i)
{
if((*i) == pNode)
{
return true;
}
}
return false;
}
bool Graph::compareNodes(Node* pFirst, Node* pSecond)
{
return pFirst->getFCost() < pSecond->getFCost();
}
void Graph::reset(void)
{
for(std::vector<Node*>::iterator i = mNodes.begin(); i != mNodes.end(); ++i)
{
(*i)->reset();
}
}
The function for finding the path is this one:
Graph::findPath_r
What I really want to do is preserve the edges (because they decide if the road is both or one-way).
Here are the other files:
Graph.h
#ifndef _GRAPH_H_
#define _GRAPH_H
#include "Node.h"
class Graph
{
public:
Graph(void);
~Graph(void);
//void addNode(int name, bool exists);
void addNode(int name, bool exists, Node** NodeID );
void connect_oneway(int ppFirst, int ppSecond, int moveCost);
void connect_oneway(Node* pFirst, Node* pSecond, int moveCost);
//int * findPath_r(int start, int end);
int * findPath_r(Node* pStart, Node* pEnd);
void reset(void);
private:
void findNodesx(int firstName, Node** ppFirstNode);
bool inList(Node* pNode, std::vector<Node*>* pList);
static bool compareNodes(Node* pFirst, Node* pSecond);
std::vector<Node*> mNodes;
};
#endif
Node.h
#ifndef _NODE_H_
#define _NODE_H_
#include <string>
#include <vector>
//Forward declare Node so Edge can see it
class Node;
struct Edge
{
Edge(Node* node, int cost) : pNode(node), moveCost(cost){}
Node* pNode;
int moveCost;
};
class Node
{
public:
Node(void);
Node(int name, bool exists);
~Node(void);
void createEdge(Node* pTarget, int moveCost);
void setGCost(int cost);
void setClosed(bool closed);
void setParent(Node* pParent);
int getGCost(void);
int getFCost(void);
bool getClosed(void);
Node* getParent(void);
int getName(void);
bool DoesExist(void);
bool DoesExist_yes(void);
std::vector<Edge*>* getEdges(void);
void calcFCost(void);
void reset(void);
private:
int mGCost;
int mTotal;
bool mClosed;
Node* mpParent;
int mName;
bool mHeur;
std::vector<Edge*> mEdges;
};
#endif
Node.cpp
#include "Node.h"
Node::Node(void)
{
}
Node::Node(/*const std::string&*/int name, bool exists) : mGCost(0), mTotal(0), mClosed(false), mpParent(NULL), mName(name), mHeur(exists)
{
}
Node::~Node(void)
{
while(!mEdges.empty())
{
delete mEdges.back();
mEdges.pop_back();
}
}
int Node::getName(void)
{
return mName;
}
void Node::createEdge(Node* pTarget, int moveCost)
{
mEdges.push_back(new Edge(pTarget, moveCost));
}
void Node::setClosed(bool closed)
{
mClosed = closed;
}
bool Node::getClosed(void)
{
return mClosed;
}
std::vector<Edge*>* Node::getEdges(void)
{
return &mEdges;
}
int Node::getGCost(void)
{
return mGCost;
}
void Node::setGCost(int cost)
{
mGCost = cost;
}
void Node::calcFCost(void)
{
mTotal = mGCost;
}
void Node::setParent(Node* pParent)
{
mpParent = pParent;
}
int Node::getFCost(void)
{
return mTotal;
}
bool Node::DoesExist(void)
{
return mHeur;
}
bool Node::DoesExist_yes(void)
{
mHeur = true;
return true;
}
Node* Node::getParent(void)
{
return mpParent;
}
void Node::reset(void)
{
mGCost = 0;
mTotal = 0;
mClosed = false;
mpParent = NULL;
}

Answer 1

You mentioned a library on GoogleCode. It is node clear what you want to do with, and I think the best is to write your implementation yourself.
First, you should know that Dijsktra is a special case of A*. In A*, you have an heuristic, named h; A* = possible implementation of Dijsktra when h is the null function.
Then, about your implementation, let's start with Node. It will need the following functions:
constructor, destructor
create/get edge
set/get parent
set/is closed (for speed)
set/get GCost
set/get FCost
set/is obstacle (name way more descriptive than 'DoesExist')
set/get position
reset
// optional method:
get name
Hopefully, this part of your code won't change a lot. The heuristic code will be placed in the pathfinder. The Edge class is left untouched.
Now the big one: Graph. You won't need to delete any of your public methods.
You will need a heuristic method. For the implementation which will be described, you will need an admissible consistent heuristic:
it must not over-estimate the distance to the goal (admissible)
it must be monotone (consistent)
The general case signature is int getHCost(Node* node);. If you always return 0, you will have a Dijsktra algorithm, which is not what you want. Here we will take the euclidiean distance between the node and the goal. Slower to compute than manhattan distance, but better results. You can change this afterwards.
int getHCost(Node* node, Note* goal);
This implies you must place your nodes in the 3d space. Note that the heuristic is a heuristic, ie, an estimation of the distance.
I won't write the code. I will write some pseudo-code adapted to your situation. The original pseudocode is located on the Wikipedia A* page. This pseudo-code is your findPath_r function:
function A*(start,goal)
set all nodes to not closed // The set of nodes already evaluated.
openset = {start} // The set of tentative nodes to be evaluated, initially containing the start node
start.gcost = 0 // Cost from start along best known path.
// Estimated total cost from start to goal through y.
start.fcost = start.gcost + getHCost(start, goal)
while openset is not empty
current = the node in openset having the lowest f_cost (usually the first if you use a sorted list)
if current == goal
return construct_path(goal)
remove current from openset
current.closed = true
for each neighbor in (node connected by edge in current.edges) // Here is the condition for one-way edges
if neighbor.closed or neighbor.obstacle
continue
gcost = current.gcost + dist_between(current,neighbor) // via edge distance
if neighbor not in openset
add neighbor to openset
neighbor.parent = current
neighbor.gcost = gcost
neighbor.fcost = neighbor.gcost + getHCost(neighbor, goal)
else if gcost < neighbor.gcost
neighbor.parent = current
neighbor.gcost = gcost
neighbor.fcost = neighbor.gcost + getHCost(neighbor, goal)
update neighbor position in openset
return failure
function construct_path(current_node)
std::vector<Node*> path
while current_node != 0
path.push_front(current_node)
current_node = current_node.parent
return path
The implementation above use one-way edges.
You were able to write Dijsktra algorithm in C++, so writing this pseudocode in C++ shouldn't be a problem.
Second part, performances. First, measure ;).
I have some hints that can improve performances:
use a memory pool for allocation deallocation
use an intrusive list for the open list (you can also make it auto-sorted with this technique)
I advise you to read A* for beginners, which is a useful reading, even if you don't use tilemap.