/*
* 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;
}
}
