src/WorldModelHighLevel.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 
00049 #include<list>            // needed for list<double>
00050 #include<stdio.h>         // needed for printf
00051 #include "WorldModel.h"
00052 
00058 int WorldModel::getNrInSetInCircle( ObjectSetT set, Circle c )
00059 {
00060   double dConfThr = PS->getPlayerConfThr();
00061   int    iNr      = 0;
00062   int    iIndex;
00063 
00064   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00065        o != OBJECT_ILLEGAL;
00066        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00067   {
00068     if( c.isInside( getGlobalPosition( o ) ) )
00069       iNr++;
00070   }
00071   iterateObjectDone( iIndex );
00072 
00073   return iNr;
00074 }
00075 
00085 int WorldModel::getNrInSetInRectangle( ObjectSetT set, Rect *rect  )
00086 {
00087   double dConfThr = PS->getPlayerConfThr();
00088   int    iNr      = 0;
00089   int    iIndex;
00090 
00091   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00092        o != OBJECT_ILLEGAL;
00093        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00094   {
00095     if( rect == NULL || rect->isInside( getGlobalPosition( o ) ) )
00096       iNr++;
00097   }
00098   iterateObjectDone( iIndex );
00099   return iNr;
00100 }
00101 
00113 int WorldModel::getNrInSetInCone( ObjectSetT set, double dWidth,
00114                                       VecPosition start , VecPosition end )
00115 {
00116   double      dConfThr   = PS->getPlayerConfThr();
00117   int         iNr        = 0;
00118   int         iIndex;
00119   Line        line       = Line::makeLineFromTwoPoints( start, end );
00120   VecPosition posOnLine;
00121   VecPosition posObj;
00122 
00123   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00124        o != OBJECT_ILLEGAL;
00125        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00126   {
00127     posObj    = getGlobalPosition( o );
00128     posOnLine = line.getPointOnLineClosestTo( posObj );
00129     // whether posOnLine lies in cone is checked by three constraints
00130     // - does it lie in triangle (to infinity)
00131     // - lies between start and end (and thus not behind me)
00132     // - does it lie in circle
00133     if(posOnLine.getDistanceTo(posObj) < dWidth*posOnLine.getDistanceTo(start)
00134        && line.isInBetween( posOnLine, start, end )
00135        && start.getDistanceTo( posObj ) < start.getDistanceTo( end ) )
00136         iNr++;
00137   }
00138   iterateObjectDone( iIndex );
00139   return iNr;
00140 }
00141 
00142 
00145 bool WorldModel::isEmptySpace( ObjectT obj, AngDeg ang, double dDist )
00146 {
00147   if( obj == OBJECT_ILLEGAL )
00148     return false;
00149 
00150   VecPosition pos = getGlobalPosition( obj );
00151    pos += VecPosition( dDist, ang, POLAR );
00152 
00153   if( getNrInSetInCircle( OBJECT_SET_OPPONENTS, Circle( pos, dDist ) ) == 0 )
00154     return true;
00155 
00156   return false;
00157 }
00158 
00159 
00160 bool WorldModel::coordinateWith( ObjectT obj )
00161 {
00162   VecPosition pos = getGlobalPosition( obj );
00163   if( pos.getDistanceTo( getBallPos() ) < 30.0 &&
00164       pos.getX() > getBallPos().getX() - 5.0 )
00165   {
00166     
00167     if( getFastestInSetTo( OBJECT_SET_TEAMMATES, OBJECT_BALL ) ==
00168         getAgentObjectType() )
00169     {
00170       logCircle( 700, pos, 2.5 );
00171     }
00172     Log.log( 700, "coordinate with %d %f %f (%f %f)", 
00173              obj, pos.getDistanceTo( getBallPos() ),
00174              pos.getX(), getBallPos().getX(), getBallPos().getY() );
00175     return true;
00176   }
00177   
00178   return false;
00179 }
00180 
00196 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, ObjectT objTarget,
00197                                         double *dDist, double dConfThr, double dMinRange, Rect *rect )
00198 {
00199   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00200   ObjectT     closestObject = OBJECT_ILLEGAL;
00201   double      dMinMag       = 1000.0;
00202   VecPosition v;
00203   VecPosition op = getGlobalPosition( objTarget ); 
00204   VecPosition ob = getGlobalPosition( objTarget ); 
00205   int         iIndex;
00206 
00207   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00208        o != OBJECT_ILLEGAL;
00209        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00210   {
00211     if( o != objTarget )               // do not include target object
00212     {
00213       ob = getGlobalPosition( o );
00214       v = op - ob;
00215       if( v.getMagnitude() < dMinMag && v.getMagnitude() >= dMinRange
00216         && (rect == NULL || (rect != NULL && rect->isInside(ob))))
00217       {
00218         dMinMag       = v.getMagnitude();
00219         closestObject = o;
00220       }
00221     }
00222   }
00223 
00224   iterateObjectDone( iIndex );
00225   if( dDist != NULL )
00226     *dDist = dMinMag;
00227   return closestObject;
00228 }
00229 
00244 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, VecPosition pos,
00245                                        double *dDist,  double dConfThr, double dMinRange, Rect *rect )
00246 {
00247   ObjectT     closestObject   = OBJECT_ILLEGAL;
00248   double      dMinMag         = 1000.0;
00249   VecPosition v;
00250   VecPosition ob;
00251   int         iIndex;
00252 
00253   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00254   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00255        o != OBJECT_ILLEGAL;
00256        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00257   {
00258     ob = getGlobalPosition( o );
00259     v = pos - ob;
00260     if( v.getMagnitude() < dMinMag && v.getMagnitude() >= dMinRange 
00261       && (rect == NULL || (rect != NULL && rect->isInside(ob))))
00262     {
00263       dMinMag         = v.getMagnitude();
00264       closestObject   = o;
00265     }
00266   }
00267   iterateObjectDone( iIndex );
00268   if( dDist != NULL )
00269     *dDist = dMinMag;
00270   return closestObject;
00271 }
00272 
00288 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, Line l,
00289                               VecPosition pos1, VecPosition pos2,
00290                               double *dDistObjToLine, double *dDistPos1ToPoint)
00291 {
00292   VecPosition posObj;
00293   VecPosition posOnLine;
00294   double      dConfThr  = PS->getPlayerConfThr();
00295   ObjectT     obj       = OBJECT_ILLEGAL;
00296   double      dDist     ;
00297   double      dMinDist  = 1000.0;
00298   double      dDistPos1 = 1000.0;
00299   int         iIndex;
00300 
00301   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00302        o != OBJECT_ILLEGAL;
00303        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00304   {
00305     posObj    = getGlobalPosition( o );
00306     posOnLine = l.getPointOnLineClosestTo( posObj );
00307     dDist     = posObj.getDistanceTo( posOnLine );
00308     if( l.isInBetween( posOnLine, pos1, pos2 ) && dDist < dMinDist )
00309     {
00310       dMinDist  = dDist;
00311       obj       = o;
00312       dDistPos1 = pos1.getDistanceTo( posOnLine );
00313     }
00314   }
00315   iterateObjectDone( iIndex );
00316   if( dDistObjToLine != NULL )
00317     *dDistObjToLine = dMinDist;
00318   if( dDistPos1ToPoint != NULL )
00319     *dDistPos1ToPoint = dDistPos1;
00320 
00321   return obj;
00322 }
00323 
00330 ObjectT WorldModel::getClosestRelativeInSet( ObjectSetT set, double *dDist, double dMinRange, Rect *rect )
00331 {
00332   ObjectT     closestObject = OBJECT_ILLEGAL;
00333   double      dMinMag       = 1000.0;
00334   int         iIndex;
00335 
00336   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00337        o != OBJECT_ILLEGAL;
00338        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00339   {
00340     double d = getRelativeDistance( o ); 
00341     if( d < dMinMag && d >= dMinRange
00342       && (rect == NULL || (rect != NULL && rect->isInside(getGlobalPosition(o)))))
00343     {
00344       dMinMag       = d;
00345       closestObject = o;
00346     }
00347   }
00348 
00349   iterateObjectDone( iIndex );
00350   if( dDist != NULL )
00351     *dDist = dMinMag;
00352   return closestObject;
00353 }
00354 
00365 ObjectT WorldModel::getSecondClosestInSetTo ( ObjectSetT set, ObjectT obj,
00366                                               double *dDist,  double dConfThr )
00367 {
00368   VecPosition v;
00369   ObjectT     closestObject       = OBJECT_ILLEGAL;
00370   ObjectT     secondClosestObject = OBJECT_ILLEGAL;
00371   double      dMinMag             = 1000.0;
00372   double      dSecondMinMag       = 1000.0;
00373   int         iIndex;
00374 
00375   if( dConfThr == -1.0 ) dConfThr = PS->getPlayerConfThr();
00376 
00377   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00378        o != OBJECT_ILLEGAL;
00379        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00380   {
00381     if( o != obj )
00382     {
00383       v = getGlobalPosition( obj ) - getGlobalPosition( o );
00384       if( v.getMagnitude() < dMinMag )                  // closer then first
00385       {
00386         dSecondMinMag         = dMinMag;                // put first to second
00387         secondClosestObject   = closestObject;
00388         dMinMag               = v.getMagnitude();       // and this to first
00389         closestObject         = o;
00390       }
00391       else if( v.getMagnitude() < dSecondMinMag )       // between 1st and 2nd
00392       {
00393         dSecondMinMag         = v.getMagnitude();       // put this to second
00394         secondClosestObject   = o;
00395       }
00396     }
00397   }
00398   iterateObjectDone( iIndex );
00399   if( dDist != NULL )
00400     *dDist = dSecondMinMag;
00401   return secondClosestObject;
00402 }
00403 
00410 ObjectT WorldModel::getSecondClosestRelativeInSet( ObjectSetT set, 
00411                                                    double *dDist )
00412 {
00413   ObjectT     closestObject       = OBJECT_ILLEGAL;
00414   ObjectT     secondClosestObject = OBJECT_ILLEGAL;
00415   double      dMinMag             = 1000.0;
00416   double      dSecondMinMag       = 1000.0;
00417   double      d;
00418   int         iIndex;
00419 
00420   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00421        o != OBJECT_ILLEGAL;
00422        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00423   {
00424     d = getRelativeDistance( o );
00425     if( d < dMinMag )                                 // closer then first
00426     {
00427       dSecondMinMag         = dMinMag;                // put first to second
00428       secondClosestObject   = closestObject;
00429       dMinMag               = d;                      // and this to first
00430       closestObject         = o;
00431     }
00432     else if( d < dSecondMinMag )                      // between first and 2nd
00433     {
00434       dSecondMinMag         = d;                      // put this to second
00435       secondClosestObject   = o;
00436     }
00437   }
00438   iterateObjectDone( iIndex );
00439   if( dDist != NULL )
00440     *dDist = dSecondMinMag;
00441   return secondClosestObject;
00442 }
00443 
00444 
00456 ObjectT WorldModel::getFurthestInSetTo( ObjectSetT set, ObjectT objTarget,
00457                                         double *dDist, double dConfThr )
00458 {
00459   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00460 
00461   ObjectT     furthestObject = OBJECT_ILLEGAL;
00462   double      dMaxMag       = -1000.0;
00463   VecPosition v;
00464   int         iIndex;
00465 
00466   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00467        o != OBJECT_ILLEGAL;
00468        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00469   {
00470     if( o != objTarget )
00471     {
00472       v = getGlobalPosition( objTarget ) - getGlobalPosition( o );
00473       if( v.getMagnitude() > dMaxMag )
00474       {
00475         dMaxMag        = v.getMagnitude();
00476         furthestObject = o;
00477       }
00478     }
00479   }
00480   iterateObjectDone( iIndex );
00481   if( dDist != NULL )
00482     *dDist = dMaxMag;
00483   return furthestObject;
00484 }
00485 
00492 ObjectT WorldModel::getFurthestRelativeInSet( ObjectSetT set, double *dDist  )
00493 {
00494   ObjectT     furthestObject = OBJECT_ILLEGAL;
00495   double      dMaxMag       = -1000.0;
00496   int         iIndex;
00497 
00498   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00499        o != OBJECT_ILLEGAL;
00500        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00501   {
00502     if( getRelativeDistance( o ) > dMaxMag )
00503     {
00504       dMaxMag        = getRelativeDistance( o );
00505       furthestObject = o;
00506     }
00507   }
00508   iterateObjectDone( iIndex );
00509   if( dDist != NULL )
00510     *dDist = dMaxMag;
00511   return furthestObject;
00512 }
00513 
00514 
00515 VecPosition WorldModel::getPosClosestOpponentTo( double *dDist, ObjectT o )
00516 {
00517   if( o == OBJECT_ILLEGAL )
00518     o = getAgentObjectType();
00519   ObjectT objOpp = getClosestInSetTo( OBJECT_SET_OPPONENTS, o, dDist );
00520   if( objOpp == OBJECT_ILLEGAL )
00521     return VecPosition( UnknownDoubleValue, UnknownDoubleValue );
00522     
00523   return getGlobalPosition( objOpp );
00524 }
00525 
00526 double WorldModel::getMaxTraveledDistance( ObjectT o )
00527 {
00528   return (getCurrentTime() - getTimeLastSeen( o ) )*SS->getPlayerSpeedMax();
00529 }
00530 
00531 
00532 void WorldModel::createInterceptFeatures( )
00533 {
00534   static int count = 0;
00535   static Time timeLastCalled(0,0);
00536   
00537   if( timeLastCalled == getTimeLastSenseMessage() )
00538     count++;
00539   else
00540     count = 0;
00541     
00542   if( count > 4 )
00543     cerr << getPlayerNumber() << " called createIntercept too often: " << 
00544        count << endl;
00545   // we check all possible next positions of the ball and see
00546   // whether a player (opponent or teammate) can reach the ball at that point
00547   // if so, we log this as a feature. We finish when all features have been
00548   // found.
00549   ObjectSetT      set = OBJECT_SET_PLAYERS;
00550   int             iCycles       = -1;
00551   int             iMinCyclesTeam    = 100;
00552   int             iMinCyclesOpp     = 100;
00553   bool            bOnlyMe           = false;
00554 
00555   VecPosition     posObj;
00556   int             iIndex;
00557   int             iCyclesToObj  ;
00558 
00559   // no feature available, calculate information
00560   ObjectT         objFastestTeam             = OBJECT_ILLEGAL;
00561   ObjectT         objFastestTeamNoGoalie     = OBJECT_ILLEGAL;
00562   ObjectT         objFastestOpp              = OBJECT_ILLEGAL;
00563   ObjectT         objFastestPlayer           = OBJECT_ILLEGAL;
00564 
00565   int             iCyclesFastestPlayer       = -1;
00566   int             iCyclesFastestTeam         = -1;
00567   int             iCyclesFastestTeamNoGoalie = -1;
00568   int             iCyclesFastestOpp          = -1;
00569   int             iCyclesFastestMe           = -1;
00570 
00571   bool            bFinishedPlayer            = false;
00572   bool            bFinishedTeammates         = false;
00573   bool            bFinishedTeammatesNoGoalie = false;
00574   bool            bFinishedOpponents         = false;
00575   bool            bFinishedMe                = false;
00576   bool            bFinished                  = false;
00577 
00578   ObjectT         objLog                     = OBJECT_ILLEGAL;
00579   int             iCyclesLog                 = -1;
00580   FeatureT        featLog                    = FEATURE_ILLEGAL;
00581 
00582   // for each next position of the ball
00583   while( bFinished == false && iCycles <= PS->getPlayerWhenToIntercept() )
00584   {
00585     iCycles++;
00586     iMinCyclesTeam = 100;
00587     iMinCyclesOpp  = 100;
00588     Log.log( 460, "fastest loop: %d", iCycles );
00589 
00590     // determine its position and traverse all players to check the teammate
00591     // and opponent who can reach it first
00592     posObj     = predictPosAfterNrCycles( OBJECT_BALL, iCycles );
00593     for( ObjectT o = iterateObjectStart( iIndex, set );
00594        o != OBJECT_ILLEGAL;
00595        o = iterateObjectNext ( iIndex, set ) )
00596     {
00597       if( getGlobalPosition(o).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
00598           < iCycles + 1 && (bOnlyMe == false || SoccerTypes::isOpponent( o )
00599           || o == getAgentObjectType() ) )
00600       {
00601         Log.log( 460, "call predictNrCyclesToPoint %d %d %d",
00602                        iCycles, iMinCyclesTeam, iMinCyclesOpp );
00603         iCyclesToObj = predictNrCyclesToPoint( o, posObj );
00604 
00605         if( iCyclesToObj < iMinCyclesOpp && SoccerTypes::isOpponent( o ) )
00606         {
00607           iMinCyclesOpp = iCyclesToObj;
00608           objFastestOpp = o;
00609         }
00610         if( iCyclesToObj < iMinCyclesTeam && SoccerTypes::isTeammate( o ) )
00611         {
00612           iMinCyclesTeam = iCyclesToObj;
00613           objFastestTeam = o;
00614         }
00615       }
00616     }
00617     iterateObjectDone( iIndex );
00618 
00619     bool bContinue = true;
00620     bool bLastCall = ( iCycles == PS->getPlayerWhenToIntercept() );
00621     // log all features that have been solved
00622     while( bContinue )
00623     {
00624       featLog = FEATURE_ILLEGAL;
00625       if( bLastCall )
00626         iCycles = 100;
00627 
00628       // if player not set yet and either team or opp is smaller than iCycles
00629       // set fastest player
00630       if( bFinishedPlayer == false &&
00631           ( min( iMinCyclesTeam, iMinCyclesOpp ) <= iCycles 
00632             ||
00633             bLastCall == true ) )
00634       {
00635         featLog              = FEATURE_FASTEST_PLAYER_TO_BALL;
00636         iCyclesLog           = iCycles;
00637         iCyclesFastestPlayer = iCycles;
00638         objLog               = (iMinCyclesTeam<=iMinCyclesOpp) ?
00639                                     objFastestTeam : objFastestOpp;
00640         objFastestPlayer     = objLog;
00641         bFinishedPlayer      = true;
00642       }
00643       // if teammate not set yet and min cycles team smaller set it
00644       else if( bFinishedTeammates == false &&
00645                (iMinCyclesTeam <= iCycles || bFinishedOpponents == true 
00646                 || bLastCall))
00647       {
00648         if( bFinishedOpponents == true )
00649           objFastestTeam = getFastestInSetTo( OBJECT_SET_TEAMMATES, posObj,
00650                              VecPosition(0,0), 0, &iCycles );
00651         featLog            = FEATURE_FASTEST_TEAMMATE_TO_BALL;
00652         iCyclesLog         = iCycles;
00653         iCyclesFastestTeam = iCycles;
00654         objLog             = objFastestTeam;
00655         bFinishedTeammates = true;
00656       }
00657       else if( bFinishedTeammatesNoGoalie == false &&
00658          ( ( iMinCyclesTeam <= iCycles && objFastestTeam != getOwnGoalieType())
00659            || bFinishedOpponents == true || bLastCall ) )
00660       {
00661         if( bFinishedOpponents == true && objFastestTeam == getOwnGoalieType())
00662           objFastestTeam=getFastestInSetTo( OBJECT_SET_TEAMMATES_NO_GOALIE,
00663                             posObj, VecPosition(0,0), 0, &iCycles );
00664         featLog                   = FEATURE_FASTEST_TEAMMATE_TO_BALL_NO_GOALIE;
00665         iCyclesLog                 = iCycles;
00666         iCyclesFastestTeamNoGoalie = iCycles;
00667         objLog                     = objFastestTeam;
00668         objFastestTeamNoGoalie     = objFastestTeam;
00669         bFinishedTeammatesNoGoalie = true;
00670       }
00671       else if( bFinishedMe == false &&
00672         ((iMinCyclesTeam <= iCycles && objFastestTeam == getAgentObjectType())
00673         || bFinishedOpponents == true || bLastCall ) )
00674       {
00675         if( bFinishedOpponents == true && 
00676             objFastestTeam != getAgentObjectType())
00677           iCycles = predictNrCyclesToPoint( getAgentObjectType(), posObj );
00678         featLog          = FEATURE_INTERCEPT_CYCLES_ME;
00679         iCyclesLog       = iCycles;
00680         iCyclesFastestMe = iCycles;
00681         objLog           = getAgentObjectType();
00682         bFinishedMe      = true;
00683       }
00684       else if( bFinishedOpponents == false &&
00685                ( iMinCyclesOpp <= iCycles  || bLastCall ) )
00686       {
00687         featLog            = FEATURE_FASTEST_OPPONENT_TO_BALL;
00688         iCyclesLog         = iCycles;
00689         iCyclesFastestOpp  = iCycles;
00690         objLog             = objFastestOpp;
00691         bFinishedOpponents = true;
00692 
00693       }
00694       else
00695         bContinue = false;
00696 
00697       if( featLog != FEATURE_ILLEGAL )
00698       {
00699         Log.log( 460, "log feature %d object %d in %d cycles sense %d see %d",
00700           featLog, objLog, iCyclesLog,getTimeLastSenseMessage().getTime(),
00701           getTimeLastSeeMessage().getTime()  );
00702         setFeature( featLog,
00703                     Feature( getTimeLastSeeMessage(),
00704                              getTimeLastSenseMessage(),
00705                              getTimeLastHearMessage(), objLog,
00706                              getTimeLastSeeMessage().getTime() + iCyclesLog));
00707       }
00708     }
00709     bFinished = bFinishedTeammates && bFinishedTeammatesNoGoalie;
00710     if( bFinished == true )
00711       bOnlyMe = true;
00712     bFinished &= bFinishedMe ;
00713     if( bFinished == true )
00714       set = OBJECT_SET_OPPONENTS;
00715     bFinished &= bFinishedOpponents;
00716   }
00717   Log.log( 460, "creatIntercept: team %d me %d opp %d",
00718      iCyclesFastestTeamNoGoalie, iCyclesFastestMe, iCyclesFastestOpp );
00719 }
00720 
00721 
00731 ObjectT WorldModel::getFastestInSetTo( ObjectSetT set, ObjectT obj,
00732                               int *iCyclesToIntercept )
00733 {
00734   ObjectT  objFastestOpp  = OBJECT_ILLEGAL, objFastestTeam  = OBJECT_ILLEGAL;
00735   int      iCyclesFastestOpp = 30; // how much do we try
00736   int     iCyclesFastestTeam;
00737   bool  bSkip = false;
00738 
00739   FeatureT        feature_type = FEATURE_ILLEGAL;
00740   ObjectT         fastestObject = OBJECT_ILLEGAL;
00741   int             iCycles       = -1;
00742 
00743   if( obj == OBJECT_BALL )
00744   {
00745     switch( set )
00746     {
00747       case OBJECT_SET_OPPONENTS:
00748         feature_type = FEATURE_FASTEST_OPPONENT_TO_BALL;
00749         break;
00750       case OBJECT_SET_TEAMMATES:
00751         feature_type = FEATURE_FASTEST_TEAMMATE_TO_BALL;
00752         break;
00753       case OBJECT_SET_TEAMMATES_NO_GOALIE:
00754         feature_type = FEATURE_FASTEST_TEAMMATE_TO_BALL_NO_GOALIE;
00755         break;
00756       case OBJECT_SET_PLAYERS:
00757         objFastestOpp =
00758          getFastestInSetTo( OBJECT_SET_OPPONENTS, obj, &iCyclesFastestOpp);
00759         objFastestTeam =
00760          getFastestInSetTo( OBJECT_SET_TEAMMATES, obj, &iCyclesFastestTeam);
00761         if( iCyclesFastestOpp < iCyclesFastestTeam )
00762         {
00763           fastestObject = objFastestOpp;
00764           iCycles = iCyclesFastestOpp;
00765         }
00766         else
00767         {
00768           fastestObject = objFastestTeam;
00769           iCycles = iCyclesFastestTeam;
00770         }
00771         bSkip = true;
00772         feature_type = FEATURE_FASTEST_PLAYER_TO_BALL;
00773         break;
00774       default:
00775         cerr << "WorldModel::getFastestInSetTo unknown set: " << set << endl;
00776         return OBJECT_ILLEGAL;
00777     }
00778     if( isFeatureRelevant( feature_type ) )
00779     {
00780       int i = max(0,
00781              ((int)getFeature( feature_type ).getInfo() - getCurrentCycle() ));
00782       if( iCyclesToIntercept != NULL )
00783         *iCyclesToIntercept  = i;
00784       return getFeature( feature_type ).getObject();
00785     }
00786 
00787     Log.log( 460, "create intercept features" );
00788     createInterceptFeatures( );
00789     Log.log( 460, "call fastest again" );
00790     return getFastestInSetTo( set, obj, iCyclesToIntercept );
00791     if( set == OBJECT_SET_TEAMMATES || set == OBJECT_SET_TEAMMATES_NO_GOALIE )
00792       objFastestOpp =
00793          getFastestInSetTo( OBJECT_SET_OPPONENTS, obj, &iCyclesFastestOpp);
00794   }
00795 
00796   // no feature available, calculate information
00797   double          dConfThr      = PS->getPlayerConfThr();
00798   int             iCyclesToObj  ;
00799   int             iMinCycles    = 100;
00800   int             iIndex;
00801   VecPosition     posObj;
00802 
00803 
00804   while( bSkip == false &&
00805          iCycles < iMinCycles &&
00806          iCycles <= iCyclesFastestOpp )
00807   {
00808     iCycles++;
00809     iMinCycles = 100;
00810     posObj     = predictPosAfterNrCycles( obj, iCycles );
00811     Log.log( 460, "fastest loop: %d fastest_opp %d", 
00812              iCycles, iCyclesFastestOpp );
00813     for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00814        o != OBJECT_ILLEGAL;
00815        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00816     {
00817       if( getGlobalPosition(o).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
00818               < iMinCycles &&
00819           getGlobalPosition(o).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
00820               < iCycles + 1 )
00821       {
00822         Log.log( 460, "call predictNrCyclesToPoint %d %d", 
00823                  iCycles,iMinCycles );
00824         iCyclesToObj = predictNrCyclesToPoint( o, posObj );
00825         if( iCyclesToObj < iMinCycles )
00826         {
00827           iMinCycles = iCyclesToObj;
00828           fastestObject = o;
00829         }
00830       }
00831     }
00832     iterateObjectDone( iIndex );
00833   }
00834   
00835   // opponent is faster and we haven't calculated who can go to the 
00836   // interception point the fastest
00837   if( fastestObject == OBJECT_ILLEGAL )
00838     fastestObject = getFastestInSetTo(set,posObj,VecPosition(0,0),0, &iCycles);
00839 
00840   if( iCyclesToIntercept != NULL )
00841     *iCyclesToIntercept  = iCycles;
00842 
00843   if( feature_type != FEATURE_ILLEGAL )
00844   {
00845     Log.log( 460, "log feature %d object %d in %d cycles sense %d see %d",
00846              feature_type, fastestObject, iCycles,getTimeLastSenseMessage().
00847              getTime(), getTimeLastSeeMessage().getTime()  );
00848     setFeature( feature_type,
00849                 Feature( getTimeLastSeeMessage(),
00850                          getTimeLastSenseMessage(), 
00851                          getTimeLastHearMessage(), fastestObject,
00852                          getTimeLastSeeMessage().getTime() + iCycles ) );
00853   }
00854 
00855   return fastestObject;
00856 }
00857 
00868 ObjectT WorldModel::getFastestInSetTo( ObjectSetT set, VecPosition pos,
00869                     VecPosition vel, double dDecay, int *iCyclesToIntercept)
00870 {
00871   double  dConfThr      = PS->getPlayerConfThr();
00872   ObjectT fastestObject = OBJECT_ILLEGAL;
00873   int     iCycles       = 0;
00874   int     iCyclesToObj  ;
00875   int     iMinCycles    = 100;
00876   int     iIndex;
00877 
00878   while( iCycles <= iMinCycles && iCycles < 100)
00879   {
00880     iCycles    = iCycles + 1  ;
00881     iMinCycles = 100;
00882     Log.log( 460, "fastest to point: %d", iCycles );
00883     for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00884        o != OBJECT_ILLEGAL;
00885        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00886     {
00887        if( getGlobalPosition(o).getDistanceTo(pos)/SS->getPlayerSpeedMax()
00888            < iMinCycles )
00889        {
00890          iCyclesToObj = predictNrCyclesToPoint( o, pos );
00891          if( iCyclesToObj < iMinCycles )
00892          {
00893            iMinCycles    = iCyclesToObj;
00894            fastestObject = o;
00895          }
00896        }
00897     }
00898     iterateObjectDone( iIndex );
00899     pos += vel;
00900     vel *= dDecay;
00901     if( vel.getMagnitude( ) < EPSILON ) // we can quit
00902     {
00903       iCycles = iMinCycles;
00904       iMinCycles--;
00905     }
00906   }
00907   
00908   if( iCyclesToIntercept != NULL )
00909     *iCyclesToIntercept = iCycles;
00910   return fastestObject;
00911 }
00912 
00927 ObjectT WorldModel::getFirstEmptySpotInSet( ObjectSetT set, int iUnknownPlayer)
00928 {
00929   int     iIndex;
00930 
00931   for( ObjectT o = iterateObjectStart( iIndex, set,  0.0, true );
00932        o != OBJECT_ILLEGAL;
00933        o = iterateObjectNext ( iIndex, set, 0.0, true ) )
00934   {
00935     if( getConfidence( o ) <= PS->getPlayerConfThr() &&
00936         o != getAgentObjectType() )
00937       return o;
00938   }
00939   return OBJECT_ILLEGAL;
00940 }
00941 
00942 
00949 bool WorldModel::isVisible( ObjectT o )
00950 {
00951   Object *object = getObjectPtrFromType( o );
00952 
00953   if( object != NULL &&
00954       object->getTimeLastSeen() == getTimeLastSeeMessage() )
00955     return true;
00956 
00957   return false;
00958 }
00959 
00966 bool WorldModel::isBallKickable()
00967 {
00968   return getRelativeDistance( OBJECT_BALL ) < SS->getMaximalKickDist();
00969 }
00970 
00980 bool WorldModel::isBallCatchable()
00981 {
00982   return getTimeSinceLastCatch()            > SS->getCatchBanCycle()  &&
00983          getRelativeDistance( OBJECT_BALL ) <= SS->getCatchableAreaL() &&
00984          isInOwnPenaltyArea( getBallPos() );
00985 }
00986 
00996 bool WorldModel::isBallHeadingToGoal(  )
00997 {
00998   int iSide = 1;
00999 
01000   if( isPenaltyUs() || isPenaltyThem() )
01001     iSide = ( getSide() == getSidePenalty() ) ? 1 : -1;
01002 
01003   if( !isConfidenceGood( OBJECT_BALL ) ||
01004       fabs( getBallPos().getX() ) <  PENALTY_X - 5.0 )
01005   {
01006     Log.log( 553, "ball not towards goal: confidence too low" );
01007     return false;
01008   }
01009 
01010   // make line from ball heading and goal line
01011   Line l = Line::makeLineFromPositionAndAngle(getBallPos(),getBallDirection());
01012   Line l2= Line::makeLineFromTwoPoints( getPosOwnGoal(), getPosOwnGoal() +
01013                                                          VecPosition( 0, 10 ));
01014 
01015   // if intersection is outside goalwidth, not heading to goal
01016   VecPosition posIntersect = l.getIntersection( l2 );
01017   if( fabs(posIntersect.getY()) > SS->getGoalWidth()/2.0 + 3.0)
01018   {
01019     Log.log( 553, "ball not towards goal: outside goal %f",
01020        posIntersect.getY());
01021     return false;
01022   }
01023 
01024   // check whether ball will be behind goal line within 20 cycles.
01025   VecPosition pos    = getBallPos();
01026   int         iCycle = 1;
01027   while( fabs( pos.getX() ) < PITCH_LENGTH/2.0 && iCycle < 20)
01028   {
01029     pos = predictPosAfterNrCycles( OBJECT_BALL, iCycle );
01030     Log.log( 553, "predicted pos %d cycles: (%f,%f)" ,
01031       iCycle, pos.getX(), pos.getY() );
01032     iCycle ++;
01033   }
01034 
01035   return ( iCycle == 20 ) ? false : true;
01036 }
01037 
01042 bool WorldModel::isBallInOurPossesion( )
01043 {
01044   int     iCyc;
01045   ObjectT o = getFastestInSetTo( OBJECT_SET_PLAYERS, OBJECT_BALL, &iCyc );
01046 
01047   if( o == OBJECT_ILLEGAL )
01048     return false;
01049   if( SoccerTypes::isTeammate( o ) )
01050     return true;
01051   else
01052     return false;
01053 }
01054 
01057 bool WorldModel::isBallInOwnPenaltyArea( )
01058 {
01059   return isInOwnPenaltyArea( getBallPos() );
01060 }
01061 
01066 bool WorldModel::isInOwnPenaltyArea( VecPosition pos )
01067 {
01068   ObjectT     objFlag = ( getSide() == SIDE_LEFT  )
01069                               ?  OBJECT_FLAG_P_L_C
01070                               :  OBJECT_FLAG_P_R_C ;
01071 
01072   if( isPenaltyUs() || isPenaltyThem() )
01073     objFlag = ( getSidePenalty() == SIDE_LEFT ) ? OBJECT_FLAG_P_L_C 
01074                                                 : OBJECT_FLAG_P_R_C ;
01075   VecPosition posFlag =SoccerTypes::getGlobalPositionFlag( objFlag, getSide());
01076   if( fabs(pos.getX())   > fabs(posFlag.getX()) &&
01077       fabs( pos.getY() ) < PENALTY_AREA_WIDTH/2.0 )
01078     return true;
01079 
01080   return false;
01081 }
01082 
01087 bool WorldModel::isInTheirPenaltyArea( VecPosition pos )
01088 {
01089   ObjectT     objFlag = ( getSide() == SIDE_LEFT )
01090                               ?  OBJECT_FLAG_P_R_C
01091                               :  OBJECT_FLAG_P_L_C ;
01092   VecPosition posFlag = SoccerTypes::getGlobalPositionFlag( objFlag,getSide());
01093 
01094   if ( pos.getX() > posFlag.getX() &&
01095        fabs(pos.getY()) < PENALTY_AREA_WIDTH/2.0 )
01096     return true;
01097 
01098   return false;
01099 }
01100 
01109 bool WorldModel::isConfidenceGood( ObjectT o )
01110 {
01111   return getConfidence( o ) > PS->getPlayerConfThr() &&
01112          o != getAgentObjectType();
01113 }
01114 
01123 bool WorldModel::isConfidenceVeryGood( ObjectT o )
01124 {
01125   return getConfidence( o ) > PS->getPlayerHighConfThr() &&
01126          o != getAgentObjectType();
01127 }
01128 
01132 bool WorldModel::isOnside( ObjectT obj )
01133 {
01134   return getGlobalPosition( obj ).getX() < getOffsideX() - 0.5 ;
01135 }
01136 
01144 bool WorldModel::isOpponentAtAngle( AngDeg ang , double dDist )
01145 {
01146   VecPosition posAgent   = getAgentGlobalPosition();
01147   VecPosition posOpp;
01148   AngDeg      angOpp;
01149   int         iIndex;
01150 
01151   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS );
01152        o != OBJECT_ILLEGAL;
01153        o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS ) )
01154   {
01155     posOpp    = getGlobalPosition( o );
01156     angOpp    = ( posOpp - posAgent ).getDirection() ;
01157     if( fabs( angOpp - ang ) < 60 &&
01158         posAgent.getDistanceTo( posOpp ) < dDist )
01159       return true;
01160     else if( fabs( angOpp - ang ) < 120 &&
01161              posAgent.getDistanceTo( posOpp ) < dDist/2.0 )
01162       return true;
01163   }
01164   iterateObjectDone( iIndex );
01165   return false;
01166 }
01167 
01174 Time WorldModel::getTimeFromConfidence( double dConf )
01175 {
01176   return getCurrentTime()-(int)((1.00-dConf)*100);
01177 }
01178 
01183 ObjectT WorldModel::getLastOpponentDefender( double *dX )
01184 {
01185   double  dHighestX = 0.0;
01186   double  dSecondX  = 0.0, x;
01187 
01188   ObjectT o, oLast = OBJECT_ILLEGAL, oSecondLast = OBJECT_ILLEGAL;
01189   for( int i = 0; i < MAX_OPPONENTS ; i ++ )
01190   {
01191     o = Opponents[i].getType();
01192     if( isConfidenceGood( o ) )
01193     {
01194       x = Opponents[i].getGlobalPosition().getX();
01195       if( x > dHighestX )         // if larger x than highest
01196       {
01197         dSecondX    = dHighestX;  // make second the previous highest
01198         dHighestX   = x;          // and this the new one
01199         oSecondLast = oLast;
01200         oLast       = o;
01201       }
01202       else if( x > dSecondX )     // if smaller than 1st  and larger than 2nd
01203       {
01204         dSecondX    = x;          // make it the second
01205         oSecondLast = o;
01206       }
01207     }
01208   }
01209   
01210   // if highest x is outside pen_area, it cannot be the goalie (unless playing
01211   // Portugal ;-) ), so assume goalie is just not seen
01212   if( dHighestX < PENALTY_X && getOppGoalieType() == OBJECT_ILLEGAL )
01213   {
01214     dSecondX    = dHighestX;
01215     oSecondLast = oLast;
01216   }
01217   if( dX != NULL )
01218     *dX = dSecondX ;
01219   return oSecondLast;
01220 }
01221 
01230 double WorldModel::getOffsideX( bool bIncludeComm )
01231 {
01232   double  x, dAgentX;
01233 
01234   getLastOpponentDefender( &dAgentX );
01235   x = getBallPos().getX();
01236   x = max( x, dAgentX );
01237   if( bIncludeComm == true && getCurrentTime() - m_timeCommOffsideX < 3 )
01238     x = max( x, m_dCommOffsideX );
01239   return x ;
01240 }
01241 
01256 VecPosition WorldModel::getOuterPositionInField( VecPosition pos, AngDeg ang,
01257                                   double dDist, bool bWithPenalty )
01258 {
01259   VecPosition posShoot;
01260 
01261   // make shooting line using position and desired direction
01262   Line lineObj     = Line::makeLineFromPositionAndAngle( pos, ang );
01263 
01264   // get intersection point between the created line and goal line
01265   Line lineLength  = Line::makeLineFromPositionAndAngle(
01266                             VecPosition( PITCH_LENGTH/2.0 - dDist, 0.0 ), 90 );
01267   posShoot         = lineObj.getIntersection( lineLength );
01268 
01269   // check whether it first crosses the penalty line
01270   Line linePenalty = Line::makeLineFromPositionAndAngle(
01271                             VecPosition( PENALTY_X - dDist, 0.0 ), 90.0 );
01272   double dPenaltyY = lineObj.getIntersection(linePenalty).getY();
01273 
01274   if( bWithPenalty && fabs(dPenaltyY) < PENALTY_AREA_WIDTH/2.0 )
01275   {
01276     if( fabs(dPenaltyY) < PENALTY_AREA_WIDTH/2.0 - 5.0 ||   // crosses inside
01277         fabs(posShoot.getY()) <  PENALTY_AREA_WIDTH/2.0 )   // or ends inside
01278       posShoot = lineObj.getIntersection( linePenalty );
01279   }
01280 
01281   // check where it crosses the side line
01282   Line lineSide = ( ang < 0 )
01283      ? Line::makeLineFromPositionAndAngle(
01284                            VecPosition( 0.0, - PITCH_WIDTH/2.0 + dDist ),0.0 )
01285      : Line::makeLineFromPositionAndAngle(
01286                            VecPosition( 0.0, + PITCH_WIDTH/2.0 - dDist ),0.0 );
01287 
01288   if( fabs(posShoot.getY()) > PITCH_WIDTH/2.0 - dDist )
01289     posShoot = lineObj.getIntersection( lineSide );
01290 
01291   return posShoot;
01292 }
01293 
01305 AngDeg WorldModel::getDirectionOfWidestAngle(VecPosition posOrg, AngDeg angMin,
01306                                AngDeg angMax, AngDeg *angLargest, double dDist)
01307 {
01308   list<double> v;
01309   list<double> v2;
01310   double       temp;
01311   int          iIndex;
01312   double       dConf  = PS->getPlayerConfThr();
01313 
01314   // add all angles of all the opponents to the list v
01315   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS, dConf );
01316        o != OBJECT_ILLEGAL;
01317        o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS, dConf ) )
01318   {
01319     if( getRelativeDistance( o ) < dDist )
01320       v.push_back( (getGlobalPosition(o)-posOrg).getDirection());
01321   }
01322   iterateObjectDone( iIndex );
01323   v.sort();
01324 
01325   // if goalkeeper is spotted and he is located within the range that we want
01326   // to shoot at, make sure the angle with the goalkeeper is large enough, since
01327   // he has better intercepting capabilities than the normal players
01328 
01329   ObjectT     objGoalie = getOppGoalieType();
01330   VecPosition posGoalie = getGlobalPosition( objGoalie );
01331   AngDeg      angGoalie;
01332 
01333   if( objGoalie != OBJECT_ILLEGAL && posOrg.getX() > PITCH_LENGTH/4.0 &&
01334       posOrg.getDistanceTo( posGoalie ) < dDist )
01335   {
01336     angGoalie = ( posGoalie - posOrg ).getDirection();
01337     Log.log( 560, "direction_widest_angle: min %f max %f angGoalie %f",
01338                                                   angMin, angMax, angGoalie );
01339 
01340     if( posOrg.getY() > 0 ) // right side of the field
01341     {
01342       angGoalie = VecPosition::normalizeAngle( angGoalie - 33 );
01343       angMax    = max( angMin, min( angGoalie, angMax ) );
01344     }
01345     else
01346     {
01347       angGoalie = VecPosition::normalizeAngle( angGoalie + 33 );
01348       angMin    = min( angMax, max( angMin, angGoalie ) );
01349     }
01350     Log.log( 560, "direction_widest_angle after: %f %f", angMin, angMax );
01351   }
01352 
01353 
01354   // Create new list with only opponents from interval [angMin..angMax].
01355   // Note that opponents outside angMin and angMax can have an influence
01356   // on the largest angle between the opponents, so they should be accounted
01357   // for. To this end, a projection is defined in both angMin and angMax.
01358   // The opponent with the smallest global angle difference a to angMin
01359   // (either inside or outside the interval [angMin..angMax]) is determined
01360   // and an extra angle angMin - a is added to the list. The situation for
01361   // angMax is analogous.
01362 
01363   double absMin     = 1000;
01364   double absMax     = 1000;
01365   double angProjMin = angMin;
01366   double angProjMax = angMax;
01367   double array[MAX_OPPONENTS+2];
01368 
01369   while( v.size() > 0 )
01370   {
01371     if( fabs( v.front() - angMin ) < absMin )      // opp near angMin
01372     {
01373       absMin     = fabs( v.front() - angMin ) ;    // make angMin wider
01374       angProjMin = angMin - absMin;                // to take him into account
01375     }
01376     if( fabs( v.front() - angMax ) < absMax )      // opp near angMax
01377     {
01378       absMax     = fabs( v.front() - angMax ) ;    // make angMax wider
01379       angProjMax = angMax + absMax;                // to take him into account
01380     }
01381     if( v.front() > angMin && v.front() < angMax ) // opp in range
01382       v2.push_back( v.front() );                   // add him
01383     v.pop_front();
01384   }
01385 
01386   // make all angles relative to angProjMin which has angle 0 and set them in
01387   // the range 0..360, where the range -180..0 is moved to 180..360. Do this by
01388   // adding 360 and then subtracting 360 if value is larger than 360.
01389   v.push_back( 0 );
01390   while( v2.size() > 0 )  // for all the opponents
01391   {
01392     temp = VecPosition::normalizeAngle(v2.front()-angProjMin)+360.0;
01393     if( temp > 360 )
01394       temp -= 360;
01395     v.push_back( temp );
01396     v2.pop_front();
01397   }
01398   // add max projection.
01399   temp = VecPosition::normalizeAngle(angProjMax-angProjMin)+360.0;
01400   if( temp > 360 )
01401     temp -= 360;
01402 
01403   v.push_back( temp );
01404 
01405   // sort the list
01406   v.sort();
01407 
01408   // put all the values in an array
01409   int i = 0;
01410   while( v.size() > 0 )
01411   {
01412     array[i++] = v.front();
01413     v.pop_front();
01414   }
01415 
01416   // find the largest angle and determine the associated midpoint direction
01417   double dLargest = -1000;
01418   double d;
01419   double ang      = UnknownAngleValue;
01420   for( int j = 0; j < i - 1 ; j ++ )
01421   {
01422     d = VecPosition::normalizeAngle(( array[j+1] - array[j] )/2.0);
01423     if( d > dLargest )
01424     {
01425       ang = angProjMin + array[j] + d;
01426       ang = VecPosition::normalizeAngle( ang );
01427       dLargest = d;
01428     }
01429   }
01430 
01431   if( ang == UnknownAngleValue ) // no angle found -> get angle in between
01432   {
01433     ang = getBisectorTwoAngles( angMin, angMax );
01434     if( angLargest != NULL )
01435       *angLargest = 360;
01436   }
01437   else if( angLargest != NULL )
01438     *angLargest = dLargest;
01439 
01440   return ang;
01441 }
01442 
01444 bool WorldModel::isInField( VecPosition pos, double dMargin )
01445 {
01446   return Rect( 
01447              VecPosition( + PITCH_LENGTH/2.0 - dMargin, 
01448                           - PITCH_WIDTH/2.0  + dMargin ),
01449              VecPosition( - PITCH_LENGTH/2.0 + dMargin, 
01450                           + PITCH_WIDTH/2.0  - dMargin ) 
01451              ).isInside( pos );
01452 }
01453 
01455 bool WorldModel::isBeforeGoal( VecPosition pos )
01456 {
01457   return Rect(
01458              VecPosition( + PENALTY_X - 2,    - ( SS->getGoalWidth()/2.0 + 1)),
01459              VecPosition( + PITCH_LENGTH/2.0, + ( SS->getGoalWidth()/2.0 + 1))
01460              ).isInside( pos );
01461 }
01462 
01473 VecPosition WorldModel::getStrategicPosition( ObjectT obj, FormationT ft )
01474 {
01475   return getStrategicPosition( SoccerTypes::getIndex( obj ), ft );
01476 }
01477   
01490 VecPosition WorldModel::getStrategicPosition( int iPlayer, FormationT ft )
01491 {
01492   if( iPlayer > MAX_TEAMMATES )
01493     cerr << "WM:getStrategicPosition with player nr " << iPlayer << endl;
01494 
01495   VecPosition pos, posBall = getBallPos();
01496   bool bOwnBall = isBallInOurPossesion();
01497 
01498   // -1 is default -> get player number in formation
01499   if( iPlayer == -1 )
01500     iPlayer = formations->getPlayerInFormation();
01501 
01502   // get maximal allowed x coordinate, this is offside x coordinate
01503   double dMaxX = max( -0.5, getOffsideX() - 1.5 );
01504 
01505   if( bOwnBall && 
01506       getGlobalPosition(
01507         SoccerTypes::getTeammateObjectFromIndex(iPlayer)).getX() 
01508       < posBall.getX() )
01509     dMaxX = max( dMaxX, posBall.getX()  );
01510 
01511   // after standing offside we are not allowed to move for ball
01512   // with a goal kick of them we are not allowed to move into penalty area
01513 
01514   if( isGoalKickThem() )
01515     dMaxX = min( dMaxX, PENALTY_X - 1.0 );
01516   else if( isBeforeKickOff() )
01517     dMaxX = min( dMaxX, -2.0 );
01518   else if ( isOffsideUs() )
01519     dMaxX = posBall.getX() - 0.5;
01520 
01521   // change the ball position on which strategic position is based
01522   // depending on the different deadball situation and thus the
01523   // expected movement of the ball
01524   if( isBeforeKickOff() )
01525     posBall.setVecPosition( 0, 0 );
01526   else if( isGoalKickUs() ||
01527       getTimeSinceLastCatch(  ) < PS->getCyclesCatchWait() + 5  ||
01528       ( isFreeKickUs() && posBall.getX() < - PENALTY_X ) )
01529     posBall.setX( -PITCH_LENGTH/4 + 5.0 );
01530   else if( getConfidence( OBJECT_BALL ) < PS->getBallConfThr() )
01531     posBall.setVecPosition( 0.0, 0.0 );
01532   else if( isGoalKickThem() ||
01533            ( isFreeKickThem() && posBall.getX() > PENALTY_X ) )
01534     posBall.setX( PENALTY_X - 10.0 );
01535   else if( isFreeKickThem() )
01536     posBall.setX( posBall.getX() - 5.0 );
01537   else if( isBallInOurPossesion() && 
01538            !( isDeadBallUs() || isDeadBallThem() ) )
01539     posBall.setX( posBall.getX() + 5.0 );
01540   else if( posBall.getX() < - PENALTY_X + 5.0 )
01541     posBall = predictPosAfterNrCycles( OBJECT_BALL, 3 );
01542 
01543   // get the strategic position
01544   pos = formations->getStrategicPosition( iPlayer, posBall, dMaxX,
01545                           bOwnBall, PS->getMaxYPercentage(),
01546                                           ft );
01547   return pos;
01548 }
01549 
01570 VecPosition WorldModel::getMarkingPosition( VecPosition pos, double dDist, 
01571                                             MarkT mark)
01572 {
01573   VecPosition posBall  = getBallPos();
01574     //edictPosAfterNrCycles( OBJECT_BALL, 3 );
01575   VecPosition posGoal  = getPosOwnGoal( );
01576   if( posBall.getX() < - PITCH_LENGTH/2.0 + 10.0 )
01577     posGoal.setX( posBall.getX() + 1  );
01578   else if( posBall.getX() > -PITCH_LENGTH/3.0 )
01579   {
01580     posGoal.setX( -PITCH_LENGTH/2.0 );
01581     double dY  = posBall.getY();
01582     if( fabs( dY ) > 12 )
01583       dY += ( sign( dY ) > 0   ) ? -5 : 5 ;
01584     posGoal.setY( dY );
01585   }
01586       
01587   VecPosition posAgent = getAgentGlobalPosition();
01588   VecPosition posMark;
01589   AngDeg      ang, angToGoal, angToBall;
01590 
01591   if( mark == MARK_GOAL )                       // position in direction goal
01592   {   
01593     angToGoal = (posGoal-pos).getDirection( );    
01594     Line line = Line::makeLineFromTwoPoints( pos, posGoal );
01595 
01596     // we want to know when distance from ball to point p equals distance
01597     // from opp to point p :
01598     // d1 + d3 = sqrt(d2^2 + d3^2) > (d1+d3)^2 = d2^2 + d3^2 =>
01599     // d1^2 + 2*d1*d3 = d2^2 -> d3 = (d2^2 - d1^2 ) / 2*d1
01600     double dCalcDist;
01601     VecPosition posIntersect = line.getPointOnLineClosestTo( posAgent );
01602     double dDistAgent = posIntersect.getDistanceTo( posAgent );
01603     double dDistOpp = posIntersect.getDistanceTo( pos );
01604     dCalcDist = (dDistAgent*dDistAgent-dDistOpp*dDistOpp)/(2*dDistOpp);
01605     double dExtra = 2.0;
01606     //    if( posBall.getX() <  PENALTY_X + 5 )
01607     if( pos.getDistanceTo(posAgent) < 5 )
01608       dExtra = 0.0;
01609     dCalcDist += dDistOpp + dExtra;
01610     Log.log( 513, "dDistOpp %f dDistAgent %f calc %f min %f",
01611              dDistOpp, dDistAgent, dCalcDist, 0.75*pos.getDistanceTo(posGoal));
01612     dCalcDist = min( dCalcDist, 0.75*pos.getDistanceTo( posGoal ) );
01613     double x = -PITCH_LENGTH/2 + 4;
01614     double y = line.getYGivenX( x);
01615     posMark = pos + VecPosition( dCalcDist, angToGoal, POLAR );
01616     if( posMark.getX() < x )
01617       {
01618         Log.log( 513, "change posmark to (%f,%f)", x, y );
01619         posMark.setVecPosition( x, y );
01620       }
01621     // if interception point iss outside range or very close to marking
01622     // point, but far away from opp (is this possible?) move closer.
01623     if( ! line.isInBetween( posMark, pos, posGoal ) ||
01624         ( posMark.getDistanceTo( posAgent ) < 1.5 &&
01625           posMark.getDistanceTo( pos ) > 2*dDist ) )
01626     {
01627       Log.log( 513, "set marking position at dDist %f", min(dDistAgent,7.0) );
01628       posMark   = pos + VecPosition( min( dDistAgent, 7.0 ), angToGoal, POLAR );
01629     }
01630     Log.log( 513, "marking position calc (%f,%f) pos(%f,%f) calcdist %f", 
01631              posMark.getX(), posMark.getY(), pos.getX(), pos.getY(), 
01632              dCalcDist );
01633   }
01634   else if( mark == MARK_BALL )                  // position in direction ball
01635   {
01636     angToBall = (posBall-pos).getDirection( );
01637     posMark   = pos + VecPosition( dDist, angToBall, POLAR );
01638   }
01639   else if( mark == MARK_BISECTOR )             // pos between ball and goal
01640   {
01641     angToBall = (posBall - pos).getDirection( );
01642     angToGoal = (posGoal - pos).getDirection( );
01643     ang       = getBisectorTwoAngles( angToBall, angToGoal );
01644     posMark   = pos + VecPosition( dDist, ang ,POLAR );
01645   }
01646   if( fabs( posMark.getX() ) > PITCH_LENGTH/2.0 - 2.0 )
01647     posMark.setX( sign(posMark.getX())*(PITCH_LENGTH/2.0 - 2.0) );
01648   return posMark;
01649 
01650 }
01651 
01652 
01664 double WorldModel::getActualKickPowerRate( )
01665 {
01666  // true indicates that relative angle to body should be returned
01667  double dir_diff      = fabs( getRelativeAngle( OBJECT_BALL, true ) );
01668  double dist          = getRelativeDistance( OBJECT_BALL ) -
01669                         SS->getPlayerSize( ) - SS->getBallSize( );
01670  return SS->getKickPowerRate() *
01671           ( 1 - 0.25 * dir_diff/180.0 - 0.25 * dist / SS->getKickableMargin());
01672 }
01673 
01688 double WorldModel::getKickPowerForSpeed( double dDesiredSpeed )
01689 {
01690   // acceleration after kick is calculated by power * eff_kick_power_rate
01691   // so actual kick power is acceleration / eff_kick_power_rate
01692   return dDesiredSpeed / getActualKickPowerRate( );
01693 }
01694 
01695 
01702 double WorldModel::getKickSpeedToTravel( double dDistance, double dEndSpeed )
01703 {
01704   // if endspeed is zero we have an infinite series and return the first term
01705   // that corresponds to the distance that has to be travelled.
01706   if( dEndSpeed < 0.0001  )
01707     return Geometry::getFirstInfGeomSeries(dDistance, SS->getBallDecay() );
01708 
01709   // use geometric series to calculate number of steps and with that the
01710   // velocity to give to the ball, we start at endspeed and therefore use
01711   // the inverse of the ball decay (r).
01712   // s = a + a*r + .. a*r^n since we calculated from endspeed (a) to
01713   // firstspeed, firstspeed equals a*r^n = endspeed*r^nr_steps
01714   double dNrSteps = Geometry::getLengthGeomSeries( dEndSpeed,
01715                                   1.0/SS->getBallDecay( ), dDistance );
01716   return getFirstSpeedFromEndSpeed( dEndSpeed, (int)rint(dNrSteps) ) ;
01717 }
01718 
01719 
01727 double WorldModel::getFirstSpeedFromEndSpeed( double dEndSpeed, double dCycles,
01728                                               double dDecay  )
01729 {
01730   if( dDecay < 0 )
01731     dDecay = SS->getBallDecay();
01732 
01733   // geometric serie: s = a + a*r^1 + .. + a*r^n, now given endspeed = a*r^n ->
01734   // endspeed = firstspeed * ratio ^ length ->
01735   // firstpeed = endspeed * ( 1 / ratio ) ^ length
01736   return dEndSpeed * pow( 1 / dDecay, dCycles );
01737 }
01738 
01747 double WorldModel::getFirstSpeedFromDist( double dDist, double dCycles, double
01748                    dDecay )
01749 {
01750   if( dDecay < 0 )
01751     dDecay = SS->getBallDecay();
01752 
01753   return Geometry::getFirstGeomSeries( dDist, dDecay, dCycles);
01754 }
01755 
01762 double WorldModel::getEndSpeedFromFirstSpeed(double dFirstSpeed,double dCycles)
01763 {
01764   // geometric series: s = a + a*r^1 + .. + a*r^n, with firstspeed = a ->
01765   // endspeed = firstspeed * ratio ^ length ;
01766   return dFirstSpeed * pow( SS->getBallDecay(), dCycles );
01767 }
01768 
01776 AngDeg WorldModel::getAngleForTurn( AngDeg angDesiredAngle, double dSpeed,
01777                                     ObjectT obj )
01778 {
01779   AngDeg a = angDesiredAngle * (1.0 + getInertiaMoment( obj ) * dSpeed );
01780   if( a > SS->getMaxMoment() )
01781     return SS->getMaxMoment() ;
01782   else if ( a < SS->getMinMoment() )
01783     return SS->getMinMoment() ;
01784   else
01785     return a;
01786 }
01787 
01794 AngDeg WorldModel::getActualTurnAngle( AngDeg angTurn,double dSpeed,ObjectT o )
01795 {
01796   return angTurn / (1.0 + getInertiaMoment( o ) * dSpeed );
01797 }
01798 
01810 double WorldModel::getPowerForDash( VecPosition posRelTo, AngDeg angBody,
01811               VecPosition vel, double dEffort, int iCycles )
01812 {
01813   // the distance desired is the x-direction to the relative position we
01814   // we want to move to. If point lies far away, we dash maximal. Furthermore
01815   // we subtract the x contribution of the velocity because it is not necessary
01816   // to dash maximal.
01817   double dDist = posRelTo.rotate(-angBody).getX(); // get distance in direction
01818   if( iCycles <= 0 ) iCycles = 1;
01819   double dAcc  = getFirstSpeedFromDist(dDist,iCycles,SS->getPlayerDecay());
01820                                                    // get speed to travel now
01821   if( dAcc > SS->getPlayerSpeedMax() )             // if too far away
01822     dAcc = SS->getPlayerSpeedMax();                // set maximum speed
01823   dAcc -= vel.rotate(-angBody).getX();             // subtract current velocity
01824 
01825   // acceleration = dash_power * dash_power_rate * effort ->
01826   // dash_power = acceleration / (dash_power_rate * effort )
01827   double dDashPower = dAcc/(SS->getDashPowerRate() * dEffort );
01828   if( dDashPower > SS->getMaxPower() )
01829     return SS->getMaxPower();
01830   else if( dDashPower < SS->getMinPower() )
01831     return SS->getMinPower();
01832   else
01833     return dDashPower;
01834 }
01835 
01836 
01846 int WorldModel::getClosestPlayerInFormationTo( VecPosition pos, 
01847                                                bool bIncludeGoalie,
01848                                                ObjectT objWithout,
01849                                                PlayerSetT ps, 
01850                                                FormationT ft )
01851 {
01852   double      dDist = 1000.0;
01853   VecPosition posStrat;
01854   int         iPlayer = -1;
01855 
01856   for( int i = 0; i < MAX_TEAMMATES; i++ )
01857   {
01858     if( bIncludeGoalie == false && i == 0 )
01859       continue;
01860     else if( objWithout == SoccerTypes::getTeammateObjectFromIndex( i ) )
01861       continue;
01862     else if( !SoccerTypes::isPlayerTypeInSet( 
01863               formations->getPlayerType(i,ft), ps ))
01864       continue;
01865 
01866     posStrat = getStrategicPosition( i, ft );
01867 
01868     if( isDeadBallUs( )&&
01869          getBallPos().getX() < PITCH_LENGTH/3.0 &&
01870          i >= 9 )
01871       ;  // don't use attackers when in dead ball situation and not upfront
01872     else if( posStrat.getDistanceTo( pos ) < dDist )
01873     {
01874       dDist   = posStrat.getDistanceTo( pos );
01875       iPlayer = i;
01876     }
01877   }
01878   return iPlayer;
01879 }

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