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