AIEXAMPLE_API int GetTurretRotateSpeed()
{
if(abs(CompareAngles(tank.turretAngle, tank.fireAngle)) < MAX_TURRET_ROTATE_SPEED)
{
return abs(CompareAngles(tank.turretAngle, tank.fireAngle));
}
return MAX_TURRET_ROTATE_SPEED;
}
AIEXAMPLE_API int GetTurretRotateDirection()
{
if(CompareAngles(tank.turretAngle, tank.fireAngle) > 0)
{
return 1;
}
if(CompareAngles(tank.turretAngle, tank.fireAngle) < 0)
{
return -1;
}
return 0;
}
AIEXAMPLE_API int GetRotateDirection()
{
if(CompareAngles(tank.baseAngle, tank.movingAngle) > 0)
{
return 1;
}
if(CompareAngles(tank.baseAngle, tank.movingAngle) < 0)
{
return -1;
}
return 0;
}
int MapPathFinder::CalculatePath(const Map_BoolType &MainMap, const POSE &Start, const POSE &Destination, MapPath &Path)
{
// Check that start is a valid position
if( MainMap.GetPixel(Start.Position) != OCCUPANCYGRID_DISCRETE_EMPTY )
return -1;
// Get Distance Map
Map_IntType distMap;
MapPathFinder::CalculateDistMap(MainMap, Destination.Position, distMap);
// Check if Destination is reachable from start
unsigned int curDist, nextDist;
if(distMap.GetPixel(Start.Position, curDist) < 0)
return -2;
if(curDist >= distMap.GetHeight()*distMap.GetWidth())
return -3;
// Follow negative gradient from start until destination is reached
PATH_DATA_LOCAL newMoveOrder;
POSE curPose = Start;
POSE nextPose;
newMoveOrder.PathLength = 0;
newMoveOrder.RotAngle = 0;
while(curDist > 0)
{
// Look at neighbors and find lowest distance to target
nextDist = curDist;
for(int i=-1; i<=1; i++)
{
for(int j=-1; j<=1; j++)
{
unsigned int tmpDist;
POS_2D tmpPos;
tmpPos.X = curPose.Position.X + i;
tmpPos.Y = curPose.Position.Y + j;
// Check that this pixel is valid
if(distMap.GetPixel(tmpPos, tmpDist) < 0)
continue;
// Check if new distance is less than old distance and if so, set as next position
if(tmpDist < nextDist)
{
nextDist = tmpDist;
nextPose.Position = tmpPos;
}
}
}
// After going through neighbors, check that a lower distance was found
// Report error if not
if(nextDist <= curDist)
{
std::cerr << "MapPathFinder::CalculatePath() ERROR: No lower distance found\n";
return -4;
}
// Check whether old move order simply needs to be elongated, or if a new move order is required
nextPose.RotationAngle = atan2((nextPose.Position.Y - curPose.Position.Y), (nextPose.Position.X - curPose.Position.X));
if(CompareAngles(nextPose.RotationAngle, curPose.RotationAngle))
{
// Both angles are equal, just elongate current move order
newMoveOrder.PathLength += sqrt((nextPose.Position.Y - curPose.Position.Y)*(nextPose.Position.Y - curPose.Position.Y) + (nextPose.Position.X-curPose.Position.X)*(nextPose.Position.X-curPose.Position.X));
}
else
{
// Add old order to Path
Path.AddMoveOrder(newMoveOrder);
// Create new order
newMoveOrder.RotAngle = nextPose.RotationAngle-curPose.RotationAngle; // Rotate to new movement direction
newMoveOrder.PathLength = sqrt((nextPose.Position.Y - curPose.Position.Y)*(nextPose.Position.Y - curPose.Position.Y) + (nextPose.Position.X-curPose.Position.X)*(nextPose.Position.X-curPose.Position.X));
}
// Update for next round
curPose = nextPose;
curDist = nextDist;
}
// Check whether last move order needs to be added to Path
if(newMoveOrder.PathLength != 0)
{
Path.AddMoveOrder(newMoveOrder);
}
// Check whether the robot needs to be rotated to face the correct direction
if(curPose.RotationAngle != Destination.RotationAngle)
{
newMoveOrder.PathLength = 0;
newMoveOrder.RotAngle = Destination.RotationAngle - curPose.RotationAngle;
Path.AddMoveOrder(newMoveOrder);
}
return 1;
}
int EdgeInsertPosition(
const vertex& a, const vertex& b,
int nE, graph_edge **E,
int *eleft, int *eright,
int *code_left, int *code_right )
{
vertex v1,v2,v3;
int left,right,mid,sign,s1,s2,s3,tmp;
int match = 0;
left = 0;
right = nE-1;
if( right > 0 )
{
if( E[left]->v[0]->v == a ) v1 = E[left]->v[1]->v;
else v1 = E[left]->v[0]->v;
if( E[right]->v[0]->v == a ) v2 = E[right]->v[1]->v;
else v2 = E[right]->v[0]->v;
sign = CompareAngles( a.v(), b.v(), v1.v(), v2.v(),
&match, &s1, &s2 );
if( sign > 0 )
{
if( match )
{
if( right-1 != left ) s1 = 0;
left = right-1;
}
else
{
mid = (left+right)/2;
while( mid != left )
{
if( E[mid]->v[0]->v == a ) v3 = E[mid]->v[1]->v;
else v3 = E[mid]->v[0]->v;
sign = CompareAngles( a.v(), b.v(), v1.v(), v3.v(),
&match, &s1, &s3 );
if( match )
{
right = mid;
s2 = s3;
if( mid-1 != left ) s1 = 0;
left = mid-1;
break;
}
if( sign > 0 )
{
right = mid;
s2 = s3;
}
else if( sign < 0 )
{
left = mid;
v1 = v3;
s1 = s3;
}
else
throw InternalError(); /* this cannot be */
mid = (left+right)/2;
}
}
}
else
{
gul::Assert< gul::InternalError >( ndebug ||
((sign != 0) && (left == 0) && (right = nE-1)) );
tmp = left; left = right; right = tmp;
tmp = s1; s1 = s2; s2 = tmp;
}
}
else
{
right = left;
s1 = s2 = 0;
}
if( s1 == 0 )
{
if( E[left]->v[0]->v == a ) v1 = E[left]->v[1]->v;
else v1 = E[left]->v[0]->v;
s1 = DetermineOrientation( a.v(), b.v(), v1.v() ) + 2;
}
if( s2 == 0 )
{
if( right != left )
{
if( E[right]->v[0]->v == a ) v2 = E[right]->v[1]->v;
else v2 = E[right]->v[0]->v;
s2 = DetermineOrientation( a.v(), b.v(), v2.v() ) + 2;
}
else
s2 = s1;
}
*eleft = left;
*eright = right;
*code_left = s1;
*code_right = s2;
return(match);
}
