//----------------------------------------------------------------
//FindShortestPath.cpp
//
//Author: Hao Han
//
/*Note:
// I get an array to store all vertices in the terrain. //vertexArray[];
//
*/
#include "findShortestPath.h"
#include <iostream>
#include <float.h>
#include <assert.h>
#include <stack>
using namespace std;
#define min(a, b) a < b ? a : b
#define R 5
Robot& Robot()
{
return *(OpenGLRobot::instance);
}
//transform coordinate to the Index of vertexArray
int transCoordToIndex(const coord_t &coord)
{
int temp = (coord.first + coord.second * Robot().getTerrainXSize() );
return temp;
}
//transform the Index of vertexArray to coordinate
coord_t transIndexToCoord(const int &index)
{
coord_t temp;
int length = Robot().getTerrainXSize();
temp.first = index % length;
temp.second = index / length;
return temp;
}
//initial array.
void initialGraph(Vertex * vertexArray, int size)
{
for(int i = 0; i < size; i++)
vertexArray[i].init(i, DBL_MAX); // initial array. set the index and weight(infinite);
}
//--------------------------------------------------------------------
//Dynamic Programming
//Author: Hao Han
//
//Get the shortest path from anyone vertexes to start_Vertex.
//
//
//It does work, but it is inefficience.
//Cost more than 6 mins to find the shortest path in 64X64 terrain.
double DynamicProgram(Vertex * vertexArray, coord_t start_t, coord_t end_t)
{
double dist(DBL_MAX);
double weight, minDist(DBL_MAX);
int startIndex, endIndex, tempIndex;
startIndex = transCoordToIndex(start_t);
endIndex = transCoordToIndex(end_t);
if(start_t == end_t)
{
dist = 0;
vertexArray[startIndex].setWeight(dist); //set distance = 0;
return dist;
}
coord_t temp1_t, temp2_t;
tempIndex = endIndex;
while(tempIndex >= startIndex)
{
--tempIndex;
temp1_t = transIndexToCoord(tempIndex);
weight = Robot().getCostOfMove(temp1_t.first, temp1_t.second, end_t.first, end_t.second);
dist = min(dist, DynamicProgram(vertexArray, start_t, temp1_t)+ weight);
vertexArray[endIndex].setWeight(dist);
}
return dist;
}
//--------------------------------------------------------------------
//Dijkstra's algorithm - check all vertices to start_Vertex, and find the best path.
//Author: Hao Han
//
//It does work. But it is also inefficiece.
//Cost 3min 20sec to find the shortest path in 64X64 terrain.
void Dijkstra(Vertex * vertexArray, coord_t start_t, coord_t end_t)
{
//transform coord to index number of vertexArray.
int startIndex = transCoordToIndex(start_t);
int endIndex = transCoordToIndex(end_t);
vertexArray[startIndex].init(startIndex,0, false, startIndex);// inital the start vertex.
int size = Robot().getTerrainXSize() * Robot().getTerrainYSize();
//Create a STL min_priority_queue to store gcd_point;
priority_queue<Vertex, vector<Vertex>,less<vector<Vertex>::value_type> > gcdPQ;
int tempIndex(startIndex);
gcdPQ.push(vertexArray[tempIndex]);
while(!gcdPQ.empty())
{
Vertex tempVer;
tempVer = gcdPQ.top();
gcdPQ.pop();
coord_t temp1 = transIndexToCoord(tempVer.getIndex());
double tempDist;
if (!vertexArray[tempVer.getIndex()].bVisited())
{
vertexArray[tempVer.getIndex()].visit(); //set the visited flag.
// hao test
for(int j = tempVer.getIndex() +1; j < size; j++) //visit all vertices
//for(int j = 0; j < size; j++) //visit all vertices
{
coord_t temp2 = transIndexToCoord(j);
double weight = Robot().getCostOfMove(temp1.first, temp1.second, temp2.first, temp2.second);
tempDist = weight + tempVer.getWeight();
if(tempDist < vertexArray[j].getWeight()) //find vertex in the best path.
{
vertexArray[j].setWeight(tempDist);
vertexArray[j].setPrevIndex(tempVer.getIndex());
gcdPQ.push(vertexArray[j]);
}
}
}
}
}
//--------------------------------------------------------------------
//SSSP algorithm
//Set the neighbour hood of distance R around(0,0). It's a square.(a circle in the silde)
//
//The best algorithm for big size terrain.
//Cost no more than 8secs to find the shortest path in 64X64 terrain.
void SSSP(Vertex * vertexArray, coord_t start_t, coord_t end_t)
{
//transform coord to index number of vertexArray.
int startIndex = transCoordToIndex(start_t);
int endIndex = transCoordToIndex(end_t);
//get Terrain Size;
int length = Robot().getTerrainXSize();
int width = Robot().getTerrainYSize();
int size = length * width;
// inital the start vertex.
vertexArray[startIndex].init(startIndex,0, false, startIndex);
//Create a priority_queue to store gcd_point;
//priority_queue<Vertex, vector<Vertex>,less<vector<Vertex>::value_type> > gcdPQ;
priority_queue<Vertex> gcdPQ;
int tempIndex(startIndex);
double dist(DBL_MAX);
//push the start vertex into Priority_queue;
gcdPQ.push(vertexArray[tempIndex]);
while(!gcdPQ.empty())
{
Vertex centerVer = gcdPQ.top();
gcdPQ.pop();
coord_t center_Coord = transIndexToCoord(centerVer.getIndex());//the center vertex's coord.
coord_t gcd_Coord;//the gcd_vertex's coord;
int gcd_Index; // the gcd_vertex's index number in VertexArray;
//check the center_vertices which is visited.
if (!vertexArray[centerVer.getIndex()].bVisited())
{
vertexArray[centerVer.getIndex()].visit(); //set visited flag;
//get gcd_vertex set.
for ( int a = -R; a <= R; a++)
{
//check the gcdVertex which is not out of Terrian
gcd_Coord.first = center_Coord.first +a;
if((gcd_Coord.first < 0) || (gcd_Coord.first > length))
continue;
for (int b = -R; b <= R; b++)
{
//check the gcdVertex which is not out of Terrain
gcd_Coord.second = center_Coord.second +b;
if((gcd_Coord.second < 0) || ( gcd_Coord.second> width))
continue;
//get the weight from center_Coord to gcd_coord;
double weight = Robot().getCostOfMove(center_Coord.first, center_Coord.second, gcd_Coord.first, gcd_Coord.second);
if (gcd(abs(a), abs(b)) == 1 && (weight < DBL_MAX))
{
gcd_Index = transCoordToIndex(gcd_Coord);
//get the distance from start_coord to gcd_coord;
dist = weight + centerVer.getWeight();
//if dist is min_dist, set it and push it into the gcd_PriorityQueue.
if(dist < vertexArray[gcd_Index].getWeight())
{
vertexArray[gcd_Index].setWeight(dist);
vertexArray[gcd_Index].setPrevIndex(centerVer.getIndex());
//check whether this point is in PriorityQueue and is visited, or not;
if((! vertexArray[gcd_Index].bVisited()) && (!checkDataInQueue(gcdPQ, vertexArray[gcd_Index])))
gcdPQ.push(vertexArray[gcd_Index]);
}
}
}
}
}
}
}
//--------------------------------------------------------------------
//AStar algorithm
//Set the neighbour hood of distance R around(0,0). It's a square.(a circle in the silde)
//
//The best algorithm for big size terrain.
//Cost no more than 8secs to find the shortest path in 64X64 terrain.
void AStar(Vertex * vertexArray, coord_t start_t, coord_t end_t)
{
//transform coord to index number of vertexArray.
int startIndex = transCoordToIndex(start_t);
int endIndex = transCoordToIndex(end_t);
//get Terrain Size;
int length = Robot().getTerrainXSize();
int width = Robot().getTerrainYSize();
int m_sizeIndex = length + width * Robot().getTerrainXSize();
// inital the start vertex.
vertexArray[startIndex].init(startIndex,0, false, startIndex);
//Create a priority_queue to store gcd_point;
//priority_queue<Vertex, vector<Vertex>,less<vector<Vertex>::value_type> > gcdPQ;
priority_queue<Vertex> gcdPQ;
int tempIndex(startIndex);
double dist(DBL_MAX);
//push the start vertex into Priority_queue;
gcdPQ.push(vertexArray[tempIndex]);
while(!gcdPQ.empty())
{
Vertex centerVer = gcdPQ.top();
gcdPQ.pop();
const int centerIndex = centerVer.getIndex();
const double centerWeight = centerVer.getWeight();
coord_t center_Coord = transIndexToCoord(centerIndex);//the center vertex's coord.
coord_t gcd_Coord;//the gcd_vertex's coord;
//check the center_vertices which is visited.
if (!vertexArray[centerIndex].bVisited())
{
vertexArray[centerIndex].visit(); //set visited flag;
//get gcd_vertex set.
for ( int a = -R; a <= R; a++)
{
//check the gcdVertex which is not out of Terrian
gcd_Coord.first = center_Coord.first +a;
if((gcd_Coord.first < 0) || (gcd_Coord.first > length))
continue;
for (int b = -R; b <= R; b++)
{
gcd_Coord.second = center_Coord.second +b;
//check the gcdVertex which is not out of Terrain
if( (gcd_Coord.second < 0) || ( gcd_Coord.second> width) )
continue;
//check index
int gcd_Index = transCoordToIndex(gcd_Coord);
if( (gcd_Index > m_sizeIndex) )
continue;
//get the weight from center_Coord to gcd_coord;
double GCost = Robot().getCostOfMove(center_Coord.first, center_Coord.second,
gcd_Coord.first, gcd_Coord.second);
if (gcd(abs(a), abs(b)) == 1 && (GCost < DBL_MAX))
{
//calculate the heuristic cost from this node to the target (H)
double HCost = heuristicCost(start_t, end_t, gcd_Coord);
//get the distance from start_coord to gcd_coord;
dist = GCost + HCost + centerWeight;
//if dist is min_dist, set it and push it into the gcd_PriorityQueue.
if(dist < vertexArray[gcd_Index].getWeight())
//if(dist != DBL_MAX)
{
//if(dist < m_vertexArray[gcd_Index].getWeight())
{
vertexArray[gcd_Index].setWeight(dist);
vertexArray[gcd_Index].setPrevIndex(centerIndex);
}
//check whether this point is in PriorityQueue and is visited, or not;
if( !checkDataInQueue(gcdPQ, vertexArray[gcd_Index]) )
gcdPQ.push(vertexArray[gcd_Index]);
if(gcd_Coord == end_t) //already get a path from start to destination;
return;
}
}
}
}
}
}
}
void find_path(int start_x, int start_y, int end_x, int end_y)
{
robot_move_to(start_x, start_y);
int length, width, size(0);
length = Robot().getTerrainXSize();
width = Robot().getTerrainYSize();
size = length*width;
Vertex * vertexArray=new Vertex[size];
// initial graph
initialGraph(vertexArray, size);
coord_t start_t(start_x, start_y);
coord_t end_t(end_x, end_y);
/*-----it does work. Cost more than 6mins to find the best path;-----*/
//DynamicProgram(vertexArray, start_t, end_t);
/*-----it does work. Cost 3mins 20secs to find the best path;-----*/
//Dijkstra(vertexArray, start_t, end_t);
/*-----The best algorithm. Cost no more than 8secs to find the best path;-----*/
//SSSP(vertexArray, start_t, end_t);
/*-----Cost no more than 8secs to find the best path;-----*/
AStar(vertexArray, start_t, end_t);
//get the vertices in the shortest path to move the robot.
stack<int> s;
int i = transCoordToIndex(end_t);
//push all vertices into the stack.
while(vertexArray[i].m_prevIndex != transCoordToIndex(start_t))
{
s.push(vertexArray[i].m_index);
i = vertexArray[i].m_prevIndex;
}
s.push(vertexArray[i].m_index);
//get the vertices from the stack and draw the shortest path.
cout << "The shortest Path:" << endl;
cout << 0;
while (!s.empty())
{
int temp = s.top();
s.pop();
cout << " -> " << temp ; // print vertices.
coord_t t = transIndexToCoord(temp);
robot_move_to(t.first, t.second);
}
cout << endl;
//print total cost;
cout << endl;
cout << "Total Distance: " << vertexArray[size-1].getWeight() << endl;
}
//Copy gcd() function from Viki.
unsigned int gcd(unsigned int u, unsigned int v)
{
int shift;
/* GCD(0,x) := x */
if (u == 0 || v == 0)
return u | v;
/* Let shift := lg K, where K is the greatest power of 2
dividing both u and v. */
for (shift = 0; ((u | v) & 1) == 0; ++shift) {
u >>= 1;
v >>= 1;
}
while ((u & 1) == 0)
u >>= 1;
/* From here on, u is always odd. */
do {
while ((v & 1) == 0) /* Loop X */
v >>= 1;
/* Now u and v are both odd, so diff(u, v) is even.
Let u = min(u, v), v = diff(u, v)/2. */
if (u <= v) {
v -= u;
} else {
int diff = u - v;
u = v;
v = diff;
}
v >>= 1;
} while (v != 0);
return u << shift;
}
void find_tour()
{
}
//calculate the distances from current point to start and destination.
double heuristicCost(const coord_t &start_t, const coord_t &end_t, const coord_t ¤t_t)
{
float x = abs(current_t.first - start_t.first);
float y = abs(current_t.second - start_t.second);
float dist1 = sqrt(x * x + y * y); //distance from current point to start;
x = abs(current_t.first - end_t.first);
y = abs(current_t.second - end_t.second);
float dist2 = sqrt(x * x + y * y); //distance from current point to end;
return 0.15 * (dist1 + dist2);
}
// priority_queue<Vertex, vector<Vertex>,less<vector<Vertex>::value_type> > gcdPQ;
bool checkDataInQueue(const priority_queue<Vertex> &gcdPQ, Vertex ver)
{
priority_queue<Vertex, vector<Vertex>,less<vector<Vertex>::value_type> > tempPQ = gcdPQ;
//stack<Vertex, vector<Vertex>> s;
for(int i = 0; i < gcdPQ.size(); i++)
{
//s.push();
if( tempPQ.top().getIndex() == ver.getIndex())
return true;
tempPQ.pop();
}
return false;
}
Refactorings
No refactoring yet !
Old code, do not optimize.