Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

WorldModelUpdate.C

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2000,2001, Jelle Kok, University of Amsterdam
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without 
00006 modification, are permitted provided that the following conditions are met:
00007 
00008 1. Redistributions of source code must retain the above copyright notice, this 
00009 list of conditions and the following disclaimer. 
00010 
00011 2. Redistributions in binary form must reproduce the above copyright notice, 
00012 this list of conditions and the following disclaimer in the documentation 
00013 and/or other materials provided with the distribution. 
00014 
00015 3. Neither the name of the University of Amsterdam nor the names of its 
00016 contributors may be used to endorse or promote products derived from this 
00017 software without specific prior written permission. 
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00020 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00021 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
00022 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
00023 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
00024 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
00025 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
00026 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
00027 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
00028 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 */
00044 #include "WorldModel.h"
00045 #include <stdio.h>      // needed for sprintf
00046 
00047 /******************************************************************************/
00048 /********************** CLASS WORLDMODEL **************************************/
00049 /******************************************************************************/
00050 
00051 
00063 void WorldModel::processSeeGlobalInfo( ObjectT o, Time time,
00064       VecPosition pos, VecPosition vel, AngDeg angBody, AngDeg angNeck)
00065 {
00066   DynamicObject * dobj;
00067   PlayerObject * pobj;
00068 
00069   if( o == OBJECT_ILLEGAL )
00070     return;
00071   if( SoccerTypes::isPlayer( o ) )
00072   {
00073     pobj = (PlayerObject*)getObjectPtrFromType( o );
00074     pobj->setTimeLastSeen( time );
00075     pobj->setGlobalPosition( pos, time );
00076     pobj->setGlobalVelocity( vel, time );
00077     pobj->setGlobalBodyAngle( angBody, time );
00078     pobj->setGlobalNeckAngle( VecPosition::normalizeAngle(angBody+angNeck),
00079                                                                      time );
00080     pobj->setIsKnownPlayer( true );
00081   }
00082   else if( SoccerTypes::isBall( o ) )
00083   {
00084     dobj = (DynamicObject*)getObjectPtrFromType( o );
00085     dobj->setTimeLastSeen( time );
00086     dobj->setGlobalPosition( pos, time );
00087     dobj->setGlobalVelocity( vel, time );
00088   }
00089 }
00090 
00102 bool WorldModel::processNewAgentInfo( ViewQualityT vq, ViewAngleT va,
00103      double dStamina, double dEffort, double dSpeed, AngDeg angSpeed,
00104      AngDeg angHeadAngle )
00105 {
00106   Stamina sta = agentObject.getStamina();
00107 
00108   sta.setStamina                   ( dStamina                              );
00109   sta.setEffort                    ( dEffort                               );
00110   agentObject.setStamina           ( sta                                   );
00111   agentObject.setViewQuality       ( vq                                    );
00112   agentObject.setViewAngle         ( va                                    );
00113   agentObject.setSpeedRelToNeck    ( VecPosition( dSpeed, angSpeed, POLAR) );
00114   agentObject.setBodyAngleRelToNeck( angHeadAngle                          );
00115 
00116   return true;
00117 }
00118 
00138 void WorldModel::processNewObjectInfo( ObjectT o, Time time,
00139       double dDist, int iDir, double dDistChange, double dDirChange,
00140       AngDeg angRelBodyAng,   AngDeg angRelNeckAng, bool isGoalie )
00141 {
00142   if( dDist == UnknownDoubleValue || o == OBJECT_ILLEGAL )
00143     return; // no sense to update when only angle is known.
00144 
00145   if( SoccerTypes::isFlag( o ) )
00146   {
00147     Flags[SoccerTypes::getIndex(o)].setRelativePosition(dDist,(double)iDir,time);
00148     Flags[SoccerTypes::getIndex(o)].setTimeLastSeen    ( time                );
00149     Flags[SoccerTypes::getIndex(o)].setType            ( o                   );
00150   }
00151   else if( SoccerTypes::isPlayer( o ) || SoccerTypes::isBall( o ) )
00152   {
00153     DynamicObject *d = NULL;
00154 
00155     // if we do not have all information, update UnknownPlayer array
00156     if( !( SoccerTypes::isKnownPlayer( o ) || SoccerTypes::isBall( o ) ) )
00157     {
00158       UnknownPlayers[iNrUnknownPlayers].setIsKnownPlayer( false );
00159       d = &UnknownPlayers[iNrUnknownPlayers];
00160       iNrUnknownPlayers++;
00161     }
00162     else // else update the known object (teammate, opponent, ball)
00163     {
00164       d = (DynamicObject*)getObjectPtrFromType( o );
00165       if( SoccerTypes::isPlayer( o ) )
00166         ((PlayerObject*)d)->setIsKnownPlayer( true );
00167     }
00168 
00169     if( d != NULL )  // if object was known
00170     {
00171       // set all values for this dynamicobject      
00172       d->setRelativePosition( dDist, (double)iDir, time );
00173       if( dDistChange != UnknownDoubleValue )
00174         d->setRelativeDistanceChange( dDistChange, time );
00175       if( dDirChange != UnknownDoubleValue )
00176         d->setRelativeAngleChange( dDirChange, time );
00177       if( angRelBodyAng != UnknownAngleValue )
00178         ((PlayerObject*)d)->setRelativeBodyAngle( angRelBodyAng, time );
00179       if( angRelNeckAng != UnknownAngleValue )
00180         ((PlayerObject*)d)->setRelativeNeckAngle( angRelNeckAng, time );
00181       if( isGoalie == true && SoccerTypes::isPlayer( o ))
00182         ((PlayerObject*)d)->setIsGoalie( true );
00183       else if( SoccerTypes::isPlayer( o ))
00184         ((PlayerObject*)d)->setIsGoalie( false );
00185       d->setType( o );
00186       d->setTimeLastSeen( time );
00187     }
00188   }
00189   else if( SoccerTypes::isLine( o ) )
00190   {
00191     // angle returned is angle of neck angle with line, convert to angle
00192     // of neck with orthogonal to line
00193     iDir = ( iDir < 0 ) ? (90 + iDir ) : - (90 - iDir );
00194     Lines[SoccerTypes::getIndex(o)].setRelativePosition(dDist,(double)iDir,time);
00195     Lines[SoccerTypes::getIndex(o)].setTimeLastSeen( time );
00196     Lines[SoccerTypes::getIndex(o)].setType( o );
00197   }
00198 }
00199 
00207 bool WorldModel::processPerfectHearInfoBall( VecPosition posGlobal,
00208                              VecPosition vel, double dConf )
00209 {
00210   if( Ball.getConfidence( getCurrentTime() ) < dConf )
00211   {
00212     Time time = getTimeFromConfidence( dConf );
00213     Ball.setGlobalPosition( posGlobal, time );
00214     Ball.setGlobalVelocity( vel,       time );
00215     updateObjectRelativeFromGlobal( OBJECT_BALL );
00216     return true;
00217   }
00218   return false;
00219 }
00220 
00232 bool WorldModel::processPerfectHearInfo( ObjectT o, VecPosition posGlobal,
00233                                          double dConf, bool bIsGoalie )
00234 {
00235   if( SoccerTypes::isBall( o ) || o == getAgentObjectType() )
00236     return false; // ball should be called with processPerfectHearInfoBall
00237   else if( !SoccerTypes::isKnownPlayer( o ) )
00238     return processUnsureHearInfo( o, posGlobal, dConf );
00239 
00240   PlayerObject *object = (PlayerObject *)getObjectPtrFromType( o );
00241   if( object == NULL )
00242     return false;
00243 
00244   Time time = getTimeFromConfidence( dConf ) ;
00245 
00246   // if we are not sure about the exact player number of this player in
00247   // the world model (getIsKnownPlayer() == false) we overwrite the
00248   // information of this player since the player who said this information
00249   // is sure about it (otherwise processUnsureHearInfo would be called instead
00250   // of processPERFECTHearInfo)
00251   if( object->getConfidence( getCurrentTime() ) < dConf ||
00252       object->getIsKnownPlayer() == false  )
00253   {
00254     object->setGlobalPosition     ( posGlobal         , time );
00255     object->setTimeLastSeen       ( time                     );
00256     object->setGlobalVelocity     ( VecPosition( 0, 0), time );
00257     object->setIsKnownPlayer      ( true                     );
00258     object->setIsGoalie           ( bIsGoalie                );
00259     updateObjectRelativeFromGlobal( o                        );
00260     return true;
00261   }
00262   return false;
00263 }
00264 
00276 bool WorldModel::processUnsureHearInfo( ObjectT o, VecPosition pos,
00277                                                double dConf )
00278 {
00279   double     dMinDist;        // used to find closest player to pos
00280   ObjectT    objInitial = o;
00281 
00282   if( o != OBJECT_TEAMMATE_UNKNOWN && o != OBJECT_OPPONENT_UNKNOWN )
00283     return false;
00284 
00285   // if o is a teammate find closest teammate to pos and store distance
00286   if( SoccerTypes::isTeammate( o ) )
00287     o = getClosestInSetTo( OBJECT_SET_TEAMMATES, pos, &dMinDist);
00288   else if( SoccerTypes::isOpponent( o ) )  // if o is an opponent, do the same
00289     o = getClosestInSetTo( OBJECT_SET_OPPONENTS, pos, &dMinDist);
00290 
00291   if( o == getAgentObjectType() && 
00292      pos.getDistanceTo(getAgentGlobalPosition())<PS->getPlayerDistTolerance())
00293     return false;  // do not update my own position, localization is better
00294 
00295   // if opponent or teammate was found and distance lies in tolerance distance
00296   //  update this opponent or teammate with the specified information.
00297   // else put the information in the first player position of which we have
00298   //  no information.
00299   else if( SoccerTypes::isKnownPlayer(o) && 
00300            dMinDist < PS->getPlayerDistTolerance())
00301   {
00302     processPerfectHearInfo( o, pos, dConf );
00303     return true;
00304   }
00305 
00306   if( objInitial == OBJECT_TEAMMATE_UNKNOWN )
00307     o = getFirstEmptySpotInSet( OBJECT_SET_TEAMMATES );
00308   else if( objInitial == OBJECT_OPPONENT_UNKNOWN )
00309     o = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
00310   else
00311     return false ;  // in case of OBJECT_PLAYER_UNKNOWN
00312 
00313   if( o != OBJECT_ILLEGAL )   // can be the case that there is no empty spot
00314   {
00315     processPerfectHearInfo( o, pos, dConf );
00316     setIsKnownPlayer( o, false );
00317   }
00318   return true;
00319 }
00320 
00341 bool WorldModel::processNewHeteroPlayer( int iIndex,     double dPlayerSpeedMax,
00342             double dStaminaIncMax, double dPlayerDecay,  double dInertiaMoment,
00343             double dDashPowerRate, double dPlayerSize,   double dKickableMargin,
00344             double dKickRand,      double dExtraStamina, double dEffortMax,
00345             double dEffortMin )
00346 {
00347    pt[iIndex].dPlayerSpeedMax = dPlayerSpeedMax;
00348    pt[iIndex].dStaminaIncMax  = dStaminaIncMax;
00349    pt[iIndex].dPlayerDecay    = dPlayerDecay;
00350    pt[iIndex].dInertiaMoment  = dInertiaMoment;
00351    pt[iIndex].dDashPowerRate  = dDashPowerRate;
00352    pt[iIndex].dPlayerSize     = dPlayerSize;
00353    pt[iIndex].dKickableMargin = dKickableMargin;
00354    pt[iIndex].dKickRand       = dKickRand;
00355    pt[iIndex].dExtraStamina   = dExtraStamina;
00356    pt[iIndex].dEffortMax      = dEffortMax;
00357    pt[iIndex].dEffortMin      = dEffortMin;
00358    return true;
00359 }
00360 
00369 void WorldModel::processCatchedBall( RefereeMessageT rm, Time time )
00370 {
00371   if( rm == REFC_GOALIE_CATCH_BALL_LEFT && sideSide == SIDE_LEFT )
00372     timeLastCatch = time;
00373   else if( rm == REFC_GOALIE_CATCH_BALL_RIGHT && sideSide == SIDE_RIGHT )
00374     timeLastCatch = time;
00375   Ball.setGlobalVelocity( VecPosition(0,0), getCurrentTime() );
00376 }
00377 
00386 void WorldModel::processQueuedCommands( SoccerCommand commands[],int iCommands )
00387 {
00388   if( iCommands > MAX_COMMANDS )
00389   {
00390     cerr << "(WorldModel::setQueuedCommands) queuedCommands array cannot "
00391          << "contain so many commands!\n";
00392     return;
00393   }
00394 
00395   // put all sent commands to the array which stores queued commands.
00396   for( int i = 0 ; i < iCommands ; i ++ )
00397   {
00398     commands[i].time                             = getCurrentTime();
00399     queuedCommands[(int)commands[i].commandType] = commands[i];
00400   }
00401 }
00402 
00410 bool WorldModel::updateAll( )
00411 {
00412   bool        bReturn            = false, bUpdateAfterSee = false;
00413   bool        bUpdateAfterSense  = false;
00414   static Time timeBeginInterval;
00415   static Time timePlayersCounted;  
00416   static int  iNrHolesLastTime   = 0;
00417   static Time timeLastSenseUpdate;
00418   static Time timeLastSeeUpdate;
00419 
00420   // check if last update of agent was not more than one cycle ago
00421   if( agentObject.getTimeGlobalPosition() < getCurrentTime() - 1  )
00422     Log.log( 3, "(WorldModel::updateAfterSenseMessage) missed a sense??" );
00423 
00424   // call update method depending on last received message
00425   if( getTimeLastSeeMessage() > timeLastSeeUpdate )
00426     bUpdateAfterSee = true;
00427   if( getTimeLastSenseMessage() > timeLastSenseUpdate )
00428     bUpdateAfterSense = true;
00429 
00430   // rare situation: can occur that see arrives between sense and calling 
00431   // updateAll or sense arrives between see and calling updateAll.    
00432   if( bUpdateAfterSee && bUpdateAfterSense )
00433   {
00434     Log.log( 3, "!!! Two updates !!! " );
00435     Log.log( 3, "see: %d sense: %d", getTimeLastSeeMessage().getTime(),
00436        getTimeLastSenseMessage().getTime() );
00437     if( getTimeLastSeeMessage( ) == getTimeLastSenseMessage() )
00438     {
00439       bReturn  = updateAfterSenseMessage( );    
00440       bReturn &= updateAfterSeeMessage( );    
00441     }
00442     else if( getTimeLastSeeMessage( ) < getTimeLastSenseMessage() )
00443     {
00444       bReturn  = updateAfterSeeMessage( );    
00445       bReturn &= updateAfterSenseMessage( );          
00446       updateRelativeFromGlobal();  
00447     }
00448   
00449     timeLastSenseUpdate = getTimeLastSenseMessage();            
00450      timeLastSeeUpdate = getTimeLastSeeMessage();
00451   }
00452   else if( bUpdateAfterSee )
00453   {
00454     bReturn  = updateAfterSeeMessage( );    
00455      timeLastSeeUpdate = getTimeLastSeeMessage();
00456   }    
00457   else if( bUpdateAfterSense )
00458   {
00459     bReturn  = updateAfterSenseMessage( );    
00460      timeLastSenseUpdate = getTimeLastSenseMessage();
00461     updateRelativeFromGlobal();      
00462   }
00463 
00464   // determine number of holes in last time interval and act accordingly
00465   int    iTimeDiff = getCurrentTime() - timeBeginInterval;
00466   double dHolePerc = (double)(iNrHoles - iNrHolesLastTime)/iTimeDiff*100;
00467   if( ! isLastMessageSee( ) && iTimeDiff % 400 == 0 && dHolePerc > 1.0 &&
00468       PS->getFractionWaitSeeEnd() > 0.70 )
00469   {
00470     PS->setFractionWaitSeeEnd( PS->getFractionWaitSeeEnd() - 0.05 );
00471     timeBeginInterval = getCurrentTime();
00472     cerr << "Lowered send time to " << PS->getFractionWaitSeeEnd() <<
00473             " for player number "   << getPlayerNumber()           <<
00474             "; nr of holes is "<< dHolePerc << "%" << endl;
00475     iNrHolesLastTime   = iNrHoles;
00476   }
00477 
00478 
00479   // store some statistics about number of players seen each cycle
00480   if( bUpdateAfterSee == true  && ! isTimeStopped() &&
00481       getCurrentTime() != timePlayersCounted )
00482   {
00483     iNrTeammatesSeen += getNrInSetInRectangle( OBJECT_SET_TEAMMATES );
00484     iNrOpponentsSeen += getNrInSetInRectangle( OBJECT_SET_OPPONENTS );
00485     timePlayersCounted = getCurrentTime();
00486   }
00487      
00488   // log specific information when log level is set
00489   if( Log.isInLogLevel( 456 ) )
00490     logObjectInformation( 456, getAgentObjectType() );
00491 
00492   if( bUpdateAfterSee == true )
00493     Log.logWithTime( 3, "  finished update_all see; start determining action" );
00494   if( bUpdateAfterSense == true )
00495     Log.logWithTime( 3, "  finished update_all sense; start determining action" );
00496   Log.log( 3, "global pos: (%f, %f)", getAgentGlobalPosition().getX(), 
00497               getAgentGlobalPosition().getY() );
00498   return bReturn;
00499 
00500 }
00501 
00502 /******************************************************************************/
00503 /*************** WORLDMODEL: SEE RELATED UPDATES ******************************/
00504 /******************************************************************************/
00505 
00511 bool WorldModel::updateAfterSeeMessage( )
00512 {
00513    // update the agent (global position and angle using flags and lines)
00514   if( getCurrentTime().getTime() != -1 )
00515     updateAgentObjectAfterSee( );
00516 
00517   // walk past all players on the field an when new information was perceived
00518   // (and put in the relative attributes) update this dynamic object. When it
00519   // was not seen, convert its global position (this is an estimate from the
00520   // sense message) to a relative position
00521   double dConfThr = PS->getPlayerConfThr();
00522   int    iIndex;
00523 
00524   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
00525        o != OBJECT_ILLEGAL;
00526        o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
00527   {
00528     if( getTimeLastSeen( o ) == getTimeLastSeeMessage() &&
00529         o != getAgentObjectType() )
00530       updateDynamicObjectAfterSee   ( o );
00531     else
00532       updateObjectRelativeFromGlobal( o );
00533   }
00534   iterateObjectDone( iIndex);
00535 
00536   // if ball was seen update him, otherwise make estimated global relative
00537   if( getTimeLastSeen( OBJECT_BALL ) == getTimeLastSeeMessage() )
00538     updateDynamicObjectAfterSee   ( OBJECT_BALL );
00539   else
00540     updateObjectRelativeFromGlobal( OBJECT_BALL );
00541 
00542   // delete objects from wordmodel that should have been seen, but aren't
00543   removeGhosts();
00544 
00545   return true;
00546 }
00547 
00552 bool WorldModel::updateAgentObjectAfterSee(  )
00553 {
00554   VecPosition posGlobal, velGlobal;
00555   AngDeg      angGlobal;
00556 
00557   // calculate the state of the agent
00558   calculateStateAgent( &posGlobal, &velGlobal, &angGlobal );
00559 
00560   // and set the needed attributes
00561   agentObject.setTimeLastSeen         ( getTimeLastSeeMessage() );
00562   // store difference with predicted global position to compensate for error
00563   // in global position when global velocity is calculated for other objects.
00564   agentObject.setPositionDifference(posGlobal- agentObject.getGlobalPosition());
00565   agentObject.setGlobalPosition       ( posGlobal, getTimeLastSeeMessage() );
00566   agentObject.setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00567   agentObject.setGlobalNeckAngle      ( angGlobal );
00568   agentObject.setGlobalVelocity       ( velGlobal, getTimeLastSeeMessage() );
00569 
00570   return true;
00571 }
00572 
00573 
00579 bool WorldModel::updateDynamicObjectAfterSee( ObjectT o )
00580 {
00581   VecPosition posGlobal, velGlobal;
00582 
00583   if( o == OBJECT_BALL )
00584   {
00585     calculateStateBall  ( &posGlobal, &velGlobal );
00586     Ball.setGlobalVelocity       ( velGlobal, getTimeLastSeeMessage() );
00587     Ball.setGlobalPosition       ( posGlobal, getTimeLastSeeMessage() );
00588     Ball.setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00589     return true;
00590   }
00591   else if( SoccerTypes::isKnownPlayer( o ) )
00592   {
00593     calculateStatePlayer( o, &posGlobal, &velGlobal );
00594 
00595     PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
00596 
00597     pob->setGlobalVelocity       ( velGlobal, getTimeLastSeeMessage() );
00598     pob->setGlobalPosition       ( posGlobal, getTimeLastSeeMessage() );
00599     pob->setGlobalPositionLastSee( posGlobal, getTimeLastSeeMessage() );
00600 
00601     if( pob->getTimeRelativeAngles() == getTimeLastSeeMessage() )
00602     {
00603       AngDeg ang = getAgentGlobalNeckAngle()+pob->getRelativeBodyAngle();
00604       ang = VecPosition::normalizeAngle( ang );
00605       pob->setGlobalBodyAngle( ang, getTimeLastSeeMessage() );
00606       ang = getAgentGlobalNeckAngle() + pob->getRelativeNeckAngle();
00607       ang = VecPosition::normalizeAngle( ang );
00608       pob->setGlobalNeckAngle( ang, getTimeLastSeeMessage() );
00609     }
00610     return true;
00611   }
00612 
00613   return false;
00614 }
00615 
00616 
00617 /******************************************************************************/
00618 /******************** SENSE RELATED UPDATES ***********************************/
00619 /******************************************************************************/
00620 
00629 bool WorldModel::updateAfterSenseMessage( )
00630 {
00631   // update agent information
00632   updateAgentAndBallAfterSense( );
00633 
00634   // update all global information of players that have a good confidence
00635   double dConfThr = PS->getPlayerConfThr();
00636   int    iIndex;
00637 
00638   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
00639        o != OBJECT_ILLEGAL;
00640        o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
00641   {
00642     if( o != getAgentObjectType() )
00643       updateDynamicObjectForNextCycle( o,
00644                                  getCurrentTime() - getTimeGlobalPosition(o) );
00645   }
00646   iterateObjectDone( iIndex);
00647 
00648   // update ball for collision with players
00649   updateBallForCollision();
00650 
00651   return true;
00652 }
00653 
00659 bool WorldModel::updateAgentAndBallAfterSense( )
00660 {
00661   // get info from commands from previous cycle (if timestopped current cycle)
00662   bool        bProcessedHole = false;
00663   bool        bBallUpdated   = false;
00664   VecPosition pos            = getAgentGlobalPosition();
00665   AngDeg      angGlobalNeck  = getAgentGlobalNeckAngle();
00666   AngDeg      angGlobalBody  = getAgentGlobalBodyAngle();
00667   Stamina     sta            = getAgentStamina();
00668   VecPosition vel            = agentObject.getSpeedRelToNeck(); // !!
00669   Time        time           = getCurrentTime() - 1 ;
00670 
00671   // calculate velocity at end of previous cycle using velocity from current
00672   // cycle. Although we do not know direction yet (this is relative to neck
00673   // which is not yet known), we can use the magnitude to determine travelled
00674   // distance (speed) of the agent
00675   // After neck angle is estimated, we can rotate velocity vector to get
00676   // actual velocity.
00677   vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
00678 
00679   // turn_neck is done before other commands, so should be estimated first
00680   int i = (int)CMD_TURNNECK;
00681   if( performedCommands[i] == true &&           // turn_neck was performed
00682       ( queuedCommands[i].time.getTime() == time.getTime() ||      // no hole
00683         queuedCommands[i].time.getTime() == time.getTime() - 1 ) ) // hole
00684   {
00685 
00686     if( queuedCommands[i].time.getTime() == time.getTime() - 1  &&
00687         time.getTime() > 0)
00688     {
00689       bProcessedHole = true;
00690       iNrHoles++;
00691     }
00692 
00693     predictStateAfterCommand( queuedCommands[i], &pos, &vel, &angGlobalBody,
00694                               &angGlobalNeck, &sta ) ;
00695     queuedCommands[i].time.updateTime( -1 );
00696   }
00697 
00698 
00699   // now all the other commands
00700   for( i = 0; i < MAX_COMMANDS; i ++  )
00701   {
00702     if( performedCommands[i] == true && // sense msg indicates we executed this
00703         ( queuedCommands[i].time.getTime() == time.getTime() ||      // no hole
00704           queuedCommands[i].time.getTime() == time.getTime() - 1 ) )// hole
00705     {
00706       if( queuedCommands[i].time.getTime() == time.getTime() - 1 &&
00707           bProcessedHole == false && time.getTime() > 0 )
00708       {
00709         bProcessedHole = true;                                 // process hole
00710         iNrHoles++;
00711       }
00712 
00713       switch( queuedCommands[i].commandType )
00714       {
00715         case CMD_KICK:
00716           updateBallAfterKick( queuedCommands[i].dPower,
00717                                queuedCommands[i].dAngle);
00718           bBallUpdated  = true;             // should not be updated later on.        
00719           break;
00720         case CMD_TURN:
00721           predictStateAfterCommand( queuedCommands[i], &pos, &vel,
00722                                     &angGlobalBody, &angGlobalNeck, &sta );                                  
00723           break;
00724         case CMD_MOVE:
00725           pos.setVecPosition( queuedCommands[i].dX, queuedCommands[i].dY );
00726           initParticlesAgent( pos );
00727           break;
00728         case CMD_DASH:       // not necessary to do anything, since resulting
00729                              // velocity is already available from sense, this
00730                              // velocity can be used to update global position
00731           break;
00732         case CMD_TURNNECK:   // already handled before this loop
00733           default:
00734             break;
00735       }
00736       queuedCommands[i].time.updateTime( -1 );                  // processed
00737     }
00738   }
00739 
00740 
00741   // reset pos and vel information to previous cycle (since can be changed in
00742   // predictStateAfterCommand)
00743   vel = agentObject.getSpeedRelToNeck();
00744   pos = getAgentGlobalPosition();
00745   vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
00746   vel.rotate( angGlobalNeck ); // rotate velocity using information about neck
00747 
00748   // update particles that keep track of position of agent using this vel
00749   updateParticlesAgent( vel, true );
00750 
00751   // predict the global position using the calculated velocity at the end of
00752   // the previous cycle (power and direction can thus both be set to zero).
00753   // There is just little noise in this perception, so gives a good estimate
00754   predictStateAfterDash( 0.0, &pos, &vel, &sta, 0 );
00755 
00756   // update ball if it hasn't been done yet
00757   if( !bBallUpdated )
00758   {
00759     updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
00760     updateParticlesBall( particlesPosBall, particlesVelBall, iNrParticlesBall, 0, 0 );    
00761   }
00762 
00763   // body angle is not set since relative angle to neck is already contained
00764   // in sense_body message, same holds for stamina
00765   agentObject.setGlobalPosition ( pos,        getCurrentTime() );
00766   agentObject.setGlobalVelocity ( vel,        getCurrentTime() );
00767   agentObject.setGlobalNeckAngle( angGlobalNeck );
00768 
00769 
00770   return true;
00771 }
00772 
00781 bool WorldModel::updateBallAfterKick( double dPower, AngDeg ang )
00782 {
00783   if( getRelativeDistance( OBJECT_BALL ) < SS->getMaximalKickDist() )
00784   {
00785     // make angle relative to body
00786     // calculate added acceleration and add it to current velocity
00787     ang = VecPosition::normalizeAngle(ang + getAgentGlobalBodyAngle() );
00788     VecPosition acc( getActualKickPowerRate()*dPower, ang, POLAR ) ;
00789     acc += getGlobalVelocity( OBJECT_BALL );
00790     if( acc.getMagnitude() > SS->getBallSpeedMax() )
00791       acc.setMagnitude( SS->getBallSpeedMax() );
00792     Ball.setGlobalPosition( getGlobalPosition( OBJECT_BALL ) + acc,
00793                                       getCurrentTime()  );
00794     Ball.setGlobalVelocity( acc*SS->getBallDecay(), getCurrentTime()  );
00795     updateParticlesBall( particlesPosBall, particlesVelBall, iNrParticlesBall, dPower, ang );
00796   }
00797   else
00798   {
00799     updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
00800     updateParticlesBall( particlesPosBall, particlesVelBall, iNrParticlesBall, 0, 0 );    
00801     Log.log( 21, "(WorldModel::%s) KICK command, but ball not kickable (%f)",
00802         __FUNCTION__, getRelativeDistance( OBJECT_BALL ) );
00803   }
00804   return true;
00805 }
00806 
00813 bool WorldModel::updateDynamicObjectForNextCycle( ObjectT obj, int iCycles)
00814 {
00815   DynamicObject *o = (DynamicObject*) getObjectPtrFromType( obj );
00816   if( o == NULL )
00817     return false;
00818 
00819   // get velocity and add it to current global position
00820   VecPosition vel = getGlobalVelocity( obj );
00821   VecPosition pos = getGlobalPosition( obj );
00822 
00823   for( int i = 0; i < iCycles ; i++ )
00824   {
00825     pos += vel;
00826     vel *= (obj==OBJECT_BALL) ? SS->getBallDecay() : SS->getPlayerDecay();
00827   }
00828   o->setGlobalVelocity ( vel, getCurrentTime() );
00829   o->setGlobalPosition ( pos, getCurrentTime() );
00830 
00831   return true;
00832 }
00833 
00834 
00839 bool WorldModel::updateBallForCollision()
00840 {
00841   VecPosition posBall = getGlobalPosition( OBJECT_BALL );
00842   VecPosition velBall = getGlobalVelocity( OBJECT_BALL );
00843   double      dist    = SS->getPlayerSize() + SS->getBallSize();
00844 
00845   // if collision occurs, set ball back to old position and multiply
00846   // vel ball with -0.1 (hope this is accurate enough)
00847   // do not check other players, due of noise
00848   if( getAgentGlobalPosition().getDistanceTo( posBall ) < dist )
00849   {
00850     Log.log( 552, "Collision occured" );
00851     posBall -= (velBall/SS->getBallDecay());     // use speed from last cycle
00852     Ball.setGlobalPosition( posBall, getCurrentTime() );
00853     Ball.setGlobalVelocity( velBall*-0.1, getCurrentTime() );
00854     return true;
00855   }
00856 
00857   return false;
00858 
00859 }
00860 
00866 bool WorldModel::updateRelativeFromGlobal()
00867 {
00868 
00869   double dConfThr = PS->getPlayerConfThr();
00870   int    iIndex;
00871 
00872   // update all player objects
00873   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_PLAYERS, dConfThr );
00874        o != OBJECT_ILLEGAL;
00875        o = iterateObjectNext ( iIndex, OBJECT_SET_PLAYERS, dConfThr ) )
00876   {
00877     if( o != getAgentObjectType() )
00878       updateObjectRelativeFromGlobal( o );
00879   }
00880   iterateObjectDone( iIndex);
00881 
00882   // and also the ball
00883   if( isConfidenceGood( OBJECT_BALL ) )
00884     updateObjectRelativeFromGlobal( OBJECT_BALL );
00885 
00886   // flags and lines are not updated, since they are not used except immediately
00887   // after see message when they're up to date.
00888   return true;
00889 }
00890 
00896 bool WorldModel::updateObjectRelativeFromGlobal( ObjectT o )
00897 {
00898   Object *obj = (Object*) getObjectPtrFromType( o );
00899   if( obj == NULL )
00900     return false;
00901 
00902   // get global position and translate and rotate it to agent neck
00903   VecPosition rel = obj->getGlobalPosition();
00904   rel.globalToRelative( getAgentGlobalPosition(), getAgentGlobalNeckAngle() );
00905   obj->setRelativePosition( rel, obj->getTimeGlobalPosition() );
00906   return true;
00907 }
00908 
00917 bool WorldModel::calculateStateAgent( VecPosition *posGlobal,
00918                      VecPosition *velGlobal, AngDeg *angGlobal )
00919 {
00920   int    iNrLeft;
00921 
00922   // first determine global neck angle
00923   ObjectT objLine = getFurthestRelativeInSet( OBJECT_SET_LINES );
00924   if( objLine != OBJECT_ILLEGAL )
00925   {
00926     double angGlobalLine = getGlobalAngle  ( objLine );
00927     AngDeg angRelLine    = getRelativeAngle( objLine );
00928     *angGlobal           = angGlobalLine - angRelLine;
00929     *angGlobal           = VecPosition::normalizeAngle( *angGlobal );
00930   }
00931   else
00932   {
00933     Log.log( 21, "(WorldModel::%s) no line in last see message", __FUNCTION__ );
00934     *angGlobal           = getAgentGlobalNeckAngle();
00935   }
00936 
00937   // use global neck angle to determine estimate of current global velocity
00938   // neck angle is better estimate than after sense -> better estimate velocity
00939   // update all position particles of the agent with this velocity
00940   //  'false' denotes update after see message. Since global neck angle can
00941   //  be determined more precise after 'see' it is better to predict position
00942   //  again (although it was already done after sense)
00943   // and then check which particles are possible given current perceptions
00944   *velGlobal = agentObject.getSpeedRelToNeck().rotate( *angGlobal );
00945   velGlobal->setMagnitude( velGlobal->getMagnitude()/SS->getPlayerDecay() );
00946 
00947   updateParticlesAgent          ( *velGlobal, false );
00948   iNrLeft = checkParticlesAgent ( *angGlobal        );
00949 
00950   if( iNrLeft == 0 ) // if no particles left, initialize all particles
00951   {
00952     // initialize particles (this makes random samples using one flag)
00953     // these particles are then again check with check method
00954     initParticlesAgent ( *angGlobal );
00955     iNrLeft = checkParticlesAgent( *angGlobal );
00956     if( iNrLeft == 0 )
00957     {
00958       // also does not succeed, use second method (weighted average flags)
00959       // and initialize all particles to this position
00960       calculateStateAgent2( posGlobal, velGlobal, angGlobal );
00961       initParticlesAgent( *posGlobal );
00962       return false;
00963     }
00964   }
00965 
00966   // determine global position (= average of all particles)
00967   // and resample all particles
00968   *posGlobal = averageParticles( particlesPosAgent, iNrLeft );
00969   resampleParticlesAgent( iNrLeft );
00970 
00971   // use the position to calculate better global neck angle of the agent
00972   // and recalculate global velocity with improved neck angle
00973   AngDeg ang = calculateAngleAgentWithPos( *posGlobal );
00974   if( ang != UnknownAngleValue )
00975     *angGlobal = ang;
00976 
00977   *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
00978   return true;
00979 }
00980 
00988 void WorldModel::initParticlesAgent( AngDeg angGlobal )
00989 {
00990   double  dDist, dMaxRadius, dMinRadius, dInput;
00991   AngDeg  ang;
00992 
00993   // get closest flag from which samples will be generated
00994   ObjectT objFlag = getClosestRelativeInSet( OBJECT_SET_FLAGS );
00995 
00996  if( objFlag == OBJECT_ILLEGAL )
00997  {
00998     Log.log( 21, "(WorldModel::%s) no flag in last see message", __FUNCTION__ );
00999     return;
01000  }
01001 
01002   // get the distance to this flag and the possible range it was derived from.
01003   dInput = getRelativeDistance( objFlag );
01004   getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01005                                                SS->getQuantizeStepL(), 0.1 ) ;
01006 
01007   // get the perceived angle to this flag (add 180 to get angle relative from
01008   // flag to agent ) and make it global by adding global neck angle agent.
01009   AngDeg angFlag   = getRelativeAngle( objFlag ) + 180 + angGlobal ;
01010 
01011   // for all particles
01012   for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01013   {
01014 
01015     // determine random point from distance range and
01016     // determine random point from the range it could be generated from
01017     // angles are rounded and since noise is in global neck angle and relative
01018     // angle flag, maximum error is in range [-0.5,0.5] + [-0.5,0.5] = [-1,1].
01019     dDist = dMinRadius + drand48()*(dMaxRadius-dMinRadius);
01020     ang   = VecPosition::normalizeAngle( angFlag - 1.0 + 2*drand48() );
01021 
01022     // create random point from possible interval
01023     particlesPosAgent[i] = getGlobalPosition( objFlag ) +
01024                            VecPosition( dDist, ang, POLAR );
01025   }
01026 }
01027 
01033 void WorldModel::initParticlesAgent( VecPosition posInitial )
01034 {
01035   for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01036     particlesPosAgent[i] = posInitial;
01037 }
01038 
01047 int WorldModel::checkParticlesAgent( AngDeg angGlobalNeck  )
01048 {
01049   double dMaxRadius, dMinRadius, dInput, dDist;
01050   AngDeg ang;
01051   int    iIndex, iNrLeft = 0, iLength = iNrParticlesAgent;
01052 
01053   // for all current perceived flags
01054   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_FLAGS, 1.0 );
01055        o != OBJECT_ILLEGAL;
01056        o = iterateObjectNext ( iIndex, OBJECT_SET_FLAGS, 1.0 ) )
01057   {
01058     iNrLeft = 0;                        // reset number of correct particles
01059     dInput = getRelativeDistance( o );  // get possible distance range
01060     getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01061                                     SS->getQuantizeStepL(), 0.1 )  ;
01062 
01063     // find all "correct points"
01064     for( int i = 0; i < iLength; i ++ )
01065     {
01066       // determine distance particle to flag
01067       // determine difference in direction between direction between global
01068       //  flag and agent position and global perceived direction
01069       dDist = particlesPosAgent[i].getDistanceTo( getGlobalPosition( o ) );
01070       ang = (getGlobalPosition(o) - particlesPosAgent[i]).getDirection();
01071       ang = ang - ( getRelativeAngle( o ) + angGlobalNeck );
01072 
01073       // if in possible range, save it (maximum angle range is 0.5 for
01074       // neck angle and 0.5 for relative flag angle gives 1.0)
01075       if( dDist > dMinRadius && dDist < dMaxRadius &&
01076           fabs(VecPosition::normalizeAngle( ang )) <= 1.0 )
01077         particlesPosAgent[iNrLeft++] = particlesPosAgent[i];
01078     }
01079     // change maximum of correct particles
01080     iLength = iNrLeft;
01081 
01082   }
01083   return iNrLeft;
01084 }
01085 
01096 void WorldModel::updateParticlesAgent( VecPosition vel, bool bAfterSense )
01097 {
01098   // used to denote last added velocity
01099   static VecPosition prev_vel;
01100 
01101   for( int i = 0; i < iNrParticlesAgent  ; i ++ )
01102   {
01103     if( bAfterSense == false ) // if after see, subtract last added 'vel'
01104     {
01105       particlesPosAgent[i].setX( particlesPosAgent[i].getX() - prev_vel.getX());
01106       particlesPosAgent[i].setY( particlesPosAgent[i].getY() - prev_vel.getY());
01107     }
01108 
01109     particlesPosAgent[i].setX( particlesPosAgent[i].getX( ) + vel.getX() );
01110     particlesPosAgent[i].setY( particlesPosAgent[i].getY( ) + vel.getY() );
01111   }
01112   prev_vel = vel;
01113 }
01114 
01115 
01120 VecPosition WorldModel::averageParticles( VecPosition posArray[], int iLength )
01121 {
01122   if( iLength == 0 )
01123     return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
01124 
01125   // take average of particles
01126   double x = 0, y = 0;
01127   for( int i = 0; i < iLength  ; i ++ )
01128   {
01129      x += posArray[ i ].getX( );
01130      y += posArray[ i ].getY( );
01131   }
01132 
01133   return VecPosition( x/iLength, y/iLength );
01134 }
01135 
01142 void WorldModel::resampleParticlesAgent( int iLeft )
01143 {
01144   for ( int i = iLeft; i < iNrParticlesAgent; i ++ )
01145     particlesPosAgent[ i ] = particlesPosAgent[ (int)(drand48()*iLeft) ];
01146 }
01147 
01159 VecPosition WorldModel::calculatePosAgentWith2Flags( ObjectT objFlag1,
01160                                                      ObjectT objFlag2 )
01161 {
01162     double      xA, yA, xB, yB, rA, rB;
01163     AngDeg      aA, aB;
01164 
01165     // get all information of the two flags
01166     xA = getGlobalPosition  ( objFlag1 ).getX();
01167     yA = getGlobalPosition  ( objFlag1 ).getY();
01168     rA = getRelativeDistance( objFlag1 );
01169     aA = getRelativeAngle   ( objFlag1 );
01170     xB = getGlobalPosition  ( objFlag2 ).getX();
01171     yB = getGlobalPosition  ( objFlag2 ).getY();
01172     rB = getRelativeDistance( objFlag2 );
01173     aB = getRelativeAngle   ( objFlag2 );
01174 
01175     double      L, dx, dy, d_par, d_orth;
01176     double      x, y;
01177     // Sign is like this 'because' y-axis increases from top to bottom
01178     double sign = ((aB - aA) > 0.0) ? 1.0 : -1.0;
01179 
01180     // From Cosinus rule: rB rB = L L + rA rA - 2 L rA cos(aB)
01181     // with:              rA cos(aB) = d_par
01182     // and:               d_par^2 + d_orth^2 = rA^2
01183     // Finally:
01184     // Position from landmark position and vectors parallel and orthogonal
01185     // to line from landmark A to B
01186 
01187     dx = xB - xA;
01188     dy = yB - yA;
01189     L = sqrt(dx*dx + dy*dy);                   // distance between two flags
01190 
01191     dx /= L; dy /= L;                          // normalize
01192 
01193     d_par = (L*L + rA*rA - rB*rB) / (2.0 * L); // dist from flag1 to orth proj
01194     double arg = rA*rA - d_par*d_par;
01195     d_orth = (arg > 0.0) ? sqrt(arg) : 0.0;
01196 
01197     x = xA + d_par * dx - sign * d_orth * dy;
01198     y = yA + d_par * dy + sign * d_orth * dx;
01199 
01200     return VecPosition( x, y );
01201 }
01202 
01212 AngDeg WorldModel::calculateAngleAgentWithPos( VecPosition pos )
01213 {
01214   int    iNrFlags = 0, iIndex;
01215   double dCosX=0, dSinX=0 ,dAngleNow, xA, yA, aA;
01216 
01217   for( ObjectT obj = iterateObjectStart( iIndex, OBJECT_SET_FLAGS, 1.0 );
01218        obj != OBJECT_ILLEGAL;
01219        obj = iterateObjectNext ( iIndex, OBJECT_SET_FLAGS, 1.0 ) )
01220   {
01221     xA = getGlobalPosition( obj ).getX();
01222     yA = getGlobalPosition( obj ).getY();
01223     aA = getRelativeAngle( obj );
01224 
01225     // calculate global direction between flag and agent
01226     // calculate global neck angle by subtracting relative angle to flag
01227     dAngleNow = atan2Deg( yA - pos.getY(), xA - pos.getX() );
01228     dAngleNow = VecPosition::normalizeAngle( dAngleNow - aA );
01229 
01230     // add cosine part of angle and sine part separately; this to avoid
01231     // boundary problem when computing average angle (average of -176 and
01232     // 178 equals -179 and not 1).
01233     dCosX += cosDeg( dAngleNow );
01234     dSinX += sinDeg( dAngleNow );
01235     iNrFlags++;
01236 
01237   }
01238   iterateObjectDone( iIndex );
01239 
01240   // calculate average cosine and sine part and determine corresponding angle
01241   dCosX /= (double)iNrFlags;
01242   dSinX /= (double)iNrFlags;
01243   if( iNrFlags == 0 )
01244     return UnknownAngleValue;
01245 
01246   return VecPosition::normalizeAngle( atan2Deg( dSinX, dCosX  ) )  ;
01247 }
01248 
01254 bool WorldModel::calculateStateBall( VecPosition *posGlobal,
01255                                      VecPosition *velGlobal )
01256 { 
01257   // initialize particles left with total number of particles
01258   int  iNrParticlesLeft = iNrParticlesBall;
01259 
01260   // check which particles are correct. If all gone, re-initialize all particles
01261   checkParticlesBall ( particlesPosBall, particlesVelBall, 
01262                                          iNrParticlesBall, &iNrParticlesLeft );
01263   if( iNrParticlesLeft == 0 )
01264   {
01265     initParticlesBall ( particlesPosBall, particlesVelBall, iNrParticlesBall );        
01266     iNrParticlesLeft = iNrParticlesBall;
01267   }
01268 
01269   // determine position and velocity and resample all deleted particles
01270   *posGlobal = averageParticles( particlesPosBall, iNrParticlesLeft );
01271   *velGlobal = averageParticles( particlesVelBall, iNrParticlesLeft );
01272   resampleParticlesBall( particlesPosBall, particlesVelBall, 
01273                                            iNrParticlesBall, iNrParticlesLeft );
01274 
01275   // if we had no chance information, but but ball is within feel distance
01276   // and we have also seen it in the previous cycle -> use position based vel.
01277   if( Ball.getTimeChangeInformation( ) != getTimeLastSeen( OBJECT_BALL ) &&
01278       getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() &&
01279       getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() == 1 )
01280   {
01281     // get the difference with the previous known global position
01282     // and subtract the position difference (this difference is caused by
01283     // the fact that the global position calculation contains a lot of noise
01284     // now the noise is filtered, since we compare the velocity as if its
01285     // position was seen "with the noise" of the last cycle).
01286 
01287     VecPosition posGlobalDiff = *posGlobal - Ball.getGlobalPositionLastSee()
01288                                            - agentObject.getPositionDifference();
01289 
01290     *velGlobal = posGlobalDiff*SS->getBallDecay();
01291   }
01292 
01293   // adapt velocity and position based on specific situations
01294   // set velocity to zero when in kick distance opponent for example
01295   if( getTimeSinceLastCatch() < 2 || getPlayMode() != PM_PLAY_ON )
01296     velGlobal->setMagnitude( 0.0 );
01297   else if( getNrInSetInCircle( OBJECT_SET_OPPONENTS,
01298                             Circle(*posGlobal,SS->getMaximalKickDist())) > 0 )
01299     velGlobal->setMagnitude( 0.0 );
01300   else if( velGlobal->getMagnitude() >
01301                            ( 1.0 + SS->getBallRand() )*SS->getBallSpeedMax() )
01302     velGlobal->setMagnitude( SS->getBallSpeedMax() );
01303 
01304   if( isBeforeKickOff() )
01305     posGlobal->setVecPosition( 0, 0 );
01306     
01307   return true;
01308 }
01309 
01319 void WorldModel::initParticlesBall( VecPosition posArray[], 
01320                                     VecPosition velArray[], int iLength )
01321 {
01322   // declare a bunch of variables
01323   double dDistBall = UnknownDoubleValue, dDistChange = UnknownDoubleValue;
01324   AngDeg angBall   = UnknownAngleValue,  angChange   = UnknownAngleValue;
01325   double dMinDist, dMaxDist, dMinCh, dMaxCh, dDistTmp, dDistChTmp, dVelX, dVelY;
01326   AngDeg angChMin, angChMax,     angTmp,   angChTmp;
01327   
01328   // no information  received -> no initialization
01329   if( Ball.getTimeRelativePosition() != getTimeLastSeeMessage() )
01330     return;
01331 
01332   // get perceived values for distance and angle with ball
01333   dDistBall = getRelativeDistance( OBJECT_BALL );
01334   angBall   = getRelativeAngle( OBJECT_BALL );
01335 
01336   // get perceived values for distance and direction change
01337   if( Ball.getTimeChangeInformation( ) == getTimeLastSeeMessage() )
01338   {
01339     dDistChange = Ball.getRelativeDistanceChange();
01340     angChange   = Ball.getRelativeAngleChange();
01341   }
01342 
01343   // get ranges from which values could originate
01344   getMinMaxDistQuantizeValue( dDistBall, &dMinDist, &dMaxDist, 0.1, 0.1 );  
01345   getMinMaxDistChange( dDistChange, dDistBall, &dMinCh, &dMaxCh, 0.02, 0.1,0.1);  
01346   getMinMaxDirChange ( angChange, &angChMin, &angChMax, 0.1 );    
01347   
01348   for( int i = 0; i < iLength; i ++ )
01349   {
01350     // make random distance and angle from range (angle is rounded) and make pos
01351     dDistTmp   = dMinDist + drand48()*fabs(dMaxDist - dMinDist); // angle->sign
01352     angTmp     = angBall  + drand48() - 0.5;
01353 
01354     posArray[i].setVecPosition( dDistTmp, angTmp, POLAR );
01355     posArray[i].relativeToGlobal( getAgentGlobalPosition(), 
01356                                   getAgentGlobalNeckAngle() );
01357      
01358     if( dDistChange != UnknownDoubleValue )
01359     {
01360       // make random values for direction and distance change and make velocity
01361       angChTmp   = angChMin + drand48()*(angChMax-angChMin);
01362       dDistChTmp = dMinCh   + drand48()*(dMaxCh-dMinCh);
01363 
01364       dVelX=dDistChTmp*cosDeg(angTmp)-Deg2Rad(angChTmp)*dDistTmp*sinDeg(angTmp);
01365       dVelY=dDistChTmp*sinDeg(angTmp)+Deg2Rad(angChTmp)*dDistTmp*cosDeg(angTmp);
01366       
01367       velArray[i].setVecPosition( dVelX, dVelY );
01368       velArray[i].relativeToGlobal( getAgentGlobalVelocity(),
01369                                     getAgentGlobalNeckAngle() );
01370     }
01371     else
01372       velArray[i].setVecPosition( 0, 0 );
01373   }
01374 }
01375 
01386 void WorldModel::checkParticlesBall( VecPosition posArray[],
01387              VecPosition velArray[], int iLength, int *iNrParticlesLeft )
01388 {
01389   bool   bIllegal;
01390   double dMinDist, dMaxDist, dMinCh, dMaxCh, dMag;
01391   double dDistBall = UnknownDoubleValue, dDistChange = UnknownDoubleValue;
01392   AngDeg angBall   = UnknownAngleValue,  angChange   = UnknownAngleValue;
01393   double dDistChTmp;
01394   AngDeg angChTmp, angChMin, angChMax;
01395   VecPosition pos_rel, vel_rel;
01396 
01397   int i1, i2, i3, i4;
01398   i1 = i2 = i3 = i4 = 0;
01399 
01400   // no new perceptions, do not check
01401   if( getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
01402     return;
01403 
01404   // initialize values distance, direction, distance change and direction change
01405   // and get the associated ranges
01406   dDistBall = getRelativeDistance( OBJECT_BALL );
01407   angBall   = getRelativeAngle( OBJECT_BALL );
01408   getMinMaxDistQuantizeValue( dDistBall, &dMinDist, &dMaxDist, 0.1, 0.1 );
01409   
01410   if( getTimeLastSeen( OBJECT_BALL ) == Ball.getTimeChangeInformation( ) )
01411   {
01412     dDistChange = Ball.getRelativeDistanceChange();
01413     angChange   = Ball.getRelativeAngleChange();
01414     getMinMaxDirChange ( angChange, &angChMin, &angChMax, 0.1);
01415     getMinMaxDistChange( dDistChange, dDistBall, &dMinCh, &dMaxCh,0.02,0.1,0.1);
01416     
01417   }
01418 
01419   *iNrParticlesLeft = 0;
01420   for( int i = 0; i < iLength; i ++ )
01421   {
01422     // get particles and make them relative to the agent to compare 
01423     pos_rel = posArray[i];
01424     vel_rel = velArray[i];
01425     pos_rel.globalToRelative( getAgentGlobalPosition(),
01426                               getAgentGlobalNeckAngle() );
01427     vel_rel.globalToRelative( getAgentGlobalVelocity(),
01428                               getAgentGlobalNeckAngle() );
01429 
01430     bIllegal = false;
01431 
01432     dMag = pos_rel.getMagnitude();
01433     if( dMag <  dMinDist || dMag > dMaxDist )
01434     {
01435       i1++;
01436       bIllegal = true;
01437     }
01438     if( fabs( VecPosition::normalizeAngle(pos_rel.getDirection() - angBall) )
01439          > 0.5 )
01440     {
01441       bIllegal = true;
01442       i2++;
01443     }
01444 
01445     if( dDistChange != UnknownDoubleValue )
01446     {
01447       dDistChTmp = (vel_rel.getX()*(pos_rel.getX()/dMag)) +
01448                    (vel_rel.getY()*(pos_rel.getY()/dMag));
01449       angChTmp   = Rad2Deg( ((vel_rel.getY()*pos_rel.getX()/dMag) -
01450                              (vel_rel.getX()*pos_rel.getY()/dMag)))/dMag ;
01451 
01452       if( angChTmp < angChMin || angChTmp > angChMax )
01453       {
01454         bIllegal = true;
01455         i3++;
01456       }
01457       if( dDistChTmp < dMinCh || dDistChTmp > dMaxCh )
01458       {
01459         bIllegal = true;
01460         i4++;
01461       }
01462     }
01463 
01464     // if not illegal, save particles and raise iNrParticlesLeft
01465     if( bIllegal == false )
01466     {
01467       posArray[*iNrParticlesLeft]     = posArray[i];
01468       velArray[(*iNrParticlesLeft)++] = velArray[i];
01469     }
01470   }
01471 }
01472 
01481 void WorldModel::updateParticlesBall( VecPosition posArray[], 
01482                VecPosition velArray[], int iLength, double dPower, AngDeg ang )
01483 {
01484   double dRand = SS->getBallRand();
01485   double dMaxRand;
01486 
01487   for( int i = 0; i < iLength; i ++ )
01488   {
01489     // if power supplied, assume ball (and thus particles) are kicked
01490     if( dPower > EPSILON )
01491     {
01492       ang = VecPosition::normalizeAngle(ang + getAgentGlobalBodyAngle() );
01493       velArray[i] += VecPosition( getActualKickPowerRate()*dPower, ang, POLAR) ;
01494       if( velArray[i].getMagnitude() > SS->getBallSpeedMax() )
01495         velArray[i].setMagnitude( SS->getBallSpeedMax() );
01496     }
01497 
01498     // add noise in same way server does. 
01499     dMaxRand = dRand * velArray[i].getMagnitude();
01500     velArray[i] += VecPosition(
01501                    (-1 + 2*drand48())*dMaxRand,
01502                    (-1 + 2*drand48())*dMaxRand );
01503     posArray[i] += velArray[i];
01504     velArray[i] *= SS->getBallDecay();
01505   }
01506 }
01516 void WorldModel::resampleParticlesBall( VecPosition posArray[],
01517 VecPosition velArray[], int iLength, int iLeft )
01518 {
01519   int iRand = 0;
01520   for ( int i = iLeft; i < iLength; i ++ )
01521   {
01522     iRand = (int)(drand48()*iLeft);       // pick random particle
01523     posArray[ i ] = posArray[ iRand ];    // and copy contents
01524     velArray[ i ] = velArray[ iRand ];
01525   }
01526 }
01527 
01533 VecPosition WorldModel::calculateVelocityDynamicObject( ObjectT o )
01534 {
01535   DynamicObject * dobj = (DynamicObject*) getObjectPtrFromType( o );
01536   if( dobj == NULL )
01537     return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
01538   double dDistCh = dobj->getRelativeDistanceChange(   );
01539   double angCh   = dobj->getRelativeAngleChange   (   );
01540   double dDist   = getRelativeDistance            ( o );
01541   double ang     = getRelativeAngle               ( o );
01542 
01543   double velx = dDistCh * cosDeg(ang) - Deg2Rad(angCh) * dDist * sinDeg( ang );
01544   double vely = dDistCh * sinDeg(ang) + Deg2Rad(angCh) * dDist * cosDeg( ang );
01545 
01546   VecPosition vel( velx, vely );
01547   return vel.relativeToGlobal( getAgentGlobalVelocity(),
01548                                getAgentGlobalNeckAngle() );
01549 }
01550 
01551 
01558 bool WorldModel::calculateStatePlayer( ObjectT o, VecPosition *posGlobal,
01559                                        VecPosition *velGlobal )
01560 {
01561   PlayerObject *pob = (PlayerObject*) getObjectPtrFromType( o );
01562   if( pob == NULL )
01563     return false;
01564 
01565   // set the global position of this dynamic object as follows:
01566   //  - get the relative position from the agent to it in world-axis
01567   //  - add global position agent to this relative position
01568   VecPosition posRelWorld =
01569       VecPosition( getRelativeDistance( o ),
01570                    getRelativeAngle( o ) + agentObject.getGlobalNeckAngle(),
01571                    POLAR );
01572   *posGlobal = getAgentGlobalPosition() + posRelWorld;
01573 
01574   velGlobal->setVecPosition( 0, 0);
01575   if( pob->getTimeChangeInformation( ) == getTimeLastSeen( o ) )
01576   {
01577     // calculate the global velocity using the distance and angle change
01578     // with the formula from the soccermanual
01579     *velGlobal = calculateVelocityDynamicObject( o );
01580   }
01581   else
01582       ; // object too far away do not estimate velocity, sense has updated it
01583         // already and does not really matter then
01584 
01585   if( velGlobal->getMagnitude() >=
01586                       ( 1.0 + SS->getPlayerRand())*SS->getPlayerSpeedMax() )
01587     velGlobal->setMagnitude( SS->getPlayerSpeedMax() );
01588 
01589   return true;
01590 }
01591 
01592 
01604 bool WorldModel::getMinMaxDistQuantizeValue( double dOutput, double *dMin,
01605                                         double *dMax,   double x1, double x2 )
01606 {
01607   // change output a little bit to circumvent boundaries
01608   // q = quantize(e^(quantize(ln(V),x1)),x2) with quantize(V,Q) = rint(V/Q)*Q
01609   // e^(quantize(ln(V),x1)_min = invQuantize( q, x2 )
01610   // quantize(ln(V),x1) = ln ( invQuantize( q, x2 ) )
01611   // ln(V) = invQuantize( ln ( invQuantize( q, x2 ) ), x1 )
01612   // V_min = e^( invQuantize( ln ( invQuantize( q, x2 ) ), x1 ) )
01613   // apply inverse quantize twice to get correct value
01614   dOutput -= 1.0e-10;
01615   *dMin = exp( invQuantizeMin( log( invQuantizeMin(dOutput,x2) ), x1 ) );
01616   dOutput += 2.0e-10;
01617   *dMax = exp( invQuantizeMax( log( invQuantizeMax(dOutput,x2) ), x1 ) );
01618   return true;
01619 }
01620 
01630 bool WorldModel::getMinMaxDirChange( double dOutput, double *dMin,
01631                                      double *dMax,   double x1     )
01632 {
01633  *dMin = invQuantizeMin( dOutput, x1 ) ;
01634  *dMax = invQuantizeMax( dOutput, x1 ) ;
01635  return true;
01636 }
01637 
01652 bool WorldModel::getMinMaxDistChange( double dOutput, double dDist,
01653          double *dMin, double *dMax, double x1, double xDist1, double xDist2)
01654 {
01655   // Q_dist = quantize(e^(quantize(ln(V),xDist1)),xDist2)
01656   // q = Q_dist * Quantize( distance_change/distance, x1 )
01657   // dOutput = q/Q_dist = Quantize( distance_change/distance, x1 )
01658   // (distance_change/distance)_min = invQmin(q/Q_dist, x1 )
01659   // real distance is not know so should take into account distance range
01660   double dMinDist, dMaxDist;
01661   getMinMaxDistQuantizeValue( dDist, &dMinDist, &dMaxDist, xDist1, xDist2 );
01662   dOutput = dOutput/dDist;
01663   double dMinCh = invQuantizeMin( dOutput, x1 ) ;
01664   double dMaxCh = invQuantizeMax( dOutput, x1 ) ;
01665   *dMin = min( dMinDist*dMinCh, dMaxDist*dMinCh );
01666   *dMax = max( dMinDist*dMaxCh, dMaxDist*dMaxCh );
01667   return true;
01668 }
01669 
01675 double WorldModel::invQuantizeMin( double dOutput, double dQuantizeStep )
01676 {
01677   // q = quantize( V, Q ) = rint(V/Q)*Q -> q/Q = rint( V/Q)
01678   // min = rint(q/Q)-0.5 = V_min/Q -> V_min = (rint(q/Q)-0.5)*Q
01679   return (rint( dOutput / dQuantizeStep )-0.5 )*dQuantizeStep;
01680 }
01681 
01687 double WorldModel::invQuantizeMax( double dOutput, double dQuantizeStep )
01688 {
01689   // q = quantize( V, Q ) = rint(V/Q)*Q -> q/Q = rint( V/Q)
01690   // max = rint(q/Q)+0.5 = V_max/Q -> V_max = (rint(q/Q)-0.5)*Q
01691   return (rint( dOutput/dQuantizeStep) + 0.5 )*dQuantizeStep;
01692 }
01693 
01702 void WorldModel::mapUnknownPlayers( Time time)
01703 {
01704   double      dist, dMinDist;
01705   VecPosition rel;
01706   ObjectT     o, o_new;
01707 
01708   // for all unknown players, try to map it to closest teammate or opponent
01709   for( int j = 0; j < iNrUnknownPlayers; j ++ )
01710   {
01711     rel.setVecPosition( UnknownPlayers[j].getRelativeDistance(),
01712                         UnknownPlayers[j].getRelativeAngle(), POLAR );
01713     dist  = dMinDist = 1000.0;
01714     o     = UnknownPlayers[j].getType();
01715     o_new = OBJECT_ILLEGAL;
01716     if( ! SoccerTypes::isOpponent( o ) ) // TEAMMATE_UNKNOWN or PLAYER_UNKNOWN
01717     {
01718       for( int i = 0 ; i < MAX_TEAMMATES ; i++ )
01719       {
01720         if( isConfidenceGood( Teammates[i].getType() ) &&
01721             Teammates[i].getTimeLastSeen() != time )
01722         {
01723           dist = rel.getDistanceTo( Teammates[i].getRelativePosition( ) );
01724           if( dist < dMinDist )
01725           {
01726             o_new    = Teammates[i].getType();
01727             dMinDist = dist;
01728           }
01729         }
01730       }
01731     }
01732     if( ! SoccerTypes::isTeammate( o ) ) // OPPONENT_UNKNOWN or PLAYER_UNKNOWN
01733     {
01734       for( int i = 0 ; i < MAX_OPPONENTS ; i++ )
01735       {
01736         if( isConfidenceGood( Opponents[i].getType() ) &&
01737             Opponents[i].getTimeLastSeen() != time )
01738         {
01739           dist = rel.getDistanceTo( Opponents[i].getRelativePosition( ) );
01740           if( dist < dMinDist )
01741           {
01742             o_new    = Opponents[i].getType();
01743             dMinDist = dist;
01744           }
01745         }
01746       }
01747     }
01748     Log.log( 601, "try to map %d distance %f and %f to %d", 
01749     UnknownPlayers[j].getType(), rel.getMagnitude(), dMinDist,
01750       o_new  );
01751     // if player found and in tolerated distance
01752     //   update player information.
01753     // else if not mapped to player
01754     //   put information in first player position of which we have no info.
01755     // if we do not know it is a teammate or opponent only assume opponent when
01756     // it is very close since then it is probably an opponent trying to get ball
01757     if( SoccerTypes::isKnownPlayer(o_new)
01758         && dMinDist < PS->getPlayerDistTolerance())
01759     {
01760       Log.log( 601, "known player: pos %d", o_new );          
01761       UnknownPlayers[j].setType( o_new );
01762       if( SoccerTypes::isTeammate( o_new ) )
01763         Teammates[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01764       else if( SoccerTypes::isOpponent( o_new ) )
01765         Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01766     }
01767     else if( UnknownPlayers[j].getType() == OBJECT_TEAMMATE_UNKNOWN )
01768     {
01769       o_new = getFirstEmptySpotInSet( OBJECT_SET_TEAMMATES );
01770       Log.log( 601, "unknown teammate: pos %d", o_new );
01771       if( o_new != OBJECT_ILLEGAL )
01772       {            
01773         UnknownPlayers[j].setType( o_new );
01774         Teammates[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01775       }
01776     }
01777     else if( UnknownPlayers[j].getType() == OBJECT_OPPONENT_UNKNOWN )
01778     {                                      // could not map info to a player
01779       o_new = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
01780       Log.log( 601, "unknown opp: pos %d", o_new );            
01781       if( o_new != OBJECT_ILLEGAL )
01782       {
01783         UnknownPlayers[j].setType( o_new );
01784         Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01785       }
01786     }
01787     else if( UnknownPlayers[j].getType() == OBJECT_PLAYER_UNKNOWN &&
01788              UnknownPlayers[j].getRelativeDistance() < SS->getVisibleDistance() )
01789     {
01790       o_new = getFirstEmptySpotInSet( OBJECT_SET_OPPONENTS );
01791       Log.log( 601, "unknown player: pos %d", o_new );      
01792       if( o_new != OBJECT_ILLEGAL )
01793       {
01794         UnknownPlayers[j].setType( o_new );
01795         Opponents[SoccerTypes::getIndex(o_new)] = UnknownPlayers[j];
01796       }
01797     }
01798     else
01799      Log.log( 601, "not mapped" );
01800   }
01801 
01802   iNrUnknownPlayers  = 0;
01803 }
01804 
01812 bool WorldModel::updateSSToHeteroPlayerType( int iIndex )
01813 {
01814    SS->setPlayerSpeedMax( pt[iIndex].dPlayerSpeedMax );
01815    SS->setStaminaIncMax ( pt[iIndex].dStaminaIncMax  );
01816    SS->setPlayerDecay   ( pt[iIndex].dPlayerDecay    );
01817    SS->setInertiaMoment ( pt[iIndex].dInertiaMoment  );
01818    SS->setDashPowerRate ( pt[iIndex].dDashPowerRate  );
01819    SS->setPlayerSize    ( pt[iIndex].dPlayerSize     );
01820    SS->setKickableMargin( pt[iIndex].dKickableMargin );
01821    SS->setKickRand      ( pt[iIndex].dKickRand       );
01822    SS->setExtraStamina  ( pt[iIndex].dExtraStamina   );
01823    SS->setEffortMax     ( pt[iIndex].dEffortMax      );
01824    SS->setEffortMin     ( pt[iIndex].dEffortMin      );
01825 
01826    return true;
01827 }
01828 
01832 bool WorldModel::resetTimeObjects( )
01833 {
01834   Ball.setTimeLastSeen             ( Time( -1, 0) );
01835   for( int i = 0 ; i < MAX_TEAMMATES ; i ++ )
01836     Teammates[i].setTimeLastSeen   ( Time( -1, 0) );
01837   for( int i = 0 ; i < MAX_OPPONENTS ; i ++ )
01838     Opponents[i].setTimeLastSeen   ( Time( -1, 0) );
01839   for( int i = 0 ; i < MAX_FLAGS     ; i ++ )
01840     Flags[i].setTimeLastSeen   ( Time( -1, 0) );
01841   for( int i = 0 ; i < MAX_LINES     ; i ++ )
01842     Lines[i].setTimeLastSeen   ( Time( -1, 0) );
01843   agentObject.setTimeLastSeen      ( Time( -1, 0) );
01844   return true;
01845 }
01846 
01850 void WorldModel::removeGhosts( )
01851 {
01852   AngDeg dAngle=SoccerTypes::getHalfViewAngleValue(agentObject.getViewAngle());
01853   dAngle -= 0.15*dAngle; // make somewhat smaller for error
01854 
01855   if( fabs( getRelativeAngle( OBJECT_BALL ) ) < dAngle
01856        && getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
01857   {
01858     Ball.setTimeLastSeen( Time( -1, 0 ) );
01859   }
01860 }
01861 
01862 
01863 
01864 
01865 
01874 bool WorldModel::calculateStateAgent2( VecPosition *posGlobal,
01875                                    VecPosition *velGlobal, AngDeg *angGlobal)
01876 {
01877   double      x=0.0, y=0.0, dMinRadius1, dMaxRadius1, dMinRadius2, dMaxRadius2;
01878   double      dTotalVar = UnknownDoubleValue, dVar, K;
01879   int         iIndex1, iIndex2;
01880   ObjectT     obj2;
01881   VecPosition pos;
01882 
01883   // for all flags that were perceived in last see message
01884   for( ObjectT obj1 = iterateObjectStart( iIndex1, OBJECT_SET_FLAGS, 1.0 );
01885        obj1 != OBJECT_ILLEGAL;
01886        obj1 = iterateObjectNext ( iIndex1, OBJECT_SET_FLAGS, 1.0 ) )
01887   {
01888     // calculate global position with all other flags using two flags
01889     iIndex2 = iIndex1;
01890     for( obj2 = iterateObjectNext ( iIndex2, OBJECT_SET_FLAGS, 1.0 ) ;
01891          obj2 != OBJECT_ILLEGAL;
01892          obj2 = iterateObjectNext ( iIndex2, OBJECT_SET_FLAGS, 1.0 ) )
01893     {
01894       // calculate the position using the two flags
01895       pos = calculatePosAgentWith2Flags( obj1, obj2 );
01896 
01897       // get distance range from which perceived value can originate from
01898       // calculate variance (=weighted factor) based on the distance to the
01899       // two flags, use variance corresponding to uniform distribution
01900       // this is not completely correct, better would be to use the
01901       // intersection area size of the two circle, but is too computational
01902       // intensive
01903       getMinMaxDistQuantizeValue(getRelativeDistance(obj1),
01904                  &dMinRadius1, &dMaxRadius1, SS->getQuantizeStepL(), 0.1 )  ;
01905       getMinMaxDistQuantizeValue(getRelativeDistance(obj2),
01906                  &dMinRadius2, &dMaxRadius2, SS->getQuantizeStepL(), 0.1 )  ;
01907       dVar =  (dMaxRadius1-dMinRadius1)*(dMaxRadius1-dMinRadius1)/12;
01908       dVar += (dMaxRadius2-dMinRadius2)*(dMaxRadius2-dMinRadius2)/12;
01909 
01910       if( pos.getX() != UnknownDoubleValue &&
01911           dTotalVar  == UnknownDoubleValue )
01912       {
01913         dTotalVar = dVar;                     // initialize the position
01914         x         = pos.getX();
01915         y         = pos.getY();
01916       }
01917       else if( pos.getX() != UnknownDoubleValue )
01918       {
01919         K = dTotalVar / ( dVar + dTotalVar ); // otherwise use new position
01920         x += K*( pos.getX() - x );            // based on weighted variance
01921         y += K*( pos.getY() - y );
01922         dTotalVar -= K*dTotalVar;
01923       }
01924     }
01925     iterateObjectDone( iIndex2 );
01926   }
01927   iterateObjectDone( iIndex1 );
01928   posGlobal->setVecPosition( x, y );
01929 
01930   // now calculate global position (experiments show best results are obtained
01931   // when average with all perceived flags is taken).
01932   *angGlobal = calculateAngleAgentWithPos( *posGlobal );
01933 
01934   // update velocity since after 'see' we have a better estimate of neck angle
01935   *velGlobal = agentObject.getSpeedRelToNeck().rotate(*angGlobal);
01936 
01937   return true;
01938 }
01939 
01947 bool WorldModel::calculateStateBall2( VecPosition *posGlobal,
01948                                      VecPosition *velGlobal )
01949 {
01950   // set the global position of the ball as follows:
01951   //  - get the relative position from the agent to it in world-axis
01952   //  - add global position agent to this relative position
01953   VecPosition posRelWorld =
01954       VecPosition( getRelativeDistance( OBJECT_BALL ),
01955                    getRelativeAngle( OBJECT_BALL ) + getAgentGlobalNeckAngle(),
01956                    POLAR );
01957   *posGlobal = getAgentGlobalPosition() + posRelWorld;
01958 
01959   if( isBeforeKickOff() )
01960     posGlobal->setVecPosition( 0, 0 );
01961     
01962   *velGlobal = getGlobalVelocity(OBJECT_BALL);
01963   if( Ball.getTimeChangeInformation( ) == getTimeLastSeen( OBJECT_BALL ) )
01964   {
01965     *velGlobal = calculateVelocityDynamicObject( OBJECT_BALL );;
01966 
01967     // average result with predicted velocity from last see message
01968     // this has the best result for the ball: see program absvelocity.C
01969     // only use average when no big chance (kick,turn or dash) has occured
01970     if( fabs(velGlobal->getX() - getGlobalVelocity(OBJECT_BALL).getX())
01971                                       <= 2*SS->getBallRand()*getBallSpeed() &&
01972         fabs(velGlobal->getY() - getGlobalVelocity(OBJECT_BALL).getY())
01973                                       <= 2*SS->getBallRand()*getBallSpeed() )
01974     {
01975       *velGlobal = (*velGlobal + getGlobalVelocity(OBJECT_BALL))/2.0;
01976     }
01977   }
01978   else if( getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() &&
01979            getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() < 3 )
01980   {
01981     // no change information but have got feel info -> use position based vel.
01982     // get the difference with the previous known global position
01983     // and subtract the position difference (this difference is caused by
01984     // the fact that the global position calculation contains a lot of noise
01985     // now the noise is filtered, since we compare the velocity as if its
01986     // position was seen "with the noise" of the last cycle).
01987 
01988     VecPosition posGlobalDiff   = *posGlobal - Ball.getGlobalPositionLastSee()
01989                                          - agentObject.getPositionDifference();
01990 
01991     // difference in global positions is distance traveled, we have to make
01992     // distinction whether this distance is traveled in one or two cycles.
01993     // 1 cycle:
01994     //   distance difference equals last velocity so only have to multiply with
01995     //   decay
01996     // 2 cycles:
01997     //   do not update, since kick can be performed causing large change
01998     //   of which we have no see information (so thus use sense update)
01999 
02000     if( getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() == 1  )
02001       *velGlobal = posGlobalDiff*SS->getBallDecay();
02002     else if( getTimeLastSeeMessage()-Ball.getTimeGlobalPosDerivedFromSee() > 2 )
02003     {
02004       Log.log( 20, "(WorldModel:%s) time difference too large" ,__FUNCTION__ );
02005     }
02006   }
02007   else
02008     ;  // object too far away do not estimate velocity, sense has updated it
02009        // already
02010 
02011   // change velocity when the ball
02012   //   has been catched or play mode is not play_on
02013   //   is in kickable distance opponent
02014   //   higher than max speed with added noise
02015   if( getTimeSinceLastCatch() < 2 || getPlayMode() != PM_PLAY_ON )
02016     velGlobal->setMagnitude( 0.0 );
02017   else if( getNrInSetInCircle( OBJECT_SET_OPPONENTS,
02018                             Circle(*posGlobal,SS->getMaximalKickDist())) > 0 )
02019     velGlobal->setMagnitude( 0.0 );
02020   else if( velGlobal->getMagnitude() >
02021                            ( 1.0 + SS->getBallRand() )*SS->getBallSpeedMax() )
02022     velGlobal->setMagnitude( SS->getBallSpeedMax() );
02023 
02024   return true;
02025 }

Generated on Thu Mar 7 00:37:44 2002 for UvA Trilearn 2001 by doxygen1.2.12 written by Dimitri van Heesch, © 1997-2001