behavior.cpp.svn-base

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

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


  // 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 Behavior::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);