worldmodelhighlevel.cpp

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

        objLog             = objFastestOpp;
        bFinishedOpponents = true;

      }
      else
        bContinue = false;

      if( featLog != FEATURE_ILLEGAL )
      {
        Log.log( 460, "log feature %d object %d in %d cycles sense %d see %d",
          featLog, objLog, iCyclesLog,getTimeLastSenseMessage().getTime(),
          getTimeLastSeeMessage().getTime()  );
        setFeature( featLog,
                    Feature( getTimeLastSeeMessage(),
                             getTimeLastSenseMessage(),
                             getTimeLastHearMessage(), objLog,
                             getTimeLastSeeMessage().getTime() + iCyclesLog));
      }
    }
    bFinished = bFinishedTeammates && bFinishedTeammatesNoGoalie;
    if( bFinished == true )
      bOnlyMe = true;
    bFinished &= bFinishedMe ;
    if( bFinished == true )
      set = OBJECT_SET_OPPONENTS;
    bFinished &= bFinishedOpponents;
  }
  Log.log( 460, "creatIntercept: team %d me %d opp %d",
     iCyclesFastestTeamNoGoalie, iCyclesFastestMe, iCyclesFastestOpp );
}


/*! This method returns the fastest object to a specified object and fills the
    last argument with the predicted amount of cycles needed to intercept
    this object. Only objects within the set 'set' are taken into
    consideration and the objects have to have a confidence higher than the
    player confidence threshold defined in PlayerSettings.
    \param set ObjectSetT which denotes objects taken into consideration
    \param obj object type of object that should be intercepted
    \param iCyclesToIntercept will be filled with the amount of cycles needed
    \returns object that can intercept object obj fastest */
ObjectT WorldModel::getFastestInSetTo( ObjectSetT set, ObjectT obj,
                              int *iCyclesToIntercept )
{
  ObjectT  objFastestOpp  = OBJECT_ILLEGAL, objFastestTeam  = OBJECT_ILLEGAL;
  int      iCyclesFastestOpp = 30; // how much do we try
  int     iCyclesFastestTeam;
  bool  bSkip = false;

  FeatureT        feature_type = FEATURE_ILLEGAL;
  ObjectT         fastestObject = OBJECT_ILLEGAL;
  int             iCycles       = -1;

  if( obj == OBJECT_BALL )
  {
    switch( set )
    {
      case OBJECT_SET_OPPONENTS:
        feature_type = FEATURE_FASTEST_OPPONENT_TO_BALL;
        break;
      case OBJECT_SET_TEAMMATES:
        feature_type = FEATURE_FASTEST_TEAMMATE_TO_BALL;
        break;
      case OBJECT_SET_TEAMMATES_NO_GOALIE:
        feature_type = FEATURE_FASTEST_TEAMMATE_TO_BALL_NO_GOALIE;
        break;
      case OBJECT_SET_PLAYERS:
        objFastestOpp =
         getFastestInSetTo( OBJECT_SET_OPPONENTS, obj, &iCyclesFastestOpp);
        objFastestTeam =
         getFastestInSetTo( OBJECT_SET_TEAMMATES, obj, &iCyclesFastestTeam);
        if( iCyclesFastestOpp < iCyclesFastestTeam )
        {
          fastestObject = objFastestOpp;
          iCycles = iCyclesFastestOpp;
        }
        else
        {
          fastestObject = objFastestTeam;
          iCycles = iCyclesFastestTeam;
        }
        bSkip = true;
        feature_type = FEATURE_FASTEST_PLAYER_TO_BALL;
        break;
      default:
        cerr << "WorldModel::getFastestInSetTo unknown set: " << set << endl;
        return OBJECT_ILLEGAL;
    }
    if( isFeatureRelevant( feature_type ) )
    {
      int i = max(0,
             ((int)getFeature( feature_type ).getInfo() - getCurrentCycle() ));
      if( iCyclesToIntercept != NULL )
        *iCyclesToIntercept  = i;
      return getFeature( feature_type ).getObject();
    }

    Log.log( 460, "create intercept features" );
    createInterceptFeatures( );
    Log.log( 460, "call fastest again" );
    return getFastestInSetTo( set, obj, iCyclesToIntercept );
    if( set == OBJECT_SET_TEAMMATES || set == OBJECT_SET_TEAMMATES_NO_GOALIE )
      objFastestOpp =
         getFastestInSetTo( OBJECT_SET_OPPONENTS, obj, &iCyclesFastestOpp);
  }

  // no feature available, calculate information
  double          dConfThr      = PS->getPlayerConfThr();
  int             iCyclesToObj  ;
  int             iMinCycles    = 100;
  int             iIndex;
  VecPosition     posObj;


  while( bSkip == false &&
         iCycles < iMinCycles &&
         iCycles <= iCyclesFastestOpp )
  {
    iCycles++;
    iMinCycles = 100;
    posObj     = predictPosAfterNrCycles( obj, iCycles );
    Log.log( 460, "fastest loop: %d fastest_opp %d", 
             iCycles, iCyclesFastestOpp );
    for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
    {
      if( getGlobalPosition(o).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
              < iMinCycles &&
          getGlobalPosition(o).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
              < iCycles + 1 )
      {
        Log.log( 460, "call predictNrCyclesToPoint %d %d", 
                 iCycles,iMinCycles );
        iCyclesToObj = predictNrCyclesToPoint( o, posObj );
        if( iCyclesToObj < iMinCycles )
        {
          iMinCycles = iCyclesToObj;
          fastestObject = o;
        }
      }
    }
    iterateObjectDone( iIndex );
  }
  
  // opponent is faster and we haven't calculated who can go to the 
  // interception point the fastest
  if( fastestObject == OBJECT_ILLEGAL )
    fastestObject = getFastestInSetTo(set,posObj,VecPosition(0,0),0, &iCycles);

  if( iCyclesToIntercept != NULL )
    *iCyclesToIntercept  = iCycles;

  if( feature_type != FEATURE_ILLEGAL )
  {
    Log.log( 460, "log feature %d object %d in %d cycles sense %d see %d",
             feature_type, fastestObject, iCycles,getTimeLastSenseMessage().
             getTime(), getTimeLastSeeMessage().getTime()  );
    setFeature( feature_type,
                Feature( getTimeLastSeeMessage(),
                         getTimeLastSenseMessage(), 
                         getTimeLastHearMessage(), fastestObject,
                         getTimeLastSeeMessage().getTime() + iCycles ) );
  }

  return fastestObject;
}

/*! This method returns the fastest object to another object that is currently
    located at position 'pos' and has velocity 'vel' that decays with a value
    'dDecay'. The last argument will be filled with the predicted amount of
    cycles needed to reach this object.
    \param set ObjectSetT which denotes objects taken into consideration
    \param pos current position of the object
    \param vel current velocity of the object
    \param dDecay decay value of the velocity of the object
    \param iCyclesToIntercept will be filled with the amount of cycles needed
    \returns object that can reach it fastest */
ObjectT WorldModel::getFastestInSetTo( ObjectSetT set, VecPosition pos,
                    VecPosition vel, double dDecay, int *iCyclesToIntercept)
{
  double  dConfThr      = PS->getPlayerConfThr();
  ObjectT fastestObject = OBJECT_ILLEGAL;
  int     iCycles       = 0;
  int     iCyclesToObj  ;
  int     iMinCycles    = 100;
  int     iIndex;

  while( iCycles <= iMinCycles && iCycles < 100)
  {
    iCycles    = iCycles + 1  ;
    iMinCycles = 100;
    Log.log( 460, "fastest to point: %d", iCycles );
    for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
    {
       if( getGlobalPosition(o).getDistanceTo(pos)/SS->getPlayerSpeedMax()
           < iMinCycles )
       {
         iCyclesToObj = predictNrCyclesToPoint( o, pos );
         if( iCyclesToObj < iMinCycles )
         {
           iMinCycles    = iCyclesToObj;
           fastestObject = o;
         }
       }
    }
    iterateObjectDone( iIndex );
    pos += vel;
    vel *= dDecay;
    if( vel.getMagnitude( ) < EPSILON ) // we can quit
    {
      iCycles = iMinCycles;
      iMinCycles--;
    }
  }
  
  if( iCyclesToIntercept != NULL )
    *iCyclesToIntercept = iCycles;
  return fastestObject;
}

/*! This method returns the first empty spot in the set 'set'. The
    first empty spot is returned as the object which has a lower
    confidence than the threshold player_conf_thr defined in the
    PlayerSettings. This can be used when information of an unknown
    object is perceived. It is set on the first position where there
    is currently no information stored. If 'iUnknownPlayer' is
    specified, the range that corresponds to this unknown player is
    used to dermine the position.

    \param set ObjectSetT consisting of the objects to check
    \param iUnknownPlayer indicates the unknownplayer that has to be mapped

    \return object type of which currently no up to date information
    is stored */
ObjectT WorldModel::getFirstEmptySpotInSet( ObjectSetT set, int iUnknownPlayer)
{
  int     iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set,  0.0, true );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, 0.0, true ) )
  {
    if( getConfidence( o ) <= PS->getPlayerConfThr() &&
        o != getAgentObjectType() )
      return o;
  }
  return OBJECT_ILLEGAL;
}


/*! This method returns a truth value that represents whether the object
    supplied as the first argument was seen in the last see message. If
    "touch" information was received, i.e. the object was very close to the
    agent but not in its view cone, false is also returned.
    \param ObjectT that represent the type of the object to check
    \return bool indicating whether o was seen in the last see message */
bool WorldModel::isVisible( ObjectT o )
{
  Object *object = getObjectPtrFromType( o );

  if( object != NULL &&
      object->getTimeLastSeen() == getTimeLastSeeMessage() )
    return true;

  return false;
}

/*! This method determines whether the ball is kicakble, i.e. the ball
    is in the kickable range of the agent (see ServerSettings). This
    value can be different for the different heterogeneous player
    types.

    \return bool indicating whether ball can be kicked. */
bool WorldModel::isBallKickable()
{
  return getRelativeDistance( OBJECT_BALL ) < SS->getMaximalKickDist();
}

/*! This method determines whether the ball is catchable. This only applies
    to a goalie. Three things are tested:
    - the ball hasn't been catched in catch_ban_cycles (see ServerSettings)
      before the current cycle
    - the relative distance to the ball is smaller than the length of the
      catchable area length of the goalie (see also ServerSettings)
    - the ball is in the (own) penalty area.

    \return true when above three constraints are met, false otherwise. */
bool WorldModel::isBallCatchable()
{
  return getTimeSinceLastCatch()            > SS->getCatchBanCycle()  &&
         getRelativeDistance( OBJECT_BALL ) <= SS->getCatchableAreaL() &&
         isInOwnPenaltyArea( getBallPos() );
}

/*! This method checks whether the ball is currently heading towards our own
    goal. For the ball to be heading to our goal a few constraints must be met:
    - ball must be located in our penalty area
    - line of ball heading must intersect with goal line within goal width +
    small constant.
    - ball must pass goal line within 20 cycles.

    If all these constraints are met true is returned, false otherwise
    \return bool indicating whether the ball is heading towards our own goal */
bool WorldModel::isBallHeadingToGoal(  )
{
  int iSide = 1;

  if( isPenaltyUs() || isPenaltyThem() )
    iSide = ( getSide() == getSidePenalty() ) ? 1 : -1;

  if( !isConfidenceGood( OBJECT_BALL ) ||
      fabs( getBallPos().getX() ) <  PENALTY_X - 5.0 )
  {
    Log.log( 553, "ball not towards goal: confidence too low" );
    return false;
  }

  // make line from ball heading and goal line
  Line l = Line::makeLineFromPositionAndAngle(getBallPos(),getBallDirection());
  Line l2= Line::makeLineFromTwoPoints( getPosOwnGoal(), getPosOwnGoal() +
                                                         VecPosition( 0, 10 ));

  // if intersection is outside goalwidth, not heading to goal
  VecPosition posIntersect = l.getIntersection( l2 );
  if( fabs(posIntersect.getY()) > SS->getGoalWidth()/2.0 + 3.0)
  {
    Log.log( 553, "ball not towards goal: outside goal %f",
       posIntersect.getY());
    return false;
  }

  // check whether ball will be behind goal line within 20 cycles.
  VecPosition pos    = getBallPos();
  int         iCycle = 1;
  while( fabs( pos.getX() ) < PITCH_LENGTH/2.0 && iCycle < 20)
  {
    pos = predictPosAfterNrCycles( OBJECT_BALL, iCycle );