00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00047 #include "WorldModel.h"
00048 #include "Parse.h"
00049 #include <stdio.h>
00050 #include <list>
00051
00052
00053
00054
00055
00056
00068 void WorldModel::processSeeGlobalInfo( ObjectT o, Time time,
00069 VecPosition pos, VecPosition vel, AngDeg angBody, AngDeg angNeck)
00070 {
00071 DynamicObject * dobj;
00072 PlayerObject * pobj;
00073
00074 if( o == OBJECT_ILLEGAL )
00075 return;
00076 if( SoccerTypes::isPlayer( o ) )
00077 {
00078 pobj = (PlayerObject*)getObjectPtrFromType( o );
00079 pobj->setTimeLastSeen( time );
00080 pobj->setGlobalPosition( pos, time );
00081 pobj->setGlobalVelocity( vel, time );
00082 pobj->setGlobalBodyAngle( angBody, time );
00083 pobj->setGlobalNeckAngle( VecPosition::normalizeAngle(angBody+angNeck),
00084 time );
00085 pobj->setIsKnownPlayer( true );
00086 }
00087 else if( SoccerTypes::isBall( o ) )
00088 {
00089 dobj = (DynamicObject*)getObjectPtrFromType( o );
00090 dobj->setTimeLastSeen( time );
00091 dobj->setGlobalPosition( pos, time );
00092 dobj->setGlobalVelocity( vel, time );
00093 }
00094 }
00095
00107 bool WorldModel::processNewAgentInfo( ViewQualityT vq, ViewAngleT va,
00108 double dStamina, double dEffort, double dSpeed, AngDeg angSpeed,
00109 AngDeg angHeadAngle )
00110 {
00111 Stamina sta = agentObject.getStamina();
00112
00113 sta.setStamina ( dStamina );
00114 sta.setEffort ( dEffort );
00115 agentObject.setStamina ( sta );
00116 agentObject.setViewQuality ( vq );
00117 agentObject.setViewAngle ( va );
00118 agentObject.setSpeedRelToNeck ( VecPosition( dSpeed, angSpeed, POLAR) );
00119 agentObject.setBodyAngleRelToNeck( angHeadAngle );
00120
00121 return true;
00122 }
00123
00143 void WorldModel::processNewObjectInfo( ObjectT o, Time time,
00144 double dDist, int iDir, double dDistChange, double dDirChange,
00145 AngDeg angRelBodyAng, AngDeg angRelNeckAng, bool isGoalie,
00146 ObjectT objMin, ObjectT objMax )
00147 {
00148 if( dDist == UnknownDoubleValue || o == OBJECT_ILLEGAL )
00149 return;
00150
00151 if( SoccerTypes::isFlag( o ) )
00152 {
00153 Flags[SoccerTypes::getIndex(o)].setRelativePosition(dDist,(double)iDir,time);
00154 Flags[SoccerTypes::getIndex(o)].setTimeLastSeen ( time );
00155 Flags[SoccerTypes::getIndex(o)].setType ( o );
00156 }
00157 else if( SoccerTypes::isPlayer( o ) || SoccerTypes::isBall( o ) )
00158 {
00159 DynamicObject *d = NULL;
00160
00161
00162 if( !( SoccerTypes::isKnownPlayer( o ) || SoccerTypes::isBall( o ) ) )
00163 {
00164 UnknownPlayers[iNrUnknownPlayers].setIsKnownPlayer( false );
00165 UnknownPlayers[iNrUnknownPlayers].setPossibleRange( objMin, objMax );
00166 d = &UnknownPlayers[iNrUnknownPlayers];
00167 iNrUnknownPlayers++;
00168 Log.log( 459, "range %f: %d (%d)-%d (%d)",
00169 dDist, objMin, SoccerTypes::getIndex(objMin)+1,
00170 objMax, SoccerTypes::getIndex(objMax)+1 );
00171 }
00172 else
00173 {
00174 d = (DynamicObject*)getObjectPtrFromType( o );
00175 if( SoccerTypes::isPlayer( o ) )
00176 ((PlayerObject*)d)->setIsKnownPlayer( true );
00177 }
00178
00179 if( d != NULL )
00180 {
00181
00182 d->setRelativePosition( dDist, (double)iDir, time );
00183 if( dDistChange != UnknownDoubleValue )
00184 d->setRelativeDistanceChange( dDistChange, time );
00185 if( dDirChange != UnknownDoubleValue )
00186 d->setRelativeAngleChange( dDirChange, time );
00187 if( angRelBodyAng != UnknownAngleValue )
00188 ((PlayerObject*)d)->setRelativeBodyAngle( angRelBodyAng, time );
00189 if( angRelNeckAng != UnknownAngleValue )
00190 ((PlayerObject*)d)->setRelativeNeckAngle( angRelNeckAng, time );
00191 if( isGoalie == true && SoccerTypes::isPlayer( o ))
00192 ((PlayerObject*)d)->setIsGoalie( true );
00193 else if( SoccerTypes::isPlayer( o ))
00194 ((PlayerObject*)d)->setIsGoalie( false );
00195 d->setType( o );
00196 d->setTimeLastSeen( time );
00197
00198
00199
00200 if( SoccerTypes::isPlayer( o ) && SoccerTypes::isKnownPlayer( o ) )
00201 {
00202 int iIndex;
00203 ObjectT objMin = OBJECT_ILLEGAL;
00204 double dMinDist = 1000.0, dTmp;
00205 ObjectSetT set = SoccerTypes::isOpponent( o )
00206 ? OBJECT_SET_OPPONENTS
00207 : OBJECT_SET_TEAMMATES;
00208
00209 for( ObjectT obj = iterateObjectStart( iIndex, set );
00210 obj != OBJECT_ILLEGAL;
00211 obj = iterateObjectNext ( iIndex, set ) )
00212 {
00213 if( obj != getAgentObjectType() && isKnownPlayer( obj ) == false )
00214 {
00215 dTmp=(getRelativePosition(obj)-d->getRelativePosition()).getMagnitude();
00216 if( dTmp < dMinDist )
00217 {
00218 objMin = obj;
00219 dMinDist = dTmp;
00220 }
00221 }
00222 }
00223 iterateObjectDone( iIndex );
00224 if( objMin != OBJECT_ILLEGAL &&
00225 dMinDist < PS->getPlayerDistTolerance() )
00226 {
00227 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( objMin );
00228 pob->setTimeLastSeen( -1 );
00229 }
00230 }
00231 }
00232 }
00233 else if( SoccerTypes::isLine( o ) )
00234 {
00235
00236
00237 iDir = ( iDir < 0 ) ? (90 + iDir ) : - (90 - iDir );
00238 Lines[SoccerTypes::getIndex(o)].setRelativePosition(dDist,(double)iDir,time);
00239 Lines[SoccerTypes::getIndex(o)].setTimeLastSeen( time );
00240 Lines[SoccerTypes::getIndex(o)].setType( o );
00241 }
00242 }
00243
00251 bool WorldModel::storePlayerMessage( int iPlayer, char *strMsg, int iCycle )
00252 {
00253 strcpy( m_strPlayerMsg, strMsg );
00254 m_iCycleInMsg = iCycle;
00255 m_timePlayerMsg = getCurrentTime();
00256 m_iMessageSender = iPlayer;
00257 return true;
00258 }
00259
00261 bool WorldModel::processPlayerMessage( )
00262 {
00263 static char strMessage[MAX_MSG];
00264 int iDiff = getCurrentCycle() - m_iCycleInMsg;
00265 double dDiff = (double)iDiff/100.0;
00266 char *strMsg;
00267 strcpy( strMessage, m_strPlayerMsg );
00268 strMsg = strMessage;
00269
00270
00271 double dBallX = (int)(strMsg[0]-'0')*10 + (int)(strMsg[1] - '0' ) - 52.5;
00272 double dBallY = (int)(strMsg[2]-'0')*10 + (int)(strMsg[3] - '0' ) - 34.0;
00273 double dBallVelX = (int)(strMsg[4]-'0') + (int)(strMsg[5] - '0' )/10.0 - 2.7;
00274 double dBallVelY = (int)(strMsg[6]-'0') + (int)(strMsg[7] - '0' )/10.0 - 2.7;
00275
00276 VecPosition pos( dBallX, dBallY );
00277 VecPosition vel( dBallVelX, dBallVelY );
00278
00279
00280 if( getTimeChangeInformation( OBJECT_BALL ) < getCurrentTime() - 3 &&
00281 getRelativeDistance( OBJECT_BALL ) > SS->getVisibleDistance() )
00282 processPerfectHearInfoBall( pos, vel, 1.00 - dDiff ) ;
00283
00284 return true;
00285 }
00286
00294 bool WorldModel::processPerfectHearInfoBall( VecPosition posGlobal,
00295 VecPosition vel, double dConf )
00296 {
00297 if( Ball.getConfidence( getCurrentTime() ) < dConf )
00298 {
00299 Time time = getTimeFromConfidence( dConf );
00300 Ball.setGlobalPosition( posGlobal, time );
00301 Ball.setGlobalVelocity( vel, time );
00302 Ball.setTimeLastSeen ( time );
00303 updateObjectRelativeFromGlobal( OBJECT_BALL );
00304 return true;
00305 }
00306 return false;
00307 }
00308
00320 bool WorldModel::processPerfectHearInfo( ObjectT o, VecPosition posGlobal,
00321 double dConf, bool bIsGoalie )
00322 {
00323 if( SoccerTypes::isBall( o ) || o == getAgentObjectType() )
00324 return false;
00325 else if( !SoccerTypes::isKnownPlayer( o ) )
00326 return processUnsureHearInfo( o, posGlobal, dConf );
00327
00328 PlayerObject *object = (PlayerObject *)getObjectPtrFromType( o );
00329 if( object == NULL )
00330 return false;
00331
00332 Time time = getTimeFromConfidence( dConf ) ;
00333
00334
00335
00336
00337
00338
00339 if( object->getConfidence( getCurrentTime() ) < dConf ||
00340 object->getIsKnownPlayer() == false )
00341 {
00342 object->setGlobalPosition ( posGlobal , time );
00343 object->setTimeLastSeen ( time );
00344 object->setGlobalVelocity ( VecPosition( 0, 0), time );
00345 object->setIsKnownPlayer ( true );
00346 object->setIsGoalie ( bIsGoalie );
00347 updateObjectRelativeFromGlobal( o );
00348 return true;
00349 }
00350 return false;
00351 }
00352
00364 bool WorldModel::processUnsureHearInfo( ObjectT o, VecPosition pos,
00365 double dConf )
00366 {
00367 double dMinDist;
00368 ObjectT objInitial = o;
00369
00370 if( o != OBJECT_TEAMMATE_UNKNOWN && o != OBJECT_OPPONENT_UNKNOWN )
00371 return false;
00372
00373
00374 if( SoccerTypes::isTeammate( o ) )
00375 o = getClosestInSetTo( OBJECT_SET_TEAMMATES, pos, &dMinDist);
00376 else if( SoccerTypes::isOpponent( o ) )
00377 o = getClosestInSetTo( OBJECT_SET_OPPONENTS, pos, &dMinDist);
00378
00379 if( o == getAgentObjectType() &&
00380 pos.getDistanceTo(getAgentGlobalPosition())<PS->getPlayerDistTolerance())
00381 return false;
00382
00383
00384
00385
00386
00387 else if( SoccerTypes::isKnownPlayer(o) &&
00388 dMinDist < PS->getPlayerDistTolerance())
00389 {
00390 processPerfectHearInfo( o, pos, dConf );
00391 return true;
00392 }
00393
00394 if( objInitial == OBJECT_TEAMMATE_UNKNOWN )
00395 o = getFirstEmptySpotInSet( OBJECT_SET_TEAMMATES );
00396 else if( objInitial == OBJECT_OPPONENT_UNKNOWN )
00397 o = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
00398 else
00399 return false ;
00400
00401 if( o != OBJECT_ILLEGAL )
00402 {
00403 processPerfectHearInfo( o, pos, dConf );
00404 setIsKnownPlayer( o, false );
00405 }
00406 return true;
00407 }
00408
00429 bool WorldModel::processNewHeteroPlayer( int iIndex, double dPlayerSpeedMax,
00430 double dStaminaIncMax, double dPlayerDecay, double dInertiaMoment,
00431 double dDashPowerRate, double dPlayerSize, double dKickableMargin,
00432 double dKickRand, double dExtraStamina, double dEffortMax,
00433 double dEffortMin )
00434 {
00435 pt[iIndex].dPlayerSpeedMax = dPlayerSpeedMax;
00436 pt[iIndex].dStaminaIncMax = dStaminaIncMax;
00437 pt[iIndex].dPlayerDecay = dPlayerDecay;
00438 pt[iIndex].dInertiaMoment = dInertiaMoment;
00439 pt[iIndex].dDashPowerRate = dDashPowerRate;
00440 pt[iIndex].dPlayerSize = dPlayerSize;
00441 pt[iIndex].dKickableMargin = dKickableMargin;
00442 pt[iIndex].dKickRand = dKickRand;
00443 pt[iIndex].dExtraStamina = dExtraStamina;
00444 pt[iIndex].dEffortMax = dEffortMax;
00445 pt[iIndex].dEffortMin = dEffortMin;
00446 return true;
00447 }
00448
00457 void WorldModel::processCatchedBall( RefereeMessageT rm, Time time )
00458 {
00459 if( rm == REFC_GOALIE_CATCH_BALL_LEFT && sideSide == SIDE_LEFT )
00460 timeLastCatch = time;
00461 else if( rm == REFC_GOALIE_CATCH_BALL_RIGHT && sideSide == SIDE_RIGHT )
00462 timeLastCatch = time;
00463 Ball.setGlobalVelocity( VecPosition(0,0), getCurrentTime() );
00464 }
00465
00474 void WorldModel::processQueuedCommands( SoccerCommand commands[],int iCommands )
00475 {
00476 if( iCommands > MAX_COMMANDS )
00477 {
00478 cerr << "(WorldModel::setQueuedCommands) queuedCommands array cannot "
00479 << "contain so many commands!\n";
00480 return;
00481 }
00482
00483
00484 for( int i = 0 ; i < iCommands ; i ++ )
00485 {
00486 commands[i].time = getCurrentTime();
00487 queuedCommands[(int)commands[i].commandType] = commands[i];
00488 }
00489 }
00490
00498 bool WorldModel::updateAll( )
00499 {
00500 bool bReturn = false, bUpdateAfterSee = false;
00501 bool bUpdateAfterSense = false;
00502 static Time timeLastHoleRecorded;
00503 static Time timeBeginInterval;
00504 static Time timePlayersCounted;
00505 static int iNrHolesLastTime = 0;
00506 static Time timeLastSenseUpdate;
00507 static Time timeLastSeeUpdate;
00508 static Time timeLastSayUpdate;
00509
00510
00511 if( agentObject.getTimeGlobalPosition() < getCurrentTime() - 1 )
00512 Log.log( 3, "(WorldModel::updateAfterSenseMessage) missed a sense??" );
00513
00514 if( isQueuedActionPerformed() == false &&
00515 timeLastHoleRecorded != getCurrentTime() )
00516 {
00517 Log.log( 2, "HOLE recorded" );
00518 timeLastHoleRecorded = getCurrentTime();
00519 iNrHoles++;
00520 }
00521
00522
00523 if( getTimeLastSeeMessage() > timeLastSeeUpdate )
00524 bUpdateAfterSee = true;
00525 if( getTimeLastSenseMessage() > timeLastSenseUpdate )
00526 bUpdateAfterSense = true;
00527
00528
00529
00530 if( bUpdateAfterSee && bUpdateAfterSense )
00531 {
00532 Log.log( 3, "!!! Two updates !!! " );
00533 Log.log( 3, "see: %d sense: %d", getTimeLastSeeMessage().getTime(),
00534 getTimeLastSenseMessage().getTime() );
00535 if( getTimeLastSeeMessage( ) == getTimeLastSenseMessage() )
00536 {
00537 bReturn = updateAfterSenseMessage( );
00538 bReturn &= updateAfterSeeMessage( );
00539 }
00540 else if( getTimeLastSeeMessage( ) < getTimeLastSenseMessage() )
00541 {
00542 bReturn = updateAfterSeeMessage( );
00543 bReturn &= updateAfterSenseMessage( );
00544 updateRelativeFromGlobal();
00545 }
00546
00547 timeLastSenseUpdate = getTimeLastSenseMessage();
00548 timeLastSeeUpdate = getTimeLastSeeMessage();
00549 }
00550 else if( bUpdateAfterSee )
00551 {
00552 bReturn = updateAfterSeeMessage( );
00553 timeLastSeeUpdate = getTimeLastSeeMessage();
00554 }
00555 else if( bUpdateAfterSense )
00556 {
00557 bReturn = updateAfterSenseMessage( );
00558 timeLastSenseUpdate = getTimeLastSenseMessage();
00559 updateRelativeFromGlobal();
00560 }
00561
00562 if( timeLastSayUpdate != m_timePlayerMsg )
00563 {
00564 timeLastSayUpdate = m_timePlayerMsg;
00565 processPlayerMessage();
00566 }
00567
00568
00569 int iTimeDiff = getCurrentTime() - timeBeginInterval;
00570 double dHolePerc = (double)(iNrHoles - iNrHolesLastTime)/iTimeDiff*100;
00571 if( ! isLastMessageSee( ) && iTimeDiff % 400 == 0 && dHolePerc > 1.0 &&
00572 PS->getFractionWaitSeeEnd() > 0.70 )
00573 {
00574 PS->setFractionWaitSeeEnd( PS->getFractionWaitSeeEnd() - 0.05 );
00575 timeBeginInterval = getCurrentTime();
00576 cerr << "Lowered send time to " << PS->getFractionWaitSeeEnd() <<
00577 " for player number " << getPlayerNumber() <<
00578 "; nr of holes is "<< dHolePerc << "%" << endl;
00579 iNrHolesLastTime = iNrHoles;
00580 }
00581
00582
00583 if( bUpdateAfterSense == true && ! isTimeStopped() &&
00584 getCurrentTime() != timePlayersCounted )
00585 {
00586 iNrTeammatesSeen += getNrInSetInRectangle( OBJECT_SET_TEAMMATES );
00587 iNrOpponentsSeen += getNrInSetInRectangle( OBJECT_SET_OPPONENTS );
00588 timePlayersCounted = getCurrentTime();
00589 }
00590
00591
00592 if( Log.isInLogLevel( 456 ) )
00593 logObjectInformation( 456, getAgentObjectType() );
00594
00595 if( bUpdateAfterSee == true )
00596 Log.logWithTime(3, " finished update_all see; start determining action" );
00597 if( bUpdateAfterSense == true )
00598 Log.logWithTime(3, " finished update_all sense; start determining action");
00599
00600 if( Log.isInLogLevel( 459 ) )
00601 {
00602 Log.log( 459, "%s%s", strLastSeeMessage, strLastSenseMessage );
00603 show( OBJECT_BALL, Log.getOutputStream() );
00604 show( OBJECT_SET_PLAYERS, Log.getOutputStream() );
00605 }
00606 if( ( Log.isInLogLevel( 101 ) && getRelativeDistance( OBJECT_BALL ) < 2.0 ) )
00607 show( OBJECT_BALL, Log.getOutputStream() );
00608 if( Log.isInLogLevel( 556 ) )
00609 {
00610 Log.log( 556, "%s", strLastSeeMessage );
00611 show( OBJECT_BALL, Log.getOutputStream() );
00612 }
00613
00614 return bReturn;
00615 }
00616
00617
00618
00619
00620
00623 void WorldModel::processLastSeeMessage( )
00624 {
00625 ObjectT o;
00626 double dDist, dDistChange, dDirChange, dTemp;
00627 int iDir, iBodyFacingDir, iHeadFacingDir, iTime = UnknownTime;
00628 Time time = getTimeLastSenseMessage();
00629 bool isGoalie;
00630 static char strTmp[MAX_MSG];
00631 char *strMsg = strLastSeeMessage ;
00632 iTime = Parse::parseFirstInt( &strMsg );
00633
00634 ObjectT objTeamMin = OBJECT_ILLEGAL;
00635 ObjectT objMax = OBJECT_ILLEGAL;
00636 ObjectT objOppMin = OBJECT_ILLEGAL;
00637 ObjectT objMin = OBJECT_ILLEGAL;
00638
00639 while( *strMsg != ')' )
00640 {
00641 dDist = dDistChange = dDirChange = dTemp = UnknownDoubleValue;
00642 iDir = iBodyFacingDir = iHeadFacingDir = UnknownIntValue;
00643 strMsg += 2;
00644
00645
00646 o = SoccerTypes::getObjectFromStr( &strMsg, &isGoalie, getTeamName() );
00647
00648 if( o == OBJECT_ILLEGAL )
00649 {
00650 Log.log( 4, "Illegal object in: ", strLastSeeMessage );
00651 Log.log( 4, "rest of message: ", strMsg );
00652 }
00653 else if( SoccerTypes::isKnownPlayer( o ) )
00654 {
00655 if( SoccerTypes::isTeammate( o ) )
00656 objTeamMin = o;
00657 else
00658 objOppMin = o;
00659 }
00660 else if( SoccerTypes::isPlayer( o ) )
00661 {
00662 if( SoccerTypes::isTeammate( o ) )
00663 {
00664 if( objTeamMin == OBJECT_ILLEGAL )
00665 objTeamMin = OBJECT_TEAMMATE_1;
00666 else
00667 objTeamMin = (ObjectT)((int)objTeamMin + 1);
00668 }
00669 else if( SoccerTypes::isOpponent( o ) )
00670 {
00671 if( objOppMin == OBJECT_ILLEGAL )
00672 objOppMin = OBJECT_OPPONENT_1;
00673 else
00674 objOppMin = (ObjectT)((int)objOppMin + 1);
00675 }
00676 strcpy( strTmp, strMsg );
00677 objMax = getMaxRangeUnknownPlayer( o, strTmp );
00678 }
00679
00680 dTemp = Parse::parseFirstDouble( &strMsg );
00681 if ( *strMsg == ')' )
00682 iDir = (int)dTemp;
00683 else
00684 {
00685 dDist = dTemp;
00686 iDir = Parse::parseFirstInt( &strMsg );
00687 if( *strMsg != ')' )
00688 {
00689 dDistChange = Parse::parseFirstDouble( &strMsg );
00690 dDirChange = Parse::parseFirstDouble( &strMsg );
00691 }
00692 if( *strMsg != ')' )
00693 {
00694 iBodyFacingDir = Parse::parseFirstInt( &strMsg );
00695 iHeadFacingDir = Parse::parseFirstInt( &strMsg );
00696 }
00697 }
00698
00699
00700 Parse::gotoFirstOccurenceOf( ')', &strMsg );
00701 strMsg++;
00702
00703 if( SoccerTypes::isPlayer( o ) )
00704 {
00705 objMin = (SoccerTypes::isTeammate(o) == true ) ? objTeamMin : objOppMin ;
00706 if( objMin != OBJECT_ILLEGAL && objMin == objMax )
00707 o = objMin;
00708 }
00709
00710
00711 processNewObjectInfo( o, time, dDist, iDir, dDistChange,
00712 dDirChange, (AngDeg)iBodyFacingDir, (AngDeg)iHeadFacingDir,
00713 isGoalie, objMin, objMax );
00714 }
00715 mapUnknownPlayers( time );
00716 }
00717
00723 bool WorldModel::updateAfterSeeMessage( )
00724 {
00725 processLastSeeMessage( );
00726
00727 if( getCurrentTime().getTime() != -1 )
00728 updateAgentObjectAfterSee( );
00729
00730
00731
00732
00733
00734 double dConfThr = PS->getPlayerConfThr();
00735 int iIndex;
00736
00737 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
00738 o != OBJECT_ILLEGAL;
00739 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
00740 {
00741 if( getTimeLastSeen( o ) == getTimeLastSeeMessage() &&
00742 o != getAgentObjectType() )
00743 updateDynamicObjectAfterSee ( o );
00744 else
00745 updateObjectRelativeFromGlobal( o );
00746 }
00747 iterateObjectDone( iIndex );
00748
00749
00750 if( getTimeLastSeen( OBJECT_BALL ) == getTimeLastSeeMessage() )
00751 updateDynamicObjectAfterSee ( OBJECT_BALL );
00752 else
00753 updateObjectRelativeFromGlobal( OBJECT_BALL );
00754
00755
00756 removeGhosts();
00757
00758 return true;
00759 }
00760
00765 bool WorldModel::updateAgentObjectAfterSee( )
00766 {
00767 VecPosition posGlobal, velGlobal;
00768 AngDeg angGlobal;
00769
00770
00771 calculateStateAgent( &posGlobal, &velGlobal, &angGlobal );
00772
00773
00774 agentObject.setTimeLastSeen ( getTimeLastSeeMessage() );
00775
00776
00777 agentObject.setPositionDifference(posGlobal- agentObject.getGlobalPosition());
00778 agentObject.setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
00779 agentObject.setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00780 agentObject.setGlobalNeckAngle ( angGlobal );
00781 agentObject.setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
00782
00783 return true;
00784 }
00785
00786
00792 bool WorldModel::updateDynamicObjectAfterSee( ObjectT o )
00793 {
00794 VecPosition posGlobal, velGlobal;
00795
00796 if( o == OBJECT_BALL )
00797 {
00798 calculateStateBall ( &posGlobal, &velGlobal );
00799 Ball.setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
00800 Ball.setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
00801 Ball.setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00802 return true;
00803 }
00804 else if( SoccerTypes::isKnownPlayer( o ) )
00805 {
00806 calculateStatePlayer( o, &posGlobal, &velGlobal );
00807
00808 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
00809
00810 pob->setGlobalVelocity ( velGlobal, getTimeLastSeeMessage() );
00811 pob->setGlobalPosition ( posGlobal, getTimeLastSeeMessage() );
00812 pob->setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00813
00814 if( pob->getTimeRelativeAngles() == getTimeLastSeeMessage() )
00815 {
00816 AngDeg ang = getAgentGlobalNeckAngle() + pob->getRelativeBodyAngle();
00817 ang = VecPosition::normalizeAngle( ang );
00818 pob->setGlobalBodyAngle( ang, getTimeLastSeeMessage() );
00819 ang = getAgentGlobalNeckAngle() + pob->getRelativeNeckAngle();
00820 ang = VecPosition::normalizeAngle( ang );
00821 pob->setGlobalNeckAngle( ang, getTimeLastSeeMessage() );
00822 }
00823 return true;
00824 }
00825
00826 return false;
00827 }
00828
00829
00830
00831
00832
00833
00842 bool WorldModel::updateAfterSenseMessage( )
00843 {
00844
00845 updateAgentAndBallAfterSense( );
00846
00847
00848 double dConfThr = PS->getPlayerConfThr();
00849 int iIndex;
00850
00851 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
00852 o != OBJECT_ILLEGAL;
00853 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
00854 {
00855 if( o != getAgentObjectType() && getTimeGlobalPosition(o) > 0 )
00856 {
00857 updateDynamicObjectForNextCycle( o,
00858 getCurrentTime() - getTimeGlobalPosition(o) );
00859 }
00860 }
00861 iterateObjectDone( iIndex);
00862
00863 return true;
00864 }
00865
00871 bool WorldModel::updateAgentAndBallAfterSense( )
00872 {
00873
00874 bool bBallUpdated = false;
00875 VecPosition pos = getAgentGlobalPosition();
00876 AngDeg angGlobalNeck = getAgentGlobalNeckAngle();
00877 AngDeg angGlobalBody = getAgentGlobalBodyAngle();
00878 Stamina sta = getAgentStamina();
00879 VecPosition velNeck = agentObject.getSpeedRelToNeck();
00880 VecPosition vel = getAgentGlobalVelocity();
00881 Time time = getCurrentTime() - 1 ;
00882
00883
00884
00885 for( int i = 0; i < MAX_COMMANDS; i ++ )
00886 {
00887 if( performedCommands[i] == true &&
00888 ( queuedCommands[i].time.getTime() == time.getTime() ||
00889 queuedCommands[i].time.getTime() == time.getTime() - 1 ) )
00890 {
00891 switch( queuedCommands[i].commandType )
00892 {
00893 case CMD_TURN:
00894 case CMD_TURNNECK:
00895 case CMD_DASH:
00896 predictStateAfterCommand( queuedCommands[i], &pos, &vel,
00897 &angGlobalBody, &angGlobalNeck, &sta );
00898 break;
00899 case CMD_KICK:
00900 updateBallAfterKick( queuedCommands[i].dPower,
00901 queuedCommands[i].dAngle);
00902 bBallUpdated = true;
00903 queuedCommands[i].time.updateTime( -1 );
00904 break;
00905 case CMD_MOVE:
00906 pos.setVecPosition( queuedCommands[i].dX, queuedCommands[i].dY );
00907 initParticlesAgent( pos );
00908 break;
00909 default:
00910 break;
00911 }
00912 }
00913 }
00914
00915
00916 if( !bBallUpdated )
00917 {
00918 m_bPerformedKick = false;
00919 updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
00920 }
00921 else
00922 m_bPerformedKick = true;
00923
00924
00925 velNeck.rotate( angGlobalNeck );
00926 double dDistBall = pos.getDistanceTo( getBallPos() );
00927 if( ( dDistBall<SS->getPlayerSize()+SS->getBallSize() &&
00928 velNeck.getMagnitude( ) < EPSILON
00929 )
00930 ||
00931 ( dDistBall < SS->getMaximalKickDist() + 0.5 &&
00932 velNeck.getMagnitude( ) > EPSILON &&
00933 sign( vel.getX() ) != sign( velNeck.getX() ) &&
00934 sign( vel.getY() ) != sign( velNeck.getY() ) &&
00935 velNeck.getMagnitude() < 0.15 * vel.getMagnitude() ) )
00936 {
00937 m_bWasCollision = true;
00938 double dDistPlayer;
00939 getClosestInSetTo(
00940 OBJECT_SET_PLAYERS, getAgentObjectType(), &dDistPlayer );
00941 if( dDistBall < dDistPlayer )
00942 {
00943 updateBallForCollision( pos );
00944 Log.log( 101, "COLLISION WITH BALL %f, vel(%f,%f) velNeck(%f,%f)",
00945 dDistBall, vel.getX(), vel.getY(), velNeck.getX(), velNeck.getY() );
00946 }
00947 else
00948 Log.log( 101, "COLLISION WITH PLAYER vel(%f,%f) velNeck(%f,%f)",
00949 vel.getX(), vel.getY(), velNeck.getX(), velNeck.getY() );
00950
00951
00952 vel = velNeck;
00953 }
00954 else
00955 {
00956 if( dDistBall < SS->getVisibleDistance() )
00957 Log.log( 101, "No collision: dist: %f, velNeck (%f,%f), vel (%f,%f)",
00958 dDistBall, velNeck.getX(), velNeck.getY(), vel.getX(), vel.getY() );
00959 m_bWasCollision = false;
00960
00961 pos = getAgentGlobalPosition();
00962 angGlobalNeck = getAgentGlobalNeckAngle();
00963 angGlobalBody = getAgentGlobalBodyAngle();
00964 sta = getAgentStamina();
00965 vel = agentObject.getSpeedRelToNeck();
00966
00967
00968
00969
00970
00971
00972
00973 vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
00974
00975 for( int i = 0; i < MAX_COMMANDS; i ++ )
00976 {
00977 if( performedCommands[i] == true &&
00978 ( queuedCommands[i].time.getTime() == time.getTime() ||
00979 queuedCommands[i].time.getTime() == time.getTime() - 1 ) )
00980 {
00981 switch( queuedCommands[i].commandType )
00982 {
00983 case CMD_TURN:
00984 case CMD_TURNNECK:
00985 predictStateAfterCommand( queuedCommands[i], &pos, &vel,
00986 &angGlobalBody, &angGlobalNeck, &sta );
00987 break;
00988 case CMD_MOVE:
00989 pos.setVecPosition( queuedCommands[i].dX, queuedCommands[i].dY );
00990 initParticlesAgent( pos );
00991 break;
00992 case CMD_DASH:
00993
00994
00995 break;
00996 default:
00997 break;
00998 }
00999 }
01000 queuedCommands[i].time.updateTime( -1 );
01001 }
01002
01003
01004 vel = agentObject.getSpeedRelToNeck();
01005 pos = getAgentGlobalPosition();
01006 vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
01007 vel.rotate( angGlobalNeck );
01008
01009
01010
01011
01012
01013 predictStateAfterDash( 0.0, &pos, &vel, &sta, 0 );
01014 }
01015
01016
01017 updateParticlesAgent( vel, true );
01018
01019
01020
01021 agentObject.setGlobalPosition ( pos, getCurrentTime() );
01022 agentObject.setGlobalVelocity ( vel, getCurrentTime() );
01023 agentObject.setGlobalNeckAngle( angGlobalNeck );
01024
01025 return true;
01026 }
01027
01036 bool WorldModel::updateBallAfterKick( double dPower, AngDeg ang )
01037 {
01038 if( getRelativeDistance( OBJECT_BALL ) < SS->getMaximalKickDist() )
01039 {
01040
01041
01042 ang = VecPosition::normalizeAngle(ang + getAgentGlobalBodyAngle() );
01043 VecPosition acc( getActualKickPowerRate()*dPower, ang, POLAR ) ;
01044 acc += getGlobalVelocity( OBJECT_BALL );
01045 if( acc.getMagnitude() > SS->getBallSpeedMax() )
01046 acc.setMagnitude( SS->getBallSpeedMax() );
01047 Ball.setGlobalPosition( getGlobalPosition( OBJECT_BALL ) + acc,
01048 getCurrentTime() );
01049 Ball.setGlobalVelocity( acc*SS->getBallDecay(), getCurrentTime() );
01050 }
01051 else
01052 {
01053 updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
01054 Log.log( 21, "(WorldModel::%s) KICK command, but ball not kickable (%f)",
01055 __FUNCTION__, getRelativeDistance( OBJECT_BALL ) );
01056 }
01057 return true;
01058 }
01059
01066 bool WorldModel::updateDynamicObjectForNextCycle( ObjectT obj, int iCycles)
01067 {
01068 DynamicObject *o = (DynamicObject*) getObjectPtrFromType( obj );
01069 if( o == NULL )
01070 return false;
01071
01072
01073 VecPosition vel = getGlobalVelocity( obj );
01074 VecPosition pos = getGlobalPosition( obj );
01075
01076 if( SoccerTypes::isBall( obj ) )
01077 {
01078 for( int i = 0; i < iCycles ; i++ )
01079 {
01080 pos += vel;
01081 vel *= SS->getBallDecay();
01082 }
01083 }
01084 else if( SoccerTypes::isPlayer( obj ) )
01085 {
01086 for( int i = 0; i < iCycles ; i++ )
01087 {
01088
01089 pos += vel;
01090 vel *= SS->getPlayerDecay();
01091 }
01092 }
01093
01094 o->setGlobalVelocity ( vel, getCurrentTime() );
01095 o->setGlobalPosition ( pos, getCurrentTime() );
01096
01097 return true;
01098 }
01099
01100
01105 bool WorldModel::updateBallForCollision( VecPosition posAgent )
01106 {
01107 VecPosition posBall = getGlobalPosition( OBJECT_BALL );
01108 VecPosition velBall = getGlobalVelocity( OBJECT_BALL );
01109
01110
01111
01112 AngDeg ang = (posBall - posAgent).getDirection();
01113 Ball.setGlobalPosition( posAgent + VecPosition( 0.2, ang, POLAR ),
01114 getCurrentTime() );
01115 Ball.setGlobalVelocity( velBall*-0.1, getCurrentTime() );
01116 Log.log( 101, "updateBallForCollision; vel from (%f,%f) to (%f,%f)",
01117 velBall.getX(), velBall.getY(), getGlobalVelocity(OBJECT_BALL).getX(),
01118 getGlobalVelocity(OBJECT_BALL).getY() );
01119 return true;
01120 }
01121
01127 bool WorldModel::updateRelativeFromGlobal()
01128 {
01129 double dConfThr = PS->getPlayerConfThr();
01130 int iIndex;
01131
01132
01133 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
01134 o != OBJECT_ILLEGAL;
01135 o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
01136 {
01137 if( o != getAgentObjectType() )
01138 updateObjectRelativeFromGlobal( o );
01139 }
01140 iterateObjectDone( iIndex);
01141
01142
01143 if( isConfidenceGood( OBJECT_BALL ) )
01144 updateObjectRelativeFromGlobal( OBJECT_BALL );
01145
01146
01147
01148 return true;
01149 }
01150
01156 bool WorldModel::updateObjectRelativeFromGlobal( ObjectT o )
01157 {
01158 Object *obj = (Object*) getObjectPtrFromType( o );
01159 if( obj == NULL )
01160 return false;
01161
01162
01163 VecPosition rel = obj->getGlobalPosition();
01164 rel.globalToRelative( getAgentGlobalPosition(), getAgentGlobalNeckAngle() );
01165 obj->setRelativePosition( rel, obj->getTimeGlobalPosition() );
01166 return true;
01167 }
01168
01177 bool WorldModel::calculateStateAgent( VecPosition *posGlobal,
01178 VecPosition *velGlobal, AngDeg *angGlobal )
01179 {
01180 int iNrLeft;
01181
01182
01183 ObjectT objLine = getFurthestRelativeInSet( OBJECT_SET_LINES );
01184 if( objLine != OBJECT_ILLEGAL )
01185 {
01186 double angGlobalLine = getGlobalAngle ( objLine );
01187 AngDeg angRelLine = getRelativeAngle( objLine );
01188 *angGlobal = angGlobalLine - angRelLine;
01189 *angGlobal = VecPosition::normalizeAngle( *angGlobal );
01190 }
01191 else
01192 {
01193 Log.log( 21, "(WorldModel::%s) no line in last see message", __FUNCTION__ );
01194 *angGlobal = getAgentGlobalNeckAngle();
01195 }
01196
01197
01198
01199
01200
01201
01202
01203
01204 *velGlobal = agentObject.getSpeedRelToNeck().rotate( *angGlobal );
01205 velGlobal->setMagnitude( velGlobal->getMagnitude()/SS->getPlayerDecay() );
01206
01207 updateParticlesAgent ( *velGlobal, false );
01208 iNrLeft = checkParticlesAgent ( *angGlobal );
01209
01210 if( iNrLeft == 0 )
01211 {
01212
01213
01214 initParticlesAgent ( *angGlobal );
01215 iNrLeft = checkParticlesAgent( *angGlobal );
01216 if( iNrLeft == 0 )
01217 {
01218
01219
01220 calculateStateAgent2( posGlobal, velGlobal, angGlobal );
01221 initParticlesAgent( *posGlobal );
01222 return false;
01223 }
01224 }
01225
01226
01227
01228 *posGlobal = averageParticles( particlesPosAgent, iNrLeft );
01229 resampleParticlesAgent( iNrLeft );
01230
01231
01232
01233 AngDeg ang = calculateAngleAgentWithPos( *posGlobal );
01234 if( ang != UnknownAngleValue )
01235 *angGlobal = ang;
01236
01237 *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
01238 return true;
01239 }
01240
01248 void WorldModel::initParticlesAgent( AngDeg angGlobal )
01249 {
01250 double dDist, dMaxRadius, dMinRadius, dInput;
01251 AngDeg ang;
01252
01253
01254 ObjectT objFlag = getClosestRelativeInSet( OBJECT_SET_FLAGS );
01255
01256 if( objFlag == OBJECT_ILLEGAL )
01257 {
01258 Log.log( 21, "(WorldModel::%s) no flag in last see message", __FUNCTION__ );
01259 return;
01260 }
01261
01262
01263 dInput = getRelativeDistance( objFlag );
01264 getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01265 SS->getQuantizeStepL(), 0.1 ) ;
01266
01267
01268
01269 AngDeg angFlag = getRelativeAngle( objFlag ) + 180 + angGlobal ;
01270
01271
01272 for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01273 {
01274
01275
01276
01277
01278 dDist = dMinRadius + drand48()*(dMaxRadius-dMinRadius);
01279 ang = VecPosition::normalizeAngle( angFlag - 1.0 + 2*drand48() );
01280
01281
01282 particlesPosAgent[i] = getGlobalPosition( objFlag ) +
01283 VecPosition( dDist, ang, POLAR );
01284 }
01285 }
01286
01292 void WorldModel::initParticlesAgent( VecPosition posInitial )
01293 {
01294 for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01295 particlesPosAgent[i] = posInitial;
01296 }
01297
01306 int WorldModel::checkParticlesAgent( AngDeg angGlobalNeck )
01307 {
01308 double dMaxRadius, dMinRadius, dInput, dDist;
01309 AngDeg ang;
01310 int iIndex, iNrLeft = 0, iLength = iNrParticlesAgent;
01311
01312
01313 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_FLAGS, 1.0 );
01314 o != OBJECT_ILLEGAL;
01315 o = iterateObjectNext ( iIndex, OBJECT_SET_FLAGS, 1.0 ) )
01316 {
01317 iNrLeft = 0;
01318 dInput = getRelativeDistance( o );
01319 getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01320 SS->getQuantizeStepL(), 0.1 ) ;
01321
01322
01323 for( int i = 0; i < iLength; i ++ )
01324 {
01325
01326
01327
01328 dDist = particlesPosAgent[i].getDistanceTo( getGlobalPosition( o ) );
01329 ang = (getGlobalPosition(o) - particlesPosAgent[i]).getDirection();
01330 ang = ang - ( getRelativeAngle( o ) + angGlobalNeck );
01331
01332
01333
01334 if( dDist > dMinRadius && dDist < dMaxRadius &&
01335 fabs(VecPosition::normalizeAngle( ang )) <= 1.0 )
01336 particlesPosAgent[iNrLeft++] = particlesPosAgent[i];
01337 }
01338
01339 iLength = iNrLeft;
01340 }
01341 return iNrLeft;
01342 }
01343
01354 void WorldModel::updateParticlesAgent( VecPosition vel, bool bAfterSense )
01355 {
01356
01357 static VecPosition prev_vel;
01358
01359 for( int i = 0; i < iNrParticlesAgent ; i ++ )
01360 {
01361 if( bAfterSense == false )
01362 {
01363 particlesPosAgent[i].setX( particlesPosAgent[i].getX() - prev_vel.getX());
01364 particlesPosAgent[i].setY( particlesPosAgent[i].getY() - prev_vel.getY());
01365 }
01366
01367 particlesPosAgent[i].setX( particlesPosAgent[i].getX( ) + vel.getX() );
01368 particlesPosAgent[i].setY( particlesPosAgent[i].getY( ) + vel.getY() );
01369 }
01370 prev_vel = vel;
01371 }
01372
01373
01378 VecPosition WorldModel::averageParticles( VecPosition posArray[], int iLength )
01379 {
01380 if( iLength == 0 )
01381 return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
01382
01383
01384 double x = 0, y = 0;
01385 for( int i = 0; i < iLength ; i ++ )
01386 {
01387 x += posArray[ i ].getX( );
01388 y += posArray[ i ].getY( );
01389 }
01390
01391 return VecPosition( x/iLength, y/iLength );
01392 }
01393
01400 void WorldModel::resampleParticlesAgent( int iLeft )
01401 {
01402 for ( int i = iLeft; i < iNrParticlesAgent; i ++ )
01403 particlesPosAgent[ i ] = particlesPosAgent[ (int)(drand48()*iLeft) ];
01404 }
01405
01414 bool WorldModel::calculateStateAgent2( VecPosition *posGlobal,
01415 VecPosition *velGlobal, AngDeg *angGlobal)
01416 {
01417 double x=0.0, y=0.0, dMinRadius1, dMaxRadius1, dMinRadius2, dMaxRadius2;
01418 double dTotalVar = UnknownDoubleValue, dVar, K;
01419 int iIndex1, iIndex2;
01420 ObjectT obj2;
01421 VecPosition pos;
01422
01423
01424 for( ObjectT obj1 = iterateObjectStart( iIndex1, OBJECT_SET_FLAGS, 1.0 );
01425 obj1 != OBJECT_ILLEGAL;
01426 obj1 = iterateObjectNext ( iIndex1, OBJECT_SET_FLAGS, 1.0 ) )
01427 {
01428
01429 iIndex2 = iIndex1;
01430 for( obj2 = iterateObjectNext ( iIndex2, OBJECT_SET_FLAGS, 1.0 ) ;
01431 obj2 != OBJECT_ILLEGAL;
01432 obj2 = iterateObjectNext ( iIndex2, OBJECT_SET_FLAGS, 1.0 ) )
01433 {
01434
01435 pos = calculatePosAgentWith2Flags( obj1, obj2 );
01436
01437
01438
01439
01440
01441
01442
01443 getMinMaxDistQuantizeValue(getRelativeDistance(obj1),
01444 &dMinRadius1, &dMaxRadius1, SS->getQuantizeStepL(), 0.1 ) ;
01445 getMinMaxDistQuantizeValue(getRelativeDistance(obj2),
01446 &dMinRadius2, &dMaxRadius2, SS->getQuantizeStepL(), 0.1 ) ;
01447 dVar = (dMaxRadius1-dMinRadius1)*(dMaxRadius1-dMinRadius1)/12;
01448 dVar += (dMaxRadius2-dMinRadius2)*(dMaxRadius2-dMinRadius2)/12;
01449
01450 if( pos.getX() != UnknownDoubleValue &&
01451 dTotalVar == UnknownDoubleValue )
01452 {
01453 dTotalVar = dVar;
01454 x = pos.getX();
01455 y = pos.getY();
01456 }
01457 else if( pos.getX() != UnknownDoubleValue )
01458 {
01459 K = dTotalVar / ( dVar + dTotalVar );
01460 x += K*( pos.getX() - x );
01461 y += K*( pos.getY() - y );
01462 dTotalVar -= K*dTotalVar;
01463 }
01464 }
01465 iterateObjectDone( iIndex2 );
01466 }
01467 iterateObjectDone( iIndex1 );
01468 posGlobal->setVecPosition( x, y );
01469
01470
01471
01472 *angGlobal = calculateAngleAgentWithPos( *posGlobal );
01473
01474
01475 *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
01476
01477 return true;
01478 }
01479
01491 VecPosition WorldModel::calculatePosAgentWith2Flags( ObjectT objFlag1,
01492 ObjectT objFlag2 )
01493 {
01494 double xA, yA, xB, yB, rA, rB;
01495 AngDeg aA, aB;
01496
01497
01498 xA = getGlobalPosition ( objFlag1 ).getX();
01499 yA = getGlobalPosition ( objFlag1 ).getY();
01500 rA = getRelativeDistance( objFlag1 );
01501 aA = getRelativeAngle ( objFlag1 );
01502 xB = getGlobalPosition ( objFlag2 ).getX();
01503 yB = getGlobalPosition ( objFlag2 ).getY();
01504 rB = getRelativeDistance( objFlag2 );
01505 aB = getRelativeAngle ( objFlag2 );
01506
01507 double L, dx, dy, d_par, d_orth;
01508 double x, y;
01509
01510 double sign = ((aB - aA) > 0.0) ? 1.0 : -1.0;
01511
01512
01513
01514
01515
01516
01517
01518
01519 dx = xB - xA;
01520 dy = yB - yA;
01521 L = sqrt(dx*dx + dy*dy);
01522
01523 dx /= L; dy /= L;
01524
01525 d_par = (L*L + rA*rA - rB*rB) / (2.0 * L);
01526 double arg = rA*rA - d_par*d_par;
01527 d_orth = (arg > 0.0) ? sqrt(arg) : 0.0;
01528
01529 x = xA + d_par * dx - sign * d_orth * dy;
01530 y = yA + d_par * dy + sign * d_orth * dx;
01531
01532 return VecPosition( x, y );
01533 }
01534
01544 AngDeg WorldModel::calculateAngleAgentWithPos( VecPosition pos )
01545 {
01546 int iNrFlags = 0, iIndex;
01547 double dCosX=0, dSinX=0 ,dAngleNow, xA, yA, aA;
01548
01549 for( ObjectT obj = iterateObjectStart( iIndex, OBJECT_SET_FLAGS, 1.0 );
01550 obj != OBJECT_ILLEGAL;
01551 obj = iterateObjectNext ( iIndex, OBJECT_SET_FLAGS, 1.0 ) )
01552 {
01553 xA = getGlobalPosition( obj ).getX();
01554 yA = getGlobalPosition( obj ).getY();
01555 aA = getRelativeAngle( obj );
01556
01557
01558
01559 dAngleNow = atan2Deg( yA - pos.getY(), xA - pos.getX() );
01560 dAngleNow = VecPosition::normalizeAngle( dAngleNow - aA );
01561
01562
01563
01564
01565 dCosX += cosDeg( dAngleNow );
01566 dSinX += sinDeg( dAngleNow );
01567 iNrFlags++;
01568
01569 }
01570 iterateObjectDone( iIndex );
01571
01572
01573 dCosX /= (double)iNrFlags;
01574 dSinX /= (double)iNrFlags;
01575 if( iNrFlags == 0 )
01576 return UnknownAngleValue;
01577
01578 return VecPosition::normalizeAngle( atan2Deg( dSinX, dCosX ) ) ;
01579 }
01580
01581
01587 VecPosition WorldModel::calculateVelocityDynamicObject( ObjectT o )
01588 {
01589 DynamicObject * dobj = (DynamicObject*) getObjectPtrFromType( o );
01590 if( dobj == NULL )
01591 return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
01592 double dDistCh = dobj->getRelativeDistanceChange( );
01593 double angCh = dobj->getRelativeAngleChange ( );
01594 double dDist = getRelativeDistance ( o );
01595 double ang = getRelativeAngle ( o );
01596
01597 double velx = dDistCh * cosDeg(ang) - Deg2Rad(angCh) * dDist * sinDeg( ang );
01598 double vely = dDistCh * sinDeg(ang) + Deg2Rad(angCh) * dDist * cosDeg( ang );
01599
01600 VecPosition vel( velx, vely );
01601 return vel.relativeToGlobal( getAgentGlobalVelocity(),
01602 getAgentGlobalNeckAngle() );
01603 }
01604
01605
01611 bool WorldModel::calculateStateBall( VecPosition *posGlobal,
01612 VecPosition *velGlobal )
01613 {
01614
01615
01616
01617 VecPosition posRelWorld =
01618 VecPosition( getRelativeDistance( OBJECT_BALL ),
01619 getRelativeAngle( OBJECT_BALL ) + getAgentGlobalNeckAngle(),
01620 POLAR );
01621 *posGlobal = getAgentGlobalPosition() + posRelWorld;
01622
01623 if( isBeforeKickOff() )
01624 posGlobal->setVecPosition( 0, 0 );
01625
01626 *velGlobal = getGlobalVelocity(OBJECT_BALL);
01627 if( Ball.getTimeChangeInformation( ) == getTimeLastSeen( OBJECT_BALL ) )
01628 {
01629 *velGlobal = calculateVelocityDynamicObject( OBJECT_BALL );;
01630 Log.log( 458, "vel based on change info: (%f,%f)", velGlobal->getX(),
01631 velGlobal->getY() );
01632
01633
01634
01635 if( fabs(velGlobal->getX() - getGlobalVelocity(OBJECT_BALL).getX())
01636 <= 2*SS->getBallRand()*getBallSpeed() &&
01637 fabs(velGlobal->getY() - getGlobalVelocity(OBJECT_BALL).getY())
01638 <= 2*SS->getBallRand()*getBallSpeed() )
01639 {
01640 *velGlobal = (*velGlobal + getGlobalVelocity(OBJECT_BALL))/2.0;
01641 Log.log( 458, "average ball vel to (%f,%f)", velGlobal->getX(),
01642 velGlobal->getY() );
01643 }
01644 }
01645 else if( getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() &&
01646 getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() < 3 )
01647 {
01648
01649
01650
01651
01652
01653
01654
01655 VecPosition posGlobalDiff = *posGlobal - Ball.getGlobalPositionLastSee()
01656 - agentObject.getPositionDifference();
01657 Log.log( 101, "2 pos: ball(%f,%f), ball_prev(%f,%f), agentdiff(%f,%f)",
01658 posGlobal->getX(), posGlobal->getY(),
01659 Ball.getGlobalPositionLastSee().getX(),
01660 Ball.getGlobalPositionLastSee().getY(),
01661 agentObject.getPositionDifference().getX(),
01662 agentObject.getPositionDifference().getY() );
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673 if( getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() == 1 &&
01674 m_bWasCollision == false )
01675 {
01676 *velGlobal = posGlobalDiff*SS->getBallDecay();
01677 Log.log( 458, "vel based on 2 pos: (%f,%f)", velGlobal->getX(),
01678 velGlobal->getY() );
01679 Log.log( 101, "vel based on 2 pos: (%f,%f)", velGlobal->getX(),
01680 velGlobal->getY() );
01681 }
01682 else if( getTimeLastSeeMessage()-Ball.getTimeGlobalPosDerivedFromSee() > 2 )
01683 {
01684 Log.log( 20, "(WorldModel:%s) time difference too large" ,__FUNCTION__ );
01685 }
01686 }
01687 else
01688 Log.log( 458, "vel ball not updated", velGlobal->getX(), velGlobal->getY() );
01689 ;
01690
01691
01692
01693
01694
01695
01696
01697
01698 if( getTimeSinceLastCatch() < 2 ||
01699 (getPlayMode() != PM_PLAY_ON && !isGoalKickUs() && !isGoalKickThem()))
01700 velGlobal->setMagnitude( 0.0 );
01701 else if( getNrInSetInCircle( OBJECT_SET_OPPONENTS,
01702 Circle(*posGlobal,SS->getMaximalKickDist())) > 0
01703 && getRelativeDistance( OBJECT_BALL ) > SS->getMaximalKickDist() )
01704 velGlobal->setMagnitude( 0.0 );
01705 else if( velGlobal->getMagnitude() >
01706 ( 1.0 + SS->getBallRand() )*SS->getBallSpeedMax() )
01707 velGlobal->setMagnitude( SS->getBallSpeedMax() );
01708
01709 if( isKickInUs() || isKickInThem() )
01710 posGlobal->setY( sign( posGlobal->getY() ) * PITCH_WIDTH/2.0 );
01711 else if( isBackPassUs() )
01712 posGlobal->setVecPosition( - PENALTY_X,
01713 sign(posGlobal->getY())*PENALTY_AREA_WIDTH/2.0 );
01714 else if( isBackPassThem() )
01715 posGlobal->setVecPosition( + PENALTY_X,
01716 sign(posGlobal->getY())*PENALTY_AREA_WIDTH/2.0 );
01717
01718
01719 Log.log( 458, "final ball vel: (%f,%f)", velGlobal->getX(),velGlobal->getY());
01720 if( getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() )
01721 Log.log( 101, "direction old: %f, new: %f",
01722 ( getGlobalPosition( OBJECT_BALL ) - getAgentGlobalPosition()).getDirection(),
01723 ( *posGlobal - getAgentGlobalPosition()).getDirection() );
01724 return true;
01725 }
01726
01727
01734 bool WorldModel::calculateStatePlayer( ObjectT o, VecPosition *posGlobal,
01735 VecPosition *velGlobal )
01736 {
01737 PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
01738 if( pob == NULL )
01739 return false;
01740
01741
01742
01743
01744 VecPosition posRelWorld =
01745 VecPosition( getRelativeDistance( o ),
01746 getRelativeAngle( o ) + agentObject.getGlobalNeckAngle(),
01747 POLAR );
01748 *posGlobal = getAgentGlobalPosition() + posRelWorld;
01749
01750 velGlobal->setVecPosition( 0, 0);
01751 if( pob->getTimeChangeInformation( ) == getTimeLastSeen( o ) )
01752 {
01753
01754
01755 *velGlobal = calculateVelocityDynamicObject( o );
01756 }
01757 else
01758 ;
01759
01760
01761 if( velGlobal->getMagnitude() >=
01762 ( 1.0 + SS->getPlayerRand())*SS->getPlayerSpeedMax() )
01763 velGlobal->setMagnitude( SS->getPlayerSpeedMax() );
01764
01765 return true;
01766 }
01767
01768
01780 bool WorldModel::getMinMaxDistQuantizeValue( double dOutput, double *dMin,
01781 double *dMax, double x1, double x2 )
01782 {
01783
01784
01785
01786
01787
01788
01789
01790 dOutput -= 1.0e-10;
01791 *dMin = exp( invQuantizeMin( log( invQuantizeMin(dOutput,x2) ), x1 ) );
01792 dOutput += 2.0e-10;
01793 *dMax = exp( invQuantizeMax( log( invQuantizeMax(dOutput,x2) ), x1 ) );
01794 return true;
01795 }
01796
01806 bool WorldModel::getMinMaxDirChange( double dOutput, double *dMin,
01807 double *dMax, double x1 )
01808 {
01809 *dMin = invQuantizeMin( dOutput, x1 ) ;
01810 *dMax = invQuantizeMax( dOutput, x1 ) ;
01811 return true;
01812 }
01813
01828 bool WorldModel::getMinMaxDistChange( double dOutput, double dDist,
01829 double *dMin, double *dMax, double x1, double xDist1, double xDist2)
01830 {
01831
01832
01833
01834
01835
01836 double dMinDist, dMaxDist;
01837 getMinMaxDistQuantizeValue( dDist, &dMinDist, &dMaxDist, xDist1, xDist2 );
01838 dOutput = dOutput/dDist;
01839 double dMinCh = invQuantizeMin( dOutput, x1 ) ;
01840 double dMaxCh = invQuantizeMax( dOutput, x1 ) ;
01841 *dMin = min( dMinDist*dMinCh, dMaxDist*dMinCh );
01842 *dMax = max( dMinDist*dMaxCh, dMaxDist*dMaxCh );
01843 return true;
01844 }
01845
01851 double WorldModel::invQuantizeMin( double dOutput, double dQuantizeStep )
01852 {
01853
01854
01855 return (rint( dOutput / dQuantizeStep )-0.5 )*dQuantizeStep;
01856 }
01857
01863 double WorldModel::invQuantizeMax( double dOutput, double dQuantizeStep )
01864 {
01865
01866
01867 return (rint( dOutput/dQuantizeStep) + 0.5 )*dQuantizeStep;
01868 }
01869
01878 void WorldModel::mapUnknownPlayers( Time time)
01879 {
01880 double dist, dMinDist;
01881 VecPosition rel;
01882 ObjectT o, o_new, objTmp;
01883
01884
01885 for( int j = 0; j < iNrUnknownPlayers; j ++ )
01886 {
01887 rel.setVecPosition( UnknownPlayers[j].getRelativeDistance(),
01888 UnknownPlayers[j].getRelativeAngle(), POLAR );
01889 dist = dMinDist = 1000.0;
01890 o = UnknownPlayers[j].getType();
01891 o_new = OBJECT_ILLEGAL;
01892 if( ! SoccerTypes::isOpponent( o ) )
01893 {
01894 for( int i = 0 ; i < MAX_TEAMMATES ; i++ )
01895 {
01896 objTmp = Teammates[i].getType();
01897 if( isConfidenceGood( objTmp ) && getTimeLastSeen( objTmp ) != time &&
01898 ( o == OBJECT_PLAYER_UNKNOWN ||
01899 UnknownPlayers[j].isInRange( objTmp ) ) )
01900 {
01901 dist = rel.getDistanceTo( Teammates[i].getRelativePosition( ) );
01902 if( dist < dMinDist )
01903 {
01904 o_new = objTmp;
01905 dMinDist = dist;
01906 }
01907 }
01908 }
01909 }
01910 if( ! SoccerTypes::isTeammate( o ) )
01911 {
01912 for( int i = 0 ; i < MAX_OPPONENTS ; i++ )
01913 {
01914 objTmp = Opponents[i].getType();
01915 if( isConfidenceGood( objTmp ) && getTimeLastSeen( objTmp ) != time &&
01916 ( o == OBJECT_PLAYER_UNKNOWN ||
01917 UnknownPlayers[j].isInRange( objTmp ) ) )
01918 {
01919 dist = rel.getDistanceTo( Opponents[i].getRelativePosition( ) );
01920 if( dist < dMinDist )
01921 {
01922 o_new = objTmp;
01923 dMinDist = dist;
01924 }
01925 }
01926 }
01927 }
01928
01929
01930
01931
01932
01933
01934 if( SoccerTypes::isKnownPlayer(o_new)
01935 && dMinDist < PS->getPlayerDistTolerance())
01936 {
01937 UnknownPlayers[j].setType( o_new );
01938 if( SoccerTypes::isTeammate( o_new ) )
01939 Teammates[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01940 else if( SoccerTypes::isOpponent( o_new ) )
01941 Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01942 }
01943 else if( UnknownPlayers[j].getType() == OBJECT_TEAMMATE_UNKNOWN )
01944 {
01945 o_new = getFirstEmptySpotInSet( OBJECT_SET_TEAMMATES, j );
01946
01947 if( o_new != OBJECT_ILLEGAL )
01948 {
01949 UnknownPlayers[j].setType( o_new );
01950 Teammates[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01951 }
01952 }
01953 else if( UnknownPlayers[j].getType() == OBJECT_OPPONENT_UNKNOWN )
01954 {
01955 o_new = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS, j );
01956
01957 if( o_new != OBJECT_ILLEGAL )
01958 {
01959 UnknownPlayers[j].setType( o_new );
01960 Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01961 }
01962 }
01963 else if( UnknownPlayers[j].getType() == OBJECT_PLAYER_UNKNOWN &&
01964 UnknownPlayers[j].getRelativeDistance() < SS->getVisibleDistance() )
01965 {
01966 o_new = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
01967
01968 if( o_new != OBJECT_ILLEGAL )
01969 {
01970 UnknownPlayers[j].setType( o_new );
01971 Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01972 }
01973 }
01974 }
01975
01976 iNrUnknownPlayers = 0;
01977 }
01978
01986 bool WorldModel::updateSSToHeteroPlayerType( int iIndex )
01987 {
01988 SS->setPlayerSpeedMax( pt[iIndex].dPlayerSpeedMax );
01989 SS->setStaminaIncMax ( pt[iIndex].dStaminaIncMax );
01990 SS->setPlayerDecay ( pt[iIndex].dPlayerDecay );
01991 SS->setInertiaMoment ( pt[iIndex].dInertiaMoment );
01992 SS->setDashPowerRate ( pt[iIndex].dDashPowerRate );
01993 SS->setPlayerSize ( pt[iIndex].dPlayerSize );
01994 SS->setKickableMargin( pt[iIndex].dKickableMargin );
01995 SS->setKickRand ( pt[iIndex].dKickRand );
01996 SS->setExtraStamina ( pt[iIndex].dExtraStamina );
01997 SS->setEffortMax ( pt[iIndex].dEffortMax );
01998 SS->setEffortMin ( pt[iIndex].dEffortMin );
01999
02000 return true;
02001 }
02002
02006 bool WorldModel::resetTimeObjects( )
02007 {
02008 Ball.setTimeLastSeen ( Time( -1, 0) );
02009 for( int i = 0 ; i < MAX_TEAMMATES ; i ++ )
02010 Teammates[i].setTimeLastSeen ( Time( -1, 0) );
02011 for( int i = 0 ; i < MAX_OPPONENTS ; i ++ )
02012 Opponents[i].setTimeLastSeen ( Time( -1, 0) );
02013 for( int i = 0 ; i < MAX_FLAGS ; i ++ )
02014 Flags[i].setTimeLastSeen ( Time( -1, 0) );
02015 for( int i = 0 ; i < MAX_LINES ; i ++ )
02016 Lines[i].setTimeLastSeen ( Time( -1, 0) );
02017 agentObject.setTimeLastSeen ( Time( -1, 0) );
02018 return true;
02019 }
02020
02024 void WorldModel::removeGhosts( )
02025 {
02026 AngDeg dAngle=SoccerTypes::getHalfViewAngleValue(agentObject.getViewAngle());
02027 dAngle -= 0.15*dAngle;
02028
02029 if( fabs( getRelativeAngle( OBJECT_BALL ) ) < dAngle
02030 && getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
02031 {
02032 Log.log( 556, "ball not in cone: set time ball at -1 %f %f",
02033 fabs( getRelativeAngle( OBJECT_BALL ) ), dAngle );
02034 Ball.setTimeLastSeen( Time( -1, 0 ) );
02035 }
02036
02037
02038 if( fabs( 0.9*getRelativeDistance( OBJECT_BALL ) ) < SS->getVisibleDistance()
02039 && getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
02040 {
02041 Log.log( 556, "ball not in vis. dist: set time ball at -1 %f %f",
02042 fabs( getRelativeAngle( OBJECT_BALL ) ), dAngle );
02043 Ball.setTimeLastSeen( Time( -1, 0 ) );
02044 }
02045
02046
02047
02048
02049
02050 int iIndex;
02051 for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS );
02052 o != OBJECT_ILLEGAL;
02053 o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS ) )
02054 {
02055 if( fabs( getRelativeAngle( o ) ) < dAngle
02056 && getTimeLastSeen( o ) != getTimeLastSeeMessage() )
02057 {
02058 Log.log( 556, "opp %d not in cone: set time at -1 %f %f",
02059 SoccerTypes::getIndex( o ) + 1,
02060 fabs( getRelativeAngle( o ) ), dAngle );
02061 setTimeLastSeen( o, Time( -1, 0 ) );
02062 }
02063 }
02064 }
02065
02071 bool WorldModel::calculateStateBall2( VecPosition *posGlobal,
02072 VecPosition *velGlobal )
02073 {
02074
02075 int iNrParticlesLeft = iNrParticlesBall;
02076
02077
02078 checkParticlesBall ( particlesPosBall, particlesVelBall,
02079 iNrParticlesBall, &iNrParticlesLeft );
02080 if( iNrParticlesLeft == 0 )
02081 {
02082 initParticlesBall ( particlesPosBall, particlesVelBall, iNrParticlesBall );
02083 iNrParticlesLeft = iNrParticlesBall;
02084 }
02085
02086
02087 *posGlobal = averageParticles( particlesPosBall, iNrParticlesLeft );
02088 *velGlobal = averageParticles( particlesVelBall, iNrParticlesLeft );
02089 resampleParticlesBall( particlesPosBall, particlesVelBall,
02090 iNrParticlesBall, iNrParticlesLeft );
02091
02092
02093
02094 if( Ball.getTimeChangeInformation( ) != getTimeLastSeen( OBJECT_BALL ) &&
02095 getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() &&
02096 getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() == 1 )
02097 {
02098
02099
02100
02101
02102
02103
02104 VecPosition posGlobalDiff = *posGlobal - Ball.getGlobalPositionLastSee()
02105 - agentObject.getPositionDifference();
02106
02107 *velGlobal = posGlobalDiff*SS->getBallDecay();
02108 }
02109
02110
02111
02112 if( getTimeSinceLastCatch() < 2 || getPlayMode() != PM_PLAY_ON )
02113 velGlobal->setMagnitude( 0.0 );
02114 else if( getNrInSetInCircle( OBJECT_SET_OPPONENTS,
02115 Circle(*posGlobal,SS->getMaximalKickDist())) > 0 )
02116 velGlobal->setMagnitude( 0.0 );
02117 else if( velGlobal->getMagnitude() >
02118 ( 1.0 + SS->getBallRand() )*SS->getBallSpeedMax() )
02119 velGlobal->setMagnitude( SS->getBallSpeedMax() );
02120
02121 if( isBeforeKickOff() )
02122 posGlobal->setVecPosition( 0, 0 );
02123
02124 return true;
02125 }
02126
02136 void WorldModel::initParticlesBall( VecPosition posArray[],
02137 VecPosition velArray[], int iLength )
02138 {
02139
02140 double dDistBall = UnknownDoubleValue, dDistChange = UnknownDoubleValue;
02141 AngDeg angBall = UnknownAngleValue, angChange = UnknownAngleValue;
02142 double dMinDist, dMaxDist, dMinCh, dMaxCh, dDistTmp, dDistChTmp, dVelX, dVelY;
02143 AngDeg angChMin, angChMax, angTmp, angChTmp;
02144
02145
02146 if( Ball.getTimeRelativePosition() != getTimeLastSeeMessage() )
02147 return;
02148
02149
02150 dDistBall = getRelativeDistance( OBJECT_BALL );
02151 angBall = getRelativeAngle( OBJECT_BALL );
02152
02153
02154 if( Ball.getTimeChangeInformation( ) == getTimeLastSeeMessage() )
02155 {
02156 dDistChange = Ball.getRelativeDistanceChange();
02157 angChange = Ball.getRelativeAngleChange();
02158 }
02159
02160
02161 getMinMaxDistQuantizeValue( dDistBall, &dMinDist, &dMaxDist, 0.1, 0.1 );
02162 getMinMaxDistChange( dDistChange, dDistBall, &dMinCh, &dMaxCh, 0.02, 0.1,0.1);
02163 getMinMaxDirChange ( angChange, &angChMin, &angChMax, 0.1 );
02164
02165 for( int i = 0; i < iLength; i ++ )
02166 {
02167
02168 dDistTmp = dMinDist + drand48()*fabs(dMaxDist - dMinDist);
02169 angTmp = angBall + drand48() - 0.5;
02170
02171 posArray[i].setVecPosition( dDistTmp, angTmp, POLAR );
02172 posArray[i].relativeToGlobal( getAgentGlobalPosition(),
02173 getAgentGlobalNeckAngle() );
02174
02175 if( dDistChange != UnknownDoubleValue )
02176 {
02177
02178 angChTmp = angChMin + drand48()*(angChMax-angChMin);
02179 dDistChTmp = dMinCh + drand48()*(dMaxCh-dMinCh);
02180
02181 dVelX=dDistChTmp*cosDeg(angTmp)-Deg2Rad(angChTmp)*dDistTmp*sinDeg(angTmp);
02182 dVelY=dDistChTmp*sinDeg(angTmp)+Deg2Rad(angChTmp)*dDistTmp*cosDeg(angTmp);
02183
02184 velArray[i].setVecPosition( dVelX, dVelY );
02185 velArray[i].relativeToGlobal( getAgentGlobalVelocity(),
02186 getAgentGlobalNeckAngle() );
02187 }
02188 else
02189 velArray[i].setVecPosition( 0, 0 );
02190 }
02191 }
02192
02203 void WorldModel::checkParticlesBall( VecPosition posArray[],
02204 VecPosition velArray[], int iLength, int *iNrParticlesLeft )
02205 {
02206 bool bIllegal;
02207 double dMinDist, dMaxDist, dMinCh, dMaxCh, dMag;
02208 double dDistBall = UnknownDoubleValue, dDistChange = UnknownDoubleValue;
02209 AngDeg angBall = UnknownAngleValue, angChange = UnknownAngleValue;
02210 double dDistChTmp;
02211 AngDeg angChTmp, angChMin, angChMax;
02212 VecPosition pos_rel, vel_rel;
02213
02214 int i1, i2, i3, i4;
02215 i1 = i2 = i3 = i4 = 0;
02216
02217
02218 if( getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
02219 return;
02220
02221
02222
02223 dDistBall = getRelativeDistance( OBJECT_BALL );
02224 angBall = getRelativeAngle( OBJECT_BALL );
02225 getMinMaxDistQuantizeValue( dDistBall, &dMinDist, &dMaxDist, 0.1, 0.1 );
02226
02227 if( getTimeLastSeen( OBJECT_BALL ) == Ball.getTimeChangeInformation( ) )
02228 {
02229 dDistChange = Ball.getRelativeDistanceChange();
02230 angChange = Ball.getRelativeAngleChange();
02231 getMinMaxDirChange ( angChange, &angChMin, &angChMax, 0.1);
02232 getMinMaxDistChange( dDistChange, dDistBall, &dMinCh, &dMaxCh,0.02,0.1,0.1);
02233
02234 }
02235
02236 *iNrParticlesLeft = 0;
02237 for( int i = 0; i < iLength; i ++ )
02238 {
02239
02240 pos_rel = posArray[i];
02241 vel_rel = velArray[i];
02242 pos_rel.globalToRelative( getAgentGlobalPosition(),
02243 getAgentGlobalNeckAngle() );
02244 vel_rel.globalToRelative( getAgentGlobalVelocity(),
02245 getAgentGlobalNeckAngle() );
02246
02247 bIllegal = false;
02248
02249 dMag = pos_rel.getMagnitude();
02250 if( dMag < dMinDist || dMag > dMaxDist )
02251 {
02252 i1++;
02253 bIllegal = true;
02254 }
02255 if( fabs( VecPosition::normalizeAngle(pos_rel.getDirection() - angBall) )
02256 > 0.5 )
02257 {
02258 bIllegal = true;
02259 i2++;
02260 }
02261
02262 if( dDistChange != UnknownDoubleValue )
02263 {
02264 dDistChTmp = (vel_rel.getX()*(pos_rel.getX()/dMag)) +
02265 (vel_rel.getY()*(pos_rel.getY()/dMag));
02266 angChTmp = Rad2Deg( ((vel_rel.getY()*pos_rel.getX()/dMag) -
02267 (vel_rel.getX()*pos_rel.getY()/dMag)))/dMag ;
02268
02269 if( angChTmp < angChMin || angChTmp > angChMax )
02270 {
02271 bIllegal = true;
02272 i3++;
02273 }
02274 if( dDistChTmp < dMinCh || dDistChTmp > dMaxCh )
02275 {
02276 bIllegal = true;
02277 i4++;
02278 }
02279 }
02280
02281
02282 if( bIllegal == false )
02283 {
02284 posArray[*iNrParticlesLeft] = posArray[i];
02285 velArray[(*iNrParticlesLeft)++] = velArray[i];
02286 }
02287 }
02288 }
02289
02298 void WorldModel::updateParticlesBall( VecPosition posArray[],
02299 VecPosition velArray[], int iLength, double dPower, AngDeg ang )
02300 {
02301 double dRand = SS->getBallRand();
02302 double dMaxRand;
02303
02304 for( int i = 0; i < iLength; i ++ )
02305 {
02306
02307 if( dPower > EPSILON )
02308 {
02309 ang = VecPosition::normalizeAngle(ang + getAgentGlobalBodyAngle() );
02310 velArray[i] += VecPosition( getActualKickPowerRate()*dPower, ang, POLAR) ;
02311 if( velArray[i].getMagnitude() > SS->getBallSpeedMax() )
02312 velArray[i].setMagnitude( SS->getBallSpeedMax() );
02313 }
02314
02315
02316 dMaxRand = dRand * velArray[i].getMagnitude();
02317 velArray[i] += VecPosition(
02318 (-1 + 2*drand48())*dMaxRand,
02319 (-1 + 2*drand48())*dMaxRand );
02320 posArray[i] += velArray[i];
02321 velArray[i] *= SS->getBallDecay();
02322 }
02323 }
02324
02325
02335 void WorldModel::resampleParticlesBall( VecPosition posArray[],
02336 VecPosition velArray[], int iLength, int iLeft )
02337 {
02338 int iRand = 0;
02339 for ( int i = iLeft; i < iLength; i ++ )
02340 {
02341 iRand = (int)(drand48()*iLeft);
02342 posArray[ i ] = posArray[ iRand ];
02343 velArray[ i ] = velArray[ iRand ];
02344 }
02345 }
02346
02356 ObjectT WorldModel::getMaxRangeUnknownPlayer( ObjectT obj, char* strMsg )
02357 {
02358 if( obj == OBJECT_PLAYER_UNKNOWN )
02359 return OBJECT_ILLEGAL;
02360
02361 list<ObjectT> l;
02362 ObjectT o;
02363 bool isGoalie, bCont = true;
02364 int i;
02365 bool isTeammate = SoccerTypes::isTeammate( obj );
02366 ObjectT objMax = (isTeammate==true) ? OBJECT_TEAMMATE_11 : OBJECT_OPPONENT_11;
02367
02368 while( bCont == true )
02369 {
02370 i = Parse::gotoFirstOccurenceOf( '(', &strMsg );
02371 if( i == -1 )
02372 bCont = false;
02373 else
02374 {
02375 strMsg++;
02376 o = SoccerTypes::getObjectFromStr(&strMsg,&isGoalie,getTeamName());
02377 if( ( isTeammate && SoccerTypes::isTeammate( o ) ) ||
02378 ( !isTeammate && SoccerTypes::isOpponent( o ) ) )
02379 l.push_back( o );
02380 }
02381 }
02382
02383 while( ! l.empty() )
02384 {
02385 o = l.back();
02386 l.pop_back();
02387 if( SoccerTypes::isKnownPlayer( o ) )
02388 objMax = o;
02389
02390 i = SoccerTypes::getIndex( objMax );
02391 if( isTeammate )
02392 objMax = SoccerTypes::getTeammateObjectFromIndex( i - 1 );
02393 else
02394 objMax = SoccerTypes::getOpponentObjectFromIndex( i - 1 );
02395 }
02396
02397 return objMax;
02398 }
02399