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
00044 #include<list>
00045 #include<stdio.h>
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
00126
00127
00128
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 )
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 )
00322 {
00323 dSecondMinMag = dMinMag;
00324 secondClosestObject = closestObject;
00325 dMinMag = v.getMagnitude();
00326 closestObject = o;
00327 }
00328 else if( v.getMagnitude() < dSecondMinMag )
00329 {
00330 dSecondMinMag = v.getMagnitude();
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 )
00362 {
00363 dSecondMinMag = dMinMag;
00364 secondClosestObject = closestObject;
00365 dMinMag = d;
00366 closestObject = o;
00367 }
00368 else if( d < dSecondMinMag )
00369 {
00370 dSecondMinMag = d;
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
00626 Line l = Line::makeLineFromPositionAndAngle(getBallPos(), getBallDirection());
00627 Line l2= Line::makeLineFromTwoPoints( getPosOwnGoal(), getPosOwnGoal() +
00628 VecPosition( 0, 10 ));
00629
00630
00631 VecPosition posIntersect = l.getIntersection( l2 );
00632 if( fabs(posIntersect.getY()) > SS->getGoalWidth()/2.0 + 3.0)
00633 return false;
00634
00635
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 )
00799 {
00800 dSecondX = dHighestX;
00801 dHighestX = x;
00802 }
00803 else if( x > dSecondX )
00804 dSecondX = x;
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
00831 Line lineObj = Line::makeLineFromPositionAndAngle( pos, ang );
00832
00833
00834 Line lineLength = Line::makeLineFromPositionAndAngle(
00835 VecPosition( PITCH_LENGTH/2.0 - dDist, 0.0 ), 90 );
00836 posShoot = lineObj.getIntersection( lineLength );
00837
00838
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 ||
00846 fabs(posShoot.getY()) < PENALTY_AREA_WIDTH/2.0 )
00847 posShoot = lineObj.getIntersection( linePenalty );
00848 }
00849
00850
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
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
00895
00896
00897
00898
00899
00900
00901
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 )
00912 {
00913 absMin = fabs( v.front() - angMin ) ;
00914 angProjMin = angMin - absMin;
00915 }
00916 if( fabs( v.front() - angMax ) < absMax )
00917 {
00918 absMax = fabs( v.front() - angMax ) ;
00919 angProjMax = angMax + absMax;
00920 }
00921 if( v.front() > angMin && v.front() < angMax )
00922 v2.push_back( v.front() );
00923 v.pop_front();
00924 }
00925
00926
00927
00928
00929 v.push_back( 0 );
00930 while( v2.size() > 0 )
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
00939 temp = VecPosition::normalizeAngle(angProjMax-angProjMin)+360.0;
00940 if( temp > 360 )
00941 temp -= 360;
00942
00943 v.push_back( temp );
00944
00945
00946 v.sort();
00947
00948
00949 int i = 0;
00950 while( v.size() > 0 )
00951 {
00952 array[i++] = v.front();
00953 v.pop_front();
00954 }
00955
00956
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 )
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
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
01021
01022 return dDesiredSpeed / getActualKickPowerRate( );
01023 }
01024
01025
01032 double WorldModel::getKickSpeedToTravel( double dDistance, double dEndSpeed )
01033 {
01034
01035
01036 if( dEndSpeed < 0.0001 )
01037 return Geometry::getFirstInfGeomSeries(dDistance, SS->getBallDecay() );
01038
01039
01040
01041
01042
01043
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
01059
01060
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
01084
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
01130
01131
01132
01133 double dAcc = posRelTo.rotate(-angBody).getX();
01134 if( dAcc > SS->getPlayerSpeedMax() )
01135 dAcc = SS->getPlayerSpeedMax();
01136 dAcc -= vel.rotate(-angBody).getX();
01137
01138
01139
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