worldmodelhighlevel.cpp

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

  if( dDist != NULL )
    *dDist = dMinMag;
  return closestObject;
}

/*! This method returns the object type of the second closest object to the
    object type that is supplied as the second argument. Only objects are taken
    into account within set 'set' and with a confidence higher than the
    supplied threshold. If no threshold is supplied, the threshold defined in
    PlayerSettings is used.
    \param set ObjectSetT which denotes objects taken into consideration
    \param obj ObjectT that represent the type of the object to check
    \param dDist will be filled with the distance to this player.
    \param dConfThr minimum confidence threshold for the objects in 'set'
    \return ObjectType that is second closest to obj */
ObjectT WorldModel::getSecondClosestInSetTo ( ObjectSetT set, ObjectT obj,
                                              double *dDist,  double dConfThr )
{
  VecPosition v;
  ObjectT     closestObject       = OBJECT_ILLEGAL;
  ObjectT     secondClosestObject = OBJECT_ILLEGAL;
  double      dMinMag             = 1000.0;
  double      dSecondMinMag       = 1000.0;
  int         iIndex;

  if( dConfThr == -1.0 ) dConfThr = PS->getPlayerConfThr();

  for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
  {
    if( o != obj )
    {
      v = getGlobalPosition( obj ) - getGlobalPosition( o );
      if( v.getMagnitude() < dMinMag )                  // closer then first
      {
        dSecondMinMag         = dMinMag;                // put first to second
        secondClosestObject   = closestObject;
        dMinMag               = v.getMagnitude();       // and this to first
        closestObject         = o;
      }
      else if( v.getMagnitude() < dSecondMinMag )       // between 1st and 2nd
      {
        dSecondMinMag         = v.getMagnitude();       // put this to second
        secondClosestObject   = o;
      }
    }
  }
  iterateObjectDone( iIndex );
  if( dDist != NULL )
    *dDist = dSecondMinMag;
  return secondClosestObject;
}

/*! This method returns the object type of the second closest object relative
    to the agent. Only objects are taken into account within set 'set' and
    which where seen in the last see message.
    \param set ObjectSetT which denotes objects taken into consideration
    \param dDist will be filled with the distance to this this object
    \return ObjectType that is second closest to the agent */
ObjectT WorldModel::getSecondClosestRelativeInSet( ObjectSetT set, 
                                                   double *dDist )
{
  ObjectT     closestObject       = OBJECT_ILLEGAL;
  ObjectT     secondClosestObject = OBJECT_ILLEGAL;
  double      dMinMag             = 1000.0;
  double      dSecondMinMag       = 1000.0;
  double      d;
  int         iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, 1.0 ) )
  {
    d = getRelativeDistance( o );
    if( d < dMinMag )                                 // closer then first
    {
      dSecondMinMag         = dMinMag;                // put first to second
      secondClosestObject   = closestObject;
      dMinMag               = d;                      // and this to first
      closestObject         = o;
    }
    else if( d < dSecondMinMag )                      // between first and 2nd
    {
      dSecondMinMag         = d;                      // put this to second
      secondClosestObject   = o;
    }
  }
  iterateObjectDone( iIndex );
  if( dDist != NULL )
    *dDist = dSecondMinMag;
  return secondClosestObject;
}


/*! This method returns the object type of the furthest object to the
    ObjectT that is supplied as the second argument. Only objects are
    taken into account that are part of the set 'set' and have a
    confidence higher than the supplied threshold. If no threshold is
    supplied, the threshold defined in PlayerSettings is used.

    \param set ObjectSetT which denotes objects taken into consideration
    \param o ObjectT that represent the type of the object to compare to
    \param dDist will be filled with the furthest distance
    \param dConfThr minimum confidence threshold for the objects in 'set'
    \return ObjectType that is furthest to o */
ObjectT WorldModel::getFurthestInSetTo( ObjectSetT set, ObjectT objTarget,
                                        double *dDist, double dConfThr )
{
  if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();

  ObjectT     furthestObject = OBJECT_ILLEGAL;
  double      dMaxMag       = -1000.0;
  VecPosition v;
  int         iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
  {
    if( o != objTarget )
    {
      v = getGlobalPosition( objTarget ) - getGlobalPosition( o );
      if( v.getMagnitude() > dMaxMag )
      {
        dMaxMag        = v.getMagnitude();
        furthestObject = o;
      }
    }
  }
  iterateObjectDone( iIndex );
  if( dDist != NULL )
    *dDist = dMaxMag;
  return furthestObject;
}

/*! This method returns the type of the object that is located furthest
    relative to the agent. Only objects are taken into account
    that are part of the set 'set'.
    \param set ObjectSetT which denotes objects taken into consideration
    \param dDist will be filled with the furthest relative distance
    \return ObjectType that is furthest to the agent */
ObjectT WorldModel::getFurthestRelativeInSet( ObjectSetT set, double *dDist  )
{
  ObjectT     furthestObject = OBJECT_ILLEGAL;
  double      dMaxMag       = -1000.0;
  int         iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, 1.0 ) )
  {
    if( getRelativeDistance( o ) > dMaxMag )
    {
      dMaxMag        = getRelativeDistance( o );
      furthestObject = o;
    }
  }
  iterateObjectDone( iIndex );
  if( dDist != NULL )
    *dDist = dMaxMag;
  return furthestObject;
}


VecPosition WorldModel::getPosClosestOpponentTo( double *dDist, ObjectT o )
{
  if( o == OBJECT_ILLEGAL )
    o = getAgentObjectType();
  ObjectT objOpp = getClosestInSetTo( OBJECT_SET_OPPONENTS, o, dDist );
  if( objOpp == OBJECT_ILLEGAL )
    return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
    
  return getGlobalPosition( objOpp );
}

double WorldModel::getMaxTraveledDistance( ObjectT o )
{
  return (getCurrentTime() - getTimeLastSeen( o ) )*SS->getPlayerSpeedMax();
}


void WorldModel::createInterceptFeatures( )
{
  static int count = 0;
  static Time timeLastCalled(0,0);
  
  if( timeLastCalled == getTimeLastSenseMessage() )
    count++;
  else
    count = 0;
    
  if( count > 4 )
    cerr << getPlayerNumber() << " called createIntercept too often: " << 
       count << endl;
  // we check all possible next positions of the ball and see
  // whether a player (opponent or teammate) can reach the ball at that point
  // if so, we log this as a feature. We finish when all features have been
  // found.
  ObjectSetT      set = OBJECT_SET_PLAYERS;
  int             iCycles       = -1;
  int             iMinCyclesTeam    = 100;
  int             iMinCyclesOpp     = 100;
  bool            bOnlyMe           = false;

  VecPosition     posObj;
  int             iIndex;
  int             iCyclesToObj  ;

  // no feature available, calculate information
  ObjectT         objFastestTeam             = OBJECT_ILLEGAL;
  ObjectT         objFastestTeamNoGoalie     = OBJECT_ILLEGAL;
  ObjectT         objFastestOpp              = OBJECT_ILLEGAL;
  ObjectT         objFastestPlayer           = OBJECT_ILLEGAL;

  int             iCyclesFastestPlayer       = -1;
  int             iCyclesFastestTeam         = -1;
  int             iCyclesFastestTeamNoGoalie = -1;
  int             iCyclesFastestOpp          = -1;
  int             iCyclesFastestMe           = -1;

  bool            bFinishedPlayer            = false;
  bool            bFinishedTeammates         = false;
  bool            bFinishedTeammatesNoGoalie = false;
  bool            bFinishedOpponents         = false;
  bool            bFinishedMe                = false;
  bool            bFinished                  = false;

  ObjectT         objLog                     = OBJECT_ILLEGAL;
  int             iCyclesLog                 = -1;
  FeatureT        featLog                    = FEATURE_ILLEGAL;

  // for each next position of the ball
  while( bFinished == false && iCycles <= PS->getPlayerWhenToIntercept() )
  {
    iCycles++;
    iMinCyclesTeam = 100;
    iMinCyclesOpp  = 100;
    Log.log( 460, "fastest loop: %d", iCycles );

    // determine its position and traverse all players to check the teammate
    // and opponent who can reach it first
    posObj     = predictPosAfterNrCycles( OBJECT_BALL, iCycles );
    for( ObjectT o = iterateObjectStart( iIndex, set );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set ) )
    {
      if( getGlobalPosition(o).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
          < iCycles + 1 && (bOnlyMe == false || SoccerTypes::isOpponent( o )
          || o == getAgentObjectType() ) )
      {
        Log.log( 460, "call predictNrCyclesToPoint %d %d %d",
                       iCycles, iMinCyclesTeam, iMinCyclesOpp );
        iCyclesToObj = predictNrCyclesToPoint( o, posObj );

        if( iCyclesToObj < iMinCyclesOpp && SoccerTypes::isOpponent( o ) )
        {
          iMinCyclesOpp = iCyclesToObj;
          objFastestOpp = o;
        }
        if( iCyclesToObj < iMinCyclesTeam && SoccerTypes::isTeammate( o ) )
        {
          iMinCyclesTeam = iCyclesToObj;
          objFastestTeam = o;
        }
      }
    }
    iterateObjectDone( iIndex );

    bool bContinue = true;
    bool bLastCall = ( iCycles == PS->getPlayerWhenToIntercept() );
    // log all features that have been solved
    while( bContinue )
    {
      featLog = FEATURE_ILLEGAL;
      if( bLastCall )
        iCycles = 100;

      // if player not set yet and either team or opp is smaller than iCycles
      // set fastest player
      if( bFinishedPlayer == false &&
          ( min( iMinCyclesTeam, iMinCyclesOpp ) <= iCycles 
            ||
            bLastCall == true ) )
      {
        featLog              = FEATURE_FASTEST_PLAYER_TO_BALL;
        iCyclesLog           = iCycles;
        iCyclesFastestPlayer = iCycles;
        objLog               = (iMinCyclesTeam<=iMinCyclesOpp) ?
                                    objFastestTeam : objFastestOpp;
        objFastestPlayer     = objLog;
        bFinishedPlayer      = true;
      }
      // if teammate not set yet and min cycles team smaller set it
      else if( bFinishedTeammates == false &&
               (iMinCyclesTeam <= iCycles || bFinishedOpponents == true 
                || bLastCall))
      {
        if( bFinishedOpponents == true )
          objFastestTeam = getFastestInSetTo( OBJECT_SET_TEAMMATES, posObj,
                             VecPosition(0,0), 0, &iCycles );
        featLog            = FEATURE_FASTEST_TEAMMATE_TO_BALL;
        iCyclesLog         = iCycles;
        iCyclesFastestTeam = iCycles;
        objLog             = objFastestTeam;
        bFinishedTeammates = true;
      }
      else if( bFinishedTeammatesNoGoalie == false &&
         ( ( iMinCyclesTeam <= iCycles && objFastestTeam != getOwnGoalieType())
           || bFinishedOpponents == true || bLastCall ) )
      {
        if( bFinishedOpponents == true && objFastestTeam == getOwnGoalieType())
          objFastestTeam=getFastestInSetTo( OBJECT_SET_TEAMMATES_NO_GOALIE,
                            posObj, VecPosition(0,0), 0, &iCycles );
        featLog                   = FEATURE_FASTEST_TEAMMATE_TO_BALL_NO_GOALIE;
        iCyclesLog                 = iCycles;
        iCyclesFastestTeamNoGoalie = iCycles;
        objLog                     = objFastestTeam;
        objFastestTeamNoGoalie     = objFastestTeam;
        bFinishedTeammatesNoGoalie = true;
      }
      else if( bFinishedMe == false &&
        ((iMinCyclesTeam <= iCycles && objFastestTeam == getAgentObjectType())
        || bFinishedOpponents == true || bLastCall ) )
      {
        if( bFinishedOpponents == true && 
	    objFastestTeam != getAgentObjectType())
          iCycles = predictNrCyclesToPoint( getAgentObjectType(), posObj );
        featLog          = FEATURE_INTERCEPT_CYCLES_ME;
        iCyclesLog       = iCycles;
        iCyclesFastestMe = iCycles;
        objLog           = getAgentObjectType();
        bFinishedMe      = true;
      }
      else if( bFinishedOpponents == false &&
               ( iMinCyclesOpp <= iCycles  || bLastCall ) )
      {
        featLog            = FEATURE_FASTEST_OPPONENT_TO_BALL;
        iCyclesLog         = iCycles;
        iCyclesFastestOpp  = iCycles;