GT2005: Modules/SelfLocator/GT2005SelfLocator/GT2005LineCrossingsTable.cpp Source File

00001 /*
00002 * position and type of line crossings on the field
00003 *
00004 * @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
00005 * @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
00006 * @author Max Risler
00007 */
00008 
00009 
00010 #include "GT2005LineCrossingsTable.h"
00011 
00012 int GT2005LineCrossingsTable::NO_POINT_DISTANCE   = 100000;
00013 
00014 GT2005LineCrossingsTable::GT2005LineCrossingsTable()
00015 {
00016   for(int i = 0; i < numOfCrossingClasses; i++)
00017     for(int j = 0; j < 4; j++)
00018       numOfCrossings[i][j] = 0;
00019   initVirtualCrossings();
00020   initTCrossings();
00021   initLCrossings();
00022   initXCrossings();
00023   initFalseCrossings();
00024 }
00025 
00026 void GT2005LineCrossingsTable::initVirtualCrossings()
00027 {
00028   addCrossing(virtualCrossing, 0, xPosOwnPenaltyArea, yPosLeftGroundline);
00029   addCrossing(virtualCrossing, 0, xPosOwnPenaltyArea, yPosRightGroundline);
00030 
00031 // why were these removed? probably not required
00032 // addCrossing(virtualCrossing, 1, xPosHalfWayLine, yPosLeftPenaltyArea);
00033 // addCrossing(virtualCrossing, 1, xPosHalfWayLine, yPosRightPenaltyArea);
00034 
00035   addCrossing(virtualCrossing, 0, xPosOpponentPenaltyArea, yPosLeftGroundline);
00036   addCrossing(virtualCrossing, 0, xPosOpponentPenaltyArea, yPosRightGroundline);
00037 
00038   addCrossing(outerVirtualCrossing, 0, xPosHalfWayLine, yPosLeftFieldBorder);
00039   addCrossing(outerVirtualCrossing, 0, xPosHalfWayLine, yPosRightFieldBorder);
00040 
00041   addCrossing(outerVirtualCrossing, 0, xPosOwnGroundline, yPosLeftFieldBorder);
00042   addCrossing(outerVirtualCrossing, 0, xPosOwnGroundline, yPosRightFieldBorder);
00043 
00044   addCrossing(outerVirtualCrossing, 0, xPosOpponentGroundline, yPosLeftFieldBorder);
00045   addCrossing(outerVirtualCrossing, 0, xPosOpponentGroundline, yPosRightFieldBorder);
00046 
00047   addCrossing(outerVirtualCrossing, 1, xPosOpponentFieldBorder, yPosLeftGroundline);
00048   addCrossing(outerVirtualCrossing, 1, xPosOpponentFieldBorder, yPosRightGroundline);
00049 
00050   addCrossing(outerVirtualCrossing, 1, xPosOwnFieldBorder, yPosLeftGroundline);
00051   addCrossing(outerVirtualCrossing, 1, xPosOwnFieldBorder, yPosRightGroundline);
00052 }
00053 
00054 void GT2005LineCrossingsTable::initTCrossings()
00055 {
00056   addCrossing(outerTCrossing, 3, xPosOwnGroundline, yPosLeftPenaltyArea);
00057   addCrossing(outerTCrossing, 3, xPosOwnGroundline, yPosRightPenaltyArea);
00058 
00059   addCrossing(outerTCrossing, 2, xPosHalfWayLine, yPosLeftGroundline);
00060   addCrossing(outerTCrossing, 0, xPosHalfWayLine, yPosRightGroundline);
00061 
00062   addCrossing(outerTCrossing, 1, xPosOpponentGroundline, yPosLeftPenaltyArea);
00063   addCrossing(outerTCrossing, 1, xPosOpponentGroundline, yPosRightPenaltyArea);
00064 }
00065 
00066 void GT2005LineCrossingsTable::initLCrossings()
00067 {
00068   addCrossing(outerLCrossing, 3, xPosOwnGroundline, yPosLeftGroundline);
00069   addCrossing(outerLCrossing, 0, xPosOwnGroundline, yPosRightGroundline);
00070   addCrossing(outerLCrossing, 2, xPosOpponentGroundline, yPosLeftGroundline);
00071   addCrossing(outerLCrossing, 1, xPosOpponentGroundline, yPosRightGroundline);
00072 
00073   addCrossing(lCrossing, 2, xPosOwnPenaltyArea, yPosLeftPenaltyArea);
00074   addCrossing(lCrossing, 1, xPosOwnPenaltyArea, yPosRightPenaltyArea);
00075   addCrossing(lCrossing, 3, xPosOpponentPenaltyArea, yPosLeftPenaltyArea);
00076   addCrossing(lCrossing, 0, xPosOpponentPenaltyArea, yPosRightPenaltyArea);
00077 
00078   addCrossing(outerLCrossing, 3, xPosOwnFieldBorder, yPosLeftFieldBorder);
00079   addCrossing(outerLCrossing, 0, xPosOwnFieldBorder, yPosRightFieldBorder);
00080   addCrossing(outerLCrossing, 3, xPosOpponentFieldBorder, yPosLeftFieldBorder);
00081   addCrossing(outerLCrossing, 0, xPosOpponentFieldBorder, yPosRightFieldBorder);
00082 }
00083 
00084 void GT2005LineCrossingsTable::initFalseCrossings()
00085 {
00086   addCrossing(falseCrossing, 0, xPosHalfWayLine, yPosLeftPenaltyArea);
00087   addCrossing(falseCrossing, 0, xPosHalfWayLine, yPosRightPenaltyArea);
00088 }
00089 
00090 void GT2005LineCrossingsTable::initXCrossings()
00091 {
00092   addCrossing(xCrossing, 0, xPosHalfWayLine, centerCircleRadius);
00093   addCrossing(xCrossing, 0, xPosHalfWayLine, -centerCircleRadius);
00094 }
00095 
00096 Vector2<double> GT2005LineCrossingsTable::getClosestPoint(const Vector2<double>& point, CrossingClass crossingClass, int orientation) const
00097 {
00098   Vector2<double> minimum(NO_POINT_DISTANCE,NO_POINT_DISTANCE);
00099   double minDist = NO_POINT_DISTANCE;
00100 
00101   updateClosestPoint(minimum, minDist, point, crossingClass, orientation, crossingClass);
00102   return minimum;
00103 }
00104 
00105 void GT2005LineCrossingsTable::updateClosestPoint(Vector2<double>& minimum, double& minDist, const Vector2<double>& point, CrossingClass crossingClass, int orientation, CrossingClass& minCrossingClass) const
00106 {
00107   for(int i = 0; i < numOfCrossings[crossingClass][orientation]; i++)
00108   {
00109     Vector2<double> diff = point - crossings[crossingClass][orientation][i];
00110     if (diff.abs()<minDist)
00111     {
00112       minDist = diff.abs();
00113       minimum = crossings[crossingClass][orientation][i];
00114       minCrossingClass = crossingClass;
00115     }
00116   }
00117 }
00118 
00119 Vector2<double> GT2005LineCrossingsTable::getClosestPoint(const Pose2D& pose,
00120   const LinesPercept::LineCrossingPoint& crossingPoint, 
00121   CrossingClass& crossingClass) const
00122 {
00123   Vector2<double> minimum(NO_POINT_DISTANCE,NO_POINT_DISTANCE);
00124   double minDist = NO_POINT_DISTANCE;
00125 
00126   double poseAngle = normalize(pose.rotation + pi_4);
00127   if (poseAngle < 0) poseAngle += pi2;
00128   int poseOrientation = (int) (poseAngle / pi_2);
00129 
00130   if (crossingPoint.outOfImage ||
00131       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00132        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00133        crossingPoint.side3 != LinesPercept::lineOnThisSide &&
00134        crossingPoint.side4 != LinesPercept::lineOnThisSide))
00135     // possibly L, orientation 0
00136     updateClosestPoint(minimum, minDist, pose.translation, lCrossing, poseOrientation, crossingClass);
00137   if (crossingPoint.outOfImage ||
00138       (crossingPoint.side1 != LinesPercept::lineOnThisSide &&
00139        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00140        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00141        crossingPoint.side4 != LinesPercept::lineOnThisSide))
00142     // possibly L, orientation 1
00143     updateClosestPoint(minimum, minDist, pose.translation, lCrossing, (poseOrientation + 1) % 4, crossingClass);
00144   if (crossingPoint.outOfImage ||
00145       (crossingPoint.side1 != LinesPercept::lineOnThisSide &&
00146        crossingPoint.side2 != LinesPercept::lineOnThisSide &&
00147        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00148        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00149     // possibly L, orientation 2
00150     updateClosestPoint(minimum, minDist, pose.translation, lCrossing, (poseOrientation + 2) % 4, crossingClass);
00151   if (crossingPoint.outOfImage ||
00152       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00153        crossingPoint.side2 != LinesPercept::lineOnThisSide &&
00154        crossingPoint.side3 != LinesPercept::lineOnThisSide &&
00155        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00156     // possibly L, orientation 3
00157     updateClosestPoint(minimum, minDist, pose.translation, lCrossing, (poseOrientation + 3) % 4, crossingClass);
00158   if (crossingPoint.outOfImage ||
00159       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00160        crossingPoint.side2 != LinesPercept::noLineOnThisSide))
00161     // possibly outer L, orientation 0
00162     updateClosestPoint(minimum, minDist, pose.translation, outerLCrossing, poseOrientation, crossingClass);
00163   if (crossingPoint.outOfImage ||
00164       (crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00165        crossingPoint.side3 != LinesPercept::noLineOnThisSide))
00166     // possibly outer L, orientation 1
00167     updateClosestPoint(minimum, minDist, pose.translation, outerLCrossing, (poseOrientation + 1) % 4, crossingClass);
00168   if (crossingPoint.outOfImage ||
00169       (crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00170        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00171     // possibly outer L, orientation 2
00172     updateClosestPoint(minimum, minDist, pose.translation, outerLCrossing, (poseOrientation + 2) % 4, crossingClass);
00173   if (crossingPoint.outOfImage ||
00174       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00175        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00176     // possibly outer L, orientation 3
00177     updateClosestPoint(minimum, minDist, pose.translation, outerLCrossing, (poseOrientation + 3) % 4, crossingClass);
00178   if (crossingPoint.outOfImage ||
00179       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00180        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00181        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00182        crossingPoint.side4 != LinesPercept::lineOnThisSide))
00183     // possibly T, orientation 0
00184     updateClosestPoint(minimum, minDist, pose.translation, tCrossing, poseOrientation, crossingClass);
00185   if (crossingPoint.outOfImage ||
00186       (crossingPoint.side1 != LinesPercept::lineOnThisSide &&
00187        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00188        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00189        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00190     // possibly T, orientation 1
00191     updateClosestPoint(minimum, minDist, pose.translation, tCrossing, (poseOrientation + 1) % 4, crossingClass);
00192   if (crossingPoint.outOfImage ||
00193       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00194        crossingPoint.side2 != LinesPercept::lineOnThisSide &&
00195        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00196        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00197     // possibly T, orientation 2
00198     updateClosestPoint(minimum, minDist, pose.translation, tCrossing, (poseOrientation + 2) % 4, crossingClass);
00199   if (crossingPoint.outOfImage ||
00200       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00201        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00202        crossingPoint.side3 != LinesPercept::lineOnThisSide &&
00203        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00204     // possibly T, orientation 3
00205     updateClosestPoint(minimum, minDist, pose.translation, tCrossing, (poseOrientation + 3) % 4, crossingClass);
00206   if (crossingPoint.outOfImage ||
00207       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00208        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00209        crossingPoint.side3 != LinesPercept::noLineOnThisSide))
00210     // possibly outer T, orientation 0
00211     updateClosestPoint(minimum, minDist, pose.translation, outerTCrossing, poseOrientation, crossingClass);
00212   if (crossingPoint.outOfImage ||
00213       (crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00214        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00215        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00216     // possibly outer T, orientation 1
00217     updateClosestPoint(minimum, minDist, pose.translation, outerTCrossing, (poseOrientation + 1) % 4, crossingClass);
00218   if (crossingPoint.outOfImage ||
00219       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00220        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00221        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00222     // possibly outer T, orientation 2
00223     updateClosestPoint(minimum, minDist, pose.translation, outerTCrossing, (poseOrientation + 2) % 4, crossingClass);
00224   if (crossingPoint.outOfImage ||
00225       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00226        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00227        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00228     // possibly outer T, orientation 3
00229     updateClosestPoint(minimum, minDist, pose.translation, outerTCrossing, (poseOrientation + 3) % 4, crossingClass);
00230   if (crossingPoint.outOfImage ||
00231       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00232        crossingPoint.side2 != LinesPercept::lineOnThisSide &&
00233        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00234        crossingPoint.side4 != LinesPercept::lineOnThisSide))
00235     // possibly virtual, orientation 0
00236     // there are only two possible orientations for virtual crossings, so we use orientation % 2
00237     updateClosestPoint(minimum, minDist, pose.translation, virtualCrossing, poseOrientation % 2, crossingClass);
00238   if (crossingPoint.outOfImage ||
00239       (crossingPoint.side1 != LinesPercept::lineOnThisSide &&
00240        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00241        crossingPoint.side3 != LinesPercept::lineOnThisSide &&
00242        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00243     // possibly virtual, orientation 1
00244     updateClosestPoint(minimum, minDist, pose.translation, virtualCrossing, (poseOrientation + 1) % 2, crossingClass);
00245   if (crossingPoint.outOfImage ||
00246       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00247        crossingPoint.side3 != LinesPercept::noLineOnThisSide))
00248     // possibly outer virtual, orientation 0
00249     updateClosestPoint(minimum, minDist, pose.translation, outerVirtualCrossing, poseOrientation % 2, crossingClass);
00250   if (crossingPoint.outOfImage ||
00251       (crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00252        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00253     // possibly outer virtual, orientation 1
00254     updateClosestPoint(minimum, minDist, pose.translation, outerVirtualCrossing, (poseOrientation + 1) % 2, crossingClass);
00255   if (crossingPoint.outOfImage ||
00256       (crossingPoint.side1 != LinesPercept::noLineOnThisSide &&
00257        crossingPoint.side2 != LinesPercept::noLineOnThisSide &&
00258        crossingPoint.side3 != LinesPercept::noLineOnThisSide &&
00259        crossingPoint.side4 != LinesPercept::noLineOnThisSide))
00260     // possibly x
00261     updateClosestPoint(minimum, minDist, pose.translation, xCrossing, 0, crossingClass);
00262 
00263   return minimum;
00264 }