behavior.cpp.svn-base

一个巴西大学编写的2D仿真组足球机器人程序

SoccerCommand Behavior::dashToPoint( VecPosition pos, int iCycles )
{
  double dDashPower = WM->getPowerForDash(
                                 pos - WM->getAgentGlobalPosition(),
                                 WM->getAgentGlobalBodyAngle(),
                                 WM->getAgentGlobalVelocity(),
                                 WM->getAgentEffort(),
                                 iCycles                              );
  return SoccerCommand( CMD_DASH, dDashPower );
}

/*! This skill enables an agent to freeze a moving ball, i.e. it returns a kick
    command that stops the ball dead at its current position. Since ball
    movement in the soccer server is implemented as a vector addition, the ball
    will stop in the next cycle when it is kicked in such a way that the
    resulting acceleration vector has the same length and opposite direction to
    the current ball velocity. The desired speed that should be given to the
    ball on the kick thus equals the current ball speed. Furthermore, the
    direction of the kick should equal the direction of the current ball
    velocity plus 180 degrees. Note that this direction must be made relative
    to the agent's global body angle before it can be passed as an argument to
    the kick command.
    \return SoccerCommand to freeze the ball. */
SoccerCommand Behavior::freezeBall( )
{
  // determine power needed to kick the ball to compensate for current speed
  // get opposite direction (current direction + 180) relative to body
  // and make the kick command.
  VecPosition posAgentPred = WM->predictAgentPos( 1, 0 );

  double dPower = WM->getKickPowerForSpeed( WM->getBallSpeed() );
  if( dPower > SS->getMaxPower() )
  {
    Log.log( 552, "%d: freeze ball has to much power", WM->getCurrentCycle() );
    dPower = (double)SS->getMaxPower();
  }
  double dAngle = WM->getBallDirection() + 180 - WM->getAgentGlobalBodyAngle();
  dAngle        = VecPosition::normalizeAngle( dAngle );
  SoccerCommand soc( CMD_KICK, dPower, dAngle );
  VecPosition posBall, velBall;
  WM->predictBallInfoAfterCommand( soc, &posBall, &velBall );
  if( posBall.getDistanceTo( posAgentPred ) < 0.8 * SS->getMaximalKickDist() )
    return soc;
  Log.log( 101, "freeze ball will end up oustide -> accelerate" );
  posBall = WM->getBallPos();
  // kick ball to position inside to compensate when agent is moving
  VecPosition posTo   = posAgentPred + 
    VecPosition( min( 0.7 * SS->getMaximalKickDist(),
		      posBall.getDistanceTo( posAgentPred ) - 0.1 ),
		 (posBall-posAgentPred).getDirection(),
		 POLAR );
  VecPosition velDes( posTo - posBall );
  return accelerateBallToVelocity( velDes );
}

/*! This skill enables an agent to kick the ball close to his body. It receives
    an angle 'ang' as its only argument and returns a kick command that causes
    the ball to move to a point at a relative angle of 'ang' degrees and at a
    close distance (kickable margin/6 to be precise) from the agent's body.
    To this end the ball has to be kicked from its current position to the
    desired point relative to the predicted position of the agent in the next
    cycle. In general, this skill will be used when the agent wants to kick the
    ball to a certain position on the field which cannot be reached with a
    single kick. Since the efficiency of a kick is highest when the ball is
    positioned just in front of the agent's body, calling this skill with
    'ang = 0' will have the effect that the agent can kick the ball with more
    power after it is executed.
    Note that this skill will only be executed if it is possible to actually
    reach the desired ball position with a single kick. If the required power
    does exceed the maximum then the ball is frozen at its current position
    using the freezeBall skill. In general, it will then always be possible to
    shoot the motionless ball to the desired point in the next cycle.
    \param 'ang' relative angle to body to which the ball should be kicked
    \param relative ratio to which the ball is kicked
    \return SoccerCommand to kick the ball close to the body */
SoccerCommand Behavior::kickBallCloseToBody( AngDeg ang, double dKickRatio )
{
  AngDeg      angBody    = WM->getAgentGlobalBodyAngle();
  VecPosition posAgent   = WM->predictAgentPos( 1, 0 );
  double      dDist      = SS->getPlayerSize() +
                           SS->getBallSize()   +
                           SS->getKickableMargin()*dKickRatio;
  AngDeg      angGlobal  = VecPosition::normalizeAngle( angBody + ang );
  VecPosition posDesBall = posAgent   + VecPosition( dDist, angGlobal, POLAR );
  if( fabs( posDesBall.getY() ) > PITCH_WIDTH/2.0 ||
      fabs( posDesBall.getX() ) > PITCH_LENGTH/2.0 )
  {
    Line lineBody = Line::makeLineFromPositionAndAngle( posAgent, angGlobal );
    Line lineSide(0,0,0);
    if( fabs( posDesBall.getY() ) > PITCH_WIDTH/2.0 )
      lineSide = Line::makeLineFromPositionAndAngle(
	  VecPosition( 0, sign(posDesBall.getY() )* PITCH_WIDTH/2.0 ), 0 );
    else
      lineSide = Line::makeLineFromPositionAndAngle(
	  VecPosition( 0, sign(posDesBall.getX() )* PITCH_LENGTH/2.0 ),  90 );
    VecPosition posIntersect = lineSide.getIntersection( lineBody );
    posDesBall = posAgent + 
      VecPosition( posIntersect.getDistanceTo( posAgent ) - 0.2, 
		   angGlobal, POLAR );
  }
    
  VecPosition vecDesired = posDesBall - WM->getBallPos();
  VecPosition vecShoot   = vecDesired - WM->getGlobalVelocity( OBJECT_BALL );
  double      dPower     = WM->getKickPowerForSpeed( vecShoot.getMagnitude() );
  AngDeg      angActual  = vecShoot.getDirection() - angBody;
              angActual  = VecPosition::normalizeAngle( angActual );

  if( dPower > SS->getMaxPower() && WM->getBallSpeed() > 0.1 )
  {
    Log.log( 500, "kickBallCloseToBody: cannot compensate ball speed, freeze");
    Log.log( 101, "kickBallCloseToBody: cannot compensate ball speed, freeze");
    return freezeBall();
  }
  else if( dPower > SS->getMaxPower() )
  {
    if( WM->isDeadBallUs() )
    {
      if( WM->getRelativeAngle( OBJECT_BALL ) > 25 )
      {
	Log.log( 101, "dead ball situation, turn to ball" );
	return turnBodyToObject( OBJECT_BALL );
      }
    }
    else
    {
      Log.log( 101, "kickBallCloseToBody: ball has no speed, but far away" );
      dPower = 100;
    }
  }
  else
    Log.log( 101, "(kick %f %f), vecDesired (%f,%f) %f posDes(%f,%f)", 
	     dPower, angActual,vecDesired.getX(), vecDesired.getY(),ang,
	     posDesBall.getX(), posDesBall.getY() );
  return SoccerCommand( CMD_KICK, dPower, angActual );
}

/*! This skill enables an agent to accelerate the ball in such a way that it
    gets a certain velocity after the kick. It receives the desired velocity
    'vecDes' as its only argument and returns a kick command that causes the
    ball to be accelerated to this velocity. If the power that must be supplied
    to the kick command to get the desired result does not exceed the maximum
    kick power then the desired velocity can be realized with a single kick.
    The kick direction should then be equal to the direction of the
    acceleration vector relative to the agent's global body angle. However,
    if the desired velocity is too great or if the current ball velocity is
    too high then the required acceleration cannot be realized with a single
    kick. In this case, the ball is kicked in such a way that the acceleration
    vector has the maximum possible length and a direction that aligns the
    resulting ball movement with 'vecDes'. This means that after the kick the
    ball will move in the same direction as 'vecDes' but at a lower speed. To
    this end the acceleration vector has to compensate for the current ball
    velocity in the `wrong' direction (y-component).
    \param velDes desired ball velocity
    \return SoccerCommand that accelerates the ball to 'vecDes' */
SoccerCommand Behavior::accelerateBallToVelocity( VecPosition velDes )
{
  AngDeg      angBody  = WM->getAgentGlobalBodyAngle();
  VecPosition velBall  = WM->getGlobalVelocity( OBJECT_BALL );
  VecPosition accDes   = velDes - velBall;
  double      dPower;
  double      angActual;

  // if acceleration can be reached, create shooting vector
  if( accDes.getMagnitude() < SS->getBallAccelMax() )
  {
    dPower    = WM->getKickPowerForSpeed   ( accDes.getMagnitude() );
    angActual = VecPosition::normalizeAngle( accDes.getDirection() - angBody );
    if( dPower <= SS->getMaxPower() )
      return SoccerCommand( CMD_KICK, dPower, angActual );
  }

  // else determine vector that is in direction 'velDes' (magnitude is lower)
         dPower    = SS->getMaxPower();
  double dSpeed    = WM->getActualKickPowerRate() * dPower;
  double tmp       = velBall.rotate(-velDes.getDirection()).getY();
         angActual = velDes.getDirection() - asinDeg( tmp / dSpeed );
         angActual = VecPosition::normalizeAngle( angActual - angBody );

  return SoccerCommand( CMD_KICK, dPower, angActual );
}

/*! This skill enables an agent to catch the ball and can only be executed
    when the agent is a goalkeeper. It returns a catch command that takes the
    angle of the ball relative to the body of the agent as its only argument.
    The correct value for this argument is computed by determining the global
    direction between the current ball position and the agent's current
    position and by making this direction relative to the agent's global body
    angle.
    \return SoccerCommand to catch the ball */
SoccerCommand Behavior::catchBall()
{
  // true means returned angle is relative to body instead of neck
  return SoccerCommand( CMD_CATCH, WM->getRelativeAngle( OBJECT_BALL, true ));
}


/*! This skill enables an agent to communicate with other players on the field.
    It receives a string message as its only argument and returns a say command
    that causes the message to be broadcast to all players within a certain
    distance from the speaker.
    \return SoccerCommand to say the specified string 'str' */
SoccerCommand Behavior::communicate( char *str )
{
  return SoccerCommand( CMD_SAY, str );
}

/*! This method returns a 'move' command to teleport the agent directly to the
    specified global position.
    \param pos global position to which should be moved.
    \return SoccerCommand to move directly to 'pos'. */
SoccerCommand Behavior::teleportToPos( VecPosition pos )
{
  return SoccerCommand( CMD_MOVE, pos.getX(), pos.getY() );

}

/*! This method returns a 'attentionto' command to listen to the specified
    object. In most occasions this is a teammate. */
SoccerCommand Behavior::listenTo( ObjectT obj )
{
  if( !SoccerTypes::isKnownPlayer( obj ) )
    return SoccerCommand( CMD_ATTENTIONTO, -1.0, -1.0 );

  return SoccerCommand( CMD_ATTENTIONTO,
     1.0,                                       // 1.0 denotes our team
     SoccerTypes::getIndex( obj ) + 1 );
}

/*! This method returns the command to tackle the ball. */
SoccerCommand Behavior::tackle( )
{
  return SoccerCommand( CMD_TACKLE, 100.0 );
}

/********************** INTERMEDIATE LEVEL SKILLS ****************************/

/*! This skill enables an agent to turn his body towards an object o which is
    supplied to it as an argument. To this end the object's global position
    o in the next cycle is predicted based on its current velocity.
    This predicted position is passed as an argument to the turnBodyToPoint
    skill which generates a turn command that causes the agent to turn his
    body towards the object.
    \param o object to which agent wants to turn
    \return SoccerCommand that turns to this object */
SoccerCommand Behavior::turnBodyToObject( ObjectT o )
{
  return turnBodyToPoint( WM->predictPosAfterNrCycles(o, 1) );
}

/*! This skill enables an agent to turn his neck towards an object. It
    receives as arguments this object o as well as a primary action command
    'soc' that will be executed by the agent at the end of the current cycle.
    Turning the neck towards an object amounts to predicting the object's
    global position in the next cycle and passing this predicted position
    together with the 'soc' command as arguments to the turnNeckToPoint skill.
    This low-level skill will then generate a turn neck command that causes the
    agent to turn his neck towards the given object. Note that the 'soc'
    command is supplied as an argument for predicting the agent's global
    position and neck angle after executing the command. This is necessary
    because a turn neck command can be executed in the same cycle as a kick,
    dash, turn , move or catch command.
   \param o object to which the agent wants to turn his neck
   \param soc SoccerCommand that is performed in this cycle.
   \return SoccerCommand that turns the neck of the agent to this object */
SoccerCommand Behavior::turnNeckToObject( ObjectT o, SoccerCommand soc )
{