basicplayer.cpp

自己写的robocup-2d程序

    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" );
      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;
  }