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,2001, 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 */
00043 #include<stdio.h>    // needed for printf
00044 #include<strings.h>  // needed for strcpy
00045 #include<errno.h>    // needed for ETIMEDOUT
00046 #include<pthread.h>  // needed for pthread_mutex_init
00047 #include<string.h>   // needed for strcpy
00048 #include<math.h>     // needed for erf
00049 #include"WorldModel.h"
00050 
00051 /*****************************************************************************/
00052 /********************** CLASS WORLDMODEL *************************************/
00053 /*****************************************************************************/
00054 
00059 WorldModel::WorldModel( ServerSettings *ss, PlayerSettings *ps):
00060     agentObject(  )
00061 {
00062   SS                    = ss;
00063   PS                    = ps;
00064   bNewInfo              = false;
00065 
00066   setSide                ( SIDE_ILLEGAL ); // is set by init message
00067   strTeamName[0]        = '\0';
00068   setPlayMode            ( PM_BEFORE_KICK_OFF );
00069   iGoalDiff             = 0;
00070 
00071   int i;
00072   for( i = 0; i < MAX_TEAMMATES ; i ++ )
00073     Teammates[i].setType( SoccerTypes::getTeammateObjectFromIndex( i ) );
00074   for( i = 0; i < MAX_OPPONENTS ; i ++ )
00075     Opponents[i].setType( SoccerTypes::getOpponentObjectFromIndex( i ) );
00076   for( i = 0; i < MAX_OPPONENTS + MAX_TEAMMATES ; i ++ )
00077     UnknownPlayers[i].setType( OBJECT_ILLEGAL );
00078   for( i = 0; i < MAX_FLAGS; i ++ )
00079     Flags[i].setType( OBJECT_ILLEGAL );
00080   for( i = 0; i < MAX_LINES; i ++ )
00081     Lines[i].setType( OBJECT_ILLEGAL );
00082 
00083   iNrUnknownPlayers     = 0;
00084 
00085   Ball.setType             ( OBJECT_BALL    );
00086   agentObject.setType      ( OBJECT_ILLEGAL );
00087   agentObject.setStamina   ( Stamina(SS->getStaminaMax(),1.0,1.0) );
00088 
00089   for( i = 0 ; i < CMD_MAX_COMMANDS ; i ++ )
00090   {
00091     queuedCommands[i].commandType = (CommandT)i;
00092     performedCommands[i]          = false;
00093     iCommandCounters[i]           = 0;
00094   }
00095 
00096   iNrHoles              = 0;
00097   iNrOpponentsSeen      = 0;
00098   iNrTeammatesSeen      = 0;
00099   bsCheckBall           = BS_ILLEGAL;
00100 
00101   // initialize the mutex for bNewInfo
00102   pthread_mutex_init( &mutex_newInfo, NULL );
00103   pthread_cond_init ( &cond_newInfo, NULL );
00104 }
00105 
00110 Object* WorldModel::getObjectPtrFromType( ObjectT o )
00111 {
00112   Object *object = NULL;
00113   if( o == OBJECT_ILLEGAL )
00114     return NULL;
00115 
00116   if( SoccerTypes::isKnownPlayer( o ) )
00117   {
00118     if( o == agentObject.getType() )
00119       object = &agentObject;
00120     else if( SoccerTypes::isTeammate( o ) )
00121       object = &Teammates[SoccerTypes::getIndex(o)];
00122     else
00123       object = &Opponents[SoccerTypes::getIndex(o)];
00124   }
00125   else if( SoccerTypes::isFlag( o ) )
00126     object = &Flags[SoccerTypes::getIndex(o)];
00127   else if( SoccerTypes::isLine( o ) )
00128     object = &Lines[SoccerTypes::getIndex(o)];
00129   else if( SoccerTypes::isBall( o ) )
00130     object = &Ball;
00131   else if( o == OBJECT_OPPONENT_GOALIE )
00132     return getObjectPtrFromType( getOppGoalieType() );
00133   else if( o == OBJECT_TEAMMATE_GOALIE )
00134     return getObjectPtrFromType( getOwnGoalieType() );
00135  return object;
00136 }
00137 
00143 void WorldModel::setTimeLastCatch( Time time )
00144 {
00145   timeLastCatch = time;
00146 }
00147 
00150 int WorldModel::getTimeSinceLastCatch()
00151 {
00152   if( timeLastCatch.getTime() == -1 )
00153     return 1000;
00154   return timeLastSenseMessage - timeLastCatch;
00155 }
00156 
00160 bool WorldModel::setTimeLastRefereeMessage( Time time )
00161 {
00162   timeLastRefMessage = time;
00163   return true;
00164 }
00165 
00168 Time WorldModel::getTimeLastRefereeMessage( )
00169 {
00170   return timeLastRefMessage;
00171 }
00172 
00177 Time WorldModel::getCurrentTime()
00178 {
00179   if( getPlayerNumber() == 0 )
00180     return getTimeLastSeeGlobalMessage();
00181   else
00182     return getTimeLastSenseMessage();
00183 }
00184 
00189 int WorldModel::getCurrentCycle()
00190 {
00191   if( getPlayerNumber() == 0 )
00192     return getTimeLastSeeGlobalMessage().getTime();
00193   else
00194     return getTimeLastSenseMessage().getTime();
00195 }
00196 
00197 
00201 bool WorldModel::isTimeStopped()
00202 {
00203   return getCurrentTime().isStopped();
00204 }
00205 
00208 bool WorldModel::isLastMessageSee() const
00209 {
00210   return getTimeLastSeeMessage() == getTimeLastSenseMessage() ;
00211 }
00212 
00216 Time WorldModel::getTimeLastSeeGlobalMessage( ) const
00217 {
00218   return getTimeLastSeeMessage();
00219 }
00220 
00227 bool WorldModel::setTimeLastSeeGlobalMessage( Time time )
00228 {
00229   return setTimeLastSeeMessage( time ); // set see message
00230 }
00231 
00234 Time WorldModel::getTimeLastSeeMessage( ) const
00235 {
00236   return timeLastSeeMessage;
00237 }
00238 
00244 bool WorldModel::setTimeLastSeeMessage( Time time )
00245 {
00246   timeLastSeeMessage  = time;
00247   pthread_mutex_lock  ( &mutex_newInfo );
00248   bNewInfo            = true;
00249   pthread_cond_signal ( &cond_newInfo );
00250   pthread_mutex_unlock( &mutex_newInfo );
00251 
00252   return true;
00253 }
00254 
00257 Time WorldModel::getTimeLastSenseMessage( ) const
00258 {
00259   return timeLastSenseMessage ;
00260 }
00261 
00268 bool WorldModel::setTimeLastSenseMessage( Time time )
00269 {
00270   timeLastSenseMessage = time;
00271   pthread_mutex_lock( &mutex_newInfo );
00272   bNewInfo = true;
00273   pthread_cond_signal( &cond_newInfo );
00274   pthread_mutex_unlock( &mutex_newInfo );
00275 
00276   return true;
00277 }
00278 
00282 int WorldModel::getPlayerNumber( ) const
00283 {
00284   return iPlayerNumber;
00285 }
00286 
00291 bool WorldModel::setPlayerNumber( int i )
00292 {
00293   iPlayerNumber = i;
00294   return true;
00295 }
00296 
00300 SideT WorldModel::getSide( ) const
00301 {
00302   return sideSide;
00303 }
00304 
00308 bool WorldModel::setSide( SideT s )
00309 {
00310   sideSide = s;
00311   return true;
00312 }
00313 
00316 const char* WorldModel::getTeamName( ) const
00317 {
00318   return strTeamName ;
00319 }
00320 
00325 bool WorldModel::setTeamName( char * str )
00326 {
00327   strcpy( strTeamName, str );
00328   return true;
00329 }
00330 
00334 PlayModeT WorldModel::getPlayMode( ) const
00335 {
00336   return playMode ;
00337 }
00338 
00342 bool WorldModel::setPlayMode( PlayModeT pm )
00343 {
00344   playMode = pm;
00345   if( ( pm == PM_GOAL_KICK_LEFT  && getSide() == SIDE_LEFT  ) ||
00346       ( pm == PM_GOAL_KICK_RIGHT && getSide() == SIDE_RIGHT )    )
00347     setTimeLastCatch( getTimeLastSenseMessage() );
00348   return true;
00349 }
00350 
00355 int WorldModel::getGoalDiff( ) const
00356 {
00357   return iGoalDiff;
00358 }
00359 
00363 int WorldModel::addOneToGoalDiff( )
00364 {
00365   return ++iGoalDiff;
00366 }
00367 
00371 int WorldModel::subtractOneFromGoalDiff()
00372 {
00373   return --iGoalDiff;
00374 }
00375 
00380 int WorldModel::getNrOfCommands( CommandT c ) const
00381 {
00382   return iCommandCounters[ (int) c ];
00383 }
00384 
00394 bool WorldModel::setNrOfCommands( CommandT c, int i )
00395 {
00396   int iIndex = (int) c;
00397 
00398   // if counter is the same as before, no command is performed, otherwise it is
00399   performedCommands[iIndex] = ( iCommandCounters[iIndex] == i ) ? false : true;
00400   iCommandCounters [iIndex] = i;
00401   return true;
00402 }
00403 
00407 Time WorldModel::getTimeCheckBall( ) const
00408 {
00409   return timeCheckBall;
00410 }
00411 
00416 bool WorldModel::setTimeCheckBall( Time time )
00417 {
00418   timeCheckBall = time;
00419   return true;
00420 }
00421 
00426 BallStatusT WorldModel::getCheckBallStatus( ) const
00427 {
00428   return bsCheckBall;
00429 }
00430 
00435 bool WorldModel::setCheckBallStatus( BallStatusT bs )
00436 {
00437   bsCheckBall = bs;
00438   return true;
00439 }
00440 
00449 ObjectT WorldModel::iterateObjectStart(int& iIndex,ObjectSetT g,double dConf)
00450 {
00451   iIndex = -1;
00452   return iterateObjectNext( iIndex, g, dConf);
00453 }
00454 
00463 ObjectT WorldModel::iterateObjectNext(int& iIndex,ObjectSetT g, double dConf)
00464 {
00465   ObjectT o = OBJECT_ILLEGAL;;
00466 
00467   if( iIndex < 0 )
00468     iIndex = -1;
00469   for( int i = iIndex + 1 ; i < OBJECT_MAX_OBJECTS; i ++ )
00470   {
00471     o = (ObjectT) i;
00472     if( SoccerTypes::isInSet( o, g ) )
00473     {
00474       if( getConfidence( o ) >= dConf )
00475       {
00476         iIndex = i;
00477         return o;
00478       }
00479       else if( dConf == 1.0 && getTimeLastSeeMessage() == getTimeLastSeen( o ) )
00480       {
00481         iIndex = i; // confidence of 1.0 can only be in same cycle as see
00482         return o;   // message. Therefore first test should succeed normally;
00483       }             // in cases where this method is called after see message,
00484                     // but new sense has already arrived, confidence is lowered
00485     }               // but we want to return object that was seen in last see
00486   }                 // message; this compensates for those cases.
00487   return OBJECT_ILLEGAL;
00488 }
00489 
00494 void WorldModel::iterateObjectDone( int &iIndex )
00495 {
00496   iIndex = -1;
00497 }
00498 
00502 ObjectT WorldModel::getAgentObjectType( ) const
00503 {
00504   return agentObject.getType();
00505 }
00506 
00511 bool WorldModel::setAgentObjectType( ObjectT o )
00512 {
00513   agentObject.setType( o );
00514   return true;
00515 }
00516 
00519 AngDeg WorldModel::getAgentBodyAngleRelToNeck( ) const
00520 {
00521   return agentObject.getBodyAngleRelToNeck();
00522 }
00523 
00526 AngDeg WorldModel::getAgentGlobalNeckAngle(  ) const
00527 {
00528   return agentObject.getGlobalNeckAngle(  );
00529 }
00530 
00533 AngDeg WorldModel::getAgentGlobalBodyAngle(  )
00534 {
00535   return agentObject.getGlobalBodyAngle(  );
00536 }
00537 
00540 Stamina WorldModel::getAgentStamina( ) const
00541 {
00542   return agentObject.getStamina();
00543 }
00544 
00547 double WorldModel::getAgentEffort( ) const
00548 {
00549   return agentObject.getStamina().getEffort();
00550 }
00551 
00554 VecPosition WorldModel::getAgentGlobalVelocity( ) const
00555 {
00556   return agentObject.getGlobalVelocity();
00557 }
00558 
00561 double WorldModel::getAgentSpeed( ) const
00562 {
00563   return agentObject.getSpeed();
00564 }
00565 
00568 VecPosition WorldModel::getAgentGlobalPosition( ) const
00569 {
00570   return agentObject.getGlobalPosition();
00571 }
00572 
00575 ViewAngleT WorldModel::getAgentViewAngle( ) const
00576 {
00577   return agentObject.getViewAngle();
00578 }
00579 
00582 ViewQualityT WorldModel::getAgentViewQuality( ) const
00583 {
00584   return agentObject.getViewQuality();
00585 }
00586 
00591 double WorldModel::getAgentViewFrequency( ) const
00592 {
00593   double dViewQualityFactor = 0.0;
00594   double dViewWidthFactor   = 0.0;
00595 
00596   switch( getAgentViewAngle() )
00597   {
00598     case VA_NARROW:  dViewWidthFactor   = 0.5; break;
00599     case VA_NORMAL:  dViewWidthFactor   = 1.0; break;
00600     case VA_WIDE:    dViewWidthFactor   = 2.0; break;
00601     case VA_ILLEGAL:
00602     default:         dViewWidthFactor   = 0.0; break;
00603   }
00604 
00605   switch( getAgentViewQuality() )
00606   {
00607     case VQ_LOW:     dViewQualityFactor = 0.5; break;
00608     case VQ_HIGH:    dViewQualityFactor = 1.0; break;
00609     case VQ_ILLEGAL:
00610     default:         dViewQualityFactor = 0.0; break;
00611   }
00612 
00613   return dViewQualityFactor*dViewWidthFactor;
00614 }
00615 
00620 VecPosition  WorldModel::getBallPos()
00621 {
00622   return getGlobalPosition( OBJECT_BALL );
00623 }
00624 
00627 double WorldModel::getBallSpeed()
00628 {
00629   return Ball.getGlobalVelocity().getMagnitude();
00630 }
00631 
00634 AngDeg WorldModel::getBallDirection()
00635 {
00636   return Ball.getGlobalVelocity().getDirection();
00637 }
00638 
00643 Time WorldModel::getTimeGlobalPosition( ObjectT o )
00644 {
00645   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00646   if( object != NULL )
00647     return object->getTimeGlobalPosition();
00648   return UnknownTime;
00649 }
00650 
00651 
00658 VecPosition WorldModel::getGlobalPosition( ObjectT o )
00659 {
00660   Object *object = getObjectPtrFromType( o );
00661   if( object != NULL )
00662   {
00663     if( SoccerTypes::isFlag( o ) || SoccerTypes::isGoal( o ) )
00664       return SoccerTypes::getGlobalPositionFlag(o,getSide(),SS->getGoalWidth());
00665     else
00666       return object->getGlobalPosition();
00667   }
00668   return VecPosition( UnknownDoubleValue, UnknownDoubleValue);
00669 }
00670 
00675 Time WorldModel::getTimeGlobalVelocity( ObjectT o )
00676 {
00677   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00678   if( object != NULL )
00679     return object->getTimeGlobalVelocity();
00680   return UnknownTime;
00681 }
00682 
00683 
00689 VecPosition WorldModel::getGlobalVelocity( ObjectT o )
00690 {
00691   DynamicObject *object = (DynamicObject*)getObjectPtrFromType( o );
00692   if( object != NULL )
00693     return object->getGlobalVelocity(  );
00694   return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
00695 
00696 }
00697 
00703 double WorldModel::getRelativeDistance( ObjectT o )
00704 {
00705   Object *object = getObjectPtrFromType( o );
00706   if( object != NULL )
00707     return object->getRelativeDistance();
00708   return UnknownDoubleValue;
00709 }
00710 
00716 VecPosition  WorldModel::getRelativePosition( ObjectT o )
00717 {
00718   Object *object = getObjectPtrFromType( o );
00719   if( object != NULL )
00720     return object->getRelativePosition();
00721   return VecPosition(UnknownDoubleValue, UnknownDoubleValue);
00722 }
00723 
00734 AngDeg WorldModel::getRelativeAngle( ObjectT o, bool bWithBody )
00735 {
00736   Object *object = getObjectPtrFromType( o );
00737   double dBody   = 0.0;
00738 
00739   if( object != NULL )
00740   {
00741     if( bWithBody == true )
00742       dBody = getAgentBodyAngleRelToNeck();
00743     return VecPosition::normalizeAngle( object->getRelativeAngle() - dBody );
00744   }
00745   return UnknownDoubleValue;
00746 }
00747 
00752 Time WorldModel::getTimeGlobalAngles( ObjectT o )
00753 {
00754   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00755   if( object != NULL )
00756     return object->getTimeGlobalAngles();
00757   return Time( -1, 0);
00758 }
00759 
00765 AngDeg WorldModel::getGlobalBodyAngle( ObjectT o )
00766 {
00767   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00768   if( object != NULL )
00769     return object->getGlobalBodyAngle();
00770   return UnknownAngleValue;
00771 }
00772 
00778 AngDeg WorldModel::getGlobalNeckAngle( ObjectT o )
00779 {
00780   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00781   if( object != NULL )
00782     return object->getGlobalNeckAngle();
00783   return UnknownAngleValue;
00784 }
00785 
00790 AngDeg WorldModel::getGlobalAngle( ObjectT o )
00791 {
00792   if( SoccerTypes::isLine( o ) )
00793     return SoccerTypes::getGlobalAngleLine( o, getSide() );
00794   return UnknownAngleValue;
00795 }
00796 
00797 
00803 double WorldModel::getConfidence( ObjectT o)
00804 {
00805   Object *object = getObjectPtrFromType( o );
00806   if( object != NULL )
00807     return object->getConfidence( getCurrentTime() );
00808   return 0.0;
00809 }
00810 
00818 bool WorldModel::isKnownPlayer( ObjectT o )
00819 {
00820   PlayerObject *object = (PlayerObject *)getObjectPtrFromType( o );
00821   if( object != NULL  )
00822     return object->getIsKnownPlayer();
00823   return false;
00824 
00825 }
00826 
00835 ObjectT WorldModel::getOppGoalieType()
00836 {
00837   ObjectT objOppMaxX = OBJECT_ILLEGAL;
00838   double  x = -100.0, y = UnknownDoubleValue;
00839 
00840   for( int i = 0; i < MAX_OPPONENTS; i++ )
00841   {
00842     if( isConfidenceGood( Opponents[i].getType( ) ) )
00843     {
00844       if( Opponents[i].getIsGoalie() == true )
00845         return Opponents[i].getType();
00846       if( Opponents[i].getGlobalPosition().getX() > x )
00847       {
00848         x          = Opponents[i].getGlobalPosition().getX();
00849         y          = Opponents[i].getGlobalPosition().getY();
00850         objOppMaxX = Opponents[i].getType();
00851       }
00852     }
00853   }
00854 
00855   // if opponent with highest x is nr 1, assume it is goalkeeper when standing
00856   // in own penalty area, otherwise assume goalkeeper closest player to goal.
00857   if(  objOppMaxX == OBJECT_OPPONENT_1 && x > PENALTY_X + 4.0 ||
00858       (objOppMaxX != OBJECT_ILLEGAL    && x > PITCH_LENGTH/2.0 - 6.0 &&
00859        fabs( y ) < SS->getGoalWidth()/2.0 ))
00860     return objOppMaxX;
00861   return OBJECT_ILLEGAL;
00862 }
00863 
00864 
00873 ObjectT WorldModel::getOwnGoalieType()
00874 {
00875   ObjectT objOwnMinX = OBJECT_ILLEGAL;
00876   double x = -100.0, y = UnknownDoubleValue;
00877   for( int i = 0; i < MAX_TEAMMATES; i++ )
00878   {
00879     if( isConfidenceGood( Teammates[i].getType( ) ) )
00880     {
00881       if( Teammates[i].getIsGoalie() == true )
00882         return Teammates[i].getType();
00883       if( Teammates[i].getGlobalPosition().getX() < x )
00884       {
00885         x          = Teammates[i].getGlobalPosition().getX();
00886         y          = Teammates[i].getGlobalPosition().getY();
00887         objOwnMinX = Teammates[i].getType();
00888       }
00889     }
00890   }
00891   if(  objOwnMinX == OBJECT_TEAMMATE_1 && x < - ( PENALTY_X + 4.0 ) ||
00892       (objOwnMinX != OBJECT_ILLEGAL    && x < - ( PITCH_LENGTH/2.0 - 6.0 ) &&
00893        fabs( y ) < SS->getGoalWidth()/2.0 ))
00894     return objOwnMinX;
00895   return OBJECT_ILLEGAL;
00896 }
00897 
00898 
00903 Time WorldModel::getTimeLastSeen( ObjectT o )
00904 {
00905   Object *object = getObjectPtrFromType( o );
00906   if( object != NULL )
00907     return object->getTimeLastSeen(  );
00908   return Time( -1, 0);
00909 
00910 }
00911 
00920 bool WorldModel::setIsKnownPlayer( ObjectT o, bool isKnownPlayer )
00921 {
00922   PlayerObject *object = (PlayerObject*) getObjectPtrFromType( o );
00923   return object->setIsKnownPlayer( isKnownPlayer );
00924 }
00925 
00928 VecPosition WorldModel::getPosOpponentGoal( )
00929 {
00930   return SoccerTypes::getGlobalPositionFlag(
00931                         SoccerTypes::getGoalOpponent( getSide() ),
00932                         getSide( ),
00933                         SS->getGoalWidth() );
00934 }
00935 
00938 VecPosition WorldModel::getPosOwnGoal( )
00939 {
00940   return SoccerTypes::getGlobalPositionFlag(
00941                         SoccerTypes::getOwnGoal( getSide() ),
00942                         getSide( ),
00943                         SS->getGoalWidth() );
00944 }
00945 
00948 double  WorldModel::getRelDistanceOpponentGoal()
00949 {
00950   VecPosition posGoal;
00951   if( sideSide == SIDE_LEFT )
00952     posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_R, sideSide );
00953   else
00954     posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_L, sideSide );
00955 
00956   return getAgentGlobalPosition().getDistanceTo( posGoal );
00957 
00958 }
00959 
00964 double  WorldModel::getRelAngleOpponentGoal()
00965 {
00966   VecPosition posGoal;
00967   if( sideSide == SIDE_LEFT )
00968     posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_R, sideSide );
00969   else
00970     posGoal = SoccerTypes::getGlobalPositionFlag( OBJECT_GOAL_L, sideSide );
00971 
00972   return ( posGoal - getAgentGlobalPosition()).getDirection() ;
00973 
00974 }
00975 
00979 HeteroPlayerSettings WorldModel::getInfoHeteroPlayer( int iIndex )
00980 {
00981   return pt[iIndex];
00982 }
00983 
00992 bool WorldModel::isQueuedActionPerformed()
00993 {
00994   // for all possible commands check if it is sent in previous cycle,
00995   // but not performed
00996   for( int i = 0 ; i < MAX_COMMANDS ; i++ )
00997     if( queuedCommands[i].time   == getTimeLastSenseMessage() - 1 &&
00998         performedCommands[i]     == false )
00999       return false;
01000 
01001 
01002   return true;
01003 }
01004 
01008 bool WorldModel::isFreeKickUs( )
01009 {
01010   return ( playMode == PM_FREE_KICK_LEFT  && sideSide == SIDE_LEFT  ) ||
01011          ( playMode == PM_FREE_KICK_RIGHT && sideSide == SIDE_RIGHT ) ;
01012 }
01013 
01017 bool WorldModel::isFreeKickThem( )
01018 {
01019   return ( playMode == PM_FREE_KICK_RIGHT  && sideSide == SIDE_LEFT  ) ||
01020          ( playMode == PM_FREE_KICK_LEFT   && sideSide == SIDE_RIGHT ) ;
01021 }
01022 
01028 bool WorldModel::isBeforeKickOff( )
01029 {
01030   return playMode == PM_BEFORE_KICK_OFF  || playMode == PM_GOAL_LEFT  ||
01031          playMode == PM_GOAL_RIGHT ;
01032 }
01033 
01038 bool WorldModel::isDeadBallUs( )
01039 {
01040   return isFreeKickUs() || isKickInUs() || isCornerKickUs() || isKickOffUs();
01041 }
01042 
01043 
01049 bool WorldModel::isDeadBallThem( )
01050 {
01051   return isFreeKickThem() || isKickInThem() || isCornerKickThem() ||
01052          isKickOffThem();
01053 }
01054 
01058 bool WorldModel::isCornerKickUs( )
01059 {
01060   return ( playMode == PM_CORNER_KICK_LEFT  && sideSide == SIDE_LEFT  ) ||
01061          ( playMode == PM_CORNER_KICK_RIGHT && sideSide == SIDE_RIGHT ) ;
01062 }
01063 
01067 bool WorldModel::isCornerKickThem( )
01068 {
01069   return ( playMode == PM_CORNER_KICK_RIGHT  && sideSide == SIDE_LEFT  ) ||
01070          ( playMode == PM_CORNER_KICK_LEFT   && sideSide == SIDE_RIGHT ) ;
01071 }
01072 
01076 bool WorldModel::isOffsideUs( )
01077 {
01078   return ( playMode == PM_OFFSIDE_RIGHT  && sideSide == SIDE_RIGHT ) ||
01079          ( playMode == PM_OFFSIDE_LEFT   && sideSide == SIDE_LEFT );
01080 }
01081 
01085 bool WorldModel::isOffsideThem( )
01086 {
01087   return ( playMode == PM_OFFSIDE_LEFT  && sideSide == SIDE_RIGHT ) ||
01088          ( playMode == PM_OFFSIDE_RIGHT && sideSide == SIDE_LEFT );
01089 }
01090 
01094 bool WorldModel::isKickInUs( )
01095 {
01096   return ( playMode == PM_KICK_IN_LEFT  && sideSide == SIDE_LEFT  ) ||
01097          ( playMode == PM_KICK_IN_RIGHT && sideSide == SIDE_RIGHT ) ;
01098 }
01099 
01103 bool WorldModel::isKickInThem( )
01104 {
01105   return ( playMode == PM_KICK_IN_RIGHT  && sideSide == SIDE_LEFT  ) ||
01106          ( playMode == PM_KICK_IN_LEFT   && sideSide == SIDE_RIGHT ) ;
01107 }
01108 
01112 bool WorldModel::isKickOffUs( )
01113 {
01114   return ( playMode == PM_KICK_OFF_LEFT  && sideSide == SIDE_LEFT  ) ||
01115          ( playMode == PM_KICK_OFF_RIGHT && sideSide == SIDE_RIGHT ) ;
01116 }
01117 
01121 bool WorldModel::isKickOffThem( )
01122 {
01123   return ( playMode == PM_KICK_OFF_RIGHT  && sideSide == SIDE_LEFT  ) ||
01124          ( playMode == PM_KICK_OFF_LEFT   && sideSide == SIDE_RIGHT ) ;
01125 }
01126 
01130 bool WorldModel::isGoalKickUs( )
01131 {
01132   return ( playMode == PM_GOAL_KICK_LEFT  && sideSide == SIDE_LEFT  ) ||
01133          ( playMode == PM_GOAL_KICK_RIGHT && sideSide == SIDE_RIGHT ) ;
01134 }
01135 
01139 bool WorldModel::isGoalKickThem( )
01140 {
01141   return ( playMode == PM_GOAL_KICK_RIGHT  && sideSide == SIDE_LEFT  ) ||
01142          ( playMode == PM_GOAL_KICK_LEFT   && sideSide == SIDE_RIGHT ) ;
01143 }
01144 
01145 
01150 void WorldModel::show( ostream & os )
01151 {
01152   int i;
01153   os << "Worldmodel (" << getCurrentTime() << ")\n" <<
01154         "========================\n";
01155   os << "Teamname: " << getTeamName() << endl;
01156   if( Ball.getTimeLastSeen( ).getTime() != -1 )
01157     Ball.show();
01158   os << "Teammates: " << endl;
01159   for( i = 0; i < MAX_TEAMMATES ; i++ )
01160     if( isConfidenceGood( Teammates[i].getType() ) )
01161       Teammates[i].show( getTeamName() );
01162   os << "Opponents: " << endl;
01163   for( i = 0; i < MAX_OPPONENTS ; i++ )
01164     if( isConfidenceGood( Opponents[i].getType() ) )
01165       Opponents[i].show( DEFAULT_OPPONENT_NAME );
01166   os << "Agent: " << endl;
01167   agentObject.show( getTeamName() );
01168 
01169   os << "General Info: " << endl <<
01170           "side: "      << SoccerTypes::getSideStr( getSide() )    << endl <<
01171           "kicks: "     << getNrOfCommands( CMD_KICK )             << endl <<
01172           "turns: "     << getNrOfCommands( CMD_TURN )             << endl <<
01173           "dashes: "    << getNrOfCommands( CMD_DASH )             << endl <<
01174           "turnnecks: " << getNrOfCommands( CMD_TURNNECK )         << endl <<
01175           "says: "      << getNrOfCommands( CMD_SAY )              << endl <<
01176           "playmode: "  << SoccerTypes::getPlayModeStr( playMode ) << endl <<
01177           "===================================="                   << endl;
01178 
01179 }
01180 
01185 void WorldModel::showObjects( ostream & os )
01186 {
01187   os << getCurrentTime() << endl << "b: ";
01188   if( Ball.getTimeLastSeen( ).getTime() != -1 )
01189     os << Ball.getGlobalPosition();
01190   os << endl << "T: ";
01191   for( int i = 0; i < MAX_TEAMMATES ; i++ )
01192     if( isConfidenceGood( Teammates[i].getType() ) )
01193     {
01194       os  << i+1 << " "  << Teammates[i].getGlobalPosition() <<
01195       " " << getConfidence( Teammates[i].getType() ) << ", " ;
01196     }
01197   os << endl << "O: " ;
01198   for( int i = 0; i < MAX_OPPONENTS ; i++ )
01199     if( isConfidenceGood( Opponents[i].getType() ) )
01200     {
01201       os << i+1 << " " << Opponents[i].getGlobalPosition() <<
01202       getConfidence( Opponents[i].getType() ) << " " ;
01203     }
01204   os << endl;
01205 }
01206 
01210 void WorldModel::showQueuedCommands( ostream & os )
01211 {
01212   os << "Commands in queue:\n" ;
01213   for( int i = 0; i < MAX_COMMANDS; i++ )
01214     if( queuedCommands[i].commandType != CMD_ILLEGAL )
01215       queuedCommands[i].show( os );
01216 }
01217 
01222 void WorldModel::show( ObjectT o, ostream &os )
01223 {
01224   Object *object = getObjectPtrFromType( o );
01225   if( object != NULL )
01226     object->show( os );
01227 }
01228 
01234 bool WorldModel::waitForNewInformation( )
01235 {
01236   bool bReturn = true;
01237   if( bNewInfo == false ) // there hasn't arrived any information yet
01238   {
01239     struct timeval now;
01240     struct timespec timeout;
01241     gettimeofday(&now, NULL);
01242     timeout.tv_sec = now.tv_sec + PS->getServerTimeOut();
01243     timeout.tv_nsec = now.tv_usec*1000;
01244 
01245     // lock mutex and wait till it is unlocked by Sense thread
01246     // this happens in setTimeLastSeeMessage, setTimeLastSenseMessage
01247     // or setTimeLastSeeGlobalMessage
01248     pthread_mutex_lock( &mutex_newInfo );
01249     int ret;
01250     while( (ret = pthread_cond_timedwait( &cond_newInfo,
01251                     &mutex_newInfo, &timeout) ) == EINTR )
01252      printf("(WorldModel::waitForNewInformation) failure in loop!!\n");
01253     if( ret == ETIMEDOUT ) // if no information was received but timer timed out
01254       bReturn = false;
01255     pthread_mutex_unlock( &mutex_newInfo );
01256   }
01257   // reset the indication of new visual information
01258   bNewInfo = false;
01259 
01260   return bReturn;
01261 }
01262 
01279 void WorldModel::logObjectInformation( int iLogLevel, ObjectT obj )
01280 {
01281   
01282   char   str[2048];
01283   double dConf     = PS->getPlayerConfThr();
01284   sprintf( str, "(%4d,%3d) %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f",
01285                                   getCurrentTime().getTime(),
01286                                   getCurrentTime().getTimeStopped(),
01287                                   getGlobalPosition(obj).getX(),
01288                                   getGlobalPosition(obj).getY(),
01289                                   getGlobalVelocity(obj).getX(),
01290                                   getGlobalVelocity(obj).getY(),
01291                                   getGlobalBodyAngle(obj),
01292                                   getGlobalNeckAngle(obj) );
01293 
01294   if( getConfidence      ( OBJECT_BALL ) > dConf &&
01295       getRelativeDistance( OBJECT_BALL ) < 20.0 )
01296     sprintf( &str[strlen(str)], " %12.6f %12.6f",
01297                                   getGlobalPosition(OBJECT_BALL).getX(),
01298                                   getGlobalPosition(OBJECT_BALL).getY() );
01299   else
01300     sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
01301 
01302   if( getTimeGlobalVelocity( OBJECT_BALL ) > getTimeFromConfidence( dConf ) &&
01303       getRelativeDistance  ( OBJECT_BALL ) < 20.0 )
01304     sprintf( &str[strlen(str)], " %12.6f %12.6f",
01305                                   getGlobalVelocity(OBJECT_BALL).getX(),
01306                                   getGlobalVelocity(OBJECT_BALL).getY() );
01307   else
01308     sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
01309   sprintf( &str[strlen(str)], " %12.6f", getConfidence(OBJECT_BALL) );
01310 
01311   int    iIndex=-1;
01312   int    iIndexPlayer;
01313   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS );
01314        o != OBJECT_ILLEGAL;
01315        o = iterateObjectNext( iIndex, OBJECT_SET_PLAYERS ) )
01316   {
01317     iIndexPlayer = (SoccerTypes::isTeammate(o))
01318                      ? SoccerTypes::getIndex(o) + 1
01319                      : SoccerTypes::getIndex(o) + 12;
01320     sprintf( &str[strlen(str)], " %d", iIndexPlayer );
01321     if( getConfidence      ( o ) > dConf && isKnownPlayer(o) &&
01322         getRelativeDistance( o ) < 20.0 )
01323       sprintf( &str[strlen(str)], " %12.6f %12.6f",
01324                                     getGlobalPosition(o).getX(),
01325                                     getGlobalPosition(o).getY() );
01326     else
01327        sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
01328 
01329     if( getTimeGlobalVelocity( o ) > getTimeFromConfidence( dConf ) &&
01330         getRelativeDistance  ( o ) < 20.0 && isKnownPlayer(o)  )
01331       sprintf( &str[strlen(str)], " %12.6f %12.6f",
01332                                     getGlobalVelocity(o).getX(),
01333                                     getGlobalVelocity(o).getY() );
01334     else
01335       sprintf( &str[strlen(str)], " %12.6f %12.6f", -10.0, -10.0 );
01336 
01337     if( isKnownPlayer(o) )
01338       sprintf( &str[strlen(str)], " %12.6f", getConfidence(o) );
01339     else
01340       sprintf( &str[strlen(str)], " %12.6f", -10.0 );
01341   }
01342   if( getCurrentCycle() != 3000  )
01343       Log.log( iLogLevel, str );
01344 }

Generated on Thu Mar 7 00:37:43 2002 for UvA Trilearn 2001 by doxygen1.2.12 written by Dimitri van Heesch, © 1997-2001