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
00044 #include "WorldModel.h"
00045 #include <stdio.h>
00046
00047
00048
00049
00050
00051
00063 void WorldModel::processSeeGlobalInfo( ObjectT o, Time time,
00064 VecPosition pos, VecPosition vel, AngDeg angBody, AngDeg angNeck)
00065 {
00066 DynamicObject * dobj;
00067 PlayerObject * pobj;
00068
00069 if( o == OBJECT_ILLEGAL )
00070 return;
00071 if( SoccerTypes::isPlayer( o ) )
00072 {
00073 pobj = (PlayerObject*)getObjectPtrFromType( o );
00074 pobj->setTimeLastSeen( time );
00075 pobj->setGlobalPosition( pos, time );
00076 pobj->setGlobalVelocity( vel, time );
00077 pobj->setGlobalBodyAngle( angBody, time );
00078 pobj->setGlobalNeckAngle( VecPosition::normalizeAngle(angBody+angNeck),
00079 time );
00080 pobj->setIsKnownPlayer( true );
00081 }
00082 else if( SoccerTypes::isBall( o ) )
00083 {
00084 dobj = (DynamicObject*)getObjectPtrFromType( o );
00085 dobj->setTimeLastSeen( time );
00086 dobj->setGlobalPosition( pos, time );
00087 dobj->setGlobalVelocity( vel, time );
00088 }
00089 }
00090
00102 bool WorldModel::processNewAgentInfo( ViewQualityT vq, ViewAngleT va,
00103 double dStamina, double dEffort, double dSpeed, AngDeg angSpeed,
00104 AngDeg angHeadAngle )
00105 {
00106 Stamina sta = agentObject.getStamina();
00107
00108 sta.setStamina ( dStamina );
00109 sta.setEffort ( dEffort );
00110 agentObject.setStamina ( sta );
00111 agentObject.setViewQuality ( vq );
00112 agentObject.setViewAngle ( va );
00113 agentObject.setSpeedRelToNeck ( VecPosition( dSpeed, angSpeed, POLAR) );
00114 agentObject.setBodyAngleRelToNeck( angHeadAngle );
00115
00116 return true;
00117 }
00118
00138 void WorldModel::processNewObjectInfo( ObjectT o, Time time,
00139 double dDist, int iDir, double dDistChange, double dDirChange,
00140 AngDeg angRelBodyAng, AngDeg angRelNeckAng, bool isGoalie )
00141 {
00142 if( dDist == UnknownDoubleValue || o == OBJECT_ILLEGAL )
00143 return;
00144
00145 if( SoccerTypes::isFlag( o ) )
00146 {
00147 Flags[SoccerTypes::getIndex(o)].setRelativePosition(dDist,(double)iDir,time);
00148 Flags[SoccerTypes::getIndex(o)].setTimeLastSeen ( time );
00149 Flags[SoccerTypes::getIndex(o)].setType ( o );
00150 }
00151 else if( SoccerTypes::isPlayer( o ) || SoccerTypes::isBall( o ) )
00152 {
00153 DynamicObject *d = NULL;
00154
00155
00156 if( !( SoccerTypes::isKnownPlayer( o ) || SoccerTypes::isBall( o ) ) )
00157 {
00158 UnknownPlayers[iNrUnknownPlayers].setIsKnownPlayer( false );
00159 d = &UnknownPlayers[iNrUnknownPlayers];
00160 iNrUnknownPlayers++;
00161 }
00162 else
00163 {
00164 d = (DynamicObject*)getObjectPtrFromType( o );
00165 if( SoccerTypes::isPlayer( o ) )
00166 ((PlayerObject*)d)->setIsKnownPlayer( true );
00167 }
00168
00169 if( d != NULL )
00170 {
00171
00172 d->setRelativePosition( dDist, (double)iDir, time );
00173 if( dDistChange != UnknownDoubleValue )
00174 d->setRelativeDistanceChange( dDistChange, time );
00175 if( dDirChange != UnknownDoubleValue )
00176 d->setRelativeAngleChange( dDirChange, time );
00177 if( angRelBodyAng != UnknownAngleValue )
00178 ((PlayerObject*)d)->setRelativeBodyAngle( angRelBodyAng, time );
00179 if( angRelNeckAng != UnknownAngleValue )
00180 ((PlayerObject*)d)->setRelativeNeckAngle( angRelNeckAng, time );
00181 if( isGoalie == true && SoccerTypes::isPlayer( o ))
00182 ((PlayerObject*)d)->setIsGoalie( true );
00183 else if( SoccerTypes::isPlayer( o ))
00184 ((PlayerObject*)d)->setIsGoalie( false );
00185 d->setType( o );
00186 d->setTimeLastSeen( time );
00187 }
00188 }
00189 else if( SoccerTypes::isLine( o ) )
00190 {
00191
00192
00193 iDir = ( iDir < 0 ) ? (90 + iDir ) : - (90 - iDir );
00194 Lines[SoccerTypes::getIndex(o)].setRelativePosition(dDist,(double)iDir,time);
00195 Lines[SoccerTypes::getIndex(o)].setTimeLastSeen( time );
00196 Lines[SoccerTypes::getIndex(o)].setType( o );
00197 }
00198 }
00199
00207 bool WorldModel::processPerfectHearInfoBall( VecPosition posGlobal,
00208 VecPosition vel, double dConf )
00209 {
00210 if( Ball.getConfidence( getCurrentTime() ) < dConf )
00211 {
00212 Time time = getTimeFromConfidence( dConf );
00213 Ball.setGlobalPosition( posGlobal, time );
00214 Ball.setGlobalVelocity( vel, time );
00215 updateObjectRelativeFromGlobal( OBJECT_BALL );
00216 return true;
00217 }
00218 return false;
00219 }
00220
00232 bool WorldModel::processPerfectHearInfo( ObjectT o, VecPosition posGlobal,
00233 double dConf, bool bIsGoalie )
00234 {
00235 if( SoccerTypes::isBall( o ) || o == getAgentObjectType() )
00236 return false;
00237 else if( !SoccerTypes::isKnownPlayer( o ) )
00238 return processUnsureHearInfo( o, posGlobal, dConf );
00239
00240 PlayerObject *object = (PlayerObject *)getObjectPtrFromType( o );
00241 if( object == NULL )
00242 return false;
00243
00244 Time time = getTimeFromConfidence( dConf ) ;
00245
00246
00247
00248
00249
00250
00251 if( object->getConfidence( getCurrentTime() ) < dConf ||
00252 object->getIsKnownPlayer() == false )
00253 {
00254 object->setGlobalPosition ( posGlobal , time );
00255 object->setTimeLastSeen ( time );
00256 object->setGlobalVelocity ( VecPosition( 0, 0), time );
00257 object->setIsKnownPlayer ( true );
00258 object->setIsGoalie ( bIsGoalie );
00259 updateObjectRelativeFromGlobal( o );
00260 return true;
00261 }
00262 return false;
00263 }
00264
00276 bool WorldModel::processUnsureHearInfo( ObjectT o, VecPosition pos,
00277 double dConf )
00278 {
00279 double dMinDist;
00280 ObjectT objInitial = o;
00281
00282 if( o != OBJECT_TEAMMATE_UNKNOWN && o != OBJECT_OPPONENT_UNKNOWN )
00283 return false;
00284
00285
00286 if( SoccerTypes::isTeammate( o ) )
00287 o = getClosestInSetTo( OBJECT_SET_TEAMMATES, pos, &dMinDist);
00288 else if( SoccerTypes::isOpponent( o ) )
00289 o = getClosestInSetTo( OBJECT_SET_OPPONENTS, pos, &dMinDist);
00290
00291 if( o == getAgentObjectType() &&
00292 pos.getDistanceTo(getAgentGlobalPosition())<PS->getPlayerDistTolerance())
00293 return false;
00294
00295
00296
00297
00298
00299 else if( SoccerTypes::isKnownPlayer(o) &&
00300 dMinDist < PS->getPlayerDistTolerance())
00301 {
00302 processPerfectHearInfo( o, pos, dConf );
00303 return true;
00304 }
00305
00306 if( objInitial == OBJECT_TEAMMATE_UNKNOWN )
00307 o = getFirstEmptySpotInSet( OBJECT_SET_TEAMMATES );
00308 else if( objInitial == OBJECT_OPPONENT_UNKNOWN )
00309 o = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
00310 else
00311 return false ;
00312
00313 if( o != OBJECT_ILLEGAL )
00314 {
00315 processPerfectHearInfo( o, pos, dConf );
00316 setIsKnownPlayer( o, false );
00317 }
00318 return true;
00319 }
00320
00341 bool WorldModel::processNewHeteroPlayer( int iIndex, double dPlayerSpeedMax,
00342 double dStaminaIncMax, double dPlayerDecay, double dInertiaMoment,
00343 double dDashPowerRate, double dPlayerSize, double dKickableMargin,
00344 double dKickRand, double dExtraStamina, double dEffortMax,
00345 double dEffortMin )
00346 {
00347 pt[iIndex].dPlayerSpeedMax = dPlayerSpeedMax;
00348 pt[iIndex].dStaminaIncMax = dStaminaIncMax;
00349 pt[iIndex].dPlayerDecay = dPlayerDecay;
00350 pt[iIndex].dInertiaMoment = dInertiaMoment;
00351 pt[iIndex].dDashPowerRate = dDashPowerRate;
00352 pt[iIndex].dPlayerSize = dPlayerSize;
00353 pt[iIndex].dKickableMargin = dKickableMargin;
00354 pt[iIndex].dKickRand = dKickRand;
00355 pt[iIndex].dExtraStamina = dExtraStamina;
00356 pt[iIndex].dEffortMax = dEffortMax;
00357 pt[iIndex].dEffortMin = dEffortMin;
00358 return true;
00359 }
00360
00369 void WorldModel::processCatchedBall( RefereeMessageT rm, Time time )
00370 {
00371 if( rm == REFC_GOALIE_CATCH_BALL_LEFT && sideSide == SIDE_LEFT )
00372 timeLastCatch = time;
00373 else if( rm == REFC_GOALIE_CATCH_BALL_RIGHT && sideSide == SIDE_RIGHT )
00374 timeLastCatch = time;
00375 Ball.setGlobalVelocity( VecPosition(0,0), getCurrentTime() );
00376 }
00377
00386 void WorldModel::processQueuedCommands( SoccerCommand commands[],int iCommands )
00387 {
00388 if( iCommands > MAX_COMMANDS )
00389 {
00390 cerr << "(WorldModel::setQueuedCommands) queuedCommands array cannot "
00391 << "contain so many commands!\n";
00392 return;
00393 }
00394
00395
00396 for( int i = 0 ; i < iCommands ; i ++ )
00397 {
00398 commands[i].time = getCurrentTime();
00399 queuedCommands[(int)commands[i].commandType] = commands[i];
00400 }
00401 }
00402
00410 bool WorldModel::updateAll( )
00411 {
00412 bool bReturn = false, bUpdateAfterSee = false;
00413 bool bUpdateAfterSense = false;
00414 static Time timeBeginInterval;
00415 static Time timePlayersCounted;
00416 static int iNrHolesLastTime = 0;
00417 static Time timeLastSenseUpdate;
00418 static Time timeLastSeeUpdate;
00419
00420
00421 if( agentObject.getTimeGlobalPosition() < getCurrentTime() - 1 )
00422 Log.log( 3, "(WorldModel::updateAfterSenseMessage) missed a sense??" );
00423
00424
00425 if( getTimeLastSeeMessage() > timeLastSeeUpdate )
00426 bUpdateAfterSee = true;
00427 if( getTimeLastSenseMessage() > timeLastSenseUpdate )
00428 bUpdateAfterSense = true;
00429
00430
00431
00432 if( bUpdateAfterSee && bUpdateAfterSense )
00433 {
00434 Log.log( 3, "!!! Two updates !!! " );
00435 Log.log( 3, "see: %d sense: %d", getTimeLastSeeMessage().getTime(),
00436 getTimeLastSenseMessage().getTime() );
00437 if( getTimeLastSeeMessage( ) == getTimeLastSenseMessage() )
00438 {
00439 bReturn = updateAfterSenseMessage( );
00440 bReturn &= updateAfterSeeMessage( );
00441 }
00442 else if( getTimeLastSeeMessage( ) < getTimeLastSenseMessage() )
00443 {
00444 bReturn = updateAfterSeeMessage( );
00445 bReturn &= updateAfterSenseMessage( );
00446 updateRelativeFromGlobal();
00447 }
00448
00449 timeLastSenseUpdate = getTimeLastSenseMessage();
00450 timeLastSeeUpdate = getTimeLastSeeMessage();
00451 }
00452 else if( bUpdateAfterSee )
00453 {
00454 bReturn = updateAfterSeeMessage( );
00455 timeLastSeeUpdate = getTimeLastSeeMessage();
00456 }
00457 else if( bUpdateAfterSense )
00458 {
00459 bReturn = updateAfterSenseMessage( );
00460 timeLastSenseUpdate = getTimeLastSenseMessage();
00461 updateRelativeFromGlobal();
00462 }
00463
00464
00465 int iTimeDiff = getCurrentTime() - timeBeginInterval;
00466 double dHolePerc = (double)(iNrHoles - iNrHolesLastTime)/iTimeDiff*100;
00467 if( ! isLastMessageSee( ) && iTimeDiff % 400 == 0 && dHolePerc > 1.0 &&
00468 PS->getFractionWaitSeeEnd() > 0.70 )
00469 {
00470 PS->setFractionWaitSeeEnd( PS->getFractionWaitSeeEnd() - 0.05 );
00471 timeBeginInterval = getCurrentTime();
00472 cerr << "Lowered send time to " << PS->getFractionWaitSeeEnd() <<
00473 " for player number " << getPlayerNumber() <<
00474 "; nr of holes is "<< dHolePerc << "%" << endl;
00475 iNrHolesLastTime = iNrHoles;
00476 }
00477
00478
00479
00480 if( bUpdateAfterSee == true && ! isTimeStopped() &&
00481 getCurrentTime() != timePlayersCounted )
00482 {
00483 iNrTeammatesSeen += getNrInSetInRectangle( OBJECT_SET_TEAMMATES );
00484 iNrOpponentsSeen += getNrInSetInRectangle( OBJECT_SET_OPPONENTS );
00485 timePlayersCounted = getCurrentTime();
00486 }
00487
00488
00489 if( Log.isInLogLevel( 456 ) )
00490 logObjectInformation( 456, getAgentObjectType() );
00491
00492 if( bUpdateAfterSee == true )
00493 Log.logWithTime( 3, " finished update_all see; start determining action" );
00494 if( bUpdateAfterSense == true )
00495 Log.logWithTime( 3, " finished update_all sense; start determining action" );
00496 Log.log( 3, "global pos: (%f, %f)", getAgentGlobalPosition().getX(),
00497 getAgentGlobalPosition().getY() );
00498 return bReturn;
00499
00500 }
00501
00502
00503
00504
00505
00511 bool WorldModel::updateAfterSeeMessage( )
00512 {
00513
00514 if( getCurrentTime().getTime() != -1 )
00515 updateAgentObjectAfterSee( );
00516
00517
00518
00519
00520
00521 double dConfThr = PS->getPlayerConfThr();
00522 int iIndex;
00523
00524 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
00525 o != OBJECT_ILLEGAL;
00526 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
00527 {
00528 if( getTimeLastSeen( o ) == getTimeLastSeeMessage() &&
00529 o != getAgentObjectType() )
00530 updateDynamicObjectAfterSee ( o );
00531 else
00532 updateObjectRelativeFromGlobal( o );
00533 }
00534 iterateObjectDone( iIndex);
00535
00536
00537 if( getTimeLastSeen( OBJECT_BALL ) == getTimeLastSeeMessage() )
00538 updateDynamicObjectAfterSee ( OBJECT_BALL );
00539 else
00540 updateObjectRelativeFromGlobal( OBJECT_BALL );
00541
00542
00543 removeGhosts();
00544
00545 return true;
00546 }
00547
00552 bool WorldModel::updateAgentObjectAfterSee( )
00553 {
00554 VecPosition posGlobal, velGlobal;
00555 AngDeg angGlobal;
00556
00557
00558 calculateStateAgent( &posGlobal, &velGlobal, &angGlobal );
00559
00560
00561 agentObject.setTimeLastSeen ( getTimeLastSeeMessage() );
00562
00563
00564 agentObject.setPositionDifference(posGlobal- agentObject.getGlobalPosition());
00565 agentObject.setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
00566 agentObject.setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00567 agentObject.setGlobalNeckAngle ( angGlobal );
00568 agentObject.setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
00569
00570 return true;
00571 }
00572
00573
00579 bool WorldModel::updateDynamicObjectAfterSee( ObjectT o )
00580 {
00581 VecPosition posGlobal, velGlobal;
00582
00583 if( o == OBJECT_BALL )
00584 {
00585 calculateStateBall ( &posGlobal, &velGlobal );
00586 Ball.setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
00587 Ball.setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
00588 Ball.setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00589 return true;
00590 }
00591 else if( SoccerTypes::isKnownPlayer( o ) )
00592 {
00593 calculateStatePlayer( o, &posGlobal, &velGlobal );
00594
00595 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
00596
00597 pob->setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
00598 pob->setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
00599 pob->setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00600
00601 if( pob->getTimeRelativeAngles() == getTimeLastSeeMessage() )
00602 {
00603 AngDeg ang = getAgentGlobalNeckAngle()+pob->getRelativeBodyAngle();
00604 ang = VecPosition::normalizeAngle( ang );
00605 pob->setGlobalBodyAngle( ang, getTimeLastSeeMessage() );
00606 ang = getAgentGlobalNeckAngle() + pob->getRelativeNeckAngle();
00607 ang = VecPosition::normalizeAngle( ang );
00608 pob->setGlobalNeckAngle( ang, getTimeLastSeeMessage() );
00609 }
00610 return true;
00611 }
00612
00613 return false;
00614 }
00615
00616
00617
00618
00619
00620
00629 bool WorldModel::updateAfterSenseMessage( )
00630 {
00631
00632 updateAgentAndBallAfterSense( );
00633
00634
00635 double dConfThr = PS->getPlayerConfThr();
00636 int iIndex;
00637
00638 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
00639 o != OBJECT_ILLEGAL;
00640 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
00641 {
00642 if( o != getAgentObjectType() )
00643 updateDynamicObjectForNextCycle( o,
00644 getCurrentTime() - getTimeGlobalPosition(o) );
00645 }
00646 iterateObjectDone( iIndex);
00647
00648
00649 updateBallForCollision();
00650
00651 return true;
00652 }
00653
00659 bool WorldModel::updateAgentAndBallAfterSense( )
00660 {
00661
00662 bool bProcessedHole = false;
00663 bool bBallUpdated = false;
00664 VecPosition pos = getAgentGlobalPosition();
00665 AngDeg angGlobalNeck = getAgentGlobalNeckAngle();
00666 AngDeg angGlobalBody = getAgentGlobalBodyAngle();
00667 Stamina sta = getAgentStamina();
00668 VecPosition vel = agentObject.getSpeedRelToNeck();
00669 Time time = getCurrentTime() - 1 ;
00670
00671
00672
00673
00674
00675
00676
00677 vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
00678
00679
00680 int i = (int)CMD_TURNNECK;
00681 if( performedCommands[i] == true &&
00682 ( queuedCommands[i].time.getTime() == time.getTime() ||
00683 queuedCommands[i].time.getTime() == time.getTime() - 1 ) )
00684 {
00685
00686 if( queuedCommands[i].time.getTime() == time.getTime() - 1 &&
00687 time.getTime() > 0)
00688 {
00689 bProcessedHole = true;
00690 iNrHoles++;
00691 }
00692
00693 predictStateAfterCommand( queuedCommands[i], &pos, &vel, &angGlobalBody,
00694 &angGlobalNeck, &sta ) ;
00695 queuedCommands[i].time.updateTime( -1 );
00696 }
00697
00698
00699
00700 for( i = 0; i < MAX_COMMANDS; i ++ )
00701 {
00702 if( performedCommands[i] == true &&
00703 ( queuedCommands[i].time.getTime() == time.getTime() ||
00704 queuedCommands[i].time.getTime() == time.getTime() - 1 ) )
00705 {
00706 if( queuedCommands[i].time.getTime() == time.getTime() - 1 &&
00707 bProcessedHole == false && time.getTime() > 0 )
00708 {
00709 bProcessedHole = true;
00710 iNrHoles++;
00711 }
00712
00713 switch( queuedCommands[i].commandType )
00714 {
00715 case CMD_KICK:
00716 updateBallAfterKick( queuedCommands[i].dPower,
00717 queuedCommands[i].dAngle);
00718 bBallUpdated = true;
00719 break;
00720 case CMD_TURN:
00721 predictStateAfterCommand( queuedCommands[i], &pos, &vel,
00722 &angGlobalBody, &angGlobalNeck, &sta );
00723 break;
00724 case CMD_MOVE:
00725 pos.setVecPosition( queuedCommands[i].dX, queuedCommands[i].dY );
00726 initParticlesAgent( pos );
00727 break;
00728 case CMD_DASH:
00729
00730
00731 break;
00732 case CMD_TURNNECK:
00733 default:
00734 break;
00735 }
00736 queuedCommands[i].time.updateTime( -1 );
00737 }
00738 }
00739
00740
00741
00742
00743 vel = agentObject.getSpeedRelToNeck();
00744 pos = getAgentGlobalPosition();
00745 vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
00746 vel.rotate( angGlobalNeck );
00747
00748
00749 updateParticlesAgent( vel, true );
00750
00751
00752
00753
00754 predictStateAfterDash( 0.0, &pos, &vel, &sta, 0 );
00755
00756
00757 if( !bBallUpdated )
00758 {
00759 updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
00760 updateParticlesBall( particlesPosBall, particlesVelBall, iNrParticlesBall, 0, 0 );
00761 }
00762
00763
00764
00765 agentObject.setGlobalPosition ( pos, getCurrentTime() );
00766 agentObject.setGlobalVelocity ( vel, getCurrentTime() );
00767 agentObject.setGlobalNeckAngle( angGlobalNeck );
00768
00769
00770 return true;
00771 }
00772
00781 bool WorldModel::updateBallAfterKick( double dPower, AngDeg ang )
00782 {
00783 if( getRelativeDistance( OBJECT_BALL ) < SS->getMaximalKickDist() )
00784 {
00785
00786
00787 ang = VecPosition::normalizeAngle(ang + getAgentGlobalBodyAngle() );
00788 VecPosition acc( getActualKickPowerRate()*dPower, ang, POLAR ) ;
00789 acc += getGlobalVelocity( OBJECT_BALL );
00790 if( acc.getMagnitude() > SS->getBallSpeedMax() )
00791 acc.setMagnitude( SS->getBallSpeedMax() );
00792 Ball.setGlobalPosition( getGlobalPosition( OBJECT_BALL ) + acc,
00793 getCurrentTime() );
00794 Ball.setGlobalVelocity( acc*SS->getBallDecay(), getCurrentTime() );
00795 updateParticlesBall( particlesPosBall, particlesVelBall, iNrParticlesBall, dPower, ang );
00796 }
00797 else
00798 {
00799 updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
00800 updateParticlesBall( particlesPosBall, particlesVelBall, iNrParticlesBall, 0, 0 );
00801 Log.log( 21, "(WorldModel::%s) KICK command, but ball not kickable (%f)",
00802 __FUNCTION__, getRelativeDistance( OBJECT_BALL ) );
00803 }
00804 return true;
00805 }
00806
00813 bool WorldModel::updateDynamicObjectForNextCycle( ObjectT obj, int iCycles)
00814 {
00815 DynamicObject *o = (DynamicObject*) getObjectPtrFromType( obj );
00816 if( o == NULL )
00817 return false;
00818
00819
00820 VecPosition vel = getGlobalVelocity( obj );
00821 VecPosition pos = getGlobalPosition( obj );
00822
00823 for( int i = 0; i < iCycles ; i++ )
00824 {
00825 pos += vel;
00826 vel *= (obj==OBJECT_BALL) ? SS->getBallDecay() : SS->getPlayerDecay();
00827 }
00828 o->setGlobalVelocity ( vel, getCurrentTime() );
00829 o->setGlobalPosition ( pos, getCurrentTime() );
00830
00831 return true;
00832 }
00833
00834
00839 bool WorldModel::updateBallForCollision()
00840 {
00841 VecPosition posBall = getGlobalPosition( OBJECT_BALL );
00842 VecPosition velBall = getGlobalVelocity( OBJECT_BALL );
00843 double dist = SS->getPlayerSize() + SS->getBallSize();
00844
00845
00846
00847
00848 if( getAgentGlobalPosition().getDistanceTo( posBall ) < dist )
00849 {
00850 Log.log( 552, "Collision occured" );
00851 posBall -= (velBall/SS->getBallDecay());
00852 Ball.setGlobalPosition( posBall, getCurrentTime() );
00853 Ball.setGlobalVelocity( velBall*-0.1, getCurrentTime() );
00854 return true;
00855 }
00856
00857 return false;
00858
00859 }
00860
00866 bool WorldModel::updateRelativeFromGlobal()
00867 {
00868
00869 double dConfThr = PS->getPlayerConfThr();
00870 int iIndex;
00871
00872
00873 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
00874 o != OBJECT_ILLEGAL;
00875 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
00876 {
00877 if( o != getAgentObjectType() )
00878 updateObjectRelativeFromGlobal( o );
00879 }
00880 iterateObjectDone( iIndex);
00881
00882
00883 if( isConfidenceGood( OBJECT_BALL ) )
00884 updateObjectRelativeFromGlobal( OBJECT_BALL );
00885
00886
00887
00888 return true;
00889 }
00890
00896 bool WorldModel::updateObjectRelativeFromGlobal( ObjectT o )
00897 {
00898 Object *obj = (Object*) getObjectPtrFromType( o );
00899 if( obj == NULL )
00900 return false;
00901
00902
00903 VecPosition rel = obj->getGlobalPosition();
00904 rel.globalToRelative( getAgentGlobalPosition(), getAgentGlobalNeckAngle() );
00905 obj->setRelativePosition( rel, obj->getTimeGlobalPosition() );
00906 return true;
00907 }
00908
00917 bool WorldModel::calculateStateAgent( VecPosition *posGlobal,
00918 VecPosition *velGlobal, AngDeg *angGlobal )
00919 {
00920 int iNrLeft;
00921
00922
00923 ObjectT objLine = getFurthestRelativeInSet( OBJECT_SET_LINES );
00924 if( objLine != OBJECT_ILLEGAL )
00925 {
00926 double angGlobalLine = getGlobalAngle ( objLine );
00927 AngDeg angRelLine = getRelativeAngle( objLine );
00928 *angGlobal = angGlobalLine - angRelLine;
00929 *angGlobal = VecPosition::normalizeAngle( *angGlobal );
00930 }
00931 else
00932 {
00933 Log.log( 21, "(WorldModel::%s) no line in last see message", __FUNCTION__ );
00934 *angGlobal = getAgentGlobalNeckAngle();
00935 }
00936
00937
00938
00939
00940
00941
00942
00943
00944 *velGlobal = agentObject.getSpeedRelToNeck().rotate( *angGlobal );
00945 velGlobal->setMagnitude( velGlobal->getMagnitude()/SS->getPlayerDecay() );
00946
00947 updateParticlesAgent ( *velGlobal, false );
00948 iNrLeft = checkParticlesAgent ( *angGlobal );
00949
00950 if( iNrLeft == 0 )
00951 {
00952
00953
00954 initParticlesAgent ( *angGlobal );
00955 iNrLeft = checkParticlesAgent( *angGlobal );
00956 if( iNrLeft == 0 )
00957 {
00958
00959
00960 calculateStateAgent2( posGlobal, velGlobal, angGlobal );
00961 initParticlesAgent( *posGlobal );
00962 return false;
00963 }
00964 }
00965
00966
00967
00968 *posGlobal = averageParticles( particlesPosAgent, iNrLeft );
00969 resampleParticlesAgent( iNrLeft );
00970
00971
00972
00973 AngDeg ang = calculateAngleAgentWithPos( *posGlobal );
00974 if( ang != UnknownAngleValue )
00975 *angGlobal = ang;
00976
00977 *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
00978 return true;
00979 }
00980
00988 void WorldModel::initParticlesAgent( AngDeg angGlobal )
00989 {
00990 double dDist, dMaxRadius, dMinRadius, dInput;
00991 AngDeg ang;
00992
00993
00994 ObjectT objFlag = getClosestRelativeInSet( OBJECT_SET_FLAGS );
00995
00996 if( objFlag == OBJECT_ILLEGAL )
00997 {
00998 Log.log( 21, "(WorldModel::%s) no flag in last see message", __FUNCTION__ );
00999 return;
01000 }
01001
01002
01003 dInput = getRelativeDistance( objFlag );
01004 getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01005 SS->getQuantizeStepL(), 0.1 ) ;
01006
01007
01008
01009 AngDeg angFlag = getRelativeAngle( objFlag ) + 180 + angGlobal ;
01010
01011
01012 for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01013 {
01014
01015
01016
01017
01018
01019 dDist = dMinRadius + drand48()*(dMaxRadius-dMinRadius);
01020 ang = VecPosition::normalizeAngle( angFlag - 1.0 + 2*drand48() );
01021
01022
01023 particlesPosAgent[i] = getGlobalPosition( objFlag ) +
01024 VecPosition( dDist, ang, POLAR );
01025 }
01026 }
01027
01033 void WorldModel::initParticlesAgent( VecPosition posInitial )
01034 {
01035 for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01036 particlesPosAgent[i] = posInitial;
01037 }
01038
01047 int WorldModel::checkParticlesAgent( AngDeg angGlobalNeck )
01048 {
01049 double dMaxRadius, dMinRadius, dInput, dDist;
01050 AngDeg ang;
01051 int iIndex, iNrLeft = 0, iLength = iNrParticlesAgent;
01052
01053
01054 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_FLAGS, 1.0 );
01055 o != OBJECT_ILLEGAL;
01056 o = iterateObjectNext ( iIndex, OBJECT_SET_FLAGS, 1.0 ) )
01057 {
01058 iNrLeft = 0;
01059 dInput = getRelativeDistance( o );
01060 getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01061 SS->getQuantizeStepL(), 0.1 ) ;
01062
01063
01064 for( int i = 0; i < iLength; i ++ )
01065 {
01066
01067
01068
01069 dDist = particlesPosAgent[i].getDistanceTo( getGlobalPosition( o ) );
01070 ang = (getGlobalPosition(o) - particlesPosAgent[i]).getDirection();
01071 ang = ang - ( getRelativeAngle( o ) + angGlobalNeck );
01072
01073
01074
01075 if( dDist > dMinRadius && dDist < dMaxRadius &&
01076 fabs(VecPosition::normalizeAngle( ang )) <= 1.0 )
01077 particlesPosAgent[iNrLeft++] = particlesPosAgent[i];
01078 }
01079
01080 iLength = iNrLeft;
01081
01082 }
01083 return iNrLeft;
01084 }
01085
01096 void WorldModel::updateParticlesAgent( VecPosition vel, bool bAfterSense )
01097 {
01098
01099 static VecPosition prev_vel;
01100
01101 for( int i = 0; i < iNrParticlesAgent ; i ++ )
01102 {
01103 if( bAfterSense == false )
01104 {
01105 particlesPosAgent[i].setX( particlesPosAgent[i].getX() - prev_vel.getX());
01106 particlesPosAgent[i].setY( particlesPosAgent[i].getY() - prev_vel.getY());
01107 }
01108
01109 particlesPosAgent[i].setX( particlesPosAgent[i].getX( ) + vel.getX() );
01110 particlesPosAgent[i].setY( particlesPosAgent[i].getY( ) + vel.getY() );
01111 }
01112 prev_vel = vel;
01113 }
01114
01115
01120 VecPosition WorldModel::averageParticles( VecPosition posArray[], int iLength )
01121 {
01122 if( iLength == 0 )
01123 return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
01124
01125
01126 double x = 0, y = 0;
01127 for( int i = 0; i < iLength ; i ++ )
01128 {
01129 x += posArray[ i ].getX( );
01130 y += posArray[ i ].getY( );
01131 }
01132
01133 return VecPosition( x/iLength, y/iLength );
01134 }
01135
01142 void WorldModel::resampleParticlesAgent( int iLeft )
01143 {
01144 for ( int i = iLeft; i < iNrParticlesAgent; i ++ )
01145 particlesPosAgent[ i ] = particlesPosAgent[ (int)(drand48()*iLeft) ];
01146 }
01147
01159 VecPosition WorldModel::calculatePosAgentWith2Flags( ObjectT objFlag1,
01160 ObjectT objFlag2 )
01161 {
01162 double xA, yA, xB, yB, rA, rB;
01163 AngDeg aA, aB;
01164
01165
01166 xA = getGlobalPosition ( objFlag1 ).getX();
01167 yA = getGlobalPosition ( objFlag1 ).getY();
01168 rA = getRelativeDistance( objFlag1 );
01169 aA = getRelativeAngle ( objFlag1 );
01170 xB = getGlobalPosition ( objFlag2 ).getX();
01171 yB = getGlobalPosition ( objFlag2 ).getY();
01172 rB = getRelativeDistance( objFlag2 );
01173 aB = getRelativeAngle ( objFlag2 );
01174
01175 double L, dx, dy, d_par, d_orth;
01176 double x, y;
01177
01178 double sign = ((aB - aA) > 0.0) ? 1.0 : -1.0;
01179
01180
01181
01182
01183
01184
01185
01186
01187 dx = xB - xA;
01188 dy = yB - yA;
01189 L = sqrt(dx*dx + dy*dy);
01190
01191 dx /= L; dy /= L;
01192
01193 d_par = (L*L + rA*rA - rB*rB) / (2.0 * L);
01194 double arg = rA*rA - d_par*d_par;
01195 d_orth = (arg > 0.0) ? sqrt(arg) : 0.0;
01196
01197 x = xA + d_par * dx - sign * d_orth * dy;
01198 y = yA + d_par * dy + sign * d_orth * dx;
01199
01200 return VecPosition( x, y );
01201 }
01202
01212 AngDeg WorldModel::calculateAngleAgentWithPos( VecPosition pos )
01213 {
01214 int iNrFlags = 0, iIndex;
01215 double dCosX=0, dSinX=0 ,dAngleNow, xA, yA, aA;
01216
01217 for( ObjectT obj = iterateObjectStart( iIndex, OBJECT_SET_FLAGS, 1.0 );
01218 obj != OBJECT_ILLEGAL;
01219 obj = iterateObjectNext ( iIndex, OBJECT_SET_FLAGS, 1.0 ) )
01220 {
01221 xA = getGlobalPosition( obj ).getX();
01222 yA = getGlobalPosition( obj ).getY();
01223 aA = getRelativeAngle( obj );
01224
01225
01226
01227 dAngleNow = atan2Deg( yA - pos.getY(), xA - pos.getX() );
01228 dAngleNow = VecPosition::normalizeAngle( dAngleNow - aA );
01229
01230
01231
01232
01233 dCosX += cosDeg( dAngleNow );
01234 dSinX += sinDeg( dAngleNow );
01235 iNrFlags++;
01236
01237 }
01238 iterateObjectDone( iIndex );
01239
01240
01241 dCosX /= (double)iNrFlags;
01242 dSinX /= (double)iNrFlags;
01243 if( iNrFlags == 0 )
01244 return UnknownAngleValue;
01245
01246 return VecPosition::normalizeAngle( atan2Deg( dSinX, dCosX ) ) ;
01247 }
01248
01254 bool WorldModel::calculateStateBall( VecPosition *posGlobal,
01255 VecPosition *velGlobal )
01256 {
01257
01258 int iNrParticlesLeft = iNrParticlesBall;
01259
01260
01261 checkParticlesBall ( particlesPosBall, particlesVelBall,
01262 iNrParticlesBall, &iNrParticlesLeft );
01263 if( iNrParticlesLeft == 0 )
01264 {
01265 initParticlesBall ( particlesPosBall, particlesVelBall, iNrParticlesBall );
01266 iNrParticlesLeft = iNrParticlesBall;
01267 }
01268
01269
01270 *posGlobal = averageParticles( particlesPosBall, iNrParticlesLeft );
01271 *velGlobal = averageParticles( particlesVelBall, iNrParticlesLeft );
01272 resampleParticlesBall( particlesPosBall, particlesVelBall,
01273 iNrParticlesBall, iNrParticlesLeft );
01274
01275
01276
01277 if( Ball.getTimeChangeInformation( ) != getTimeLastSeen( OBJECT_BALL ) &&
01278 getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() &&
01279 getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() == 1 )
01280 {
01281
01282
01283
01284
01285
01286
01287 VecPosition posGlobalDiff = *posGlobal - Ball.getGlobalPositionLastSee()
01288 - agentObject.getPositionDifference();
01289
01290 *velGlobal = posGlobalDiff*SS->getBallDecay();
01291 }
01292
01293
01294
01295 if( getTimeSinceLastCatch() < 2 || getPlayMode() != PM_PLAY_ON )
01296 velGlobal->setMagnitude( 0.0 );
01297 else if( getNrInSetInCircle( OBJECT_SET_OPPONENTS,
01298 Circle(*posGlobal,SS->getMaximalKickDist())) > 0 )
01299 velGlobal->setMagnitude( 0.0 );
01300 else if( velGlobal->getMagnitude() >
01301 ( 1.0 + SS->getBallRand() )*SS->getBallSpeedMax() )
01302 velGlobal->setMagnitude( SS->getBallSpeedMax() );
01303
01304 if( isBeforeKickOff() )
01305 posGlobal->setVecPosition( 0, 0 );
01306
01307 return true;
01308 }
01309
01319 void WorldModel::initParticlesBall( VecPosition posArray[],
01320 VecPosition velArray[], int iLength )
01321 {
01322
01323 double dDistBall = UnknownDoubleValue, dDistChange = UnknownDoubleValue;
01324 AngDeg angBall = UnknownAngleValue, angChange = UnknownAngleValue;
01325 double dMinDist, dMaxDist, dMinCh, dMaxCh, dDistTmp, dDistChTmp, dVelX, dVelY;
01326 AngDeg angChMin, angChMax, angTmp, angChTmp;
01327
01328
01329 if( Ball.getTimeRelativePosition() != getTimeLastSeeMessage() )
01330 return;
01331
01332
01333 dDistBall = getRelativeDistance( OBJECT_BALL );
01334 angBall = getRelativeAngle( OBJECT_BALL );
01335
01336
01337 if( Ball.getTimeChangeInformation( ) == getTimeLastSeeMessage() )
01338 {
01339 dDistChange = Ball.getRelativeDistanceChange();
01340 angChange = Ball.getRelativeAngleChange();
01341 }
01342
01343
01344 getMinMaxDistQuantizeValue( dDistBall, &dMinDist, &dMaxDist, 0.1, 0.1 );
01345 getMinMaxDistChange( dDistChange, dDistBall, &dMinCh, &dMaxCh, 0.02, 0.1,0.1);
01346 getMinMaxDirChange ( angChange, &angChMin, &angChMax, 0.1 );
01347
01348 for( int i = 0; i < iLength; i ++ )
01349 {
01350
01351 dDistTmp = dMinDist + drand48()*fabs(dMaxDist - dMinDist);
01352 angTmp = angBall + drand48() - 0.5;
01353
01354 posArray[i].setVecPosition( dDistTmp, angTmp, POLAR );
01355 posArray[i].relativeToGlobal( getAgentGlobalPosition(),
01356 getAgentGlobalNeckAngle() );
01357
01358 if( dDistChange != UnknownDoubleValue )
01359 {
01360
01361 angChTmp = angChMin + drand48()*(angChMax-angChMin);
01362 dDistChTmp = dMinCh + drand48()*(dMaxCh-dMinCh);
01363
01364 dVelX=dDistChTmp*cosDeg(angTmp)-Deg2Rad(angChTmp)*dDistTmp*sinDeg(angTmp);
01365 dVelY=dDistChTmp*sinDeg(angTmp)+Deg2Rad(angChTmp)*dDistTmp*cosDeg(angTmp);
01366
01367 velArray[i].setVecPosition( dVelX, dVelY );
01368 velArray[i].relativeToGlobal( getAgentGlobalVelocity(),
01369 getAgentGlobalNeckAngle() );
01370 }
01371 else
01372 velArray[i].setVecPosition( 0, 0 );
01373 }
01374 }
01375
01386 void WorldModel::checkParticlesBall( VecPosition posArray[],
01387 VecPosition velArray[], int iLength, int *iNrParticlesLeft )
01388 {
01389 bool bIllegal;
01390 double dMinDist, dMaxDist, dMinCh, dMaxCh, dMag;
01391 double dDistBall = UnknownDoubleValue, dDistChange = UnknownDoubleValue;
01392 AngDeg angBall = UnknownAngleValue, angChange = UnknownAngleValue;
01393 double dDistChTmp;
01394 AngDeg angChTmp, angChMin, angChMax;
01395 VecPosition pos_rel, vel_rel;
01396
01397 int i1, i2, i3, i4;
01398 i1 = i2 = i3 = i4 = 0;
01399
01400
01401 if( getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
01402 return;
01403
01404
01405
01406 dDistBall = getRelativeDistance( OBJECT_BALL );
01407 angBall = getRelativeAngle( OBJECT_BALL );
01408 getMinMaxDistQuantizeValue( dDistBall, &dMinDist, &dMaxDist, 0.1, 0.1 );
01409
01410 if( getTimeLastSeen( OBJECT_BALL ) == Ball.getTimeChangeInformation( ) )
01411 {
01412 dDistChange = Ball.getRelativeDistanceChange();
01413 angChange = Ball.getRelativeAngleChange();
01414 getMinMaxDirChange ( angChange, &angChMin, &angChMax, 0.1);
01415 getMinMaxDistChange( dDistChange, dDistBall, &dMinCh, &dMaxCh,0.02,0.1,0.1);
01416
01417 }
01418
01419 *iNrParticlesLeft = 0;
01420 for( int i = 0; i < iLength; i ++ )
01421 {
01422
01423 pos_rel = posArray[i];
01424 vel_rel = velArray[i];
01425 pos_rel.globalToRelative( getAgentGlobalPosition(),
01426 getAgentGlobalNeckAngle() );
01427 vel_rel.globalToRelative( getAgentGlobalVelocity(),
01428 getAgentGlobalNeckAngle() );
01429
01430 bIllegal = false;
01431
01432 dMag = pos_rel.getMagnitude();
01433 if( dMag < dMinDist || dMag > dMaxDist )
01434 {
01435 i1++;
01436 bIllegal = true;
01437 }
01438 if( fabs( VecPosition::normalizeAngle(pos_rel.getDirection() - angBall) )
01439 > 0.5 )
01440 {
01441 bIllegal = true;
01442 i2++;
01443 }
01444
01445 if( dDistChange != UnknownDoubleValue )
01446 {
01447 dDistChTmp = (vel_rel.getX()*(pos_rel.getX()/dMag)) +
01448 (vel_rel.getY()*(pos_rel.getY()/dMag));
01449 angChTmp = Rad2Deg( ((vel_rel.getY()*pos_rel.getX()/dMag) -
01450 (vel_rel.getX()*pos_rel.getY()/dMag)))/dMag ;
01451
01452 if( angChTmp < angChMin || angChTmp > angChMax )
01453 {
01454 bIllegal = true;
01455 i3++;
01456 }
01457 if( dDistChTmp < dMinCh || dDistChTmp > dMaxCh )
01458 {
01459 bIllegal = true;
01460 i4++;
01461 }
01462 }
01463
01464
01465 if( bIllegal == false )
01466 {
01467 posArray[*iNrParticlesLeft] = posArray[i];
01468 velArray[(*iNrParticlesLeft)++] = velArray[i];
01469 }
01470 }
01471 }
01472
01481 void WorldModel::updateParticlesBall( VecPosition posArray[],
01482 VecPosition velArray[], int iLength, double dPower, AngDeg ang )
01483 {
01484 double dRand = SS->getBallRand();
01485 double dMaxRand;
01486
01487 for( int i = 0; i < iLength; i ++ )
01488 {
01489
01490 if( dPower > EPSILON )
01491 {
01492 ang = VecPosition::normalizeAngle(ang + getAgentGlobalBodyAngle() );
01493 velArray[i] += VecPosition( getActualKickPowerRate()*dPower, ang, POLAR) ;
01494 if( velArray[i].getMagnitude() > SS->getBallSpeedMax() )
01495 velArray[i].setMagnitude( SS->getBallSpeedMax() );
01496 }
01497
01498
01499 dMaxRand = dRand * velArray[i].getMagnitude();
01500 velArray[i] += VecPosition(
01501 (-1 + 2*drand48())*dMaxRand,
01502 (-1 + 2*drand48())*dMaxRand );
01503 posArray[i] += velArray[i];
01504 velArray[i] *= SS->getBallDecay();
01505 }
01506 }
01516 void WorldModel::resampleParticlesBall( VecPosition posArray[],
01517 VecPosition velArray[], int iLength, int iLeft )
01518 {
01519 int iRand = 0;
01520 for ( int i = iLeft; i < iLength; i ++ )
01521 {
01522 iRand = (int)(drand48()*iLeft);
01523 posArray[ i ] = posArray[ iRand ];
01524 velArray[ i ] = velArray[ iRand ];
01525 }
01526 }
01527
01533 VecPosition WorldModel::calculateVelocityDynamicObject( ObjectT o )
01534 {
01535 DynamicObject * dobj = (DynamicObject*) getObjectPtrFromType( o );
01536 if( dobj == NULL )
01537 return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
01538 double dDistCh = dobj->getRelativeDistanceChange( );
01539 double angCh = dobj->getRelativeAngleChange ( );
01540 double dDist = getRelativeDistance ( o );
01541 double ang = getRelativeAngle ( o );
01542
01543 double velx = dDistCh * cosDeg(ang) - Deg2Rad(angCh) * dDist * sinDeg( ang );
01544 double vely = dDistCh * sinDeg(ang) + Deg2Rad(angCh) * dDist * cosDeg( ang );
01545
01546 VecPosition vel( velx, vely );
01547 return vel.relativeToGlobal( getAgentGlobalVelocity(),
01548 getAgentGlobalNeckAngle() );
01549 }
01550
01551
01558 bool WorldModel::calculateStatePlayer( ObjectT o, VecPosition *posGlobal,
01559 VecPosition *velGlobal )
01560 {
01561 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
01562 if( pob == NULL )
01563 return false;
01564
01565
01566
01567
01568 VecPosition posRelWorld =
01569 VecPosition( getRelativeDistance( o ),
01570 getRelativeAngle( o ) + agentObject.getGlobalNeckAngle(),
01571 POLAR );
01572 *posGlobal = getAgentGlobalPosition() + posRelWorld;
01573
01574 velGlobal->setVecPosition( 0, 0);
01575 if( pob->getTimeChangeInformation( ) == getTimeLastSeen( o ) )
01576 {
01577
01578
01579 *velGlobal = calculateVelocityDynamicObject( o );
01580 }
01581 else
01582 ;
01583
01584
01585 if( velGlobal->getMagnitude() >=
01586 ( 1.0 + SS->getPlayerRand())*SS->getPlayerSpeedMax() )
01587 velGlobal->setMagnitude( SS->getPlayerSpeedMax() );
01588
01589 return true;
01590 }
01591
01592
01604 bool WorldModel::getMinMaxDistQuantizeValue( double dOutput, double *dMin,
01605 double *dMax, double x1, double x2 )
01606 {
01607
01608
01609
01610
01611
01612
01613
01614 dOutput -= 1.0e-10;
01615 *dMin = exp( invQuantizeMin( log( invQuantizeMin(dOutput,x2) ), x1 ) );
01616 dOutput += 2.0e-10;
01617 *dMax = exp( invQuantizeMax( log( invQuantizeMax(dOutput,x2) ), x1 ) );
01618 return true;
01619 }
01620
01630 bool WorldModel::getMinMaxDirChange( double dOutput, double *dMin,
01631 double *dMax, double x1 )
01632 {
01633 *dMin = invQuantizeMin( dOutput, x1 ) ;
01634 *dMax = invQuantizeMax( dOutput, x1 ) ;
01635 return true;
01636 }
01637
01652 bool WorldModel::getMinMaxDistChange( double dOutput, double dDist,
01653 double *dMin, double *dMax, double x1, double xDist1, double xDist2)
01654 {
01655
01656
01657
01658
01659
01660 double dMinDist, dMaxDist;
01661 getMinMaxDistQuantizeValue( dDist, &dMinDist, &dMaxDist, xDist1, xDist2 );
01662 dOutput = dOutput/dDist;
01663 double dMinCh = invQuantizeMin( dOutput, x1 ) ;
01664 double dMaxCh = invQuantizeMax( dOutput, x1 ) ;
01665 *dMin = min( dMinDist*dMinCh, dMaxDist*dMinCh );
01666 *dMax = max( dMinDist*dMaxCh, dMaxDist*dMaxCh );
01667 return true;
01668 }
01669
01675 double WorldModel::invQuantizeMin( double dOutput, double dQuantizeStep )
01676 {
01677
01678
01679 return (rint( dOutput / dQuantizeStep )-0.5 )*dQuantizeStep;
01680 }
01681
01687 double WorldModel::invQuantizeMax( double dOutput, double dQuantizeStep )
01688 {
01689
01690
01691 return (rint( dOutput/dQuantizeStep) + 0.5 )*dQuantizeStep;
01692 }
01693
01702 void WorldModel::mapUnknownPlayers( Time time)
01703 {
01704 double dist, dMinDist;
01705 VecPosition rel;
01706 ObjectT o, o_new;
01707
01708
01709 for( int j = 0; j < iNrUnknownPlayers; j ++ )
01710 {
01711 rel.setVecPosition( UnknownPlayers[j].getRelativeDistance(),
01712 UnknownPlayers[j].getRelativeAngle(), POLAR );
01713 dist = dMinDist = 1000.0;
01714 o = UnknownPlayers[j].getType();
01715 o_new = OBJECT_ILLEGAL;
01716 if( ! SoccerTypes::isOpponent( o ) )
01717 {
01718 for( int i = 0 ; i < MAX_TEAMMATES ; i++ )
01719 {
01720 if( isConfidenceGood( Teammates[i].getType() ) &&
01721 Teammates[i].getTimeLastSeen() != time )
01722 {
01723 dist = rel.getDistanceTo( Teammates[i].getRelativePosition( ) );
01724 if( dist < dMinDist )
01725 {
01726 o_new = Teammates[i].getType();
01727 dMinDist = dist;
01728 }
01729 }
01730 }
01731 }
01732 if( ! SoccerTypes::isTeammate( o ) )
01733 {
01734 for( int i = 0 ; i < MAX_OPPONENTS ; i++ )
01735 {
01736 if( isConfidenceGood( Opponents[i].getType() ) &&
01737 Opponents[i].getTimeLastSeen() != time )
01738 {
01739 dist = rel.getDistanceTo( Opponents[i].getRelativePosition( ) );
01740 if( dist < dMinDist )
01741 {
01742 o_new = Opponents[i].getType();
01743 dMinDist = dist;
01744 }
01745 }
01746 }
01747 }
01748 Log.log( 601, "try to map %d distance %f and %f to %d",
01749 UnknownPlayers[j].getType(), rel.getMagnitude(), dMinDist,
01750 o_new );
01751
01752
01753
01754
01755
01756
01757 if( SoccerTypes::isKnownPlayer(o_new)
01758 && dMinDist < PS->getPlayerDistTolerance())
01759 {
01760 Log.log( 601, "known player: pos %d", o_new );
01761 UnknownPlayers[j].setType( o_new );
01762 if( SoccerTypes::isTeammate( o_new ) )
01763 Teammates[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01764 else if( SoccerTypes::isOpponent( o_new ) )
01765 Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01766 }
01767 else if( UnknownPlayers[j].getType() == OBJECT_TEAMMATE_UNKNOWN )
01768 {
01769 o_new = getFirstEmptySpotInSet( OBJECT_SET_TEAMMATES );
01770 Log.log( 601, "unknown teammate: pos %d", o_new );
01771 if( o_new != OBJECT_ILLEGAL )
01772 {
01773 UnknownPlayers[j].setType( o_new );
01774 Teammates[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01775 }
01776 }
01777 else if( UnknownPlayers[j].getType() == OBJECT_OPPONENT_UNKNOWN )
01778 {
01779 o_new = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
01780 Log.log( 601, "unknown opp: pos %d", o_new );
01781 if( o_new != OBJECT_ILLEGAL )
01782 {
01783 UnknownPlayers[j].setType( o_new );
01784 Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01785 }
01786 }
01787 else if( UnknownPlayers[j].getType() == OBJECT_PLAYER_UNKNOWN &&
01788 UnknownPlayers[j].getRelativeDistance() < SS->getVisibleDistance() )
01789 {
01790 o_new = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
01791 Log.log( 601, "unknown player: pos %d", o_new );
01792 if( o_new != OBJECT_ILLEGAL )
01793 {
01794 UnknownPlayers[j].setType( o_new );
01795 Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01796 }
01797 }
01798 else
01799 Log.log( 601, "not mapped" );
01800 }
01801
01802 iNrUnknownPlayers = 0;
01803 }
01804
01812 bool WorldModel::updateSSToHeteroPlayerType( int iIndex )
01813 {
01814 SS->setPlayerSpeedMax( pt[iIndex].dPlayerSpeedMax );
01815 SS->setStaminaIncMax ( pt[iIndex].dStaminaIncMax );
01816 SS->setPlayerDecay ( pt[iIndex].dPlayerDecay );
01817 SS->setInertiaMoment ( pt[iIndex].dInertiaMoment );
01818 SS->setDashPowerRate ( pt[iIndex].dDashPowerRate );
01819 SS->setPlayerSize ( pt[iIndex].dPlayerSize );
01820 SS->setKickableMargin( pt[iIndex].dKickableMargin );
01821 SS->setKickRand ( pt[iIndex].dKickRand );
01822 SS->setExtraStamina ( pt[iIndex].dExtraStamina );
01823 SS->setEffortMax ( pt[iIndex].dEffortMax );
01824 SS->setEffortMin ( pt[iIndex].dEffortMin );
01825
01826 return true;
01827 }
01828
01832 bool WorldModel::resetTimeObjects( )
01833 {
01834 Ball.setTimeLastSeen ( Time( -1, 0) );
01835 for( int i = 0 ; i < MAX_TEAMMATES ; i ++ )
01836 Teammates[i].setTimeLastSeen ( Time( -1, 0) );
01837 for( int i = 0 ; i < MAX_OPPONENTS ; i ++ )
01838 Opponents[i].setTimeLastSeen ( Time( -1, 0) );
01839 for( int i = 0 ; i < MAX_FLAGS ; i ++ )
01840 Flags[i].setTimeLastSeen ( Time( -1, 0) );
01841 for( int i = 0 ; i < MAX_LINES ; i ++ )
01842 Lines[i].setTimeLastSeen ( Time( -1, 0) );
01843 agentObject.setTimeLastSeen ( Time( -1, 0) );
01844 return true;
01845 }
01846
01850 void WorldModel::removeGhosts( )
01851 {
01852 AngDeg dAngle=SoccerTypes::getHalfViewAngleValue(agentObject.getViewAngle());
01853 dAngle -= 0.15*dAngle;
01854
01855 if( fabs( getRelativeAngle( OBJECT_BALL ) ) < dAngle
01856 && getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
01857 {
01858 Ball.setTimeLastSeen( Time( -1, 0 ) );
01859 }
01860 }
01861
01862
01863
01864
01865
01874 bool WorldModel::calculateStateAgent2( VecPosition *posGlobal,
01875 VecPosition *velGlobal, AngDeg *angGlobal)
01876 {
01877 double x=0.0, y=0.0, dMinRadius1, dMaxRadius1, dMinRadius2, dMaxRadius2;
01878 double dTotalVar = UnknownDoubleValue, dVar, K;
01879 int iIndex1, iIndex2;
01880 ObjectT obj2;
01881 VecPosition pos;
01882
01883
01884 for( ObjectT obj1 = iterateObjectStart( iIndex1, OBJECT_SET_FLAGS, 1.0 );
01885 obj1 != OBJECT_ILLEGAL;
01886 obj1 = iterateObjectNext ( iIndex1, OBJECT_SET_FLAGS, 1.0 ) )
01887 {
01888
01889 iIndex2 = iIndex1;
01890 for( obj2 = iterateObjectNext ( iIndex2, OBJECT_SET_FLAGS, 1.0 ) ;
01891 obj2 != OBJECT_ILLEGAL;
01892 obj2 = iterateObjectNext ( iIndex2, OBJECT_SET_FLAGS, 1.0 ) )
01893 {
01894
01895 pos = calculatePosAgentWith2Flags( obj1, obj2 );
01896
01897
01898
01899
01900
01901
01902
01903 getMinMaxDistQuantizeValue(getRelativeDistance(obj1),
01904 &dMinRadius1, &dMaxRadius1, SS->getQuantizeStepL(), 0.1 ) ;
01905 getMinMaxDistQuantizeValue(getRelativeDistance(obj2),
01906 &dMinRadius2, &dMaxRadius2, SS->getQuantizeStepL(), 0.1 ) ;
01907 dVar = (dMaxRadius1-dMinRadius1)*(dMaxRadius1-dMinRadius1)/12;
01908 dVar += (dMaxRadius2-dMinRadius2)*(dMaxRadius2-dMinRadius2)/12;
01909
01910 if( pos.getX() != UnknownDoubleValue &&
01911 dTotalVar == UnknownDoubleValue )
01912 {
01913 dTotalVar = dVar;
01914 x = pos.getX();
01915 y = pos.getY();
01916 }
01917 else if( pos.getX() != UnknownDoubleValue )
01918 {
01919 K = dTotalVar / ( dVar + dTotalVar );
01920 x += K*( pos.getX() - x );
01921 y += K*( pos.getY() - y );
01922 dTotalVar -= K*dTotalVar;
01923 }
01924 }
01925 iterateObjectDone( iIndex2 );
01926 }
01927 iterateObjectDone( iIndex1 );
01928 posGlobal->setVecPosition( x, y );
01929
01930
01931
01932 *angGlobal = calculateAngleAgentWithPos( *posGlobal );
01933
01934
01935 *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
01936
01937 return true;
01938 }
01939
01947 bool WorldModel::calculateStateBall2( VecPosition *posGlobal,
01948 VecPosition *velGlobal )
01949 {
01950
01951
01952
01953 VecPosition posRelWorld =
01954 VecPosition( getRelativeDistance( OBJECT_BALL ),
01955 getRelativeAngle( OBJECT_BALL ) + getAgentGlobalNeckAngle(),
01956 POLAR );
01957 *posGlobal = getAgentGlobalPosition() + posRelWorld;
01958
01959 if( isBeforeKickOff() )
01960 posGlobal->setVecPosition( 0, 0 );
01961
01962 *velGlobal = getGlobalVelocity(OBJECT_BALL);
01963 if( Ball.getTimeChangeInformation( ) == getTimeLastSeen( OBJECT_BALL ) )
01964 {
01965 *velGlobal = calculateVelocityDynamicObject( OBJECT_BALL );;
01966
01967
01968
01969
01970 if( fabs(velGlobal->getX() - getGlobalVelocity(OBJECT_BALL).getX())
01971 <= 2*SS->getBallRand()*getBallSpeed() &&
01972 fabs(velGlobal->getY() - getGlobalVelocity(OBJECT_BALL).getY())
01973 <= 2*SS->getBallRand()*getBallSpeed() )
01974 {
01975 *velGlobal = (*velGlobal + getGlobalVelocity(OBJECT_BALL))/2.0;
01976 }
01977 }
01978 else if( getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() &&
01979 getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() < 3 )
01980 {
01981
01982
01983
01984
01985
01986
01987
01988 VecPosition posGlobalDiff = *posGlobal - Ball.getGlobalPositionLastSee()
01989 - agentObject.getPositionDifference();
01990
01991
01992
01993
01994
01995
01996
01997
01998
01999
02000 if( getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() == 1 )
02001 *velGlobal = posGlobalDiff*SS->getBallDecay();
02002 else if( getTimeLastSeeMessage()-Ball.getTimeGlobalPosDerivedFromSee() > 2 )
02003 {
02004 Log.log( 20, "(WorldModel:%s) time difference too large" ,__FUNCTION__ );
02005 }
02006 }
02007 else
02008 ;
02009
02010
02011
02012
02013
02014
02015 if( getTimeSinceLastCatch() < 2 || getPlayMode() != PM_PLAY_ON )
02016 velGlobal->setMagnitude( 0.0 );
02017 else if( getNrInSetInCircle( OBJECT_SET_OPPONENTS,
02018 Circle(*posGlobal,SS->getMaximalKickDist())) > 0 )
02019 velGlobal->setMagnitude( 0.0 );
02020 else if( velGlobal->getMagnitude() >
02021 ( 1.0 + SS->getBallRand() )*SS->getBallSpeedMax() )
02022 velGlobal->setMagnitude( SS->getBallSpeedMax() );
02023
02024 return true;
02025 }