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