worldmodelpredict.cpp

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

                ( fabs( angTo ) < 90 && bMoveBack == true ) ) ) )
         && turn < 5 && fabs( angTmp ) > PS->getPlayerWhenToTurnAngle() )
  {

     ang = (posTo - posPred).getDirection() + (( bMoveBack == true )?180:0);
     ang = VecPosition::normalizeAngle( ang - angBody );
     soc = SoccerCommand(CMD_TURN,getAngleForTurn(ang,vel.getMagnitude(),obj));
     Log.log( 468, "angTo %f, dDist %f, ang %f %d angBody %f soc %f vel %f %f",
              angTo, dDist, ang, obj, angBody, soc.dAngle, vel.getMagnitude(),
              getInertiaMoment( obj ));
     if( bFirst == true )
       socFirst = soc;
     bFirst = false;
     predictStateAfterTurn(soc.dAngle, &pos, &vel, &angBody,&angNeck,obj,&sta);
     line = Line::makeLineFromPositionAndAngle( posPred, angBody );
     dDist = line.getDistanceWithPoint( posTo );
     angTo = (posTo - posPred).getDirection();
     angTo = VecPosition::normalizeAngle( angTo - angBody );
     turn++;
  }

  // if very close and have to turn a lot, it may be better to move with our
  // back to that point
  if( turn > 1 && iCycles < 4 && posPred.getDistanceTo( posTo ) < dDistBack &&
      bMoveBack == false)
  {
     angBody = ( angBodyIn == NULL ) ? getGlobalBodyAngle( obj ) : *angBodyIn;
     pos     = ( posIn     == NULL ) ? getGlobalPosition ( obj ) : *posIn;
     vel     = ( velIn     == NULL ) ? getGlobalVelocity ( obj ) : *velIn;
     ang = (posTo - posPred).getDirection() + 180;
     ang = VecPosition::normalizeAngle( ang - angBody );
     soc = SoccerCommand(CMD_TURN,getAngleForTurn(ang,vel.getMagnitude(),obj));
     predictStateAfterTurn( soc.dAngle,&pos,&vel,&angBody,&angNeck,obj,&sta);
     line = Line::makeLineFromPositionAndAngle( posPred, angBody );
     dDist = line.getDistanceWithPoint( posTo );
     if( dDist < 0.9*getMaximalKickDist( obj )  )
     {
       Log.log( 463, "turn around and intercept with back" );
       return soc;
     }
  }

  return socFirst;
}

/*! This method returns the command to move to a position, first it checks
    whether a turn is necessary. When this is the case, it performs the turn.
    Otherwise a dash command is generated to move in 'iCycles' cycles to
    the point 'posTo'. */
SoccerCommand WorldModel::predictCommandToMoveToPos( ObjectT obj, 
  VecPosition posTo, int iCycles, double dDistBack, 
  bool bMoveBack,VecPosition *posIn, VecPosition *velIn, AngDeg *angBodyIn)
{
  VecPosition   pos, vel;
  AngDeg        angBody;
  SoccerCommand soc;
  double        dPower;

  // fill in all values
  angBody = ( angBodyIn == NULL ) ? getGlobalBodyAngle( obj ) : *angBodyIn;
  pos     = ( posIn     == NULL ) ? getGlobalPosition ( obj ) : *posIn;
  vel     = ( velIn     == NULL ) ? getGlobalVelocity ( obj ) : *velIn;

  soc = predictCommandTurnTowards(obj, posTo, iCycles, dDistBack, bMoveBack,
                                       posIn, velIn,   angBodyIn);
  if( ! soc.isIllegal() )
    return soc;

  dPower = getPowerForDash( posTo-pos, angBody, vel,getAgentEffort(),iCycles );
  return SoccerCommand( CMD_DASH, dPower );
}

/*! This command returns the command for object 'obj' to intercept the ball.
    It needs the command 'socClose' as the command to intercept a close ball
    (may be CMD_ILLEGAL). 'iCycles' will be filled with the number of cycles
    to get to this ball position and 'posIntercept' will be filled with the
    final interception point. When posIn, velInn, angBody are equal to NULL,
    the agent information is used in the calculations. */
SoccerCommand WorldModel::predictCommandToInterceptBall( ObjectT obj,
    SoccerCommand socClose, int *iCycles, VecPosition *posIntercept, 
    VecPosition *posIn, VecPosition *velIn, AngDeg *angBodyIn )
{
  FeatureT feature_type = FEATURE_INTERCEPTION_POINT_BALL;
  
  // check whether we already have calculated this value
  if( isFeatureRelevant( feature_type ) )
  {
    int i = max(0,((int)getFeature(feature_type).getInfo()-getCurrentCycle()));
    if( iCycles != NULL )
      *iCycles = i;
    if( posIntercept != NULL )
      *posIntercept = predictPosAfterNrCycles( OBJECT_BALL, i );

    Log.log( 463, "intercept, use old info, feature %d: %d", feature_type,
        max(0,((int)getFeature( feature_type ).getInfo()-getCurrentCycle())));
    return getFeature( feature_type ).getCommand();
  }

  // declare all needed variables
  SoccerCommand soc;
  VecPosition   pos, vel, posPred, posBall(0,0), posBallTmp, velBall, posAgent;
  AngDeg        angBody, angNeck;
  int           iMinCyclesBall=100, iFirstBall=100;
  double        dMaxDist = getMaximalKickDist( obj );
  double        dBestX = UnknownDoubleValue;
  Stamina       sta;
  double        dMinOldIntercept = 100, dDistanceOfIntercept = 10.0;
  int           iOldIntercept = UnknownIntValue;
  static Time   timeLastIntercepted(-1,0);
  static VecPosition posOldIntercept;

  // didn't intercept ball in last two cycles -> reset old interception point
  if( (getCurrentTime() - timeLastIntercepted) > 2 )
    posOldIntercept.setVecPosition( UnknownDoubleValue, UnknownDoubleValue);
  timeLastIntercepted = getCurrentTime();
                                     
  int iCyclesBall = 0;

  Log.log( 468, "old interception point: (%f,%f)", posOldIntercept.getX(),
   posOldIntercept.getY() );

  // for each new pos of the ball, check whether agent can reach ball 
  // and update the best interception point
  while( iCyclesBall <= PS->getPlayerWhenToIntercept() &&
         iCyclesBall <= iFirstBall + 20 &&
         isInField( posBall ) == true )
  {
    // re-initialize all variables
    angBody = ( angBodyIn == NULL ) ? getGlobalBodyAngle( obj ) : *angBodyIn;
    angNeck = getGlobalNeckAngle( obj );
    pos     = ( posIn     == NULL ) ? getGlobalPosition ( obj ) : *posIn;
    vel     = ( velIn     == NULL ) ? getGlobalVelocity ( obj ) : *velIn;
    sta     = getAgentStamina();
    soc.commandType = CMD_ILLEGAL;

    // predict the ball position after iCycles and from that its velocity 
    posBallTmp = predictPosAfterNrCycles( OBJECT_BALL, iCyclesBall );
    if( iCyclesBall == 0 )
      velBall = getGlobalVelocity( OBJECT_BALL );
    else
      velBall = posBallTmp - posBall;
    posBall   = posBallTmp;
    
    // predict the agent position 
    posPred   = predictPosAfterNrCycles( obj, min(iCyclesBall,4), 0 );
    posAgent  = getGlobalPosition( obj );

    // if too far away, we can never reach it and try next cycle
    if( posPred.getDistanceTo(posBall)/getPlayerSpeedMax( obj )
          > iCyclesBall + dMaxDist || isInField( posBall ) == false )
    {
      iCyclesBall++;
      continue;
    }

    // predict our position after the same nr of cycles when intercepting 
    for( int i = 0; i < iCyclesBall; i++ )
    {
      soc = predictCommandToMoveToPos( obj, posBall, iCyclesBall - i ,
              2.5, false, &pos, &vel, &angBody );
      predictStateAfterCommand( soc, &pos, &vel, &angBody, &angNeck, obj );
    }

    // if in kickable distance, we can reach the ball!
    if (pos.getDistanceTo( posBall ) < dMaxDist  )
    {
      Log.log( 468, "can intercept ball in %d cycles, dist %f, old %f obj %d",
               iCyclesBall, pos.getDistanceTo( posBall ),
               posBall.getDistanceTo( posOldIntercept ), obj  );

      if( iMinCyclesBall == 100 ) // log first possible interception point
        iFirstBall = iMinCyclesBall = iCyclesBall;

      // too get some consistency in the interception point and avoid
      // too many turns, also keep track of the current possible
      // interception point.  This is the point close to the old
      // interception point. Two constraints are that the ball has to
      // have some speed (else it does not really matter where to
      // intercept) and the ball must be intercepted safely, that is
      // the ball is close to the body when intercepting. 
      if( posBall.getDistanceTo( posOldIntercept ) < 
                           min( 1.0, dMinOldIntercept ) &&
          pos.getDistanceTo( posBall ) < 0.70*getMaximalKickDist( obj ) &&
          velBall.getMagnitude() > 0.6 )
      {
        Log.log( 468, "update old interception point %d", iCyclesBall );
        dBestX           = posBall.getX();
        iOldIntercept    = iCyclesBall;
        dDistanceOfIntercept = pos.getDistanceTo( posBall );
        dMinOldIntercept = posBall.getDistanceTo( posOldIntercept );
      }
      // determine the safest interception point. This point must be
      // better than the current intercept, the distance to ball must
      // be very small after interception and close to the previous
      // calculated interception point
      else if( pos.getDistanceTo( posBall ) < dDistanceOfIntercept &&
               dDistanceOfIntercept > 0.50*getMaximalKickDist( obj ) &&
               ( iCyclesBall <= iMinCyclesBall + 3  ||
                 iCyclesBall <= iOldIntercept + 3 ) &&
               fabs( posBall.getY() ) < 32.0 &&
               fabs( posBall.getX() ) < 50.0 )
      {
        iMinCyclesBall = iCyclesBall;
        dDistanceOfIntercept = pos.getDistanceTo( posBall );
        Log.log( 468, "safer interception at %d", iMinCyclesBall );
        if( iOldIntercept == iMinCyclesBall - 1 )
        {
          Log.log( 468, "old interception point -> safer" );
          iOldIntercept = iMinCyclesBall;
        }
      }
    }
    else
      Log.log( 468, "cannot intercept ball in %d cycles, dist %f, %f and %f",
        iCyclesBall, pos.getDistanceTo(posBall), pos.getDistanceTo( posAgent ),
         posBall.getDistanceTo( posAgent ) - dMaxDist);;

    iCyclesBall++;
  }

  Log.log( 463, "first interception point:        %d cycles", iFirstBall );
  Log.log( 463, "best interception point:         %d cycles", iMinCyclesBall );
  Log.log( 463, "old interception point           %d cycles", iOldIntercept );

  // check special situations where we move to special position.
  if( !( iMinCyclesBall > iOldIntercept + 2 ) &&
      iOldIntercept != UnknownIntValue  )
  {
    Log.log( 463, "move to old interception point." );
    iMinCyclesBall = iOldIntercept;
  }
  else 
  {
    Log.log( 463, "move to first intercept" );
    iMinCyclesBall = iFirstBall;
  }

  posBall = predictPosAfterNrCycles( OBJECT_BALL, iMinCyclesBall );
  Log.log( 463, "choose %d cycles", iMinCyclesBall );
  logCircle( 463, posBall, 1.0 );
  if( iCycles != NULL )
    *iCycles = iMinCyclesBall;

  posOldIntercept = posBall;
  posPred = predictPosAfterNrCycles( obj, min(iMinCyclesBall,4), 0 );
  if( posIntercept != NULL )
    *posIntercept = posBall;

  if( iMinCyclesBall < 3 && ! socClose.isIllegal() )
  {
    Log.log( 463, "do close intercept" );
    iMinCyclesBall = 1;
    soc = socClose;
  }
  else if( posPred.getDistanceTo( posBall ) < 0.5 )
  {
    Log.log( 463, "intercept: do not move already close" );
    soc = SoccerCommand( CMD_ILLEGAL );
  }
  else
  {
    Log.log( 463, "intercept: move to (%f,%f)", posBall.getX(),posBall.getY());
    Log.log( 560, "intercept: move to (%f,%f) in %d cycles", 
             posBall.getX(),posBall.getY(), iMinCyclesBall);
    if( isDeadBallUs() && !isGoalKickUs())     // do not dash backwards
      soc = predictCommandToMoveToPos( obj, posBall, iMinCyclesBall, 0 );
    else
      soc = predictCommandToMoveToPos( obj, posBall, iMinCyclesBall );    
  }

  // store the calculated action as a feature
  if( obj == getAgentObjectType() )
    setFeature( feature_type,
                Feature( getTimeLastSeeMessage(),
                         getTimeLastSenseMessage(),
                         getTimeLastHearMessage(),                       
                         OBJECT_ILLEGAL,
                         getTimeLastSeeMessage().getTime() + iMinCyclesBall,
                         soc ) );
  return soc;
}

/*! This method determines whether a dash command (supplied as the first
    argument) will result in collision with another player.
    This is checked by determing the global position after the command
    and then check whether the positions of one of the other players lies
    with the player size. Since it cannot be known what kind of action the
    other player takes in this cycle, it is also difficult to predict what the
    global position of the player will be in the next cycle. This method
    therefore assumes the other players have issued a dash with maximum power
    in the last cycle.
    \return bool indicating whether dash will result in a collision. */
bool WorldModel::isCollisionAfterCommand( SoccerCommand soc )
{
  VecPosition posPred, velPred;
  AngDeg ang1, ang2;
  Stamina sta;
  
  predictAgentStateAfterCommand( soc, &posPred, &velPred, &ang1,&ang2,&sta );
  velPred /= SS->getPlayerDecay();
  VecPosition posBall = predictPosAfterNrCycles( OBJECT_BALL, 1 );
  if( soc.commandType == CMD_KICK )
    predictBallInfoAfterCommand( soc, &posBall );
  double  dDist   = posPred.getDistanceTo( posBall ) - 
                          SS->getPlayerSize() - SS->getBallSize();
  Log.log( 510, "check collision dist %f, noise_ball %f noise_me %f",
           dDist, getBallSpeed()*SS->getBallRand(), 
           velPred.getMagnitude()*SS->getPlayerRand() );

  // we could also take into account the error in player movement, but this
  // is very large, so we would in many cases get a dash
  if( dDist < getBallSpeed()*SS->getBallRand() )
    return true;

  return false;
}