basicplayer.cpp

2003年RoboCup仿真组世界冠军源代码 足球机器人 仿真组 的源代码

    whether he should perform a turn or a dash. Turning has the
    advantage that in the next cycle the agent will be orientated
    correctly towards the point he wants to reach. However, it has the
    disadvantage that performing the turn will cost a cycle and will
    reduce the agent's velocity since no acceleration vector is added
    in that cycle.  Apart from the target position 'pos', this skill
    receives several additional arguments for determining whether a
    turn or dash should be performed in the current situation. If the
    target point is in front of the agent then a dash is performed
    when the relative angle to this point is smaller than a given
    angle 'angWhenToTurn'. However, if the target point is behind the
    agent then a dash is only performed if the distance to point is
    less than a given value 'dDistBack' and if the angle relative to
    the back direction of the agent is smaller than
    'angWhenToTurn'. In all other cases a turn is performed. Note that
    in the case of the goalkeeper it is sometimes desirable that he
    moves backwards towards his goal in order to keep sight of the
    rest of the field. To this end an additional boolean argument
    'bMoveBack' is supplied to this skill that indicates whether the
    agent should always move backwards to the target point. If this
    value equals true then the agent will turn his back towards the
    target point if the angle relative to his back direction is larger
    than 'angToTurn'. In all other cases he will perform a (backward)
    dash towards 'posTo' regardless of whether the distance to this
    point is larger than 'dDistBack'.

    \param posTo global target position to which the agent wants to move
    \param angWhenToTurn angle determining when turn command is returned
    \param dDistBack when posTo lies closer than this value to the back of
          the agent (and within angWhenToTurn) a backward dash is returned
    \param bMoveBack boolean determing whether to move backwards to 'posTo'
    \return SoccerCommand that determines next action to move to 'posTo' */
SoccerCommand BasicPlayer::moveToPos( VecPosition posTo, AngDeg angWhenToTurn,
                            double dDistBack, bool bMoveBack, int iCycles )
{
// previously we only turned relative to position in next cycle, now take
// angle relative to position when you're totally rolled out...
//  VecPosition posPred   = WM->predictAgentPos( 1, 0 );
  VecPosition posAgent  = WM->getAgentGlobalPosition();
  VecPosition posPred   = WM->predictFinalAgentPos();
  
  AngDeg      angBody   = WM->getAgentGlobalBodyAngle();
  AngDeg      angTo     = ( posTo - posPred ).getDirection();
              angTo     = VecPosition::normalizeAngle( angTo - angBody );
  AngDeg      angBackTo = VecPosition::normalizeAngle( angTo + 180 );
  
  double      dDist     = posAgent.getDistanceTo( posTo );

  Log.log( 509, "moveToPos (%f,%f): body %f to %f diff %f now %f when %f", 
          posTo.getX(), posTo.getY(), angBody, 
           ( posTo - posPred ).getDirection(), angTo, 
           ( posTo -  WM->predictAgentPos( 1, 0 )).getDirection(),
           angWhenToTurn );
  if( bMoveBack )
  {
    if( fabs( angBackTo ) < angWhenToTurn )
      return dashToPoint( posTo, iCycles );
    else
      return turnBackToPoint( posTo );
  }
  else if(  fabs( angTo     ) < angWhenToTurn ||
           (fabs( angBackTo ) < angWhenToTurn && dDist < dDistBack ) )
    return dashToPoint( posTo, iCycles );
  else
    return directTowards( posTo, angWhenToTurn );
//return turnBodyToPoint( posTo );
}


/*! This method returns a command that can be used to collide with the ball
    on purpose. When this is not possible. CMD_ILLEGAL is returned. Colliding
    with the ball may be useful when the player is turned with his back to the
    opponent goal and is intercepting a moving ball, by colliding both the ball
    and the player will loose all their velocity. Now the player can turn at
    once to the desired direction. Otherwise he first has to freeze the ball,
    freeze his own speed and then turn around. */
SoccerCommand BasicPlayer::collideWithBall( )
{
  SoccerCommand soc( CMD_ILLEGAL );
  if( WM->getRelativeDistance( OBJECT_BALL ) > 
      WM->getBallSpeed() + SS->getPlayerSpeedMax() )
    return soc;
   
  VecPosition posBallPred  = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );
  
  // first try turn
  soc                      = turnBodyToPoint( WM->getAgentGlobalPosition() + 
                                              VecPosition( 1, 0, POLAR ) );
  VecPosition posAgentPred = WM->predictAgentPosAfterCommand( soc );
  if( posAgentPred.getDistanceTo( posBallPred ) < 
      SS->getBallSize() + SS->getPlayerSize() )
  {
    Log.log( 511, "can collide with ball by turning" );
    return soc;
  }
                               
  soc          = dashToPoint( posBallPred, 1 );
  posAgentPred = WM->predictAgentPosAfterCommand( soc );
  if( posAgentPred.getDistanceTo( posBallPred ) < 
      SS->getBallSize() + SS->getPlayerSize() )
  {
    Log.log( 511, "can collide with ball by dashing %f", soc.dPower );
    return soc;
  }
    
  return SoccerCommand( CMD_ILLEGAL );  
}
/*! This skill enables an agent to intercept a ball which is close to him. The
    objective is to move in such a way that the ball will come within the
    kickable distance from the agent in one or two cycles. To this end the
    prediction methods from the world model are used to predict the ball
    position in the next cycle and two cycles into the future. It is then
    determined whether it is possible to move the agent within kickable
    distance from one of these positions using all logical combinations of
    turn and dash commands. If it is not possible to intercept the ball within
    two cycles then this skill returns an illegal command to indicate that
    it cannot be performed. First it is determined whether the agent can
    intercept the ball in one cycle. To this end the position of the ball in
    the next cycle is predicted and a calculation is performed to decide
    whether a single dash can move the agent within the kickable distance from
    this position. In order to be able to kick the ball efficiently after
    intercepting it, it is important that the agent moves to a good position
    relative to the ball (i.e. the ball must be in front of him). At the same
    time the agent must make sure that he does not collide with the ball when
    trying to intercept it. Let l be a line that runs forwards and backwards
    from the predicted position of the agent in the next cycle into the
    direction of his body. This line thus denotes the possible movement
    direction of the agent. Note that we have to use the agent's predicted
    position in the next cycle since his current velocity must be taken into
    account. In addition, let c be a circle which is centered on the predicted
    ball position and which has a radius equal to the sum of the radius of the
    agent, the radius of the ball and a small buffer (kickable margin/6). It
    is now determined whether the agent can intercept the ball in the next
    cycle by looking at the number of intersection points between l and c. If
    l and c have exactly one point in common then this point is the desired
    interception point for the next cycle. However, if the number of
    intersection points equals two then the desired point is the one for
    which the absolute angle of the ball relative to that point is the
    smallest. This amounts to the intersection point which is closest to
    the agent when the ball lies in front of him and to the furthest one when
    the ball is behind his back. As a result, the desired interception point
    will always be such that the agent has the ball in front of him in the
    next cycle. Then a dash command is generated
    that will bring the agent as close as possible to the desired point.
    Next, the position of the agent after executing this command is predicted
    and if it turns out that this predicted position lies within the kickable
    distance from the ball then the dash is performed. However, if the
    predicted position is not close enough to the ball or if l and c have no
    points in common then it is assumed that the ball cannot be intercepted
    with a single dash. In these cases, two alternatives are explored to see
    if the ball can be intercepted in two cycles.
    The first alternative is to determine whether the agent can intercept the
    ball by performing a turn followed by a dash. To this end the global
    position of the ball is predicted two cycles into the future and a turn
    command is generated that will turn
    the agent towards this point. The agent's position after executing this
    command is then predicted after which a dash command is generated
    that will bring the agent as close as possible to
    the predicted ball position in two cycles. If it turns out that the
    predicted position of the agent after the dash lies within kickable
    distance from the ball then the first command (i.e. the turn) in the
    sequence of two is performed. Otherwise, a second alternative is tried to
    determine whether the agent can intercept the ball by performing two dash
    commands. To this end two dash commands are generated to get closer to the
    predicted ball position after two cycles.
    If the predicted position of the agent after these two dashes lies within
    kickable distance from the ball then the first dash is performed.
    Otherwise, an illegal command is returned to indicate that the skill
    cannot be performed. The close interception procedure is heavily based on
    a similar method introduced in CMU'99 by Peter Stone.
    \return command to intercept ball in two cycles, CMD_ILLEGAL otherwise */
SoccerCommand BasicPlayer::interceptClose( )
{
  FeatureT feature_type = FEATURE_INTERCEPT_CLOSE;
  if( WM->isFeatureRelevant( feature_type ) )
    return WM->getFeature( feature_type ).getCommand();

  SoccerCommand soc, socDash1, socFinal, socCollide, socTurn(CMD_ILLEGAL);
  double        dPower, dDist;
  AngDeg        ang,    ang2;
  VecPosition   s1,     s2;
  bool          bReady = false;

  // first determine whether the distance to the ball is not too large
  dDist = 3*SS->getPlayerSpeedMax()
          + (1.0 + SS->getBallDecay())*SS->getBallSpeedMax()
          + SS->getMaximalKickDist();
  if( WM->getRelativeDistance( OBJECT_BALL ) > dDist )
  {
    bReady = true;
    socFinal = SoccerCommand( CMD_ILLEGAL ); // do not quit, but log feature
  }

  socCollide = collideWithBall( );
  // initialize all variables with information from the worldmodel.
  VecPosition   posAgent = WM->getAgentGlobalPosition( );
  VecPosition   posPred  = WM->predictAgentPos( 1, 0 ), posDash1;
  VecPosition   posBall  = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );
  VecPosition   velMe    = WM->getAgentGlobalVelocity( );
  Stamina       sta      = WM->getAgentStamina( );
  AngDeg        angBody  = WM->getAgentGlobalBodyAngle( ), angTurn, angNeck=0;
  double        dDesBody = 0.0;

  // our desired heading after the intercept is 0 degrees, only when we
  // are far up front we want to be headed toward the opponent goal
  if( posAgent.getX() > PENALTY_X - 5.0 )
    dDesBody = (WM->getPosOpponentGoal()-posAgent).getDirection();

  // get the distance to the closest opponent
  double dDistOpp;
  ObjectT objOpp = WM->getClosestInSetTo( OBJECT_SET_OPPONENTS,
     WM->getAgentObjectType(), &dDistOpp, PS->getPlayerConfThr() );
  angTurn =VecPosition::normalizeAngle(dDesBody-WM->getAgentGlobalBodyAngle());

  // check the distance to the ball when we do not dash (e.g., and only turn)
  posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );
  VecPosition posPred1 = WM->predictAgentPos( 1, 0 );
  double dDist1 = posPred1.getDistanceTo( posBall );
  posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
  VecPosition posPred2 = WM->predictAgentPos( 2, 0 );
  double dDist2 = posPred2.getDistanceTo( posBall );
  posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 3 );
  VecPosition posPred3 = WM->predictAgentPos( 3, 0 );
  double dDist3 = posPred3.getDistanceTo( posBall );
  Log.log( 508, "dist 1: %f, 2: %f 3: %f, 0.6: %f", dDist1, dDist2, dDist3,
                                        0.7*SS->getMaximalKickDist() );

  AngDeg angThreshold = 25;
  bool   bOppClose = ( objOpp != OBJECT_ILLEGAL && dDistOpp < 3.0 )  ;

  // make a line from center of body in next cycle with direction of body
  // use next cycle since current velocity is always propogated to position in
  // next cycle.  Make a circle around the ball with a radius equal to the
  // sum of your own body, the ball size and a small buffer. Then calculate
  // the intersection between the line and this circle. These are the (two)
  // points that denote the possible agent locations close to the ball
  // From these two points we take the point where the body direction of the
  // agent makes the smallest angle with the ball (with backward
  // dashing we sometime have to dash "over" the ball to face it up front)
  posAgent  = WM->getAgentGlobalPosition( );
  posBall   = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );
  angBody   = WM->getAgentGlobalBodyAngle();
  velMe     = WM->getAgentGlobalVelocity( );
  sta       = WM->getAgentStamina( );
  Line line = Line::makeLineFromPositionAndAngle(posPred1,angBody);
  dDist     = SS->getPlayerSize()+SS->getBallSize()+SS->getKickableMargin()/6;
  int  iSol = line.getCircleIntersectionPoints(
                                            Circle(posBall,dDist), &s1, &s2);
  if (iSol > 0)                                          // if a solution
  {
    if (iSol == 2)                                       // take the best one
    {
     ang = VecPosition::normalizeAngle((posBall - s1).getDirection() -angBody);
     ang2= VecPosition::normalizeAngle((posBall - s2).getDirection() -angBody);
//     if ( fabs(ang2) < 90)
     if( s2.getX() > s1.getX() ) // move as much forward as possible
       s1 = s2;                                          // and put it in s1
    }

    // try one dash
    // now we have the interception point we try to reach in one cycle. We
    // calculate the needed dash power from the current position to this point,
    // predict were we will stand if we execute this command and check whether
    // we are in the kickable distance
    dPower = WM->getPowerForDash(s1-posAgent, angBody, velMe,sta.getEffort() );
    posDash1 = WM->predictAgentPos( 1, (int)dPower);
    if ( posDash1.getDistanceTo( posBall ) < 0.95*SS->getMaximalKickDist() )
    {
      Log.log( 508, "dash 1x possible at s1" );