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

WorldModelHighLevel.C

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2000-2002, Jelle Kok, University of Amsterdam
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without 
00006 modification, are permitted provided that the following conditions are met:
00007 
00008 1. Redistributions of source code must retain the above copyright notice, this 
00009 list of conditions and the following disclaimer. 
00010 
00011 2. Redistributions in binary form must reproduce the above copyright notice, 
00012 this list of conditions and the following disclaimer in the documentation 
00013 and/or other materials provided with the distribution. 
00014 
00015 3. Neither the name of the University of Amsterdam nor the names of its 
00016 contributors may be used to endorse or promote products derived from this 
00017 software without specific prior written permission. 
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00020 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00021 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
00022 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
00023 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
00024 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
00025 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
00026 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
00027 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
00028 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 */
00048 #include<list>            // needed for list<double>
00049 #include<stdio.h>         // needed for printf
00050 #include "WorldModel.h"
00051 
00052 
00061 int WorldModel::getNrInSetInRectangle( ObjectSetT set,
00062                                        Rectangle *rect=NULL)
00063 {
00064   double dConfThr = PS->getPlayerConfThr();
00065   int    iNr      = 0;
00066   int    iIndex;
00067 
00068   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00069        o != OBJECT_ILLEGAL;
00070        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00071   {
00072     if( rect == NULL || rect->isInside( getGlobalPosition( o ) ) )
00073       iNr++;
00074   }
00075   iterateObjectDone( iIndex );
00076   return iNr;
00077 }
00078 
00084 int WorldModel::getNrInSetInCircle( ObjectSetT set, Circle c )
00085 {
00086   double dConfThr = PS->getPlayerConfThr();
00087   int    iNr      = 0;
00088   int    iIndex;
00089 
00090   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00091        o != OBJECT_ILLEGAL;
00092        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00093   {
00094     if( c.isInside( getGlobalPosition( o ) ) )
00095       iNr++;
00096   }
00097   iterateObjectDone( iIndex );
00098 
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 
00152 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, ObjectT objTarget,
00153                                         double *dDist, double dConfThr )
00154 {
00155   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00156   ObjectT     closestObject = OBJECT_ILLEGAL;
00157   double      dMinMag       = 1000.0;
00158   VecPosition v;
00159   int         iIndex;
00160 
00161   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00162        o != OBJECT_ILLEGAL;
00163        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00164   {
00165     if( o != objTarget )               // do not include target object
00166     {
00167       v = getGlobalPosition( objTarget ) - getGlobalPosition( o );
00168       if( v.getMagnitude() < dMinMag )
00169       {
00170         dMinMag       = v.getMagnitude();
00171         closestObject = o;
00172       }
00173     }
00174   }
00175 
00176   iterateObjectDone( iIndex );
00177   if( dDist != NULL )
00178     *dDist = dMinMag;
00179   return closestObject;
00180 }
00181 
00192 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, VecPosition pos,
00193                                        double *dDist,  double dConfThr )
00194 {
00195   ObjectT     closestObject   = OBJECT_ILLEGAL;
00196   double      dMinMag         = 1000.0;
00197   VecPosition v;
00198   int         iIndex;
00199 
00200   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00201   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00202        o != OBJECT_ILLEGAL;
00203        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00204   {
00205     v = pos - getGlobalPosition( o );
00206     if( v.getMagnitude() < dMinMag )
00207     {
00208       dMinMag         = v.getMagnitude();
00209       closestObject   = o;
00210     }
00211   }
00212   iterateObjectDone( iIndex );
00213   if( dDist != NULL )
00214     *dDist = dMinMag;
00215   return closestObject;
00216 }
00217 
00231 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, Line l,
00232                               VecPosition pos1, VecPosition pos2,
00233                               double *dDistObjToLine, double *dDistPos1ToPoint)
00234 {
00235   VecPosition posObj;
00236   VecPosition posOnLine;
00237   double      dConfThr  = PS->getPlayerConfThr();
00238   ObjectT     obj       = OBJECT_ILLEGAL;
00239   double      dDist     = 1000.0;
00240   double      dMinDist  = 1000.0;
00241   double      dDistPos1 = 1000.0;
00242   int         iIndex;
00243 
00244   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00245        o != OBJECT_ILLEGAL;
00246        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00247   {
00248     posObj    = getGlobalPosition( o );
00249     posOnLine = l.getPointOnLineClosestTo( posObj );
00250     dDist     = posObj.getDistanceTo( posOnLine );
00251     if( l.isInBetween( posOnLine, pos1, pos2 ) && dDist < dMinDist )
00252     {
00253       dMinDist  = dDist;
00254       obj       = o;
00255       dDistPos1 = pos1.getDistanceTo( posOnLine );
00256     }
00257   }
00258   iterateObjectDone( iIndex );
00259   if( dDistObjToLine != NULL )
00260     *dDistObjToLine = dMinDist;
00261   if( dDistPos1ToPoint != NULL )
00262     *dDistPos1ToPoint = dDistPos1;
00263 
00264   return obj;
00265 }
00266 
00273 ObjectT WorldModel::getClosestRelativeInSet( ObjectSetT set, double *dDist )
00274 {
00275   ObjectT     closestObject = OBJECT_ILLEGAL;
00276   double      dMinMag       = 1000.0;
00277   int         iIndex;
00278 
00279   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00280        o != OBJECT_ILLEGAL;
00281        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00282   {
00283     if( getRelativeDistance( o ) < dMinMag )
00284     {
00285       dMinMag       = getRelativeDistance( o );
00286       closestObject = o;
00287     }
00288   }
00289 
00290   iterateObjectDone( iIndex );
00291   if( dDist != NULL )
00292     *dDist = dMinMag;
00293   return closestObject;
00294 }
00295 
00306 ObjectT WorldModel::getSecondClosestInSetTo ( ObjectSetT set, ObjectT obj,
00307                                               double *dDist,  double dConfThr )
00308 {
00309   VecPosition v;
00310   ObjectT     closestObject       = OBJECT_ILLEGAL;
00311   ObjectT     secondClosestObject = OBJECT_ILLEGAL;
00312   double      dMinMag             = 1000.0;
00313   double      dSecondMinMag       = 1000.0;
00314   int         iIndex;
00315 
00316   if( dConfThr == -1.0 ) dConfThr = PS->getPlayerConfThr();
00317 
00318   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00319        o != OBJECT_ILLEGAL;
00320        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00321   {
00322     if( o != obj )
00323     {
00324       v = getGlobalPosition( obj ) - getGlobalPosition( o );
00325       if( v.getMagnitude() < dMinMag )                  // closer then first
00326       {
00327         dSecondMinMag         = dMinMag;                // put first to second
00328         secondClosestObject   = closestObject;
00329         dMinMag               = v.getMagnitude();       // and this to first
00330         closestObject         = o;
00331       }
00332       else if( v.getMagnitude() < dSecondMinMag )       // between first and 2nd
00333       {
00334         dSecondMinMag         = v.getMagnitude();       // put this to second
00335         secondClosestObject   = o;
00336       }
00337     }
00338   }
00339   iterateObjectDone( iIndex );
00340   if( dDist != NULL )
00341     *dDist = dSecondMinMag;
00342   return secondClosestObject;
00343 }
00344 
00351 ObjectT WorldModel::getSecondClosestRelativeInSet( ObjectSetT set, double *dDist )
00352 {
00353   ObjectT     closestObject       = OBJECT_ILLEGAL;
00354   ObjectT     secondClosestObject = OBJECT_ILLEGAL;
00355   double      dMinMag             = 1000.0;
00356   double      dSecondMinMag       = 1000.0;
00357   double      d;
00358   int         iIndex;
00359 
00360   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00361        o != OBJECT_ILLEGAL;
00362        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00363   {
00364     d = getRelativeDistance( o );
00365     if( d < dMinMag )                                 // closer then first
00366     {
00367       dSecondMinMag         = dMinMag;                // put first to second
00368       secondClosestObject   = closestObject;
00369       dMinMag               = d;                      // and this to first
00370       closestObject         = o;
00371     }
00372     else if( d < dSecondMinMag )                      // between first and 2nd
00373     {
00374       dSecondMinMag         = d;                      // put this to second
00375       secondClosestObject   = o;
00376     }
00377   }
00378   iterateObjectDone( iIndex );
00379   if( dDist != NULL )
00380     *dDist = dSecondMinMag;
00381   return secondClosestObject;
00382 }
00383 
00384 
00395 ObjectT WorldModel::getFurthestInSetTo( ObjectSetT set, ObjectT objTarget,
00396                                         double *dDist, double dConfThr )
00397 {
00398   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00399 
00400   ObjectT     furthestObject = OBJECT_ILLEGAL;
00401   double      dMaxMag       = -1000.0;
00402   VecPosition v;
00403   int         iIndex;
00404 
00405   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00406        o != OBJECT_ILLEGAL;
00407        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00408   {
00409     if( o != objTarget )
00410     {
00411       v = getGlobalPosition( objTarget ) - getGlobalPosition( o );
00412       if( v.getMagnitude() > dMaxMag )
00413       {
00414         dMaxMag        = v.getMagnitude();
00415         furthestObject = o;
00416       }
00417     }
00418   }
00419   iterateObjectDone( iIndex );
00420   if( dDist != NULL )
00421     *dDist = dMaxMag;
00422   return furthestObject;
00423 }
00424 
00431 ObjectT WorldModel::getFurthestRelativeInSet( ObjectSetT set, double *dDist  )
00432 {
00433   ObjectT     furthestObject = OBJECT_ILLEGAL;
00434   double      dMaxMag       = -1000.0;
00435   int         iIndex;
00436 
00437   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00438        o != OBJECT_ILLEGAL;
00439        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00440   {
00441     if( getRelativeDistance( o ) > dMaxMag )
00442     {
00443       dMaxMag        = getRelativeDistance( o );
00444       furthestObject = o;
00445     }
00446   }
00447   iterateObjectDone( iIndex );
00448   if( dDist != NULL )
00449     *dDist = dMaxMag;
00450   return furthestObject;
00451 }
00452 
00457 double WorldModel::getMaxTraveledDistance( ObjectT o )
00458 {
00459   return (getCurrentTime() - getTimeLastSeen( o ) )*SS->getPlayerSpeedMax();
00460 
00461 }
00462 
00472 ObjectT WorldModel::getFastestInSetTo( ObjectSetT set, ObjectT obj,
00473                                         int *iCyclesToIntercept )
00474 {
00475   double      dConfThr      = PS->getPlayerConfThr();
00476   ObjectT     fastestObject = OBJECT_ILLEGAL;
00477   int         iCycles       = -1;
00478   int         iCyclesToObj  = 100;
00479   int         iMinCycles    = 100;
00480   int         iIndex;
00481   VecPosition posObj;
00482 
00483   while( iCycles < iMinCycles && iCycles < 100)
00484   {
00485     iCycles    = iCycles + 1  ;
00486     iMinCycles = 100;
00487     posObj     = predictPosAfterNrCycles( obj, iCycles );
00488 
00489     for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00490        o != OBJECT_ILLEGAL;
00491        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00492     {
00493       if( getGlobalPosition(o).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
00494               < iMinCycles )
00495       {
00496           iCyclesToObj = (int)rint(getGlobalPosition(o).getDistanceTo(posObj)
00497                              /SS->getPlayerSpeedMax());
00498 //        iCyclesToObj = predictNrCyclesToPoint( o, posObj,
00499 //                                               PS->getPlayerWhenToTurnAngle() );
00500 //        if( iCyclesToObj < iMinCycles )
00501         {
00502           iMinCycles = iCyclesToObj;
00503           fastestObject = o;
00504         }
00505       }
00506     }
00507     iterateObjectDone( iIndex );
00508   }
00509 
00510   if( iCyclesToIntercept != NULL )
00511     *iCyclesToIntercept  = iMinCycles;
00512   return fastestObject;
00513 }
00514 
00525 ObjectT WorldModel::getFastestInSetTo( ObjectSetT set, VecPosition pos,
00526                       VecPosition vel, double dDecay, int *iCyclesToIntercept)
00527 {
00528   double  dConfThr      = PS->getPlayerConfThr();
00529   ObjectT fastestObject = OBJECT_ILLEGAL;
00530   int     iCycles       = 0;
00531   int     iCyclesToObj  = 100;
00532   int     iMinCycles    = 100;
00533   int     iIndex;
00534 
00535   while( iCycles < iMinCycles && iCycles < 100)
00536   {
00537     iCycles    = iCycles + 1  ;
00538     iMinCycles = 100;
00539 
00540     for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00541        o != OBJECT_ILLEGAL;
00542        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00543     {
00544        if( getGlobalPosition(o).getDistanceTo(pos)/SS->getPlayerSpeedMax()
00545               < iMinCycles )
00546       {
00547         iCyclesToObj = (int)rint(getGlobalPosition(o).getDistanceTo(pos)
00548                              /SS->getPlayerSpeedMax() );
00549 //        iCyclesToObj = predictNrCyclesToPoint( o, pos,
00550 //                                               PS->getPlayerWhenToTurnAngle() );
00551 //        if( iCyclesToObj < iMinCycles )
00552         {
00553           iMinCycles    = iCyclesToObj;
00554           fastestObject = o;
00555         }
00556       }
00557     }
00558     iterateObjectDone( iIndex );
00559     pos += vel;
00560     vel *= dDecay;
00561   }
00562 
00563   if( iCyclesToIntercept != NULL )
00564     *iCyclesToIntercept = iMinCycles;
00565   return fastestObject;
00566 }
00567 
00578 ObjectT WorldModel::getFirstEmptySpotInSet( ObjectSetT set, int iUnknownPlayer)
00579 {
00580   int     iIndex;
00581 
00582   for( ObjectT o = iterateObjectStart( iIndex, set,  0.0 );
00583        o != OBJECT_ILLEGAL;
00584        o = iterateObjectNext ( iIndex, set, 0.0 ) )
00585   {
00586     if( getConfidence( o ) < PS->getPlayerConfThr() &&
00587         o != getAgentObjectType() && 
00588         ( iUnknownPlayer == -1 || UnknownPlayers[iUnknownPlayer].isInRange(o)))
00589       return o;
00590   }
00591   return OBJECT_ILLEGAL;
00592 }
00593 
00594 
00601 bool WorldModel::isVisible( ObjectT o )
00602 {
00603   Object *object = getObjectPtrFromType( o );
00604 
00605   if( object != NULL &&
00606       object->getTimeLastSeen() == getTimeLastSeeMessage() )
00607     return true;
00608 
00609   return false;
00610 }
00611 
00616 bool WorldModel::isBallKickable()
00617 {
00618   return getRelativeDistance( OBJECT_BALL ) < SS->getMaximalKickDist();
00619 }
00620 
00630 bool WorldModel::isBallCatchable()
00631 {
00632   return getTimeSinceLastCatch()            > SS->getCatchBanCycle()  &&
00633          getRelativeDistance( OBJECT_BALL ) < SS->getCatchableAreaL() &&
00634          isInOwnPenaltyArea( getBallPos() );
00635 }
00636 
00646 bool WorldModel::isBallHeadingToGoal(  )
00647 {
00648   if( !isConfidenceGood( OBJECT_BALL ) ||
00649       getBallPos().getX() >  - PENALTY_X + 5.0 )
00650     return false;
00651 
00652   // make line from ball heading and goal line
00653   Line l = Line::makeLineFromPositionAndAngle(getBallPos(), getBallDirection());
00654   Line l2= Line::makeLineFromTwoPoints( getPosOwnGoal(), getPosOwnGoal() +
00655                                                          VecPosition( 0, 10 ));
00656 
00657   // if intersection is outside goalwidth, not heading to goal
00658   VecPosition posIntersect = l.getIntersection( l2 );
00659   if( fabs(posIntersect.getY()) > SS->getGoalWidth()/2.0 + 3.0)
00660     return false;
00661 
00662   // check whether ball will be behind goal line within 20 cycles.
00663   VecPosition pos    = getBallPos();
00664   int         iCycle = 1;
00665   while( pos.getX() > - PITCH_LENGTH/2.0 && iCycle < 20)
00666   {
00667     pos = predictPosAfterNrCycles( OBJECT_BALL, iCycle );
00668     iCycle ++;
00669   }
00670 
00671   return ( iCycle == 20 ) ? false : true;
00672 }
00673 
00678 bool WorldModel::isBallInOurPossesion( )
00679 {
00680   int     iCyc;
00681   ObjectT o = getFastestInSetTo( OBJECT_SET_PLAYERS, OBJECT_BALL, &iCyc );
00682 
00683   if( o == OBJECT_ILLEGAL )
00684     return false;
00685   if( SoccerTypes::isTeammate( o ) )
00686     return true;
00687   else
00688     return false;
00689 }
00690 
00693 bool WorldModel::isBallInOwnPenaltyArea( )
00694 {
00695   return isInOwnPenaltyArea( getBallPos() );
00696 }
00697 
00702 bool WorldModel::isInOwnPenaltyArea( VecPosition pos )
00703 {
00704   ObjectT     objFlag = ( getSide() == SIDE_LEFT )
00705                               ?  OBJECT_FLAG_P_L_C
00706                               :  OBJECT_FLAG_P_R_C ;
00707   VecPosition posFlag = SoccerTypes::getGlobalPositionFlag( objFlag, getSide());
00708   if( pos.getX()         < posFlag.getX() &&
00709       fabs( pos.getY() ) < PENALTY_AREA_WIDTH/2.0 )
00710     return true;
00711 
00712   return false;
00713 }
00714 
00719 bool WorldModel::isInTheirPenaltyArea( VecPosition pos )
00720 {
00721   ObjectT     objFlag = ( getSide() == SIDE_LEFT )
00722                               ?  OBJECT_FLAG_P_R_C
00723                               :  OBJECT_FLAG_P_L_C ;
00724   VecPosition posFlag = SoccerTypes::getGlobalPositionFlag( objFlag, getSide());
00725 
00726   if ( pos.getX() > posFlag.getX() &&
00727        fabs(pos.getY()) < PENALTY_AREA_WIDTH/2.0 )
00728     return true;
00729 
00730   return false;
00731 }
00732 
00739 bool WorldModel::isConfidenceGood( ObjectT o )
00740 {
00741   return getConfidence( o ) > PS->getPlayerConfThr() &&
00742          o != getAgentObjectType();
00743 }
00744 
00751 bool WorldModel::isConfidenceVeryGood( ObjectT o )
00752 {
00753   return getConfidence( o ) > PS->getPlayerHighConfThr() &&
00754          o != getAgentObjectType();
00755 }
00756 
00760 bool WorldModel::isOnside( ObjectT obj )
00761 {
00762   return getGlobalPosition( obj ).getX() < getOffsideX() - 0.5;
00763 }
00764 
00772 bool WorldModel::isOpponentAtAngle( AngDeg ang , double dDist )
00773 {
00774   VecPosition posAgent   = getAgentGlobalPosition();
00775   VecPosition posOpp;
00776   AngDeg      angOpp;
00777   int         iIndex;
00778 
00779   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS );
00780        o != OBJECT_ILLEGAL;
00781        o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS ) )
00782   {
00783     posOpp    = getGlobalPosition( o );
00784     angOpp    = ( posOpp - posAgent ).getDirection() ;
00785     if( fabs( angOpp - ang ) < 60 &&
00786         posAgent.getDistanceTo( posOpp ) < dDist )
00787       return true;
00788     else if( fabs( angOpp - ang ) < 120 &&
00789              posAgent.getDistanceTo( posOpp ) < dDist/2.0 )
00790       return true;
00791   }
00792   iterateObjectDone( iIndex );
00793   return false;
00794 }
00795 
00802 Time WorldModel::getTimeFromConfidence( double dConf )
00803 {
00804   return getCurrentTime()-(int)((1.00-dConf)*100);
00805 }
00806 
00815 double WorldModel::getOffsideX( bool bIncludeComm )
00816 {
00817   double  dHighestX = 0.0;
00818   double  dSecondX  = 0.0;
00819   double  x;
00820   ObjectT o;
00821   for( int i = 0; i < MAX_OPPONENTS ; i ++ )
00822   {
00823     o = Opponents[i].getType();
00824     if( isConfidenceVeryGood( o ) )
00825     {
00826       x = Opponents[i].getGlobalPosition().getX();
00827       if( x > dHighestX )         // if larger x than highest
00828       {
00829         dSecondX  = dHighestX;    // make second the previous highest
00830         dHighestX = x;            // and this the new one
00831       }
00832       else if( x > dSecondX )     // if smaller than highest and larger than 2nd
00833         dSecondX = x;             // make it the second
00834     }
00835   }
00836   // if highest x is outside pen_area, it cannot be the goalie (unless playing
00837   // Portugal ;-) ), so assume goalie is just not seen
00838   if( dHighestX < PENALTY_X && getOppGoalieType() == OBJECT_ILLEGAL )
00839     dSecondX = dHighestX;
00840     
00841   x = getBallPos().getX();
00842   x = max( x, dSecondX );
00843 
00844   return x;
00845 }
00846 
00861 VecPosition WorldModel::getOuterPositionInField( VecPosition pos, AngDeg ang,
00862                                   double dDist, bool bWithPenalty )
00863 {
00864   VecPosition posShoot;
00865 
00866   // make shooting line using position and desired direction
00867   Line lineObj     = Line::makeLineFromPositionAndAngle( pos, ang );
00868 
00869   // get intersection point between the created line and goal line
00870   Line lineLength  = Line::makeLineFromPositionAndAngle(
00871                             VecPosition( PITCH_LENGTH/2.0 - dDist, 0.0 ), 90 );
00872   posShoot         = lineObj.getIntersection( lineLength );
00873 
00874   // check whether it first crosses the penalty line
00875   Line linePenalty = Line::makeLineFromPositionAndAngle(
00876                             VecPosition( PENALTY_X - dDist, 0.0 ), 90.0 );
00877   double dPenaltyY = lineObj.getIntersection(linePenalty).getY();
00878 
00879   if( bWithPenalty && fabs(dPenaltyY) < PENALTY_AREA_WIDTH/2.0 )
00880   {
00881     if( fabs(dPenaltyY) < PENALTY_AREA_WIDTH/2.0 - 5.0 ||   // crosses inside
00882         fabs(posShoot.getY()) <  PENALTY_AREA_WIDTH/2.0 )   // or ends inside
00883       posShoot = lineObj.getIntersection( linePenalty );
00884   }
00885 
00886   // check where it crosses the side line
00887   Line lineSide = ( ang < 0 )
00888      ? Line::makeLineFromPositionAndAngle(
00889                            VecPosition( 0.0, - PITCH_WIDTH/2.0 + dDist ), 0.0 )
00890      : Line::makeLineFromPositionAndAngle(
00891                            VecPosition( 0.0, + PITCH_WIDTH/2.0 - dDist ), 0.0 );
00892 
00893   if( fabs(posShoot.getY()) > PITCH_WIDTH/2.0 - dDist )
00894     posShoot = lineObj.getIntersection( lineSide );
00895 
00896   return posShoot;
00897 }
00898 
00910 AngDeg WorldModel::getDirectionOfWidestAngle(VecPosition posOrg, AngDeg angMin,
00911                                AngDeg angMax, AngDeg *angLargest, double dDist)
00912 {
00913   list<double> v;
00914   list<double> v2;
00915   double       temp;
00916   int          iIndex;
00917   double       dConf  = PS->getPlayerConfThr();
00918 
00919   // add all angles of all the opponents to the list v
00920   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS, dConf );
00921        o != OBJECT_ILLEGAL;
00922        o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS, dConf ) )
00923   {
00924     if( getRelativeDistance( o ) < dDist )
00925       v.push_back( (getGlobalPosition(o)-posOrg).getDirection());
00926   }
00927   iterateObjectDone( iIndex );
00928   v.sort();
00929 
00930   // if goalkeeper is spotted and he is located within the range that we want
00931   // to shoot at, make sure the angle with the goalkeeper is large enough, since
00932   // he has better intercepting capabilities than the normal players
00933   
00934   ObjectT     objGoalie = getOppGoalieType();
00935   VecPosition posGoalie = getGlobalPosition( objGoalie );
00936   AngDeg      angGoalie;
00937   
00938   if( objGoalie != OBJECT_ILLEGAL && posOrg.getX() > PITCH_LENGTH/4.0 &&
00939       posOrg.getDistanceTo( posGoalie ) < dDist )
00940   {
00941     angGoalie = ( posGoalie - posOrg ).getDirection();  
00942     Log.log( 560, "direction_widest_angle: min %f max %f angGoalie %f", 
00943                                                   angMin, angMax, angGoalie );    
00944     if( posOrg.getY() > 0 ) // right side of the field
00945     {
00946       angGoalie = VecPosition::normalizeAngle( angGoalie - 33 );
00947       angMax    = max( angMin, min( angGoalie, angMax ) );
00948     }
00949     else
00950     {
00951       angGoalie = VecPosition::normalizeAngle( angGoalie + 33 );
00952       angMin    = min( angMax, max( angMin, angGoalie ) );      
00953     }
00954     Log.log( 560, "direction_widest_angle after: %f %f", angMin, angMax );
00955   }
00956        
00957 
00958   // Create new list with only opponents from interval [angMin..angMax].
00959   // Note that opponents outside angMin and angMax can have an influence
00960   // on the largest angle between the opponents, so they should be accounted
00961   // for. To this end, a projection is defined in both angMin and angMax.
00962   // The opponent with the smallest global angle difference a to angMin
00963   // (either inside or outside the interval [angMin..angMax]) is determined
00964   // and an extra angle angMin - a is added to the list. The situation for
00965   // angMax is analogous.
00966 
00967   double absMin     = 1000;
00968   double absMax     = 1000;
00969   double angProjMin = angMin;
00970   double angProjMax = angMax;
00971   double array[MAX_OPPONENTS+2];
00972 
00973   while( v.size() > 0 )
00974   {
00975     if( fabs( v.front() - angMin ) < absMin )      // opp near angMin
00976     {
00977       absMin     = fabs( v.front() - angMin ) ;    // make angMin wider
00978       angProjMin = angMin - absMin;                // to take him into account
00979     }
00980     if( fabs( v.front() - angMax ) < absMax )      // opp near angMax
00981     {
00982       absMax     = fabs( v.front() - angMax ) ;    // make angMax wider
00983       angProjMax = angMax + absMax;                // to take him into account
00984     }
00985     if( v.front() > angMin && v.front() < angMax ) // opp in range
00986       v2.push_back( v.front() );                   // add him
00987     v.pop_front();
00988   }
00989 
00990   // make all angles relative to angProjMin which has angle 0 and set them in
00991   // the range 0..360, where the range -180..0 is moved to 180..360. Do this by
00992   // adding 360 and then subtracting 360 if value is larger than 360.
00993   v.push_back( 0 );
00994   while( v2.size() > 0 )  // for all the opponents
00995   {
00996     temp = VecPosition::normalizeAngle(v2.front()-angProjMin)+360.0;
00997     if( temp > 360 )
00998       temp -= 360;
00999     v.push_back( temp );
01000     v2.pop_front();
01001   }
01002   // add max projection.
01003   temp = VecPosition::normalizeAngle(angProjMax-angProjMin)+360.0;
01004   if( temp > 360 )
01005     temp -= 360;
01006 
01007   v.push_back( temp );
01008 
01009   // sort the list
01010   v.sort();
01011 
01012   // put all the values in an array
01013   int i = 0;
01014   while( v.size() > 0 )
01015   {
01016     array[i++] = v.front();
01017     v.pop_front();
01018   }
01019 
01020   // find the largest angle and determine the associated midpoint direction
01021   double dLargest = -1000;
01022   double d;
01023   double ang      = UnknownAngleValue;
01024   for( int j = 0; j < i - 1 ; j ++ )
01025   {
01026     d = VecPosition::normalizeAngle(( array[j+1] - array[j] )/2.0);
01027     if( d > dLargest )
01028     {
01029       ang = angProjMin + array[j] + d;
01030       ang = VecPosition::normalizeAngle( ang );
01031       dLargest = d;
01032     }
01033   }
01034 
01035   if( ang == UnknownAngleValue ) // no angle found -> get angle in between
01036   {
01037     ang = getBisectorTwoAngles( angMin, angMax );
01038     if( angLargest != NULL )
01039       *angLargest = 360;
01040   }
01041   else if( angLargest != NULL )
01042     *angLargest = dLargest;
01043 
01044   return ang;
01045 }
01046 
01047 
01059 VecPosition WorldModel::getStrategicPosition( int iPlayer )
01060 {
01061   VecPosition pos;
01062   
01063   // -1 is default -> get player number in formation
01064   if( iPlayer == -1 )
01065     iPlayer = formations->getPlayerInFormation();
01066 
01067   // get maximal allowed x coordinate, this is offside x coordinate
01068   double dMaxX = getOffsideX() - 3.0;
01069 
01070   // after standing offside we are not allowed to move for ball
01071   // with a goal kick of them we are not allowed to move into penalty area
01072   if( isOffsideUs( ) )
01073     dMaxX = min( dMaxX, getBallPos().getX() - 1.0 );
01074   if( isGoalKickThem() )
01075     dMaxX = min( dMaxX, PENALTY_X - 1.0 );
01076   if( isBeforeKickOff() )
01077     dMaxX = min( dMaxX, -2.0 );
01078 
01079   // change the ball position on which strategic position is based depending on
01080   // the different deadball situation and thus the expected movement of the ball
01081   VecPosition posBall = getBallPos();    
01082   if( isGoalKickUs() || 
01083       getTimeSinceLastCatch(  ) < PS->getCyclesCatchWait() + 5  )    
01084     posBall.setX( -PITCH_LENGTH/4 - 5.0 );
01085   else if( getConfidence( OBJECT_BALL ) < PS->getBallConfThr() )
01086     posBall.setVecPosition( 0.0, 0.0 );
01087   else if( isGoalKickThem() || 
01088            ( isFreeKickThem() && posBall.getX() > PENALTY_X ) )
01089     posBall.setX( PENALTY_X - 10.0 );
01090 
01091   // get the strategic position
01092   pos = formations->getStrategicPosition( iPlayer, posBall, dMaxX );
01093 
01094   return pos;
01095 }
01096 
01097 
01109 double WorldModel::getActualKickPowerRate( )
01110 {
01111  // true indicates that relative angle to body should be returned
01112  double dir_diff      = fabs( getRelativeAngle( OBJECT_BALL, true ) );
01113  double dist          = getRelativeDistance( OBJECT_BALL ) -
01114                         SS->getPlayerSize( ) - SS->getBallSize( );
01115  return SS->getKickPowerRate() *
01116            ( 1 - 0.25 * dir_diff/180.0 - 0.25 * dist / SS->getKickableMargin());
01117 }
01118 
01133 double WorldModel::getKickPowerForSpeed( double dDesiredSpeed )
01134 {
01135   // acceleration after kick is calculated by power * eff_kick_power_rate
01136   // so actual kick power is acceleration / eff_kick_power_rate
01137   return dDesiredSpeed / getActualKickPowerRate( );
01138 }
01139 
01140 
01147 double WorldModel::getKickSpeedToTravel( double dDistance, double dEndSpeed )
01148 {
01149   // if endspeed is zero we have an infinite series and return the first term
01150   // that corresponds to the distance that has to be travelled.
01151   if( dEndSpeed < 0.0001  )
01152     return Geometry::getFirstInfGeomSeries(dDistance, SS->getBallDecay() );
01153 
01154   // use geometric series to calculate number of steps and with that the
01155   // velocity to give to the ball, we start at endspeed and therefore use
01156   // the inverse of the ball decay (r).
01157   // s = a + a*r + .. a*r^n since we calculated from endspeed (a) to
01158   // firstspeed, firstspeed equals a*r^n = endspeed*r^nr_steps
01159   double dNrSteps = Geometry::getLengthGeomSeries( dEndSpeed,
01160                                   1.0/SS->getBallDecay( ), dDistance );
01161   return getFirstSpeedFromEndSpeed( dEndSpeed, (int)rint(dNrSteps) ) ;
01162 }
01163 
01171 double WorldModel::getFirstSpeedFromEndSpeed( double dEndSpeed, double dCycles )
01172 {
01173   // geometric series: s = a + a*r^1 + .. + a*r^n, now given endspeed = a*r^n ->
01174   // endspeed = firstspeed * ratio ^ length ->
01175   // firstpeed = endspeed * ( 1 / ratio ) ^ length
01176   return dEndSpeed * pow( 1 / SS->getBallDecay(), dCycles );
01177 }
01178 
01185 double WorldModel::getFirstSpeedFromDist( double dDist, double dCycles )
01186 {
01187   return Geometry::getFirstGeomSeries( dDist, SS->getBallDecay(), dCycles);
01188 }
01189 
01196 double WorldModel::getEndSpeedFromFirstSpeed(double dFirstSpeed, double dCycles)
01197 {
01198   // geometric series: s = a + a*r^1 + .. + a*r^n, with firstspeed = a ->
01199   // endspeed = firstspeed * ratio ^ length ;
01200   return dFirstSpeed * pow( SS->getBallDecay(), dCycles );
01201 }
01202 
01209 AngDeg WorldModel::getAngleForTurn( AngDeg angDesiredAngle, double dSpeed )
01210 {
01211   AngDeg a = angDesiredAngle * (1.0 + SS->getInertiaMoment() * dSpeed );
01212   if( a > SS->getMaxMoment() )
01213     return SS->getMaxMoment() ;
01214   else if ( a < SS->getMinMoment() )
01215     return SS->getMinMoment() ;
01216   else
01217     return a;
01218 }
01219 
01226 AngDeg WorldModel::getActualTurnAngle( AngDeg angTurn, double dSpeed )
01227 {
01228   return angTurn / (1.0 + SS->getInertiaMoment() * dSpeed );
01229 }
01230 
01241 double WorldModel::getPowerForDash( VecPosition posRelTo, AngDeg angBody,
01242               VecPosition vel, double dEffort )
01243 {
01244   // the acceleration desired is the x-direction to the relative position we
01245   // we want to move to. If point lies far away, we dash maximal. Furthermore
01246   // we subtract the x contribution of the velocity because it is not necessary
01247   // to dash maximal.
01248   double dAcc = posRelTo.rotate(-angBody).getX();  // get distance in direction
01249   if( dAcc > SS->getPlayerSpeedMax() )             // if too far away
01250     dAcc = SS->getPlayerSpeedMax();                // set maximum speed
01251   dAcc -= vel.rotate(-angBody).getX();             // subtract current velocity
01252 
01253   // acceleration = dash_power * dash_power_rate * effort ->
01254   // dash_power = acceleration / (dash_power_rate * effort )
01255   double dDashPower = dAcc/(SS->getDashPowerRate() * dEffort );
01256   if( dDashPower > SS->getMaxPower() )
01257     return SS->getMaxPower();
01258   else if( dDashPower < SS->getMinPower() )
01259     return SS->getMinPower();
01260   else
01261     return dDashPower;
01262 }
01263 

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