worldmodel.cpp

用robocup2d改编设计的２d球队。

			MaxValue=tempValue;
			m=tempM,n=tempN;
		}
	}
	return 	MaxValue;	
}
VecPosition WorldModel::GetPosition(int m,int n)
{
	VecPosition posAgent = getAgentGlobalPosition();
	VecPosition tempPos,arm;
	double dist=n*30.0/SN+15.0/SN;
	double ang=-90.0+m*180.0/HALF_SM+90.0/HALF_SM;
	tempPos.setX(dist*cos(ang/180.0*PAI));
	tempPos.setY(dist*sin(ang/180.0*PAI));
	arm=posAgent+tempPos;
	return arm;
}
/*! This constructor creates the worldmodel, all variables are initialized by
    default values
    \param ss reference to class in which all server parameters are stored
    \param ps reference to class in which all client parameters are stored
    \param fs reference to class in which all formation information is stored*/
WorldModel::WorldModel( ServerSettings *ss, PlayerSettings *ps, 
                        Formations *fs):agentObject(  )
{
  dTotalVarVel   = 0.0;
  dTotalVarPos   = 0.0;
  SS             = ss;
  PS             = ps;
  formations     = fs;
  bNewInfo       = false;

  setSide         ( SIDE_ILLEGAL ); // is set by init message
  strTeamName[0] = '\0';
  setPlayMode     ( PM_BEFORE_KICK_OFF );
  iGoalDiff      = 0;
  m_sidePenalty = SIDE_ILLEGAL;

  int i;
  for( i = 0; i < MAX_TEAMMATES ; i ++ )
    Teammates[i].setType( SoccerTypes::getTeammateObjectFromIndex( i ) );
  for( i = 0; i < MAX_OPPONENTS ; i ++ )
    Opponents[i].setType( SoccerTypes::getOpponentObjectFromIndex( i ) );
  for( i = 0; i < MAX_OPPONENTS + MAX_TEAMMATES ; i ++ )
    UnknownPlayers[i].setType( OBJECT_ILLEGAL );
  for( i = 0; i < MAX_FLAGS; i ++ )
    Flags[i].setType( OBJECT_ILLEGAL );
  for( i = 0; i < MAX_LINES; i ++ )
    Lines[i].setType( OBJECT_ILLEGAL );

  iNrUnknownPlayers     = 0;

  Ball.setType             ( OBJECT_BALL    );
  agentObject.setType      ( OBJECT_ILLEGAL );
  agentObject.setStamina   ( Stamina(SS->getStaminaMax(),1.0,1.0) );

  for( i = 0 ; i < CMD_MAX_COMMANDS ; i ++ )
  {
    queuedCommands[i].commandType = (CommandT)i;
    performedCommands[i]          = false;
    iCommandCounters[i]           = 0;
  }

  iNrHoles              = 0;
  iNrOpponentsSeen      = 0;
  iNrTeammatesSeen      = 0;
  bsCheckBall           = BS_ILLEGAL;

  // initialize the mutex for bNewInfo
#ifdef WIN32
  InitializeCriticalSection( &mutex_newInfo );
  event_newInfo = CreateEvent( NULL, TRUE, FALSE, NULL );
#else
  pthread_mutex_init( &mutex_newInfo, NULL );
  pthread_cond_init ( &cond_newInfo,  NULL );
#endif
  m_bRecvThink          = false;
  timeLastSenseMessage  = Time( 0, 1 );

  strcpy( m_colorPlayers[0], "ffb0b0" );   // pink
  strcpy( m_colorPlayers[1], "ffb000" );   // orange
  strcpy( m_colorPlayers[2], "00b000" );   // darkgreen
  strcpy( m_colorPlayers[3], "00ffff" );   // lightblue
  strcpy( m_colorPlayers[4], "ff00ff" );   // purple
  strcpy( m_colorPlayers[5], "ffff00" );   // yellow
  strcpy( m_colorPlayers[6], "ffffff" );   // white
  strcpy( m_colorPlayers[7], "0000ff" );   // blue
  strcpy( m_colorPlayers[8], "00ff00" );   // green
  strcpy( m_colorPlayers[9], "ff0000" );   // red
  strcpy( m_colorPlayers[10],"aaaaaa" );   // gray

  for( i = 0 ; i < MAX_FEATURES ; i ++ )
  {
    m_features[i].setTimeSee( Time( UnknownTime, 0 ) );
    m_features[i].setTimeSense( Time( UnknownTime, 0 ) );
  }
}

/*! Destructor */
WorldModel::~WorldModel()
{
#ifdef WIN32
  DeleteCriticalSection( &mutex_newInfo );
#endif
}

/*! This method returns a pointer to the Object information of the object
    type that is passed as the first argument.
    \param o ObjectType of which information should be returned
    \return pointer to object information of supplied ObjectT argument */
Object* WorldModel::getObjectPtrFromType( ObjectT o )
{
  Object *object = NULL;
  if( o == OBJECT_ILLEGAL )
    return NULL;

  if( SoccerTypes::isKnownPlayer( o ) )
  {
    if( o == agentObject.getType() )
      object = &agentObject;
    else if( SoccerTypes::isTeammate( o ) )
      object = &Teammates[SoccerTypes::getIndex(o)];
    else
      object = &Opponents[SoccerTypes::getIndex(o)];
  }
  else if( SoccerTypes::isFlag( o ) )
    object = &Flags[SoccerTypes::getIndex(o)];
  else if( SoccerTypes::isLine( o ) )
    object = &Lines[SoccerTypes::getIndex(o)];
  else if( SoccerTypes::isBall( o ) )
    object = &Ball;
  else if( o == OBJECT_OPPONENT_GOALIE )
    return getObjectPtrFromType( getOppGoalieType() );
  else if( o == OBJECT_TEAMMATE_GOALIE )
    return getObjectPtrFromType( getOwnGoalieType() );
 return object;
}

/*! This method sets the time of the last catch cycle. This information is
    received by the SenseHandler when the referee has sent this message.
    After a catch, the goalie is not allowed to catch the ball for
    catch_ban_cycles (defined in ServerSettings).
    \param iTime time the ball was catched. */
void WorldModel::setTimeLastCatch( Time time )
{
  timeLastCatch = time;
}

/*! This method returns the number of cycles since the last catch.
    \return cycles since last catch. */
int WorldModel::getTimeSinceLastCatch()
{
  if( timeLastCatch.getTime() == -1 )
    return 1000;
  return timeLastSenseMessage - timeLastCatch;
}

/*! This method sets the time of the last received referee message. This
    information is received by the SenseHandler.
    \param iTime time the referee sent the last message. */
bool WorldModel::setTimeLastRefereeMessage( Time time )
{
  timeLastRefMessage = time;
  return true;
}

/*! This method returns the time of the last received referee message.
    \return time of last received referee message. */
Time WorldModel::getTimeLastRefereeMessage( )
{
  return timeLastRefMessage;
}

/*! This method returns the current time. In case of a player this is the
    time of the last sense message, in case of the coach this is the time of
    the last see_global message.
    \return actual time */
Time WorldModel::getCurrentTime()
{
  if( getPlayerNumber() == 0 )
    return getTimeLastSeeGlobalMessage();
  else
    return getTimeLastRecvSenseMessage();
}

/*! This method returns the current cycle number. In case of a player this is
    the cycle of the last sense message, in case of the coach this is the cycle
    of the last see_global message.
    \return actual time */
int WorldModel::getCurrentCycle()
{
  return getCurrentTime().getTime();
}


/*! This method returns whether the time of the server stands
    still. This occurs during non play-on modes (kick_in, kick_off,
    etc.).

    \return bool indicating whether time of the server stands still. */
bool WorldModel::isTimeStopped()
{
  return getCurrentTime().isStopped();
}

/*! This method returns whether the last received message was a see or not.
    \return bool indicating whether the last received message was a see.*/
bool WorldModel::isLastMessageSee() const
{
  return getTimeLastSeeMessage() == getTimeLastSenseMessage() ;
}

/*! This method returns the time of the last see global message.
    This message can only be received by the coach.
    \return time of last see_global message */
Time WorldModel::getTimeLastSeeGlobalMessage( ) const
{
  return getTimeLastRecvSeeMessage();
}

/*! This method sets the time of the last see_global message.
    \param time see message has arrived
    \return true when update was succesful */
bool WorldModel::setTimeLastSeeGlobalMessage( Time time )
{
  updateRelativeFromGlobal( );
  return setTimeLastSeeMessage( time ); // set see message
}

/*! This method returns the time of the last see message
    \return time of last see message */
Time WorldModel::getTimeLastSeeMessage( ) const
{
  return timeLastSeeMessage;
}

/*! This method returns the time of the last received see message.
    The difference with getTimeLastSeeMessage is that that method returns
    the last see message that has been updated in the world model. In most
    cases these are equal.
    \return time of last received sense message */
Time WorldModel::getTimeLastRecvSeeMessage( ) const
{
  return timeLastRecvSeeMessage ;
}
/*! This method sets the time of the last see message. It also sends a
    condition signal to indicate that variable bNewInfo has
    changed. When main thread is stopped in waitForNewInformation, it
    is unblocked.

    \param time see message has arrived
    \return true when update was succesful */
bool WorldModel::setTimeLastSeeMessage( Time time )
{
  timeLastRecvSeeMessage  = time;
  if( SS->getSynchMode() == false )
  {
#ifdef WIN32
    //EnterCriticalSection( &mutex_newInfo );
    bNewInfo            = true;
    SetEvent            (  event_newInfo );
    //LeaveCriticalSection( &mutex_newInfo );
#else
    pthread_mutex_lock  ( &mutex_newInfo );
    bNewInfo            = true;
    pthread_cond_signal ( &cond_newInfo );
    pthread_mutex_unlock( &mutex_newInfo );
#endif
  }

  return true;
}

/*! This method returns the time of the last sense message
    \return time of last sense message */
Time WorldModel::getTimeLastSenseMessage( ) const
{
  return timeLastSenseMessage ;
}

/*! This method returns the time of the last received sense message.
    The difference with getTimeLastSenseMessage is that that method returns
    the last sense message that has been updated in the world model. In most
    cases these are equal.
    \return time of last received sense message */
Time WorldModel::getTimeLastRecvSenseMessage( ) const
{
  return timeLastRecvSenseMessage ;
}

  
/*! This method sets the time of the last sense message. It also send a
    condition signal to indicate that variable bNewInfo has changed.
    When main thread is stopped in waitForNewInformation, it is
    unblocked.
    \param time sense message has arrived
    \return true when update was succesful */
bool WorldModel::setTimeLastSenseMessage( Time time )
{
  timeLastRecvSenseMessage = time;
  if( SS->getSynchMode() == false )
  {
#ifdef WIN32
    //EnterCriticalSection( &mutex_newInfo );
    bNewInfo = true;
    SetEvent            (  event_newInfo );
  //LeaveCriticalSection( &mutex_newInfo );
#else
    pthread_mutex_lock  ( &mutex_newInfo );
    bNewInfo = true;
    pthread_cond_signal ( &cond_newInfo );
    pthread_mutex_unlock( &mutex_newInfo );
#endif
  }

  return true;
}

/*! This method returns the time of the last hear message
    \return time of last hear message */
Time WorldModel::getTimeLastHearMessage( ) const
{
  return timeLastHearMessage ;
}

/*! This method sets the time of the last hear message. 
    \param time hear message has arrived
    \return true when update was succesful */
bool WorldModel::setTimeLastHearMessage( Time time )
{
  timeLastHearMessage = time;
  return true;
}

/*! This method returns the player number of the agent. The player number is
    the fixed number which is given by the server after initialization.
    \return player number of this player. */
int WorldModel::getPlayerNumber( ) const
{
  return iPlayerNumber;
}

/*! This method sets the player number of the agent. This value is available in
    the conformation message sent by the soccerserver after the initialization.
    \param i new player number of the agent.
    \return bool indicating whether player number was set. */
bool WorldModel::setPlayerNumber( int i )
{
  iPlayerNumber = i;
  return true;
}

/*! This method returns the side of the agent. Note that
    the side of the agent does not change after half time.
    \return side (SIDE_LEFT or SIDE_RIGHT) for agent */
SideT WorldModel::getSide( ) const
{
  return sideSide;
}

/*! This method sets the side of the agent
    \param s (SIDE_LEFT or SIDE_RIGHT) for agent
    \return bool indicating whether update was succesful.*/
bool WorldModel::setSide( SideT s )
{
  sideSide = s;
  m_iMultX = (getSide() == SIDE_LEFT ) ?  1 : -1 ;    // set the draw info
  m_iMultY = (getSide() == SIDE_LEFT ) ? -1 :  1 ;    // from Logger.C
  return true;
}

/*! This method returns the teamname of the agent in this worldmodel
    \return teamname for the agent in this worldmodel */
const char* WorldModel::getTeamName( ) const
{
  return strTeamName ;
}

/*! This method sets the teamname of the agent. The maximum team name is