int Dijkstra(NodeRef start, int want, color color){ //to find distance, create node.prev which contains the prev node.you can go backwards and add up all nodes.
priority_queue<NodeRef, vector<NodeRef>, Compare> openSet;
vector<NodeRef> closedSet;
openSet.push(start); //add initial node to open set
while(openSet.size()!=0){
NodeRef current = openSet.top(); //node with least cost
closedSet.push_back(current);
current -> visited = true;
if(current->GetId() == want){ //if wanted node is found
return CalculateCost(edgeList.at(want));
}
openSet.pop(); //remove current node from open set
if(current->GetCost()==INT_MAX){ //current node has no connections
cout<<"No connections found, break"<<endl;
break;
}
for(int link=0; link<(current)->links.size(); link++){
if((!current->links.at(link)->visited) && current->links.at(link)->color == color){ //if unvisited and correct color
current->links.at(link)->Visited(true);
current->links.at(link)->prev = current;
int alt = CalculateCost(current) + (CalculateCost(current->links.at(link)) - CalculateCost(current)); //relaxing the edge
if(alt<CalculateCost(current)){ //if found a faster way
(current->links.at(link))->cost = alt;
}
openSet.push(current->links.at(link));
}
}
}
return INT_MAX; //failed to find node
}
int CalculateCost(NodeRef n){ //run backwards up the list to calculate total cost//works
int cost = 0;
while(n->prev!=NULL){
cost = cost+ n->GetCost();
n = n->prev;
}
return cost;
}
/*Uses stack to run backwards and calculate cost from start to this node. Used for Dijkstra.*/
int CalculateCost(NodeRef n){
int cost = 0;
while(n->prev!=NULL){
cost = cost+ n->GetCost();
n=n->prev;
}
return cost;
}
bool operator() (NodeRef a, NodeRef b){
return a->GetCost()<b->GetCost();
}
