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

# Representations/Cognition/BallModel.cpp

Go to the documentation of this file.

    00001 //------------------------------------------------------------------------------
    00002 /**
    00003 * @file BallModel.cpp
    00004 *
    00005 * Implementation of class BallModel.
    00006 *
    00007 * @author <A href="mailto:xiang@informatik.uni-bremen.de">Hong Xiang</A>
    00008 * @author <A href="mailto:rlx@informatik.uni-bremen.de">Lang Lang</A>
    00009 * @author <A href="mailto:roefer@tzi.de">Thomas Röfer</A>
    00010 * @author <A href="mailto:jan@aiboteamhumboldt.com">Jan Hoffmann</A>
    00011 * @author <A href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</A>
    00012 */
    00013 //------------------------------------------------------------------------------
    00014 #include "BallModel.h"
    00015 #include "Platform/SystemCall.h"
    00016 #include "Tools/FieldDimensions.h"
    00017 //------------------------------------------------------------------------------
    00018 BallState::BallState()
    00019 {
    00020   reset();
    00021   useGivenProbabilities = false;
    00022 }
    00023 //------------------------------------------------------------------------------
    00024 void BallState::reset()
    00025 {
    00026   positionProb = 0;
    00027   velocityProb = 0;
    00028
    00029
    00030   positionRobot = Vector2<double>(0.0, 0.0);
    00031   speedRobot = Vector2<double>(0.0, 0.0);
    00032   positionField = Vector2<double>(0.0, 0.0);
    00033   speedField = Vector2<double>(0.0, 0.0);
    00034   robotPose = RobotPose();
    00035
    00036   ballInFrontOfOpponentGoal = false;
    00037   ballRollsByLeft = false;
    00038   ballRollsByRight = false;
    00039   ballRollsFarByLeft = false;
    00040   ballRollsFarByRight = false;
    00041   ballRollsTowardsRobot = false;
    00042   ballRollsFast = false;
    00043   projectedDistanceOnYAxis = 0.0;
    00044   timeTillBallCrossesYAxis = 1000000000;
    00045
    00046   positionGaussSet = false;
    00047   speedGaussSet = false;
    00048 }
    00049 //------------------------------------------------------------------------------
    00050 void BallState::setBallDataRelativeToRobot(
    00051                     const RobotPose& robotPose,
    00052                     double px,
    00053                     double py,
    00054                     double vx,
    00055                     double vy)
    00056 {
    00057   this->robotPose = robotPose;
    00058   positionRobot.x = px;
    00059   positionRobot.y = py;
    00060   speedRobot.x = vx;
    00061   speedRobot.y = vy;
    00062
    00063   positionField = positionVectorFromRobotToField(robotPose, positionRobot);
    00064   speedField = directionVectorFromRobotToField(robotPose, speedRobot);
    00065
    00066   setBallFlags();
    00067 }
    00068 //------------------------------------------------------------------------------
    00069 void BallState::setBallDataRelativeToField(
    00070                     const RobotPose& robotPose,
    00071                     double px,
    00072                     double py,
    00073                     double vx,
    00074                     double vy)
    00075 {
    00076   this->robotPose = robotPose;
    00077   positionField.x = px;
    00078   positionField.y = py;
    00079   speedField.x = vx;
    00080   speedField.y = vy;
    00081
    00082   positionRobot = positionVectorFromFieldToRobot(robotPose, positionField);
    00083   speedRobot = directionVectorFromFieldToRobot(robotPose, speedField);
    00084
    00085   setBallFlags();
    00086 }
    00087 //------------------------------------------------------------------------------
    00088 void BallState::setPositionGaussianDataRelativeToRobot(
    00089                      const RobotPose& robotPose,
    00090                      double orientation,
    00091                      double majorAxis,
    00092                      double minorAxis)
    00093 {
    00094   positionGaussSet = true;
    00095   positionGaussOrientationRobot = orientation;
    00096   positionGaussOrientationField =
    00097     orientationFromRobotToField(robotPose, orientation);
    00098   positionGaussMajAxis = majorAxis;
    00099   positionGaussMinAxis = minorAxis;
    00100 }
    00101 //------------------------------------------------------------------------------
    00102 void BallState::setPositionGaussianDataRelativeToField(
    00103                      const RobotPose& robotPose,
    00104                      double orientation,
    00105                      double majorAxis,
    00106                      double minorAxis)
    00107 {
    00108   positionGaussSet = true;
    00109   positionGaussOrientationField = orientation;
    00110   positionGaussOrientationRobot =
    00111     orientationFromFieldToRobot(robotPose, orientation);
    00112   positionGaussMajAxis = majorAxis;
    00113   positionGaussMinAxis = minorAxis;
    00114 }
    00115 //------------------------------------------------------------------------------
    00116 void BallState::setSpeedGaussianDataRelativeToRobot(
    00117                      const RobotPose& robotPose,
    00118                      double orientation,
    00119                      double majorAxis,
    00120                      double minorAxis)
    00121 {
    00122   speedGaussSet = true;
    00123   speedGaussOrientationRobot = orientation;
    00124   speedGaussOrientationField =
    00125     orientationFromRobotToField(robotPose, orientation);
    00126   speedGaussMajAxis = majorAxis;
    00127   speedGaussMinAxis = minorAxis;
    00128 }
    00129 //------------------------------------------------------------------------------
    00130 void BallState::setSpeedGaussianDataRelativeToField(
    00131                      const RobotPose& robotPose,
    00132                      double orientation,
    00133                      double majorAxis,
    00134                      double minorAxis)
    00135 {
    00136   speedGaussSet = true;
    00137   speedGaussOrientationField = orientation;
    00138   speedGaussOrientationRobot =
    00139     orientationFromFieldToRobot(robotPose, orientation);
    00140   speedGaussMajAxis = majorAxis;
    00141   speedGaussMinAxis = minorAxis;
    00142 }
    00143 //------------------------------------------------------------------------------
    00144 double BallState::getPositionValidity() const
    00145 {
    00146   if (useGivenProbabilities)
    00147     return positionProb;
    00148
    00149   if (!positionGaussSet)
    00150     return 1.0;
    00151   return max(0.0, 1.0 - (positionGaussMajAxis + positionGaussMinAxis)/1000.0);
    00152 }
    00153 //------------------------------------------------------------------------------
    00154 double BallState::getSpeedValidity() const
    00155 {
    00156   if (useGivenProbabilities)
    00157     return velocityProb;
    00158
    00159   if (!speedGaussSet)
    00160     return 1.0;
    00161   return max(0.0, 1.0 - (speedGaussMajAxis + speedGaussMinAxis)/1000.0);
    00162 }
    00163 //------------------------------------------------------------------------------
    00164 void BallState::drawPosition(Drawings::Color color) const
    00165 {
    00166   CIRCLE(ballLocatorField, positionField.x, positionField.y,
    00167          100, 20, Drawings::ps_solid, color);
    00168 }
    00169 //------------------------------------------------------------------------------
    00170 void BallState::drawSpeed(Drawings::Color color) const
    00171 {
    00172   ARROW(ballLocatorField, positionField.x, positionField.y,
    00173         positionField.x + speedField.x, positionField.y + speedField.y,
    00174         10, Drawings::ps_solid, color);
    00175 }
    00176 //------------------------------------------------------------------------------
    00177 void BallState::drawPositionGaussian(Drawings::Color color) const
    00178 {
    00179   if (!positionGaussSet)
    00180     return;
    00181
    00182   double c = cos(positionGaussOrientationField);
    00183   double s = sin(positionGaussOrientationField);
    00184   double x1 = positionGaussMajAxis*c;
    00185   double y1 = positionGaussMajAxis*s;
    00186   double x2 = -positionGaussMinAxis*s;
    00187   double y2 = positionGaussMinAxis*c;
    00188
    00189   LINE(ballLocatorField,
    00190        positionField.x-x1, positionField.y-y1,
    00191        positionField.x+x1, positionField.y+y1,
    00192        20, Drawings::ps_solid, color);
    00193
    00194   LINE(ballLocatorField,
    00195        positionField.x-x2, positionField.y-y2,
    00196        positionField.x+x2, positionField.y+y2,
    00197        20, Drawings::ps_solid, color);
    00198
    00199 }
    00200 //------------------------------------------------------------------------------
    00201 void BallState::drawSpeedGaussian(Drawings::Color color) const
    00202 {
    00203 }
    00204 //------------------------------------------------------------------------------
    00205 void BallState::setBallFlags()
    00206 {
    00207   // Reset flags
    00208   ballInFrontOfOpponentGoal = false;
    00209   ballRollsByLeft = false;
    00210   ballRollsByRight = false;
    00211   ballRollsFarByLeft = false;
    00212   ballRollsFarByRight = false;
    00213   ballRollsTowardsRobot = false;
    00214   ballRollsFast = false;
    00215   projectedDistanceOnYAxis = 0.0;
    00216   timeTillBallCrossesYAxis = 1000000000;
    00217
    00218   // Check if ball the line from robot to ball crosses the goal line
    00219   if ((robotPose.translation.x < xPosOpponentGroundline) &&
    00220       (robotPose.translation.x < positionField.x))
    00221   {
    00222     double p1x = xPosOpponentGoal;
    00223     double p1y = yPosRightGoal;
    00224     double p2y = yPosLeftGoal;
    00225     double bx = positionField.x;
    00226     double by = positionField.y;
    00227     double rx = robotPose.translation.x;
    00228     double ry = robotPose.translation.y;
    00229     double lambda = (p1x*by - p1x*ry + bx*ry - p1y*bx + p1y*rx - rx*by) /
    00230                     ((bx - rx)*(p1y - p2y));
    00231     ballInFrontOfOpponentGoal = ((lambda > 0.0) && (lambda < 1.0));
    00232   }
    00233
    00234   // Ball is considered fast if speed is higher than 400.0mm/s
    00235   if (speedRobot.abs() > 400.0)
    00236     ballRollsFast = true;
    00237
    00238   // Ball must be approaching faster than 50 millimeters per second
    00239   if (speedRobot.x > -50.0)
    00240     return;
    00241
    00242   // Calculate y-position (side) of ball when the ball has the same height
    00243   // as the robot und set appropriate ball state
    00244   //timeTillBallCrossesYAxis = (long)(-speedRobot.y/speedRobot.x * 1000.0);
    00245   timeTillBallCrossesYAxis = (long)(-positionRobot.x / speedRobot.x);
    00246   projectedDistanceOnYAxis =
    00247     positionRobot.y - (speedRobot.y/speedRobot.x)*positionRobot.x;
    00248
    00249   // Ball must be nearer than 1 meter
    00250   double dist = positionRobot.abs();
    00251   if (dist > 1000.0)
    00252     return;
    00253
    00254   if ((projectedDistanceOnYAxis < -150.0) &&
    00255       (projectedDistanceOnYAxis > -500.0))
    00256   {
    00257     ballRollsByRight = true;
    00258   }
    00259   else if ((projectedDistanceOnYAxis > 150.0) &&
    00260            (projectedDistanceOnYAxis < 500.0))
    00261   {
    00262     ballRollsByLeft = true;
    00263   }
    00264   else if ((projectedDistanceOnYAxis >= -150.0) &&
    00265            (projectedDistanceOnYAxis <= 150.0))
    00266   {
    00267     ballRollsTowardsRobot = true;
    00268   }
    00269   if ((projectedDistanceOnYAxis < -500.0) &&
    00270       (projectedDistanceOnYAxis > -1000.0))
    00271   {
    00272     ballRollsFarByRight = true;
    00273   }
    00274   else if ((projectedDistanceOnYAxis > 500.0) &&
    00275            (projectedDistanceOnYAxis < 1000.0))
    00276   {
    00277     ballRollsFarByLeft = true;
    00278   }
    00279 }
    00280 //------------------------------------------------------------------------------
    00281 Vector2<double> BallState::positionVectorFromRobotToField(
    00282                                 const RobotPose& robotPose,
    00283                                 const Vector2<double>& pv)
    00284 {
    00285   double c = robotPose.getCos();
    00286   double s = robotPose.getSin();
    00287   Vector2<double> res(pv.x*c - pv.y*s + robotPose.translation.x,
    00288                       pv.x*s + pv.y*c + robotPose.translation.y);
    00289   return res;
    00290 }
    00291 //------------------------------------------------------------------------------
    00292 Vector2<double> BallState::directionVectorFromRobotToField(
    00293                                 const RobotPose& robotPose,
    00294                                 const Vector2<double>& dv)
    00295 {
    00296   double c = robotPose.getCos();
    00297   double s = robotPose.getSin();
    00298   Vector2<double> res(dv.x*c - dv.y*s,
    00299                       dv.x*s + dv.y*c);
    00300   return res;
    00301 }
    00302 //------------------------------------------------------------------------------
    00303 double BallState::orientationFromRobotToField(
    00304                        const RobotPose& robotPose,
    00305                        double orientation)
    00306 {
    00307   return normalize(robotPose.rotation + orientation);
    00308 }
    00309 //------------------------------------------------------------------------------
    00310 Vector2<double> BallState::positionVectorFromFieldToRobot(
    00311                                 const RobotPose& robotPose,
    00312                                 const Vector2<double>& pv)
    00313 {
    00314   double c = robotPose.getCos();
    00315   double s = robotPose.getSin();
    00316   double x = robotPose.translation.x;
    00317   double y = robotPose.translation.y;
    00318   Vector2<double> res((pv.x - x)*c + (pv.y - y)*s,
    00319                       (pv.y - y)*c - (pv.x - x)*s);
    00320   return res;
    00321 }
    00322 //------------------------------------------------------------------------------
    00323 Vector2<double> BallState::directionVectorFromFieldToRobot(
    00324                                 const RobotPose& robotPose,
    00325                                 const Vector2<double>& dv)
    00326 {
    00327   double c = robotPose.getCos();
    00328   double s = robotPose.getSin();
    00329   Vector2<double> res(dv.x*c + dv.y*s,
    00330                       -dv.x*s + dv.y*c);
    00331   return res;
    00332 }
    00333 //------------------------------------------------------------------------------
    00334 double BallState::orientationFromFieldToRobot(
    00335                        const RobotPose& robotPose,
    00336                        double orientation)
    00337 {
    00338   return normalize(orientation - robotPose.rotation);
    00339 }
    00340 //------------------------------------------------------------------------------
    00341 void BallState::serialize(In* in, Out* out)
    00342 {
    00343   STREAM_REGISTER_BEGIN();
    00344   STREAM( positionRobot);
    00345   STREAM( speedRobot);
    00346   STREAM( positionField);
    00347   STREAM( speedField);
    00348   STREAM( robotPose);
    00349   STREAM( ballInFrontOfOpponentGoal);
    00350   STREAM( ballRollsByLeft);
    00351   STREAM( ballRollsByRight);
    00352   STREAM( ballRollsTowardsRobot);
    00353   STREAM( ballRollsFast);
    00354   STREAM( projectedDistanceOnYAxis);
    00355   STREAM( timeTillBallCrossesYAxis);
    00356   STREAM( positionGaussSet);
    00357   STREAM( positionGaussOrientationRobot);
    00358   STREAM( positionGaussOrientationField);
    00359   STREAM( positionGaussMajAxis);
    00360   STREAM( positionGaussMinAxis);
    00361   STREAM( speedGaussSet);
    00362   STREAM( speedGaussOrientationRobot);
    00363   STREAM( speedGaussOrientationField);
    00364   STREAM( speedGaussMajAxis);
    00365   STREAM( speedGaussMinAxis);
    00366   STREAM_REGISTER_FINISH();
    00367 }
    00368
    00369 /*Out& operator<<(Out& stream, const BallState& bs)
    00370 {
    00371   char boolHelp;
    00372
    00373   stream << bs.positionRobot << bs.speedRobot;
    00374   stream << bs.positionField << bs.speedField;
    00375   stream << bs.robotPose;
    00376
    00377   boolHelp = bs.ballInFrontOfOpponentGoal? 1: 0;
    00378   stream << boolHelp;
    00379   boolHelp = bs.ballRollsByLeft? 1: 0;
    00380   stream << boolHelp;
    00381   boolHelp = bs.ballRollsByRight? 1: 0;
    00382   stream << boolHelp;
    00383   boolHelp = bs.ballRollsTowardsRobot? 1: 0;
    00384   stream << boolHelp;
    00385   boolHelp = bs.ballRollsFast? 1: 0;
    00386   stream << boolHelp;
    00387   stream << bs.projectedDistanceOnYAxis;
    00388   stream << bs.timeTillBallCrossesYAxis;
    00389
    00390   boolHelp = bs.positionGaussSet? 1: 0;
    00391   stream << boolHelp;
    00392   stream << bs.positionGaussOrientationRobot
    00393          << bs.positionGaussOrientationField;
    00394   stream << bs.positionGaussMajAxis << bs.positionGaussMinAxis;
    00395
    00396   boolHelp = bs.speedGaussSet? 1: 0;
    00397   stream << boolHelp;
    00398   stream << bs.speedGaussOrientationRobot << bs.speedGaussOrientationField;
    00399   stream << bs.speedGaussMajAxis << bs.speedGaussMinAxis;
    00400
    00401   return stream;
    00402 }
    00403 //------------------------------------------------------------------------------
    00404 In& operator>>(In& stream, BallState& bs)
    00405 {
    00406   char boolHelp;
    00407
    00408   stream >> bs.positionRobot >> bs.speedRobot;
    00409   stream >> bs.positionField >> bs.speedField;
    00410   stream >> bs.robotPose;
    00411
    00412   stream >> boolHelp;
    00413   bs.ballInFrontOfOpponentGoal = (boolHelp == 1);
    00414   stream >> boolHelp;
    00415   bs.ballRollsByLeft = (boolHelp == 1);
    00416   stream >> boolHelp;
    00417   bs.ballRollsByRight = (boolHelp == 1);
    00418   stream >> boolHelp;
    00419   bs.ballRollsTowardsRobot = (boolHelp == 1);
    00420   stream >> boolHelp;
    00421   bs.ballRollsFast = (boolHelp == 1);
    00422   stream >> bs.projectedDistanceOnYAxis;
    00423   stream >> bs.timeTillBallCrossesYAxis;
    00424
    00425   stream >> boolHelp;
    00426   bs.positionGaussSet = (boolHelp == 1);
    00427   stream >> bs.positionGaussOrientationRobot
    00428          >> bs.positionGaussOrientationField;
    00429   stream >> bs.positionGaussMajAxis >> bs.positionGaussMinAxis;
    00430
    00431   stream >> boolHelp;
    00432   bs.speedGaussSet = (boolHelp == 1);
    00433   stream >> bs.speedGaussOrientationRobot >> bs.speedGaussOrientationField;
    00434   stream >> bs.speedGaussMajAxis >> bs.speedGaussMinAxis;
    00435
    00436   return stream;
    00437 }*/
    00438 //------------------------------------------------------------------------------
    00439 SeenBallState::SeenBallState()
    00440 : BallState()
    00441 {
    00442   reset();
    00443 }
    00444 //------------------------------------------------------------------------------
    00445 void SeenBallState::reset()
    00446 {
    00447   BallState::reset();
    00448   timeWhenLastSeen = 0;
    00449   timeWhenFirstSeenConsecutively = 0;
    00450   timeUntilSeenConsecutively = 0;
    00451 }
    00452 //------------------------------------------------------------------------------
    00453 long SeenBallState::getConsecutivelySeenTime() const
    00454 {
    00455   long notSeenTime = (long)SystemCall::getTimeSince(timeUntilSeenConsecutively);
    00456   if (notSeenTime<200)
    00457   {
    00458     return ((long)timeUntilSeenConsecutively -
    00459             (long)timeWhenFirstSeenConsecutively);
    00460   }
    00461   else
    00462   {
    00463     return -notSeenTime;
    00464   }
    00465 }
    00466
    00467 void SeenBallState::serialize(In *in, Out *out)
    00468 {
    00469   STREAM_REGISTER_BEGIN();
    00470   getStreamHandler().registerBase();
    00471   STREAM_BASE(BallState);
    00472   STREAM( timeWhenLastSeen);
    00473   STREAM( timeWhenFirstSeenConsecutively);
    00474   STREAM( timeUntilSeenConsecutively);
    00475   STREAM_REGISTER_FINISH();
    00476 }
    00477
    00478 /*
    00479 //------------------------------------------------------------------------------
    00480 Out& operator<<(Out& stream, const SeenBallState& sbs)
    00481 {
    00482   stream << static_cast<const BallState&>(sbs);
    00483   stream << sbs.timeWhenLastSeen;
    00484   stream << sbs.timeWhenFirstSeenConsecutively;
    00485   stream << sbs.timeUntilSeenConsecutively;
    00486
    00487   return stream;
    00488 }
    00489 //------------------------------------------------------------------------------
    00490 In& operator>>(In& stream, SeenBallState& sbs)
    00491 {
    00492   stream >> static_cast<BallState&>(sbs);
    00493   stream >> sbs.timeWhenLastSeen;
    00494   stream >> sbs.timeWhenFirstSeenConsecutively;
    00495   stream >> sbs.timeUntilSeenConsecutively;
    00496
    00497   return stream;
    00498 }*/
    00499 //------------------------------------------------------------------------------
    00500 PropagatedBallState::PropagatedBallState()
    00501 : BallState()
    00502 {
    00503   reset();
    00504 }
    00505 //------------------------------------------------------------------------------
    00506 void PropagatedBallState::reset()
    00507 {
    00508   lastSeenBallState.reset();
    00509   BallState::reset();
    00510 }
    00511
    00512 void PropagatedBallState::serialize(In *in, Out *out)
    00513 {
    00514   STREAM_REGISTER_BEGIN();
    00515   STREAM_BASE(BallState);
    00516   STREAM( lastSeenBallState);
    00517   STREAM_REGISTER_FINISH();
    00518 }
    00519 //------------------------------------------------------------------------------
    00520 /*Out& operator<<(Out& stream, const PropagatedBallState& pbs)
    00521 {
    00522   stream << static_cast<const BallState&>(pbs);
    00523   stream << pbs.lastSeenBallState;
    00524
    00525   return stream;
    00526 }
    00527 //------------------------------------------------------------------------------
    00528 In& operator>>(In& stream, PropagatedBallState& pbs)
    00529 {
    00530   stream >> static_cast<BallState&>(pbs);
    00531   stream >> pbs.lastSeenBallState;
    00532
    00533   return stream;
    00534 }*/
    00535 //------------------------------------------------------------------------------
    00536 CommunicatedBallState::CommunicatedBallState()
    00537 : BallState()
    00538 {
    00539   reset();
    00540 }
    00541 //------------------------------------------------------------------------------
    00542 void CommunicatedBallState::reset()
    00543 {
    00544   timeWhenLastObserved = 0;
    00545   BallState::reset();
    00546 }
    00547 void CommunicatedBallState::serialize(In *in, Out *out)
    00548 {
    00549   STREAM_REGISTER_BEGIN();
    00550   STREAM_BASE(BallState);
    00551   STREAM( timeWhenLastObserved);
    00552   STREAM_REGISTER_FINISH();
    00553 }
    00554 /*//------------------------------------------------------------------------------
    00555 Out& operator<<(Out& stream, const CommunicatedBallState& cbs)
    00556 {
    00557   stream << static_cast<const BallState&>(cbs);
    00558   stream << cbs.timeWhenLastObserved;
    00559
    00560   return stream;
    00561 }
    00562 //------------------------------------------------------------------------------
    00563 In& operator>>(In& stream, CommunicatedBallState& cbs)
    00564 {
    00565   stream >> static_cast<BallState&>(cbs);
    00566   stream >> cbs.timeWhenLastObserved;
    00567
    00568   return stream;
    00569 }*/
    00570 //------------------------------------------------------------------------------
    00571 HypotheticalBallState::HypotheticalBallState()
    00572 : BallState()
    00573 {
    00574 }
    00575 //------------------------------------------------------------------------------
    00576 /*Out& operator<<(Out& stream, const HypotheticalBallState& hbs)
    00577 {
    00578   stream << static_cast<const BallState&>(hbs);
    00579   return stream;
    00580 }
    00581 //------------------------------------------------------------------------------
    00582 In& operator>>(In& stream, HypotheticalBallState& hbs)
    00583 {
    00584   stream >> static_cast<BallState&>(hbs);
    00585   return stream;
    00586 }*/
    00587 //------------------------------------------------------------------------------
    00588 BallModel::BallModel()
    00589 {
    00590   reset();
    00591 }
    00592 //------------------------------------------------------------------------------
    00593 void BallModel::reset()
    00594 {
    00595   frameNumber = 0;
    00596   numberOfImagesWithBallPercept = 0;
    00597   numberOfImagesWithoutBallPercept = 0;
    00598   ballWasSeen = false;
    00599   seen.reset();
    00600   propagated.reset();
    00601   communicated.reset();
    00602   hypothetical.reset();
    00603   //behaviorBallState = noneBallHandling;
    00604 }
    00605 //------------------------------------------------------------------------------
    00606 void BallModel::setFrameNumber(unsigned long frameNumber)
    00607 {
    00608   this->frameNumber = frameNumber;
    00609 }
    00610 //------------------------------------------------------------------------------
    00611 const Vector2<double>& BallModel::getKnownPosition(
    00612   unsigned long timeAfterWhichCommunicatedBallsAreAccepted) const
    00613 {
    00614   if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
    00615       timeAfterWhichCommunicatedBallsAreAccepted)
    00616   {
    00617     // use the seen position
    00618     return seen.positionField;
    00619   }
    00620   else
    00621   {
    00622     if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
    00623         SystemCall::getTimeSince(communicated.timeWhenLastObserved))
    00624     {
    00625       return seen.positionField;
    00626     }
    00627     else
    00628     {
    00629       return communicated.positionField;
    00630     }
    00631   }
    00632 }
    00633 //------------------------------------------------------------------------------
    00634 const Vector2<double>& BallModel::getKnownPosition(
    00635   unsigned long timeAfterWhichCommunicatedBallsAreAccepted,
    00636   unsigned long timeAfterWhichPropagatedAreUsed,
    00637   double maxDistanceToUseSeen,
    00638   double minDistanceToUseSeen) const
    00639 {
    00640   //in the BallSymbols: get-known-or-near
    00641   if ((SystemCall::getTimeSince(seen.timeWhenLastSeen)
    00642        < timeAfterWhichPropagatedAreUsed)
    00643       && (seen.positionRobot.abs() < maxDistanceToUseSeen)
    00644       && (seen.positionRobot.abs() > minDistanceToUseSeen)
    00645       && (seen.getPositionValidity() > 0.5))
    00646   {
    00647     INFO(printPixelUsage, idText, text, "known-or-near decided to use seen");
    00648     return seen.positionField;
    00649   }
    00650   else
    00651   {
    00652     if ((SystemCall::getTimeSince(seen.timeWhenLastSeen) <
    00653          timeAfterWhichCommunicatedBallsAreAccepted))
    00654     {
    00655       if ((propagated.getPositionValidity() > 0.1)
    00656         || (propagated.getPositionValidity() > communicated.getPositionValidity()))
    00657       {
    00658         INFO(printPixelUsage, idText, text, "known-or-near decided to use propagated");
    00659         return propagated.positionField;
    00660       }
    00661       else
    00662       {
    00663         INFO(printPixelUsage, idText, text, "known-or-near decided to use communicated");
    00664         return communicated.positionField;
    00665       }
    00666     }
    00667     else
    00668     {
    00669       if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
    00670           SystemCall::getTimeSince(communicated.timeWhenLastObserved))
    00671       {
    00672         INFO(printPixelUsage, idText, text, "known-or-near decided to use propagated");
    00673         return propagated.positionField;
    00674       }
    00675       else
    00676       {
    00677         INFO(printPixelUsage, idText, text, "known-or-near decided to use communicated");
    00678         return communicated.positionField;
    00679       }
    00680     }
    00681   }
    00682 }
    00683 //------------------------------------------------------------------------------
    00684 const BallState& BallModel::getKnownBallState(
    00685   unsigned long timeAfterWhichCommunicatedBallsAreAccepted,
    00686   unsigned long timeAfterWhichPropagatedAreUsed,
    00687   double maxDistanceToUseSeen,
    00688   double minDistanceToUseSeen) const
    00689 {
    00690   //in the BallSymbols: get-known-or-near
    00691   if ((SystemCall::getTimeSince(seen.timeWhenLastSeen)
    00692        < timeAfterWhichPropagatedAreUsed)
    00693       && (seen.positionRobot.abs() < maxDistanceToUseSeen)
    00694       && (seen.positionRobot.abs() > minDistanceToUseSeen)
    00695       && (seen.getPositionValidity() > 0.5))
    00696   {
    00697     return seen;
    00698   }
    00699   else
    00700   {
    00701     if ((SystemCall::getTimeSince(seen.timeWhenLastSeen) <
    00702       timeAfterWhichCommunicatedBallsAreAccepted))
    00703     {
    00704       if ((propagated.getPositionValidity() > 0.1)
    00705         || (propagated.getPositionValidity() > communicated.getPositionValidity()))
    00706       {
    00707         return propagated;
    00708       }
    00709       else
    00710       {
    00711         return communicated;
    00712       }
    00713     }
    00714     else
    00715     {
    00716       if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
    00717           SystemCall::getTimeSince(communicated.timeWhenLastObserved))
    00718       {
    00719         return propagated;
    00720       }
    00721       else
    00722       {
    00723         return communicated;
    00724       }
    00725     }
    00726   }
    00727 }
    00728 //------------------------------------------------------------------------------
    00729 unsigned long BallModel::getTimeSinceLastKnown(
    00730   unsigned long timeAfterWhichCommunicatedBallsAreAccepted) const
    00731 {
    00732   if (SystemCall::getTimeSince(seen.timeWhenLastSeen) <
    00733       timeAfterWhichCommunicatedBallsAreAccepted)
    00734   {
    00735     // use the seen position
    00736     return SystemCall::getTimeSince(seen.timeWhenLastSeen);
    00737   }
    00738   else
    00739   {
    00740     return min(SystemCall::getTimeSince(seen.timeWhenLastSeen),
    00741       SystemCall::getTimeSince(communicated.timeWhenLastObserved));
    00742   }
    00743 }