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
00030
00048 #include "WorldModel.h"
00049 #include "Parse.h"
00050 #include <stdio.h>
00051 #include <list>
00052
00053 #include <sys/time.h>
00054
00055 #include <unistd.h>
00056
00057
00058
00059
00060
00061
00062
00074 void WorldModel::processSeeGlobalInfo( ObjectT o, Time time,
00075 VecPosition pos, VecPosition vel, AngDeg angBody, AngDeg angNeck)
00076 {
00077 DynamicObject * dobj;
00078 PlayerObject * pobj;
00079
00080 if( o == OBJECT_ILLEGAL )
00081 return;
00082 if( SoccerTypes::isPlayer( o ) )
00083 {
00084 pobj = (PlayerObject*)getObjectPtrFromType( o );
00085 pobj->setTimeLastSeen( time );
00086 pobj->setGlobalPositionLastSee( pos, time );
00087 pobj->setTimeChangeInformation( time );
00088 pobj->setGlobalPosition( pos, time );
00089 pobj->setGlobalVelocity( vel, time );
00090 pobj->setGlobalBodyAngle( angBody, time );
00091 pobj->setGlobalNeckAngle( VecPosition::normalizeAngle(angBody+angNeck),
00092 time );
00093 pobj->setIsKnownPlayer( true );
00094
00095 }
00096 else if( SoccerTypes::isBall( o ) )
00097 {
00098 dobj = (DynamicObject*)getObjectPtrFromType( o );
00099 dobj->setTimeLastSeen( time );
00100 dobj->setGlobalPosition( pos, time );
00101 dobj->setGlobalVelocity( vel, time );
00102 }
00103 }
00104
00120 bool WorldModel::processNewAgentInfo( ViewQualityT vq, ViewAngleT va,
00121 double dStamina, double dEffort, double dSpeed, AngDeg angSpeed,
00122 AngDeg angHeadAngle, int iTackleExpires, int iArmMovable,
00123 int iArmExpires, VecPosition posArm )
00124 {
00125 Stamina sta = agentObject.getStamina();
00126
00127 sta.setStamina ( dStamina );
00128 sta.setEffort ( dEffort );
00129 agentObject.setStamina ( sta );
00130
00131
00132 if( vq == VQ_ILLEGAL )
00133 agentObject.setViewQuality ( vq );
00134 if( va == VA_ILLEGAL )
00135 agentObject.setViewAngle ( va );
00136 agentObject.setSpeedRelToNeck ( VecPosition( dSpeed, angSpeed, POLAR) );
00137 agentObject.setBodyAngleRelToNeck( angHeadAngle );
00138 agentObject.setTackleExpires ( iTackleExpires );
00139 agentObject.setArmMovable ( iArmMovable == 0 );
00140 agentObject.setArmExpires ( iArmExpires );
00141 agentObject.setGlobalArmPosition ( getAgentGlobalPosition() + posArm );
00142
00143 return true;
00144 }
00145
00169 void WorldModel::processNewObjectInfo( ObjectT o, Time time,
00170 double dDist, int iDir, double dDistChange, double dDirChange,
00171 AngDeg angRelBodyAng, AngDeg angRelNeckAng, bool isGoalie,
00172 ObjectT objMin, ObjectT objMax, double dPointDir, bool isTackling )
00173 {
00174 if( dDist == UnknownDoubleValue || o == OBJECT_ILLEGAL )
00175 return;
00176
00177 if( SoccerTypes::isFlag( o ) )
00178 {
00179
00180 Flags[SoccerTypes::getIndex(o)].setRelativePosition(
00181 dDist,(double)iDir,time);
00182 Flags[SoccerTypes::getIndex(o)].setTimeLastSeen ( time );
00183 Flags[SoccerTypes::getIndex(o)].setType ( o );
00184 }
00185 else if( SoccerTypes::isPlayer( o ) || SoccerTypes::isBall( o ) )
00186 {
00187 DynamicObject *d ;
00188
00189
00190 if( !( SoccerTypes::isKnownPlayer( o ) || SoccerTypes::isBall( o ) ) )
00191 {
00192 UnknownPlayers[iNrUnknownPlayers].setIsKnownPlayer( false );
00193 UnknownPlayers[iNrUnknownPlayers].setPossibleRange( objMin, objMax );
00194 d = &UnknownPlayers[iNrUnknownPlayers];
00195 iNrUnknownPlayers++;
00196 }
00197 else
00198 {
00199 d = (DynamicObject*)getObjectPtrFromType( o );
00200 if( SoccerTypes::isPlayer( o ) )
00201 ((PlayerObject*)d)->setIsKnownPlayer( true );
00202 }
00203
00204 if( d != NULL )
00205 {
00206
00207 d->setRelativePosition( dDist, (double)iDir, time );
00208 if( dDistChange != UnknownDoubleValue )
00209 d->setRelativeDistanceChange( dDistChange, time );
00210 if( dDirChange != UnknownDoubleValue )
00211 d->setRelativeAngleChange( dDirChange, time );
00212 if( angRelBodyAng != UnknownAngleValue )
00213 ((PlayerObject*)d)->setRelativeBodyAngle( angRelBodyAng, time );
00214 if( angRelNeckAng != UnknownAngleValue )
00215 ((PlayerObject*)d)->setRelativeNeckAngle( angRelNeckAng, time );
00216 if( isGoalie == true && SoccerTypes::isPlayer( o ))
00217 ((PlayerObject*)d)->setIsGoalie( true );
00218 else if( SoccerTypes::isPlayer( o ))
00219 ((PlayerObject*)d)->setIsGoalie( false );
00220 d->setType( o );
00221 d->setTimeLastSeen( time );
00222
00223 if( isTackling )
00224 {
00225
00226 if( ((PlayerObject*)d)->getTimeTackle() + SS->getTackleCycles()
00227 < getCurrentTime() )
00228 ((PlayerObject*)d)->setTimeTackle( getCurrentTime() - 1 );
00229 }
00230 else
00231 ((PlayerObject*)d)->setTimeTackle( Time(-1,0) );
00232
00233 if( dPointDir != UnknownDoubleValue )
00234 ((PlayerObject*)d)->setGlobalArm( dPointDir, getCurrentTime() );
00235
00236
00237
00238 if( SoccerTypes::isPlayer( o ) && SoccerTypes::isKnownPlayer( o ) )
00239 {
00240 int iIndex;
00241 ObjectT objMin = OBJECT_ILLEGAL;
00242 double dMinDist = 1000.0, dTmp;
00243 ObjectSetT set = SoccerTypes::isOpponent( o )
00244 ? OBJECT_SET_OPPONENTS
00245 : OBJECT_SET_TEAMMATES;
00246
00247 for( ObjectT obj = iterateObjectStart( iIndex, set );
00248 obj != OBJECT_ILLEGAL;
00249 obj = iterateObjectNext ( iIndex, set ) )
00250 {
00251 if( obj != getAgentObjectType() && isKnownPlayer( obj ) == false )
00252 {
00253 dTmp=(getRelativePosition(obj)-d->getRelativePosition()
00254 ).getMagnitude();
00255 if( dTmp < dMinDist )
00256 {
00257 objMin = obj;
00258 dMinDist = dTmp;
00259 }
00260 }
00261 }
00262 iterateObjectDone( iIndex );
00263 if( objMin != OBJECT_ILLEGAL &&
00264 dMinDist < PS->getPlayerDistTolerance() &&
00265 dMinDist < getMaxTraveledDistance( objMin ) )
00266 {
00267 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( objMin );
00268 pob->setTimeLastSeen( -1 );
00269 Log.log( 464, "set time objMin %d to -1 mapped to player %d %f %f",
00270 objMin, o, dMinDist, getMaxTraveledDistance( objMin ) );
00271 }
00272 else
00273 Log.log( 464, "don't set time objMin %d to -1 player %d dist %f",
00274 objMin, o, dMinDist );
00275
00276 }
00277 }
00278 }
00279 else if( SoccerTypes::isLine( o ) )
00280 {
00281
00282
00283 iDir = ( iDir < 0 ) ? (90 + iDir ) : - (90 - iDir );
00284
00285 Lines[SoccerTypes::getIndex(o)].setRelativePosition(
00286 dDist,(double)iDir,time);
00287 Lines[SoccerTypes::getIndex(o)].setTimeLastSeen( time );
00288 Lines[SoccerTypes::getIndex(o)].setType( o );
00289 }
00290 }
00291
00299 bool WorldModel::storePlayerMessage( int iPlayer, char *strMsg, int iCycle )
00300 {
00301 strcpy( m_strPlayerMsg, strMsg );
00302 m_iCycleInMsg = iCycle;
00303 m_timePlayerMsg = getCurrentTime();
00304 m_iMessageSender = iPlayer;
00305 return true;
00306 }
00307
00309 bool WorldModel::processPlayerMessage( )
00310 {
00311 static char strMessage[MAX_MSG];
00312 int iDiff = getCurrentCycle() - m_iCycleInMsg;
00313 double dDiff = (double)iDiff/100.0;
00314 char *strMsg;
00315 strcpy( strMessage, m_strPlayerMsg );
00316 strMsg = strMessage;
00317
00318 char cOffside = strMsg[1];
00319 if( cOffside >= 'a' && cOffside <= 'z' )
00320 {
00321 m_dCommOffsideX = (double)(cOffside - 'a');
00322 m_timeCommOffsideX = getCurrentTime() - 1;
00323 }
00324 else if( cOffside >= 'A' && cOffside <= 'Z' )
00325 {
00326 m_dCommOffsideX = 26 + (double)(cOffside - 'A');
00327 m_timeCommOffsideX = getCurrentTime() - 1;
00328 }
00329 else
00330 return false;
00331
00332 if( strMsg[2] >= '0' && strMsg[2] <= '9' &&
00333 strlen( strMessage ) == 12)
00334 {
00335
00336 double dBallX = (int)(strMsg[2]-'0')*10 +(int)(strMsg[3] - '0' ) - 48.0;
00337 double dBallY = (int)(strMsg[4]-'0')*10 +(int)(strMsg[5] - '0' ) - 34.0;
00338 double dBallVelX = (int)(strMsg[6]-'0') + (int)(strMsg[7] - '0' )/10.0-2.7;
00339 double dBallVelY = (int)(strMsg[8]-'0') + (int)(strMsg[9] - '0' )/10.0-2.7;
00340
00341 VecPosition pos( dBallX, dBallY );
00342 VecPosition vel( dBallVelX, dBallVelY );
00343 for( int i = 0; i < iDiff ; i ++ )
00344 {
00345 pos += vel;
00346 vel *= SS->getBallDecay();
00347 }
00348
00349
00350 if( getTimeLastSeen( OBJECT_BALL ) == -1 ||
00351 (
00352 getTimeChangeInformation( OBJECT_BALL ) < getCurrentTime() - 3 &&
00353 getRelativeDistance( OBJECT_BALL ) > SS->getVisibleDistance()
00354 ) ||
00355 (
00356 getTimeChangeInformation( OBJECT_BALL ) < getCurrentTime() - iDiff &&
00357 vel.getDistanceTo( getGlobalVelocity(OBJECT_BALL) ) > 0.3 &&
00358 getRelativeDistance( OBJECT_BALL ) > SS->getVisibleDistance()
00359 )
00360 )
00361 {
00362 Log.log( 600,
00363 "update ball comm. (%1.2f,%1.2f)(%1.2f,%1.2f) diff %d, last %d",
00364 pos.getX(), pos.getY(), vel.getX(), vel.getY(), iDiff,
00365 getTimeLastSeen( OBJECT_BALL ).getTime() );
00366 Log.log( 601, "update ball from comm (%1.2f,%1.2f)(%1.2f,%1.2f) diff %d",
00367 pos.getX(), pos.getY(), vel.getX(), vel.getY(), iDiff );
00368 processPerfectHearInfoBall( pos, vel, 1.00 - dDiff - 0.01 ) ;
00369 }
00370 else
00371 Log.log( 600, "do not update ball time_change %d, now %d, diff %d, d %f",
00372 getTimeChangeInformation( OBJECT_BALL ).getTime(),
00373 getCurrentCycle(),
00374 iDiff,
00375 vel.getDistanceTo( getGlobalVelocity(OBJECT_BALL) ) );
00376 }
00377 else if( strlen( strMessage ) == 12 &&
00378 strMsg[2] >= 'a' && strMsg[2] <= 'a' + 10 &&
00379 strMsg[6] == ' ' )
00380 {
00381 ObjectT objOpp
00382 = SoccerTypes::getOpponentObjectFromIndex((int)(strMsg[2]-'a'));
00383 char *str = &strMsg[3];
00384 double dOppX = -1*Parse::parseFirstInt( &str );
00385 dOppX /= 10.0;
00386 double dOppY = Parse::parseFirstInt( &str );
00387 dOppY /= 10.0;
00388 VecPosition posOpp( dOppX, dOppY );
00389 processPerfectHearInfo( objOpp, posOpp, 0.99, false ) ;
00390 }
00391
00392 return true;
00393 }
00394
00395 bool WorldModel::processRecvThink( bool b )
00396 {
00397 m_bRecvThink = b;
00398 if( b == true && SS->getSynchMode() == true )
00399 {
00400 #ifdef WIN32
00401
00402 bNewInfo = true;
00403 SetEvent ( event_newInfo );
00404
00405 #else
00406 pthread_mutex_lock ( &mutex_newInfo );
00407 bNewInfo = true;
00408 pthread_cond_signal ( &cond_newInfo );
00409 pthread_mutex_unlock( &mutex_newInfo );
00410 #endif
00411 }
00412 return true;
00413 }
00414
00422 bool WorldModel::processPerfectHearInfoBall( VecPosition posGlobal,
00423 VecPosition vel, double dConf )
00424 {
00425 Log.log( 501, "ball conf: %f %d", getConfidence( OBJECT_BALL ),
00426 getTimeLastSeen( OBJECT_BALL ).getTime() );
00427 if( Ball.getConfidence( getCurrentTime() ) < dConf ||
00428 vel.getDistanceTo( getGlobalVelocity(OBJECT_BALL) ) > 0.3 )
00429 {
00430 Time time = getTimeFromConfidence( dConf );
00431 Ball.setGlobalPosition( posGlobal, time );
00432 Ball.setGlobalVelocity( vel, time );
00433 Ball.setTimeLastSeen ( time );
00434 updateObjectRelativeFromGlobal( OBJECT_BALL );
00435 setTimeLastHearMessage( getCurrentTime() );
00436 return true;
00437 }
00438 return false;
00439 }
00440
00452 bool WorldModel::processPerfectHearInfo( ObjectT o, VecPosition posGlobal,
00453 double dConf, bool bIsGoalie )
00454 {
00455 if( SoccerTypes::isBall( o ) || o == getAgentObjectType() )
00456 return false;
00457 else if( !SoccerTypes::isKnownPlayer( o ) )
00458 return processUnsureHearInfo( o, posGlobal, dConf );
00459
00460 PlayerObject *object = (PlayerObject *)getObjectPtrFromType( o );
00461 if( object == NULL )
00462 return false;
00463
00464 Time time = getTimeFromConfidence( dConf ) ;
00465
00466
00467
00468
00469
00470
00471 if( object->getConfidence( getCurrentTime() ) < dConf ||
00472 object->getIsKnownPlayer() == false )
00473 {
00474 object->setGlobalPosition ( posGlobal , time );
00475 object->setTimeLastSeen ( time );
00476 object->setGlobalVelocity ( VecPosition( 0, 0), time );
00477 object->setIsKnownPlayer ( true );
00478 object->setIsGoalie ( bIsGoalie );
00479 updateObjectRelativeFromGlobal( o );
00480 setTimeLastHearMessage( getCurrentTime() );
00481 return true;
00482 }
00483 return false;
00484 }
00485
00497 bool WorldModel::processUnsureHearInfo( ObjectT o, VecPosition pos,
00498 double dConf )
00499 {
00500 double dMinDist;
00501 ObjectT objInitial = o;
00502
00503 if( o != OBJECT_TEAMMATE_UNKNOWN && o != OBJECT_OPPONENT_UNKNOWN )
00504 return false;
00505
00506
00507 if( SoccerTypes::isTeammate( o ) )
00508 o = getClosestInSetTo( OBJECT_SET_TEAMMATES, pos, &dMinDist);
00509 else if( SoccerTypes::isOpponent( o ) )
00510 o = getClosestInSetTo( OBJECT_SET_OPPONENTS, pos, &dMinDist);
00511
00512 if( o == getAgentObjectType() &&
00513 pos.getDistanceTo(getAgentGlobalPosition())<PS->getPlayerDistTolerance())
00514 return false;
00515
00516
00517
00518
00519
00520 else if( SoccerTypes::isKnownPlayer(o) &&
00521 dMinDist < PS->getPlayerDistTolerance())
00522 {
00523 processPerfectHearInfo( o, pos, dConf );
00524 return true;
00525 }
00526
00527 if( objInitial == OBJECT_TEAMMATE_UNKNOWN )
00528 o = getFirstEmptySpotInSet( OBJECT_SET_TEAMMATES );
00529 else if( objInitial == OBJECT_OPPONENT_UNKNOWN )
00530 o = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
00531 else
00532 return false ;
00533
00534 if( o != OBJECT_ILLEGAL )
00535 {
00536 processPerfectHearInfo( o, pos, dConf );
00537 setIsKnownPlayer( o, false );
00538 }
00539 return true;
00540 }
00541
00562 bool WorldModel::processNewHeteroPlayer( int iIndex, double dPlayerSpeedMax,
00563 double dStaminaIncMax, double dPlayerDecay, double dInertiaMoment,
00564 double dDashPowerRate, double dPlayerSize, double dKickableMargin,
00565 double dKickRand, double dExtraStamina,double dEffortMax,
00566 double dEffortMin )
00567 {
00568 pt[iIndex].dPlayerSpeedMax = dPlayerSpeedMax;
00569 pt[iIndex].dStaminaIncMax = dStaminaIncMax;
00570 pt[iIndex].dPlayerDecay = dPlayerDecay;
00571 pt[iIndex].dInertiaMoment = dInertiaMoment;
00572 pt[iIndex].dDashPowerRate = dDashPowerRate;
00573 pt[iIndex].dPlayerSize = dPlayerSize;
00574 pt[iIndex].dKickableMargin = dKickableMargin;
00575 pt[iIndex].dKickRand = dKickRand;
00576 pt[iIndex].dExtraStamina = dExtraStamina;
00577 pt[iIndex].dEffortMax = dEffortMax;
00578 pt[iIndex].dEffortMin = dEffortMin;
00579 pt[iIndex].dMaximalKickDist = dKickableMargin +
00580 dPlayerSize +
00581 SS->getBallSize();
00582
00583 return true;
00584 }
00585
00595 void WorldModel::processCatchedBall( RefereeMessageT rm, Time time )
00596 {
00597 if( rm == REFC_GOALIE_CATCH_BALL_LEFT && sideSide == SIDE_LEFT )
00598 timeLastCatch = time;
00599 else if( rm == REFC_GOALIE_CATCH_BALL_RIGHT && sideSide == SIDE_RIGHT )
00600 timeLastCatch = time;
00601 Ball.setGlobalVelocity( VecPosition(0,0), getCurrentTime() );
00602 }
00603
00612 void WorldModel::processQueuedCommands( SoccerCommand commands[],
00613 int iCommands )
00614 {
00615 if( iCommands > CMD_MAX_COMMANDS )
00616 {
00617 cerr << "(WorldModel::setQueuedCommands) queuedCommands array cannot "
00618 << "contain so many commands!\n";
00619 return;
00620 }
00621
00622
00623 for( int i = 0 ; i < iCommands ; i ++ )
00624 {
00625 commands[i].time = getCurrentTime();
00626 queuedCommands[(int)commands[i].commandType] = commands[i];
00627 }
00628 }
00629
00637 bool WorldModel::updateAll( )
00638 {
00639 static Timing timer;
00640 double dTimeSense = 0.0, dTimeSee = 0.0, dTimeComm=0.0, dTimeFastest = 0.0;
00641 static clock_t t1, t2;
00642
00643
00644 bool bReturn = false, bUpdateAfterSee = false;
00645 bool bUpdateAfterSense = false, bDebug = false;
00646 static Time timeLastHoleRecorded;
00647 static Time timeBeginInterval;
00648 static Time timePlayersCounted;
00649 static int iNrHolesLastTime = 0;
00650 static Time timeLastSenseUpdate;
00651 static Time timeLastSeeUpdate;
00652 static Time timeLastSayUpdate;
00653 if( bDebug )
00654 {
00655 timer.restartTime();
00656
00657 t1 = clock() * 1000 / CLOCKS_PER_SEC;
00658 }
00659
00660
00661 if( agentObject.getTimeGlobalPosition() < getCurrentTime() - 1 )
00662 Log.log( 3, "(WorldModel::updateAll) missed a sense??" );
00663
00664
00665 if( isFullStateOn( ) == true )
00666 {
00667 Log.log( 4, "full state is on" );
00668 updateRelativeFromGlobal();
00669 timeLastSenseMessage = timeLastRecvSeeMessage;
00670 timeLastSeeMessage = timeLastRecvSeeMessage;
00671 bUpdateAfterSee = bUpdateAfterSense = false;
00672 bReturn = true;
00673 }
00674 else
00675 {
00676 Log.log( 4, "full state is off" );
00677 if( getTimeLastRecvSeeMessage() > timeLastSeeUpdate )
00678 bUpdateAfterSee = true;
00679 if( getTimeLastRecvSenseMessage() > timeLastSenseUpdate )
00680 bUpdateAfterSense = true;
00681 }
00682
00683
00684
00685 if( bUpdateAfterSee && bUpdateAfterSense )
00686 {
00687 Log.log( 3, "!!! Two updates !!! " );
00688 Log.log( 3, "see: %d sense: %d", getTimeLastRecvSeeMessage().getTime(),
00689 getTimeLastRecvSenseMessage().getTime() );
00690 if( getTimeLastRecvSeeMessage( ) == getTimeLastRecvSenseMessage() )
00691 {
00692 Log.log( 3, "update sense" );
00693 timeLastSenseMessage = timeLastRecvSenseMessage;
00694 bReturn = updateAfterSenseMessage( );
00695 if( bDebug ) dTimeSense = timer.getElapsedTime(1000);
00696 updateRelativeFromGlobal();
00697 Log.log( 3, "update see" );
00698 timeLastSeeMessage = timeLastRecvSeeMessage;
00699 bReturn &= updateAfterSeeMessage( );
00700 if( bDebug ) dTimeSee = timer.getElapsedTime(1000) - dTimeSense;
00701 }
00702 else if( getTimeLastRecvSeeMessage( ) < getTimeLastRecvSenseMessage() )
00703 {
00704 Log.log( 3, "update see" );
00705 timeLastSeeMessage = timeLastRecvSeeMessage;
00706 bReturn = updateAfterSeeMessage( );
00707 if( bDebug ) dTimeSee = timer.getElapsedTime(1000);
00708 Log.log( 3, "update sense" );
00709 timeLastSenseMessage = timeLastRecvSenseMessage;
00710 bReturn &= updateAfterSenseMessage( );
00711 updateRelativeFromGlobal();
00712 if( bDebug ) dTimeSense = timer.getElapsedTime(1000) - dTimeSee;
00713 }
00714 timeLastSenseUpdate = getTimeLastSenseMessage();
00715 timeLastSeeUpdate = getTimeLastSeeMessage();
00716 }
00717 else if( bUpdateAfterSee )
00718 {
00719 Log.log( 3, "update see" );
00720 timeLastSeeMessage = timeLastRecvSeeMessage;
00721 bReturn = updateAfterSeeMessage( );
00722 timeLastSeeUpdate = getTimeLastSeeMessage();
00723 if( bDebug ) dTimeSee = timer.getElapsedTime(1000);
00724 }
00725 else if( bUpdateAfterSense )
00726 {
00727 Log.log( 3, "update sense" );
00728 timeLastSenseMessage = timeLastRecvSenseMessage;
00729 bReturn = updateAfterSenseMessage( );
00730 timeLastSenseUpdate = getTimeLastSenseMessage();
00731 updateRelativeFromGlobal();
00732 if( bDebug ) dTimeSense = timer.getElapsedTime(1000);
00733 }
00734
00735 if( timeLastSayUpdate != m_timePlayerMsg &&
00736 isFullStateOn() == false )
00737 {
00738 Log.log( 3, "update hear" );
00739 if( bDebug ) dTimeComm = timer.getElapsedTime(1000);
00740 timeLastSayUpdate = m_timePlayerMsg;
00741 processPlayerMessage();
00742 if( bDebug ) dTimeComm = timer.getElapsedTime(1000) - dTimeComm;
00743 }
00744
00745 SoccerCommand soc = getChangeViewCommand( );
00746 if( ! soc.isIllegal() )
00747 {
00748 setAgentViewAngle( soc.va );
00749 setAgentViewQuality( soc.vq );
00750 }
00751
00752
00753 if( isQueuedActionPerformed() == false &&
00754 timeLastHoleRecorded != getCurrentTime() &&
00755 isFullStateOn() == false )
00756 {
00757 Log.log( 2, "HOLE recorded" );
00758 timeLastHoleRecorded = getCurrentTime();
00759 iNrHoles++;
00760 }
00761
00762
00763 int iTimeDiff = getCurrentTime() - timeBeginInterval;
00764 double dHolePerc = (double)(iNrHoles - iNrHolesLastTime)/iTimeDiff*100;
00765 if( ! isLastMessageSee( ) && iTimeDiff % 400 == 0 && dHolePerc > 1.0 &&
00766 PS->getFractionWaitSeeEnd() > 0.70 )
00767 {
00768 PS->setFractionWaitSeeEnd( PS->getFractionWaitSeeEnd() - 0.05 );
00769 timeBeginInterval = getCurrentTime();
00770 cerr << getCurrentCycle() << ": lowered send time to " <<
00771 PS->getFractionWaitSeeEnd() << " for player number " <<
00772 getPlayerNumber() <<
00773 "; nr of holes is "<< dHolePerc << "%" << "( " << iNrHoles << ", "
00774 << iNrHolesLastTime << ")" << endl;
00775 iNrHolesLastTime = iNrHoles;
00776 }
00777
00778
00779 if( bUpdateAfterSense == true && ! isTimeStopped() &&
00780 getCurrentTime() != timePlayersCounted )
00781 {
00782 iNrTeammatesSeen += getNrInSetInRectangle( OBJECT_SET_TEAMMATES );
00783 iNrOpponentsSeen += getNrInSetInRectangle( OBJECT_SET_OPPONENTS );
00784 timePlayersCounted = getCurrentTime();
00785 }
00786
00787
00788 if( Log.isInLogLevel( 456 ) )
00789 logObjectInformation( 456, getAgentObjectType() );
00790
00791 if( bUpdateAfterSee == true )
00792 Log.logWithTime(3, " finished update_all see; start determining action" );
00793 if( bUpdateAfterSense == true )
00794 Log.logWithTime(3, " finished update_all sense;start determining action");
00795
00796 if( Log.isInLogLevel( 459 ) )
00797 {
00798 Log.log( 459, "%s%s", strLastSeeMessage, strLastSenseMessage );
00799 show( OBJECT_BALL, Log.getOutputStream() );
00800 show( OBJECT_SET_PLAYERS, Log.getOutputStream() );
00801 }
00802 if( ( Log.isInLogLevel( 101 ) && getRelativeDistance( OBJECT_BALL ) < 2.0 ) )
00803 show( OBJECT_BALL, Log.getOutputStream() );
00804 if( Log.isInLogLevel( 556 ) &&
00805 getRelativeDistance( OBJECT_BALL ) < SS->getVisibleDistance() )
00806 {
00807 Log.log( 556, "%s", strLastSeeMessage );
00808 show( OBJECT_SET_PLAYERS, Log.getOutputStream() );
00809 show( OBJECT_BALL, Log.getOutputStream() );
00810 }
00811 if( LogDraw.isInLogLevel( 460 ) )
00812 {
00813 int iCycles;
00814 dTimeFastest = timer.getElapsedTime( 1000 );
00815 ObjectT obj = getFastestInSetTo(OBJECT_SET_OPPONENTS,OBJECT_BALL,&iCycles);
00816 logCircle( 460, getGlobalPosition( obj ), 1.5 );
00817 logCircle( 460, predictPosAfterNrCycles( OBJECT_BALL, iCycles ), 0.5 );
00818 obj=getFastestInSetTo(OBJECT_SET_TEAMMATES_NO_GOALIE,OBJECT_BALL,&iCycles);
00819 logCircle( 460, getGlobalPosition( obj ), 1.5 );
00820 logCircle( 460, predictPosAfterNrCycles( OBJECT_BALL, iCycles ), 0.75 );
00821 dTimeFastest = timer.getElapsedTime( 1000 ) - dTimeFastest;
00822 }
00823
00824 if( LogDraw.isInLogLevel( 701 ) )
00825 drawCoordinationGraph( );
00826
00827 logLine( 602, VecPosition( getOffsideX(), -PITCH_WIDTH/2.0),
00828 VecPosition( getOffsideX(), PITCH_WIDTH/2.0) );
00829 if( bDebug )
00830 {
00831 Log.logWithTime( 461, "time update all: %f\n time comm: %1.5f\n\
00832 time see: %1.5f\n time sense: %1.5f\n time fastest: %1.5f\n\
00833 time rest: %1.5f\n utime: %1.5f",
00834 timer.getElapsedTime()*1000, dTimeComm, dTimeSee, dTimeSense,dTimeFastest,
00835 timer.getElapsedTime()*1000-(dTimeComm+dTimeSee+dTimeSense+dTimeFastest ),
00836 t2 - t1
00837
00838 );
00839 }
00840 return bReturn;
00841 }
00842
00843
00844
00845
00846
00847 void WorldModel::processLastSeeMessage( )
00848 {
00849 ObjectT o;
00850 double dDist, dDistChange, dDirChange, dPointDir, dTemp;
00851 int iDir;
00852 AngDeg angBodyFacingDir, angHeadFacingDir;
00853 Time time = getTimeLastSeeMessage();
00854 bool isGoalie, isTackling;
00855 static char strTmp[MAX_MSG];
00856 char *strMsg = strLastSeeMessage ;
00857 Parse::parseFirstInt( &strMsg );
00858
00859 ObjectT objMin = OBJECT_ILLEGAL;
00860 ObjectT objMax = OBJECT_ILLEGAL;
00861
00862 while( *strMsg != ')' )
00863 {
00864 dDist = dDistChange = dDirChange = dTemp = dPointDir = UnknownDoubleValue;
00865 angBodyFacingDir = angHeadFacingDir = UnknownAngleValue;
00866 strMsg += 2;
00867 isTackling = false;
00868
00869
00870
00871 o = SoccerTypes::getObjectFromStr( &strMsg, &isGoalie, getTeamName() );
00872
00873 if( o == OBJECT_ILLEGAL )
00874 {
00875 Log.log( 4, "Illegal object in: %s", strLastSeeMessage );
00876 Log.log( 4, "rest of message: %s", strMsg );
00877 }
00878 else if( SoccerTypes::isKnownPlayer( o ) )
00879 objMin = o;
00880 else if( SoccerTypes::isPlayer( o ) )
00881 {
00882 if( SoccerTypes::isTeammate( o ) &&
00883 ( objMin == OBJECT_ILLEGAL || SoccerTypes::isOpponent( objMin ) ) )
00884 objMin = OBJECT_TEAMMATE_1 ;
00885 else if( SoccerTypes::isOpponent( o ) &&
00886 ( objMin == OBJECT_ILLEGAL || SoccerTypes::isTeammate( objMin ) ) )
00887 objMin = OBJECT_OPPONENT_1 ;
00888 else if( objMin == OBJECT_ILLEGAL )
00889 objMin = (getSide() == SIDE_LEFT ) ? OBJECT_TEAMMATE_1
00890 : OBJECT_OPPONENT_1 ;
00891 else if( objMin == OBJECT_TEAMMATE_11 )
00892 objMin = OBJECT_OPPONENT_1;
00893 else if( objMin == OBJECT_OPPONENT_11 )
00894 objMin = OBJECT_TEAMMATE_1;
00895 else
00896 objMin = (ObjectT)((int)objMin + 1);
00897
00898 if( objMin == getAgentObjectType() )
00899 {
00900 if( objMin == OBJECT_TEAMMATE_11 )
00901 objMin = OBJECT_OPPONENT_1;
00902 else if( objMin == OBJECT_OPPONENT_11 )
00903 objMin = OBJECT_TEAMMATE_1;
00904 else
00905 objMin = (ObjectT)((int)objMin + 1);
00906 }
00907 strcpy( strTmp, strMsg );
00908 objMax = getMaxRangeUnknownPlayer( objMin, strTmp );
00909 }
00910
00911 dTemp = Parse::parseFirstDouble( &strMsg );
00912 if ( *strMsg == ')' )
00913 iDir = (int)dTemp;
00914 else
00915 {
00916 dDist = dTemp;
00917 iDir = Parse::parseFirstInt( &strMsg );
00918
00919 double dValues[7];
00920 int i = 0;
00921 if( *strMsg == ' ' )
00922 {
00923 while( *strMsg != ')' && *(strMsg+1) != 't' && i < 7 )
00924 {
00925 dValues[i] = Parse::parseFirstDouble( &strMsg );
00926 i++;
00927 }
00928 }
00929
00930 switch( i )
00931 {
00932 case 0:
00933 break;
00934 case 1:
00935 dPointDir = dValues[0];
00936 break;
00937 case 5:
00938 dPointDir = dValues[4];
00939 case 4:
00940 angBodyFacingDir = dValues[2];
00941 angHeadFacingDir = dValues[3];
00942 case 2:
00943 dDistChange = dValues[0];
00944 dDirChange = dValues[1];
00945 break;
00946 default:
00947 cerr << "(WorldModelUpdate::analyzeSee) wrong param nr " << i <<endl;
00948
00949 }
00950
00951 if( *(strMsg+1) == 't')
00952 isTackling = true;
00953 }
00954
00955
00956
00957 Parse::gotoFirstOccurenceOf( ')', &strMsg );
00958 strMsg++;
00959
00960 if( SoccerTypes::isPlayer( o ) && !SoccerTypes::isKnownPlayer( o ) )
00961 {
00962 if( objMin != OBJECT_ILLEGAL )
00963 {
00964 if( objMin == objMax )
00965 {
00966 Log.log( 459, "range, only %d left", objMin );
00967 o = objMin;
00968 }
00969 else if( SoccerTypes::isTeammate( objMin ) &&
00970 SoccerTypes::isTeammate( objMax ) )
00971 o = OBJECT_TEAMMATE_UNKNOWN;
00972 else if( SoccerTypes::isOpponent( objMin ) &&
00973 SoccerTypes::isOpponent( objMax ) )
00974 o = OBJECT_OPPONENT_UNKNOWN;
00975 }
00976 }
00977
00978
00979 processNewObjectInfo( o, time, dDist, iDir, dDistChange,
00980 dDirChange, angBodyFacingDir, angHeadFacingDir,
00981 isGoalie, objMin, objMax, dPointDir, isTackling );
00982 }
00983 }
00984
00990 bool WorldModel::updateAfterSeeMessage( )
00991 {
00992 processLastSeeMessage( );
00993
00994
00995 if( getCurrentTime().getTime() != -1 )
00996 updateAgentObjectAfterSee( );
00997
00998 mapUnknownPlayers( getTimeLastSeeMessage() );
00999
01000
01001
01002
01003
01004 double dConfThr = PS->getPlayerConfThr();
01005 int iIndex;
01006
01007 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
01008 o != OBJECT_ILLEGAL;
01009 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
01010 {
01011 if( getTimeLastSeen( o ) == getTimeLastSeeMessage() &&
01012 o != getAgentObjectType() )
01013 updateDynamicObjectAfterSee ( o );
01014 else
01015 updateObjectRelativeFromGlobal( o );
01016 }
01017 iterateObjectDone( iIndex );
01018
01019
01020 if( getTimeLastSeen( OBJECT_BALL ) == getTimeLastSeeMessage() )
01021 updateDynamicObjectAfterSee ( OBJECT_BALL );
01022 else
01023 updateObjectRelativeFromGlobal( OBJECT_BALL );
01024
01025
01026 if( Log.isInLogLevel( 2 ) )
01027 show( OBJECT_BALL, Log.getOutputStream() );
01028 removeGhosts();
01029 if( Log.isInLogLevel( 2 ) )
01030 show( OBJECT_BALL, Log.getOutputStream() );
01031
01032
01033 return true;
01034 }
01035
01040 bool WorldModel::updateAgentObjectAfterSee( )
01041 {
01042 VecPosition posGlobal, velGlobal;
01043 AngDeg angGlobal;
01044
01045
01046 calculateStateAgent( &posGlobal, &velGlobal, &angGlobal );
01047
01048
01049 agentObject.setTimeLastSeen ( getTimeLastSeeMessage() );
01050
01051
01052 agentObject.setPositionDifference(posGlobal-agentObject.getGlobalPosition());
01053 agentObject.setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
01054 agentObject.setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
01055 agentObject.setGlobalNeckAngle ( angGlobal );
01056 agentObject.setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
01057
01058 return true;
01059 }
01060
01061
01067 bool WorldModel::updateDynamicObjectAfterSee( ObjectT o )
01068 {
01069 VecPosition posGlobal, velGlobal;
01070
01071 if( o == OBJECT_BALL )
01072 {
01073 calculateStateBall ( &posGlobal, &velGlobal );
01074 Ball.setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
01075 Ball.setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
01076 Ball.setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
01077 return true;
01078 }
01079 else if( SoccerTypes::isKnownPlayer( o ) )
01080 {
01081 calculateStatePlayer( o, &posGlobal, &velGlobal );
01082
01083 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
01084
01085
01086
01087 pob->setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
01088 pob->setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
01089 pob->setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
01090
01091 if( pob->getTimeRelativeAngles() == getTimeLastSeeMessage() )
01092 {
01093 AngDeg ang = getAgentGlobalNeckAngle() + pob->getRelativeBodyAngle();
01094 ang = VecPosition::normalizeAngle( ang );
01095 pob->setGlobalBodyAngle( ang, getTimeLastSeeMessage() );
01096 pob->setGlobalBodyAngleLastSee( ang );
01097 ang = getAgentGlobalNeckAngle() + pob->getRelativeNeckAngle();
01098 ang = VecPosition::normalizeAngle( ang );
01099 pob->setGlobalNeckAngle( ang, getTimeLastSeeMessage() );
01100 pob->setGlobalVelocityLastSee( velGlobal );
01101 }
01102 return true;
01103 }
01104
01105 return false;
01106 }
01107
01108
01109
01110
01111
01112
01113 void WorldModel::processLastSenseMessage( )
01114 {
01115 char *strMsg = strLastSenseMessage ;
01116
01117 Parse::parseFirstInt( &strMsg );
01118 strMsg++;
01119
01120 Parse::gotoFirstOccurenceOf( ' ', &strMsg );
01121 strMsg++;
01122
01123 ViewQualityT vq = SoccerTypes::getViewQualityFromStr( strMsg );
01124 Parse::gotoFirstOccurenceOf( ' ', &strMsg );
01125 strMsg++;
01126 ViewAngleT va = SoccerTypes::getViewAngleFromStr( strMsg );
01127 double dStamina = Parse::parseFirstDouble( &strMsg );
01128 double dEffort = Parse::parseFirstDouble( &strMsg );
01129
01130 double dSpeed = Parse::parseFirstDouble( &strMsg );
01131 AngDeg angSpeed = Parse::parseFirstDouble( &strMsg );
01132
01133
01134 int iHeadAngle = - Parse::parseFirstInt( &strMsg );
01135
01136
01137 setNrOfCommands( CMD_KICK , Parse::parseFirstInt( &strMsg ) );
01138 setNrOfCommands( CMD_DASH , Parse::parseFirstInt( &strMsg ) );
01139 setNrOfCommands( CMD_TURN , Parse::parseFirstInt( &strMsg ) );
01140 setNrOfCommands( CMD_SAY , Parse::parseFirstInt( &strMsg ) );
01141 setNrOfCommands( CMD_TURNNECK , Parse::parseFirstInt( &strMsg ) );
01142 setNrOfCommands( CMD_CATCH , Parse::parseFirstInt( &strMsg ) );
01143 setNrOfCommands( CMD_MOVE , Parse::parseFirstInt( &strMsg ) );
01144 setNrOfCommands( CMD_CHANGEVIEW , Parse::parseFirstInt( &strMsg ) );
01145 int iArmMovable = Parse::parseFirstInt( &strMsg );
01146 int iArmExpires = Parse::parseFirstInt( &strMsg );
01147 double dArmDist = Parse::parseFirstDouble( &strMsg );
01148 AngDeg angArm = Parse::parseFirstDouble( &strMsg );
01149
01150 setNrOfCommands( CMD_POINTTO , Parse::parseFirstInt( &strMsg ) );
01151
01152
01153 int i = Parse::parseFirstInt( &strMsg );
01154 char c = (i >= 10 ) ? *(strMsg-4) : *(strMsg-3);
01155 Log.log( 602, "focus %d |%c|", i, c );
01156 if( c == 'l' || c == 'r' )
01157 {
01158 Log.log( 602, "set focus: %d",
01159 SoccerTypes::getTeammateObjectFromIndex( -1 + i ) );
01160 setObjectFocus( SoccerTypes::getTeammateObjectFromIndex( -1 + i ) );
01161 i = Parse::parseFirstInt( &strMsg );
01162 }
01163 setNrOfCommands( CMD_ATTENTIONTO , i );
01164
01165 int iTackleExpires = Parse::parseFirstInt( &strMsg );
01166 setNrOfCommands( CMD_TACKLE , Parse::parseFirstInt( &strMsg ) );
01167
01168 angArm = VecPosition::normalizeAngle( angArm );
01169 processNewAgentInfo( vq, va, dStamina, dEffort, dSpeed,
01170 (AngDeg) angSpeed, (AngDeg)iHeadAngle, iTackleExpires,
01171 iArmMovable, iArmExpires, VecPosition( dArmDist, angArm, POLAR ));
01172 }
01173
01182 bool WorldModel::updateAfterSenseMessage( )
01183 {
01184 processLastSenseMessage( );
01185
01186
01187 updateAgentAndBallAfterSense( );
01188
01189
01190 double dConfThr = PS->getPlayerConfThr();
01191 int iIndex;
01192
01193 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
01194 o != OBJECT_ILLEGAL;
01195 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
01196 {
01197 if( o != getAgentObjectType() && getTimeGlobalPosition(o) > 0 &&
01198 ! isBeforeKickOff() )
01199 {
01200 updateDynamicObjectForNextCycle( o,
01201 getCurrentTime() - getTimeGlobalPosition(o) );
01202 }
01203 }
01204 iterateObjectDone( iIndex);
01205
01206
01207 if( isBeforeKickOff() )
01208 {
01209 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_TEAMMATES, -2000 );
01210 o != OBJECT_ILLEGAL;
01211 o = iterateObjectNext ( iIndex, OBJECT_SET_TEAMMATES, -2000 ) )
01212 {
01213 if( o == getAgentObjectType() )
01214 continue;
01215 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
01216
01217 VecPosition pos = getStrategicPosition( SoccerTypes::getIndex( o ) ) ;
01218 VecPosition vel( 0, 0 );
01219
01220 pob->setGlobalVelocity ( vel, getTimeLastSenseMessage() );
01221 pob->setGlobalPosition ( pos, getTimeLastSenseMessage() );
01222 pob->setGlobalPositionLastSee( pos, getTimeLastSenseMessage() );
01223 pob->setTimeLastSeen ( getTimeLastSenseMessage() - 2 );
01224 updateObjectRelativeFromGlobal( o );
01225 Log.log( 459, "set info %d (%f,%f)", SoccerTypes::getIndex( o ),
01226 pos.getX(), pos.getY() );
01227 }
01228 Ball.setGlobalPosition( VecPosition(0,0), getTimeLastSenseMessage() );
01229 Ball.setGlobalVelocity( VecPosition(0,0), getTimeLastSenseMessage() );
01230 }
01231
01232 iterateObjectDone( iIndex);
01233
01234 return true;
01235 }
01236
01242 bool WorldModel::updateAgentAndBallAfterSense( )
01243 {
01244
01245 bool bBallUpdated = false;
01246 VecPosition pos = getAgentGlobalPosition();
01247 AngDeg angGlobalNeck = getAgentGlobalNeckAngle();
01248 AngDeg angGlobalBody = getAgentGlobalBodyAngle();
01249 Stamina sta = getAgentStamina();
01250 VecPosition velNeck = agentObject.getSpeedRelToNeck();
01251 VecPosition vel = getAgentGlobalVelocity();
01252 Time time = getCurrentTime() - 1 ;
01253
01254
01255
01256 for( int i = 0; i < CMD_MAX_COMMANDS; i ++ )
01257 {
01258 if( performedCommands[i] == true &&
01259 ( queuedCommands[i].time.getTime() == time.getTime() ||
01260 queuedCommands[i].time.getTime() == time.getTime() - 1 ) )
01261 {
01262 switch( queuedCommands[i].commandType )
01263 {
01264 case CMD_TURN:
01265 case CMD_TURNNECK:
01266 case CMD_DASH:
01267 case CMD_TACKLE:
01268
01269 predictStateAfterCommand( queuedCommands[i], &pos, &vel,
01270 &angGlobalBody, &angGlobalNeck,
01271 getAgentObjectType(), &sta );
01272 break;
01273 case CMD_KICK:
01274 updateBallAfterKick( queuedCommands[i] );
01275 bBallUpdated = true;
01276 queuedCommands[i].time.updateTime( -1 );
01277 break;
01278 case CMD_MOVE:
01279 pos.setVecPosition( queuedCommands[i].dX, queuedCommands[i].dY );
01280 initParticlesAgent( pos );
01281 break;
01282 default:
01283 break;
01284 }
01285 }
01286 }
01287
01288
01289 if( !bBallUpdated )
01290 {
01291 m_bPerformedKick = false;
01292 updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
01293 }
01294 else
01295 m_bPerformedKick = true;
01296
01297
01298 velNeck.rotate( angGlobalNeck );
01299 double dDistBall = pos.getDistanceTo( getBallPos() );
01300
01301 if( velNeck.getMagnitude() < EPSILON &&
01302 vel.getMagnitude() > 0.01 &&
01303 dDistBall < 0.6 )
01304 Log.log( 102, "I assume collision after kick" );
01305
01306 if( (
01307 velNeck.getMagnitude() < EPSILON &&
01308 vel.getMagnitude() > 0.01 &&
01309 dDistBall < 0.8
01310 )
01311 ||
01312 ( dDistBall<SS->getPlayerSize()+SS->getBallSize() &&
01313 velNeck.getMagnitude( ) < EPSILON
01314 )
01315 ||
01316 ( dDistBall < SS->getMaximalKickDist() + 0.5 &&
01317 velNeck.getMagnitude( ) > EPSILON &&
01318 ( sign( vel.getX() ) != sign( velNeck.getX() ) ||
01319 fabs( vel.getX() ) < 0.08 ) &&
01320 ( sign( vel.getY() ) != sign( velNeck.getY() ) ||
01321 fabs( vel.getY() ) < 0.08 ) &&
01322 velNeck.getMagnitude() < 0.25 * vel.getMagnitude() ) )
01323 {
01324 m_bWasCollision = true;
01325 m_timeLastCollision = getCurrentTime();
01326 double dDistPlayer;
01327 getClosestInSetTo(
01328 OBJECT_SET_PLAYERS, getAgentObjectType(), &dDistPlayer );
01329 if( dDistBall < dDistPlayer )
01330 {
01331 updateBallForCollision( pos );
01332 Log.log( 102, "COLLISION WITH BALL %f, vel(%f,%f) velNeck(%f,%f)",
01333 dDistBall, vel.getX(), vel.getY(), velNeck.getX(), velNeck.getY() );
01334 }
01335 else
01336 Log.log( 102, "COLLISION WITH PLAYER vel(%f,%f) velNeck(%f,%f)",
01337 vel.getX(), vel.getY(), velNeck.getX(), velNeck.getY() );
01338
01339
01340 vel = velNeck;
01341 }
01342 else
01343 {
01344 if( dDistBall < SS->getVisibleDistance() )
01345 Log.log( 102, "No collision: dist: %f, velNeck (%f,%f), vel (%f,%f)",
01346 dDistBall, velNeck.getX(), velNeck.getY(), vel.getX(), vel.getY() );
01347 m_bWasCollision = false;
01348
01349 pos = getAgentGlobalPosition();
01350 angGlobalNeck = getAgentGlobalNeckAngle();
01351 angGlobalBody = getAgentGlobalBodyAngle();
01352 sta = getAgentStamina();
01353 vel = agentObject.getSpeedRelToNeck();
01354
01355
01356
01357
01358
01359
01360
01361 vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
01362
01363 for( int i = 0; i < CMD_MAX_COMMANDS; i ++ )
01364 {
01365 if( performedCommands[i] == true &&
01366 ( queuedCommands[i].time.getTime() == time.getTime() ||
01367 queuedCommands[i].time.getTime() == time.getTime() - 1 ) )
01368 {
01369 switch( queuedCommands[i].commandType )
01370 {
01371 case CMD_TURN:
01372 case CMD_TURNNECK:
01373 predictStateAfterCommand( queuedCommands[i], &pos, &vel,
01374 &angGlobalBody, &angGlobalNeck,
01375 getAgentObjectType(), &sta );
01376 break;
01377 case CMD_MOVE:
01378 pos.setVecPosition( queuedCommands[i].dX, queuedCommands[i].dY );
01379 initParticlesAgent( pos );
01380 break;
01381 case CMD_DASH:
01382
01383
01384 break;
01385 case CMD_TACKLE:
01386
01387 break;
01388 default:
01389 break;
01390 }
01391 }
01392 queuedCommands[i].time.updateTime( -1 );
01393 }
01394
01395
01396 vel = agentObject.getSpeedRelToNeck();
01397 pos = getAgentGlobalPosition();
01398 vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
01399 vel.rotate( angGlobalNeck );
01400
01401
01402
01403
01404
01405 predictStateAfterDash( 0.0, &pos, &vel, &sta, 0, getAgentObjectType() );
01406 }
01407
01408
01409 updateParticlesAgent( vel, true );
01410
01411
01412
01413 agentObject.setGlobalPosition ( pos, getCurrentTime() );
01414 agentObject.setGlobalVelocity ( vel, getCurrentTime() );
01415 agentObject.setGlobalNeckAngle( angGlobalNeck );
01416
01417 return true;
01418 }
01419
01427 bool WorldModel::updateBallAfterKick( SoccerCommand soc )
01428 {
01429 if( getRelativeDistance( OBJECT_BALL ) < SS->getMaximalKickDist() )
01430 {
01431 VecPosition posBall, velBall;
01432 predictBallInfoAfterCommand( soc, &posBall, &velBall );
01433 Ball.setGlobalPosition( posBall, getCurrentTime() );
01434 Ball.setGlobalVelocity( velBall, getCurrentTime() );
01435
01436
01437 }
01438 else
01439 {
01440 updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
01441
01442
01443 #ifdef WIN32
01444 Log.log( 21, "(WorldModel::%s) KICK command, but ball not kickable (%f)",
01445 "updateBallAfterKick", getRelativeDistance( OBJECT_BALL ) );
01446 #else
01447 Log.log( 21, "(WorldModel::%s) KICK command, but ball not kickable (%f)",
01448 __FUNCTION__, getRelativeDistance( OBJECT_BALL ) );
01449 #endif
01450 }
01451 return true;
01452 }
01453
01460 bool WorldModel::updateDynamicObjectForNextCycle( ObjectT obj, int iCycles)
01461 {
01462 DynamicObject *o = (DynamicObject*) getObjectPtrFromType( obj );
01463 if( o == NULL )
01464 return false;
01465
01466
01467 VecPosition vel = getGlobalVelocity( obj );
01468 VecPosition pos = getGlobalPosition( obj );
01469 VecPosition posBall = getBallPos();
01470
01471
01472 if( SoccerTypes::isBall( obj ) )
01473 {
01474 for( int i = 0; i < iCycles ; i++ )
01475 {
01476 pos += vel;
01477 vel *= SS->getBallDecay();
01478 }
01479 double dDist;
01480 ObjectT objOpp =
01481 getClosestInSetTo( OBJECT_SET_OPPONENTS, OBJECT_BALL, &dDist );
01482 if( objOpp != OBJECT_ILLEGAL &&
01483 pos.getDistanceTo( getGlobalPosition( objOpp ) )
01484 < getMaximalKickDist( objOpp ) )
01485 {
01486 Log.log( 556, "update dyn. obj; set ball velocity to 0, opp has it" );
01487 vel.setVecPosition(0,0);
01488 }
01489 }
01490 else if( SoccerTypes::isTeammate( obj ) &&
01491 obj != OBJECT_TEAMMATE_1 && getPlayMode() == PM_PLAY_ON )
01492 {
01493 for( int i = 0; i < iCycles ; i++ )
01494 {
01495
01496 pos += vel;
01497 vel *= SS->getPlayerDecay();
01498 }
01499 }
01500 else if( SoccerTypes::isOpponent( obj ) )
01501 {
01502 VecPosition velBall = getGlobalVelocity( OBJECT_BALL );
01503 for( int i = 0; i < iCycles ; i++ )
01504 {
01505
01506 if( obj == getOppGoalieType() )
01507 ;
01508
01509 else if( obj == getFastestInSetTo( OBJECT_SET_OPPONENTS, OBJECT_BALL ) )
01510 {
01511 AngDeg ang = (getBallPos()-getGlobalPosition( obj )).getDirection();
01512 pos += VecPosition( 0.3, ang, POLAR );
01513 vel *= SS->getPlayerDecay();
01514 Log.log( 556, "update fastest opp to (%f,%f) cyc %d",
01515 pos.getX(), pos.getY(), iCycles );
01516 }
01517 else if( velBall.getX() > 0 && ! ( isDeadBallUs() || isDeadBallThem() )
01518 && ! ( fabs( pos.getX() ) > PENALTY_X + 5 ) )
01519 {
01520 pos += VecPosition( (velBall.getX() > 1.0) ? 0.5 : 0.25, 0 );
01521 vel *= SS->getPlayerDecay();
01522 }
01523 else if( ! ( isDeadBallUs() || isDeadBallThem() )
01524 && ! ( fabs( pos.getX() ) > PENALTY_X + 5 ) )
01525 {
01526
01527 pos -= VecPosition( (velBall.getX() < -1.0 ) ? 0.5 : 0.25, 0 );
01528 vel *= SS->getPlayerDecay();
01529 }
01530 }
01531 }
01532
01533 o->setGlobalVelocity ( vel, getCurrentTime() );
01534 o->setGlobalPosition ( pos, getCurrentTime() );
01535
01536 return true;
01537 }
01538
01539
01544 bool WorldModel::updateBallForCollision( VecPosition posAgent )
01545 {
01546 VecPosition posBall = getGlobalPosition( OBJECT_BALL );
01547 VecPosition velBall = getGlobalVelocity( OBJECT_BALL );
01548
01549
01550
01551 AngDeg ang = (posBall - posAgent).getDirection();
01552 Ball.setGlobalPosition( posAgent + VecPosition( 0.2, ang, POLAR ),
01553 getCurrentTime() );
01554 Ball.setGlobalVelocity( velBall*-0.1, getCurrentTime() );
01555 Log.log( 102, "updateBallForCollision; vel from (%f,%f) to (%f,%f)",
01556 velBall.getX(), velBall.getY(), getGlobalVelocity(OBJECT_BALL).getX(),
01557 getGlobalVelocity(OBJECT_BALL).getY() );
01558 return true;
01559 }
01560
01566 bool WorldModel::updateRelativeFromGlobal()
01567 {
01568 double dConfThr = PS->getPlayerConfThr();
01569 int iIndex;
01570
01571
01572 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
01573 o != OBJECT_ILLEGAL;
01574 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
01575 {
01576 if( o != getAgentObjectType() )
01577 updateObjectRelativeFromGlobal( o );
01578 }
01579 iterateObjectDone( iIndex);
01580
01581
01582 if( isConfidenceGood( OBJECT_BALL ) )
01583 updateObjectRelativeFromGlobal( OBJECT_BALL );
01584
01585
01586
01587 return true;
01588 }
01589
01595 bool WorldModel::updateObjectRelativeFromGlobal( ObjectT o )
01596 {
01597 Object *obj = (Object*) getObjectPtrFromType( o );
01598 if( obj == NULL )
01599 return false;
01600
01601
01602 VecPosition rel = obj->getGlobalPosition();
01603 rel.globalToRelative( getAgentGlobalPosition(), getAgentGlobalNeckAngle() );
01604 obj->setRelativePosition( rel, obj->getTimeGlobalPosition() );
01605 return true;
01606 }
01607
01616 bool WorldModel::calculateStateAgent( VecPosition *posGlobal,
01617 VecPosition *velGlobal, AngDeg *angGlobal )
01618 {
01619 int iNrLeft;
01620
01621
01622 ObjectT objLine = getFurthestRelativeInSet( OBJECT_SET_LINES );
01623 if( objLine != OBJECT_ILLEGAL )
01624 {
01625 double angGlobalLine = getGlobalAngle ( objLine );
01626 AngDeg angRelLine = getRelativeAngle( objLine );
01627 *angGlobal = angGlobalLine - angRelLine;
01628 *angGlobal = VecPosition::normalizeAngle( *angGlobal );
01629 }
01630 else
01631 {
01632 Log.log( 21,
01633 "(WorldModel::calculateStateAgent) no line in last see message" );
01634 *angGlobal = getAgentGlobalNeckAngle();
01635 }
01636
01637
01638
01639
01640
01641
01642
01643
01644 *velGlobal = agentObject.getSpeedRelToNeck().rotate( *angGlobal );
01645 velGlobal->setMagnitude( velGlobal->getMagnitude()/SS->getPlayerDecay() );
01646
01647 updateParticlesAgent ( *velGlobal, false );
01648 iNrLeft = checkParticlesAgent ( *angGlobal );
01649
01650 if( iNrLeft == 0 )
01651 {
01652
01653
01654 initParticlesAgent ( *angGlobal );
01655 iNrLeft = checkParticlesAgent( *angGlobal );
01656 if( iNrLeft == 0 )
01657 {
01658
01659
01660 calculateStateAgent2( posGlobal, velGlobal, angGlobal );
01661 initParticlesAgent( *posGlobal );
01662 return false;
01663 }
01664 }
01665
01666
01667
01668 *posGlobal = averageParticles( particlesPosAgent, iNrLeft );
01669 resampleParticlesAgent( iNrLeft );
01670
01671
01672
01673 AngDeg ang = calculateAngleAgentWithPos( *posGlobal );
01674 if( ang != UnknownAngleValue )
01675 *angGlobal = ang;
01676
01677 *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
01678 return true;
01679 }
01680
01688 void WorldModel::initParticlesAgent( AngDeg angGlobal )
01689 {
01690 double dDist, dMaxRadius, dMinRadius, dInput;
01691 AngDeg ang;
01692
01693
01694 ObjectT objFlag = getClosestRelativeInSet( OBJECT_SET_FLAGS );
01695
01696 if( objFlag == OBJECT_ILLEGAL )
01697 {
01698 Log.log( 21, "(WorldModel::%s) no flag in last see message",
01699 "initParticlesAgent" );
01700 return;
01701 }
01702
01703
01704 dInput = getRelativeDistance( objFlag );
01705 getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01706 SS->getQuantizeStepL(), 0.1 ) ;
01707
01708
01709
01710 AngDeg angFlag = getRelativeAngle( objFlag ) + 180 + angGlobal ;
01711
01712 for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01713 {
01714
01715
01716
01717
01718 dDist = dMinRadius + drand48()*(dMaxRadius-dMinRadius);
01719 ang = VecPosition::normalizeAngle( angFlag - 1.0 + 2*drand48() );
01720
01721
01722 particlesPosAgent[i] = getGlobalPosition( objFlag ) +
01723 VecPosition( dDist, ang, POLAR );
01724 }
01725 }
01726
01732 void WorldModel::initParticlesAgent( VecPosition posInitial )
01733 {
01734 for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01735 particlesPosAgent[i] = posInitial;
01736 }
01737
01747 int WorldModel::checkParticlesAgent( AngDeg angGlobalNeck )
01748 {
01749 double dMaxRadius, dMinRadius, dInput, dDist;
01750 AngDeg ang;
01751 int iIndex, iNrLeft = iNrParticlesAgent, iLength = iNrParticlesAgent;
01752
01753
01754 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_FLAGS, 1.0 );
01755 o != OBJECT_ILLEGAL;
01756 o = iterateObjectNext ( iIndex, OBJECT_SET_FLAGS, 1.0 ) )
01757 {
01758 iNrLeft = 0;
01759 dInput = getRelativeDistance( o );
01760 getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01761 SS->getQuantizeStepL(), 0.1 ) ;
01762
01763
01764 for( int i = 0; i < iLength; i ++ )
01765 {
01766
01767
01768
01769 dDist = particlesPosAgent[i].getDistanceTo( getGlobalPosition( o ) );
01770 ang = (getGlobalPosition(o) - particlesPosAgent[i]).getDirection();
01771 ang = ang - ( getRelativeAngle( o ) + angGlobalNeck );
01772
01773
01774
01775 if( dDist > dMinRadius && dDist < dMaxRadius &&
01776 fabs(VecPosition::normalizeAngle( ang )) <= 1.0 )
01777 particlesPosAgent[iNrLeft++] = particlesPosAgent[i];
01778 }
01779
01780 iLength = iNrLeft;
01781 }
01782 return iNrLeft;
01783 }
01784
01795 void WorldModel::updateParticlesAgent( VecPosition vel, bool bAfterSense )
01796 {
01797
01798 static VecPosition prev_vel;
01799
01800 for( int i = 0; i < iNrParticlesAgent ; i ++ )
01801 {
01802 if( bAfterSense == false )
01803 {
01804 particlesPosAgent[i].setX( particlesPosAgent[i].getX()-prev_vel.getX());
01805 particlesPosAgent[i].setY( particlesPosAgent[i].getY()-prev_vel.getY());
01806 }
01807
01808 particlesPosAgent[i].setX( particlesPosAgent[i].getX( ) + vel.getX() );
01809 particlesPosAgent[i].setY( particlesPosAgent[i].getY( ) + vel.getY() );
01810 }
01811 prev_vel = vel;
01812 }
01813
01814
01819 VecPosition WorldModel::averageParticles( VecPosition posArray[], int iLength )
01820 {
01821 if( iLength == 0 )
01822 return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
01823
01824
01825 double x = 0, y = 0;
01826 for( int i = 0; i < iLength ; i ++ )
01827 {
01828 x += posArray[ i ].getX( );
01829 y += posArray[ i ].getY( );
01830 }
01831
01832 return VecPosition( x/iLength, y/iLength );
01833 }
01834
01841 void WorldModel::resampleParticlesAgent( int iLeft )
01842 {
01843 for ( int i = iLeft; i < iNrParticlesAgent; i ++ )
01844 particlesPosAgent[ i ] = particlesPosAgent[ (int)(drand48()*iLeft) ];
01845 }
01846
01855 bool WorldModel::calculateStateAgent2( VecPosition *posGlobal,
01856 VecPosition *velGlobal, AngDeg *angGlobal)
01857 {
01858 double x=0.0, y=0.0, dMinRadius1, dMaxRadius1, dMinRadius2, dMaxRadius2;
01859 double dTotalVar = UnknownDoubleValue, dVar, K;
01860 int iIndex1, iIndex2;
01861 ObjectT obj2;
01862 VecPosition pos;
01863
01864
01865 for( ObjectT obj1 = iterateObjectStart( iIndex1, OBJECT_SET_FLAGS, 1.0 );
01866 obj1 != OBJECT_ILLEGAL;
01867 obj1 = iterateObjectNext ( iIndex1, OBJECT_SET_FLAGS, 1.0 ) )
01868 {
01869
01870 iIndex2 = iIndex1;
01871 for( obj2 = iterateObjectNext ( iIndex2, OBJECT_SET_FLAGS, 1.0 ) ;
01872 obj2 != OBJECT_ILLEGAL;
01873 obj2 = iterateObjectNext ( iIndex2, OBJECT_SET_FLAGS, 1.0 ) )
01874 {
01875
01876 pos = calculatePosAgentWith2Flags( obj1, obj2 );
01877
01878
01879
01880
01881
01882
01883
01884 getMinMaxDistQuantizeValue(getRelativeDistance(obj1),
01885 &dMinRadius1, &dMaxRadius1, SS->getQuantizeStepL(), 0.1 ) ;
01886 getMinMaxDistQuantizeValue(getRelativeDistance(obj2),
01887 &dMinRadius2, &dMaxRadius2, SS->getQuantizeStepL(), 0.1 ) ;
01888 dVar = (dMaxRadius1-dMinRadius1)*(dMaxRadius1-dMinRadius1)/12;
01889 dVar += (dMaxRadius2-dMinRadius2)*(dMaxRadius2-dMinRadius2)/12;
01890
01891 if( pos.getX() != UnknownDoubleValue &&
01892 dTotalVar == UnknownDoubleValue )
01893 {
01894 dTotalVar = dVar;
01895 x = pos.getX();
01896 y = pos.getY();
01897 }
01898 else if( pos.getX() != UnknownDoubleValue )
01899 {
01900 K = dTotalVar / ( dVar + dTotalVar );
01901 x += K*( pos.getX() - x );
01902 y += K*( pos.getY() - y );
01903 dTotalVar -= K*dTotalVar;
01904 }
01905 }
01906 iterateObjectDone( iIndex2 );
01907 }
01908 iterateObjectDone( iIndex1 );
01909 posGlobal->setVecPosition( x, y );
01910
01911
01912
01913 *angGlobal = calculateAngleAgentWithPos( *posGlobal );
01914
01915
01916 *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
01917
01918 return true;
01919 }
01920
01930 bool WorldModel::calculateStateAgent3( VecPosition *posGlobal,
01931 VecPosition *velGlobal, AngDeg *angGlobal )
01932 {
01933
01934 ObjectT objFlag1 = getClosestRelativeInSet( OBJECT_SET_FLAGS );
01935 ObjectT objFlag2 = getSecondClosestRelativeInSet( OBJECT_SET_FLAGS );
01936 ObjectT objLine = getFurthestRelativeInSet( OBJECT_SET_LINES );
01937
01938
01939 if( objLine != OBJECT_ILLEGAL )
01940 {
01941 double angGlobalLine = getGlobalAngle ( objLine );
01942 AngDeg angRelLine = getRelativeAngle( objLine );
01943 *angGlobal = angGlobalLine - angRelLine;
01944 *angGlobal = VecPosition::normalizeAngle( *angGlobal );
01945 }
01946
01947
01948
01949
01950
01951 if( objFlag1 != OBJECT_ILLEGAL && objFlag2 != OBJECT_ILLEGAL &&
01952 (
01953 fabs(getRelativeAngle(objFlag1) - getRelativeAngle(objFlag2)) > 8.0
01954 || objLine == OBJECT_ILLEGAL
01955 ) )
01956 {
01957 *posGlobal = calculatePosAgentWith2Flags( objFlag1, objFlag2 );
01958 }
01959 else if( objFlag1 != OBJECT_ILLEGAL && objLine != OBJECT_ILLEGAL )
01960 {
01961
01962
01963
01964
01965
01966 VecPosition posGlobalFlag = getGlobalPosition( objFlag1 );
01967 VecPosition relPosFlag = VecPosition::getVecPositionFromPolar(
01968 getRelativeDistance( objFlag1 ),
01969 *angGlobal + getRelativeAngle( objFlag1 ) );
01970 *posGlobal = posGlobalFlag - relPosFlag;
01971 }
01972 else
01973 {
01974 cout << "(WorldModel::calculateStateAgent3) cannot determine pos in cycle "
01975 << getCurrentCycle() << endl;
01976 return false;
01977 }
01978
01979
01980 *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
01981
01982 return true;
01983 }
01984
01996 VecPosition WorldModel::calculatePosAgentWith2Flags( ObjectT objFlag1,
01997 ObjectT objFlag2 )
01998 {
01999 double xA, yA, xB, yB, rA, rB;
02000 AngDeg aA, aB;
02001
02002
02003 xA = getGlobalPosition ( objFlag1 ).getX();
02004 yA = getGlobalPosition ( objFlag1 ).getY();
02005 rA = getRelativeDistance( objFlag1 );
02006 aA = getRelativeAngle ( objFlag1 );
02007 xB = getGlobalPosition ( objFlag2 ).getX();
02008 yB = getGlobalPosition ( objFlag2 ).getY();
02009 rB = getRelativeDistance( objFlag2 );
02010 aB = getRelativeAngle ( objFlag2 );
02011
02012 double L, dx, dy, d_par, d_orth;
02013 double x, y;
02014
02015 double sign = ((aB - aA) > 0.0) ? 1.0 : -1.0;
02016
02017
02018
02019
02020
02021
02022
02023
02024 dx = xB - xA;
02025 dy = yB - yA;
02026 L = sqrt(dx*dx + dy*dy);
02027
02028 dx /= L; dy /= L;
02029
02030 d_par = (L*L + rA*rA - rB*rB) / (2.0 * L);
02031 double arg = rA*rA - d_par*d_par;
02032 d_orth = (arg > 0.0) ? sqrt(arg) : 0.0;
02033
02034 x = xA + d_par * dx - sign * d_orth * dy;
02035 y = yA + d_par * dy + sign * d_orth * dx;
02036
02037 return VecPosition( x, y );
02038 }
02039
02051 AngDeg WorldModel::calculateAngleAgentWithPos( VecPosition pos )
02052 {
02053 int iNrFlags = 0, iIndex;
02054 double dCosX=0, dSinX=0 ,dAngleNow, xA, yA, aA;
02055
02056 for( ObjectT obj = iterateObjectStart( iIndex, OBJECT_SET_FLAGS, 1.0 );
02057 obj != OBJECT_ILLEGAL;
02058 obj = iterateObjectNext ( iIndex, OBJECT_SET_FLAGS, 1.0 ) )
02059 {
02060 xA = getGlobalPosition( obj ).getX();
02061 yA = getGlobalPosition( obj ).getY();
02062 aA = getRelativeAngle( obj );
02063
02064
02065
02066 dAngleNow = atan2Deg( yA - pos.getY(), xA - pos.getX() );
02067 dAngleNow = VecPosition::normalizeAngle( dAngleNow - aA );
02068
02069
02070
02071
02072 dCosX += cosDeg( dAngleNow );
02073 dSinX += sinDeg( dAngleNow );
02074 iNrFlags++;
02075
02076 }
02077 iterateObjectDone( iIndex );
02078
02079
02080 dCosX /= (double)iNrFlags;
02081 dSinX /= (double)iNrFlags;
02082 if( iNrFlags == 0 )
02083 return UnknownAngleValue;
02084
02085 return VecPosition::normalizeAngle( atan2Deg( dSinX, dCosX ) ) ;
02086 }
02087
02088
02094 VecPosition WorldModel::calculateVelocityDynamicObject( ObjectT o )
02095 {
02096 DynamicObject * dobj = (DynamicObject*) getObjectPtrFromType( o );
02097 if( dobj == NULL )
02098 return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
02099 double dDistCh = dobj->getRelativeDistanceChange( );
02100 double angCh = dobj->getRelativeAngleChange ( );
02101 double dDist = getRelativeDistance ( o );
02102 double ang = getRelativeAngle ( o );
02103
02104 double velx = dDistCh * cosDeg(ang) - Deg2Rad(angCh) * dDist * sinDeg( ang );
02105 double vely = dDistCh * sinDeg(ang) + Deg2Rad(angCh) * dDist * cosDeg( ang );
02106
02107 VecPosition vel( velx, vely );
02108 return vel.relativeToGlobal( getAgentGlobalVelocity(),
02109 getAgentGlobalNeckAngle() );
02110 }
02111
02112
02118 bool WorldModel::calculateStateBall( VecPosition *posGlobal,
02119 VecPosition *velGlobal )
02120 {
02121
02122
02123
02124 VecPosition posRelWorld =
02125 VecPosition( getRelativeDistance( OBJECT_BALL ),
02126 getRelativeAngle( OBJECT_BALL ) + getAgentGlobalNeckAngle(),
02127 POLAR );
02128 *posGlobal = getAgentGlobalPosition() + posRelWorld;
02129
02130 if( isBeforeKickOff() )
02131 {
02132 posGlobal->setVecPosition( 0, 0 );
02133 velGlobal->setVecPosition( 0, 0 );
02134 return true;
02135 }
02136
02137 *velGlobal = getGlobalVelocity(OBJECT_BALL);
02138 if( Ball.getTimeChangeInformation( ) == getTimeLastSeen( OBJECT_BALL ) )
02139 {
02140 *velGlobal = calculateVelocityDynamicObject( OBJECT_BALL );;
02141 Log.log( 458, "vel based on change info: (%f,%f)", velGlobal->getX(),
02142 velGlobal->getY() );
02143
02144
02145
02146 if( fabs(velGlobal->getX() - getGlobalVelocity(OBJECT_BALL).getX())
02147 <= 2*SS->getBallRand()*getBallSpeed() &&
02148 fabs(velGlobal->getY() - getGlobalVelocity(OBJECT_BALL).getY())
02149 <= 2*SS->getBallRand()*getBallSpeed() )
02150 {
02151 *velGlobal = (*velGlobal + getGlobalVelocity(OBJECT_BALL))/2.0;
02152 Log.log( 458, "average ball vel to (%f,%f)", velGlobal->getX(),
02153 velGlobal->getY() );
02154 }
02155 }
02156 else if( getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() &&
02157 getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() < 3)
02158 {
02159
02160
02161
02162
02163
02164
02165
02166 VecPosition posGlobalDiff = *posGlobal - Ball.getGlobalPositionLastSee()
02167 - agentObject.getPositionDifference();
02168 Log.log( 101, "2 pos: ball(%f,%f), ball_prev(%f,%f), agentdiff(%f,%f)",
02169 posGlobal->getX(), posGlobal->getY(),
02170 Ball.getGlobalPositionLastSee().getX(),
02171 Ball.getGlobalPositionLastSee().getY(),
02172 agentObject.getPositionDifference().getX(),
02173 agentObject.getPositionDifference().getY() );
02174
02175
02176
02177
02178
02179
02180
02181
02182
02183
02184 if( getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() == 1 &&
02185 m_bWasCollision == false )
02186 {
02187 *velGlobal = posGlobalDiff*SS->getBallDecay();
02188 Log.log( 458, "vel based on 2 pos: (%f,%f)", velGlobal->getX(),
02189 velGlobal->getY() );
02190 Log.log( 101, "vel based on 2 pos: (%f,%f)", velGlobal->getX(),
02191 velGlobal->getY() );
02192 }
02193 else if(getTimeLastSeeMessage()-Ball.getTimeGlobalPosDerivedFromSee()==2 &&
02194 m_bWasCollision == false )
02195 {
02196 VecPosition velTmp, posTmp;
02197 double dDist;
02198
02199
02200
02201
02202
02203
02204
02205 velTmp = (posGlobalDiff/(1+SS->getBallDecay()))*SS->getBallDecay();
02206 posTmp = *posGlobal - velTmp;
02207 getClosestInSetTo( OBJECT_SET_PLAYERS, posTmp, &dDist );
02208 if( dDist > SS->getMaximalKickDist() &&
02209 m_bPerformedKick == false &&
02210 (getCurrentTime() - m_timeLastCollision) > 3 )
02211 {
02212 *velGlobal = velTmp * SS->getBallDecay();
02213 Log.log( 458, "vel based on 3 pos: (%f,%f)", velGlobal->getX(),
02214 velGlobal->getY() );
02215 }
02216 }
02217 else if( getTimeLastSeeMessage()-Ball.getTimeGlobalPosDerivedFromSee() > 2)
02218 {
02219 #ifdef WIN32
02220 Log.log( 20, "(WorldModel:%s) time difference too large" ,
02221 "calculateStateBall" );
02222 #else
02223 Log.log( 20, "(WorldModel:%s) time difference too large" ,__FUNCTION__ );
02224 #endif
02225
02226 }
02227 }
02228 else
02229 Log.log( 458, "vel ball not updated", velGlobal->getX(),velGlobal->getY());
02230
02231
02232
02233
02234
02235
02236
02237
02238
02239 if( getTimeSinceLastCatch() < 2 ||
02240 (getPlayMode() != PM_PLAY_ON && !isGoalKickUs() && !isGoalKickThem() &&
02241 !isPenaltyUs() && !isPenaltyThem() ))
02242 velGlobal->setMagnitude( 0.0 );
02243 else if( getNrInSetInCircle( OBJECT_SET_OPPONENTS,
02244 Circle(*posGlobal,SS->getMaximalKickDist())) > 0
02245 && getRelativeDistance( OBJECT_BALL ) > SS->getMaximalKickDist() )
02246 velGlobal->setMagnitude( 0.0 );
02247 else if( velGlobal->getMagnitude() >
02248 ( 1.0 + SS->getBallRand() )*SS->getBallSpeedMax() )
02249 velGlobal->setMagnitude( SS->getBallSpeedMax() );
02250
02251 if( isKickInUs() || isKickInThem() )
02252 posGlobal->setY( sign( posGlobal->getY() ) * PITCH_WIDTH/2.0 );
02253 else if( isBackPassUs() )
02254 posGlobal->setVecPosition( - PENALTY_X,
02255 sign(posGlobal->getY())*PENALTY_AREA_WIDTH/2.0);
02256 else if( isBackPassThem() )
02257 posGlobal->setVecPosition( + PENALTY_X,
02258 sign(posGlobal->getY())*PENALTY_AREA_WIDTH/2.0);
02259
02260 Log.log( 458, "final ball vel: (%f,%f)",velGlobal->getX(),velGlobal->getY());
02261 if( getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() )
02262 Log.log( 101, "direction old: %f, new: %f",
02263 ( getGlobalPosition( OBJECT_BALL ) -
02264 getAgentGlobalPosition()).getDirection(),
02265 ( *posGlobal - getAgentGlobalPosition()).getDirection() );
02266 return true;
02267 }
02268
02269
02270
02277 bool WorldModel::calculateStatePlayer( ObjectT o, VecPosition *posGlobal,
02278 VecPosition *velGlobal )
02279 {
02280 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
02281 if( pob == NULL )
02282 return false;
02283
02284
02285
02286
02287 VecPosition posRelWorld =
02288 VecPosition( getRelativeDistance( o ),
02289 getRelativeAngle( o ) + agentObject.getGlobalNeckAngle(),
02290 POLAR );
02291 *posGlobal = getAgentGlobalPosition() + posRelWorld;
02292
02293 *velGlobal = getGlobalVelocity( o ) ;
02294 if( pob->getTimeChangeInformation( ) == getTimeLastSeen( o ) )
02295 {
02296
02297
02298 *velGlobal = calculateVelocityDynamicObject( o );
02299 }
02300 else
02301 ;
02302
02303
02304 if( velGlobal->getMagnitude() >=
02305 ( 1.0 + SS->getPlayerRand())*SS->getPlayerSpeedMax() )
02306 velGlobal->setMagnitude( SS->getPlayerSpeedMax() );
02307
02308 return true;
02309 }
02310
02311
02325 bool WorldModel::getMinMaxDistQuantizeValue( double dOutput, double *dMin,
02326 double *dMax, double x1, double x2 )
02327 {
02328
02329
02330
02331
02332
02333
02334
02335 dOutput -= 1.0e-10;
02336 if( dOutput < 0 )
02337 *dMin = 0.0;
02338 else
02339 *dMin = exp( invQuantizeMin( log( invQuantizeMin(dOutput,x2) ), x1 ) );
02340 dOutput += 2.0e-10;
02341 *dMax = exp( invQuantizeMax( log( invQuantizeMax(dOutput,x2) ), x1 ) );
02342 return true;
02343 }
02344
02354 bool WorldModel::getMinMaxDirChange( double dOutput, double *dMin,
02355 double *dMax, double x1 )
02356 {
02357 *dMin = invQuantizeMin( dOutput, x1 ) ;
02358 *dMax = invQuantizeMax( dOutput, x1 ) ;
02359 return true;
02360 }
02361
02376 bool WorldModel::getMinMaxDistChange( double dOutput, double dDist,
02377 double *dMin, double *dMax, double x1, double xDist1, double xDist2)
02378 {
02379
02380
02381
02382
02383
02384 double dMinDist, dMaxDist;
02385 getMinMaxDistQuantizeValue( dDist, &dMinDist, &dMaxDist, xDist1, xDist2 );
02386 dOutput = dOutput/dDist;
02387 double dMinCh = invQuantizeMin( dOutput, x1 ) ;
02388 double dMaxCh = invQuantizeMax( dOutput, x1 ) ;
02389 *dMin = min( dMinDist*dMinCh, dMaxDist*dMinCh );
02390 *dMax = max( dMinDist*dMaxCh, dMaxDist*dMaxCh );
02391 return true;
02392 }
02393
02399 double WorldModel::invQuantizeMin( double dOutput, double dQuantizeStep )
02400 {
02401
02402
02403 return max(1.0e-10,(rint( dOutput / dQuantizeStep )-0.5 )*dQuantizeStep);
02404 }
02405
02411 double WorldModel::invQuantizeMax( double dOutput, double dQuantizeStep )
02412 {
02413
02414
02415 return (rint( dOutput/dQuantizeStep) + 0.5 )*dQuantizeStep;
02416 }
02417
02429 void WorldModel::mapUnknownPlayers( Time time)
02430 {
02431 double dDist, dMinDist;
02432 VecPosition pos, posAgent = getAgentGlobalPosition();
02433 ObjectT o, o_new, objTmp;
02434 int index;
02435
02436
02437 for( int j = 0; j < iNrUnknownPlayers; j ++ )
02438 {
02439 pos = posAgent + VecPosition( UnknownPlayers[j].getRelativeDistance(),
02440 VecPosition::normalizeAngle( getAgentGlobalNeckAngle() +
02441 UnknownPlayers[j].getRelativeAngle() ),
02442 POLAR );
02443 dMinDist = 1000.0;
02444 o = UnknownPlayers[j].getType();
02445 o_new = OBJECT_ILLEGAL;
02446 Log.log( 464, "map unknown player: %d %f %f (%f,%f) neck %f",
02447 o,
02448 UnknownPlayers[j].getRelativeDistance(),
02449 UnknownPlayers[j].getRelativeAngle(),
02450 pos.getX(), pos.getY(),
02451 getAgentGlobalNeckAngle() );
02452 if( ! SoccerTypes::isOpponent( o ) )
02453 {
02454 for( int i = 0 ; i < MAX_TEAMMATES ; i++ )
02455 {
02456 objTmp = Teammates[i].getType();
02457 if( isConfidenceGood( objTmp ) && getTimeLastSeen( objTmp ) != time &&
02458 objTmp != getAgentObjectType() )
02459 {
02460 dDist = pos.getDistanceTo( Teammates[i].getGlobalPosition( ) );
02461 Log.log( 464, "distance with %d %f (%f,%f)", objTmp, dDist,
02462 Teammates[i].getRelativeDistance(),
02463 Teammates[i].getRelativeAngle() );
02464 if( dDist < dMinDist )
02465 {
02466 o_new = objTmp;
02467 dMinDist = dDist;
02468 }
02469 }
02470 else
02471 Log.log( 464, "not possible: distance with %d (%f,%f) conf %d \
02472 last_seen (%d,%d), now (%d,%d)",
02473 objTmp,
02474 Teammates[i].getRelativeDistance(),
02475 Teammates[i].getRelativeAngle(),
02476 isConfidenceGood( objTmp ),
02477 getTimeLastSeen( objTmp ).getTime(),
02478 getTimeLastSeen( objTmp ).getTimeStopped(),
02479 time.getTime(),
02480 time.getTimeStopped());
02481
02482 }
02483 }
02484 if( ! SoccerTypes::isTeammate( o ) )
02485 {
02486 for( int i = 0 ; i < MAX_OPPONENTS ; i++ )
02487 {
02488 objTmp = Opponents[i].getType();
02489 if( isConfidenceGood( objTmp ) && getTimeLastSeen( objTmp ) != time )
02490 {
02491 dDist = pos.getDistanceTo( Opponents[i].getGlobalPosition( ) );
02492 Log.log( 464, "distance with %d %f (%f,%f)", objTmp, dDist,
02493 Opponents[i].getRelativeDistance(),
02494 Opponents[i].getRelativeAngle() );
02495 if( dDist < dMinDist )
02496 {
02497 o_new = objTmp;
02498 dMinDist = dDist;
02499 }
02500 }
02501 else
02502 Log.log( 464, "not possible: distance with %d (%f,%f) conf %d \
02503 last_seen (%d,%d), now (%d,%d)",
02504 objTmp,
02505 Opponents[i].getRelativeDistance(),
02506 Opponents[i].getRelativeAngle(),
02507 isConfidenceGood( objTmp ),
02508 getTimeLastSeen( objTmp ).getTime(),
02509 getTimeLastSeen( objTmp ).getTimeStopped(),
02510 time.getTime(),
02511 time.getTimeStopped());
02512 }
02513 }
02514
02515 Log.log( 464, "closest obj %d, found %f, max_move %f rel_dist %f type %d",
02516 o_new, dMinDist, getMaxTraveledDistance( o_new ),
02517 UnknownPlayers[j].getRelativeDistance(),
02518 UnknownPlayers[j].getType() );
02519
02520
02521
02522
02523
02524
02525
02526 if( SoccerTypes::isKnownPlayer(o_new)
02527 && dMinDist < PS->getPlayerDistTolerance()
02528 && dMinDist < getMaxTraveledDistance( o_new ) + 2 )
02529 {
02530 UnknownPlayers[j].setType( o_new );
02531 if( SoccerTypes::isTeammate( o_new ) )
02532 {
02533 index = SoccerTypes::getIndex(o_new);
02534 UnknownPlayers[j].setHeteroPlayerType(
02535 Teammates[index].getHeteroPlayerType( ) );
02536 Teammates[index] = UnknownPlayers[j];
02537 Log.log( 464, "map to known teammate %d (%f,%f) conf %f", index + 1,
02538 Teammates[index].getGlobalPosition().getX(),
02539 Teammates[index].getGlobalPosition().getY(),
02540 Teammates[index].getConfidence( getCurrentTime() ) );
02541 }
02542 else if( SoccerTypes::isOpponent( o_new ) )
02543 {
02544 index = SoccerTypes::getIndex(o_new );
02545 UnknownPlayers[j].setHeteroPlayerType(
02546 Opponents[index].getHeteroPlayerType( ) );
02547 Opponents[index] = UnknownPlayers[j];
02548 Log.log( 464, "map to known opponent %d (time %d)",
02549 index + 1, UnknownPlayers[j].getTimeLastSeen().getTime() );
02550 }
02551 }
02552 else if( UnknownPlayers[j].getType() == OBJECT_TEAMMATE_UNKNOWN )
02553 {
02554 o_new = getFirstEmptySpotInSet( OBJECT_SET_TEAMMATES, j );
02555
02556 if( o_new != OBJECT_ILLEGAL )
02557 {
02558 index = SoccerTypes::getIndex(o_new);
02559 UnknownPlayers[j].setHeteroPlayerType(
02560 Teammates[index].getHeteroPlayerType( ) );
02561 UnknownPlayers[j].setType( o_new );
02562 Teammates[index] = UnknownPlayers[j];
02563 Log.log( 464, "map to unknown teammate %d", index + 1 );
02564 }
02565 }
02566 else if( UnknownPlayers[j].getType() == OBJECT_OPPONENT_UNKNOWN )
02567 {
02568 o_new = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS, j );
02569 Log.log( 464, "map unkown opponent to %d", o_new );
02570 if( o_new != OBJECT_ILLEGAL )
02571 {
02572 index = SoccerTypes::getIndex(o_new);
02573 UnknownPlayers[j].setHeteroPlayerType(
02574 Opponents[index].getHeteroPlayerType( ) );
02575 UnknownPlayers[j].setType( o_new );
02576 Opponents[index] = UnknownPlayers[j];
02577 Log.log( 464, "map to unknown opponent %d", index + 1 );
02578 }
02579 else
02580 {
02581 Log.log( 464, "couldn't find empty spot for unk. opponent" );
02582 if( Log.isInLogLevel( 464 ) )
02583 show( OBJECT_SET_PLAYERS, Log.getOutputStream() );
02584
02585
02586
02587 }
02588 }
02589 else if( UnknownPlayers[j].getType() == OBJECT_PLAYER_UNKNOWN &&
02590 ( UnknownPlayers[j].getRelativeDistance() < SS->getVisibleDistance()
02591 || ( dMinDist - getMaxTraveledDistance( o_new ) ) > 10 ) )
02592 {
02593 o_new = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
02594 Log.log( 464, "map unkown player to %d", o_new );
02595 if( o_new != OBJECT_ILLEGAL )
02596 {
02597 index = SoccerTypes::getIndex(o_new);
02598 UnknownPlayers[j].setHeteroPlayerType(
02599 Opponents[index].getHeteroPlayerType( ) );
02600 UnknownPlayers[j].setType( o_new );
02601 Opponents[index] = UnknownPlayers[j];
02602 Log.log( 464, "map to unknown close opponent %d", index + 1 );
02603 }
02604 }
02605 }
02606 Log.log( 464, "end map unknown player" );
02607
02608 iNrUnknownPlayers = 0;
02609 }
02610
02618 bool WorldModel::updateSSToHeteroPlayerType( int iIndex )
02619 {
02620 SS->setPlayerSpeedMax( pt[iIndex].dPlayerSpeedMax );
02621 SS->setStaminaIncMax ( pt[iIndex].dStaminaIncMax );
02622 SS->setPlayerDecay ( pt[iIndex].dPlayerDecay );
02623 SS->setInertiaMoment ( pt[iIndex].dInertiaMoment );
02624 SS->setDashPowerRate ( pt[iIndex].dDashPowerRate );
02625 SS->setPlayerSize ( pt[iIndex].dPlayerSize );
02626 SS->setKickableMargin( pt[iIndex].dKickableMargin );
02627 SS->setKickRand ( pt[iIndex].dKickRand );
02628 SS->setExtraStamina ( pt[iIndex].dExtraStamina );
02629 SS->setEffortMax ( pt[iIndex].dEffortMax );
02630 SS->setEffortMin ( pt[iIndex].dEffortMin );
02631
02632 return true;
02633 }
02634
02638 bool WorldModel::resetTimeObjects( )
02639 {
02640 Ball.setTimeLastSeen ( Time( -1, 0) );
02641 for( int i = 0 ; i < MAX_TEAMMATES ; i ++ )
02642 Teammates[i].setTimeLastSeen ( Time( -1, 0) );
02643 for( int i = 0 ; i < MAX_OPPONENTS ; i ++ )
02644 Opponents[i].setTimeLastSeen ( Time( -1, 0) );
02645 for( int i = 0 ; i < MAX_FLAGS ; i ++ )
02646 Flags[i].setTimeLastSeen ( Time( -1, 0) );
02647 for( int i = 0 ; i < MAX_LINES ; i ++ )
02648 Lines[i].setTimeLastSeen ( Time( -1, 0) );
02649 agentObject.setTimeLastSeen ( Time( -1, 0) );
02650 return true;
02651 }
02652
02656 void WorldModel::removeGhosts( )
02657 {
02658 AngDeg dAngle=SoccerTypes::getHalfViewAngleValue(agentObject.getViewAngle());
02659 dAngle -= 0.35*dAngle;
02660 VecPosition posAgent = getAgentGlobalPosition();
02661
02662 if( fabs( getRelativeAngle( OBJECT_BALL ) ) < dAngle
02663 && getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
02664 {
02665 double dDist;
02666 ObjectT objOpp=getClosestInSetTo(OBJECT_SET_OPPONENTS,OBJECT_BALL,&dDist);
02667
02668
02669 if( dDist < 2.0 &&
02670 posAgent.getDistanceTo( getGlobalPosition( objOpp ) ) < 4.5 &&
02671 getTimeLastSeen( objOpp ) != getTimeLastSeeMessage() )
02672 {
02673 Log.log( 556, "ball not seen, but opp close, set ball to that pos" );
02674 Ball.setGlobalPosition( getGlobalPosition( objOpp ),
02675 Ball.getTimeGlobalPosition() );
02676 }
02677 else
02678 {
02679 Log.log( 556, "ball not in cone: set time ball at -1 %f %f",
02680 fabs( getRelativeAngle( OBJECT_BALL ) ), dAngle );
02681 Ball.setTimeLastSeen( Time( -1, 0 ) );
02682 }
02683 }
02684
02685
02686 if( fabs( getRelativeDistance( OBJECT_BALL ) ) < 0.9*SS->getVisibleDistance()
02687 && getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
02688 {
02689 double dDist;
02690 ObjectT objOpp=getClosestInSetTo(OBJECT_SET_OPPONENTS,OBJECT_BALL,&dDist);
02691 if( dDist < 2.0 )
02692 {
02693 Log.log( 556, "ball not seen, but opp close, set ball to that pos" );
02694 Ball.setGlobalPosition( getGlobalPosition( objOpp ),
02695 Ball.getTimeGlobalPosition() );
02696 }
02697 else
02698 {
02699 Log.log( 556, "ball not in vis. dist: set time ball at -1 %f %f",
02700 fabs( getRelativeAngle( OBJECT_BALL ) ), dAngle );
02701 Ball.setTimeLastSeen( Time( -1, 0 ) );
02702 }
02703 }
02704
02705
02706
02707
02708
02709 int iIndex;
02710 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS );
02711 o != OBJECT_ILLEGAL;
02712 o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS ) )
02713 {
02714 if( fabs( getRelativeAngle( o ) ) < dAngle
02715
02716 && getTimeLastSeen( o ) != getTimeLastSeeMessage() &&
02717 o != getOppGoalieType() )
02718 {
02719 Log.log( 556, "opponent %d not in cone: (angle %f>%f) see (%d,%d)",
02720 SoccerTypes::getIndex( o ) + 1,
02721 fabs( getRelativeAngle( o ) ), dAngle,
02722 getTimeLastSeeMessage().getTime(),getTimeLastSeen(o).getTime());
02723
02724 AngDeg ang =
02725 SoccerTypes::getHalfViewAngleValue(agentObject.getViewAngle());
02726 AngDeg angOpp = (getGlobalPosition(o)-posAgent).getDirection();
02727 AngDeg angNeck = getAgentGlobalNeckAngle();
02728
02729 if( fabs(VecPosition::normalizeAngle( angOpp - angNeck )) < 10 &&
02730 getCurrentTime() - getTimeGlobalAngles(o) < 4 )
02731 angOpp = (getGlobalPosition(o)+
02732 VecPosition(2.0, getGlobalBodyAngle(o), POLAR )-posAgent)
02733 .getDirection();
02734 if( VecPosition::normalizeAngle( angOpp - angNeck ) > 0 )
02735 angOpp = angNeck + ang;
02736 else
02737 angOpp = angNeck - ang;
02738 angOpp = VecPosition::normalizeAngle( angOpp );
02739 DynamicObject *obj = (DynamicObject*) getObjectPtrFromType( o );
02740
02741
02742 if( fabs( getRelativeAngle( o ) ) + 20 > dAngle )
02743 {
02744 VecPosition posNew = posAgent+
02745 VecPosition(getRelativeDistance(o),angOpp, POLAR);
02746 if( posNew.getDistanceTo( getGlobalPosition( o ) ) <
02747 getMaxTraveledDistance( o ) )
02748 {
02749 obj->setGlobalPosition( posNew, getCurrentTime());
02750
02751 Log.log( 556, "set opp at angle %f", angOpp );
02752 updateObjectRelativeFromGlobal( o );
02753 }
02754 else
02755 Log.log( 556, "do not reposition opp too far dist %f",
02756 posNew.getDistanceTo( getGlobalPosition( o ) ));
02757 }
02758 else
02759 {
02760 setTimeLastSeen( o, Time( -1, 0 ) );
02761 Log.log( 556, "remove opponent not in cone at angle %f %f %f", angOpp,
02762 fabs(getRelativeAngle(o)), dAngle );
02763 }
02764 }
02765 if( getRelativeDistance(o) < SS->getVisibleDistance( )
02766 && getTimeLastSeen( o ) != getTimeLastSeeMessage() )
02767 {
02768 Log.log( 556, "opp %d not felt, place him outside vis. dist", o );
02769 DynamicObject *obj = (DynamicObject*) getObjectPtrFromType( o );
02770 VecPosition posNew = posAgent + VecPosition( SS->getVisibleDistance(),
02771 (getGlobalPosition( o ) - getAgentGlobalPosition()).
02772 getDirection(), POLAR );
02773 obj->setGlobalPosition( posNew, getCurrentTime( ));
02774
02775 updateObjectRelativeFromGlobal( o );
02776 }
02777 }
02778 iterateObjectDone( iIndex );
02779
02780
02781
02782
02783
02784 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_TEAMMATES );
02785 o != OBJECT_ILLEGAL;
02786 o = iterateObjectNext ( iIndex, OBJECT_SET_TEAMMATES ) )
02787 {
02788 if( fabs( getRelativeAngle( o ) ) < dAngle
02789
02790 && getTimeLastSeen( o ) != getTimeLastSeeMessage() )
02791 {
02792 Log.log( 556, "teammate %d not in cone: (angle %f>%f) see (%d,%d)",
02793 SoccerTypes::getIndex( o ) + 1,
02794 fabs( getRelativeAngle( o ) ), dAngle,
02795 getTimeLastSeeMessage().getTime(), getTimeLastSeen( o ).getTime() );
02796 setTimeLastSeen( o, Time( -1, 0 ) );
02797 }
02798 }
02799 iterateObjectDone( iIndex );
02800
02801 }
02802
02812 void WorldModel::initParticlesBall( VecPosition posArray[],
02813 VecPosition velArray[], int iLength )
02814 {
02815
02816 double dDistBall, dDistChange = UnknownDoubleValue;
02817 AngDeg angBall, angChange = UnknownAngleValue;
02818 double dMinDist, dMaxDist, dMinCh, dMaxCh, dDistTmp, dDistChTmp, dVelX,dVelY;
02819 AngDeg angChMin, angChMax, angTmp, angChTmp;
02820
02821
02822 if( Ball.getTimeRelativePosition() != getTimeLastSeeMessage() )
02823 return;
02824
02825
02826 dDistBall = getRelativeDistance( OBJECT_BALL );
02827 angBall = getRelativeAngle( OBJECT_BALL );
02828
02829
02830 if( Ball.getTimeChangeInformation( ) == getTimeLastSeeMessage() )
02831 {
02832 dDistChange = Ball.getRelativeDistanceChange();
02833 angChange = Ball.getRelativeAngleChange();
02834 }
02835
02836
02837 getMinMaxDistQuantizeValue( dDistBall, &dMinDist, &dMaxDist, 0.1, 0.1 );
02838 getMinMaxDistChange( dDistChange, dDistBall, &dMinCh, &dMaxCh, 0.02, 0.1,0.1);
02839 getMinMaxDirChange ( angChange, &angChMin, &angChMax, 0.1 );
02840
02841 for( int i = 0; i < iLength; i ++ )
02842 {
02843
02844
02845 dDistTmp = dMinDist + drand48()*fabs(dMaxDist - dMinDist);
02846 angTmp = angBall + drand48() - 0.5;
02847
02848 posArray[i].setVecPosition( dDistTmp, angTmp, POLAR );
02849 posArray[i].relativeToGlobal( getAgentGlobalPosition(),
02850 getAgentGlobalNeckAngle() );
02851
02852 if( dDistChange != UnknownDoubleValue )
02853 {
02854
02855 angChTmp = angChMin + drand48()*(angChMax-angChMin);
02856 dDistChTmp = dMinCh + drand48()*(dMaxCh-dMinCh);
02857
02858 dVelX=dDistChTmp*
02859 cosDeg(angTmp)-Deg2Rad(angChTmp)*dDistTmp*sinDeg(angTmp);
02860 dVelY=dDistChTmp*
02861 sinDeg(angTmp)+Deg2Rad(angChTmp)*dDistTmp*cosDeg(angTmp);
02862
02863 velArray[i].setVecPosition( dVelX, dVelY );
02864 velArray[i].relativeToGlobal( getAgentGlobalVelocity(),
02865 getAgentGlobalNeckAngle() );
02866 }
02867 else
02868 velArray[i].setVecPosition( 0, 0 );
02869 }
02870 }
02871
02882 void WorldModel::checkParticlesBall( VecPosition posArray[],
02883 VecPosition velArray[], int iLength, int *iNrParticlesLeft )
02884 {
02885 bool bIllegal;
02886 double dMinDist, dMaxDist, dMinCh, dMaxCh, dMag;
02887 double dDistBall, dDistChange = UnknownDoubleValue;
02888 AngDeg angBall, angChange;
02889 double dDistChTmp;
02890 AngDeg angChTmp, angChMin, angChMax;
02891 VecPosition pos_rel, vel_rel;
02892
02893
02894 if( getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
02895 return;
02896
02897
02898
02899 dDistBall = getRelativeDistance( OBJECT_BALL );
02900 angBall = getRelativeAngle( OBJECT_BALL );
02901 getMinMaxDistQuantizeValue( dDistBall, &dMinDist, &dMaxDist, 0.1, 0.1 );
02902
02903 if( getTimeLastSeen( OBJECT_BALL ) == Ball.getTimeChangeInformation( ) )
02904 {
02905 dDistChange = Ball.getRelativeDistanceChange();
02906 angChange = Ball.getRelativeAngleChange();
02907 getMinMaxDirChange ( angChange, &angChMin, &angChMax, 0.1);
02908 getMinMaxDistChange( dDistChange,dDistBall, &dMinCh, &dMaxCh,0.02,0.1,0.1);
02909
02910 }
02911
02912 *iNrParticlesLeft = 0;
02913 for( int i = 0; i < iLength; i ++ )
02914 {
02915
02916 pos_rel = posArray[i];
02917 vel_rel = velArray[i];
02918 pos_rel.globalToRelative( getAgentGlobalPosition(),
02919 getAgentGlobalNeckAngle() );
02920 vel_rel.globalToRelative( getAgentGlobalVelocity(),
02921 getAgentGlobalNeckAngle() );
02922
02923 bIllegal = false;
02924
02925 dMag = pos_rel.getMagnitude();
02926 if( dMag < dMinDist || dMag > dMaxDist )
02927 {
02928 bIllegal = true;
02929 }
02930 if( fabs( VecPosition::normalizeAngle(pos_rel.getDirection() - angBall) )
02931 > 0.5 )
02932 {
02933 bIllegal = true;
02934 }
02935
02936 if( dDistChange != UnknownDoubleValue )
02937 {
02938 dDistChTmp = (vel_rel.getX()*(pos_rel.getX()/dMag)) +
02939 (vel_rel.getY()*(pos_rel.getY()/dMag));
02940 angChTmp = Rad2Deg( ((vel_rel.getY()*pos_rel.getX()/dMag) -
02941 (vel_rel.getX()*pos_rel.getY()/dMag)))/dMag ;
02942
02943 if( angChTmp < angChMin || angChTmp > angChMax )
02944 {
02945 bIllegal = true;
02946 }
02947 if( dDistChTmp < dMinCh || dDistChTmp > dMaxCh )
02948 {
02949 bIllegal = true;
02950 }
02951 }
02952
02953
02954 if( bIllegal == false )
02955 {
02956 posArray[*iNrParticlesLeft] = posArray[i];
02957 velArray[(*iNrParticlesLeft)++] = velArray[i];
02958 }
02959 }
02960 }
02961
02970 void WorldModel::updateParticlesBall( VecPosition posArray[],
02971 VecPosition velArray[], int iLength, double dPower, AngDeg ang )
02972 {
02973 double dRand = SS->getBallRand();
02974 double dMaxRand;
02975
02976 for( int i = 0; i < iLength; i ++ )
02977 {
02978
02979 if( dPower > EPSILON )
02980 {
02981 ang = VecPosition::normalizeAngle(ang + getAgentGlobalBodyAngle() );
02982 velArray[i] += VecPosition(getActualKickPowerRate()*dPower, ang, POLAR) ;
02983 if( velArray[i].getMagnitude() > SS->getBallSpeedMax() )
02984 velArray[i].setMagnitude( SS->getBallSpeedMax() );
02985 }
02986
02987
02988 dMaxRand = dRand * velArray[i].getMagnitude();
02989 velArray[i] += VecPosition(
02990 (-1 + 2*drand48())*dMaxRand,
02991 (-1 + 2*drand48())*dMaxRand );
02992 posArray[i] += velArray[i];
02993 velArray[i] *= SS->getBallDecay();
02994 }
02995 }
02996
02997
03007 void WorldModel::resampleParticlesBall( VecPosition posArray[],
03008 VecPosition velArray[], int iLength, int iLeft )
03009 {
03010 int iRand ;
03011 for ( int i = iLeft; i < iLength; i ++ )
03012 {
03013 iRand = (int)(drand48()*iLeft);
03014 posArray[ i ] = posArray[ iRand ];
03015 velArray[ i ] = velArray[ iRand ];
03016 }
03017 }
03018
03019 ObjectT WorldModel::getMaxRangeUnknownPlayer( ObjectT obj, char* strMsg )
03020 {
03021 list<ObjectT> l;
03022 ObjectT o;
03023 bool isGoalie, bCont = true;
03024 int i, loop;
03025
03026 ObjectT objMax = (getSide()==SIDE_LEFT) ? OBJECT_OPPONENT_11
03027 : OBJECT_TEAMMATE_11 ;
03028
03029 while( bCont == true )
03030 {
03031 i = Parse::gotoFirstOccurenceOf( '(', &strMsg );
03032 if( i == -1 )
03033 bCont = false;
03034 else
03035 {
03036 strMsg++;
03037 o = SoccerTypes::getObjectFromStr(&strMsg,&isGoalie,getTeamName());
03038 if( SoccerTypes::isPlayer( o ) )
03039 l.push_back( o );
03040 }
03041 }
03042 Log.log( 459, "list size %d", l.size() );
03043 while( ! l.empty() )
03044 {
03045 o = l.back();
03046 l.pop_back();
03047 if( SoccerTypes::isKnownPlayer( o ) )
03048 objMax = o;
03049 else if( SoccerTypes::isOpponent( o ) &&
03050 SoccerTypes::isTeammate( objMax ) )
03051 objMax = OBJECT_OPPONENT_11;
03052 else if( SoccerTypes::isTeammate( o ) &&
03053 SoccerTypes::isOpponent( objMax ) )
03054 objMax = OBJECT_TEAMMATE_11;
03055
03056 if( objMax == getAgentObjectType() )
03057 loop = 2;
03058 else
03059 loop = 1;
03060
03061 for( int j = 0; j < loop; j ++ )
03062 {
03063 i = SoccerTypes::getIndex( objMax );
03064 if( objMax == OBJECT_TEAMMATE_1 )
03065 objMax = OBJECT_OPPONENT_11;
03066 else if( objMax == OBJECT_OPPONENT_1 )
03067 objMax = OBJECT_TEAMMATE_11;
03068 else if( SoccerTypes::isTeammate( objMax ) )
03069 objMax = SoccerTypes::getTeammateObjectFromIndex( i - 1 );
03070 else if( SoccerTypes::isOpponent( objMax ) )
03071 objMax = SoccerTypes::getOpponentObjectFromIndex( i - 1 );
03072 }
03073 Log.log( 459, "processs %d new obj_max: %d", o, objMax );
03074
03075 }
03076
03077 return objMax;
03078 }