My program is reading input from a text file in the order of
A
B
C
A B 10
A C 5
B A 3
B C 2
C A 4
C B 1
and stores the data into a graph which is represented by an adjacency list. I want to write a program that finds the shortest path from A to all other nodes using Dijkstra's Algorithm.
I have watched several videos but still couldn't quite get how to implement the algorithm as the examples use graphs which are represented as adjacency matrices. Below is the program:
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <locale>
#include <sstream>
#include <string>
#include <vector>
using namespace std;
#define INFINITE -1
/* node */
struct Node
{
char key;
int distance;
Node *next;
};
/* GraphNode */
struct GraphNode
{
char key;
Node *listpointer;
};
/* Add nodes to the front of the list */
void AddNodeToFront(Node*& listpointer, char newkey, int newdistance)
{
Node *temp;
temp = new Node;
temp->key = newkey;
temp->distance=newdistance;
temp->next = listpointer;
listpointer = temp;
}
/* printf LLnodes */
void PrintLLnodes(Node*& listpointer)
{
Node *temp;
temp = listpointer;
while(temp!=NULL)
{
printf("to node %c dist: %d \n", temp->key, temp->distance);
temp=temp->next;
}
}
/* Implement the Graph class */
class Graph
{
private:
vector<GraphNode> adjlist;
int totalgraphnodes;
public:
Graph(){totalgraphnodes = 0;}
~Graph(){}
void AddNewGraphNode(char newgraphnode);
void AddNewEdgeBetweenGraphNodes(char A, char B, int distance);
void PrintAllGraphNodesWithCosts();
void DijkstrasAlgorithm(char sourcenode);
int GetTotalNodes() { return totalgraphnodes; }
};
/* graph class functions */
void Graph::AddNewGraphNode(char newgraphnode){
totalgraphnodes++;
GraphNode temp;
temp.key=newgraphnode;
temp.listpointer = NULL;//important
adjlist.push_back(temp);
}
void Graph::AddNewEdgeBetweenGraphNodes(char A, char B, int distance)
{
//find which node A is
int a;
for (a = 0;adjlist.size();a++)
{
if (A == adjlist[a].key) break;
}
AddNodeToFront(adjlist[a].listpointer, B, distance);
}
void Graph::PrintAllGraphNodesWithCosts()
{
for (unsigned int a = 0;a < adjlist.size();a++){
printf("From Node %c: \n", adjlist[a].key);
PrintLLnodes(adjlist[a].listpointer);
}
}
**// implement Dijkstra's Algorithm
void Graph::DijkstrasAlgorithm(char sourcenode)**
{
int distance[adjlist.size()];
char state[adjlist.size()];
// setting the initial distance and state of the first node
distance[0] = 0;
state[0] = 'p';
// assigning all distance and state to temp and infinite
for(int i = 1; i <= adjlist.size(); i++)
{
distance[i] = INFINITE;
state[i] = 't';
}
// get the sourcenode from the adjlist in Graph
unsigned int a;
for(a = 0; a <= adjlist.size(); a++)
{
if(sourcenode == adjlist[a].key) { break; } // break when found 'A'
}
// assign sourcenode listpointer to current
Node *current;
current = adjlist[a].listpointer;
bool stilltempvertex = true;
while(stilltempvertex) {}
}
/* declare a new Graph */
Graph mygraph;
main( /*int argc, char** argv */)
{
/* call Dijkstra */
mygraph.DijkstrasAlgorithm('A'); //always from A in this program
/* Print answer in the required format */
}
As far as implementation is concerned, all the nodes and edges are read correctly (I have ommited some code to allow for readability, but mygraph has nodes A, B, C and the corresponding edges.), its just that I cant seem to figure out how to properly implement the algorithm for this one.
The nodes are readfrom the file and stored in a vector ajdlist. Each graphnode then has its own linked list which points to adjacent nodes in the graph.
Anyone could take me step by step through with the algorithm implementation on this? Thanks in advance.
Related
I'm asking myself what's wrong with my code to find the diameter of a tree:
(By diameter of a tree, I mean the greatest distance between two nodes in that tree graph)
#include <iostream>
#include <vector>
#include <algorithm>
using namespace std;
#define MAXN 1000005
int n,a,b;
int dist[MAXN];
vector<int> adj[MAXN];
vector<int> ways;
void dfs(int U, int father){
if(father==U){
dist[U]=1;
}
else{
dist[U]=1+dist[father];
}
for(int i=0;i<(int)adj[U].size();i++){
int V=adj[U][i];
if(dist[V]==-1) dfs(V,U);
}
}
void solve(int S){
dist[S]=0;
for(int j=0;j<(int)adj[S].size();j++){
int w=adj[S][j];
dfs(w,w);
int maxdist=0;
for(int i=1;i<=n;i++){
if(dist[i]!=-1){
maxdist=max(maxdist,dist[i]);
dist[i]=0;
}
}
ways.push_back(-maxdist);
}
}
int main(){
cin>>n;
if(n==2) cout<<"1\n";
else{
for(int i=1;i<=n;i++){
dist[i]=-1;
}
for(int i=1;i<n;i++){
cin>>a>>b;
adj[a].push_back(b);
adj[b].push_back(a);
}
int vert;
for(int i=1;i<=n;i++){
if((int)adj[i].size()>1){
vert=i;
break;
}
}
solve(vert);
sort(ways.begin(),ways.end());
int ret=-(ways[0]+ways[1]);
cout<<ret<<"\n";
}
return 0;
}
For me, it sounds logical and very procedural, but I submitted it to an online judge and it was not accepted. What's going on?
You measure the distance between two nodes in a tree by A and B.
distanceBetwwen_A_and_B = Distance A -> CommonAncestor(A, B)
+ Distance B -> CommonAncestor(A, B);
So for a given Ancestor Node G it has a maximum radius of:
Diameter(G) = Distance_To_Furthest_Child(G->left)
+ Distance_To_Furthest_Child(G->right)
But one of its descendants may have a larger radius than it has itself.
Max_Diameter(G) = max(Diameter(G), Max_Diameter(G->left), Max_Diameter(G->right))
From this you should be able to depth first search and a single traversal to calculate the Max_Diameter(Root)
struct Node
{
Node* l;
Node* r;
int maxD;
};
int findMaxD(Node* root)
{
if (root == nullptr) {
return 0;
}
calcMaxDiameter(root);
return root->maxD;
}
/*
* Sets the maxD value of a node
* Returns the distance to the furthest decendant
*/
int calcMaxDiameter(Node* root)
{
// If we fall of the end
// The distance is zero.
if (root == nullptr) {
return 0;
}
int l_Distance = calcMaxDiameter(root->l);
int r_Distance = calcMaxDiameter(root->r);
// Calculate the preliminary Radius.
root->maxD = l_Distance + r_Distance;
if (root->l) {
// If there is a left node see if it is bigger
// than the current radius
root->maxD = std::max(root->maxD, root->l->maxD);
}
if (root->r) {
// If there is a right node....
root->maxD = std::max(root->maxD, root->r->maxD);
}
// Return the distance to the furthest
// ancestor. Add one for the distance from here
// to the parent.
return std::max(l_Distance, r_Distance) + 1;
}
Can anybody explain me, how to do Breadth first search in the graph that uses vector of linked lists ?
My Graph header file:
#include <string>
#include <iostream>
#include <map>
#include <vector>
using namespace std;
struct vertex {
string code;
vertex* next;
};
struct AdjList {
vertex *head;
AdjList(vertex* Given) {
head = Given;
}
};
class Graph {
map<string, string> associations;
int nodeNum; //amount of nodes or size of the graph;
vector<AdjList> adjList;
public:
Graph(int NodeNum);
~Graph();
int singleSize(string codeName);
int getSize();// must destroy every prerequisite list connected to the node
vertex* generateVertex(string codeName);
int getIndexOfVertex(vertex* givenVertex); // will find the location of the vertex in the array
void addVertex(vertex* newVertex);
void addEdge(string codeName, string linkCodeName);
void printPrerequisites(vertex* ptr, int i);
bool deleteVertex(string codeName);
bool deleteEdge(string codeName, string linkCodeName);
bool elemExistsInGraph(string codeName);
void printPrereq(string codeName);
void printCourseTitle(string codeName);
void printGraph();
};
I am trying to print all connected nodes within the graph using the breadth first search. Here is my code for the breadth first search algorithm that does not work.
void Graph::printPrereq(string codeName) {
int adjListSize = this->adjList.size();
int index = getIndexOfVertex(generateVertex(codeName));
bool visited[this->adjList.size()];
for(int i = 0; i < adjListSize; i++) {
visited[i] = false;
}
list<int> queue;
visited[index] = true;
queue.push_back(index);
while(!queue.empty()) {
index = queue.front();
vertex* pointer = this->adjList[index].head;
cout << pointer->code;
queue.pop_front();
while(pointer != nullptr){
if(!visited[getIndexOfVertex(pointer)]) {
queue.push_back(getIndexOfVertex(pointer));
visited[getIndexOfVertex(pointer)] = true;
}
cout << pointer->code <<"->";
pointer = pointer->next;
}
cout << "Null" << endl;
}
}
This algorithm outputs nodes that are only within the linked list, but not the ones that are connected through the graph.
Can anybody help and solve this problem?
Following code is about searching for neighbours in realtime. As soon as a new node is added to my graph, the function updateSeqNeighbours for this node is called. What I know is, that the new node is definitely neighbour to the last one added. In the next step I use this fact to look in the neighbourhood of the previously added node, find the one closest to the new and then search this neighbourhood for the closest neighbour.
I repeat this only for example 3 times, to limit the number of neighbours for one node to 4 to keep a constant time frame for calculation. It works wonderful, except for after ~30 nodes the calculation time increases very fast with every additional node resulting in a bad_alloc exception.
#ifndef GRAPH_NODE_H_
#define GRAPH_NODE_H_
#include <vector>
#include <cmath>
#include <iostream>
using namespace std;
class Node {
public:
double x;
double y;
Node* nodePrev;
vector<Node> seqNeighbours;
//Constructor
Node();
Node(double x, double y);
virtual ~Node();
//Operator functions
Node& operator=(const Node& n);
//Get&Set
int getID();
//Public member functions
void addNeighbour(Node& n);
bool isSeqNeighbour(int ID);
int updateSeqNeighbours();
double distanceTo(Node& n);
private:
static int count;
int ID;
void _setDefaults();
};
int Node::count = 0;
Node::Node() {
_setDefaults();
}
Node::Node(double x, double y) {
_setDefaults();
this->x = x;
this->y = y;
}
Node::~Node() {
// TODO Auto-generated destructor stub
}
//Operator functions
Node& Node::operator=(const Node& n) {
if (this != &n) {
ID = n.ID;
x = n.x;
y = n.y;
seqNeighbours.clear();
seqNeighbours = n.seqNeighbours;
nodePrev = n.nodePrev;
}
return *this;
}
//Get&Set
int Node::getID() {
return this->ID;
}
//Public member functions
void Node::addNeighbour(Node& n) {
seqNeighbours.push_back(n);
}
double Node::distanceTo(Node& n) {
return sqrt((n.x-x)*(n.x-x) + (n.y-y)*(n.y-y));
}
bool Node::isSeqNeighbour(int ID) {
for (int i = 0; i < seqNeighbours.size(); i++) {
if (seqNeighbours[i].getID() == ID) {
return true;
}
}
return false;
}
int Node::updateSeqNeighbours() {
if (nodePrev == NULL) {
return 1;
} else {
Node seed = *nodePrev; //previous node as seed
seqNeighbours.push_back(seed);
for (int i = 0; i < 3; i++) {
if (seed.nodePrev == NULL) break;
double minDist = 15353453;
Node closest;
for (int j = 0; j < seed.seqNeighbours.size(); j++) {
double dist = distanceTo(seed.seqNeighbours[j]);
if (dist < minDist) {
minDist = dist;
closest = seed.seqNeighbours[j];
}
}
if (minDist < 150) {
seqNeighbours.push_back(closest);
}
seed = closest;
}
cout << "neighbours = " << seqNeighbours.size() << endl;
}
return 0;
}
void Node::_setDefaults() {
x = 0;
y = 0;
ID = count;
nodePrev = NULL;
seqNeighbours.clear();
count++;
}
#endif /* GRAPH_NODE_H_ */
Graph:
#ifndef GRAPH_GRAPH_H_
#define GRAPH_GRAPH_H_
#include <vector>
#include <iostream>
#include "Node.h"
using namespace std;
class Graph {
public:
Graph();
virtual ~Graph();
vector<Node> list;
void addNode(Node& n);
void addSeqNode(Node& n);
private:
void _setDefaults();
};
Graph::Graph() {
// TODO Auto-generated constructor stub
}
Graph::~Graph() {
// TODO Auto-generated destructor stub
}
void Graph::addNode(Node& n) {
list.push_back(n);
}
void Graph::addSeqNode(Node& n) {
if (!list.empty()) {
n.nodePrev = &list.back();
}
n.updateSeqNeighbours();
list.push_back(n);
}
void Graph::_setDefaults() {
list.clear();
}
#endif /* GRAPH_GRAPH_H_ */
I suspect running out of memory causes this. However 40 nodes with each 4 neighbours doesn't sound much of a problem to me. Anyone any idea what goes wrong?
Edit:
Error in german, so I need to guess:
An exception accured in project prSimulation1.exe of class std::bad_alloc. Adress of Exception: '0x5476016'. Process was stopped.
Your seqNeighbours is vector<Node>. That means it stores the neighbours themselves, not pointers to them or their indices. The copy constructor, therefore, copies all the neighbours. Copying each neighbour, in turn, requires to copy its neighbours, which requires to copy their neighbours, and so on. Your assignment also copies all the neighbours, which requires to copy their neighbours, and so on. This means that each copy exponentially increases memory load, until the system is unable to store all the neighbours, neighbours of neigbours etc.
PS: on a side note, a vector called "list" is a bad idea. It is like a list called "vector", a set called "map", or a cat called Dog.
I've a question to ask.
So, I have a structure call Node as shown below:
struct Node
{
int xKoor, yKoor;
Node *parent;
char nodeId;
float G;
float H;
float F;
Node(int x, int y, int id, Node * par)
{
xKoor = x;
yKoor = y;
nodeId = id;
parent = 0;
}
Node(int x, int y, char id)
{
xKoor = x;
yKoor = y;
nodeId = id;
}
};
And I have list that contains elements of this structure:
list<Node*> OPEN;
This list's size varies in time.
What I need to do is to find the Node object which has the minimum F value, then pop out that object from the list.
So, I tried to write a function as shown below:
void enKucukFliNodeBul(list<Node*> OPEN)
{
list<Node*>::iterator it = OPEN.begin();
for(it = OPEN.begin(); it != OPEN.end(); it++)
{
if(it._Ptr->_Myval->F < it._Ptr->_Next->_Myval->F)
{
}
}
}
But I'm stuck. I'm new to STL. How can I solve this?
My best regards...
You can use std::min_element with a suitable comparison function for this.
bool nodeComp(const Node* lhs, const Node* rhs) {
return lhs->F < rhs->F;
}
#include <algorithm> // for std::min_element
list<Node*>::iterator it = std::min_element(OPEN.begin(), OPEN.end(), nodeComp);
This assumes that list<Node*> is std::list<Node*>, in which case you should be aware that std::list itself is a linked list.
Other useful operations, based on your comments:
Remove a minimum value node from the list and delete it:
OPEN.erase(it);
delete *it; //
You may need to perform other operations, if your nodes depend on each other.
Sort the list:
OPEN.sort(nodeComp);
use std::min_element algirithm and overload Compare function
bool compareF(Node *lhs, Node *rhs)
{
return lhs->F < rhs->F;
}
if you are using C++03:
std::<Node*>::itertor ter = std::min_element(OPEN.begin(),OPEN.end(), compareF);
if you are using C++11:
auto iter = std::min_element(OPEN.begin(),OPEN.end(), compareF);
To sort the list, you can call OPEN.sort(compareF); to sort your list with compareF function
Try adding this:
bool compare_node_F(Node* n1, Node* n2)
{
return n1-> F< n2-> F;
}
#include <list>
#include <algorithm>
#include <cstdlib>
#include <iostream>
int main()
{
std::list<Node*> nodes;
for(int i= 100; i--;)
{
Node* n= new Node(42, 42, 42);
n-> F= i;
nodes.push_back(n);
}
std::list<Node*>::iterator min_element_iter= std::min_element(nodes.begin(), nodes.end(), compare_node_F);
std::cout<< "Min F: "<< (*min_element_iter)-> F<< '\n';
for(std::list<Node*>::iterator d= nodes.begin(); d!= nodes.end(); ++ d)
delete *d;
}
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;
}
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.