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-2002, 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 */
00047 #include "WorldModel.h"
00048 #include "Parse.h"
00049 #include <stdio.h>      // needed for sprintf
00050 #include <list>         // needed for list
00051 
00052 /******************************************************************************/
00053 /********************** CLASS WORLDMODEL **************************************/
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; // no sense to update when only angle is known.
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     // if we do not have all information, update UnknownPlayer array
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 // else update the known object (teammate, opponent, ball)
00173     {
00174       d = (DynamicObject*)getObjectPtrFromType( o );
00175       if( SoccerTypes::isPlayer( o ) )
00176         ((PlayerObject*)d)->setIsKnownPlayer( true );
00177     }
00178 
00179     if( d != NULL )  // if object was known
00180     {
00181       // set all values for this dynamicobject
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       // check if there wasn't an unknown player located in the worldmodel
00199       // that corresponded to the same player
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 ); // delete the unknown player
00229         }
00230       }
00231     }
00232   }
00233   else if( SoccerTypes::isLine( o ) )
00234   {
00235     // angle returned is angle of neck angle with line, convert to angle
00236     // of neck with orthogonal to line
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];                       // location for message
00264   int         iDiff = getCurrentCycle() - m_iCycleInMsg; // time difference
00265   double      dDiff = (double)iDiff/100.0;               // conf difference
00266   char        *strMsg;
00267   strcpy( strMessage, m_strPlayerMsg );
00268   strMsg = strMessage;                                   // pointer to work with
00269 
00270   // translate ball position and velocity back to initial range.
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   // if ball not seen or felt for three cycles, update ball information
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; // ball should be called with processPerfectHearInfoBall
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   // if we are not sure about the exact player number of this player in
00335   // the world model (getIsKnownPlayer() == false) we overwrite the
00336   // information of this player since the player who said this information
00337   // is sure about it (otherwise processUnsureHearInfo would be called instead
00338   // of processPERFECTHearInfo)
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;        // used to find closest player to pos
00368   ObjectT    objInitial = o;
00369 
00370   if( o != OBJECT_TEAMMATE_UNKNOWN && o != OBJECT_OPPONENT_UNKNOWN )
00371     return false;
00372 
00373   // if o is a teammate find closest teammate to pos and store distance
00374   if( SoccerTypes::isTeammate( o ) )
00375     o = getClosestInSetTo( OBJECT_SET_TEAMMATES, pos, &dMinDist);
00376   else if( SoccerTypes::isOpponent( o ) )  // if o is an opponent, do the same
00377     o = getClosestInSetTo( OBJECT_SET_OPPONENTS, pos, &dMinDist);
00378 
00379   if( o == getAgentObjectType() &&
00380      pos.getDistanceTo(getAgentGlobalPosition())<PS->getPlayerDistTolerance())
00381     return false;  // do not update my own position, localization is better
00382 
00383   // if opponent or teammate was found and distance lies in tolerance distance
00384   //  update this opponent or teammate with the specified information.
00385   // else put the information in the first player position of which we have
00386   //  no information.
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 ;  // in case of OBJECT_PLAYER_UNKNOWN
00400 
00401   if( o != OBJECT_ILLEGAL )   // can be the case that there is no empty spot
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   // put all sent commands to the array which stores queued commands.
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   // check if last update of agent was not more than one cycle ago
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   // call update method depending on last received message
00523   if( getTimeLastSeeMessage() > timeLastSeeUpdate )
00524     bUpdateAfterSee = true;
00525   if( getTimeLastSenseMessage() > timeLastSenseUpdate )
00526     bUpdateAfterSense = true;
00527 
00528   // rare situation: can occur that see arrives between sense and calling
00529   // updateAll or sense arrives between see and calling updateAll.
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 )                        // process see message
00551   {
00552     bReturn           = updateAfterSeeMessage( );
00553     timeLastSeeUpdate = getTimeLastSeeMessage();
00554   }
00555   else if( bUpdateAfterSense )                      // process sense message
00556   {
00557     bReturn             = updateAfterSenseMessage( );
00558     timeLastSenseUpdate = getTimeLastSenseMessage();
00559     updateRelativeFromGlobal();
00560   }
00561 
00562   if( timeLastSayUpdate != m_timePlayerMsg )        // process communication msg
00563   {
00564     timeLastSayUpdate = m_timePlayerMsg;
00565     processPlayerMessage();
00566   }
00567 
00568   // determine number of holes in last time interval and act accordingly
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   // store some statistics about number of players seen each cycle
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   // log specific information when log level is set
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 /*************** WORLDMODEL: SEE RELATED UPDATES ******************************/
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 );         // get the time
00633 
00634   ObjectT objTeamMin = OBJECT_ILLEGAL;    // players are listed in order of
00635   ObjectT objMax     = OBJECT_ILLEGAL;    // of initialization, can use this
00636   ObjectT objOppMin  = OBJECT_ILLEGAL;    // info to determine object type
00637   ObjectT objMin     = OBJECT_ILLEGAL;    // of unknown players
00638 
00639   while( *strMsg != ')' )                          // " ((objname.." or ")"
00640   {
00641     dDist = dDistChange    = dDirChange     = dTemp = UnknownDoubleValue;
00642     iDir  = iBodyFacingDir = iHeadFacingDir =         UnknownIntValue;
00643     strMsg += 2;          // get the object name
00644 
00645     // get the object name from the first part of the string
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 ) )    // we know the player
00654     {
00655       if( SoccerTypes::isTeammate( o ) )          // set the minimum object
00656         objTeamMin = o;                           // all objects in the msg
00657       else                                        // hereafter, should have
00658         objOppMin  = o;                           // a higher index
00659     }
00660     else if( SoccerTypes::isPlayer( o ) )         // and thus an unknown player
00661     {
00662       if( SoccerTypes::isTeammate( o ) )          // raise minimum object with 1
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 );                  // get the maximum object by
00677       objMax = getMaxRangeUnknownPlayer( o, strTmp ); // looking to next object
00678     }
00679 
00680     dTemp = Parse::parseFirstDouble( &strMsg );   // parse first value
00681     if ( *strMsg == ')' )                         // if it was the only value
00682       iDir = (int)dTemp;                          // it was the direction
00683     else
00684     {
00685       dDist = dTemp;                              // else it was distance
00686       iDir = Parse::parseFirstInt( &strMsg );     // and have to get direction
00687       if( *strMsg != ')' )                        // stil not finished
00688       {                                           // get dist and dir change
00689         dDistChange = Parse::parseFirstDouble( &strMsg );
00690         dDirChange  = Parse::parseFirstDouble( &strMsg );
00691       }
00692       if( *strMsg != ')' )                        // and stil not finished
00693       {                                           // get also facing dir.
00694         iBodyFacingDir = Parse::parseFirstInt( &strMsg );
00695         iHeadFacingDir = Parse::parseFirstInt( &strMsg );
00696       }
00697     }
00698     // go to end object information
00699     // skip ending bracket of object information.
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     // process the parsed information (unread values are Unknown...)
00711     processNewObjectInfo( o, time, dDist, iDir, dDistChange,
00712            dDirChange, (AngDeg)iBodyFacingDir, (AngDeg)iHeadFacingDir,
00713            isGoalie, objMin, objMax );
00714   }
00715   mapUnknownPlayers( time );       // map players with unknown nr
00716 }
00717 
00723 bool WorldModel::updateAfterSeeMessage( )
00724 {
00725   processLastSeeMessage( );
00726    // update the agent (global position and angle using flags and lines)
00727   if( getCurrentTime().getTime() != -1 )
00728     updateAgentObjectAfterSee( );
00729 
00730   // walk past all players on the field an when new information was perceived
00731   // (and put in the relative attributes) update this dynamic object. When it
00732   // was not seen, convert its global position (this is an estimate from the
00733   // sense message) to a relative position
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   // if ball was seen update him, otherwise make estimated global relative
00750   if( getTimeLastSeen( OBJECT_BALL ) == getTimeLastSeeMessage() )
00751     updateDynamicObjectAfterSee   ( OBJECT_BALL );
00752   else
00753     updateObjectRelativeFromGlobal( OBJECT_BALL );
00754 
00755   // delete objects from wordmodel that should have been seen, but aren't
00756   removeGhosts();
00757 
00758   return true;
00759 }
00760 
00765 bool WorldModel::updateAgentObjectAfterSee(  )
00766 {
00767   VecPosition posGlobal, velGlobal;
00768   AngDeg      angGlobal;
00769 
00770   // calculate the state of the agent
00771   calculateStateAgent( &posGlobal, &velGlobal, &angGlobal );
00772 
00773   // and set the needed attributes
00774   agentObject.setTimeLastSeen         ( getTimeLastSeeMessage() );
00775   // store difference with predicted global position to compensate for error
00776   // in global position when global velocity is calculated for other objects.
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 /******************** SENSE RELATED UPDATES ***********************************/
00832 /******************************************************************************/
00833 
00842 bool WorldModel::updateAfterSenseMessage( )
00843 {
00844   // update agent information
00845   updateAgentAndBallAfterSense( );
00846 
00847   // update all global information of players that have a good confidence
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   // get info from commands from previous cycle (if timestopped current cycle)
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   // first update information based on performed actions without using
00884   // the received velocity information.
00885   for( int i = 0; i < MAX_COMMANDS; i ++  )
00886   {
00887     if( performedCommands[i] == true && // sense msg indicates we executed this
00888         ( queuedCommands[i].time.getTime() == time.getTime() ||      // no hole
00889           queuedCommands[i].time.getTime() == time.getTime() - 1 ) )// hole
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;         // should not be updated later on.
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   // update the ball when not done yet
00916   if( !bBallUpdated )
00917   {
00918     m_bPerformedKick = false;
00919     updateDynamicObjectForNextCycle( OBJECT_BALL, 1 );
00920   }
00921   else
00922     m_bPerformedKick = true;
00923 
00924   // now use velocity information to check whether a collision occured
00925   velNeck.rotate( angGlobalNeck ); // make relative info global
00926   double dDistBall = pos.getDistanceTo( getBallPos() );
00927   if( ( dDistBall<SS->getPlayerSize()+SS->getBallSize() && // ball too close AND
00928         velNeck.getMagnitude( ) < EPSILON                  // vel. gives no info
00929        ) 
00930      ||                                                    // OR
00931      ( dDistBall < SS->getMaximalKickDist() + 0.5  &&      // ball close
00932       velNeck.getMagnitude( ) > EPSILON &&                 // can compare vel
00933       sign( vel.getX() ) != sign( velNeck.getX() ) &&      // both signs have
00934       sign( vel.getY() ) != sign( velNeck.getY() ) &&      // changed and is
00935       velNeck.getMagnitude() < 0.15 * vel.getMagnitude() ) ) // lessened a lot
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     // new vel. agent is the one received from sense message
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     // use better vel. estimate in sense message to update information
00961     pos            = getAgentGlobalPosition();
00962     angGlobalNeck  = getAgentGlobalNeckAngle();
00963     angGlobalBody  = getAgentGlobalBodyAngle();
00964     sta            = getAgentStamina();
00965     vel            = agentObject.getSpeedRelToNeck();
00966 
00967     // calculate velocity at end of previous cycle using velocity from
00968     // current cycle. Although we do not know direction yet (this is
00969     // relative to neck which is not yet known), we can use the
00970     // magnitude to determine traveled  distance (speed) of the agent
00971     // After neck angle is estimated, we can rotate velocity vector
00972     // to get actual velocity.
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:     // no action needed, since resulting
00993                              // vel. is already available from sense
00994                              // this vel can be used to update pos
00995             break;
00996          default:
00997             break;
00998         }
00999       }
01000       queuedCommands[i].time.updateTime( -1 );            // processed
01001     }
01002     // reset pos and vel information to previous cycle (since can be
01003     // changed in predictStateAfterCommand)
01004     vel = agentObject.getSpeedRelToNeck();
01005     pos = getAgentGlobalPosition();
01006     vel.setMagnitude( vel.getMagnitude()/SS->getPlayerDecay() );
01007     vel.rotate( angGlobalNeck ); // rotate velocity using information about neck
01008 
01009     // predict global position using the calculated vel at the end of
01010     // the previous cycle (power and direction can thus both be set
01011     // to zero). There is just little noise in this perception, so
01012     // gives a good estimate
01013     predictStateAfterDash( 0.0, &pos, &vel, &sta, 0 );
01014   }
01015 
01016   // update particles that keep track of position of agent using vel
01017   updateParticlesAgent( vel, true );
01018 
01019   // body angle is not set since relative angle to neck is already
01020   // set after parsing sense_body message, same holds for stamina
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     // make angle relative to body
01041     // calculate added acceleration and add it to current velocity
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   // get velocity and add it to current global position
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       // no idea of intention, just let him roll out
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   // get the direction the ball was coming from and put it at small distance
01111   // and multiply the velocity with -0.1
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   // update all player objects
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   // and also the ball
01143   if( isConfidenceGood( OBJECT_BALL ) )
01144     updateObjectRelativeFromGlobal( OBJECT_BALL );
01145 
01146   // flags and lines are not updated, since they are not used except immediately
01147   // after see message when they're up to date.
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   // get global position and translate and rotate it to agent neck
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   // first determine global neck angle
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   // use global neck angle to determine estimate of current global velocity
01198   // neck angle is better estimate than after sense -> better estimate velocity
01199   // update all position particles of the agent with this velocity
01200   //  'false' denotes update after see message. Since global neck angle can
01201   //  be determined more precise after 'see' it is better to predict position
01202   //  again (although it was already done after sense)
01203   // and then check which particles are possible given current perceptions
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 ) // if no particles left, initialize all particles
01211   {
01212     // initialize particles (from random samples using closest flag)
01213     // check particles are then checked with other flags
01214     initParticlesAgent ( *angGlobal );
01215     iNrLeft = checkParticlesAgent( *angGlobal );
01216     if( iNrLeft == 0 )
01217     {
01218       // not succeeded, use second method (weighted average flags)
01219       // and initialize all particles to this position
01220       calculateStateAgent2( posGlobal, velGlobal, angGlobal );
01221       initParticlesAgent( *posGlobal );
01222       return false;
01223     }
01224   }
01225 
01226   // determine global position (= average of all particles)
01227   // and resample all particles
01228   *posGlobal = averageParticles( particlesPosAgent, iNrLeft );
01229   resampleParticlesAgent( iNrLeft );
01230 
01231   // use the position to calculate better global neck angle of the agent
01232   // and recalculate global velocity with improved neck angle
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   // get closest flag from which samples will be generated
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   // get the distance to this flag and the possible range it was derived from.
01263   dInput = getRelativeDistance( objFlag );
01264   getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01265                                                SS->getQuantizeStepL(), 0.1 ) ;
01266 
01267   // get the perceived angle to this flag (add 180 to get angle relative from
01268   // flag to agent ) and make it global by adding global neck angle agent.
01269   AngDeg angFlag   = getRelativeAngle( objFlag ) + 180 + angGlobal ;
01270 
01271   // for all particles
01272   for( int i = 0 ; i < iNrParticlesAgent ; i++ )
01273   {
01274     // determine random point from distance range and
01275     // determine random point from the range it could be generated from
01276     // angles are rounded and since noise is in global neck angle and relative
01277     // angle flag, maximum error is in range [-0.5,0.5] + [-0.5,0.5] = [-1,1].
01278     dDist = dMinRadius + drand48()*(dMaxRadius-dMinRadius);
01279     ang   = VecPosition::normalizeAngle( angFlag - 1.0 + 2*drand48() );
01280 
01281     // create random point from possible interval
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   // for all current perceived flags
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;                        // reset number of correct particles
01318     dInput = getRelativeDistance( o );  // get possible distance range
01319     getMinMaxDistQuantizeValue( dInput, &dMinRadius, &dMaxRadius,
01320                                     SS->getQuantizeStepL(), 0.1 )  ;
01321 
01322     // find all "correct points"
01323     for( int i = 0; i < iLength; i ++ )
01324     {
01325       // determine distance particle to flag
01326       // determine difference in direction between direction between global
01327       // flag and agent position and global perceived direction
01328       dDist = particlesPosAgent[i].getDistanceTo( getGlobalPosition( o ) );
01329       ang = (getGlobalPosition(o) - particlesPosAgent[i]).getDirection();
01330       ang = ang - ( getRelativeAngle( o ) + angGlobalNeck );
01331 
01332       // if in possible range, save it (maximum angle range is 0.5 for
01333       // neck angle and 0.5 for relative flag angle gives 1.0)
01334       if( dDist > dMinRadius && dDist < dMaxRadius &&
01335           fabs(VecPosition::normalizeAngle( ang )) <= 1.0 )
01336         particlesPosAgent[iNrLeft++] = particlesPosAgent[i];
01337     }
01338     // change maximum of correct particles
01339     iLength = iNrLeft;
01340   }
01341   return iNrLeft;
01342 }
01343 
01354 void WorldModel::updateParticlesAgent( VecPosition vel, bool bAfterSense )
01355 {
01356   // used to denote last added velocity
01357   static VecPosition prev_vel;
01358 
01359   for( int i = 0; i < iNrParticlesAgent  ; i ++ )
01360   {
01361     if( bAfterSense == false ) // if after see, subtract last added 'vel'
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   // take average of particles
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   // for all flags that were perceived in last see message
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     // calculate global position with all other flags using two flags
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       // calculate the position using the two flags
01435       pos = calculatePosAgentWith2Flags( obj1, obj2 );
01436 
01437       // get distance range from which perceived value can originate from
01438       // calculate variance (=weighted factor) based on the distance to the
01439       // two flags, use variance corresponding to uniform distribution
01440       // this is not completely correct, better would be to use the
01441       // intersection area size of the two circle, but is too computational
01442       // intensive
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;                     // initialize the position
01454         x         = pos.getX();
01455         y         = pos.getY();
01456       }
01457       else if( pos.getX() != UnknownDoubleValue )
01458       {
01459         K = dTotalVar / ( dVar + dTotalVar ); // otherwise use new position
01460         x += K*( pos.getX() - x );            // based on weighted variance
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   // now calculate global position (experiments show best results are obtained
01471   // when average with all perceived flags is taken).
01472   *angGlobal = calculateAngleAgentWithPos( *posGlobal );
01473 
01474   // update velocity since after 'see' we have a better estimate of neck angle
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     // get all information of the two flags
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     // Sign is like this 'because' y-axis increases from top to bottom
01510     double sign = ((aB - aA) > 0.0) ? 1.0 : -1.0;
01511 
01512     // From Cosinus rule: rB rB = L L + rA rA - 2 L rA cos(aB)
01513     // with:              rA cos(aB) = d_par
01514     // and:               d_par^2 + d_orth^2 = rA^2
01515     // Finally:
01516     // Position from landmark position and vectors parallel and orthogonal
01517     // to line from landmark A to B
01518 
01519     dx = xB - xA;
01520     dy = yB - yA;
01521     L = sqrt(dx*dx + dy*dy);                   // distance between two flags
01522 
01523     dx /= L; dy /= L;                          // normalize
01524 
01525     d_par = (L*L + rA*rA - rB*rB) / (2.0 * L); // dist from flag1 to orth proj
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     // calculate global direction between flag and agent
01558     // calculate global neck angle by subtracting relative angle to flag
01559     dAngleNow = atan2Deg( yA - pos.getY(), xA - pos.getX() );
01560     dAngleNow = VecPosition::normalizeAngle( dAngleNow - aA );
01561 
01562     // add cosine part of angle and sine part separately; this to avoid
01563     // boundary problem when computing average angle (average of -176 and
01564     // 178 equals -179 and not 1).
01565     dCosX += cosDeg( dAngleNow );
01566     dSinX += sinDeg( dAngleNow );
01567     iNrFlags++;
01568 
01569   }
01570   iterateObjectDone( iIndex );
01571 
01572   // calculate average cosine and sine part and determine corresponding angle
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   // set the global position of the ball as follows:
01615   //  - get the relative position from the agent to it in world-axis
01616   //  - add global position agent to this relative position
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     // average result with predicted velocity from last see message
01633     // this has the best result for the ball: see program absvelocity.C
01634     // only use average when no big chance (kick,turn or dash) has occured
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     // no change information but have got feel info -> use position based vel.
01649     // get the difference with the previous known global position
01650     // and subtract the position difference (this difference is caused by
01651     // the fact that the global position calculation contains a lot of noise
01652     // now the noise is filtered, since we compare the velocity as if its
01653     // position was seen "with the noise" of the last cycle).
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     // difference in global positions is distance traveled, we have to make
01665     // distinction whether this distance is traveled in one or two cycles.
01666     // 1 cycle:
01667     //   distance difference equals last velocity so only have to multiply with
01668     //   decay
01669     // 2 cycles:
01670     //   do not update, since kick can be performed causing large change
01671     //   of which we have no see information (so thus use sense update)
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     ;  // object too far away do not estimate velocity, sense has updated it
01690        // already
01691 
01692   // change velocity when the ball
01693   //   has been catched or play mode is not play_on
01694   //   is in kickable distance opponent
01695   //   higher than max speed with added noise
01696   // change position when it is a kick_in: know y coordinate
01697   // change position when it is a back_pass_[rl]: know positions
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   // set the global position of this dynamic object as follows:
01742   //  - get the relative position from the agent to it in world-axis
01743   //  - add global position agent to this relative position
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     // calculate the global velocity using the distance and angle change
01754     // with the formula from the soccermanual
01755     *velGlobal = calculateVelocityDynamicObject( o );
01756   }
01757   else
01758       ; // object too far away do not estimate velocity, sense has updated it
01759         // already and does not really matter then
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   // change output a little bit to circumvent boundaries
01784   // q = quantize(e^(quantize(ln(V),x1)),x2) with quantize(V,Q) = rint(V/Q)*Q
01785   // e^(quantize(ln(V),x1)_min = invQuantize( q, x2 )
01786   // quantize(ln(V),x1) = ln ( invQuantize( q, x2 ) )
01787   // ln(V) = invQuantize( ln ( invQuantize( q, x2 ) ), x1 )
01788   // V_min = e^( invQuantize( ln ( invQuantize( q, x2 ) ), x1 ) )
01789   // apply inverse quantize twice to get correct value
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   // Q_dist = quantize(e^(quantize(ln(V),xDist1)),xDist2)
01832   // q = Q_dist * Quantize( distance_change/distance, x1 )
01833   // dOutput = q/Q_dist = Quantize( distance_change/distance, x1 )
01834   // (distance_change/distance)_min = invQmin(q/Q_dist, x1 )
01835   // real distance is not know so should take into account distance range
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   // q = quantize( V, Q ) = rint(V/Q)*Q -> q/Q = rint( V/Q)
01854   // min = rint(q/Q)-0.5 = V_min/Q -> V_min = (rint(q/Q)-0.5)*Q
01855   return (rint( dOutput / dQuantizeStep )-0.5 )*dQuantizeStep;
01856 }
01857 
01863 double WorldModel::invQuantizeMax( double dOutput, double dQuantizeStep )
01864 {
01865   // q = quantize( V, Q ) = rint(V/Q)*Q -> q/Q = rint( V/Q)
01866   // max = rint(q/Q)+0.5 = V_max/Q -> V_max = (rint(q/Q)-0.5)*Q
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   // for all unknown players, try to map it to closest teammate or opponent
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 ) ) // TEAMMATE_UNKNOWN or PLAYER_UNKNOWN
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 ) ) // OPPONENT_UNKNOWN or PLAYER_UNKNOWN
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     // if player found and in tolerated distance
01929     //   update player information.
01930     // else if not mapped to player
01931     //   put information in first player position of which we have no info.
01932     // if we do not know it is a teammate or opponent only assume opponent when
01933     // it is very close since then it is probably an opponent trying to get ball
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     {                                      // could not map info to a player
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; // make somewhat smaller for error
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   // ball should be "seen" when it is in visible distance. 0.9 is for noise
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   // now remove all opponents. If an opponent is not seen while you expect him
02047   // somewhere, he has moved a lot. It's no use keeping his current position
02048   // in the world model, since he would still be considered in passing 
02049   // confidence, etc.
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   // initialize particles left with total number of particles
02075   int  iNrParticlesLeft = iNrParticlesBall;
02076 
02077   // check which particles are correct. If all gone, re-initialize all particles
02078   checkParticlesBall ( particlesPosBall, particlesVelBall,
02079                                          iNrParticlesBall, &iNrParticlesLeft );
02080   if( iNrParticlesLeft == 0 )
02081   {
02082     initParticlesBall ( particlesPosBall, particlesVelBall, iNrParticlesBall );
02083     iNrParticlesLeft = iNrParticlesBall;
02084   }
02085 
02086   // determine position and velocity and resample all deleted particles
02087   *posGlobal = averageParticles( particlesPosBall, iNrParticlesLeft );
02088   *velGlobal = averageParticles( particlesVelBall, iNrParticlesLeft );
02089   resampleParticlesBall( particlesPosBall, particlesVelBall,
02090                                            iNrParticlesBall, iNrParticlesLeft );
02091 
02092   // if we had no chance information, but but ball is within feel distance
02093   // and we have also seen it in the previous cycle -> use position based vel.
02094   if( Ball.getTimeChangeInformation( ) != getTimeLastSeen( OBJECT_BALL ) &&
02095       getRelativeDistance(OBJECT_BALL) < SS->getVisibleDistance() &&
02096       getTimeLastSeeMessage() - Ball.getTimeGlobalPosDerivedFromSee() == 1 )
02097   {
02098     // get the difference with the previous known global position
02099     // and subtract the position difference (this difference is caused by
02100     // the fact that the global position calculation contains a lot of noise
02101     // now the noise is filtered, since we compare the velocity as if its
02102     // position was seen "with the noise" of the last cycle).
02103 
02104     VecPosition posGlobalDiff = *posGlobal - Ball.getGlobalPositionLastSee()
02105                                            - agentObject.getPositionDifference();
02106 
02107     *velGlobal = posGlobalDiff*SS->getBallDecay();
02108   }
02109 
02110   // adapt velocity and position based on specific situations
02111   // set velocity to zero when in kick distance opponent for example
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   // declare a bunch of variables
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   // no information  received -> no initialization
02146   if( Ball.getTimeRelativePosition() != getTimeLastSeeMessage() )
02147     return;
02148 
02149   // get perceived values for distance and angle with ball
02150   dDistBall = getRelativeDistance( OBJECT_BALL );
02151   angBall   = getRelativeAngle( OBJECT_BALL );
02152 
02153   // get perceived values for distance and direction change
02154   if( Ball.getTimeChangeInformation( ) == getTimeLastSeeMessage() )
02155   {
02156     dDistChange = Ball.getRelativeDistanceChange();
02157     angChange   = Ball.getRelativeAngleChange();
02158   }
02159 
02160   // get ranges from which values could originate
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     // make random distance and angle from range (angle is rounded) and make pos
02168     dDistTmp   = dMinDist + drand48()*fabs(dMaxDist - dMinDist); // angle->sign
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       // make random values for direction and distance change and make velocity
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   // no new perceptions, do not check
02218   if( getTimeLastSeen( OBJECT_BALL ) != getTimeLastSeeMessage() )
02219     return;
02220 
02221   // initialize values distance, direction, distance change and direction change
02222   // and get the associated ranges
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     // get particles and make them relative to the agent to compare
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     // if not illegal, save particles and raise iNrParticlesLeft
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     // if power supplied, assume ball (and thus particles) are kicked
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     // add noise in same way server does.
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);       // pick random particle
02342     posArray[ i ] = posArray[ iRand ];    // and copy contents
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;                             // no more objects
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 ) )      // max range is one lower
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 

Generated on Tue Jul 2 10:18:55 2002 for UvA Trilearn 2002 by doxygen1.2.12 written by Dimitri van Heesch, © 1997-2001