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
00043 #include<stdio.h>
00044 #include<strings.h>
00045 #include<errno.h>
00046 #include<pthread.h>
00047 #include<string.h>
00048 #include<math.h>
00049 #include"WorldModel.h"
00050
00051
00052
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 );
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
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 );
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
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;
00482 return o;
00483 }
00484
00485 }
00486 }
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
00856
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
00995
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 )
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
01246
01247
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 )
01254 bReturn = false;
01255 pthread_mutex_unlock( &mutex_newInfo );
01256 }
01257
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 }