Beispiel #1
0
//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;
}