worldmodelpredict.cpp

自己写的robocup-2d程序

int WorldModel::predictNrCyclesForDistance ( ObjectT o, double dDist,
                                             double dSpeed )
{
  double dSpeedPrev = -1.0;
  int    iCycles    = 0;
  double dDecay     = getPlayerDecay( o );
  double dDashRate  = getDashPowerRate( o );
  double dMinDist   = getMaximalKickDist( o );

  // stop this loop when max speed is reached or the distance is traveled.
  while( dDist > dMinDist &&
         (fabs(dSpeed - dSpeedPrev) > EPSILON || dSpeed < 0.3 ) &&
         iCycles < 40 ) // ignore standing still and turning
  {
    dSpeedPrev = dSpeed;
    dSpeed    += SS->getMaxPower()*dDashRate;
    if( dSpeed > SS->getPlayerSpeedMax() )
      dSpeed = SS->getPlayerSpeedMax();
    dDist = max( 0, dDist - dSpeed );
    dSpeed *= dDecay;
    iCycles++;
  }
  dSpeed /= dDecay;
  
  // if distance not completely traveled yet, count the number of cycles to 
  // travel the remaining distance with this speed.
  if( dDist > dMinDist  )
   iCycles += (int)ceil(( dDist - dMinDist )/dSpeed);
  return max(0, iCycles ) ;
}


/*! This method gives an estimate for the number of cycles a player needs to
    reach a specific position. A position is reached when the player is located
    in the maximal kick distance of this position. When this is not the case
    a dash (or turn) is performed until the player is in the kickable distance.
    \param o objectT which wants to reach posTo
    \param posTo global position which has to be reached
    \param angToTurn angle to 'posTo' when is turned (instead of dashed)
    \return predicted nr of cycles for o to reach posTo  */
int WorldModel::predictNrCyclesToPoint( ObjectT o, VecPosition posTo )
{
  char          strBuf[128];
  VecPosition   posGlobal = getGlobalPositionLastSee( o ), posPred;
  VecPosition   vel;
  int           iCycles;
  AngDeg        angBody, angNeck = 0, ang;
  AngDeg        angDes = (posTo-posGlobal).getDirection();
  SoccerCommand soc;

  Log.log( 460, "predict steps for %s with dist %f (time %d) and body %f (%d)",
              SoccerTypes::getObjectStr( strBuf, o ),
              posTo.getDistanceTo( posGlobal ),
              getTimeGlobalPositionLastSee( o ).getTime(),
              getGlobalBodyAngle(o), getTimeChangeInformation(o).getTime() );

  // if already in kickable distance, return 0
  if( posTo.getDistanceTo( posGlobal ) < getMaximalKickDist( o) )
  {
    Log.log( 460, "already close: 0" );
    return 0;
  }

  // first check how old the change info (and thus body and vel.) info is
  // if too old, assume information is perfect and set time to position info
  // otherwise update all information with stored information
  iCycles = getTimeChangeInformation(o).getTime() - getCurrentCycle();
  if( o == getAgentObjectType() )
  {
    angBody = getAgentGlobalBodyAngle();
    vel     = getAgentGlobalVelocity( );
    posPred = getAgentGlobalPosition( );
    iCycles = 0;
  }
  else if( iCycles < -3 )
  {
    angBody = angDes;
    vel.setVecPosition( 0.3, angDes, POLAR );
    if( SoccerTypes::isOpponent( o ) )
      iCycles = -2; // otherwise too optimistic
    else
      iCycles = 0;
    posPred = getGlobalPositionLastSee( o );
  }
  else
  {
    angBody = getGlobalBodyAngleLastSee( o );
    vel     = getGlobalVelocityLastSee( o );
    posPred = getGlobalPositionLastSee( o );
  }

  Log.log( 460, "rel. time angle info (des %f,now %f,speed %f): %d (%d-%d)",
       angDes, angBody, vel.getMagnitude(), iCycles,
       getTimeChangeInformation(o).getTime(), getCurrentCycle() );

  if( o != getAgentObjectType() &&
      getTimeGlobalPositionLastSee( o ) > getTimeChangeInformation(o) )
  {
    Log.log( 460, "update cycles to global pos. time: %d",
                  getTimeGlobalPositionLastSee( o ).getTime() );
    iCycles = max(iCycles,
                  getTimeGlobalPositionLastSee(o).getTime()-getCurrentCycle());
  }

  soc = predictCommandToMoveToPos(o,posTo,1,2.5,false,&posPred,&vel,&angBody );
  ang = VecPosition::normalizeAngle( angBody - angDes );

  // sometimes we dash to stand still and turn then
  while( soc.commandType == CMD_TURN ||
         ( fabs( ang ) > 20 && soc.commandType == CMD_DASH && soc.dPower < 0 ))
  {
    iCycles++;
    predictStateAfterCommand( soc, &posPred, &vel, &angBody, &angNeck, o );
    if( posTo.getDistanceTo( posPred ) < getMaximalKickDist( o ) )
    {
      Log.log( 460, "reached point during turning, vel %f: %d",
                        vel.getMagnitude(), iCycles );
      return iCycles;
    }
    soc=predictCommandToMoveToPos(o,posTo,1,2.5,false,&posPred,&vel,&angBody );
    ang = VecPosition::normalizeAngle( angBody - angDes );
  }
  Log.log( 460, "cycles after turning: %d (ang %f, %f) vel %f", 
           iCycles, ang, angDes, vel.getMagnitude() );

  if( o != getAgentObjectType() )
  {
    // iCycles++; // do not count last dash -> predictState not called
    double dVel = vel.rotate(-angBody).getX(); // get distance in direction
    iCycles += predictNrCyclesForDistance(o,posPred.getDistanceTo(posTo),dVel);
  }
  else
  {
    while( posPred.getDistanceTo( posTo ) > getMaximalKickDist( o ) )
    {
      soc=predictCommandToMoveToPos(o,posTo,1,2.5,0,&posPred,&vel,&angBody);
      predictStateAfterCommand( soc, &posPred, &vel, &angBody, &angNeck, o );  
      iCycles++;
    }
  }

  Log.log( 460, "total cycles: %d", iCycles );
  return iCycles;
}

/*! This method returns the number of cycles it will take the object 'objFrom'
    to reach the object 'objTo' (usually respectively the player and the ball).
    \param objFrom ObjectT that is the object that wants to move
    \param objTo ObjectT to which is moved
    \return number of cycles it will take objFrom to move to objTo */
int WorldModel::predictNrCyclesToObject( ObjectT objFrom, ObjectT objTo )
{
  VecPosition posPrev(UnknownDoubleValue,UnknownDoubleValue);

  if( objFrom == OBJECT_ILLEGAL || objTo == OBJECT_ILLEGAL ||
      getGlobalPosition( objFrom ).getDistanceTo( getGlobalPosition( objTo )
      ) > 40 )
    return 101;

  // this is part of the intercept
  if( objFrom == getAgentObjectType() && objTo == OBJECT_BALL )
  {
     FeatureT  feature_type = FEATURE_INTERCEPT_CYCLES_ME;  
     if( isFeatureRelevant( feature_type ) )
     {
       return max(0,
             ((int)getFeature( feature_type ).getInfo() - getCurrentCycle() ));
     }
     else
     {
       Log.log( 460, "create intercept features" );
       createInterceptFeatures( );
       Log.log( 460, "call predict again" );
       return predictNrCyclesToObject( objFrom, objTo );
     }
  }

  // in case of ball with no velocity, calculate cycles to point
  if( objTo == OBJECT_BALL && getBallSpeed() < 0.01 )
    return predictNrCyclesToPoint( objFrom, getBallPos() );

  int         iCycles      = 0;
  int         iCyclesToObj = 100;
  VecPosition posObj(0,0);

  // continue calculating number of cycles to position until or we can get
  // earlier at object position, are past maximum allowed number of cycles or
  // the object does not move anymore.
  while( iCycles <= iCyclesToObj && iCycles < PS->getPlayerWhenToIntercept() &&
         posObj.getDistanceTo( posPrev ) > EPSILON )
  {
    iCycles      = iCycles + 1  ;
    posPrev      = posObj;
    posObj       = predictPosAfterNrCycles( objTo,   iCycles );

    if(getGlobalPosition(objFrom).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
              < iCycles + 1 )
    {
      Log.log( 460, "predictNrCyclesToPoint after %d cycles", iCycles );
      iCyclesToObj = predictNrCyclesToPoint ( objFrom, posObj );
    }
  }

  return iCyclesToObj;
}

/*! This method updates all the stamina variables using the calculations from
    the soccer manual. It is not really important since stamina is read from
    sense_body every cycle. That information is more up to date.
    \param power of last dash command
    \param stamina pointer to all stamina values, will change to new value
    \return stamina class will be updated to new stamina values */
void WorldModel::predictStaminaAfterDash( double dPower, Stamina *stamina )
{
  double sta = stamina->getStamina();
  double eff = stamina->getEffort();
  double rec = stamina->getRecovery();

  // double negative value when dashed backwards
  sta -= ( dPower > 0.0 ) ? dPower : -2*dPower ;
  if( sta < 0 ) sta = 0;

  // stamina below recovery threshold, lower recovery
  if( sta <= SS->getRecoverDecThr()*SS->getStaminaMax() &&
                         rec > SS->getRecoverMin() )
      rec -= SS->getRecoverDec();

  // stamina below effort decrease threshold, lower effort
  if( sta <= SS->getEffortDecThr()*SS->getStaminaMax() &&
                   eff > SS->getEffortMin() )
      eff -= SS->getEffortDec();

  // stamina higher than effort incr threshold, raise effort and check maximum
  if( sta >= SS->getEffortIncThr() * SS->getStaminaMax() &&
      eff < 1.0)
  {
    eff += SS->getEffortInc();
    if ( eff > 1.0 )
       eff = 1.0;
  }

  // increase stamina with (new) recovery value and check for maximum
  sta += rec*SS->getStaminaIncMax();
  if ( sta > SS->getStaminaMax() )
    sta = SS->getStaminaMax();

  stamina->setStamina ( sta );
  stamina->setEffort  ( eff );
  stamina->setRecovery( rec );
}

/*! This method returns the command for object 'obj' to turn towards a point
    'posTo' on the field when it has 'iCycles' to reach that point. If
    the point is within 'dDistBack' behind the object it will try to dash
    backwards. In the case that 'bMoveBack' is true, it will always try to
    move backwards. When posIn, velIn and angBodyIn are equal to NULL, the
    current agent information is used. */
SoccerCommand WorldModel::predictCommandTurnTowards( ObjectT obj, VecPosition
  posTo, int iCycles, double dDistBack, bool bMoveBack,
  VecPosition *posIn, VecPosition *velIn, AngDeg *angBodyIn )
{
  SoccerCommand soc, socFirst;
  VecPosition   pos, vel;
  AngDeg        angBody, ang, angNeck, angTo;
  Stamina       sta;
  bool          bFirst = true;

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

  // predict where we will finally stand when our current vel is propogated
  // and then check the orthogonal distance w.r.t. our body direction
  VecPosition posPred=predictPosAfterNrCycles( obj, min(iCycles,4),
                                               0, &pos, &vel, false );
  Line        line   =Line::makeLineFromPositionAndAngle( posPred, angBody );
  double      dDist  =line.getDistanceWithPoint( posTo );

  // get the angle to this point
  angTo = (posTo - posPred).getDirection();
  angTo = VecPosition::normalizeAngle( angTo - angBody );

  // determine whether we want to turn based on orthogonal distance 
  double dRatioTurn;
  if( pos.getDistanceTo(posTo) > 30.0 ) 
    dRatioTurn = 4.0;  
  if( pos.getDistanceTo(posTo) > 20.0 ) 
    dRatioTurn = 3.0;  
  else if( pos.getDistanceTo(posTo) > 10 ) 
    dRatioTurn = 2.0;
  else
    dRatioTurn = 0.90 ;

  AngDeg angTmp = angTo + (bMoveBack) ? 180 : 0;
  angTmp = VecPosition::normalizeAngle( angTmp );
  
  // turn when: 
  //  1. point lies outside our body range (forward and backwards)
  //  2. point lies outside distBack and behind us (forward move) 
  //  3. point lies outside distBack and in front of us backwards move)
  int    turn = 0;
  while( ( dDist > dRatioTurn*getMaximalKickDist( obj ) ||
          ( posPred.getDistanceTo( posTo ) > dDistBack && 
              ( ( fabs( angTo ) > 90 && bMoveBack == false  ) ||
                ( 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)