worldmodel.cpp

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

      }
    }
  }

  // if opponent with highest x is nr 1, assume it is goalkeeper when standing
  // in own penalty area, otherwise assume goalkeeper closest player to goal.
  if( (objOppMaxX == OBJECT_OPPONENT_1 && x > PENALTY_X + 4.0 ) ||
      (objOppMaxX != OBJECT_ILLEGAL    && x > PITCH_LENGTH/2.0 - 6.0 &&
       fabs( y ) < SS->getGoalWidth()/2.0 ))
    return objOppMaxX;
  return OBJECT_ILLEGAL;
}


/*! This method returns the object type of the own goalkeeper. Which object
    type is the actual goalkeeper is checked in different ways. First of all
    this information is available in the see, when (goalie) is behind the
    perceived object. When no player is stored in the world model of which
    this is perceived, the own goalkeeper is assumed the player with the lowest
    x coordinate, but only if this player stands close in front of the goal.
    \return ObjectT that represents the own goalkeeper, OBJECT_ILLEGAL if
    it cannot be determined which object type is the own goalkeeper. */
ObjectT WorldModel::getOwnGoalieType()
{
  ObjectT objOwnMinX = OBJECT_ILLEGAL;
  double x = 100.0, y = UnknownDoubleValue;
  for( int i = 0; i < MAX_TEAMMATES; i++ )
  {
    if( isConfidenceGood( Teammates[i].getType( ) ) )
    {
      if( Teammates[i].getIsGoalie() == true )
        return Teammates[i].getType();
      if( Teammates[i].getGlobalPosition().getX() < x )
      {
        x          = Teammates[i].getGlobalPosition().getX();
        y          = Teammates[i].getGlobalPosition().getY();
        objOwnMinX = Teammates[i].getType();
      }
    }
  }
  if( ( objOwnMinX == OBJECT_TEAMMATE_1 && x < - ( PENALTY_X + 4.0 ) ) ||
      (objOwnMinX != OBJECT_ILLEGAL    && x < -  PITCH_LENGTH/2.0 + 6.0  &&
       fabs( y ) < SS->getGoalWidth()/2.0 ))
    return objOwnMinX;
  return OBJECT_ILLEGAL;
}


/*! This method returns the last server cycle the specified object has been
    seen.
    \param object type of object that should be checked
    \return server time this object was last seen (in a see message). */
Time WorldModel::getTimeLastSeen( ObjectT o )
{
  Object *object = getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeLastSeen(  );
  return Time( -1, 0);

}

/*! This method returns the last server cycle the relative distance change of
    the specified object has been reported.
    \param object type of object that should be checked
    \return server time relative distance change of this object was last
           seen (in a see message). */
Time WorldModel::getTimeChangeInformation( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeChangeInformation(  );
  return Time( -1, 0);

}

/*! This method returns the last global position derived from a see message.
    The time corresponds to the method 'getTimeGlobalPositionLastSee'. 
    \param object type of object that should be checked
    \return global position related to the last see message. */
VecPosition WorldModel::getGlobalPositionLastSee( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalPositionLastSee(  );
  return VecPosition( UnknownDoubleValue, UnknownDoubleValue);
}
             
/*! This method returns the last server cycle the global position 
    the specified object has been calculated.
    \param object type of object that should be checked
    \return server time global position of this object was last
           derived (from a see message). */
Time WorldModel::getTimeGlobalPositionLastSee( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeGlobalPosDerivedFromSee(  );
  return UnknownTime;
}

/*! This method returns the last global velocity derived from a see message.
    The time corresponds to the method 'getTimeChangeInformation'. 
    \param object type of object that should be checked
    \return global velocity related to the last see message. */
VecPosition WorldModel::getGlobalVelocityLastSee( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalVelocityLastSee(  );
  return VecPosition( UnknownDoubleValue, UnknownDoubleValue);
}

/*! This method returns the last global body angle derived from a see message.
    The time corresponds to the method 'getTimeChangeInformation'. 
    \param object type of object that should be checked
    \return global body angle related to the last see message. */
AngDeg WorldModel::getGlobalBodyAngleLastSee( ObjectT o )
{
  PlayerObject *object = (PlayerObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalBodyAngleLastSee(  );
  return UnknownAngleValue;
}

/*! This method returns the number of cycles it will take the tackle to expire.
    In case of the argument OBJECT_ILLEGAL, the number of cycles for the 
    agentObject is returned. */
int WorldModel::getTackleExpires( ObjectT o )
{
  if( o == OBJECT_ILLEGAL || o == getAgentObjectType() )
    return agentObject.getTackleExpires();

  PlayerObject *object = (PlayerObject*)getObjectPtrFromType( o );
  if( object == NULL )
    return 0;
  return max(0, object->getTimeTackle( ) -
                getCurrentTime()         +
                SS->getTackleCycles());
}

/*! This method returns the arm direction of object 'o'. It does not keep track
    of how relevant this information is. When the pointing agent stops pointing
    the angle is set to UnknownAngleValue. */
AngDeg WorldModel::getGlobalArmDirection( ObjectT o )
{
  PlayerObject *object = (PlayerObject*)getObjectPtrFromType( o );
  if( object == NULL )
    return UnknownAngleValue;

  return object->getGlobalArm();
}

/*! This method returns the time related to the global arm direction of object
    'o'. When the pointing agent stops pointing the angle is set to 
    UnknownAngleValue and the returned time is not relevant. */
Time  WorldModel::getTimeGlobalArmDirection( ObjectT o )
{
  PlayerObject *object = (PlayerObject*)getObjectPtrFromType( o );
  if( object == NULL )
    return Time(-1,0);

  return object->getTimeGlobalArm();
}

/*! This method returns the probability that a tackle performed by object o
    will succeed. This probability depends on the relative distance to the
    ball in both the x and y direction and various server parameters. In the 
    case that o equals OBJECT_ILLEGAL, the returned probability corresponds
    to that of the agent object. */
double WorldModel::getProbTackleSucceeds( ObjectT o, int iExtraCycles,
                                          VecPosition *pos)
{
  if( o == OBJECT_ILLEGAL )
    o = getAgentObjectType();
     
  VecPosition posObject   = getGlobalPosition( o );
  VecPosition posBall     = (pos == NULL ) ? getBallPos() : *pos ;
  AngDeg      angBody     = getGlobalBodyAngle( o );
  int         iExtra      = 0;
  double      dTackleDist, dDist=0;

  // if opponent goalie is within 3 metres he can probably catch in next cycle
  // RC2003 HACK
  if( o == getOppGoalieType() &&
      posBall.getDistanceTo( o ) < 3.0  )
    return 1.0;

  if( o != getAgentObjectType() )
  {
    // get the number of cycles object was not seen and assume he moves 0.6
    // in every cycle. Only in case of bad body direction subtract one.
    // then move object position closer to the ball
    dDist  = posBall.getDistanceTo( posObject );
    iExtra = getCurrentTime() - getTimeLastSeen( o ) + iExtraCycles;
    AngDeg ang    = (posBall - posObject).getDirection();   

    // if body angle ok, 
    if( getCurrentTime() - getTimeGlobalAngles( o ) < 2 )
    {
      if( fabs( VecPosition::normalizeAngle( ang - angBody ) ) > 35 )
	iExtra --;
      if( getGlobalVelocity( o ).getMagnitude() < 0.2 )
	iExtra --;
    }

    double dExtra = 0.7*max( 0, iExtra );

    // if object was not seen in last see message, he stood further away
    // then the visible_distance. 
    if( getTimeLastSeen( o ) != getTimeLastSeeMessage() &&
        getCurrentTime() == getTimeLastSeeMessage() &&
        dDist - dExtra < SS->getVisibleDistance() )
    {
      Log.log( 560, "prob tackle succeeds: opp not seen raise dExtra" );
      dExtra = dDist - SS->getVisibleDistance();
    }

    // now incorporate that to kick the ball we may need more cycles         
    //    dExtra = max( 0, dExtra + iExtraCycles );    

    // do not move object more than 4.0 metres.
    posObject += VecPosition( min(4.0,min(dDist - 0.2, dExtra )), ang, POLAR );

    // object is directed towards ball
    angBody = ang;
  }

  VecPosition posBallRel  = posBall - posObject; 
  posBallRel.rotate( - angBody ); 
  if ( posBallRel.getX() > 0.0 )      // ball in front -> tackle_dist parameter
    dTackleDist = SS->getTackleDist(); 
  else
    dTackleDist = SS->getTackleBackDist();
   
  double dProb = 
     pow(fabs(posBallRel.getX())/dTackleDist         ,SS->getTackleExponent())+
     pow(fabs(posBallRel.getY())/SS->getTackleWidth(),SS->getTackleExponent());
  
  Log.log( 556, 
           "tackle relpos o %d: (%f,%f) dist %f body %f extra %d %d: prob %f",
           o, posBallRel.getX(),posBallRel.getY(), dDist, angBody, iExtra, 
           iExtraCycles, max(0,1-dProb) );
  
  return max( 0, 1 - dProb );
}

/*! This method returns a list with all the opponents that are location within
  distance of 'dDist' of position 'pos'. */
list<ObjectT> WorldModel::getListCloseOpponents( VecPosition pos,double dDist )
{
  int iIndex;
  list<ObjectT> listOpp;

  for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS  ) )
  {
    if( getGlobalPosition( o ).getDistanceTo( pos ) < dDist )
      listOpp.push_back( o );
  }
  iterateObjectDone( iIndex );
  return listOpp;
}

/*! This method returns the tackle probability of the closest opponent to the
    ball. */
double WorldModel::getProbTackleClosestOpp( int iExtraCycles )
{
  ObjectT obj = getClosestInSetTo( OBJECT_SET_OPPONENTS, OBJECT_BALL );
  if( obj == OBJECT_ILLEGAL )
    return -1.0;
  
  return getProbTackleSucceeds( obj, iExtraCycles );
}

/*! This method sets the value of the specified object to a known
    player or not.  A known player is a player of which the exact team
    name and player number are known. If the player number is not
    known, information about the object is stored at an empty position
    in the player array and the value of isKnownPlayer is set to
    'false'.

    \param o object type of which known player information should be set
    \param isKnownPlayer new known player value
    \return boolean indicating whether update was successful */
bool WorldModel::setIsKnownPlayer( ObjectT o, bool isKnownPlayer )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  return object->setIsKnownPlayer( isKnownPlayer );
}

/*! This method sets the time the object 'o' has last been seen.
    \param o object of which the time should be changed  
    \param time new time for this object
    \return bool indicating whether update was successful. */
bool WorldModel::setTimeLastSeen( ObjectT o, Time time )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  return object->setTimeLastSeen( time );
}

/*! This method sets the heterogeneous player type of the object that is
    passed as the first argument.
    \param o object type of which the heterogeneous player type should be set
    \param iPlayer new heterogeneous player type of this object. */
bool WorldModel::setHeteroPlayerType( ObjectT  o, int iPlayerType )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  if( o == getAgentObjectType() )
    updateSSToHeteroPlayerType( iPlayerType );

  return object->setHeteroPlayerType( iPlayerType );
}

/*! This method returns the player type of the object 'o' in the
  current formation. */
PlayerT WorldModel::getPlayerType ( ObjectT o )
{
  if( o == OBJECT_ILLEGAL )
    o = getAgentObjectType();
  return formations->getPlayerType( SoccerTypes::getIndex( o ) );
}

/*! This method returns whether the object 'o' is located in the set of
    player types 'ps'. */
bool WorldModel::isInPlayerSet( ObjectT o, PlayerSetT ps )
{
  return SoccerTypes::isPlayerTypeInSet( getPlayerType( o ), ps );
}


/*! This methods returns the heterogeneous player type of object 'obj'. 
    Initially, the player types of all players are set to the type of the agent
    itself. After new information from the coach, they are set correctly. */
int WorldModel::getHeteroPlayerType( ObjectT obj )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( obj );

  return object->getHeteroPlayerType( );

}


/*! This method returns the global position of the opponent goal.
    \return VecPosition containing the position of the opponent goal. */
VecPosition WorldModel::getPosOpponentGoal( )
{
  ObjectT objGoal = SoccerTypes::getGoalOpponent( getSide() );
  if( isPenaltyUs() || isPenaltyThem() )
    objGoal = ( getSidePenalty() == SIDE_LEFT ) ?OBJECT_GOAL_L:OBJECT_GOAL_R ;
  return SoccerTypes::getGlobalPositionFlag(
                        objGoal,
                        getSide( ),
                        SS->getGoalWidth() );
}

/*! This method returns the global position of the own goal.
    \return VecPosition containing the position of the own goal. */
VecPosition WorldModel::getPosOwnGoal( )
{
  SideT sideGoal = getSide();
  ObjectT objGoal = SoccerTypes::getOwnGoal( sideGoal );
  if( isPenaltyUs() || isPenaltyThem() ) 
    objGoal = (getSidePenalty() == SIDE_LEFT ) ? OBJECT_GOAL_L : OBJECT_GOAL_R;

  return SoccerTypes::getGlobalPositionFlag(
                        objGoal,
                        getSide( ),
                        SS->getGoalWidth() );