00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00047 #include<stdio.h>
00048 #include<strings.h>
00049 #include<errno.h>
00050 #include<pthread.h>
00051 #include<string.h>
00052 #include<math.h>
00053 #include"WorldModel.h"
00054
00055
00056
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 );
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
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 );
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
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
00478
00479
00480
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;
00498 return o;
00499 }
00500
00501 }
00502 }
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
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
00888
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
01051
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 ;
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 )
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
01463
01464
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 )
01473 bReturn = false;
01474 pthread_mutex_unlock( &mutex_newInfo );
01475 }
01476
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 }