Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

WorldModel.C

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2000-2002, Jelle Kok, University of Amsterdam
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without 
00006 modification, are permitted provided that the following conditions are met:
00007 
00008 1. Redistributions of source code must retain the above copyright notice, this 
00009 list of conditions and the following disclaimer. 
00010 
00011 2. Redistributions in binary form must reproduce the above copyright notice, 
00012 this list of conditions and the following disclaimer in the documentation 
00013 and/or other materials provided with the distribution. 
00014 
00015 3. Neither the name of the University of Amsterdam nor the names of its 
00016 contributors may be used to endorse or promote products derived from this 
00017 software without specific prior written permission. 
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00020 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00021 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
00022 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
00023 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
00024 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
00025 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
00026 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
00027 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
00028 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 */
00047 #include<stdio.h>    // needed for printf
00048 #include<strings.h>  // needed for strcpy
00049 #include<errno.h>    // needed for ETIMEDOUT
00050 #include<pthread.h>  // needed for pthread_mutex_init
00051 #include<string.h>   // needed for strcpy
00052 #include<math.h>     // needed for erf
00053 #include"WorldModel.h"
00054 
00055 /*****************************************************************************/
00056 /********************** CLASS WORLDMODEL *************************************/
00057 /*****************************************************************************/
00058 
00064 WorldModel::WorldModel( ServerSettings *ss, PlayerSettings *ps, Formations *fs):
00065     agentObject(  )
00066 {
00067   SS                    = ss;
00068   PS                    = ps;
00069   formations            = fs;
00070   bNewInfo              = false;
00071 
00072   setSide                ( SIDE_ILLEGAL ); // is set by init message
00073   strTeamName[0]        = '\0';
00074   setPlayMode            ( PM_BEFORE_KICK_OFF );
00075   iGoalDiff             = 0;
00076 
00077   int i;
00078   for( i = 0; i < MAX_TEAMMATES ; i ++ )
00079     Teammates[i].setType( SoccerTypes::getTeammateObjectFromIndex( i ) );
00080   for( i = 0; i < MAX_OPPONENTS ; i ++ )
00081     Opponents[i].setType( SoccerTypes::getOpponentObjectFromIndex( i ) );
00082   for( i = 0; i < MAX_OPPONENTS + MAX_TEAMMATES ; i ++ )
00083     UnknownPlayers[i].setType( OBJECT_ILLEGAL );
00084   for( i = 0; i < MAX_FLAGS; i ++ )
00085     Flags[i].setType( OBJECT_ILLEGAL );
00086   for( i = 0; i < MAX_LINES; i ++ )
00087     Lines[i].setType( OBJECT_ILLEGAL );
00088 
00089   iNrUnknownPlayers     = 0;
00090 
00091   Ball.setType             ( OBJECT_BALL    );
00092   agentObject.setType      ( OBJECT_ILLEGAL );
00093   agentObject.setStamina   ( Stamina(SS->getStaminaMax(),1.0,1.0) );
00094 
00095   for( i = 0 ; i < CMD_MAX_COMMANDS ; i ++ )
00096   {
00097     queuedCommands[i].commandType = (CommandT)i;
00098     performedCommands[i]          = false;
00099     iCommandCounters[i]           = 0;
00100   }
00101 
00102   iNrHoles              = 0;
00103   iNrOpponentsSeen      = 0;
00104   iNrTeammatesSeen      = 0;
00105   bsCheckBall           = BS_ILLEGAL;
00106 
00107   // initialize the mutex for bNewInfo
00108   pthread_mutex_init( &mutex_newInfo, NULL );
00109   pthread_cond_init ( &cond_newInfo, NULL );
00110   timeLastSenseMessage = Time( 0, 1 );
00111 }
00112 
00117 Object* WorldModel::getObjectPtrFromType( ObjectT o )
00118 {
00119   Object *object = NULL;
00120   if( o == OBJECT_ILLEGAL )
00121     return NULL;
00122 
00123   if( SoccerTypes::isKnownPlayer( o ) )
00124   {
00125     if( o == agentObject.getType() )
00126       object = &agentObject;
00127     else if( SoccerTypes::isTeammate( o ) )
00128       object = &Teammates[SoccerTypes::getIndex(o)];
00129     else
00130       object = &Opponents[SoccerTypes::getIndex(o)];
00131   }
00132   else if( SoccerTypes::isFlag( o ) )
00133     object = &Flags[SoccerTypes::getIndex(o)];
00134   else if( SoccerTypes::isLine( o ) )
00135     object = &Lines[SoccerTypes::getIndex(o)];
00136   else if( SoccerTypes::isBall( o ) )
00137     object = &Ball;
00138   else if( o == OBJECT_OPPONENT_GOALIE )
00139     return getObjectPtrFromType( getOppGoalieType() );
00140   else if( o == OBJECT_TEAMMATE_GOALIE )
00141     return getObjectPtrFromType( getOwnGoalieType() );
00142  return object;
00143 }
00144 
00150 void WorldModel::setTimeLastCatch( Time time )
00151 {
00152   timeLastCatch = time;
00153 }
00154 
00157 int WorldModel::getTimeSinceLastCatch()
00158 {
00159   if( timeLastCatch.getTime() == -1 )
00160     return 1000;
00161   return timeLastSenseMessage - timeLastCatch;
00162 }
00163 
00167 bool WorldModel::setTimeLastRefereeMessage( Time time )
00168 {
00169   timeLastRefMessage = time;
00170   return true;
00171 }
00172 
00175 Time WorldModel::getTimeLastRefereeMessage( )
00176 {
00177   return timeLastRefMessage;
00178 }
00179 
00184 Time WorldModel::getCurrentTime()
00185 {
00186   if( getPlayerNumber() == 0 )
00187     return getTimeLastSeeGlobalMessage();
00188   else
00189     return getTimeLastSenseMessage();
00190 }
00191 
00196 int WorldModel::getCurrentCycle()
00197 {
00198   if( getPlayerNumber() == 0 )
00199     return getTimeLastSeeGlobalMessage().getTime();
00200   else
00201     return getTimeLastSenseMessage().getTime();
00202 }
00203 
00204 
00208 bool WorldModel::isTimeStopped()
00209 {
00210   return getCurrentTime().isStopped();
00211 }
00212 
00215 bool WorldModel::isLastMessageSee() const
00216 {
00217   return getTimeLastSeeMessage() == getTimeLastSenseMessage() ;
00218 }
00219 
00223 Time WorldModel::getTimeLastSeeGlobalMessage( ) const
00224 {
00225   return getTimeLastSeeMessage();
00226 }
00227 
00234 bool WorldModel::setTimeLastSeeGlobalMessage( Time time )
00235 {
00236   return setTimeLastSeeMessage( time ); // set see message
00237 }
00238 
00241 Time WorldModel::getTimeLastSeeMessage( ) const
00242 {
00243   return timeLastSeeMessage;
00244 }
00245 
00251 bool WorldModel::setTimeLastSeeMessage( Time time )
00252 {
00253   timeLastSeeMessage  = time;
00254   pthread_mutex_lock  ( &mutex_newInfo );
00255   bNewInfo            = true;
00256   pthread_cond_signal ( &cond_newInfo );
00257   pthread_mutex_unlock( &mutex_newInfo );
00258 
00259   return true;
00260 }
00261 
00264 Time WorldModel::getTimeLastSenseMessage( ) const
00265 {
00266   return timeLastSenseMessage ;
00267 }
00268 
00275 bool WorldModel::setTimeLastSenseMessage( Time time )
00276 {
00277   timeLastSenseMessage = time;
00278   pthread_mutex_lock( &mutex_newInfo );
00279   bNewInfo = true;
00280   pthread_cond_signal( &cond_newInfo );
00281   pthread_mutex_unlock( &mutex_newInfo );
00282 
00283   return true;
00284 }
00285 
00289 int WorldModel::getPlayerNumber( ) const
00290 {
00291   return iPlayerNumber;
00292 }
00293 
00298 bool WorldModel::setPlayerNumber( int i )
00299 {
00300   iPlayerNumber = i;
00301   return true;
00302 }
00303 
00307 SideT WorldModel::getSide( ) const
00308 {
00309   return sideSide;
00310 }
00311 
00315 bool WorldModel::setSide( SideT s )
00316 {
00317   sideSide = s;
00318   return true;
00319 }
00320 
00323 const char* WorldModel::getTeamName( ) const
00324 {
00325   return strTeamName ;
00326 }
00327 
00332 bool WorldModel::setTeamName( char * str )
00333 {
00334   strcpy( strTeamName, str );
00335   return true;
00336 }
00337 
00341 PlayModeT WorldModel::getPlayMode( ) const
00342 {
00343   return playMode ;
00344 }
00345 
00349 bool WorldModel::setPlayMode( PlayModeT pm )
00350 {
00351   playMode = pm;
00352   if( ( pm == PM_GOAL_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
00353       ( pm == PM_GOAL_KICK_RIGHT && getSide() == SIDE_RIGHT )    )
00354     setTimeLastCatch( getTimeLastSenseMessage() );
00355   return true;
00356 }
00357 
00362 int WorldModel::getGoalDiff( ) const
00363 {
00364   return iGoalDiff;
00365 }
00366 
00370 int WorldModel::addOneToGoalDiff( )
00371 {
00372   return ++iGoalDiff;
00373 }
00374 
00378 int WorldModel::subtractOneFromGoalDiff()
00379 {
00380   return --iGoalDiff;
00381 }
00382 
00387 int WorldModel::getNrOfCommands( CommandT c ) const
00388 {
00389   return iCommandCounters[ (int) c ];
00390 }
00391 
00401 bool WorldModel::setNrOfCommands( CommandT c, int i )
00402 {
00403   int iIndex = (int) c;
00404 
00405   // if counter is the same as before, no command is performed, otherwise it is
00406   performedCommands[iIndex] = ( iCommandCounters[iIndex] == i ) ? false : true;
00407   iCommandCounters [iIndex] = i;
00408   return true;
00409 }
00410 
00414 Time WorldModel::getTimeCheckBall( ) const
00415 {
00416   return timeCheckBall;
00417 }
00418 
00423 bool WorldModel::setTimeCheckBall( Time time )
00424 {
00425   timeCheckBall = time;
00426   return true;
00427 }
00428 
00433 BallStatusT WorldModel::getCheckBallStatus( ) const
00434 {
00435   return bsCheckBall;
00436 }
00437 
00442 bool WorldModel::setCheckBallStatus( BallStatusT bs )
00443 {
00444   bsCheckBall = bs;
00445   return true;
00446 }
00447 
00456 ObjectT WorldModel::iterateObjectStart(int& iIndex,ObjectSetT g,double dConf)
00457 {
00458   iIndex = -1;
00459   return iterateObjectNext( iIndex, g, dConf);
00460 }
00461 
00470 ObjectT WorldModel::iterateObjectNext(int& iIndex,ObjectSetT g, double dConf)
00471 {
00472   ObjectT o = OBJECT_ILLEGAL;;
00473 
00474   if( iIndex < 0 ) 
00475     iIndex = -1;
00476   
00477   // when dConf is not specified it has the default value of -1.0, in this
00478   // case set it to the confidence threshold defined in PlayerSetting, but
00479   // only do this for dynamic objexts, not for flags, lines, etc. since all
00480   // should be returend.
00481   if( dConf == -1.0 && (g==OBJECT_SET_OPPONENTS || g==OBJECT_SET_PLAYERS ||
00482                         g==OBJECT_SET_TEAMMATES) )
00483     dConf = PS->getPlayerConfThr();
00484   
00485   for( int i = iIndex + 1 ; i < OBJECT_MAX_OBJECTS; i ++ )
00486   {
00487     o = (ObjectT) i;
00488     if( SoccerTypes::isInSet( o, g ) )
00489     {
00490       if( getConfidence( o ) >= dConf )
00491       {
00492         iIndex = i;
00493         return o;
00494       }
00495       else if( dConf == 1.0 && getTimeLastSeeMessage() == getTimeLastSeen( o ) )
00496       {
00497         iIndex = i; // confidence of 1.0 can only be in same cycle as see
00498         return o;   // message. Therefore first test should succeed normally;
00499       }             // in cases where this method is called after see message,
00500                     // but new sense has already arrived, confidence is lowered
00501     }               // but we want to return object that was seen in last see
00502   }                 // message; this compensates for those cases.
00503   return OBJECT_ILLEGAL;
00504 }
00505 
00510 void WorldModel::iterateObjectDone( int &iIndex )
00511 {
00512   iIndex = -1;
00513 }
00514 
00518 ObjectT WorldModel::getAgentObjectType( ) const
00519 {
00520   return agentObject.getType();
00521 }
00522 
00527 bool WorldModel::setAgentObjectType( ObjectT o )
00528 {
00529   agentObject.setType( o );
00530   return true;
00531 }
00532 
00535 AngDeg WorldModel::getAgentBodyAngleRelToNeck( ) const
00536 {
00537   return agentObject.getBodyAngleRelToNeck();
00538 }
00539 
00542 AngDeg WorldModel::getAgentGlobalNeckAngle(  ) const
00543 {
00544   return agentObject.getGlobalNeckAngle(  );
00545 }
00546 
00549 AngDeg WorldModel::getAgentGlobalBodyAngle(  )
00550 {
00551   return agentObject.getGlobalBodyAngle(  );
00552 }
00553 
00556 Stamina WorldModel::getAgentStamina( ) const
00557 {
00558   return agentObject.getStamina();
00559 }
00560 
00564 TiredNessT WorldModel::getAgentTiredNess( ) const
00565 {
00566   return getAgentStamina().getTiredNess( 
00567                     SS->getRecoverDecThr(), SS->getStaminaMax() );
00568 }
00569 
00572 double WorldModel::getAgentEffort( ) const
00573 {
00574   return agentObject.getStamina().getEffort();
00575 }
00576 
00579 VecPosition WorldModel::getAgentGlobalVelocity( ) const
00580 {
00581   return agentObject.getGlobalVelocity();
00582 }
00583 
00586 double WorldModel::getAgentSpeed( ) const
00587 {
00588   return agentObject.getSpeed();
00589 }
00590 
00593 VecPosition WorldModel::getAgentGlobalPosition( ) const
00594 {
00595   return agentObject.getGlobalPosition();
00596 }
00597 
00600 ViewAngleT WorldModel::getAgentViewAngle( ) const
00601 {
00602   return agentObject.getViewAngle();
00603 }
00604 
00607 ViewQualityT WorldModel::getAgentViewQuality( ) const
00608 {
00609   return agentObject.getViewQuality();
00610 }
00611 
00616 double WorldModel::getAgentViewFrequency( ViewAngleT va, ViewQualityT vq ) 
00617 {
00618   double dViewQualityFactor = 0.0;
00619   double dViewWidthFactor   = 0.0;
00620 
00621   if( va == VA_ILLEGAL )
00622     va = getAgentViewAngle();  
00623   if( vq == VQ_ILLEGAL )
00624     vq = getAgentViewQuality();  
00625 
00626   switch( va )
00627   {
00628     case VA_NARROW:  dViewWidthFactor   = 0.5; break;
00629     case VA_NORMAL:  dViewWidthFactor   = 1.0; break;
00630     case VA_WIDE:    dViewWidthFactor   = 2.0; break;
00631     case VA_ILLEGAL:
00632     default:         dViewWidthFactor   = 0.0; break;
00633   }
00634 
00635   switch( vq )
00636   {
00637     case VQ_LOW:     dViewQualityFactor = 0.5; break;
00638     case VQ_HIGH:    dViewQualityFactor = 1.0; break;
00639     case VQ_ILLEGAL:
00640     default:         dViewQualityFactor = 0.0; break;
00641   }
00642 
00643   return dViewQualityFactor*dViewWidthFactor;
00644 }
00645 
00650 VecPosition  WorldModel::getBallPos()
00651 {
00652   return getGlobalPosition( OBJECT_BALL );
00653 }
00654 
00657 double WorldModel::getBallSpeed()
00658 {
00659   return Ball.getGlobalVelocity().getMagnitude();
00660 }
00661 
00664 AngDeg WorldModel::getBallDirection()
00665 {
00666   return Ball.getGlobalVelocity().getDirection();
00667 }
00668 
00673 Time WorldModel::getTimeGlobalPosition( ObjectT o )
00674 {
00675   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00676   if( object != NULL )
00677     return object->getTimeGlobalPosition();
00678   return UnknownTime;
00679 }
00680 
00681 
00688 VecPosition WorldModel::getGlobalPosition( ObjectT o )
00689 {
00690   Object *object = getObjectPtrFromType( o );
00691   if( object != NULL )
00692   {
00693     if( SoccerTypes::isFlag( o ) || SoccerTypes::isGoal( o ) )
00694       return SoccerTypes::getGlobalPositionFlag(o,getSide(),SS->getGoalWidth());
00695     else
00696       return object->getGlobalPosition();
00697   }
00698   return VecPosition( UnknownDoubleValue, UnknownDoubleValue);
00699 }
00700 
00705 Time WorldModel::getTimeGlobalVelocity( ObjectT o )
00706 {
00707   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00708   if( object != NULL )
00709     return object->getTimeGlobalVelocity();
00710   return UnknownTime;
00711 }
00712 
00713 
00719 VecPosition WorldModel::getGlobalVelocity( ObjectT o )
00720 {
00721   DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
00722   if( object != NULL )
00723     return object->getGlobalVelocity(  );
00724   return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
00725 
00726 }
00727 
00733 double WorldModel::getRelativeDistance( ObjectT o )
00734 {
00735   Object *object = getObjectPtrFromType( o );
00736   if( object != NULL )
00737     return object->getRelativeDistance();
00738   return UnknownDoubleValue;
00739 }
00740 
00746 VecPosition  WorldModel::getRelativePosition( ObjectT o )
00747 {
00748   Object *object = getObjectPtrFromType( o );
00749   if( object != NULL )
00750     return object->getRelativePosition();
00751   return VecPosition(UnknownDoubleValue, UnknownDoubleValue);
00752 }
00753 
00764 AngDeg WorldModel::getRelativeAngle( ObjectT o, bool bWithBody )
00765 {
00766   Object *object = getObjectPtrFromType( o );
00767   double dBody   = 0.0;
00768 
00769   if( object != NULL )
00770   {
00771     if( bWithBody == true )
00772       dBody = getAgentBodyAngleRelToNeck();
00773     return VecPosition::normalizeAngle( object->getRelativeAngle() - dBody );
00774   }
00775   return UnknownDoubleValue;
00776 }
00777 
00782 Time WorldModel::getTimeGlobalAngles( ObjectT o )
00783 {
00784   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00785   if( object != NULL )
00786     return object->getTimeGlobalAngles();
00787   return Time( -1, 0);
00788 }
00789 
00795 AngDeg WorldModel::getGlobalBodyAngle( ObjectT o )
00796 {
00797   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00798   if( object != NULL )
00799     return object->getGlobalBodyAngle();
00800   return UnknownAngleValue;
00801 }
00802 
00808 AngDeg WorldModel::getGlobalNeckAngle( ObjectT o )
00809 {
00810   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00811   if( object != NULL )
00812     return object->getGlobalNeckAngle();
00813   return UnknownAngleValue;
00814 }
00815 
00820 AngDeg WorldModel::getGlobalAngle( ObjectT o )
00821 {
00822   if( SoccerTypes::isLine( o ) )
00823     return SoccerTypes::getGlobalAngleLine( o, getSide() );
00824   return UnknownAngleValue;
00825 }
00826 
00827 
00833 double WorldModel::getConfidence( ObjectT o)
00834 {
00835   Object *object = getObjectPtrFromType( o );
00836   if( object != NULL )
00837     return object->getConfidence( getCurrentTime() );
00838 
00839   return 0.0;
00840 }
00841 
00849 bool WorldModel::isKnownPlayer( ObjectT o )
00850 {
00851   PlayerObject *object = (PlayerObject *)getObjectPtrFromType( o );
00852   if( object != NULL  )
00853     return object->getIsKnownPlayer();
00854   return false;
00855 
00856 }
00857 
00866 ObjectT WorldModel::getOppGoalieType()
00867 {
00868   ObjectT objOppMaxX = OBJECT_ILLEGAL;
00869   double  x = -100.0, y = UnknownDoubleValue;
00870 
00871   for( int i = 0; i < MAX_OPPONENTS; i++ )
00872   {
00873     if( isConfidenceGood( Opponents[i].getType( ) ) )
00874     {
00875       if( Opponents[i].getIsGoalie() == true ) // &&
00876 //           Opponents[i].getGlobalPosition().getX() > PENALTY_X - 2.0 )
00877         return Opponents[i].getType();
00878       if( Opponents[i].getGlobalPosition().getX() > x )
00879       {
00880         x          = Opponents[i].getGlobalPosition().getX();
00881         y          = Opponents[i].getGlobalPosition().getY();
00882         objOppMaxX = Opponents[i].getType();
00883       }
00884     }
00885   }
00886 
00887   // if opponent with highest x is nr 1, assume it is goalkeeper when standing
00888   // in own penalty area, otherwise assume goalkeeper closest player to goal.
00889   if(  objOppMaxX == OBJECT_OPPONENT_1 && x > PENALTY_X + 4.0 ||
00890       (objOppMaxX != OBJECT_ILLEGAL    && x > PITCH_LENGTH/2.0 - 6.0 &&
00891        fabs( y ) < SS->getGoalWidth()/2.0 ))
00892     return objOppMaxX;
00893   return OBJECT_ILLEGAL;
00894 }
00895 
00896 
00905 ObjectT WorldModel::getOwnGoalieType()
00906 {
00907   ObjectT objOwnMinX = OBJECT_ILLEGAL;
00908   double x = -100.0, y = UnknownDoubleValue;
00909   for( int i = 0; i < MAX_TEAMMATES; i++ )
00910   {
00911     if( isConfidenceGood( Teammates[i].getType( ) ) )
00912     {
00913       if( Teammates[i].getIsGoalie() == true )
00914         return Teammates[i].getType();
00915       if( Teammates[i].getGlobalPosition().getX() < x )
00916       {
00917         x          = Teammates[i].getGlobalPosition().getX();
00918         y          = Teammates[i].getGlobalPosition().getY();
00919         objOwnMinX = Teammates[i].getType();
00920       }
00921     }
00922   }
00923   if(  objOwnMinX == OBJECT_TEAMMATE_1 && x < - ( PENALTY_X + 4.0 ) ||
00924       (objOwnMinX != OBJECT_ILLEGAL    && x < - ( PITCH_LENGTH/2.0 - 6.0 ) &&
00925        fabs( y ) < SS->getGoalWidth()/2.0 ))
00926     return objOwnMinX;
00927   return OBJECT_ILLEGAL;
00928 }
00929 
00930 
00935 Time WorldModel::getTimeLastSeen( ObjectT o )
00936 {
00937   Object *object = getObjectPtrFromType( o );
00938   if( object != NULL )
00939     return object->getTimeLastSeen(  );
00940   return Time( -1, 0);
00941 
00942 }
00943 
00949 Time WorldModel::getTimeChangeInformation( ObjectT o )
00950 {
00951   DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
00952   if( object != NULL )
00953     return object->getTimeChangeInformation(  );
00954   return Time( -1, 0);
00955 
00956 }
00957 
00966 bool WorldModel::setIsKnownPlayer( ObjectT o, bool isKnownPlayer )
00967 {
00968   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00969   return object->setIsKnownPlayer( isKnownPlayer );
00970 }
00971 
00976 bool WorldModel::setTimeLastSeen( ObjectT o, Time time )
00977 {
00978   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00979   return object->setTimeLastSeen( time );
00980 }
00981                                              
00984 VecPosition WorldModel::getPosOpponentGoal( )
00985 {
00986   return SoccerTypes::getGlobalPositionFlag(
00987                         SoccerTypes::getGoalOpponent( getSide() ),
00988                         getSide( ),
00989                         SS->getGoalWidth() );
00990 }
00991 
00994 VecPosition WorldModel::getPosOwnGoal( )
00995 {
00996   return SoccerTypes::getGlobalPositionFlag(
00997                         SoccerTypes::getOwnGoal( getSide() ),
00998                         getSide( ),
00999                         SS->getGoalWidth() );
01000 }
01001 
01004 double  WorldModel::getRelDistanceOpponentGoal()
01005 {
01006   VecPosition posGoal;
01007   if( sideSide == SIDE_LEFT )
01008     posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_R, sideSide );
01009   else
01010     posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_L, sideSide );
01011 
01012   return getAgentGlobalPosition().getDistanceTo( posGoal );
01013 
01014 }
01015 
01020 double  WorldModel::getRelAngleOpponentGoal()
01021 {
01022   VecPosition posGoal;
01023   if( sideSide == SIDE_LEFT )
01024     posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_R, sideSide );
01025   else
01026     posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_L, sideSide );
01027 
01028   return ( posGoal - getAgentGlobalPosition()).getDirection() ;
01029 
01030 }
01031 
01035 HeteroPlayerSettings WorldModel::getInfoHeteroPlayer( int iIndex )
01036 {
01037   return pt[iIndex];
01038 }
01039 
01048 bool WorldModel::isQueuedActionPerformed()
01049 {
01050   // for all possible commands check if it is sent in previous cycle,
01051   // but not performed
01052   for( int i = 0 ; i < MAX_COMMANDS ; i++ )
01053     if( queuedCommands[i].time   == getTimeLastSenseMessage() - 1 &&
01054         performedCommands[i]     == false )
01055       return false;
01056 
01057   return true;
01058 }
01059 
01066 bool WorldModel::isFreeKickUs( PlayModeT pm )
01067 {
01068   if( pm == PM_ILLEGAL ) 
01069     pm = getPlayMode();
01070     
01071   return ( pm == PM_FREE_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
01072          ( pm == PM_FREE_KICK_RIGHT && getSide() == SIDE_RIGHT ) ;
01073 }
01074 
01081 bool WorldModel::isFreeKickThem( PlayModeT pm )
01082 {
01083   if( pm == PM_ILLEGAL ) 
01084     pm = getPlayMode();
01085 
01086   return ( pm == PM_FREE_KICK_RIGHT  && getSide() == SIDE_LEFT  ) ||
01087          ( pm == PM_FREE_KICK_LEFT   && getSide() == SIDE_RIGHT ) ;
01088 }
01089 
01099 bool WorldModel::isBeforeKickOff( PlayModeT pm )
01100 {
01101   if( pm == PM_ILLEGAL ) 
01102     pm = getPlayMode();
01103 
01104   return pm == PM_BEFORE_KICK_OFF  || pm == PM_GOAL_LEFT  ||
01105          pm == PM_GOAL_RIGHT ; // || isKickOffUs( pm ) || isKickOffThem( pm );
01106 }
01107 
01116 bool WorldModel::isDeadBallUs( PlayModeT pm )
01117 {
01118   if( pm == PM_ILLEGAL ) 
01119     pm = getPlayMode();
01120 
01121   return isKickInUs  ( pm ) || isFreeKickUs  ( pm ) || isCornerKickUs     ( pm ) 
01122       || isKickOffUs ( pm ) || isOffsideThem ( pm ) || isFreeKickFaultThem( pm )
01123       || isGoalKickUs( pm ) || isBackPassThem( pm ) ;
01124 }
01125 
01126 
01135 bool WorldModel::isDeadBallThem( PlayModeT pm )
01136 {
01137   if( pm == PM_ILLEGAL ) 
01138     pm = getPlayMode();
01139 
01140   return isFreeKickThem( pm ) || isKickInThem  ( pm ) || isCornerKickThem ( pm ) 
01141      ||  isKickOffThem ( pm ) || isGoalKickThem( pm ) || isFreeKickFaultUs( pm )
01142      || isOffsideUs    ( pm ) || isBackPassUs  ( pm ) ; 
01143 }
01144 
01151 bool WorldModel::isCornerKickUs( PlayModeT pm )
01152 {
01153   if( pm == PM_ILLEGAL ) 
01154     pm = getPlayMode();
01155 
01156   return ( pm == PM_CORNER_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
01157          ( pm == PM_CORNER_KICK_RIGHT && getSide() == SIDE_RIGHT ) ;
01158 }
01159 
01166 bool WorldModel::isCornerKickThem( PlayModeT pm )
01167 {
01168   if( pm == PM_ILLEGAL ) 
01169     pm = getPlayMode();
01170 
01171   return ( pm == PM_CORNER_KICK_RIGHT  && getSide() == SIDE_LEFT  ) ||
01172          ( pm == PM_CORNER_KICK_LEFT   && getSide() == SIDE_RIGHT ) ;
01173 }
01174 
01181 bool WorldModel::isOffsideUs( PlayModeT pm )
01182 {
01183   if( pm == PM_ILLEGAL ) 
01184     pm = getPlayMode();
01185 
01186   return ( pm == PM_OFFSIDE_RIGHT  && getSide() == SIDE_RIGHT ) ||
01187          ( pm == PM_OFFSIDE_LEFT   && getSide() == SIDE_LEFT );
01188 }
01189 
01196 bool WorldModel::isOffsideThem( PlayModeT pm )
01197 {
01198   if( pm == PM_ILLEGAL ) 
01199     pm = getPlayMode();
01200 
01201   return ( pm == PM_OFFSIDE_LEFT  && getSide() == SIDE_RIGHT ) ||
01202          ( pm == PM_OFFSIDE_RIGHT && getSide() == SIDE_LEFT );
01203 }
01204 
01211 bool WorldModel::isKickInUs( PlayModeT pm )
01212 {
01213   if( pm == PM_ILLEGAL ) 
01214     pm = getPlayMode();
01215 
01216   return ( pm == PM_KICK_IN_LEFT  && getSide() == SIDE_LEFT  ) ||
01217          ( pm == PM_KICK_IN_RIGHT && getSide() == SIDE_RIGHT ) ;
01218 }
01219 
01226 bool WorldModel::isKickInThem( PlayModeT pm )
01227 {
01228   if( pm == PM_ILLEGAL ) 
01229     pm = getPlayMode();
01230 
01231   return ( pm == PM_KICK_IN_RIGHT  && getSide() == SIDE_LEFT  ) ||
01232          ( pm == PM_KICK_IN_LEFT   && getSide() == SIDE_RIGHT ) ;
01233 }
01234 
01243 bool WorldModel::isFreeKickFaultUs( PlayModeT pm )
01244 {
01245   if( pm == PM_ILLEGAL ) 
01246     pm = getPlayMode();
01247 
01248   return ( pm == PM_FREE_KICK_FAULT_LEFT  && getSide() == SIDE_LEFT  ) ||
01249          ( pm == PM_FREE_KICK_FAULT_RIGHT && getSide() == SIDE_RIGHT ) ;
01250 }
01251 
01260 bool WorldModel::isFreeKickFaultThem( PlayModeT pm )
01261 {
01262   if( pm == PM_ILLEGAL ) 
01263     pm = getPlayMode();
01264 
01265   return ( pm == PM_FREE_KICK_FAULT_RIGHT  && getSide() == SIDE_LEFT  ) ||
01266          ( pm == PM_FREE_KICK_FAULT_LEFT   && getSide() == SIDE_RIGHT ) ;
01267 }
01268 
01275 bool WorldModel::isKickOffUs( PlayModeT pm )
01276 {
01277   if( pm == PM_ILLEGAL ) 
01278     pm = getPlayMode();
01279 
01280   return ( pm == PM_KICK_OFF_LEFT  && getSide() == SIDE_LEFT  ) ||
01281          ( pm == PM_KICK_OFF_RIGHT && getSide() == SIDE_RIGHT ) ;
01282 }
01283 
01291 bool WorldModel::isKickOffThem( PlayModeT pm )
01292 {
01293   if( pm == PM_ILLEGAL ) 
01294     pm = getPlayMode();
01295 
01296   return ( pm == PM_KICK_OFF_RIGHT  && getSide() == SIDE_LEFT  ) ||
01297          ( pm == PM_KICK_OFF_LEFT   && getSide() == SIDE_RIGHT ) ;
01298 }
01299 
01308 bool WorldModel::isBackPassUs( PlayModeT pm )
01309 {
01310   if( pm == PM_ILLEGAL ) 
01311     pm = getPlayMode();
01312 
01313   return ( pm == PM_BACK_PASS_LEFT  && getSide() == SIDE_LEFT  ) ||
01314          ( pm == PM_BACK_PASS_RIGHT && getSide() == SIDE_RIGHT ) ;
01315 }
01316 
01325 bool WorldModel::isBackPassThem( PlayModeT pm )
01326 {
01327   if( pm == PM_ILLEGAL ) 
01328     pm = getPlayMode();
01329 
01330   return ( pm == PM_BACK_PASS_RIGHT && getSide() == SIDE_LEFT  ) ||
01331          ( pm == PM_BACK_PASS_LEFT  && getSide() == SIDE_RIGHT ) ;
01332 }
01333 
01340 bool WorldModel::isGoalKickUs( PlayModeT pm )
01341 {
01342   if( pm == PM_ILLEGAL ) 
01343     pm = getPlayMode();
01344 
01345   return ( pm == PM_GOAL_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
01346          ( pm == PM_GOAL_KICK_RIGHT && getSide() == SIDE_RIGHT ) ;
01347 }
01348 
01355 bool WorldModel::isGoalKickThem( PlayModeT pm )
01356 {
01357   if( pm == PM_ILLEGAL ) 
01358     pm = getPlayMode();
01359 
01360   return ( pm == PM_GOAL_KICK_RIGHT  && getSide() == SIDE_LEFT  ) ||
01361          ( pm == PM_GOAL_KICK_LEFT   && getSide() == SIDE_RIGHT ) ;
01362 }
01363 
01364 
01369 void WorldModel::show( ostream & os )
01370 {
01371   int i;
01372   os << "Worldmodel (" << getCurrentTime() << ")\n" <<
01373         "========================\n";
01374   os << "Teamname: " << getTeamName() << endl;
01375   if( Ball.getTimeLastSeen( ).getTime() != -1 )
01376     Ball.show();
01377   os << "Teammates: " << endl;
01378   for( i = 0; i < MAX_TEAMMATES ; i++ )
01379     if( isConfidenceGood( Teammates[i].getType() ) )
01380       Teammates[i].show( getTeamName() );
01381   os << "Opponents: " << endl;
01382   for( i = 0; i < MAX_OPPONENTS ; i++ )
01383     if( isConfidenceGood( Opponents[i].getType() ) )
01384       Opponents[i].show( DEFAULT_OPPONENT_NAME );
01385   os << "Agent: " << endl;
01386   agentObject.show( getTeamName() );
01387 
01388   os << "General Info: " << endl <<
01389           "side: "      << SoccerTypes::getSideStr( getSide() )    << endl <<
01390           "kicks: "     << getNrOfCommands( CMD_KICK )             << endl <<
01391           "turns: "     << getNrOfCommands( CMD_TURN )             << endl <<
01392           "dashes: "    << getNrOfCommands( CMD_DASH )             << endl <<
01393           "turnnecks: " << getNrOfCommands( CMD_TURNNECK )         << endl <<
01394           "says: "      << getNrOfCommands( CMD_SAY )              << endl <<
01395           "playmode: "  << SoccerTypes::getPlayModeStr( playMode ) << endl <<
01396           "===================================="                   << endl;
01397 
01398 }
01399 
01403 void WorldModel::show( ObjectSetT set, ostream & os )
01404 {
01405   int iIndex;
01406   for( ObjectT o = iterateObjectStart( iIndex, set, -1.0 );
01407        o != OBJECT_ILLEGAL;
01408        o = iterateObjectNext( iIndex, set, -1.0 ) )
01409     show( o, os );
01410   os << endl;
01411 }
01412 
01416 void WorldModel::showQueuedCommands( ostream & os )
01417 {
01418   os << "Commands in queue:\n" ;
01419   for( int i = 0; i < MAX_COMMANDS; i++ )
01420     if( queuedCommands[i].commandType != CMD_ILLEGAL )
01421       queuedCommands[i].show( os );
01422 }
01423 
01428 void WorldModel::show( ObjectT o, ostream &os )
01429 {
01430   Object *object = getObjectPtrFromType( o );
01431   if( object != NULL )
01432   {
01433     if( SoccerTypes::isPlayer( o ) )
01434     {
01435       PlayerObject *pobj = (PlayerObject*) object ;
01436       if( SoccerTypes::isTeammate( o ) )
01437         pobj->show( getTeamName(), os );
01438       else
01439         pobj->show( "Opponent", os );
01440     }
01441     else
01442       object->show( os );    
01443   }
01444 }
01445 
01451 bool WorldModel::waitForNewInformation( )
01452 {
01453   bool bReturn = true;
01454   if( bNewInfo == false ) // there hasn't arrived any information yet
01455   {
01456     struct timeval now;
01457     struct timespec timeout;
01458     gettimeofday(&now, NULL);
01459     timeout.tv_sec = now.tv_sec + PS->getServerTimeOut();
01460     timeout.tv_nsec = now.tv_usec*1000;
01461 
01462     // lock mutex and wait till it is unlocked by Sense thread
01463     // this happens in setTimeLastSeeMessage, setTimeLastSenseMessage
01464     // or setTimeLastSeeGlobalMessage
01465     pthread_mutex_lock( &mutex_newInfo );
01466     int ret;
01467     Log.logWithTime( 2, "go into conditional wait" );
01468     while( (ret = pthread_cond_timedwait( &cond_newInfo,
01469                     &mutex_newInfo, &timeout) ) == EINTR )
01470      printf("(WorldModel::waitForNewInformation) failure in loop!!\n");
01471     Log.logWithTime( 2, "go out of conditional wait" );     
01472     if( ret == ETIMEDOUT ) // if no information was received but timer timed out
01473       bReturn = false;
01474     pthread_mutex_unlock( &mutex_newInfo );
01475   }
01476   // reset the indication of new visual information
01477   bNewInfo = false;
01478 
01479   return bReturn;
01480 }
01481 
01498 void WorldModel::logObjectInformation( int iLogLevel, ObjectT obj )
01499 {
01500   
01501   char   str[2048];
01502   double dConf     = PS->getPlayerConfThr();
01503   sprintf( str, "(%4d,%3d) ", getCurrentTime().getTime(),
01504                               getCurrentTime().getTimeStopped() );
01505   if( obj != OBJECT_ILLEGAL )
01506     sprintf( str, "%12.6f %12.6f %12.6f %12.6f %12.6f %12.6f",
01507                                   getGlobalPosition(obj).getX(),
01508                                   getGlobalPosition(obj).getY(),
01509                                   getGlobalVelocity(obj).getX(),
01510                                   getGlobalVelocity(obj).getY(),
01511                                   getGlobalBodyAngle(obj),
01512                                   getGlobalNeckAngle(obj) );
01513 
01514   if( getConfidence      ( OBJECT_BALL ) > dConf &&
01515       getRelativeDistance( OBJECT_BALL ) < 20.0 )
01516     sprintf( &str[strlen(str)], " %12.6f %12.6f",
01517                                   getGlobalPosition(OBJECT_BALL).getX(),
01518                                   getGlobalPosition(OBJECT_BALL).getY() );
01519   else
01520     sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
01521 
01522   if( getTimeGlobalVelocity( OBJECT_BALL ) > getTimeFromConfidence( dConf ) &&
01523       getRelativeDistance  ( OBJECT_BALL ) < 20.0 )
01524     sprintf( &str[strlen(str)], " %12.6f %12.6f",
01525                                   getGlobalVelocity(OBJECT_BALL).getX(),
01526                                   getGlobalVelocity(OBJECT_BALL).getY() );
01527   else
01528     sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
01529   sprintf( &str[strlen(str)], " %12.6f", getConfidence(OBJECT_BALL) );
01530 
01531   int    iIndex=-1;
01532   int    iIndexPlayer;
01533   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, 0.0 );
01534        o != OBJECT_ILLEGAL;
01535        o = iterateObjectNext( iIndex, OBJECT_SET_PLAYERS, 0.0 ) )
01536   {
01537     bool bPrint = false;
01538     
01539     iIndexPlayer = (SoccerTypes::isTeammate(o))
01540                      ? SoccerTypes::getIndex(o) + 1
01541                      : SoccerTypes::getIndex(o) + 12;
01542     sprintf( &str[strlen(str)], " %d", iIndexPlayer );
01543     if( getConfidence      ( o ) > dConf && isKnownPlayer(o) &&
01544         getRelativeDistance( o ) < 20.0 )
01545     {
01546       sprintf( &str[strlen(str)], " %12.6f %12.6f",
01547                                     getGlobalPosition(o).getX(),
01548                                     getGlobalPosition(o).getY() );
01549       bPrint = true;
01550     }
01551     else
01552        sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
01553 
01554     if( getTimeGlobalVelocity( o ) > getTimeFromConfidence( dConf ) &&
01555         getRelativeDistance  ( o ) < 20.0 && isKnownPlayer(o)  )
01556     {
01557       sprintf( &str[strlen(str)], " %12.6f %12.6f",
01558                                     getGlobalVelocity(o).getX(),
01559                                     getGlobalVelocity(o).getY() );
01560       bPrint = true;
01561     }
01562     else
01563       sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
01564 
01565     if( bPrint )
01566       sprintf( &str[strlen(str)], " %12.6f", getConfidence(o) );
01567     else
01568       sprintf( &str[strlen(str)], " %12.6f", -10.0 );
01569   }
01570   if( getCurrentCycle() != 3000  )
01571       Log.log( iLogLevel, str );
01572 }

Generated on Tue Jul 2 10:18:54 2002 for UvA Trilearn 2002 by doxygen1.2.12 written by Dimitri van Heesch, © 1997-2001