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,2001, Jelle Kok, University of Amsterdam
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without 
00006 modification, are permitted provided that the following conditions are met:
00007 
00008 1. Redistributions of source code must retain the above copyright notice, this 
00009 list of conditions and the following disclaimer. 
00010 
00011 2. Redistributions in binary form must reproduce the above copyright notice, 
00012 this list of conditions and the following disclaimer in the documentation 
00013 and/or other materials provided with the distribution. 
00014 
00015 3. Neither the name of the University of Amsterdam nor the names of its 
00016 contributors may be used to endorse or promote products derived from this 
00017 software without specific prior written permission. 
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00020 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00021 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
00022 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
00023 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
00024 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
00025 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
00026 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
00027 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
00028 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 */
00044 #include<list>            // needed for list<double>
00045 #include<stdio.h>         // needed for printf
00046 #include "WorldModel.h"
00047 
00048 
00057 int WorldModel::getNrInSetInRectangle( ObjectSetT set,
00058                                        Rectangle *rect=NULL)
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( rect == NULL || rect->isInside( getGlobalPosition( o ) ) )
00069       iNr++;
00070   }
00071   iterateObjectDone( iIndex );
00072   return iNr;
00073 }
00074 
00080 int WorldModel::getNrInSetInCircle( ObjectSetT set, Circle c )
00081 {
00082   double dConfThr = PS->getPlayerConfThr();
00083   int    iNr      = 0;
00084   int    iIndex;
00085 
00086   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00087        o != OBJECT_ILLEGAL;
00088        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00089   {
00090     if( c.isInside( getGlobalPosition( o ) ) )
00091       iNr++;
00092   }
00093   iterateObjectDone( iIndex );
00094 
00095   return iNr;
00096 }
00097 
00109 int WorldModel::getNrInSetInCone( ObjectSetT set, double dWidth,
00110                                       VecPosition start , VecPosition end )
00111 {
00112   double      dConfThr   = PS->getPlayerConfThr();
00113   int         iNr        = 0;
00114   int         iIndex;
00115   Line        line       = Line::makeLineFromTwoPoints( start, end );
00116   VecPosition posOnLine;
00117   VecPosition posObj;
00118 
00119   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00120        o != OBJECT_ILLEGAL;
00121        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00122   {
00123     posObj    = getGlobalPosition( o );
00124     posOnLine = line.getPointOnLineClosestTo( posObj );
00125     // whether posOnLine lies in cone is checked by three constraints
00126     // - does it lie in triangle (to infinity)
00127     // - lies between start and end (and thus not behind me)
00128     // - does it lie in circle
00129     if(posOnLine.getDistanceTo(posObj) < dWidth*posOnLine.getDistanceTo(start)
00130        && line.isInBetween( posOnLine, start, end )
00131        && start.getDistanceTo( posObj ) < start.getDistanceTo( end ) )
00132         iNr++;
00133   }
00134   iterateObjectDone( iIndex );
00135   return iNr;
00136 }
00137 
00148 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, ObjectT objTarget,
00149                                         double *dDist, double dConfThr )
00150 {
00151   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00152   ObjectT     closestObject = OBJECT_ILLEGAL;
00153   double      dMinMag       = 1000.0;
00154   VecPosition v;
00155   int         iIndex;
00156 
00157   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00158        o != OBJECT_ILLEGAL;
00159        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00160   {
00161     if( o != objTarget )               // do not include target object
00162     {
00163       v = getGlobalPosition( objTarget ) - getGlobalPosition( o );
00164       if( v.getMagnitude() < dMinMag )
00165       {
00166         dMinMag       = v.getMagnitude();
00167         closestObject = o;
00168       }
00169     }
00170   }
00171 
00172   iterateObjectDone( iIndex );
00173   if( dDist != NULL )
00174     *dDist = dMinMag;
00175   return closestObject;
00176 }
00177 
00188 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, VecPosition pos,
00189                                        double *dDist,  double dConfThr )
00190 {
00191   ObjectT     closestObject   = OBJECT_ILLEGAL;
00192   double      dMinMag         = 1000.0;
00193   VecPosition v;
00194   int         iIndex;
00195 
00196   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00197   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00198        o != OBJECT_ILLEGAL;
00199        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00200   {
00201     v = pos - getGlobalPosition( o );
00202     if( v.getMagnitude() < dMinMag )
00203     {
00204       dMinMag         = v.getMagnitude();
00205       closestObject   = o;
00206     }
00207   }
00208   iterateObjectDone( iIndex );
00209   if( dDist != NULL )
00210     *dDist = dMinMag;
00211   return closestObject;
00212 }
00213 
00227 ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, Line l,
00228                               VecPosition pos1, VecPosition pos2,
00229                               double *dDistObjToLine, double *dDistPos1ToPoint)
00230 {
00231   VecPosition posObj;
00232   VecPosition posOnLine;
00233   double      dConfThr  = PS->getPlayerConfThr();
00234   ObjectT     obj       = OBJECT_ILLEGAL;
00235   double      dDist     = 1000.0;
00236   double      dMinDist  = 1000.0;
00237   double      dDistPos1 = 1000.0;
00238   int         iIndex;
00239 
00240   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00241        o != OBJECT_ILLEGAL;
00242        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00243   {
00244     posObj    = getGlobalPosition( o );
00245     posOnLine = l.getPointOnLineClosestTo( posObj );
00246     dDist     = posObj.getDistanceTo( posOnLine );
00247     if( l.isInBetween( posOnLine, pos1, pos2 ) && dDist < dMinDist )
00248     {
00249       dMinDist  = dDist;
00250       obj       = o;
00251       dDistPos1 = pos1.getDistanceTo( posOnLine );
00252     }
00253   }
00254   iterateObjectDone( iIndex );
00255   if( dDistObjToLine != NULL )
00256     *dDistObjToLine = dMinDist;
00257   if( dDistPos1ToPoint != NULL )
00258     *dDistPos1ToPoint = dDistPos1;
00259 
00260   return obj;
00261 }
00262 
00269 ObjectT WorldModel::getClosestRelativeInSet( ObjectSetT set, double *dDist )
00270 {
00271   ObjectT     closestObject = OBJECT_ILLEGAL;
00272   double      dMinMag       = 1000.0;
00273   int         iIndex;
00274 
00275   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00276        o != OBJECT_ILLEGAL;
00277        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00278   {
00279     if( getRelativeDistance( o ) < dMinMag )
00280     {
00281       dMinMag       = getRelativeDistance( o );
00282       closestObject = o;
00283     }
00284   }
00285 
00286   iterateObjectDone( iIndex );
00287   if( dDist != NULL )
00288     *dDist = dMinMag;
00289   return closestObject;
00290 }
00291 
00302 ObjectT WorldModel::getSecondClosestInSetTo ( ObjectSetT set, ObjectT obj,
00303                                               double *dDist,  double dConfThr )
00304 {
00305   VecPosition v;
00306   ObjectT     closestObject       = OBJECT_ILLEGAL;
00307   ObjectT     secondClosestObject = OBJECT_ILLEGAL;
00308   double      dMinMag             = 1000.0;
00309   double      dSecondMinMag       = 1000.0;
00310   int         iIndex;
00311 
00312   if( dConfThr == -1.0 ) dConfThr = PS->getPlayerConfThr();
00313 
00314   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00315        o != OBJECT_ILLEGAL;
00316        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00317   {
00318     if( o != obj )
00319     {
00320       v = getGlobalPosition( obj ) - getGlobalPosition( o );
00321       if( v.getMagnitude() < dMinMag )                  // closer then first
00322       {
00323         dSecondMinMag         = dMinMag;                // put first to second
00324         secondClosestObject   = closestObject;
00325         dMinMag               = v.getMagnitude();       // and this to first
00326         closestObject         = o;
00327       }
00328       else if( v.getMagnitude() < dSecondMinMag )       // between first and 2nd
00329       {
00330         dSecondMinMag         = v.getMagnitude();       // put this to second
00331         secondClosestObject   = o;
00332       }
00333     }
00334   }
00335   iterateObjectDone( iIndex );
00336   if( dDist != NULL )
00337     *dDist = dSecondMinMag;
00338   return secondClosestObject;
00339 }
00340 
00347 ObjectT WorldModel::getSecondClosestRelativeInSet( ObjectSetT set, double *dDist )
00348 {
00349   ObjectT     closestObject       = OBJECT_ILLEGAL;
00350   ObjectT     secondClosestObject = OBJECT_ILLEGAL;
00351   double      dMinMag             = 1000.0;
00352   double      dSecondMinMag       = 1000.0;
00353   double      d;
00354   int         iIndex;
00355 
00356   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00357        o != OBJECT_ILLEGAL;
00358        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00359   {
00360     d = getRelativeDistance( o );
00361     if( d < dMinMag )                                 // closer then first
00362     {
00363       dSecondMinMag         = dMinMag;                // put first to second
00364       secondClosestObject   = closestObject;
00365       dMinMag               = d;                      // and this to first
00366       closestObject         = o;
00367     }
00368     else if( d < dSecondMinMag )                      // between first and 2nd
00369     {
00370       dSecondMinMag         = d;                      // put this to second
00371       secondClosestObject   = o;
00372     }
00373   }
00374   iterateObjectDone( iIndex );
00375   if( dDist != NULL )
00376     *dDist = dSecondMinMag;
00377   return secondClosestObject;
00378 }
00379 
00380 
00391 ObjectT WorldModel::getFurthestInSetTo( ObjectSetT set, ObjectT objTarget,
00392                                         double *dDist, double dConfThr )
00393 {
00394   if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
00395 
00396   ObjectT     furthestObject = OBJECT_ILLEGAL;
00397   double      dMaxMag       = -1000.0;
00398   VecPosition v;
00399   int         iIndex;
00400 
00401   for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00402        o != OBJECT_ILLEGAL;
00403        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00404   {
00405     if( o != objTarget )
00406     {
00407       v = getGlobalPosition( objTarget ) - getGlobalPosition( o );
00408       if( v.getMagnitude() > dMaxMag )
00409       {
00410         dMaxMag        = v.getMagnitude();
00411         furthestObject = o;
00412       }
00413     }
00414   }
00415   iterateObjectDone( iIndex );
00416   if( dDist != NULL )
00417     *dDist = dMaxMag;
00418   return furthestObject;
00419 }
00420 
00427 ObjectT WorldModel::getFurthestRelativeInSet( ObjectSetT set, double *dDist  )
00428 {
00429   ObjectT     furthestObject = OBJECT_ILLEGAL;
00430   double      dMaxMag       = -1000.0;
00431   int         iIndex;
00432 
00433   for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
00434        o != OBJECT_ILLEGAL;
00435        o = iterateObjectNext ( iIndex, set, 1.0 ) )
00436   {
00437     if( getRelativeDistance( o ) > dMaxMag )
00438     {
00439       dMaxMag        = getRelativeDistance( o );
00440       furthestObject = o;
00441     }
00442   }
00443   iterateObjectDone( iIndex );
00444   if( dDist != NULL )
00445     *dDist = dMaxMag;
00446   return furthestObject;
00447 }
00448 
00449 
00459 ObjectT WorldModel::getFastestInSetTo( ObjectSetT set, ObjectT obj,
00460                                         int *iCyclesToIntercept )
00461 {
00462   double      dConfThr      = PS->getPlayerConfThr();
00463   ObjectT     fastestObject = OBJECT_ILLEGAL;
00464   int         iCycles       = -1;
00465   int         iCyclesToObj  = 100;
00466   int         iMinCycles    = 100;
00467   int         iIndex;
00468   VecPosition posObj;
00469 
00470   while( iCycles < iMinCycles && iCycles < 100)
00471   {
00472     iCycles    = iCycles + 1  ;
00473     iMinCycles = 100;
00474     posObj     = predictPosAfterNrCycles( obj, iCycles );
00475 
00476     for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00477        o != OBJECT_ILLEGAL;
00478        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00479     {
00480       if( getGlobalPosition(o).getDistanceTo(posObj)/SS->getPlayerSpeedMax()
00481               < iMinCycles )
00482       {
00483         iCyclesToObj = (int)rint(getGlobalPosition(o).getDistanceTo(posObj)
00484                              /SS->getPlayerSpeedMax());
00485         iMinCycles = iCyclesToObj;
00486         fastestObject = o;
00487       }
00488     }
00489     iterateObjectDone( iIndex );
00490   }
00491 
00492   if( iCyclesToIntercept != NULL )
00493     *iCyclesToIntercept  = iMinCycles;
00494   return fastestObject;
00495 }
00496 
00507 ObjectT WorldModel::getFastestInSetTo( ObjectSetT set, VecPosition pos,
00508                       VecPosition vel, double dDecay, int *iCyclesToIntercept)
00509 {
00510   double  dConfThr      = PS->getPlayerConfThr();
00511   ObjectT fastestObject = OBJECT_ILLEGAL;
00512   int     iCycles       = 0;
00513   int     iCyclesToObj  = 100;
00514   int     iMinCycles    = 100;
00515   int     iIndex;
00516 
00517   while( iCycles < iMinCycles && iCycles < 100)
00518   {
00519     iCycles    = iCycles + 1  ;
00520     iMinCycles = 100;
00521 
00522     for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
00523        o != OBJECT_ILLEGAL;
00524        o = iterateObjectNext ( iIndex, set, dConfThr ) )
00525     {
00526        if( getGlobalPosition(o).getDistanceTo(pos)/SS->getPlayerSpeedMax()
00527               < iMinCycles )
00528       {
00529         iCyclesToObj = (int)rint(getGlobalPosition(o).getDistanceTo(pos)
00530                              /SS->getPlayerSpeedMax() );
00531         iMinCycles    = iCyclesToObj;
00532         fastestObject = o;
00533       }
00534     }
00535     iterateObjectDone( iIndex );
00536     pos += vel;
00537     vel *= dDecay;
00538   }
00539 
00540   if( iCyclesToIntercept != NULL )
00541     *iCyclesToIntercept = iMinCycles;
00542   return fastestObject;
00543 }
00544 
00552 ObjectT WorldModel::getFirstEmptySpotInSet( ObjectSetT set )
00553 {
00554   int     iIndex;
00555 
00556   for( ObjectT o = iterateObjectStart( iIndex, set,  0.0 );
00557        o != OBJECT_ILLEGAL;
00558        o = iterateObjectNext ( iIndex, set, 0.0 ) )
00559   {
00560     if( getConfidence( o ) < PS->getPlayerConfThr() &&
00561         o != getAgentObjectType() )
00562       return o;
00563   }
00564   return OBJECT_ILLEGAL;
00565 }
00566 
00567 
00574 bool WorldModel::isVisible( ObjectT o )
00575 {
00576   Object *object = getObjectPtrFromType( o );
00577 
00578   if( object != NULL &&
00579       object->getTimeLastSeen() == getTimeLastSeeMessage() )
00580     return true;
00581 
00582   return false;
00583 }
00584 
00589 bool WorldModel::isBallKickable()
00590 {
00591   return getRelativeDistance( OBJECT_BALL ) < SS->getMaximalKickDist();
00592 }
00593 
00603 bool WorldModel::isBallCatchable()
00604 {
00605   return getTimeSinceLastCatch()            > SS->getCatchBanCycle()  &&
00606          getRelativeDistance( OBJECT_BALL ) < SS->getCatchableAreaL() &&
00607          isInOwnPenaltyArea( getBallPos() );
00608 }
00609 
00619 bool WorldModel::isBallHeadingToGoal(  )
00620 {
00621   if( !isConfidenceGood( OBJECT_BALL ) ||
00622       getBallPos().getX() >  - PENALTY_X + 5.0 )
00623     return false;
00624 
00625   // make line from ball heading and goal line
00626   Line l = Line::makeLineFromPositionAndAngle(getBallPos(), getBallDirection());
00627   Line l2= Line::makeLineFromTwoPoints( getPosOwnGoal(), getPosOwnGoal() +
00628                                                          VecPosition( 0, 10 ));
00629 
00630   // if intersection is outside goalwidth, not heading to goal
00631   VecPosition posIntersect = l.getIntersection( l2 );
00632   if( fabs(posIntersect.getY()) > SS->getGoalWidth()/2.0 + 3.0)
00633     return false;
00634 
00635   // check whether ball will be behind goal line within 20 cycles.
00636   VecPosition pos    = getBallPos();
00637   int         iCycle = 1;
00638   while( pos.getX() > - PITCH_LENGTH/2.0 && iCycle < 20)
00639   {
00640     pos = predictPosAfterNrCycles( OBJECT_BALL, iCycle );
00641     iCycle ++;
00642   }
00643 
00644   return ( iCycle == 20 ) ? false : true;
00645 }
00646 
00651 bool WorldModel::isBallInOurPossesion( )
00652 {
00653   int     iCyc;
00654   ObjectT o = getFastestInSetTo( OBJECT_SET_PLAYERS, OBJECT_BALL, &iCyc );
00655 
00656   if( o == OBJECT_ILLEGAL )
00657     return false;
00658   if( SoccerTypes::isTeammate( o ) )
00659     return true;
00660   else
00661     return false;
00662 }
00663 
00666 bool WorldModel::isBallInOwnPenaltyArea( )
00667 {
00668   return isInOwnPenaltyArea( getBallPos() );
00669 }
00670 
00675 bool WorldModel::isInOwnPenaltyArea( VecPosition pos )
00676 {
00677   ObjectT     objFlag = ( getSide() == SIDE_LEFT )
00678                               ?  OBJECT_FLAG_P_L_C
00679                               :  OBJECT_FLAG_P_R_C ;
00680   VecPosition posFlag = SoccerTypes::getGlobalPositionFlag( objFlag, getSide());
00681   if( pos.getX()         < posFlag.getX() &&
00682       fabs( pos.getY() ) < PENALTY_AREA_WIDTH/2.0 )
00683     return true;
00684 
00685   return false;
00686 }
00687 
00692 bool WorldModel::isInTheirPenaltyArea( VecPosition pos )
00693 {
00694   ObjectT     objFlag = ( getSide() == SIDE_LEFT )
00695                               ?  OBJECT_FLAG_P_R_C
00696                               :  OBJECT_FLAG_P_L_C ;
00697   VecPosition posFlag = SoccerTypes::getGlobalPositionFlag( objFlag, getSide());
00698 
00699   if ( pos.getX() > posFlag.getX() &&
00700        fabs(pos.getY()) < PENALTY_AREA_WIDTH/2.0 )
00701     return true;
00702 
00703   return false;
00704 }
00705 
00712 bool WorldModel::isConfidenceGood( ObjectT o )
00713 {
00714   return getConfidence( o ) > PS->getPlayerConfThr() &&
00715          o != getAgentObjectType();
00716 }
00717 
00724 bool WorldModel::isConfidenceVeryGood( ObjectT o )
00725 {
00726   return getConfidence( o ) > PS->getPlayerHighConfThr() &&
00727          o != getAgentObjectType();
00728 }
00729 
00733 bool WorldModel::isOnside( ObjectT obj )
00734 {
00735   return getGlobalPosition( obj ).getX() < getOffsideX() - 1.0;
00736 }
00737 
00745 bool WorldModel::isOpponentAtAngle( AngDeg ang , double dDist )
00746 {
00747   VecPosition posAgent   = getAgentGlobalPosition();
00748   VecPosition posOpp;
00749   AngDeg      angOpp;
00750   int         iIndex;
00751 
00752   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS );
00753        o != OBJECT_ILLEGAL;
00754        o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS ) )
00755   {
00756     posOpp    = getGlobalPosition( o );
00757     angOpp    = ( posOpp - posAgent ).getDirection() ;
00758     if( fabs( angOpp - ang ) < 60 &&
00759         posAgent.getDistanceTo( posOpp ) < dDist )
00760       return true;
00761     else if( fabs( angOpp - ang ) < 120 &&
00762              posAgent.getDistanceTo( posOpp ) < dDist/2.0 )
00763       return true;
00764   }
00765   iterateObjectDone( iIndex );
00766   return false;
00767 }
00768 
00775 Time WorldModel::getTimeFromConfidence( double dConf )
00776 {
00777   return getCurrentTime()-(int)((1.00-dConf)*100);
00778 }
00779 
00786 double WorldModel::getOffsideX( )
00787 {
00788   double  dHighestX = 0.0;
00789   double  dSecondX  = 0.0;
00790   double  x;
00791   ObjectT o;
00792   for( int i = 0; i < MAX_OPPONENTS ; i ++ )
00793   {
00794     o = Opponents[i].getType();
00795     if( isConfidenceGood( o ) )
00796     {
00797       x = Opponents[i].getGlobalPosition().getX();
00798       if( x > dHighestX )         // if larger x than highest
00799       {
00800         dSecondX  = dHighestX;    // make second the previous highest
00801         dHighestX = x;            // and this the new one
00802       }
00803       else if( x > dSecondX )     // if smaller than highest and larger than 2nd
00804         dSecondX = x;             // make it the second
00805     }
00806   }
00807   x = Ball.getGlobalPosition().getX();
00808   return max( x, dSecondX );
00809 }
00810 
00825 VecPosition WorldModel::getOuterPositionInField( VecPosition pos, AngDeg ang,
00826                                   double dDist, bool bWithPenalty )
00827 {
00828   VecPosition posShoot;
00829 
00830   // make shooting line using position and desired direction
00831   Line lineObj     = Line::makeLineFromPositionAndAngle( pos, ang );
00832 
00833   // get intersection point between the created line and goal line
00834   Line lineLength  = Line::makeLineFromPositionAndAngle(
00835                             VecPosition( PITCH_LENGTH/2.0 - dDist, 0.0 ), 90 );
00836   posShoot         = lineObj.getIntersection( lineLength );
00837 
00838   // check whether it first crosses the penalty line
00839   Line linePenalty = Line::makeLineFromPositionAndAngle(
00840                             VecPosition( PENALTY_X - dDist, 0.0 ), 90.0 );
00841   double dPenaltyY = lineObj.getIntersection(linePenalty).getY();
00842 
00843   if( bWithPenalty && fabs(dPenaltyY) < PENALTY_AREA_WIDTH/2.0 )
00844   {
00845     if( fabs(dPenaltyY) < PENALTY_AREA_WIDTH/2.0 - 5.0 ||   // crosses inside
00846         fabs(posShoot.getY()) <  PENALTY_AREA_WIDTH/2.0 )   // or ends inside
00847       posShoot = lineObj.getIntersection( linePenalty );
00848   }
00849 
00850   // check where it crosses the side line
00851   Line lineSide = ( ang < 0 )
00852      ? Line::makeLineFromPositionAndAngle(
00853                            VecPosition( 0.0, - PITCH_WIDTH/2.0 + dDist ), 0.0 )
00854      : Line::makeLineFromPositionAndAngle(
00855                            VecPosition( 0.0, + PITCH_WIDTH/2.0 - dDist ), 0.0 );
00856 
00857   if( fabs(posShoot.getY()) > PITCH_WIDTH/2.0 - dDist )
00858     posShoot = lineObj.getIntersection( lineSide );
00859 
00860   return posShoot;
00861 }
00862 
00874 AngDeg WorldModel::getDirectionOfWidestAngle(VecPosition posOrg, AngDeg angMin,
00875                                AngDeg angMax, AngDeg *angLargest, double dDist)
00876 {
00877   list<double> v;
00878   list<double> v2;
00879   double       temp;
00880   int          iIndex;
00881   double       dConf  = PS->getPlayerConfThr();
00882 
00883   // add all angles of all the opponents to the list v
00884   for( ObjectT o = iterateObjectStart( iIndex, OBJECT_SET_OPPONENTS, dConf );
00885        o != OBJECT_ILLEGAL;
00886        o = iterateObjectNext ( iIndex, OBJECT_SET_OPPONENTS, dConf ) )
00887   {
00888     if( getRelativeDistance( o ) < dDist )
00889       v.push_back( (getGlobalPosition(o)-posOrg).getDirection());
00890   }
00891   iterateObjectDone( iIndex );
00892   v.sort();
00893 
00894   // Create new list with only opponents from interval [angMin..angMax].
00895   // Note that opponents outside angMin and angMax can have an influence
00896   // on the largest angle between the opponents, so they should be accounted
00897   // for. To this end, a projection is defined in both angMin and angMax.
00898   // The opponent with the smallest global angle difference a to angMin
00899   // (either inside or outside the interval [angMin..angMax]) is determined
00900   // and an extra angle angMin - a is added to the list. The situation for
00901   // angMax is analogous.
00902 
00903   double absMin     = 1000;
00904   double absMax     = 1000;
00905   double angProjMin = angMin;
00906   double angProjMax = angMax;
00907   double array[MAX_OPPONENTS+2];
00908 
00909   while( v.size() > 0 )
00910   {
00911     if( fabs( v.front() - angMin ) < absMin )      // opp near angMin
00912     {
00913       absMin     = fabs( v.front() - angMin ) ;    // make angMin wider
00914       angProjMin = angMin - absMin;                // to take him into account
00915     }
00916     if( fabs( v.front() - angMax ) < absMax )      // opp near angMax
00917     {
00918       absMax     = fabs( v.front() - angMax ) ;    // make angMax wider
00919       angProjMax = angMax + absMax;                // to take him into account
00920     }
00921     if( v.front() > angMin && v.front() < angMax ) // opp in range
00922       v2.push_back( v.front() );                   // add him
00923     v.pop_front();
00924   }
00925 
00926   // make all angles relative to angProjMin which has angle 0 and set them in
00927   // the range 0..360, where the range -180..0 is moved to 180..360. Do this by
00928   // adding 360 and then subtracting 360 if value is larger than 360.
00929   v.push_back( 0 );
00930   while( v2.size() > 0 )  // for all the opponents
00931   {
00932     temp = VecPosition::normalizeAngle(v2.front()-angProjMin)+360.0;
00933     if( temp > 360 )
00934       temp -= 360;
00935     v.push_back( temp );
00936     v2.pop_front();
00937   }
00938   // add max projection.
00939   temp = VecPosition::normalizeAngle(angProjMax-angProjMin)+360.0;
00940   if( temp > 360 )
00941     temp -= 360;
00942 
00943   v.push_back( temp );
00944 
00945   // sort the list
00946   v.sort();
00947 
00948   // put all the values in an array
00949   int i = 0;
00950   while( v.size() > 0 )
00951   {
00952     array[i++] = v.front();
00953     v.pop_front();
00954   }
00955 
00956   // find the largest angle and determine the associated midpoint direction
00957   double dLargest = -1000;
00958   double d;
00959   double ang      = UnknownAngleValue;
00960   for( int j = 0; j < i - 1 ; j ++ )
00961   {
00962     d = VecPosition::normalizeAngle(( array[j+1] - array[j] )/2.0);
00963     if( d > dLargest )
00964     {
00965       ang = angProjMin + array[j] + d;
00966       ang = VecPosition::normalizeAngle( ang );
00967       dLargest = d;
00968     }
00969   }
00970 
00971   if( ang == UnknownAngleValue ) // no angle found -> get angle in between
00972   {
00973     ang = getBisectorTwoAngles( angMin, angMax );
00974     if( angLargest != NULL )
00975       *angLargest = 360;
00976   }
00977   else if( angLargest != NULL )
00978     *angLargest = dLargest;
00979 
00980   return ang;
00981 }
00982 
00994 double WorldModel::getActualKickPowerRate( )
00995 {
00996  // true indicates that relative angle to body should be returned
00997  double dir_diff      = fabs( getRelativeAngle( OBJECT_BALL, true ) );
00998  double dist          = getRelativeDistance( OBJECT_BALL ) -
00999                         SS->getPlayerSize( ) - SS->getBallSize( );
01000  return SS->getKickPowerRate() *
01001            ( 1 - 0.25 * dir_diff/180.0 - 0.25 * dist / SS->getKickableMargin());
01002 }
01003 
01018 double WorldModel::getKickPowerForSpeed( double dDesiredSpeed )
01019 {
01020   // acceleration after kick is calculated by power * eff_kick_power_rate
01021   // so actual kick power is acceleration / eff_kick_power_rate
01022   return dDesiredSpeed / getActualKickPowerRate( );
01023 }
01024 
01025 
01032 double WorldModel::getKickSpeedToTravel( double dDistance, double dEndSpeed )
01033 {
01034   // if endspeed is zero we have an infinite series and return the first term
01035   // that corresponds to the distance that has to be travelled.
01036   if( dEndSpeed < 0.0001  )
01037     return Geometry::getFirstInfGeomSeries(dDistance, SS->getBallDecay() );
01038 
01039   // use geometric series to calculate number of steps and with that the
01040   // velocity to give to the ball, we start at endspeed and therefore use
01041   // the inverse of the ball decay (r).
01042   // s = a + a*r + .. a*r^n since we calculated from endspeed (a) to
01043   // firstspeed, firstspeed equals a*r^n = endspeed*r^nr_steps
01044   double dNrSteps = Geometry::getLengthGeomSeries( dEndSpeed,
01045                                   1.0/SS->getBallDecay( ), dDistance );
01046   return getFirstSpeedFromEndSpeed( dEndSpeed, (int)rint(dNrSteps) ) ;
01047 }
01048 
01056 double WorldModel::getFirstSpeedFromEndSpeed( double dEndSpeed, double dCycles )
01057 {
01058   // geometric series: s = a + a*r^1 + .. + a*r^n, now given endspeed = a*r^n ->
01059   // endspeed = firstspeed * ratio ^ length ->
01060   // firstpeed = endspeed * ( 1 / ratio ) ^ length
01061   return dEndSpeed * pow( 1 / SS->getBallDecay(), dCycles );
01062 }
01063 
01070 double WorldModel::getFirstSpeedFromDist( double dDist, double dCycles )
01071 {
01072   return Geometry::getFirstGeomSeries( dDist, SS->getBallDecay(), dCycles);
01073 }
01074 
01081 double WorldModel::getEndSpeedFromFirstSpeed(double dFirstSpeed, double dCycles)
01082 {
01083   // geometric series: s = a + a*r^1 + .. + a*r^n, with firstspeed = a ->
01084   // endspeed = firstspeed * ratio ^ length ;
01085   return dFirstSpeed * pow( SS->getBallDecay(), dCycles );
01086 }
01087 
01094 AngDeg WorldModel::getAngleForTurn( AngDeg angDesiredAngle, double dSpeed )
01095 {
01096   AngDeg a = angDesiredAngle * (1.0 + SS->getInertiaMoment() * dSpeed );
01097   if( a > SS->getMaxMoment() )
01098     return SS->getMaxMoment() ;
01099   else if ( a < SS->getMinMoment() )
01100     return SS->getMinMoment() ;
01101   else
01102     return a;
01103 }
01104 
01111 AngDeg WorldModel::getActualTurnAngle( AngDeg angTurn, double dSpeed )
01112 {
01113   return angTurn / (1.0 + SS->getInertiaMoment() * dSpeed );
01114 }
01115 
01126 double WorldModel::getPowerForDash( VecPosition posRelTo, AngDeg angBody,
01127               VecPosition vel, double dEffort )
01128 {
01129   // the acceleration desired is the x-direction to the relative position we
01130   // we want to move to. If point lies far away, we dash maximal. Furthermore
01131   // we subtract the x contribution of the velocity because it is not necessary
01132   // to dash maximal.
01133   double dAcc = posRelTo.rotate(-angBody).getX();  // get distance in direction
01134   if( dAcc > SS->getPlayerSpeedMax() )             // if too far away
01135     dAcc = SS->getPlayerSpeedMax();                // set maximum speed
01136   dAcc -= vel.rotate(-angBody).getX();             // subtract current velocity
01137 
01138   // acceleration = dash_power * dash_power_rate * effort ->
01139   // dash_power = acceleration / (dash_power_rate * effort )
01140   double dDashPower = dAcc/(SS->getDashPowerRate() * dEffort );
01141   if( dDashPower > SS->getMaxPower() )
01142     return SS->getMaxPower();
01143   else if( dDashPower < SS->getMinPower() )
01144     return SS->getMinPower();
01145   else
01146     return dDashPower;
01147 }
01148 

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