worldmodelpredict.c

uva trilearn的robocup源程序

    \param iCycles pos is predicted after this number of cycles
    \param iDashPower dash power that is used every cycle in dash (default 100)
    \param vel will be filled with predicted global velocity after iCycles
    \return predicted global position after iCycles. */
VecPosition WorldModel::predictPosAfterNrCycles( ObjectT o, int iCycles,
                                       int iDashPower, VecPosition *velocity )
{
  VecPosition vel = getGlobalVelocity( o );
  VecPosition pos = getGlobalPosition( o );

  if( o == OBJECT_BALL )
  {
    // get the speed and the distance it travels in iCycle's.
    // use this distance and direction it travels in, to calculate new pos
    // geom series is serie s=a+ar+..+ar^n...decay=r,iCycles=n,dSpeed=a
    double dDist = Geometry::getSumGeomSeries( vel.getMagnitude(),
                                               SS->getBallDecay(),
                                               (double)iCycles);
    pos          += VecPosition( dDist, vel.getDirection(), POLAR );
  }
  else if( SoccerTypes::isKnownPlayer( o )  )
  {
    double      dDirection = 0.0; // used when no info about global body
    Stamina     stamina;          // used when object is agent

    if( getAgentObjectType() == o )
    {
      dDirection = getAgentGlobalBodyAngle();
      stamina    = getAgentStamina();
    }
    else if( getTimeGlobalAngles(o) > getCurrentTime() - 2 )
      dDirection = getGlobalBodyAngle(o);

    for( int i = 0; i < iCycles ; i ++ )
      predictStateAfterDash( iDashPower, &pos, &vel, &stamina, dDirection );
  }
  if( velocity != NULL )
    *velocity = vel;
  return pos;
}

/*! This method predicts the position of the agent after 'iCycles' when every
    cycle is dashed with 'iDashPower'. The method
    'predictGlobalPosAfterNrCycles' is used to calculate this position.
    \param iCycles number of cycles
    \param iDashPower dash power that is passed
    \return VecPosition indicating predicted global position of agent. */
VecPosition WorldModel::predictAgentPos( int iCycles, int iDashPower )
{
  return predictPosAfterNrCycles( getAgentObjectType(), iCycles, iDashPower);
}

/*! This method gives an estimate for the number of cycles it will a player 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,
                                        AngDeg angToTurn )
{
  VecPosition posGlobal        = getGlobalPosition( o );
  AngDeg      angBody;
  AngDeg      ang;

  if( getAgentObjectType() == o )
    angBody = getAgentGlobalBodyAngle();
  else if( abs(getTimeGlobalAngles(o)-getCurrentTime()) < 2 )
    angBody = getGlobalBodyAngle( o );
  else
    angBody = 180;

  int iExtraCycles;
  ang = fabs(angBody-(posTo-posGlobal).getDirection());
  if( ang > 20 && ang < 100 )
    iExtraCycles = 1;
  else if( ang > 100 )
    iExtraCycles = 2;
  else
    iExtraCycles = 0;

  if( posGlobal.getDistanceTo( posTo ) < SS->getMaximalKickDist() )
    return 0;
  else
    return iExtraCycles + int((posGlobal.getDistanceTo( posTo )
           /min(1.0, SS->getPlayerSpeedMax())));
}

/*! 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;

  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 );
    iCyclesToObj = predictNrCyclesToPoint ( objFrom, posObj,
                                           PS->getPlayerWhenToTurnAngle() );
  }
  if( iCycles == PS->getPlayerWhenToIntercept() )
    printf( "(WorldModel::predictNrCyclesToObject) Too much cycles??\n" );

  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 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::isCollisionAfterDash( SoccerCommand soc )
{
  if( soc.commandType != CMD_DASH )
    return false;

  VecPosition posPred = predictAgentPosAfterCommand( soc );
  VecPosition posBall = predictPosAfterNrCycles( OBJECT_BALL, 1 );
  if( posPred.getDistanceTo( posBall ) < 
              SS->getPlayerSize() + SS->getBallSize()  )
   return true;
   
  return false;
}