void printPath(Node *start, Node *end){
Node *nextNode = end;
Node *currentNode = end;
if (end->previous != NULL){
nextNode = start;
while(nextNode != end){
currentNode = nextNode;
nextNode = nextNode->next;
printf("%d %s -> %s\n", *(int*)getEdgeValue(currentNode->nextEdge),
(char*) currentNode->value, (char*) nextNode->value);
}
printf("Minuter: %d\n", end->currentCost);
}
}
void fastestPath(Graph *g, Node *start, Node *end){
start->currentCost = 0; //mark start as visited;
nodeEdges(g, start);
Node *previousNode = end;
Node *nextNode = end;
Node *currentNode = end;
while(previousNode != start){
nextNode = previousNode;
previousNode = previousNode->previous;
previousNode->next = nextNode;
previousNode->nextEdge = nextNode->previousEdge;
}
nextNode = start;
while(nextNode != end){
currentNode = nextNode;
nextNode = nextNode->next;
printf("%d %s -> %s\n", *(int*)getEdgeValue(currentNode->nextEdge),
(char*) currentNode->value, (char*) nextNode->value);
}
printf("Minuter: %d\n", end->currentCost);
}
void Graph::doDijkstra( unsigned int originNode, unsigned int destNode, std::list<unsigned int> *pathList, int &pathCost)
{
bool validRouteFoundToDestination = false;
unsigned int closedNodeNum = originNode;
// std::cout << "---Running shortest path algorithm from node "<<originNode << " to node " << destNode << std::endl;
// initialize the outcome
pathCost = 0;
pathList->clear();
// special case for origin == destination, just return
if(originNode == destNode)
{
return;
}
// before starting, clean the nodes of computed values in case we're re-running the algorythm
for(std::map<int, graphPoint* >::iterator itGraphNode = graphNodes.begin(); itGraphNode != graphNodes.end(); ++itGraphNode)
{
itGraphNode->second->cleanNode();
}
//
// run the shortest path algorithm on the graph passed in
//
makeOriginNode(originNode);
std::vector<unsigned int> openSet; // a set of all "not yet visited" Nodes
while(true)
{
int numNodesAddedToOpenSet = 0;
// std::cout << "New closed node: " << closedNodeNum << std::endl;
//
// add the connected nodes from this node to the open set (Step N+1)
//
std::map<int, graphPoint* >::iterator itGraphNode = graphNodes.find(closedNodeNum);
unsigned int closedNodeCost = itGraphNode->second->m_totalCost;
// std::cout << "The closed node cost is "<< closedNodeCost << std::endl;
// get all the nodes connected to this one and
// adjust the costs and (from node) values of each connected node
// itGraphEdge->first is the connected node number and itGraphEdge->second is the edge cost
for(std::map<unsigned int, unsigned int>::iterator itGraphEdge= itGraphNode->second->m_edges.begin();
itGraphEdge != itGraphNode->second->m_edges.end();
++itGraphEdge)
{
// first, mark this node as visited
itGraphNode->second->setVisited();
// std::cout << "Examining node " << itGraphEdge->first << std::endl;
std::map<int, graphPoint* >::iterator itNextEdgeNode = graphNodes.find(itGraphEdge->first);
if(itNextEdgeNode == graphNodes.end()) continue; // no node actually exists
int i;
for(i=0; i<openSet.size(); i++)
{
if(openSet[i] == itNextEdgeNode->second->m_nodeNumber) break;
}
// add it to the open set if it's not there already and it hasn't already been visited
if(i == openSet.size() && (itNextEdgeNode->second->getVisited() == false))
{
openSet.push_back(itNextEdgeNode->second->m_nodeNumber);
numNodesAddedToOpenSet++;
}
// now see if this is a lower cost path to this connected node
if(itNextEdgeNode->second->getPointCost() == (-1) ||
(closedNodeCost + itGraphEdge->second) < itNextEdgeNode->second->getPointCost())
{
// new lower cost found
itNextEdgeNode->second->setPointCost(static_cast<int>(closedNodeCost + itGraphEdge->second));
itNextEdgeNode->second->m_viaNode = closedNodeNum;
// std::cout << "found new lower cost (" << itNextEdgeNode->second->getPointCost()
// << ") to node " << itGraphEdge->first << " from node "
// << closedNodeNum << std::endl;
}
}
// std::cout << "Openset now has " << openSet.size() << " members " << std::endl;
//if we didn't add any new members to the openset and it's empty, we are done
if ( (openSet.size() == 0) && (numNodesAddedToOpenSet == 0) )
{
// std::cout << "openset is empty" << std::endl;
break;
}
// Now, find the lowest cost member of the open set and make it the new closed set member
unsigned int lowest_cost_node;
unsigned int lowest_cost_index;
for (int i=0; i<openSet.size(); i++)
{
int lowest_cost_val;
std::map<int, graphPoint* >::iterator itGraphNode = graphNodes.find(openSet[i]);
// std::cout << "i: " << i << " openset[i]: " << openSet[i] <<" nodenum: " <<
// itGraphNode->second->m_nodeNumber << " cost: " << itGraphNode->second->m_totalCost
// << std::endl;
if((i==0) || (itGraphNode->second->m_totalCost < lowest_cost_val ))
{
lowest_cost_node = itGraphNode->second->m_nodeNumber;
lowest_cost_val = itGraphNode->second->m_totalCost;
lowest_cost_index = i;
// std::cout << "found new lowest cost node : " << lowest_cost_node << std::endl;
}
}
// this is the node that we'll be evaluating in the next iteration
closedNodeNum = lowest_cost_node;
// remove that node from the open set
// std::cout << "deleting node: " << lowest_cost_node << std::endl;
for(std::vector<unsigned int>::iterator vit = openSet.begin(); vit != openSet.end(); ++vit)
{
// std::cout << "lowest_cost_node is: " << lowest_cost_node << " and found openSet member number " << *vit << std::endl;
if (*vit == lowest_cost_node)
{
// std::cout << "erasing member!" << std::endl;
openSet.erase(vit);
break;
}
}
// std::cout << "Post delete: Openset now has " << openSet.size() << " members " << std::endl;
//if that node is the dest node, we have succeeded and we are done
if (closedNodeNum == destNode)
{
validRouteFoundToDestination = true;
// std::cout << "***Found a route to the destination node!" << std::endl;
break;
}
//else keep on truckin'
}
//
// Now tell the user whether or not we've been able to find a route
// If so, print out the route
// else, well..there's not much we can do except break the bad news
//
if(validRouteFoundToDestination == true)
{
std::vector<unsigned int> routeSet;
// now print out the route
unsigned int routeNode = destNode;
unsigned int lastRouteNode = routeNode;
// reuse the openSet vector to record the final route
pathCost = 0;
while(true)
{
pathList->push_front(routeNode);
// if we've added the orig to the route record, then we're done
if(routeNode == originNode) break;
// std::cout << "route reclaimation looking for node " << routeNode << std::endl;
// std::cout << "route reclaimation found node " <<
// graphNodes.find(routeNode)->second->m_nodeNumber << std::endl;
// remember the "from" node for the cost computation
lastRouteNode = routeNode;
// find the "next" node
routeNode = graphNodes.find(routeNode)->second->m_viaNode;
// std::cout << "route reclaimation found viaNode..." << routeNode << std::endl;
// record this hop in the total cost of the path
// std::cout << "Cost of route from " << routeNode << " to " << lastRouteNode <<
// " is " << getEdgeValue(routeNode, lastRouteNode) << std::endl;
pathCost += getEdgeValue(routeNode, lastRouteNode);
}
}
else
{
// std::cout << "Count not find a route from " << originNode << " to " << destNode << std::endl;
pathList->clear(); // no elements in the list
pathCost = -1;
}
}
