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

# Modules/SelfLocator/GT2005SelfLocator/GT2005LineCrossingsTable.cpp

Go to the documentation of this 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 }