예제 #1
0
	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;
	}
예제 #2
0
	AIEXAMPLE_API int GetTurretRotateDirection()
	{
		if(CompareAngles(tank.turretAngle, tank.fireAngle) > 0)
		{
			return 1;
		}
		if(CompareAngles(tank.turretAngle, tank.fireAngle) < 0)
		{
			return -1;
		}
		return 0;
	}
예제 #3
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;
	}
예제 #4
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;
}
예제 #5
0
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);
}