basicplayer.cpp

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

      socDash1 = SoccerCommand( CMD_DASH, dPower );
    }
    else
    {
      dPower=WM->getPowerForDash(s2-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 s2" );
        socDash1 = SoccerCommand( CMD_DASH, dPower );
      }
    }
  }

  // try one dash by getting close to ball
  // this handles situation where ball cannot be reached within distance
  // SS->getKickableMargin()/6
  if( socDash1.commandType == CMD_ILLEGAL )
  {
    soc = dashToPoint( posBall );
    WM->predictAgentStateAfterCommand(soc,&posDash1,&velMe,
                                      &angBody,&ang,&sta );
    if ( posDash1.getDistanceTo( posBall ) < 0.95*SS->getMaximalKickDist() )
    {
      Log.log( 508, "dash 1x possible (special)" );
      socDash1 = soc;
    }
  }

  if( bReady != true )
  { 
    if( bOppClose && ! socDash1.isIllegal() )
    {
      Log.log( 508, "do dash 1x, opponent close" );
      WM->logCircle( 508, posDash1, SS->getMaximalKickDist(), true );
      socFinal = socDash1;
    }
    else
    {
      soc = turnBodyToPoint( posPred1 + VecPosition(1,dDesBody, POLAR), 1 );
      WM->predictAgentStateAfterCommand(soc, &posPred, &velMe,
                                        &angBody, &ang, &sta);
      posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );    
      if( posPred.getDistanceTo( posBall ) < 0.8*SS->getMaximalKickDist() )
      {    
        socTurn = soc; // we can do turn and end up ok, but can maybe improve
        ang     = VecPosition::normalizeAngle(dDesBody-angBody);
        if( fabs(ang) < angThreshold )
        {
          socFinal = soc;
          Log.log( 508, "turn 1x, dist %f, angle %f, opp %f",
             dDist1, angTurn, dDistOpp );
          WM->logCircle( 508, posPred1, SS->getMaximalKickDist(), true );
        }
          
      }
      if( socFinal.isIllegal() ) 
      {
        ang     = VecPosition::normalizeAngle(dDesBody-angBody);      
        WM->predictStateAfterTurn( 
                   WM->getAngleForTurn(ang,velMe.getMagnitude()),
                   &posPred, &velMe, &angBody, &angNeck, 
                   WM->getAgentObjectType(),
                   &sta             );
        posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );         
        if( posPred.getDistanceTo( posBall ) < 0.8*SS->getMaximalKickDist() )
        {
          socTurn = soc; // we can do turn and end up ok, but can maybe improve
          ang     = VecPosition::normalizeAngle(dDesBody-angBody);
          if( fabs(ang) < angThreshold )
          {
            Log.log( 508, "turn 2x, dist %f, angle %f, opp %f",
                  dDist2, angTurn, dDistOpp );
            WM->logCircle( 508, posPred2, SS->getMaximalKickDist(), true );
            socFinal = soc;
          }
        }
      }
      if( socFinal.isIllegal() && ! socCollide.isIllegal() &&
          fabs( angTurn ) > angThreshold ) 
      {
        Log.log( 508, "collide with ball on purpose" );
        posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );            
        WM->logCircle( 508, posBall, SS->getMaximalKickDist(), true );
        socFinal = socCollide;
      }
      if( socFinal.isIllegal() && fabs( angTurn ) > angThreshold )
      {
        posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
        soc = dashToPoint( posAgent );
        WM->predictAgentStateAfterCommand(soc,
                                          &posPred,&velMe,&angBody,&ang,&sta );
        if( posPred.getDistanceTo( posBall ) < 0.8*SS->getMaximalKickDist() )
        {
          Log.log( 508, "dash 1x (stop), turn 1x, dist %f, angle %f, opp %f",
           dDist2, angTurn, dDistOpp );
          WM->logCircle( 508, posPred, SS->getMaximalKickDist(), true );
          socFinal = soc;
        }
      }
      if( socFinal.isIllegal()  && ! socTurn.isIllegal() )
      {
        Log.log( 508, "can do turn" );
        WM->logCircle( 508, posPred1, SS->getMaximalKickDist(), true );
        socFinal = socTurn;
      }    
      if( socFinal.isIllegal()  && ! socDash1.isIllegal() )
      {
        Log.log( 508, "do dash 1x" );
        WM->logCircle( 508, posDash1, SS->getMaximalKickDist(), true );
        socFinal = socDash1;
      }
    }

  }
  // if there are no opponents, we are wrongly directed, and we will be closely
  // to the ball, see whether we can first update our heading
  else if( fabs( angTurn ) > angThreshold && !bOppClose &&
      dDist1 < 0.7*SS->getMaximalKickDist() )
  {
    soc = turnBodyToPoint( posPred1 + VecPosition(1,dDesBody, POLAR), 1 );
    Log.log( 508, "turn 1x, dist %f, angle %f, opp %f",
      dDist1, angTurn, dDistOpp );
    WM->logCircle( 508, posPred1, SS->getMaximalKickDist(), true );
    socFinal = soc;
  }
  else if( // fabs( angTurn ) > angThreshold &&
      !bOppClose &&
      dDist2 < 0.7*SS->getMaximalKickDist() )
  {
    soc = turnBodyToPoint( posPred2 + VecPosition(1,dDesBody, POLAR), 2 );
    Log.log( 508, "turn 2x, dist %f, angle %f, opp %f",
       dDist2, angTurn, dDistOpp );
    WM->logCircle( 508, posPred2, SS->getMaximalKickDist(), true );
    socFinal = soc;
  }


  else if( socCollide.commandType != CMD_ILLEGAL && 
           fabs( angTurn ) > angThreshold )
  {
    Log.log( 508, "collide with ball on purpose" );
    WM->logCircle( 508, posDash1, SS->getMaximalKickDist(), true );
    socFinal = socCollide;
  }
  else if( socDash1.commandType != CMD_ILLEGAL )
  {
    Log.log( 508, "do dash 1x" );
    WM->logCircle( 508, posDash1, SS->getMaximalKickDist(), true );
    socFinal = socDash1;
  }


  // cannot intercept ball in three cycles
  WM->setFeature( feature_type,
              Feature( WM->getTimeLastSeeMessage(),
                       WM->getTimeLastSenseMessage(),
                       WM->getTimeLastHearMessage(),                       
                       OBJECT_ILLEGAL,
                       -1,
                       socFinal ) );
  return socFinal;
}

/*! This skill enables a goalkeeper 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
    catchable 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 the
    catchable area 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 goalkeeper
    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 catchable distance from
    this position. If it turns out that this is the case, the corresponding
    dash is performed. However, if the predicted position is not close enough
    to the ball 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 two dash commands. To this end two dash commands are
    generated. If the predicted position of the agent after these two
    dashes lies within catchable distance from the ball then the first dash is
    performed. Otherwise, a second alternative is tried 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. If it turns out that the predicted position of the agent
    after the dash lies within catchable distance from the ball then the first
    command (i.e. the turn) in the sequence of two is performed.
    Otherwise, an illegal command is returned to indicate that the
    skill cannot be performed. The close interception procedure is heavily
    based on the method used by CMU'99 by Peter Stone.
    \return command to intercept ball in two cycles, CMD_ILLEGAL otherwise */
SoccerCommand BasicPlayer::interceptCloseGoalie( )
{
  SoccerCommand soc;
  double        dPower, dDist;
  AngDeg        ang;
  VecPosition   posClosestToBall;

  // initialize all variables with information from worldmodel.
  VecPosition   posPred   = WM->predictAgentPos( 1, 0 );
  VecPosition   posBall   = WM->predictPosAfterNrCycles( OBJECT_BALL, 1 );
  VecPosition   velMe     = WM->getAgentGlobalVelocity( );
  Stamina       sta       = WM->getAgentStamina( );
  AngDeg        angBody   = WM->getAgentGlobalBodyAngle( );
  Line          lineGoalie=Line::makeLineFromPositionAndAngle(posPred,angBody);

  // when it is theoretical possible
  // try one dash and check whether ball is in catchable area
  dDist =SS->getBallSpeedMax()+SS->getPlayerSpeedMax()+SS->getCatchableAreaL();
  if( WM->getRelativeDistance( OBJECT_BALL ) < dDist )
  {
    posClosestToBall = lineGoalie.getPointOnLineClosestTo( posBall );
    dPower           = WM->getPowerForDash(
                          posClosestToBall-posPred,
                          angBody,
                          velMe,
                          sta.getEffort()           );
    posPred          = WM->predictAgentPos( 1, (int)dPower);
    if ( posPred.getDistanceTo( posBall ) < SS->getCatchableAreaL() )
      return SoccerCommand( CMD_DASH, dPower );
  }

  // when it is theoretical possible
  // try two dashes and check whether ball is in catchable area
  // otherwise try first two  dashes and check whether ball is in catchable
  // area, thereafter for turn and dash.
  dDist = SS->getBallSpeedMax()*(1.0+SS->getBallDecay())
           + 2*SS->getPlayerSpeedMax()
           + SS->getCatchableAreaL();
  if( WM->getRelativeDistance( OBJECT_BALL ) < dDist )
  {
    // try two dashes
    // first predict the position in the next cycle when dash with full power
    // is performed. Then calculate the dash power to reach the point where the
    // ball will be in two cycles and predict the global position of the agent
    // after a dash with this power. If the position is in the catchable area
    // return a dash with full power.
    posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
    soc     = dashToPoint( posBall );
    WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta );
    dPower=WM->getPowerForDash(posBall-posPred,angBody,velMe,sta.getEffort());
    WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody,
                               WM->getAgentObjectType() );
    if( posPred.getDistanceTo(posBall) < SS->getCatchableAreaL() )
      return soc;

    // try one turn and a dash
    posBall = WM->predictPosAfterNrCycles( OBJECT_BALL, 2 );
    posPred = WM->predictAgentPos( 2, 0 );
    ang     = (posBall - posPred).getDirection();
    ang     = VecPosition::normalizeAngle( ang - angBody );
    if (fabs( ang ) > PS->getPlayerWhenToTurnAngle() ) // if we want to turn
    {
      soc = turnBodyToPoint( posBall, 2 );             // perform turn
      WM->predictAgentStateAfterCommand(soc,&posPred,&velMe,&angBody,&ang,&sta
);
      dPower=WM->getPowerForDash(posBall-posPred, angBody, 
                                 velMe, sta.getEffort());
      WM->predictStateAfterDash( dPower, &posPred, &velMe, &sta, angBody);
      if( posPred.getDistanceTo(posBall) < SS->getCatchableAreaL() )