src/WorldModelUpdate.cpp

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

Generated on Thu Apr 26 22:45:29 2007 for GangOfSix(GOS)-RoboCupTeamProject by  doxygen 1.5.1-p1