00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00048 #include<list>
00049 #include<stdio.h>
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
00130
00131
00132
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 )
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 )
00326 {
00327 dSecondMinMag = dMinMag;
00328 secondClosestObject = closestObject;
00329 dMinMag = v.getMagnitude();
00330 closestObject = o;
00331 }
00332 else if( v.getMagnitude() < dSecondMinMag )
00333 {
00334 dSecondMinMag = v.getMagnitude();
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 )
00366 {
00367 dSecondMinMag = dMinMag;
00368 secondClosestObject = closestObject;
00369 dMinMag = d;
00370 closestObject = o;
00371 }
00372 else if( d < dSecondMinMag )
00373 {
00374 dSecondMinMag = d;
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
00499
00500
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
00550
00551
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
00653 Line l = Line::makeLineFromPositionAndAngle(getBallPos(), getBallDirection());
00654 Line l2= Line::makeLineFromTwoPoints( getPosOwnGoal(), getPosOwnGoal() +
00655 VecPosition( 0, 10 ));
00656
00657
00658 VecPosition posIntersect = l.getIntersection( l2 );
00659 if( fabs(posIntersect.getY()) > SS->getGoalWidth()/2.0 + 3.0)
00660 return false;
00661
00662
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 )
00828 {
00829 dSecondX = dHighestX;
00830 dHighestX = x;
00831 }
00832 else if( x > dSecondX )
00833 dSecondX = x;
00834 }
00835 }
00836
00837
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
00867 Line lineObj = Line::makeLineFromPositionAndAngle( pos, ang );
00868
00869
00870 Line lineLength = Line::makeLineFromPositionAndAngle(
00871 VecPosition( PITCH_LENGTH/2.0 - dDist, 0.0 ), 90 );
00872 posShoot = lineObj.getIntersection( lineLength );
00873
00874
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 ||
00882 fabs(posShoot.getY()) < PENALTY_AREA_WIDTH/2.0 )
00883 posShoot = lineObj.getIntersection( linePenalty );
00884 }
00885
00886
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
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
00931
00932
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 )
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
00959
00960
00961
00962
00963
00964
00965
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 )
00976 {
00977 absMin = fabs( v.front() - angMin ) ;
00978 angProjMin = angMin - absMin;
00979 }
00980 if( fabs( v.front() - angMax ) < absMax )
00981 {
00982 absMax = fabs( v.front() - angMax ) ;
00983 angProjMax = angMax + absMax;
00984 }
00985 if( v.front() > angMin && v.front() < angMax )
00986 v2.push_back( v.front() );
00987 v.pop_front();
00988 }
00989
00990
00991
00992
00993 v.push_back( 0 );
00994 while( v2.size() > 0 )
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
01003 temp = VecPosition::normalizeAngle(angProjMax-angProjMin)+360.0;
01004 if( temp > 360 )
01005 temp -= 360;
01006
01007 v.push_back( temp );
01008
01009
01010 v.sort();
01011
01012
01013 int i = 0;
01014 while( v.size() > 0 )
01015 {
01016 array[i++] = v.front();
01017 v.pop_front();
01018 }
01019
01020
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 )
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
01064 if( iPlayer == -1 )
01065 iPlayer = formations->getPlayerInFormation();
01066
01067
01068 double dMaxX = getOffsideX() - 3.0;
01069
01070
01071
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
01080
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
01092 pos = formations->getStrategicPosition( iPlayer, posBall, dMaxX );
01093
01094 return pos;
01095 }
01096
01097
01109 double WorldModel::getActualKickPowerRate( )
01110 {
01111
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
01136
01137 return dDesiredSpeed / getActualKickPowerRate( );
01138 }
01139
01140
01147 double WorldModel::getKickSpeedToTravel( double dDistance, double dEndSpeed )
01148 {
01149
01150
01151 if( dEndSpeed < 0.0001 )
01152 return Geometry::getFirstInfGeomSeries(dDistance, SS->getBallDecay() );
01153
01154
01155
01156
01157
01158
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
01174
01175
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
01199
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
01245
01246
01247
01248 double dAcc = posRelTo.rotate(-angBody).getX();
01249 if( dAcc > SS->getPlayerSpeedMax() )
01250 dAcc = SS->getPlayerSpeedMax();
01251 dAcc -= vel.rotate(-angBody).getX();
01252
01253
01254
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