---

WorldModel.cpp

Go to the documentation of this file.

/*
Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the University of Amsterdam nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include<stdio.h>    // needed for printf
#include<errno.h>    // needed for ETIMEDOUT
#ifdef WIN32
  #include<windows.h>
#else
  #include<strings.h>  // needed for strcpy
  #include<pthread.h>  // needed for pthread_mutex_init
#endif
#include<string.h>   // needed for strcpy
#include<math.h>     // needed for erf
#include<map>        // needed for map
#include"WorldModel.h"

/*****************************************************************************/
/********************** CLASS WORLDMODEL *************************************/
/*****************************************************************************/

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 ) );
  }
}

WorldModel::~WorldModel()
{
#ifdef WIN32
  DeleteCriticalSection( &mutex_newInfo );
#endif
}

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;
}

void WorldModel::setTimeLastCatch( Time time )
{
  timeLastCatch = time;
}

int WorldModel::getTimeSinceLastCatch()
{
  if( timeLastCatch.getTime() == -1 )
    return 1000;
  return timeLastSenseMessage - timeLastCatch;
}

bool WorldModel::setTimeLastRefereeMessage( Time time )
{
  timeLastRefMessage = time;
  return true;
}

Time WorldModel::getTimeLastRefereeMessage( )
{
  return timeLastRefMessage;
}

Time WorldModel::getCurrentTime()
{
  if( getPlayerNumber() == 0 )
    return getTimeLastSeeGlobalMessage();
  else
    return getTimeLastRecvSenseMessage();
}

int WorldModel::getCurrentCycle()
{
  return getCurrentTime().getTime();
}


bool WorldModel::isTimeStopped()
{
  return getCurrentTime().isStopped();
}

bool WorldModel::isLastMessageSee() const
{
  return getTimeLastSeeMessage() == getTimeLastSenseMessage() ;
}

Time WorldModel::getTimeLastSeeGlobalMessage( ) const
{
  return getTimeLastRecvSeeMessage();
}

bool WorldModel::setTimeLastSeeGlobalMessage( Time time )
{
  updateRelativeFromGlobal( );
  return setTimeLastSeeMessage( time ); // set see message
}

Time WorldModel::getTimeLastSeeMessage( ) const
{
  return timeLastSeeMessage;
}

Time WorldModel::getTimeLastRecvSeeMessage( ) const
{
  return timeLastRecvSeeMessage ;
}
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;
}

Time WorldModel::getTimeLastSenseMessage( ) const
{
  return timeLastSenseMessage ;
}

Time WorldModel::getTimeLastRecvSenseMessage( ) const
{
  return timeLastRecvSenseMessage ;
}

  
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;
}

Time WorldModel::getTimeLastHearMessage( ) const
{
  return timeLastHearMessage ;
}

bool WorldModel::setTimeLastHearMessage( Time time )
{
  timeLastHearMessage = time;
  return true;
}

int WorldModel::getPlayerNumber( ) const
{
  return iPlayerNumber;
}

bool WorldModel::setPlayerNumber( int i )
{
  iPlayerNumber = i;
  return true;
}

SideT WorldModel::getSide( ) const
{
  return sideSide;
}

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;
}

const char* WorldModel::getTeamName( ) const
{
  return strTeamName ;
}

bool WorldModel::setTeamName( char * str )
{
  strcpy( strTeamName, str );
  return true;
}

PlayModeT WorldModel::getPlayMode( ) const
{
  return playMode ;
}

bool WorldModel::setPlayMode( PlayModeT pm )
{
  playMode = pm;
  if( ( pm == PM_GOAL_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
      ( pm == PM_GOAL_KICK_RIGHT && getSide() == SIDE_RIGHT )    )
    setTimeLastCatch( getTimeLastSenseMessage() );
  return true;
}

int WorldModel::getGoalDiff( ) const
{
  return iGoalDiff;
}

int WorldModel::addOneToGoalDiff( )
{
  return ++iGoalDiff;
}

int WorldModel::subtractOneFromGoalDiff()
{
  return --iGoalDiff;
}

int WorldModel::getNrOfCommands( CommandT c ) const
{
  return iCommandCounters[ (int) c ];
}

bool WorldModel::setNrOfCommands( CommandT c, int i )
{
  int iIndex = (int) c;

  // if counter is the same as before, no command is performed, otherwise it is
  performedCommands[iIndex] = ( iCommandCounters[iIndex] == i ) ? false : true;
  iCommandCounters [iIndex] = i;
  return true;
}

Time WorldModel::getTimeCheckBall( ) const
{
  return timeCheckBall;
}

bool WorldModel::setTimeCheckBall( Time time )
{
  timeCheckBall = time;
  return true;
}

BallStatusT WorldModel::getCheckBallStatus( ) const
{
  return bsCheckBall;
}

bool WorldModel::setCheckBallStatus( BallStatusT bs )
{
  bsCheckBall = bs;
  return true;
}

bool WorldModel::getRecvThink( )
{
  return m_bRecvThink;
}

char* WorldModel::getCommunicationString( )
{
  return m_strCommunicate;
}


ObjectT WorldModel::getObjectFocus( )
{
  return m_objFocus;
}


bool WorldModel::setObjectFocus( ObjectT obj )
{
  m_objFocus = obj;
  return true;
}


bool WorldModel::setCommunicationString( char *str )
{
  strncpy( m_strCommunicate, str, MAX_SAY_MSG );
  return true;
}


ObjectT WorldModel::iterateObjectStart(int& iIndex,ObjectSetT g,double dConf,
               bool bForward)
{
  iIndex = -1;
  return iterateObjectNext( iIndex, g, dConf, bForward );
}

ObjectT WorldModel::iterateObjectNext(int& iIndex,ObjectSetT g, double dConf,
              bool bForward)
{
  ObjectT o, objGoalie = OBJECT_TEAMMATE_1;
  bool bContinue = true;
  
  if( g == OBJECT_SET_TEAMMATES_NO_GOALIE )
    objGoalie = getOwnGoalieType();
    
  if( iIndex < 0 )
    iIndex = (bForward==false) ? OBJECT_MAX_OBJECTS : -1 ;

  // when dConf is not specified it has the default value of -1.0, in this
  // case set it to the confidence threshold defined in PlayerSetting, but
  // only do this for dynamic objexts, not for flags, lines, etc. since all
  // should be returend.
  if( dConf == -1.0 && (g==OBJECT_SET_OPPONENTS || g==OBJECT_SET_PLAYERS ||
                        g==OBJECT_SET_TEAMMATES) )
    dConf = PS->getPlayerConfThr();

  int i = 0;
  if( bForward == true )
    i = iIndex + 1;
  else
    i = iIndex - 1;
  bContinue = ( bForward == false ) ? ( i >= 0 ) : ( i < OBJECT_MAX_OBJECTS ); 

  while( bContinue )
  {
    o = (ObjectT) i;
    if( SoccerTypes::isInSet( o, g, objGoalie ) )
    {
      if( getConfidence( o ) >= dConf )
      {
        iIndex = i;
        return o;
      }
      else if( dConf == 1.0 && getTimeLastSeeMessage() == getTimeLastSeen( o ))
      {
        iIndex = i; // confidence of 1.0 can only be in same cycle as see
        return o;   // message. Therefore first test should succeed normally;
      }             // in cases where this method is called after see message,
                    // but new sense has already arrived, confidence is lowered
    }               // but we want to return object that was seen in last see
                    // message; this compensates for those cases.
    if( bForward == true )
      i++;
    else
      i--;
    bContinue = ( bForward == false ) ? ( i >= 0 ) :( i < OBJECT_MAX_OBJECTS); 
  }

  return OBJECT_ILLEGAL;
}

void WorldModel::iterateObjectDone( int &iIndex )
{
  iIndex = -1;
}

ObjectT WorldModel::getAgentObjectType( ) const
{
  return agentObject.getType();
}

int WorldModel::getAgentIndex( ) const
{
  return SoccerTypes::getIndex( getAgentObjectType() );
}

bool WorldModel::setAgentObjectType( ObjectT o )
{
  agentObject.setType( o );
  return true;
}

AngDeg WorldModel::getAgentBodyAngleRelToNeck( ) const
{
  return agentObject.getBodyAngleRelToNeck();
}

AngDeg WorldModel::getAgentGlobalNeckAngle(  ) const
{
  return agentObject.getGlobalNeckAngle(  );
}

AngDeg WorldModel::getAgentGlobalBodyAngle(  )
{
  return agentObject.getGlobalBodyAngle(  );
}

Stamina WorldModel::getAgentStamina( ) const
{
  return agentObject.getStamina();
}

TiredNessT WorldModel::getAgentTiredNess( ) const
{
  return getAgentStamina().getTiredNess(
                    SS->getRecoverDecThr(), SS->getStaminaMax() );
}

double WorldModel::getAgentEffort( ) const
{
  return agentObject.getStamina().getEffort();
}

VecPosition WorldModel::getAgentGlobalVelocity( ) const
{
  return agentObject.getGlobalVelocity();
}

double WorldModel::getAgentSpeed( ) const
{
  return agentObject.getSpeed();
}

VecPosition WorldModel::getAgentGlobalPosition( ) const
{
  return agentObject.getGlobalPosition();
}

bool WorldModel::setAgentViewAngle( ViewAngleT va ) 
{
  agentObject.setViewAngle( va );
  return true;
}


ViewAngleT WorldModel::getAgentViewAngle( ) const
{
  return agentObject.getViewAngle();
}

bool WorldModel::setAgentViewQuality( ViewQualityT vq ) 
{
  agentObject.setViewQuality( vq );
  return true;
}
ViewQualityT WorldModel::getAgentViewQuality( ) const
{
  return agentObject.getViewQuality();
}

double WorldModel::getAgentViewFrequency( ViewAngleT va, ViewQualityT vq )
{
  double dViewQualityFactor ;
  double dViewWidthFactor   ;

  if( va == VA_ILLEGAL )
    va = getAgentViewAngle();
  if( vq == VQ_ILLEGAL )
    vq = getAgentViewQuality();

  switch( va )
  {
    case VA_NARROW:  dViewWidthFactor   = 0.5; break;
    case VA_NORMAL:  dViewWidthFactor   = 1.0; break;
    case VA_WIDE:    dViewWidthFactor   = 2.0; break;
    case VA_ILLEGAL:
    default:         dViewWidthFactor   = 0.0; break;
  }

  switch( vq )
  {
    case VQ_LOW:     dViewQualityFactor = 0.5; break;
    case VQ_HIGH:    dViewQualityFactor = 1.0; break;
    case VQ_ILLEGAL:
    default:         dViewQualityFactor = 0.0; break;
  }

  return dViewQualityFactor*dViewWidthFactor;
}

bool WorldModel::getAgentArmMovable( )
{
  return agentObject.getArmMovable();
}

VecPosition WorldModel::getAgentArmPosition( )
{
  return agentObject.getGlobalArmPosition();
}

int WorldModel::getAgentArmExpires( )
{
  return agentObject.getArmExpires();
}

VecPosition  WorldModel::getBallPos()
{
  return getGlobalPosition( OBJECT_BALL );
}

double WorldModel::getBallSpeed()
{
  return Ball.getGlobalVelocity().getMagnitude();
}

AngDeg WorldModel::getBallDirection()
{
  return Ball.getGlobalVelocity().getDirection();
}

Time WorldModel::getTimeGlobalPosition( ObjectT o )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeGlobalPosition();
  return UnknownTime;
}


VecPosition WorldModel::getGlobalPosition( ObjectT o )
{
  Object *object = getObjectPtrFromType( o );
  if( object != NULL )
  {
    if( SoccerTypes::isFlag( o ) || SoccerTypes::isGoal( o ) )
      return SoccerTypes::getGlobalPositionFlag( o, getSide(),
                                                 SS->getGoalWidth() );
    else
      return object->getGlobalPosition();
  }
  return VecPosition( UnknownDoubleValue, UnknownDoubleValue);
}

Time WorldModel::getTimeGlobalVelocity( ObjectT o )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeGlobalVelocity();
  return UnknownTime;
}


VecPosition WorldModel::getGlobalVelocity( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalVelocity(  );
  return VecPosition( UnknownDoubleValue, UnknownDoubleValue );

}

double WorldModel::getRelativeDistance( ObjectT o )
{
  Object *object = getObjectPtrFromType( o );
  if( object != NULL )
    return object->getRelativeDistance();
  return UnknownDoubleValue;
}

VecPosition  WorldModel::getRelativePosition( ObjectT o )
{
  Object *object = getObjectPtrFromType( o );
  if( object != NULL )
    return object->getRelativePosition();
  return VecPosition(UnknownDoubleValue, UnknownDoubleValue);
}

AngDeg WorldModel::getRelativeAngle( ObjectT o, bool bWithBody )
{
  Object *object = getObjectPtrFromType( o );
  double dBody   = 0.0;

  if( object != NULL )
  {
    if( bWithBody == true )
      dBody = getAgentBodyAngleRelToNeck();
    return VecPosition::normalizeAngle( object->getRelativeAngle() - dBody );
  }
  return UnknownDoubleValue;
}

Time WorldModel::getTimeGlobalAngles( ObjectT o )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeGlobalAngles();
  return Time( -1, 0);
}

AngDeg WorldModel::getGlobalBodyAngle( ObjectT o )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalBodyAngle();
  return UnknownAngleValue;
}

AngDeg WorldModel::getGlobalNeckAngle( ObjectT o )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalNeckAngle();
  return UnknownAngleValue;
}

AngDeg WorldModel::getGlobalAngle( ObjectT o )
{
  if( SoccerTypes::isLine( o ) )
    return SoccerTypes::getGlobalAngleLine( o, getSide() );
  return UnknownAngleValue;
}


double WorldModel::getConfidence( ObjectT o)
{
  Object *object = getObjectPtrFromType( o );
  if( object != NULL )
    return object->getConfidence( getCurrentTime() );

  return 0.0;
}

bool WorldModel::isKnownPlayer( ObjectT o )
{
  PlayerObject *object = (PlayerObject *)getObjectPtrFromType( o );
  if( object != NULL  )
    return object->getIsKnownPlayer();
  return false;

}

ObjectT WorldModel::getOppGoalieType()
{
  static ObjectT objGoalieType = OBJECT_ILLEGAL;

  if( objGoalieType != OBJECT_ILLEGAL &&
      isConfidenceGood( objGoalieType ) &&
      isKnownPlayer( objGoalieType ) )
    return objGoalieType;

  ObjectT objOppMaxX = OBJECT_ILLEGAL;
  double  x = -100.0, y = UnknownDoubleValue;

  for( int i = 0; i < MAX_OPPONENTS; i++ )
  {
    if( isConfidenceGood( Opponents[i].getType( ) ) )
    {
      if( Opponents[i].getIsGoalie() == true ) // &&
//           Opponents[i].getGlobalPosition().getX() > PENALTY_X - 2.0 )
      {
        objGoalieType = Opponents[i].getType();
        return Opponents[i].getType();
      }
      if( Opponents[i].getGlobalPosition().getX() > x )
      {
        x          = Opponents[i].getGlobalPosition().getX();
        y          = Opponents[i].getGlobalPosition().getY();
        objOppMaxX = Opponents[i].getType();
      }
    }
  }

  // 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;
}


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;
}


Time WorldModel::getTimeLastSeen( ObjectT o )
{
  Object *object = getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeLastSeen(  );
  return Time( -1, 0);

}

Time WorldModel::getTimeChangeInformation( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeChangeInformation(  );
  return Time( -1, 0);

}

VecPosition WorldModel::getGlobalPositionLastSee( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalPositionLastSee(  );
  return VecPosition( UnknownDoubleValue, UnknownDoubleValue);
}
             
Time WorldModel::getTimeGlobalPositionLastSee( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getTimeGlobalPosDerivedFromSee(  );
  return UnknownTime;
}

VecPosition WorldModel::getGlobalVelocityLastSee( ObjectT o )
{
  DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalVelocityLastSee(  );
  return VecPosition( UnknownDoubleValue, UnknownDoubleValue);
}

AngDeg WorldModel::getGlobalBodyAngleLastSee( ObjectT o )
{
  PlayerObject *object = (PlayerObject*)getObjectPtrFromType( o );
  if( object != NULL )
    return object->getGlobalBodyAngleLastSee(  );
  return UnknownAngleValue;
}

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());
}

AngDeg WorldModel::getGlobalArmDirection( ObjectT o )
{
  PlayerObject *object = (PlayerObject*)getObjectPtrFromType( o );
  if( object == NULL )
    return UnknownAngleValue;

  return object->getGlobalArm();
}

Time  WorldModel::getTimeGlobalArmDirection( ObjectT o )
{
  PlayerObject *object = (PlayerObject*)getObjectPtrFromType( o );
  if( object == NULL )
    return Time(-1,0);

  return object->getTimeGlobalArm();
}

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 );
}

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;
}

double WorldModel::getProbTackleClosestOpp( int iExtraCycles )
{
  ObjectT obj = getClosestInSetTo( OBJECT_SET_OPPONENTS, OBJECT_BALL );
  if( obj == OBJECT_ILLEGAL )
    return -1.0;
  
  return getProbTackleSucceeds( obj, iExtraCycles );
}

bool WorldModel::setIsKnownPlayer( ObjectT o, bool isKnownPlayer )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  return object->setIsKnownPlayer( isKnownPlayer );
}

bool WorldModel::setTimeLastSeen( ObjectT o, Time time )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  return object->setTimeLastSeen( time );
}

bool WorldModel::setHeteroPlayerType( ObjectT  o, int iPlayerType )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
  if( o == getAgentObjectType() )
    updateSSToHeteroPlayerType( iPlayerType );

  return object->setHeteroPlayerType( iPlayerType );
}

PlayerT WorldModel::getPlayerType ( ObjectT o )
{
  if( o == OBJECT_ILLEGAL )
    o = getAgentObjectType();
  return formations->getPlayerType( SoccerTypes::getIndex( o ) );
}

bool WorldModel::isInPlayerSet( ObjectT o, PlayerSetT ps )
{
  return SoccerTypes::isPlayerTypeInSet( getPlayerType( o ), ps );
}


int WorldModel::getHeteroPlayerType( ObjectT obj )
{
  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( obj );

  return object->getHeteroPlayerType( );

}


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() );
}

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() );
}

double  WorldModel::getRelDistanceOpponentGoal()
{
  VecPosition posGoal = getPosOpponentGoal();
  return getAgentGlobalPosition().getDistanceTo( posGoal );

}

double  WorldModel::getRelAngleOpponentGoal()
{
  VecPosition posGoal;
  if( sideSide == SIDE_LEFT )
    posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_R, sideSide );
  else
    posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_L, sideSide );

  return ( posGoal - getAgentGlobalPosition()).getDirection() ;

}

HeteroPlayerSettings WorldModel::getInfoHeteroPlayer( int iIndex )
{
  return pt[iIndex];
}

HeteroPlayerSettings WorldModel::getHeteroInfoPlayer( ObjectT obj )
{
//  if( obj == getAgentObjectType() || SoccerTypes::isOpponent( obj ) )
//    return getInfoHeteroPlayer( agentObject.getHeteroPlayerType() );    
  if( ! SoccerTypes::isKnownPlayer( obj ) )
    obj = getAgentObjectType();

  PlayerObject *object = (PlayerObject*) getObjectPtrFromType( obj );
  int          iType   = object->getHeteroPlayerType();
  // default iType in Object = 0
  return getInfoHeteroPlayer( iType );
}


bool WorldModel::setSubstitutedOpp( ObjectT obj )
{
  m_setSubstitutedOpp.insert( obj );
  return true;
}

ObjectT WorldModel::getSubstitutedOpp( )
{
  if( m_setSubstitutedOpp.empty() == true )
    return OBJECT_ILLEGAL;
  
  ObjectT obj = *m_setSubstitutedOpp.begin();
  m_setSubstitutedOpp.erase( obj );
  return obj;
}


double WorldModel::getDashPowerRate( ObjectT obj )
{
  return getHeteroInfoPlayer( obj ).dDashPowerRate ;
}

double WorldModel::getPlayerSpeedMax( ObjectT obj )
{
  return getHeteroInfoPlayer( obj ).dPlayerSpeedMax ;
}

double WorldModel::getPlayerDecay( ObjectT obj )
{
  return getHeteroInfoPlayer( obj ).dPlayerDecay ;
}

double WorldModel::getMaximalKickDist( ObjectT obj )
{
  return getHeteroInfoPlayer( obj ).dMaximalKickDist ;
}

double WorldModel::getStaminaIncMax( ObjectT obj )
{
  return getHeteroInfoPlayer( obj ).dStaminaIncMax ;
}

double WorldModel::getPlayerSize( ObjectT obj )
{
  return getHeteroInfoPlayer( obj ).dPlayerSize ;
}

double WorldModel::getInertiaMoment( ObjectT obj )
{
  return getHeteroInfoPlayer( obj ).dInertiaMoment;
}

double WorldModel::getEffortMax( ObjectT obj )
{
  return getHeteroInfoPlayer( obj ).dEffortMax;
}

double WorldModel::getEffectiveMaxSpeed( ObjectT obj, bool bWithNoise )
{
  double dSpeed = 0.0;
  HeteroPlayerSettings pt = getHeteroInfoPlayer( obj );

  for( int j = 0; j < 15 ; j ++ )
  {
    dSpeed *= pt.dPlayerDecay;
    dSpeed += SS->getMaxPower()*pt.dEffortMax*pt.dDashPowerRate;
    if( dSpeed > pt.dPlayerSpeedMax )
      dSpeed = pt.dPlayerSpeedMax;
    if( bWithNoise )
      dSpeed += sqrt(2*(pow(dSpeed * SS->getPlayerRand(), 2)));
  }
  return dSpeed;
}

bool WorldModel::isQueuedActionPerformed()
{
  // for all possible commands check if it is sent in previous cycle,
  // but not performed
  for( int i = 0 ; i < CMD_MAX_COMMANDS ; i++ )
    if( queuedCommands[i].time   == getTimeLastSenseMessage() - 1 &&
        performedCommands[i]     == false )
      return false;

  return true;
}

bool WorldModel::isFreeKickUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  bool bLeftKick = (pm==PM_FREE_KICK_LEFT  || pm==PM_INDIRECT_FREE_KICK_LEFT );
  bool bRightKick= (pm==PM_FREE_KICK_RIGHT || pm==PM_INDIRECT_FREE_KICK_RIGHT);

  return ( bLeftKick   && getSide() == SIDE_LEFT  ) ||
         ( bRightKick  && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isFreeKickThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  bool bLeftKick = (pm==PM_FREE_KICK_LEFT  || pm==PM_INDIRECT_FREE_KICK_LEFT );
  bool bRightKick= (pm==PM_FREE_KICK_RIGHT || pm==PM_INDIRECT_FREE_KICK_RIGHT);

  return ( bRightKick  && getSide() == SIDE_LEFT  ) ||
         ( bLeftKick   && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isBeforeKickOff( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return pm == PM_BEFORE_KICK_OFF  || pm == PM_GOAL_LEFT  ||
         pm == PM_GOAL_RIGHT || isKickOffUs( pm ) || isKickOffThem( pm );
}

bool WorldModel::isDeadBallUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return isKickInUs  ( pm ) || isFreeKickUs  ( pm ) || isCornerKickUs     ( pm)
      || isKickOffUs ( pm ) || isOffsideThem ( pm ) || isFreeKickFaultThem( pm)
      || isGoalKickUs( pm ) || isBackPassThem( pm ) ;
}


bool WorldModel::isDeadBallThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return isFreeKickThem( pm ) || isKickInThem  ( pm ) || isCornerKickThem ( pm)
     ||  isKickOffThem ( pm ) || isGoalKickThem( pm ) || isFreeKickFaultUs( pm)
     || isOffsideUs    ( pm ) || isBackPassUs  ( pm ) ;
}

bool WorldModel::setChangeViewCommand( SoccerCommand soc )
{
  m_changeViewCommand = soc;
  return true;
}

SoccerCommand WorldModel::getChangeViewCommand( )
{
  return m_changeViewCommand;
}

SideT WorldModel::getSidePenalty( )
{
  return m_sidePenalty;
}

bool WorldModel::setSidePenalty( SideT side )
{
  m_sidePenalty = side;
  return true;
}


bool WorldModel::isCornerKickUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_CORNER_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_CORNER_KICK_RIGHT && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isCornerKickThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_CORNER_KICK_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_CORNER_KICK_LEFT   && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isOffsideUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_OFFSIDE_RIGHT  && getSide() == SIDE_RIGHT ) ||
         ( pm == PM_OFFSIDE_LEFT   && getSide() == SIDE_LEFT );
}

bool WorldModel::isOffsideThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_OFFSIDE_LEFT  && getSide() == SIDE_RIGHT ) ||
         ( pm == PM_OFFSIDE_RIGHT && getSide() == SIDE_LEFT );
}

bool WorldModel::isKickInUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_KICK_IN_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_KICK_IN_RIGHT && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isKickInThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_KICK_IN_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_KICK_IN_LEFT   && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isFreeKickFaultUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_FREE_KICK_FAULT_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_FREE_KICK_FAULT_RIGHT && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isFreeKickFaultThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_FREE_KICK_FAULT_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_FREE_KICK_FAULT_LEFT   && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isKickOffUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_KICK_OFF_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_KICK_OFF_RIGHT && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isKickOffThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_KICK_OFF_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_KICK_OFF_LEFT   && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isBackPassUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_BACK_PASS_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_BACK_PASS_RIGHT && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isBackPassThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_BACK_PASS_RIGHT && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_BACK_PASS_LEFT  && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isGoalKickUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_GOAL_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_GOAL_KICK_RIGHT && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isGoalKickThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( pm == PM_GOAL_KICK_RIGHT  && getSide() == SIDE_LEFT  ) ||
         ( pm == PM_GOAL_KICK_LEFT   && getSide() == SIDE_RIGHT ) ;
}

bool WorldModel::isPenaltyUs( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( (
             ( pm == PM_PENALTY_SETUP_LEFT ||
               pm == PM_PENALTY_READY_LEFT ||
               pm == PM_PENALTY_TAKEN_LEFT ) && getSide() == SIDE_LEFT  )   ||
           (
             ( pm == PM_PENALTY_SETUP_RIGHT ||
               pm == PM_PENALTY_READY_RIGHT ||
               pm == PM_PENALTY_TAKEN_RIGHT ) && getSide() == SIDE_RIGHT  ) );
}


bool WorldModel::isPenaltyThem( PlayModeT pm )
{
  if( pm == PM_ILLEGAL )
    pm = getPlayMode();

  return ( (
             ( pm == PM_PENALTY_SETUP_LEFT ||
               pm == PM_PENALTY_READY_LEFT ||
               pm == PM_PENALTY_TAKEN_LEFT ) && getSide() == SIDE_RIGHT  )   ||
           (
             ( pm == PM_PENALTY_SETUP_RIGHT ||
               pm == PM_PENALTY_READY_RIGHT ||
               pm == PM_PENALTY_TAKEN_RIGHT ) && getSide() == SIDE_LEFT  ) );
}

bool WorldModel::isFullStateOn( SideT s )
{
  if( s == SIDE_ILLEGAL )
    s = getSide();
  
  if( s == SIDE_LEFT )
    return SS->getFullStateLeft();
  else if( s == SIDE_RIGHT )
    return SS->getFullStateRight();
  else
    return false;
}

void WorldModel::show( ostream & os )
{
  int i;
  os << "Worldmodel (" << getCurrentTime() << ")\n" <<
        "========================\n";
  os << "Teamname: " << getTeamName() << "\n";
  if( Ball.getTimeLastSeen( ).getTime() != -1 )
    Ball.show();
  os << "Teammates: " << "\n";
  for( i = 0; i < MAX_TEAMMATES ; i++ )
    if( isConfidenceGood( Teammates[i].getType() ) )
      Teammates[i].show( getTeamName() );
  os << "Opponents: " << "\n";
  for( i = 0; i < MAX_OPPONENTS ; i++ )
    if( isConfidenceGood( Opponents[i].getType() ) )
      Opponents[i].show( DEFAULT_OPPONENT_NAME );
  os << "Agent: " << "\n";
  agentObject.show( getTeamName() );

  os << "General Info: " << "\n" <<
          "side: "      << SoccerTypes::getSideStr( getSide() )    << "\n" <<
          "kicks: "     << getNrOfCommands( CMD_KICK )             << "\n" <<
          "turns: "     << getNrOfCommands( CMD_TURN )             << "\n" <<
          "dashes: "    << getNrOfCommands( CMD_DASH )             << "\n" <<
          "turnnecks: " << getNrOfCommands( CMD_TURNNECK )         << "\n" <<
          "says: "      << getNrOfCommands( CMD_SAY )              << "\n" <<
          "playmode: "  << SoccerTypes::getPlayModeStr( playMode ) << "\n" <<
          "===================================="                   << "\n";

}

void WorldModel::show( ObjectSetT set, ostream & os )
{
  int iIndex;
  for( ObjectT o = iterateObjectStart( iIndex, set );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext( iIndex, set ) )
    show( o, os );
  os << "\n";
}

void WorldModel::showQueuedCommands( ostream & os )
{
  os << "Commands in queue:\n" ;
  for( int i = 0; i < CMD_MAX_COMMANDS; i++ )
    if( queuedCommands[i].commandType != CMD_ILLEGAL )
      queuedCommands[i].show( os );
}

void WorldModel::show( ObjectT o, ostream &os )
{
  Object *object = getObjectPtrFromType( o );
  if( object != NULL )
  {
    if( SoccerTypes::isPlayer( o ) )
    {
      PlayerObject *pobj = (PlayerObject*) object ;
      if( SoccerTypes::isTeammate( o ) )
        pobj->show( getTeamName(), os );
      else
        pobj->show( "Opponent", os );
    }
    else
      object->show( os );
  }
}

bool WorldModel::waitForNewInformation( )
{
  bool bReturn = true;
  if( bNewInfo == false ) // there hasn't arrived any information yet
  {
#ifdef WIN32
    DWORD waittime = PS->getServerTimeOut() * 1000;
    EnterCriticalSection( &mutex_newInfo );
    int ret;
    Log.logWithTime( 2, "go into conditional wait" );
    while( (ret = WaitForSingleObject( event_newInfo,
                                       waittime) ) == WAIT_ABANDONED )
      printf("(WorldModel::waitForNewInformation) failure in loop!!\n");
    Log.logWithTime( 2, "go out of conditional wait" );
    if( ret == WAIT_TIMEOUT ) // if no info was received but timer timed out
      bReturn = false;
    ResetEvent( event_newInfo );
    LeaveCriticalSection( &mutex_newInfo );
#else
    struct timeval now;
    struct timespec timeout;
    gettimeofday(&now, NULL);
    timeout.tv_sec = now.tv_sec + PS->getServerTimeOut();
    timeout.tv_nsec = now.tv_usec*1000;

    // lock mutex and wait till it is unlocked by Sense thread
    // this happens in setTimeLastSeeMessage, setTimeLastSenseMessage
    // or setTimeLastSeeGlobalMessage
    pthread_mutex_lock( &mutex_newInfo );
    int ret;
    Log.logWithTime( 2, "go into conditional wait" );
    while( (ret = pthread_cond_timedwait( &cond_newInfo,
                    &mutex_newInfo, &timeout) ) == EINTR )
     printf("(WorldModel::waitForNewInformation) failure in loop!!\n");
    Log.logWithTime( 2, "go out of conditional wait" );
    if( ret == ETIMEDOUT ) // if no info was received but timer timed out
      bReturn = false;
    pthread_mutex_unlock( &mutex_newInfo );
#endif
  }
  else
    Log.logWithTime( 2, "already new info waiting" );
    
  // update the time of the see and the sense messages not yet with the time
  // from the last received messages. This is done to circumvent that a time
  // is updated while the main thread is still determining a new action and
  // the last see message is not yet updated in the world model. This is
  // therefore then in updateAll;
  
  // reset the indication of new visual information
  bNewInfo = false;

  return bReturn;
}

void WorldModel::logObjectInformation( int iLogLevel, ObjectT obj )
{

  static char   str[2048];
  double dConf     = PS->getPlayerConfThr();
  sprintf( str, "(%4d,%3d) ", getCurrentTime().getTime(),
                              getCurrentTime().getTimeStopped() );
  if( obj != OBJECT_ILLEGAL )
    sprintf( str, "%12.6f %12.6f %12.6f %12.6f %12.6f %12.6f",
                                  getGlobalPosition(obj).getX(),
                                  getGlobalPosition(obj).getY(),
                                  getGlobalVelocity(obj).getX(),
                                  getGlobalVelocity(obj).getY(),
                                  getGlobalBodyAngle(obj),
                                  getGlobalNeckAngle(obj) );

  if( getConfidence      ( OBJECT_BALL ) > dConf &&
      getRelativeDistance( OBJECT_BALL ) < 20.0 )
    sprintf( &str[strlen(str)], " %12.6f %12.6f",
                                  getGlobalPosition(OBJECT_BALL).getX(),
                                  getGlobalPosition(OBJECT_BALL).getY() );
  else
    sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

  if( getTimeGlobalVelocity( OBJECT_BALL ) > getTimeFromConfidence( dConf ) &&
      getRelativeDistance  ( OBJECT_BALL ) < 20.0 )
    sprintf( &str[strlen(str)], " %12.6f %12.6f",
                                  getGlobalVelocity(OBJECT_BALL).getX(),
                                  getGlobalVelocity(OBJECT_BALL).getY() );
  else
    sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
  sprintf( &str[strlen(str)], " %12.6f", getConfidence(OBJECT_BALL) );

  int    iIndex=-1;
  int    iIndexPlayer;
  for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, 0.0 );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext( iIndex, OBJECT_SET_PLAYERS, 0.0 ) )
  {
    bool bPrint = false;

    iIndexPlayer = (SoccerTypes::isTeammate(o))
                     ? SoccerTypes::getIndex(o) + 1
                     : SoccerTypes::getIndex(o) + 12;
    sprintf( &str[strlen(str)], " %d", iIndexPlayer );
    if( getConfidence      ( o ) > dConf && isKnownPlayer(o) &&
        getRelativeDistance( o ) < 20.0 )
    {
      sprintf( &str[strlen(str)], " %12.6f %12.6f",
                                    getGlobalPosition(o).getX(),
                                    getGlobalPosition(o).getY() );
      bPrint = true;
    }
    else
       sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

    if( getTimeGlobalVelocity( o ) > getTimeFromConfidence( dConf ) &&
        getRelativeDistance  ( o ) < 20.0 && isKnownPlayer(o)  )
    {
      sprintf( &str[strlen(str)], " %12.6f %12.6f",
                                    getGlobalVelocity(o).getX(),
                                    getGlobalVelocity(o).getY() );
      bPrint = true;
    }
    else
      sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );

    if( bPrint )
      sprintf( &str[strlen(str)], " %12.6f", getConfidence(o) );
    else
      sprintf( &str[strlen(str)], " %12.6f", -10.0 );
  }
  if( getCurrentCycle() != 3000  )
      Log.log( iLogLevel, str );
}

void WorldModel::logDrawInfo( int iLogLevel  )
{
  if( getCurrentCycle() % ( SS->getHalfTime() * SS->getSimulatorStep()) == 0 )
    return ;

  bool   bReturn1 = false, bReturn2 = false;
  double dConfThr = PS->getPlayerConfThr();
  int    iIndex;
  char   strMsg1[MAX_MSG], strMsg2[MAX_MSG];
  char   strColorLeft[]  = "ffff00" ;
  char   strColorRight[] = "0000ff" ;
  if( getSide() != SIDE_LEFT )
  {
    sprintf( strColorLeft, strColorRight );
    sprintf( strColorRight, "ffff00" );
  }


  sprintf( strMsg1, "_2D_ CIRCLE col=%s ", strColorLeft ) ;
  // first each players logs the positions of all the teammates
  for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_TEAMMATES, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, OBJECT_SET_TEAMMATES, dConfThr ) )
  {
     bReturn1 = true;

     // add all the global positions to the string
     sprintf( &strMsg1[strlen(strMsg1)], "(%1.2f,%1.2f,1.3) ",
      m_iMultX*getGlobalPosition( o ).getX(),
      m_iMultY*getGlobalPosition( o ).getY() );
  }
  iterateObjectDone( iIndex );

  if( bReturn1 == false )
    sprintf( strMsg1, "_2D_ " );
  else
    strcat( strMsg1, "; " );

  sprintf( strMsg2, "CIRCLE col=%s ", strColorRight ) ;
  for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS, dConfThr);
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS, dConfThr ) )
  {
     bReturn2 = true;

     // add all the global positions to the string
     sprintf( &strMsg2[strlen(strMsg2)], "(%1.2f,%1.2f,1.3) ",
      m_iMultX*getGlobalPosition( o ).getX(),
      m_iMultY*getGlobalPosition( o ).getY() );
  }
  iterateObjectDone( iIndex );
  strcat( strMsg2, ";" );

  if( bReturn2 == true )
    strcat( strMsg1, strMsg2 );
  if( bReturn1 == true || bReturn2 == true)
    LogDraw.log( iLogLevel, strMsg1 );

  return ;
}

void WorldModel::logCoordInfo( int iLogLevel  )
{
  if( getCurrentCycle() % ( SS->getHalfTime() * SS->getSimulatorStep()) == 0 )
    return ;

  bool   bReturn = false;
  int    iIndex;
  double dConfThr = PS->getPlayerConfThr();
  char   strMsg[MAX_MSG];

  map<double, ObjectT > mapDistToBall;
  VecPosition posBall = getGlobalPosition( OBJECT_BALL ), posFirst;
  int i = 0;

  for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_TEAMMATES, dConfThr);
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, OBJECT_SET_TEAMMATES, dConfThr ) )
  {
     // add all the global positions to the string
     mapDistToBall.insert(
       make_pair( getGlobalPosition( o ).getDistanceTo( posBall ), o ) );
  }
  iterateObjectDone( iIndex );

  sprintf( strMsg, "all_2D_ ");
  for(  map<double, ObjectT >::iterator iter = mapDistToBall.begin();
        iter != mapDistToBall.end()  ;
        iter ++ )
  {
    if( i == 0 )
      posFirst = getGlobalPosition( iter->second );
    else if( iter->second == getAgentObjectType() && i < 4 &&
             iter->first < 20.0 )
    {
      bReturn = true;
      sprintf( &strMsg[strlen(strMsg)],
                       "LINE col=%s (%1.2f,%1.2f,%1.2f,%1.2f);",
        m_colorPlayers[ SoccerTypes::getIndex( getAgentObjectType() ) ],
        m_iMultX*posFirst.getX(),
        m_iMultY*posFirst.getY(),
        m_iMultX*getGlobalPosition( iter->second ).getX(),
        m_iMultY*getGlobalPosition( iter->second ).getY() );
    }

    i++;
  }

  if( bReturn == true )
    LogDraw.log( iLogLevel, strMsg );
}


/* This method logs a circle to the specified log level.
   \param iLogLevel log level to which the circle should be printed
   \param pos center position of the circle
   \param dRadius radius of the circle
   \param bAll indication whether it should be printed to specific drawlevel
          or not (default false). */
bool WorldModel::logCircle( int iLogLevel, VecPosition pos, double  dRadius,
                            bool  bAll                                       )
{
  if( getCurrentCycle() % ( SS->getHalfTime() * SS->getSimulatorStep()) == 0 )
    return false;

  return LogDraw.log( iLogLevel, "%s_2D_ CIRCLE col=%s (%1.2f,%1.2f,%f) ;",
           ( bAll == true ) ? "all" : "" ,
           m_colorPlayers[ getAgentIndex() ],
           m_iMultX*pos.getX(), m_iMultY*pos.getY(), dRadius );
}

/* This method logs a line to the specified log level.
   \param iLogLevel log level to which the circle should be printed
   \param pos1 first point of line
   \param pos2 second point of line
   \param bAll indication whether it should be printed to specific drawlevel
          or not (default false). */
bool WorldModel::logLine( int  iLogLevel, VecPosition pos1, VecPosition pos2,
                          bool bAll              )
{
  if( getCurrentCycle() % ( SS->getHalfTime() * SS->getSimulatorStep()) == 0 )
    return false;

  return LogDraw.log( iLogLevel, 
                      "%s_2D_ LINE col=%s (%1.2f,%1.2f,%1.2f,%1.2f) ;",
                      ( bAll == true ) ? "all" : "" ,
                      m_colorPlayers[ getAgentIndex() ],
                      m_iMultX*pos1.getX(), m_iMultY*pos1.getY(),
                      m_iMultX*pos2.getX(), m_iMultY*pos2.getY() );

}

/* This method logs the ball position and its velocity.

   \param iLogLevel log level to which the ball information should be
   printed.*/
bool WorldModel::logDrawBallInfo( int  iLogLevel )
{
  int  iPoints = 20;
  char strMsg[MAX_MSG];
  char *strColor = "ff0000";

  VecPosition vel     = getGlobalVelocity( OBJECT_BALL );
  VecPosition pos     = getBallPos(), pos1, pos2;
  AngDeg      angOrth = VecPosition::normalizeAngle( vel.getDirection( )+90 );
  VecPosition posBall = getBallPos();
  sprintf( strMsg, "_2D_ CIRCLE col=%s (%1.2f,%1.2f,0.3) ; ",
     strColor, m_iMultX*posBall.getX(), m_iMultY*posBall.getY() );
     
  for( int i = 0; i < iPoints; i ++ )
  {
    // create all the orthogonal lines
    pos += vel;
    pos1 = pos + VecPosition( -0.5*vel.getMagnitude(), angOrth, POLAR );
    pos2 = pos + VecPosition(  0.5*vel.getMagnitude(), angOrth, POLAR );
    sprintf( &strMsg[strlen(strMsg)], "LINE col=%s (%1.2f,%1.2f,%1.2f,%1.2f);",
            strColor, m_iMultX*pos1.getX(), m_iMultY*pos1.getY(),
            m_iMultX*pos2.getX(), m_iMultY*pos2.getY() );
    vel *= SS->getBallDecay();
  }
  
  // finally create the line through all orthogonal lines
  sprintf( &strMsg[strlen(strMsg)], "LINE col=%s (%1.2f,%1.2f,%1.2f,%1.2f) ;",
          strColor, m_iMultX*getBallPos().getX(), m_iMultY*getBallPos().getY(),
          m_iMultX*pos.getX(), m_iMultY*pos.getY() );

  return LogDraw.log( iLogLevel, strMsg );
}

bool WorldModel::isFeatureRelevant( FeatureT type )
{
  int  iIndex  = (int)type;
  bool bReturn = true;
  static Time  timeKickedUsed = -1;

#if 0
  Log.log( 460, "check feature (%d,%d,%d) relevance now (%d,%d,%d)",
         m_features[iIndex].getTimeSee().getTime(), 
         m_features[iIndex].getTimeSense().getTime(),
         m_features[iIndex].getTimeHear().getTime(),
         getTimeLastSeeMessage().getTime(), 
         getTimeLastSenseMessage().getTime(),
         getTimeLastHearMessage().getTime() );
#endif
  // feature is relevant when see and hear time is larger or equal than the  
  // current time
  bReturn  = m_features[iIndex].getTimeSee()  >= getTimeLastSeeMessage() &&
             m_features[iIndex].getTimeHear() >= getTimeLastHearMessage();
             
  // if ball kicked, also recheck 
  if( timeKickedUsed != getTimeLastSenseMessage() )
  {
    bReturn &= ( m_bPerformedKick == false );
    timeKickedUsed = getTimeLastSenseMessage();
  }

  // in case of interception, also recheck after sense message
  if( type ==  FEATURE_INTERCEPTION_POINT_BALL ||
      type ==  FEATURE_INTERCEPT_CLOSE        )
    bReturn &= m_features[iIndex].getTimeSense() >= getTimeLastSenseMessage();
  return bReturn;
}

Feature WorldModel::getFeature( FeatureT type )
{
  return m_features[(int)type];
}

bool WorldModel::setFeature( FeatureT type, Feature feature )
{
  m_features[(int)type] = feature;
  return true;
}

void WorldModel::drawCoordinationGraph( )
{
  bool partOfGraph = false; // is the player part of the coordination graph?
  static bool bFirst = true;
  static ofstream ofile;  
  char strTmp[128];
  if( bFirst == true )
  {
    sprintf( strTmp, "logs/coord%d.txt", getPlayerNumber() );
    ofile.open( strTmp );
    bFirst = false;
  }
  
  ObjectT objAgent = getAgentObjectType();
  ObjectT objClosestToBall = getFastestInSetTo( OBJECT_SET_TEAMMATES, 
                                                OBJECT_BALL );
  ObjectT objTeam = OBJECT_ILLEGAL;

  VecPosition vpAgent = getAgentGlobalPosition();
  VecPosition vpTeam = getGlobalPosition( objTeam ); // -1000.0, 1000.0

#if 0
  if ( objClosestToBall == objAgent ) // Agent is closest to ball
    // find the closest player that has a x-coordinate greater than the agent's
   {
     partOfGraph = true;
     
     int iIndex;
     ObjectSetT set = OBJECT_SET_TEAMMATES;
     for( ObjectT o = iterateObjectStart( iIndex, set );
          o != OBJECT_ILLEGAL;
          o = iterateObjectNext ( iIndex, set ) )
     {
       VecPosition vpNew = getGlobalPosition( o );     
       Log.log( 701, "i am fastest, check %d, last_x %f, his_x %f  \
                           last_rel %f, his_rel  %f",
                o, vpTeam.getX(), vpNew.getX(), 
                getRelativeDistance( objTeam ),
                getRelativeDistance( o ) );
       if( objTeam == OBJECT_ILLEGAL)
       {
         if( vpNew.getX() > vpAgent.getX() - 5)
         {
           objTeam = o;            
           vpTeam = vpNew;
         }
       }
       else if ( vpNew.getX() > vpTeam.getX() && o != objAgent &&
                 vpNew.getX() > vpAgent.getX() - 5 )
       {
         if ( getRelativeDistance( o ) < getRelativeDistance( objTeam ) )
         {
           objTeam = o;
           vpTeam = vpNew;
         }
       }
     }
     iterateObjectDone( iIndex );
   }                            
  else // Agent is not closest to ball
  {
  }
#else // Jelle
  objAgent = getAgentObjectType();
  
  objClosestToBall = getFastestInSetTo( OBJECT_SET_TEAMMATES, OBJECT_BALL );
  VecPosition posFastest   = getGlobalPosition( objClosestToBall );
  objTeam = OBJECT_ILLEGAL;

  vpAgent = getAgentGlobalPosition();
  vpTeam  = getGlobalPosition( objTeam ); // -1000.0, 1000.0

  // serach for closest teammate with x higher than 5 of guy with ball
  int iIndex;
  ObjectSetT set = OBJECT_SET_TEAMMATES;
  for( ObjectT o = iterateObjectStart( iIndex, set );
        o != OBJECT_ILLEGAL;
        o = iterateObjectNext ( iIndex, set ) )
  {
    VecPosition vpNew = getGlobalPosition( o );     
    Log.log( 701, "fastest %d, check %d, last_x %f, his_x %f dist_old %f dist_new %f last_rel %f, his_rel  %f",
             objClosestToBall, o, vpTeam.getX(), vpNew.getX(), 
             posFastest.getDistanceTo( vpTeam ), 
             posFastest.getDistanceTo( vpNew ), 
             getRelativeDistance( objTeam ),
             getRelativeDistance( o ) );
    if( objTeam == OBJECT_ILLEGAL)
    {
      if( vpNew.getX() > posFastest.getX() - 5 && o != objClosestToBall )
        {
          objTeam = o;            
          vpTeam = vpNew;
        }
    }
    else if ( /*vpNew.getX() > vpTeam.getX() && */o != objClosestToBall &&
              vpNew.getX() > posFastest.getX() - 5)
    {
      if( posFastest.getDistanceTo( vpNew ) < 
          posFastest.getDistanceTo( vpTeam ) )
      {
        objTeam = o;
        vpTeam = vpNew;
      }
    }
  }  
  iterateObjectDone( iIndex );                               
  Log.log( 701, "team %d fastest %d me %d", objTeam, objClosestToBall, objAgent  );
  if( objAgent == objTeam || objAgent == objClosestToBall )
    partOfGraph = true;
#endif   
  if( partOfGraph == true && getCurrentTime().isStopped() == false )
  {
    Log.log( 701, 
             "create line with me (%d) (%f,%f) and closest team %d (%f,%f)", 
             SoccerTypes::getIndex(getAgentObjectType()) +1, 
             posFastest.getX(),posFastest.getY(),     
             SoccerTypes::getIndex(objTeam) +1,
             vpTeam.getX(),vpTeam.getY()          );
    logLine( 701, posFastest, vpTeam, true );  
    ofile << getCurrentCycle() << ": " << 
      SoccerTypes::getObjectStr( strTmp, objTeam, getTeamName() ) << " "
          << SoccerTypes::getIndex( objTeam ) << 
      endl;
  }
}

---