Related
I'm trying to build a graph class where the graph is represented by adjacency lists. The graph itself is a vector of pointers where each pointer points to a linked list of nodes. For whatever reason, when I use the print graph function the program outputs nothing. Can anyone show me what I am doing wrong and perhaps where my misunderstanding of pointers is? Thanks in advance!
#include <array>
#include <vector>
#include <tuple>
#include <unordered_map>
class Node
{
public:
int vertex;
int value;
Node* next;
Node(int ver)
{
vertex = ver;
};
};
class Graph
{
public:
int n_nodes;
std::unordered_map<int,Node*> graph;
Graph(int n)
{
n_nodes = n;
for(int i=0;i<n;i++)
{
graph.insert({i,nullptr});
};
};
void add_edge(int src,int des,int val)
{
Node node_des = Node(des);
node_des.value = val;
node_des.next = graph[src];
graph[src] = &node_des;
Node node_src = Node(src);
node_src.value = val;
node_src.next = graph[des];
graph[des] = &node_src;
};
void print_graph()
{
for(int i =0; i<n_nodes;i++)
{
std::string str = "Head "+std::to_string(i);
Node node = *graph[i];
while (&node != nullptr)
{
str=str+" -> "+std::to_string(node.vertex);
node = *(node.next);
};
std::cout<<str<<std::endl;
};
};
};
int main()
{
Graph g = Graph(6);
g.add_edge(0,1,3);
g.add_edge(2,1,4);
g.add_edge(0,4,1);
g.add_edge(4,5,6);
g.add_edge(5,3,2);
g.add_edge(4,3,3);
g.add_edge(3,2,5);
g.add_edge(4,1,1);
g.add_edge(3,1,2);
g.print_graph();
return 0;
}```
If it´s possible, you may just use vector of vector instead of linked lists and not use pointers at all. Because memory cache some insertions in vectors operations may be faster than linked lists, a structure like :
struct Node2 {
int vertex;
int value;
};
struct Edge2 {
int src, des, value;
};
struct Graph2 {
int n_nodes;
std::vector<std::vector<Node2>> graph;
void add_edge(Edge2 edge) {
graph[edge.src].emplace_back(edge.des, edge.value);
graph[edge.des].emplace_back(edge.src, edge.value);
}
void add_edge(std::initializer_list<Edge2> edges)
{
std::for_each(edges.begin(), edges.end(), [this](auto &e) { add_edge(e); });
};
}
Endup bening easier and faster than linked lists;
https://quick-bench.com/q/cmX2-2IYA873TR4qn5aV4ijjUQo
Made these changes thanks to #drescherjm. The issue was that I had created a local variable and referenced its address instead of explicitly creating a pointer and setting it to a new node instance where the object's lifetime is dynamically controlled.
#include <bits/stdc++.h>
#include <array>
#include <vector>
#include <tuple>
#include <unordered_map>
class Node
{
public:
int vertex;
int value;
Node* next;
Node(int ver)
{
vertex = ver;
};
};
class Graph
{
public:
int n_nodes;
std::unordered_map<int,Node*> graph;
Graph(int n)
{
n_nodes = n;
for(int i=0;i<n;i++)
{
graph.insert({i,nullptr});
};
};
void add_edge(int src,int des,int val)
{
Node * node_des = new Node(des);
node_des->value = val;
node_des->next = graph[src];
graph[src] = node_des;
Node * node_src = new Node(src);
node_src->value = val;
node_src->next = graph[des];
graph[des] = node_src;
};
void print_graph()
{
for(int i =0; i<n_nodes;i++)
{
std::string str = "Head "+std::to_string(i);
Node * node_ptr = graph[i];
while (node_ptr != nullptr)
{
str=str+" -> "+std::to_string(node_ptr->vertex);
node_ptr = node_ptr->next;
};
std::cout<<str<<std::endl;
};
};
};
int main()
{
Graph g = Graph(6);
g.add_edge(0,1,3);
g.add_edge(2,1,4);
g.add_edge(0,4,1);
g.add_edge(4,5,6);
g.add_edge(5,3,2);
g.add_edge(4,3,3);
g.add_edge(3,2,5);
g.add_edge(4,1,1);
g.add_edge(3,1,2);
g.print_graph();
return 0;
}
I am trying to implement kruskal's algo. togather with bfs and dfs. i wrote my code to print the adjancey list and to show the bfs and dfs and now i am facing problem with writing the code for kruskal's algorithm i kind of newbie in using maps and templates. i don't know how to pass the values in the kruskals algorithm and i m constantly getting errors.
here is the code that i have written.
#include<iostream>
#include<map>
#include<queue>
#include<list>
#include<cstring>
#include<algorithm>
using namespace std;
template<typename T>
class Graph{
private:
map<T,list<pair<T,int>>> l;
void DFSHelper(T node,map<T,bool> &visited){
cout<<node<<" -> ";
visited[node]=true;
for(auto neighbours:l[node]){
if(!visited[neighbours.first]){
DFSHelper(neighbours.first,visited);
}
}
}
public:
void add(T A,T B,bool bi,int wi){
l[A].push_back(make_pair(B,wi));
if(bi == true){
l[B].push_back(make_pair(A,wi));
}
}
void print(){
for(auto c:l){
int c1 = c.first;
list<pair<int,int>> n = c.second;
cout<<c1<<" -> ";
for(auto k:n){
int dest = k.first;
int dist = k.second;
cout<<dest<<"("<<dist<<") ";
}
cout<<endl;
}
}
void bfs(T src){
map<T,bool> visited;
queue<T> q;
q.push(src);
visited[src] = true;
while(!q.empty()){
T node = q.front();
q.pop();
cout<<node<<" -> ";
for(auto children:l[node]){
if(!visited[children.first]){
visited[children.first]=true;
q.push(children.first);
}
}
}
}
void dfs(T src){
map<T,bool> visited;
int component = 1;
DFSHelper(src,visited);
}
void cmp(T src,T end){
return src.second.second<end.second.second;
}
void kruskals(){
}
};
int main(){
Graph<int> g;
g.add(1,2,true,20);
g.add(1,3,true,30);
g.add(2,4,true,50);
g.add(3,4,true,10);
g.add(4,5,true,60);
g.add(5,1,false,35);
g.print();
cout<<endl;
cout<<"BFS :- ";
g.bfs(1);
cout<<endl;
cout<<"DFS :- ";
g.dfs(1);
g.kruskals();
}
Your graph appears to be directed due to the uni-directional edge 5->1. Kruskal's algorithm only works for undirected graphs. (Why?)
In Kruskal's algorithm you need the edges sorted in non-decreasing order of edge-weights. Hence you can either maintain an extra data structure alongwith the map l and insert to it in the add() function or create it in the kruskals() function itself.
Next you need a data structure to query if any two nodes of the graph belong to two different components or not. Here two nodes are said to be in the same component if you can reach one node to the other by only considering edges encountered till that particular iteration of the Kruskal's algorithm. A Disjoint Set Union can do that efficiently.
Here is an implementation, where I use the set edge_weights to store the edges sorted by weight:
#include<iostream>
#include<map>
#include<queue>
#include<list>
#include<cstring>
#include<algorithm>
#include <set> // Added
using namespace std;
template<typename T>
class DisjointSetUnion {
map<T, T> parent;
map<T, int> sz; // stores sizes of component
public:
void make_set(T v) {
parent[v] = v;
}
T find_set(T x) {
if(x != parent[x]) parent[x] = find_set(parent[x]);
return parent[x];
}
void merge_sets(T x, T y) {
int px = find_set(x), py = find_set(y);
if(sz[px] > sz[py]) parent[py] = px;
else parent[px] = py;
if(sz[py] == sz[px]) sz[py]++;
}
};
template<typename T>
class Graph{
private:
map<T,list<pair<T,int>>> l;
set<pair<int, pair<T, T>>> edge_weights; // no parallel (or duplicate) edges exist
void DFSHelper(T node,map<T,bool> &visited){
cout<<node<<" -> ";
visited[node]=true;
for(auto neighbours:l[node]){
if(!visited[neighbours.first]){
DFSHelper(neighbours.first,visited);
}
}
}
public:
void add(T A,T B,bool bi,int wi){
l[A].push_back(make_pair(B,wi));
if(bi == true){
l[B].push_back(make_pair(A,wi));
edge_weights.insert(make_pair(wi, make_pair(A, B))); // Added
}
}
void print(){
for(auto c:l){
int c1 = c.first;
list<pair<int,int>> n = c.second;
cout<<c1<<" -> ";
for(auto k:n){
int dest = k.first;
int dist = k.second;
cout<<dest<<"("<<dist<<") ";
}
cout<<endl;
}
}
void bfs(T src){
map<T,bool> visited;
queue<T> q;
q.push(src);
visited[src] = true;
while(!q.empty()){
T node = q.front();
q.pop();
cout<<node<<" -> ";
for(auto children:l[node]){
if(!visited[children.first]){
visited[children.first]=true;
q.push(children.first);
}
}
}
}
void dfs(T src){
map<T,bool> visited;
int component = 1;
DFSHelper(src,visited);
}
void cmp(T src,T end){
return src.second.second<end.second.second;
}
void kruskals(){
DisjointSetUnion<int> dsu;
// make singleton components of each node
for(auto it: l) {
T u = it.first;
dsu.make_set(u);
}
// iterate over all edges in sorted order
for(auto ed: edge_weights) {
int w = ed.first;
T u = ed.second.first, v = ed.second.second;
// if they belong to different components then they are
// part of the MST, otherwise they create a cycle
if(dsu.find_set(u) != dsu.find_set(v)) {
// this edge is part of the MST, do what you want to do with it!
cout << "(" << u << "," << v << "," << w << "), ";
// merge the two different components
dsu.merge_sets(u, v);
}
}
}
};
int main(){
Graph<int> g;
g.add(1,2,true,20);
g.add(1,3,true,30);
g.add(2,4,true,50);
g.add(3,4,true,10);
g.add(4,5,true,60);
// Removed unidirectional edge below
// g.add(5,1,false,35);
g.print();
cout<<endl;
cout<<"BFS :- ";
g.bfs(1);
cout<<endl;
cout<<"DFS :- ";
g.dfs(1);
cout << endl;
cout << "Edges in MST (u,v,w): ";
g.kruskals();
cout << endl;
}
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.
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.