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 #ifndef _WORLD_MODEL_
00048 #define _WORLD_MODEL_
00049
00050 #include "Objects.h"
00051 #include "PlayerSettings.h"
00052 #include "Logger.h"
00053 #include "Formations.h"
00054 #include <pthread.h>
00055
00056 extern Logger Log;
00057
00058
00059
00060
00061
00077 class WorldModel
00078 {
00079 private:
00080
00081
00082
00083
00084
00086
00087 ServerSettings *SS;
00088 PlayerSettings *PS;
00089 HeteroPlayerSettings pt[MAX_HETERO_PLAYERS];
00090 Formations *formations;
00092
00093
00094
00095 Time timeLastSeeMessage;
00096 Time timeLastSenseMessage;
00097 bool bNewInfo;
00098 Time timeLastCatch;
00099 Time timeLastRefMessage;
00101
00102 char strTeamName[MAX_TEAM_NAME_LENGTH];
00103 int iPlayerNumber;
00104 SideT sideSide;
00106
00107 PlayModeT playMode;
00108 int iGoalDiff;
00111
00112
00113
00114 BallObject Ball;
00115 AgentObject agentObject;
00116 PlayerObject Teammates[MAX_TEAMMATES];
00117 PlayerObject Opponents[MAX_OPPONENTS];
00118 PlayerObject UnknownPlayers[MAX_TEAMMATES+MAX_OPPONENTS];
00121 int iNrUnknownPlayers;
00123
00124 FixedObject Flags[MAX_FLAGS];
00125 FixedObject Lines[MAX_LINES];
00127
00128
00129 static const int iNrParticlesAgent = 100;
00131 static const int iNrParticlesBall = 100;
00133 VecPosition particlesPosAgent[iNrParticlesAgent];
00135 VecPosition particlesPosBall[iNrParticlesBall];
00137 VecPosition particlesVelBall[iNrParticlesBall];
00139
00140
00141
00142 SoccerCommand queuedCommands[CMD_MAX_COMMANDS];
00144 bool performedCommands[CMD_MAX_COMMANDS];
00147 int iCommandCounters[CMD_MAX_COMMANDS];
00151
00152
00153
00154 Time timeCheckBall;
00155 BallStatusT bsCheckBall;
00157
00158 pthread_mutex_t mutex_newInfo;
00159 pthread_cond_t cond_newInfo;
00161
00162 char m_strPlayerMsg[MAX_MSG];
00163 int m_iCycleInMsg;
00164 Time m_timePlayerMsg;
00165 int m_iMessageSender;
00167
00168 bool m_bWasCollision;
00169 bool m_bPerformedKick;
00171 public:
00172
00173
00174 int iNrHoles;
00175 int iNrOpponentsSeen;
00176 int iNrTeammatesSeen;
00178
00179 char strLastSeeMessage [MAX_MSG];
00180 char strLastSenseMessage[MAX_MSG];
00181 char strLastHearMessage [MAX_MSG];
00183
00184
00185
00186
00188
00189 private:
00190
00191 Object* getObjectPtrFromType ( ObjectT o );
00192
00193 public:
00194
00195
00196 void setTimeLastCatch ( Time time );
00197 int getTimeSinceLastCatch ( );
00198 bool setTimeLastRefereeMessage ( Time time );
00199 Time getTimeLastRefereeMessage ( );
00200 Time getCurrentTime ( );
00201 int getCurrentCycle ( );
00202 bool isTimeStopped ( );
00203 bool isLastMessageSee ( ) const;
00204 Time getTimeLastSeeGlobalMessage( ) const;
00205 bool setTimeLastSeeGlobalMessage( Time time );
00206 Time getTimeLastSeeMessage ( ) const;
00207 bool setTimeLastSeeMessage ( Time time );
00208 Time getTimeLastSenseMessage ( ) const ;
00209 bool setTimeLastSenseMessage ( Time time );
00210 int getPlayerNumber ( ) const;
00211 bool setPlayerNumber ( int i );
00212 SideT getSide ( ) const;
00213 bool setSide ( SideT s );
00214 const char* getTeamName ( ) const;
00215 bool setTeamName ( char *str );
00216 PlayModeT getPlayMode ( ) const;
00217 bool setPlayMode ( PlayModeT pm );
00218 int getGoalDiff ( ) const;
00219 int addOneToGoalDiff ( );
00220 int subtractOneFromGoalDiff ( );
00221 int getNrOfCommands ( CommandT c ) const;
00222 bool setNrOfCommands ( CommandT c,
00223 int i );
00224 Time getTimeCheckBall ( ) const;
00225 bool setTimeCheckBall ( Time time );
00226 BallStatusT getCheckBallStatus ( ) const;
00227 bool setCheckBallStatus ( BallStatusT bs );
00228
00229
00230 ObjectT iterateObjectStart ( int &iIndex,
00231 ObjectSetT g,
00232 double dConf = -1.0 );
00233 ObjectT iterateObjectNext ( int &iIndex,
00234 ObjectSetT g,
00235 double dConf = -1.0 );
00236 void iterateObjectDone ( int &iIndex );
00237
00238
00239 ObjectT getAgentObjectType ( ) const;
00240 bool setAgentObjectType ( ObjectT o );
00241 AngDeg getAgentBodyAngleRelToNeck ( ) const;
00242 AngDeg getAgentGlobalNeckAngle ( ) const;
00243 AngDeg getAgentGlobalBodyAngle ( );
00244 Stamina getAgentStamina ( ) const;
00245 TiredNessT getAgentTiredNess ( ) const;
00246 double getAgentEffort ( ) const;
00247 VecPosition getAgentGlobalVelocity ( ) const;
00248 double getAgentSpeed ( ) const;
00249 VecPosition getAgentGlobalPosition ( ) const;
00250 ViewAngleT getAgentViewAngle ( ) const;
00251 ViewQualityT getAgentViewQuality ( ) const;
00252 double getAgentViewFrequency ( ViewAngleT va = VA_ILLEGAL,
00253 ViewQualityT vq = VQ_ILLEGAL );
00254
00255
00256 VecPosition getBallPos ( );
00257 double getBallSpeed ( );
00258 AngDeg getBallDirection ( );
00259
00260
00261 Time getTimeGlobalPosition ( ObjectT o );
00262 VecPosition getGlobalPosition ( ObjectT o );
00263 Time getTimeGlobalVelocity ( ObjectT o );
00264 VecPosition getGlobalVelocity ( ObjectT o );
00265 double getRelativeDistance ( ObjectT o );
00266 VecPosition getRelativePosition ( ObjectT o );
00267 AngDeg getRelativeAngle ( ObjectT o,
00268 bool bWithBody = false );
00269 Time getTimeGlobalAngles ( ObjectT o );
00270 AngDeg getGlobalBodyAngle ( ObjectT o );
00271 AngDeg getGlobalNeckAngle ( ObjectT o );
00272 AngDeg getGlobalAngle ( ObjectT o );
00273 double getConfidence ( ObjectT o );
00274 bool isKnownPlayer ( ObjectT o );
00275 ObjectT getOppGoalieType ( );
00276 ObjectT getOwnGoalieType ( );
00277 Time getTimeLastSeen ( ObjectT o );
00278 Time getTimeChangeInformation ( ObjectT o );
00279
00280
00281 bool setIsKnownPlayer ( ObjectT o,
00282 bool isKnownPlayer );
00283 bool setTimeLastSeen ( ObjectT o,
00284 Time time );
00285
00286
00287
00288 VecPosition getPosOpponentGoal ( );
00289 VecPosition getPosOwnGoal ( );
00290 double getRelDistanceOpponentGoal ( );
00291 AngDeg getRelAngleOpponentGoal ( );
00292
00293
00294
00295 HeteroPlayerSettings getInfoHeteroPlayer ( int iIndex );
00296
00297
00298 bool isQueuedActionPerformed ( );
00299
00300
00301 bool isFreeKickUs ( PlayModeT pm = PM_ILLEGAL );
00302 bool isFreeKickThem ( PlayModeT pm = PM_ILLEGAL );
00303 bool isCornerKickUs ( PlayModeT pm = PM_ILLEGAL );
00304 bool isCornerKickThem ( PlayModeT pm = PM_ILLEGAL );
00305 bool isOffsideUs ( PlayModeT pm = PM_ILLEGAL );
00306 bool isOffsideThem ( PlayModeT pm = PM_ILLEGAL );
00307 bool isKickInUs ( PlayModeT pm = PM_ILLEGAL );
00308 bool isKickInThem ( PlayModeT pm = PM_ILLEGAL );
00309 bool isFreeKickFaultUs ( PlayModeT pm = PM_ILLEGAL );
00310 bool isFreeKickFaultThem ( PlayModeT pm = PM_ILLEGAL );
00311 bool isKickOffUs ( PlayModeT pm = PM_ILLEGAL );
00312 bool isKickOffThem ( PlayModeT pm = PM_ILLEGAL );
00313 bool isBackPassUs ( PlayModeT pm = PM_ILLEGAL );
00314 bool isBackPassThem ( PlayModeT pm = PM_ILLEGAL );
00315 bool isGoalKickUs ( PlayModeT pm = PM_ILLEGAL );
00316 bool isGoalKickThem ( PlayModeT pm = PM_ILLEGAL );
00317 bool isBeforeKickOff ( PlayModeT pm = PM_ILLEGAL );
00318 bool isDeadBallUs ( PlayModeT pm = PM_ILLEGAL );
00319 bool isDeadBallThem ( PlayModeT pm = PM_ILLEGAL );
00320
00322
00323
00324 void processSeeGlobalInfo ( ObjectT o,
00325 Time time,
00326 VecPosition pos,
00327 VecPosition vel,
00328 AngDeg angBody,
00329 AngDeg angNeck );
00330 bool processNewAgentInfo ( ViewQualityT vq,
00331 ViewAngleT va,
00332 double dStamina,
00333 double dEffort,
00334 double dSpeed,
00335 AngDeg angSpeed,
00336 AngDeg angHeadAngle );
00337 void processNewObjectInfo ( ObjectT o,
00338 Time time,
00339 double dDist,
00340 int iDir,
00341 double dDistChange,
00342 double dDirChange,
00343 AngDeg angRelBodyAng,
00344 AngDeg angRelNeckAng,
00345 bool isGoalie,
00346 ObjectT objMin,
00347 ObjectT objMax );
00348 bool processPerfectHearInfo ( ObjectT o,
00349 VecPosition pos,
00350 double dConf,
00351 bool bIsGoalie=0 );
00352 bool processPerfectHearInfoBall ( VecPosition pos,
00353 VecPosition vel,
00354 double dConf );
00355 bool processUnsureHearInfo ( ObjectT o,
00356 VecPosition pos,
00357 double dConf );
00358 bool processNewHeteroPlayer ( int iIndex,
00359 double dPlayerSpeedMax,
00360 double dStaminaIncMax,
00361 double dPlayerDecay,
00362 double dInertiaMoment,
00363 double dDashPowerRate,
00364 double dPlayerSize,
00365 double dKickableMargin,
00366 double dKickRand,
00367 double dExtraStamina,
00368 double dEffortMax,
00369 double dEffortMin );
00370 void processCatchedBall ( RefereeMessageT rm,
00371 Time time );
00372 void processQueuedCommands ( SoccerCommand commands[],
00373 int iCommands );
00374 bool storePlayerMessage ( int iPlayer,
00375 char *strMsg,
00376 int iCycle );
00377 bool processPlayerMessage ( );
00378
00379
00380 bool updateAll ( );
00381
00382 private:
00383
00384 void processLastSeeMessage ( );
00385 bool updateAfterSeeMessage ( );
00386 bool updateAgentObjectAfterSee ( );
00387 bool updateDynamicObjectAfterSee( ObjectT o );
00388
00389
00390 bool updateAfterSenseMessage ( );
00391 bool updateAgentAndBallAfterSense( );
00392 bool updateBallAfterKick ( double dPower,
00393 AngDeg ang );
00394 bool updateDynamicObjectForNextCycle( ObjectT o,
00395 int iCycle );
00396 bool updateBallForCollision ( VecPosition posAgent );
00397
00398
00399 bool updateRelativeFromGlobal ( );
00400 bool updateObjectRelativeFromGlobal( ObjectT o );
00401
00402
00403 bool calculateStateAgent ( VecPosition *posGlobal,
00404 VecPosition *velGlobal,
00405 AngDeg *angGlobal );
00406 void initParticlesAgent ( AngDeg angGlobal );
00407 void initParticlesAgent ( VecPosition posInitital );
00408 int checkParticlesAgent ( AngDeg angGlobalNeck );
00409 void updateParticlesAgent ( VecPosition vel,
00410 bool bAfterSense );
00411 VecPosition averageParticles ( VecPosition posArray[],
00412 int iLength );
00413 void resampleParticlesAgent ( int iLeft );
00414 bool calculateStateAgent2 ( VecPosition *posGlobal,
00415 VecPosition *velGlobal,
00416 AngDeg *angGlobal );
00417 VecPosition calculatePosAgentWith2Flags( ObjectT objFlag1,
00418 ObjectT objFlag2 );
00419 AngDeg calculateAngleAgentWithPos ( VecPosition pos );
00420
00421
00422
00423 bool calculateStateBall ( VecPosition *posGlobal,
00424 VecPosition *velGlobal );
00425 void initParticlesBall ( VecPosition posArray[],
00426 VecPosition velArray[],
00427 int iLength );
00428 void checkParticlesBall ( VecPosition posArray[],
00429 VecPosition velArray[],
00430 int iLength,
00431 int *iNrLeft );
00432 void updateParticlesBall ( VecPosition posArray[],
00433 VecPosition velArray[],
00434 int iLength,
00435 double dPower,
00436 AngDeg ang );
00437 void resampleParticlesBall ( VecPosition posArray[],
00438 VecPosition velArray[],
00439 int iLength,
00440 int iLeft );
00441 ObjectT getMaxRangeUnknownPlayer ( ObjectT obj,
00442 char* strMsg );
00443 VecPosition calculateVelocityDynamicObject( ObjectT o );
00444 bool calculateStateBall2 ( VecPosition *posGlobal,
00445 VecPosition *velGlobal );
00446
00447
00448
00449 bool calculateStatePlayer ( ObjectT o,
00450 VecPosition *posGlobal,
00451 VecPosition *velGlobal );
00452
00453
00454 bool getMinMaxDistQuantizeValue ( double dInput,
00455 double *dMin,
00456 double *dMax,
00457 double x1,
00458 double x2 );
00459 bool getMinMaxDirChange ( double dOutput,
00460 double *dMin,
00461 double *dMax,
00462 double x1 );
00463 bool getMinMaxDistChange ( double dOutput,
00464 double dDist,
00465 double *dMin,
00466 double *dMax,
00467 double x1,
00468 double xDist1,
00469 double xDist2 );
00470 double invQuantizeMin ( double dOutput,
00471 double dQuantizeStep );
00472 double invQuantizeMax ( double dOutput,
00473 double dQuantizeStep );
00474
00475
00476 public:
00477
00478 void mapUnknownPlayers ( Time time );
00479 bool updateSSToHeteroPlayerType ( int i );
00480 bool resetTimeObjects ( );
00481 void removeGhosts ( );
00482
00483
00485
00486
00487 bool predictStateAfterCommand ( SoccerCommand com,
00488 VecPosition *pos,
00489 VecPosition *vel,
00490 AngDeg *angGlobalBody,
00491 AngDeg *angGlobalNeck,
00492 Stamina *sta = NULL );
00493 bool predictAgentStateAfterCommand(SoccerCommand com,
00494 VecPosition *pos,
00495 VecPosition *vel,
00496 AngDeg *angBody,
00497 AngDeg *angNeck,
00498 Stamina *sta );
00499 VecPosition predictAgentPosAfterCommand( SoccerCommand com );
00500 void predictStateAfterDash ( double dActualPower,
00501 VecPosition *pos,
00502 VecPosition *vel,
00503 Stamina *sta,
00504 double dDirection );
00505 void predictStateAfterTurn ( AngDeg dSendAngle,
00506 VecPosition *pos,
00507 VecPosition *vel,
00508 AngDeg *angBody,
00509 AngDeg *angNeck,
00510 Stamina *sta = NULL );
00511 VecPosition predictPosAfterNrCycles ( ObjectT o,
00512 int iCycles,
00513 int iDashPower = 100,
00514 VecPosition *vel = NULL );
00515 VecPosition predictAgentPos ( int iCycles,
00516 int iDashPower = 0 );
00517 int predictNrCyclesToPoint ( ObjectT o,
00518 VecPosition posTo,
00519 AngDeg ang );
00520 int predictNrCyclesToObject ( ObjectT objFrom,
00521 ObjectT objTo );
00522 void predictStaminaAfterDash ( double dPower,
00523 Stamina *sta );
00524 bool isCollisionAfterDash ( SoccerCommand soc );
00525
00526
00528
00529
00530 int getNrInSetInRectangle ( ObjectSetT objectSet,
00531 Rectangle *rect = NULL );
00532 int getNrInSetInCircle ( ObjectSetT objectSet,
00533 Circle c );
00534 int getNrInSetInCone ( ObjectSetT objectSet,
00535 double dWidth,
00536 VecPosition start,
00537 VecPosition end );
00538
00539
00540 ObjectT getClosestInSetTo ( ObjectSetT objectSet,
00541 ObjectT o ,
00542 double *dDist = NULL,
00543 double dConfThr = -1.0 );
00544 ObjectT getClosestInSetTo ( ObjectSetT objectSet,
00545 VecPosition pos,
00546 double *dDist = NULL,
00547 double dConfThr = -1.0 );
00548 ObjectT getClosestInSetTo ( ObjectSetT objectSet,
00549 Line l,
00550 VecPosition pos1,
00551 VecPosition pos2,
00552 double *dDistToLine = NULL,
00553 double *dDistPos1ToP= NULL);
00554 ObjectT getClosestRelativeInSet ( ObjectSetT set,
00555 double *dDist = NULL );
00556 ObjectT getSecondClosestInSetTo ( ObjectSetT objectSet,
00557 ObjectT o,
00558 double *dDist = NULL,
00559 double dConfThr = -1.0 );
00560 ObjectT getSecondClosestRelativeInSet( ObjectSetT set,
00561 double *dDist = NULL );
00562 ObjectT getFastestInSetTo ( ObjectSetT objectSet,
00563 ObjectT o,
00564 int *iCycles = NULL );
00565 ObjectT getFastestInSetTo ( ObjectSetT objectSet,
00566 VecPosition pos,
00567 VecPosition vel,
00568 double dDecay,
00569 int *iCycles = NULL );
00570 ObjectT getFurthestInSetTo ( ObjectSetT objectSet,
00571 ObjectT o ,
00572 double *dDist = NULL,
00573 double dConfThr = -1.0 );
00574 ObjectT getFurthestRelativeInSet ( ObjectSetT set,
00575 double *dDist = NULL );
00576 double getMaxTraveledDistance ( ObjectT o );
00577
00578
00579 ObjectT getFirstEmptySpotInSet ( ObjectSetT objectSet,
00580 int iUnknownPlayer=-1 );
00581
00582
00583 bool isVisible ( ObjectT o );
00584 bool isBallKickable ( );
00585 bool isBallCatchable ( );
00586 bool isBallHeadingToGoal ( );
00587 bool isBallInOurPossesion ( );
00588 bool isBallInOwnPenaltyArea ( );
00589 bool isInOwnPenaltyArea ( VecPosition pos );
00590 bool isInTheirPenaltyArea ( VecPosition pos );
00591 bool isConfidenceGood ( ObjectT );
00592 bool isConfidenceVeryGood ( ObjectT );
00593 bool isOnside ( ObjectT );
00594 bool isOpponentAtAngle ( AngDeg ang,
00595 double dDist );
00596
00597
00598 Time getTimeFromConfidence ( double dConf );
00599 double getOffsideX ( bool bIncludeComm= true);
00600 VecPosition getOuterPositionInField ( VecPosition pos,
00601 AngDeg ang,
00602 double dDist = 3.0,
00603 bool bWithPenalty = true);
00604 AngDeg getDirectionOfWidestAngle ( VecPosition posOrg,
00605 AngDeg angMin,
00606 AngDeg angMax,
00607 AngDeg *ang,
00608 double dDist );
00609
00610 VecPosition getStrategicPosition ( int iPlayer = -1 );
00611
00612
00613 double getActualKickPowerRate ( );
00614 double getKickPowerForSpeed ( double dDesiredSpeed );
00615 double getKickSpeedToTravel ( double dDistance,
00616 double dEndSpeed );
00617 double getFirstSpeedFromEndSpeed ( double dEndSpeed,
00618 double dCycles );
00619 double getFirstSpeedFromDist ( double dDist,
00620 double dCycles );
00621 double getEndSpeedFromFirstSpeed ( double dFirstSpeed,
00622 double dCycles );
00623 AngDeg getAngleForTurn ( AngDeg angDesiredAngle,
00624 double dSpeed );
00625 AngDeg getActualTurnAngle ( AngDeg angActualAngle,
00626 double dSpeed );
00627 double getPowerForDash ( VecPosition posRelTo,
00628 AngDeg angBody,
00629 VecPosition vel,
00630 double dEffort );
00631
00632
00634
00635
00636 WorldModel ( ServerSettings *ss,
00637 PlayerSettings *ps,
00638 Formations *fs );
00639
00640
00641 void show ( ostream &os = cout );
00642 void show ( ObjectSetT set,
00643 ostream &os = cout );
00644 void showQueuedCommands ( ostream &os = cout );
00645 void show ( ObjectT o,
00646 ostream &os = cout );
00647
00648
00649 bool waitForNewInformation ( );
00650
00651
00652 void logObjectInformation ( int iLogLevel,
00653 ObjectT o );
00654 };
00655
00656 #endif