/* * Method: aStar * Parameters: BasicGraph graph by reference * Vertex pointer variable for path start * Vertex pointer variable for path end * - - - - - - - - - - - - - - - - - - - - - - - - - - * Returns a Vector of Vertex pointer variables for which * each index corresponds to a step in the path between * the path's start and end points, if any exists. * This function uses A*'s algorithm, which evaluates * the estimated total cost of all neighbors' paths to the end * from the starting vertex before continuing. It differs from Dijkstra's * in its prioritization of location based on their estimated total cost * or distance from the starting point, as opposed to the current cost * up to that point. Because A* also orients subsequent vertices * to their earlier parent, this function uses a helper function to * rewind the shortest route from end to start, if applicable, * returning an empty vector if not. */ Vector<Vertex*> aStar(BasicGraph& graph, Vertex* start, Vertex* end) { graph.resetData(); Vector<Vertex*> path; PriorityQueue<Vertex*> pqueue; pqueue.enqueue(start, heuristicFunction(start, end)); while(!pqueue.isEmpty()) { Vertex* current = pqueue.dequeue(); visitVertex(current); if(BasicGraph::compare(current, end) == 0) break; for(Edge* edge : current->edges) { double cost = current->cost + edge->cost; if(!edge->finish->visited && edge->finish->getColor() != YELLOW) { enqueueVertex(edge->finish, current, cost); pqueue.enqueue(edge->finish, cost + heuristicFunction(edge->finish, end)); } if(edge->finish->getColor() == YELLOW && cost < edge->finish->cost) { enqueueVertex(edge->finish, current, cost); pqueue.changePriority(edge->finish, cost + heuristicFunction(edge->finish, end)); } } } determinePath(start, end, path); return path; }
int main(){ PriorityQueue* pr = new PriorityQueue(); pr->insertElem(4,13); pr->insertElem(3,5); pr->insertElem(2,9); pr->insertElem(1,2); for(int i=0 ; i<pr->size() ;i++){ printf("%d\n", pr->minQueue[i]->priority ); } if( pr->contains(4) ) { pr->changePriority(4,1); } while(!pr->isEmpty()){ Element * elem = pr->minPriority(); printf("(%d,%d)\n",elem->vertex,elem->priority); } }
Vector<Vertex*> dijkstrasAlgorithm(BasicGraph& graph, Vertex* start, Vertex* end) { graph.resetData(); PriorityQueue<Vertex*> pqueue; Vector<Vertex*> path; pqueue.enqueue(start, 0); start->cost = 0.0; start->setColor(YELLOW); while(!pqueue.isEmpty()) { Vertex* v = pqueue.dequeue(); v->setColor(GREEN); if (v == end) break; for (auto edge : v->edges) { Vertex* next_v = edge->finish; if (next_v->getColor() == 0) { next_v->setColor(YELLOW); double next_cost = v->cost + edge->cost; next_v->previous = v; next_v->cost = next_cost; pqueue.enqueue(next_v, next_cost); } else if (next_v->getColor() == YELLOW) { double next_cost = v->cost + edge->cost; if (next_cost < next_v->cost) { next_v->cost = next_cost; next_v->previous = v; pqueue.changePriority(next_v, next_v->cost); } } } } if (end->getColor() == GREEN) { Vertex* path_v = end; while (path_v->previous) { path.insert(0, path_v); path_v = path_v->previous; } path.insert(0, path_v); } return path; }
/* * A* search. Will explore from 'end' until it finds 'start' or can determine that no valid path exists. * It explores in reverse to make it easier to build path array. */ vector<Node *> aStar(BasicGraph& graph, Vertex* start, Vertex* end) { graph.resetData(); vector<Vertex*> path; PriorityQueue<Vertex*> openQueue; //Setup starting state end->cost = 0; end->visited = true; openQueue.enqueue(end, end->cost); Vertex* current = nullptr; //While there are still reachable nodes to explore while(!openQueue.isEmpty()){ //Set current node to the next node in the queue current = openQueue.dequeue(); current->setColor(GREEN); //If current node is target, build path and return if(current == start){ rewind(end, start, path); return path; } //Add any unvisited neighbor to queue, adjust cost for already visited neighbor nodes for(Vertex* neighbor : graph.getNeighbors(current)){ if(!neighbor->visited){ neighbor->previous = current; neighbor->visited = true; neighbor->cost = current->cost + graph.getArc(current, neighbor)->cost; openQueue.enqueue(neighbor, neighbor->cost + neighbor->heuristic(start)); neighbor->setColor(YELLOW); }else{ double newCost = current->cost + graph.getArc(current, neighbor)->cost; if(neighbor->cost > newCost){ neighbor->previous = current; neighbor->cost = newCost; openQueue.changePriority(neighbor, neighbor->cost + neighbor->heuristic(start)); neighbor->setColor(YELLOW); } } } } return path; }
/* * Function: aStar * -------------------- * This function implements aStar search (variation of dijkstras with a heuristic) * * Preconditions: * * @param: graph: The graph to be searched * start: The start vertex * end:The end vertex * @return: returns a vector of vertexes containing the path (empty if no path) */ Vector<Vertex*> aStar(BasicGraph& graph, Vertex* start, Vertex* end) { graph.resetData(); // reset Vector<Vertex*> path; // the return path Set<Vertex*> inQueue; // A vector holding the values that we have already put into the queue // INitialize every vertex to have infinite cost for(Vertex* s:graph.getVertexSet()){ s->cost = POSITIVE_INFINITY; } PriorityQueue<Vertex*> pq; // A pq to see where to search next (cost+heuristic = priority) // Set start's cost to 0 + heuristicFxn val start->cost = 0 + heuristicFunction(start,end); // Enqueue start vertex pq.enqueue(start,0); start->setColor(YELLOW); inQueue.add(start); // While there are still elements in the pq to try while(!pq.isEmpty()){ Vertex* v = pq.dequeue(); inQueue.remove(v); v->visited = true; v->setColor(GREEN); // We can stop (reached end) if(v == end){ // We have reached the end, deconstruct path.clear(); Vertex* temp = v; path.insert(0,temp); while(temp->previous!=NULL){ // deconstruct temp = temp->previous; path.insert(0,temp); } break; } // For each unvisited neighbor of v vertex for (Vertex* n: graph.getNeighbors(v)){ // And it's unvisited if(n->visited == false){ // v's cost plus weight of edge Edge* e = graph.getEdge(v,n); double edgeCost = e->cost; double costToVertexN = v->cost + edgeCost; // If the costToVertexN < n->cost, we know this is a better way to get to // vertex n if(costToVertexN < n->cost){ n->cost = costToVertexN; n->previous = v; n->setColor(YELLOW); // Check to see if pq already contains n if(inQueue.contains(n)){ // Priority now includes heuristic pq.changePriority(n,costToVertexN + heuristicFunction(n,end)); } else { pq.enqueue(n,costToVertexN + heuristicFunction(n,end)); } } } } } return path; }
//this is the same as dijkstrasAlgorithm except for one thing which is commented vector<Node *> aStar(BasicGraph& graph, Vertex* start, Vertex* end) { graph.resetData(); Vertex* unDefined; Vertex* currentNode; start->cost=0.0; PriorityQueue<Vertex*> vertexPrioQueue; map<Vertex*,Vertex*> predecessor; for (Node *node :graph.getNodeSet()){ if (node!=start){ node->cost=INFINITY; predecessor[node]=unDefined; } vertexPrioQueue.enqueue(node,node->cost);; } double alt; while (!vertexPrioQueue.isEmpty()){ currentNode= vertexPrioQueue.front(); vertexPrioQueue.dequeue(); currentNode->setColor(YELLOW); currentNode->visited=true; if (currentNode==end){ break; } for(Node *node :graph.getNeighbors(currentNode)){ if (!node->visited){ alt=currentNode->cost+graph.getArc(currentNode,node)->cost; if (alt<node->cost){ node->cost=alt; predecessor[node]=currentNode; //this is the change, the queuepriority comes from the node cost + the heuristic vertexPrioQueue.changePriority(node,node->cost+node->heuristic((end))); } } } currentNode->setColor(GREEN); } if(predecessor[end]==unDefined){ vector<Vertex*> path; return path; } else{ stack<Vertex*> vertexStack; vector<Vertex*> path; currentNode=end; while (currentNode!=start){ vertexStack.push(currentNode); currentNode=predecessor[currentNode]; } vertexStack.push(start); while (!vertexStack.empty()){ path.push_back(vertexStack.top()); vertexStack.pop(); } return path; } }
vector<Node *> dijkstrasAlgorithm(BasicGraph& graph, Vertex* start, Vertex* end) { graph.resetData(); //set as predescessor if that is undefined Vertex* unDefined; //the current node we are checking Vertex* currentNode; //sets startnode cost to zero start->cost=0.0; //create prioqueue PriorityQueue<Vertex*> vertexPrioQueue; //used to keep track of all predeccesors map<Vertex*,Vertex*> predecessor; //set all costs, sets predecessor and adds the to the queue for (Node *node :graph.getNodeSet()){ //all nodes but start should have infinity cost if (node!=start){ node->cost=INFINITY; predecessor[node]=unDefined; } //add all nodes to queue vertexPrioQueue.enqueue(node,node->cost); } //keep track of the alternative cost double alt; //while the queue is not empty while (!vertexPrioQueue.isEmpty()){ //put current node to the one with highest priority currentNode= vertexPrioQueue.front(); vertexPrioQueue.dequeue(); currentNode->setColor(YELLOW); currentNode->visited=true; if (currentNode==end){ break; } //check all the node's neighbors for(Node *node :graph.getNeighbors(currentNode)){ //if we have not visited that node if (!node->visited){ //we check the alternative cost alt=currentNode->cost+graph.getArc(currentNode,node)->cost; //if the alternative cost is lower then we set that to our new cost if (alt<node->cost){ node->cost=alt; predecessor[node]=currentNode; vertexPrioQueue.changePriority(node,alt); } } } currentNode->setColor(GREEN); } //if we havent found end if(predecessor[end]==unDefined){ vector<Vertex*> path; return path; } else{ //if we have found end we trace through the predecessor map to find the path stack<Vertex*> vertexStack; vector<Vertex*> path; currentNode=end; while (currentNode!=start){ vertexStack.push(currentNode); currentNode=predecessor[currentNode]; } vertexStack.push(start); while (!vertexStack.empty()){ path.push_back(vertexStack.top()); vertexStack.pop(); } return path; } }