//Calculates the cost of moving from Point "thru" to Point "to", uses Point "from" to determine the cost of turning
//Takes into account distance the robot has to turn, the distance between points and adds a large cost if the points are
//not connected (according to Maze).
int Routing::calcCost(Point from, Point thru, Point to)
{
const int DIAGONAL_COST = 7, // Comes from multiplying "an isosceles right angle triangle with hypotenuse = 1" by 7
ORTHOGONAL_COST = 5, // Comes from multiplying "an isosceles right angle triangle with hypotenuse = 1" by 7
UNPASSABLE_COST = 200; // This cost is for when the user forces a path to be generated and the cost of passing an UNPASSABLE node must be defined
int cost = turnCost(from, thru, to);
if( ! maze->joined(thru,to))
cost += UNPASSABLE_COST;
int deltaY = abs(thru.y - to.y);
int deltaX = abs(thru.x - to.x);
if((deltaX != 0) && (deltaY != 0))
return cost + DIAGONAL_COST;
else
return cost + ORTHOGONAL_COST;
}
int spfa(){
for(int row=0;row<height;row++)
for(int col=0;col<width;col++)
for(Orientation ori=ORI_BEGIN;ori!=ORI_END;ori=(Orientation)(ori+1))
timeArrive[row][col][ori]=INT_MAX;
timeArrive[begin.pos.row][begin.pos.col][begin.ori]=0;
std::priority_queue<Status> queue;
queue.push(begin);
while(!queue.empty()){
Status current=queue.top();
if(current.pos==dest)
return timeArrive[current.pos.row][current.pos.col][current.ori];
queue.pop();
Status next=current;
for(Orientation ori=ORI_BEGIN;ori!=ORI_END;ori=(Orientation)(ori+1)){
if(ori==current.ori)
continue;
next.ori=ori;
if(timeArrive[next.pos.row][next.pos.col][next.ori]>timeArrive[current.pos.row][current.pos.col][current.ori]+turnCost(next.ori,current.ori)){
timeArrive[next.pos.row][next.pos.col][next.ori]=timeArrive[current.pos.row][current.pos.col][current.ori]+turnCost(next.ori,current.ori);
queue.push(next);
}
}
next.ori=current.ori;
for(int step=0;step<STEP_MAX;step++){
next.pos=next.pos.forward(next.ori);
if(!next.pos.legal())
break;
if(timeArrive[next.pos.row][next.pos.col][next.ori]>timeArrive[current.pos.row][current.pos.col][current.ori]+1){
timeArrive[next.pos.row][next.pos.col][next.ori]=timeArrive[current.pos.row][current.pos.col][current.ori]+1;
queue.push(next);
}
}
}
return -1;
}