Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/BehaviorControl/CommonXabsl2Symbols/BallSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file BallSymbols.cpp
    00003 *
    00004 * Implementation of class BallSymbols.
    00005 *
    00006 * @author Martin Lötzsch
    00007 */
    00008
    00009 #include "BallSymbols.h"
    00010 #include "Tools/Math/Geometry.h"
    00011 #include "Tools/FieldDimensions.h"
    00012 #include "Modules/BallLocator/GT2005BallLocatorParameters.h"
    00013
    00014 BallSymbols::BallSymbols(const BehaviorControlInterfaces& interfaces) :
    00015 BehaviorControlInterfaces(interfaces),
    00016 playBallPrecisely(false),
    00017 ballPrecision(0),
    00018 ballCanBeIntercepted(false),
    00019 interceptionAlreadyCalculated(false){
    00020 }
    00021
    00022 void BallSymbols::registerSymbols(Xabsl2Engine& engine)
    00023 {
    00024   #define regDoubleFct( name, object) \
    00025     engine.registerDecimalInputSymbol( name ,this, \
    00026     (double (Xabsl2FunctionProvider::*)()) object );
    00027   #define regDoubleSym( name, object) \
    00028     engine.registerDecimalInputSymbol( name , object );
    00029   #define regBoolFct( name, object) \
    00030     engine.registerBooleanInputSymbol( name ,this, \
    00031     (bool (Xabsl2FunctionProvider::*)()) object );
    00032   #define regBoolSym( name, object) \
    00033     engine.registerBooleanInputSymbol( name , object );
    00034   #define regEnumOutSymbol( name, object) \
    00035     engine.registerEnumeratedOutputSymbol( name , object );
    00036   #define regEnumOutSymbolElement( nameObject , nameEnum , number ) \
    00037     engine.registerEnumeratedOutputSymbolEnumElement( nameObject ,  nameEnum , number);
    00038
    00039   //Bool's:
    00040   regBoolFct("ball.ball-can-be-intercepted", &BallSymbols::getBallCanBeIntercepted);
    00041   regBoolSym("ball.ball-was-seen", &ballModel.ballWasSeen);
    00042   regBoolFct("ball.just-seen", &BallSymbols::getJustSeen);
    00043   regBoolSym("ball.play-ball-precisely", &playBallPrecisely);
    00044   regBoolSym("ball.rolls-by-left", &(ballModel.seen.ballRollsByLeft));
    00045   regBoolSym("ball.rolls-by-right", &(ballModel.seen.ballRollsByRight));
    00046   regBoolSym("ball.rolls-far-by-left", &(ballModel.seen.ballRollsFarByLeft));
    00047   regBoolSym("ball.rolls-far-by-right", &(ballModel.seen.ballRollsFarByRight));
    00048   regBoolSym("ball.rolls-fast", &(ballModel.seen.ballRollsFast));
    00049   regBoolSym("ball.rolls-towards-robot", &(ballModel.seen.ballRollsTowardsRobot));
    00050   regBoolFct("ball.seen.ball-in-front-of-opponent-goal", &BallSymbols::getBallSeenInFrontOfOpponentGoal);
    00051
    00052   //Double's:
    00053   regDoubleFct("ball.angle-to-opponent-goal", &BallSymbols::getAngleToOpponentGoal);
    00054   regDoubleSym("ball.average-distance", &averageDistance);
    00055   regDoubleFct("ball.consecutively-seen-time", &BallSymbols::getConsecutivelySeenTime);
    00056   regDoubleFct("ball.distance-to-use-seen-max", &BallSymbols::getMaxDistanceToUseSeen);
    00057   regDoubleFct("ball.hypothetical.angle", &BallSymbols::getHypotheticalAngle);
    00058   regDoubleFct("ball.hypothetical.distance", &BallSymbols::getHypotheticalDistance);
    00059   regDoubleSym("ball.hypothetical.x", &(ballModel.hypothetical.positionField.x));
    00060   regDoubleSym("ball.hypothetical.y",&(ballModel.hypothetical.positionField.y));
    00061   regDoubleFct("ball.interception-point.x", &BallSymbols::getBallInterceptionPointX);
    00062   regDoubleFct("ball.interception-point.y", &BallSymbols::getBallInterceptionPointY);
    00063   regDoubleFct("ball.just-seen", &BallSymbols::getJustSeen);
    00064   regDoubleFct("ball.known-or-near-relative-speed.x", &BallSymbols::getKnownOrNearRelativeSpeedX);
    00065   regDoubleFct("ball.known-or-near-relative-speed.y", &BallSymbols::getKnownOrNearRelativeSpeedY);
    00066   regDoubleFct("ball.known-or-near-speed.x", &BallSymbols::getKnownOrNearSpeedX);
    00067   regDoubleFct("ball.known-or-near-speed.y", &BallSymbols::getKnownOrNearRelativeSpeedY);
    00068   regDoubleFct("ball.known-or-near-speed", &BallSymbols::getKnownOrNearSpeedAbs);
    00069   regDoubleFct("ball.known-or-near.angle-to-own-goal", &BallSymbols::getKnownOrNearAngleToOwnGoal);
    00070   regDoubleFct("ball.known-or-near.angle", &BallSymbols::getKnownOrNearAngle);
    00071   regDoubleFct("ball.known-or-near.distance-to-own-penalty-area", &BallSymbols::getKnownOrNearDistanceToOwnPenaltyArea);
    00072   regDoubleFct("ball.known-or-near.distance", &BallSymbols::getKnownOrNearDistance);
    00073   regDoubleFct("ball.known-or-near.probability", &BallSymbols::getKnownOrNearProbability);
    00074   regDoubleFct("ball.known-or-near.relative-x", &BallSymbols::getKnownOrNearRelativeX);
    00075   regDoubleFct("ball.known-or-near.relative-y", &BallSymbols::getKnownOrNearRelativeY);
    00076   regDoubleFct("ball.known-or-near.x", &BallSymbols::getKnownOrNearX);
    00077   regDoubleFct("ball.known-or-near.y", &BallSymbols::getKnownOrNearY);
    00078   regDoubleFct("ball.known.angle", &BallSymbols::getKnownAngle);
    00079   regDoubleFct("ball.known.distance", &BallSymbols::getKnownDistance);
    00080   regDoubleFct("ball.known.x", &BallSymbols::getKnownX);
    00081   regDoubleFct("ball.known.y", &BallSymbols::getKnownY);
    00082   regDoubleFct("ball.number-of-images-with-ball-percept", &BallSymbols::getNumberOfImagesWithBallPercept);
    00083   regDoubleFct("ball.number-of-images-without-ball-percept", &BallSymbols::getNumberOfImagesWithoutBallPercept);
    00084   regDoubleFct("ball.positionValidity", &BallSymbols::getPositionValidity);
    00085   regDoubleFct("ball.projected-distance-on-y-axis", &BallSymbols::getProjectedDistanceOnYAxis);
    00086   regDoubleFct("ball.propagated.angle", &BallSymbols::getPropagatedAngle);
    00087   regDoubleFct("ball.propagated.distance", &BallSymbols::getPropagatedDistance);
    00088   regDoubleFct("ball.propagated.position.probability", &BallSymbols::getPropagatedPositionValidity);
    00089   regDoubleFct("ball.propagated.relative-speed.x", &BallSymbols::getPropagatedRelativeSpeedX);
    00090   regDoubleFct("ball.propagated.relative-speed.y", &BallSymbols::getPropagatedRelativeSpeedY);
    00091   regDoubleFct("ball.propagated.relative-x", &BallSymbols::getPropagatedRelativeX);
    00092   regDoubleFct("ball.propagated.relative-y", &BallSymbols::getPropagatedRelativeY);
    00093   regDoubleFct("ball.propagated.speed.probability", &BallSymbols::getPropagatedVelocityValidity);
    00094   regDoubleFct("ball.propagated.speed.x", &BallSymbols::getPropagatedSpeedX);
    00095   regDoubleFct("ball.propagated.speed.y", &BallSymbols::getPropagatedSpeedY);
    00096   regDoubleFct("ball.propagated.x", &BallSymbols::getPropagatedX);
    00097   regDoubleFct("ball.propagated.y", &BallSymbols::getPropagatedY);
    00098   regDoubleFct("ball.seen.angle", &BallSymbols::getSeenAngle);
    00099   regDoubleFct("ball.seen.distance-to-border", &BallSymbols::getSeenDistanceToBorder);
    00100   regDoubleFct("ball.seen.distance-to-own-penalty-area", &BallSymbols::getDistanceSeenBallToOwnPenaltyArea);
    00101   regDoubleFct("ball.seen.distance-x", &BallSymbols::getSeenDistanceX);
    00102   regDoubleFct("ball.seen.distance-y", &BallSymbols::getSeenDistanceY);
    00103   regDoubleFct("ball.seen.distance", &BallSymbols::getSeenDistance);
    00104   regDoubleFct("ball.seen.position.probability", &BallSymbols::getSeenPositionValidity);
    00105   regDoubleFct("ball.seen.relative-speed.x", &BallSymbols::getSeenRelativeSpeedX);
    00106   regDoubleFct("ball.seen.relative-speed.y", &BallSymbols::getSeenRelativeSpeedY);
    00107   regDoubleFct("ball.seen.relative-x", &BallSymbols::getSeenRelativeX);
    00108   regDoubleFct("ball.seen.relative-y", &BallSymbols::getSeenRelativeY);
    00109   regDoubleFct("ball.seen.speed.probability", &BallSymbols::getSeenVelocityValidity);
    00110   regDoubleFct("ball.seen.speed.x", &BallSymbols::getSeenSpeedX);
    00111   regDoubleFct("ball.seen.speed.y", &BallSymbols::getSeenSpeedY);
    00112   regDoubleFct("ball.seen.speed", &BallSymbols::getSeenSpeed);
    00113   regDoubleSym("ball.seen.x", &(ballModel.seen.positionField.x));
    00114   regDoubleSym("ball.seen.y", &(ballModel.seen.positionField.y));
    00115   regDoubleFct("ball.speedValidity", &BallSymbols::getSpeedValidity);
    00116   regDoubleFct("ball.time-after-which-communicated-balls-are-accepted", &BallSymbols::getTimeAfterWhichCommunicatedBallAreAccepted);
    00117   regDoubleFct("ball.time-after-which-propagated-are-used", &BallSymbols::getTimeAfterWhichPropagatedAreUsed);
    00118   regDoubleFct("ball.time-since-last-known", &BallSymbols::getTimeSinceLastKnown);
    00119   regDoubleFct("ball.time-since-last-seen-consecutively", &BallSymbols::getTimeSinceLastSeenConsecutively);
    00120   regDoubleFct("ball.time-since-last-seen", &BallSymbols::getTimeSinceLastSeen);
    00121   regDoubleFct("ball.time-until-ball-crosses-y-axis", &BallSymbols::getTimeUntilBallCrossesYAxis);
    00122
    00123   //Enum's:
    00124   regEnumOutSymbol("ball.precision", &ballPrecision);
    00125   regEnumOutSymbolElement("ball.precision", "precision.penalty-shooter", 1);
    00126   regEnumOutSymbolElement("ball.precision", "precision.player", 0);
    00127
    00128   #undef regDoubleFct
    00129   #undef regDoubleSym
    00130   #undef regBoolFct
    00131   #undef regBoolSym
    00132   #undef regEnumOutSymbol
    00133   #undef regEnumOutSymbolElement
    00134 }
    00135
    00136 void BallSymbols::update()
    00137 {
    00138   ballDistanceRingBuffer.add(
    00139     (int)Geometry::distanceTo(robotPose.getPose(), ballModel.seen.positionField) );
    00140
    00141   if(ballDistanceRingBuffer.getNumberOfEntries() != 0)
    00142   {
    00143     averageDistance = ballDistanceRingBuffer.getSum() / ballDistanceRingBuffer.getNumberOfEntries();
    00144   }
    00145   else averageDistance = 1000;
    00146
    00147   NDECLARE_DEBUGDRAWING("behavior:symbols:averageDistanceToBall", "drawingOnField", "an orange bar shows the average distance to the ball");
    00148   Pose2D leftPoint = robotPose + Vector2<double>(averageDistance, 500);
    00149   Pose2D rightPoint = robotPose + Vector2<double>(averageDistance, -500);
    00150   NCIRCLE("behavior:symbols:averageDistanceToBall",
    00151     (int)robotPose.translation.x, (int)robotPose.translation.y, averageDistance,
    00152     50, Drawings::ps_dot, Drawings::orange);
    00153
    00154   playBallPrecisely = (ballPrecision == 0 ? false : true);
    00155
    00156   interceptionAlreadyCalculated = false;
    00157   ballCanBeIntercepted = false;
    00158 }
    00159
    00160
    00161 double BallSymbols::getKnownDistance()
    00162 {
    00163   return ballModel.getKnownBallState().positionRobot.abs();
    00164 }
    00165
    00166 double BallSymbols::getSeenDistance()
    00167 {
    00168   return ballModel.seen.positionRobot.abs();
    00169 }
    00170
    00171 double BallSymbols::getSeenDistanceX()
    00172 {
    00173   return ballModel.seen.positionRobot.x;
    00174 }
    00175
    00176 double BallSymbols::getSeenDistanceY()
    00177 {
    00178   return ballModel.seen.positionRobot.y;
    00179 }
    00180
    00181 double BallSymbols::getKnownAngle()
    00182 {
    00183   return toDegrees(
    00184    Geometry::angleTo(robotPose.getPose(),
    00185                      ballModel.getKnownPosition(
    00186                         BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted)));
    00187 }
    00188
    00189 double BallSymbols::getTimeAfterWhichCommunicatedBallAreAccepted()
    00190 {
    00191   return (double)BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted;
    00192 }
    00193
    00194 double BallSymbols::getSeenAngle()
    00195 {
    00196   return toDegrees(Geometry::angleTo(robotPose.getPose(),
    00197     ballModel.seen.positionField));
    00198 }
    00199
    00200 double BallSymbols::getSeenSpeed()
    00201 {
    00202   return ballModel.seen.speedField.abs();
    00203 }
    00204
    00205 double BallSymbols::getSeenRelativeSpeedX()
    00206 {
    00207   return ballModel.seen.speedRobot.x;
    00208 }
    00209
    00210 double BallSymbols::getSeenRelativeSpeedY()
    00211 {
    00212   return ballModel.seen.speedRobot.y;
    00213 }
    00214
    00215 double BallSymbols::getDistanceSeenBallToOwnPenaltyArea()
    00216 {
    00217   return FieldDimensions::distanceToOwnPenaltyArea(ballModel.seen.positionField);
    00218 }
    00219
    00220 double BallSymbols::getTimeUntilBallCrossesYAxis()
    00221 {
    00222   return (double)ballModel.seen.timeTillBallCrossesYAxis;
    00223 }
    00224
    00225 double BallSymbols::getProjectedDistanceOnYAxis()
    00226 {
    00227   return ballModel.seen.projectedDistanceOnYAxis;
    00228 }
    00229
    00230 double BallSymbols::getKnownX()
    00231 {
    00232   return ballModel.getKnownPosition(
    00233     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted).x;
    00234 }
    00235
    00236 double BallSymbols::getKnownY()
    00237 {
    00238   return ballModel.getKnownPosition(
    00239     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted).y;
    00240 }
    00241
    00242 double BallSymbols::getHypotheticalDistance()
    00243 {
    00244   return ballModel.hypothetical.positionRobot.abs();
    00245 }
    00246
    00247 double BallSymbols::getHypotheticalAngle()
    00248 {
    00249   return toDegrees(Geometry::angleTo(robotPose.getPose(),
    00250     ballModel.hypothetical.positionField) + robotPose.rotation);
    00251 }
    00252
    00253 double BallSymbols::getPositionValidity()
    00254 {
    00255   return ballModel.seen.getPositionValidity();
    00256 }
    00257
    00258 double BallSymbols::getSpeedValidity()
    00259 {
    00260   return ballModel.seen.getSpeedValidity();
    00261 }
    00262
    00263 double BallSymbols::getTimeSinceLastKnown()
    00264 {
    00265   return ballModel.getTimeSinceLastKnown(
    00266     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
    00267 }
    00268
    00269 double BallSymbols::getTimeSinceLastSeen()
    00270 {
    00271   return SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen);
    00272 }
    00273
    00274 double BallSymbols::getConsecutivelySeenTime()
    00275 {
    00276   return ballModel.seen.timeUntilSeenConsecutively
    00277     - ballModel.seen.timeWhenFirstSeenConsecutively;
    00278 }
    00279
    00280 double BallSymbols::getTimeSinceLastSeenConsecutively()
    00281 {
    00282   return SystemCall::getTimeSince(ballModel.seen.timeUntilSeenConsecutively);
    00283 }
    00284
    00285 bool BallSymbols::getBallSeenInFrontOfOpponentGoal(){
    00286   //return ballModel.seen.ballInFrontOfOpponentGoal;
    00287   return ballModel.getKnownBallState().ballInFrontOfOpponentGoal;
    00288 }
    00289
    00290 bool BallSymbols::getJustSeen()
    00291 {
    00292   return SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 150;
    00293 }
    00294
    00295 double BallSymbols::getAngleToOpponentGoal()
    00296 {
    00297   //return toDegrees(normalize(Geometry::angleTo(ballModel.seen.positionField,
    00298   //                                             Vector2<double>(xPosOpponentGroundline,
    00299   //                                                             yPosCenterGoal))));
    00300   return toDegrees(normalize(Geometry::angleTo(ballModel.getKnownBallState().positionField,
    00301                                                Vector2<double>(xPosOpponentGroundline,
    00302                                                                yPosCenterGoal))));
    00303 }
    00304
    00305 double BallSymbols::getSeenDistanceToBorder()
    00306 {
    00307   return FieldDimensions::distanceToBorder(ballModel.seen.positionField);
    00308 }
    00309
    00310 double BallSymbols::getNumberOfImagesWithBallPercept()
    00311 {
    00312   return ballModel.numberOfImagesWithBallPercept;
    00313 }
    00314
    00315 double BallSymbols::getNumberOfImagesWithoutBallPercept()
    00316 {
    00317   return ballModel.numberOfImagesWithoutBallPercept;
    00318 }
    00319
    00320 double BallSymbols::getSeenPositionValidity()
    00321 {
    00322   return ballModel.seen.positionProb;
    00323   //return ballModel.getKnownBallState().positionProb;
    00324 }
    00325
    00326 double BallSymbols::getSeenVelocityValidity()
    00327 {
    00328   return ballModel.seen.velocityProb;
    00329   //return ballModel.getKnownBallState().velocityProb;
    00330 }
    00331
    00332 double BallSymbols::getPropagatedPositionValidity()
    00333 {
    00334   return ballModel.propagated.positionProb;
    00335 }
    00336
    00337 double BallSymbols::getPropagatedVelocityValidity()
    00338 {
    00339   return ballModel.propagated.velocityProb;
    00340 }
    00341
    00342 double BallSymbols::getPropagatedAngle()
    00343 {
    00344   return toDegrees(Geometry::angleTo(robotPose.getPose(),
    00345                                      ballModel.propagated.positionField));
    00346 }
    00347
    00348 double BallSymbols::getPropagatedDistance()
    00349 {
    00350   return ballModel.propagated.positionRobot.abs();
    00351 }
    00352
    00353 double BallSymbols::getPropagatedX()
    00354 {
    00355   return ballModel.propagated.positionField.x;
    00356 }
    00357
    00358 double BallSymbols::getPropagatedY()
    00359 {
    00360   return ballModel.propagated.positionField.y;
    00361 }
    00362
    00363 double BallSymbols::getTimeAfterWhichPropagatedAreUsed()
    00364 {
    00365   return BallModel::timeAfterWhichPropagatedAreUsed;
    00366 }
    00367
    00368 double BallSymbols::getMaxDistanceToUseSeen()
    00369 {
    00370   return BallModel::maxDistanceToUseSeen;
    00371 }
    00372
    00373 double BallSymbols::getPropagatedSpeedX()
    00374 {
    00375   return ballModel.propagated.speedField.x;
    00376 }
    00377
    00378 double BallSymbols::getPropagatedSpeedY()
    00379 {
    00380   return ballModel.propagated.speedField.y;
    00381 }
    00382
    00383 double BallSymbols::getPropagatedRelativeX()
    00384 {
    00385   return ballModel.propagated.positionRobot.x;
    00386 }
    00387
    00388 double BallSymbols::getPropagatedRelativeY()
    00389 {
    00390   return ballModel.propagated.positionRobot.y;
    00391 }
    00392
    00393 double BallSymbols::getPropagatedRelativeSpeedX()
    00394 {
    00395   return ballModel.propagated.speedRobot.x;
    00396 }
    00397
    00398 double BallSymbols::getPropagatedRelativeSpeedY()
    00399 {
    00400   return ballModel.propagated.speedRobot.y;
    00401 }
    00402
    00403 double BallSymbols::getSeenSpeedX()
    00404 {
    00405   return ballModel.seen.speedField.x;
    00406   //return ballModel.getKnownBallState().speedField.x;
    00407 }
    00408
    00409 double BallSymbols::getSeenSpeedY()
    00410 {
    00411   return ballModel.seen.speedField.y;
    00412   //return ballModel.getKnownBallState().speedField.y;
    00413 }
    00414
    00415 double BallSymbols::getKnownOrNearDistance()
    00416 {
    00417   return ballModel.getKnownBallState().positionRobot.abs();
    00418   //return ballModel.getKnownPosition(
    00419   //  BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted,
    00420   //  BallModel::timeAfterWhichPropagatedAreUsed,
    00421   //  BallModel::maxDistanceToUseSeen).abs();
    00422 }
    00423
    00424
    00425 double BallSymbols::getKnownOrNearAngle()
    00426 {
    00427   return toDegrees(
    00428     Geometry::angleTo(robotPose.getPose(),
    00429                       ballModel.getKnownPosition(
    00430                         BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted,
    00431                         BallModel::timeAfterWhichPropagatedAreUsed,
    00432                         BallModel::maxDistanceToUseSeen,
    00433                         BallModel::minDistanceToUseSeen)));
    00434 }
    00435
    00436 double BallSymbols::getKnownOrNearX()
    00437 {
    00438   return ballModel.getKnownPosition(
    00439     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted,
    00440     BallModel::timeAfterWhichPropagatedAreUsed,
    00441     BallModel::maxDistanceToUseSeen,
    00442     BallModel::minDistanceToUseSeen).x;
    00443 }
    00444
    00445 double BallSymbols::getKnownOrNearProbability()
    00446 {
    00447   return ballModel.getKnownBallState(
    00448     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted,
    00449     BallModel::timeAfterWhichPropagatedAreUsed,
    00450     BallModel::maxDistanceToUseSeen,
    00451     BallModel::minDistanceToUseSeen).positionProb;
    00452 }
    00453
    00454 double BallSymbols::getKnownOrNearY()
    00455 {
    00456   return ballModel.getKnownPosition(
    00457     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted,
    00458     BallModel::timeAfterWhichPropagatedAreUsed,
    00459     BallModel::maxDistanceToUseSeen,
    00460     BallModel::minDistanceToUseSeen).y;
    00461 }
    00462
    00463 double BallSymbols::getKnownOrNearRelativeX()
    00464 {
    00465   return ballModel.getKnownBallState(
    00466     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted,
    00467     BallModel::timeAfterWhichPropagatedAreUsed,
    00468     BallModel::maxDistanceToUseSeen,
    00469     BallModel::minDistanceToUseSeen).positionRobot.x;
    00470 }
    00471
    00472 double BallSymbols::getKnownOrNearRelativeY()
    00473 {
    00474   return ballModel.getKnownBallState(
    00475     BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted,
    00476     BallModel::timeAfterWhichPropagatedAreUsed,
    00477     BallModel::maxDistanceToUseSeen,
    00478     BallModel::minDistanceToUseSeen).positionRobot.y;
    00479 }
    00480
    00481 double BallSymbols::getKnownOrNearSpeedX()
    00482 {
    00483   return ballModel.getKnownBallState().speedField.x;
    00484 }
    00485
    00486 double BallSymbols::getKnownOrNearSpeedY()
    00487 {
    00488   return ballModel.getKnownBallState().speedField.y;
    00489 }
    00490
    00491 double BallSymbols::getKnownOrNearRelativeSpeedX()
    00492 {
    00493   return ballModel.getKnownBallState().speedRobot.x;
    00494 }
    00495
    00496 double BallSymbols::getKnownOrNearRelativeSpeedY()
    00497 {
    00498   return ballModel.getKnownBallState().speedRobot.y;
    00499 }
    00500
    00501 double BallSymbols::getKnownOrNearSpeedAbs()
    00502 {
    00503   Vector2<double> speed(ballModel.getKnownBallState().speedField.x,
    00504     ballModel.getKnownBallState().speedField.y);
    00505   return speed.abs();
    00506 }
    00507
    00508 double BallSymbols::getSeenRelativeX()
    00509 {
    00510   return ballModel.seen.positionRobot.x;
    00511   //return ballModel.getKnownBallState().positionRobot.x;
    00512 }
    00513
    00514 double BallSymbols::getSeenRelativeY()
    00515 {
    00516   return ballModel.seen.positionRobot.y;
    00517   //return ballModel.getKnownBallState().positionRobot.y;
    00518 }
    00519
    00520 double BallSymbols::getKnownOrNearAngleToOwnGoal()
    00521 {
    00522   Pose2D goalpoint(0, xPosOwnGroundline, 0);
    00523   return toDegrees(Geometry::angleTo(goalpoint, ballModel.getKnownBallState().positionField));
    00524 }
    00525
    00526 double BallSymbols::getKnownOrNearDistanceToOwnPenaltyArea()
    00527 {
    00528   return FieldDimensions::distanceToOwnPenaltyArea(ballModel.getKnownBallState().positionField);
    00529 }
    00530
    00531 double BallSymbols::getBallInterceptionPointX()
    00532 {
    00533   if(interceptionAlreadyCalculated)
    00534     return interceptionPoint.x;
    00535   else
    00536     calculateInterceptionPoint();
    00537   return interceptionPoint.x;
    00538 }
    00539
    00540 double BallSymbols::getBallInterceptionPointY()
    00541 {
    00542   if(interceptionAlreadyCalculated)
    00543     return interceptionPoint.y;
    00544   else
    00545     calculateInterceptionPoint();
    00546   return interceptionPoint.y;
    00547 }
    00548
    00549 bool BallSymbols::getBallCanBeIntercepted()
    00550 {
    00551   if(interceptionAlreadyCalculated)
    00552     return ballCanBeIntercepted;
    00553   else
    00554     calculateInterceptionPoint();
    00555   return ballCanBeIntercepted;
    00556 }
    00557
    00558 void BallSymbols::calculateInterceptionPoint()
    00559 {
    00560   Vector2<double> ballPosition = ballModel.getKnownBallState().positionRobot;
    00561   Vector2<double> ballSpeed = ballModel.getKnownBallState().speedRobot;
    00562
    00563   //safe distance 300 mm
    00564   /*double safeDistance = 300;
    00565   ballPosition += ballSpeed.normalize() * safeDistance;*/
    00566
    00567   double speedRobot = 450;
    00568
    00569   double a = sqr(speedRobot) - sqr(ballSpeed.x) - sqr(ballSpeed.y);
    00570   double b = - 2 * (ballPosition.x * ballSpeed.x + ballPosition.y * ballSpeed.y);
    00571   double c = - (sqr(ballPosition.x) + sqr(ballPosition.y));
    00572
    00573   double squareRoot = sqrt(sqr(b) - 4 * a * c);
    00574   double t1 = (- b + squareRoot) / (2 * a);
    00575   double t2 = (- b - squareRoot) / (2 * a);
    00576
    00577   interceptionAlreadyCalculated = true;
    00578
    00579   ballCanBeIntercepted = true;
    00580
    00581   double t = 0;
    00582   if((t1 > 0) && (t2 > 0))
    00583     t = min(t1, t2);
    00584   else if((t1 < 0) && (t2 > 0))
    00585     t = t2;
    00586   else if((t2 < 0) && (t1 > 0))
    00587     t = t1;
    00588   else
    00589     ballCanBeIntercepted = false;
    00590
    00591   if(ballCanBeIntercepted)
    00592   {
    00593     double l = sqrt(ballSpeed.x * ballSpeed.x + ballSpeed.y * ballSpeed.y);
    00594     double newLength
    00595       = -(GT2005BallLocatorParameters::coefficientOfFriction * 9810 * t) + l;
    00596     double l2 = 0;
    00597     if(l != 0)
    00598       l2 = (1 / l) * newLength;
    00599     Vector2<double> ballSpeedNew = ballSpeed * l2;
    00600
    00601     interceptionPoint.x = ballPosition.x + t * ballSpeedNew.x;
    00602     interceptionPoint.y = ballPosition.y + t * ballSpeedNew.y;
    00603   }
    00604   else
    00605   {
    00606     interceptionPoint.x = ballPosition.x;
    00607     interceptionPoint.y = ballPosition.y;
    00608   }
    00609 }
    00610