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

# Modules/SelfLocator/SlamSelfLocator/SlamLineCrossingsTable.cpp

Go to the documentation of this file.

    00001 #include "SlamLineCrossingsTable.h"
    00002
    00003 int SlamLineCrossingsTable::NO_POINT_DISTANCE   = 100000;
    00004
    00005 double SlamLineCrossingsTable::crossingsX[] = {
    00006     xPosOwnGroundline,
    00007     xPosOwnGroundline,
    00008     xPosOwnGroundline,
    00009     xPosOwnGroundline,
    00010
    00011     xPosOwnPenaltyArea,
    00012     xPosOwnPenaltyArea,
    00013     xPosOwnPenaltyArea,
    00014     xPosOwnPenaltyArea,
    00015
    00016     xPosHalfWayLine,
    00017     xPosHalfWayLine,
    00018     xPosHalfWayLine,
    00019     xPosHalfWayLine,
    00020
    00021     xPosOpponentPenaltyArea,
    00022     xPosOpponentPenaltyArea,
    00023     xPosOpponentPenaltyArea,
    00024     xPosOpponentPenaltyArea,
    00025
    00026     xPosOpponentGroundline,
    00027     xPosOpponentGroundline,
    00028     xPosOpponentGroundline,
    00029     xPosOpponentGroundline
    00030
    00031     //xPosHalfWayLine         // for middle circle
    00032   };
    00033 double SlamLineCrossingsTable::crossingsY[]={
    00034     yPosLeftGroundline,
    00035     yPosLeftPenaltyArea,
    00036     yPosRightPenaltyArea,
    00037     yPosRightGroundline,
    00038
    00039     yPosLeftGroundline,
    00040     yPosLeftPenaltyArea,
    00041     yPosRightPenaltyArea,
    00042     yPosRightGroundline,
    00043
    00044     yPosLeftGroundline,
    00045     yPosLeftPenaltyArea,
    00046     yPosRightPenaltyArea,
    00047     yPosRightGroundline,
    00048
    00049     yPosLeftGroundline,
    00050     yPosLeftPenaltyArea,
    00051     yPosRightPenaltyArea,
    00052     yPosRightGroundline,
    00053
    00054     yPosLeftGroundline,
    00055     yPosLeftPenaltyArea,
    00056     yPosRightPenaltyArea,
    00057     yPosRightGroundline
    00058
    00059     // yPosCenterGoal        for middle circle
    00060   };
    00061
    00062 SlamLineCrossingsTable::SlamLineCrossingsTable()
    00063 {
    00064   initAllCrossings();
    00065   initVirtualCrossings();
    00066   initTCrossings();
    00067   initLCrossings();
    00068   initFalseCrossings();
    00069 }
    00070
    00071
    00072 Vector2<double> SlamLineCrossingsTable::getClosestPoint(const Vector2<double>& point, CrossingType type)
    00073 {
    00074   Vector2<double> minimum(NO_POINT_DISTANCE,NO_POINT_DISTANCE);
    00075   double minDist = NO_POINT_DISTANCE;
    00076
    00077   for(int i = 0; i < NUMBER_OF_CROSSINGS; i++)
    00078   {
    00079     Vector2<double> crossing(crossingsX[i],crossingsY[i]);
    00080     Vector2<double> diff = point - crossing;
    00081     if (diff.abs()<minDist)
    00082     {
    00083       minDist = diff.abs();
    00084       minimum = crossing;
    00085     }
    00086   }
    00087   return minimum;
    00088 }
    00089
    00090 Vector2<double> SlamLineCrossingsTable::getClassifiedClosestPoint(const Vector2<double>& point, CrossingClass crossingClass)
    00091 {
    00092   int number = 0;
    00093   Vector2<double>* crossingsArray;
    00094   Vector2<double> minimum(NO_POINT_DISTANCE,NO_POINT_DISTANCE);
    00095   double minDist = NO_POINT_DISTANCE;
    00096
    00097   switch(crossingClass)
    00098   {
    00099     case unknownCrossing:
    00100       number = NUMBER_OF_CLASSIFIED_CROSSINGS;
    00101       crossingsArray = allCrossings;
    00102       break;
    00103     case virtualCrossing:
    00104       number = NUMBER_OF_VIRTUAL_CROSSINGS;
    00105       crossingsArray = virtualCrossings;
    00106       break;
    00107     case lCrossing:
    00108       number = NUMBER_OF_L_CROSSINGS;
    00109       crossingsArray = lCrossings;
    00110       break;
    00111     case tCrossing:
    00112       number = NUMBER_OF_T_CROSSINGS;
    00113       crossingsArray = tCrossings;
    00114       break;
    00115     case falseCrossing:
    00116       number = NUMBER_OF_FALSE_CROSSINGS;
    00117       crossingsArray = falseCrossings;
    00118       break;
    00119     default:
    00120       number = NUMBER_OF_CLASSIFIED_CROSSINGS;
    00121       crossingsArray = allCrossings;
    00122   };
    00123
    00124   for(int i = 0; i < number; i++)
    00125   {
    00126     Vector2<double> diff = point - crossingsArray[i];
    00127     if (diff.abs()<minDist)
    00128     {
    00129       minDist = diff.abs();
    00130       minimum = crossingsArray[i];
    00131     }
    00132   }
    00133   return minimum;
    00134
    00135 }
    00136
    00137 void SlamLineCrossingsTable::initAllCrossings()
    00138 {
    00139   allCrossings[0].x = xPosOwnGroundline;
    00140   allCrossings[0].y = yPosLeftGroundline;
    00141
    00142   allCrossings[1].x = xPosOwnGroundline;
    00143   allCrossings[1].y = yPosLeftPenaltyArea;
    00144
    00145   allCrossings[2].x = xPosOwnGroundline;
    00146   allCrossings[2].y = yPosRightPenaltyArea;
    00147
    00148   allCrossings[3].x = xPosOwnGroundline;
    00149   allCrossings[3].y = yPosRightGroundline;
    00150
    00151   allCrossings[4].x = xPosOwnPenaltyArea;
    00152   allCrossings[4].y = yPosLeftGroundline;
    00153
    00154   allCrossings[5].x = xPosOwnPenaltyArea;
    00155   allCrossings[5].y = yPosLeftPenaltyArea;
    00156
    00157   allCrossings[6].x = xPosOwnPenaltyArea;
    00158   allCrossings[6].y = yPosRightPenaltyArea;
    00159
    00160   allCrossings[7].x = xPosOwnPenaltyArea;
    00161   allCrossings[7].y = yPosRightGroundline;
    00162
    00163   allCrossings[8].x = xPosHalfWayLine;
    00164   allCrossings[8].y = yPosLeftGroundline;
    00165
    00166   // allCrossings[9].x = xPosHalfWayLine;
    00167   // allCrossings[9].y = yPosLeftPenaltyArea;
    00168
    00169   // allCrossings[10].x = xPosHalfWayLine;
    00170   // allCrossings[10].y = yPosRightPenaltyArea;
    00171
    00172   allCrossings[9].x = xPosHalfWayLine;
    00173   allCrossings[9].y = yPosRightGroundline;
    00174
    00175   allCrossings[10].x = xPosOpponentPenaltyArea;
    00176   allCrossings[10].y = yPosLeftGroundline;
    00177
    00178   allCrossings[11].x = xPosOpponentPenaltyArea;
    00179   allCrossings[11].y = yPosLeftPenaltyArea;
    00180
    00181   allCrossings[12].x = xPosOpponentPenaltyArea;
    00182   allCrossings[12].y = yPosRightPenaltyArea;
    00183
    00184   allCrossings[13].x = xPosOpponentPenaltyArea;
    00185   allCrossings[13].y = yPosRightGroundline;
    00186
    00187   allCrossings[14].x = xPosOpponentGroundline;
    00188   allCrossings[14].y = yPosLeftGroundline;
    00189
    00190   allCrossings[15].x = xPosOpponentGroundline;
    00191   allCrossings[15].y = yPosLeftPenaltyArea;
    00192
    00193   allCrossings[16].x = xPosOpponentGroundline;
    00194   allCrossings[16].y = yPosRightPenaltyArea;
    00195
    00196   allCrossings[17].x = xPosOpponentGroundline;
    00197   allCrossings[17].y = yPosRightGroundline;
    00198 }
    00199
    00200 void SlamLineCrossingsTable::initVirtualCrossings()
    00201 {
    00202   virtualCrossings[0].x = xPosOwnPenaltyArea;
    00203   virtualCrossings[0].y = yPosLeftGroundline;
    00204
    00205   virtualCrossings[1].x = xPosOwnPenaltyArea;
    00206   virtualCrossings[1].y = yPosRightGroundline;
    00207
    00208 //   virtualCrossings[2].x = xPosHalfWayLine;
    00209 //  virtualCrossings[2].y = yPosLeftPenaltyArea;
    00210
    00211 //  virtualCrossings[3].x = xPosHalfWayLine;
    00212 //  virtualCrossings[3].y = yPosRightPenaltyArea;
    00213
    00214   virtualCrossings[2].x = xPosOpponentPenaltyArea;
    00215   virtualCrossings[2].y = yPosLeftGroundline;
    00216
    00217   virtualCrossings[3].x = xPosOpponentPenaltyArea;
    00218   virtualCrossings[3].y = yPosRightGroundline;
    00219 }
    00220
    00221 void SlamLineCrossingsTable::initTCrossings()
    00222 {
    00223   tCrossings[0].x = xPosOwnGroundline;
    00224   tCrossings[0].y = yPosLeftPenaltyArea;
    00225
    00226   tCrossings[1].x = xPosOwnGroundline;
    00227   tCrossings[1].y = yPosRightPenaltyArea;
    00228
    00229   tCrossings[2].x = xPosHalfWayLine;
    00230   tCrossings[2].y = yPosLeftGroundline;
    00231
    00232   tCrossings[3].x = xPosHalfWayLine;
    00233   tCrossings[3].y = yPosRightGroundline;
    00234
    00235   tCrossings[4].x = xPosOpponentGroundline;
    00236   tCrossings[4].y = yPosLeftPenaltyArea;
    00237
    00238   tCrossings[5].x = xPosOpponentGroundline;
    00239   tCrossings[5].y = yPosRightPenaltyArea;
    00240 }
    00241
    00242 void SlamLineCrossingsTable::initLCrossings()
    00243 {
    00244   lCrossings[0].x = xPosOwnGroundline;
    00245   lCrossings[0].y = yPosLeftGroundline;
    00246
    00247   lCrossings[1].x = xPosOwnGroundline;
    00248   lCrossings[1].y = yPosRightGroundline;
    00249
    00250   lCrossings[2].x = xPosOwnPenaltyArea;
    00251   lCrossings[2].y = yPosLeftPenaltyArea;
    00252
    00253   lCrossings[3].x = xPosOwnPenaltyArea;
    00254   lCrossings[3].y = yPosRightPenaltyArea;
    00255
    00256   lCrossings[4].x = xPosOpponentPenaltyArea;
    00257   lCrossings[4].y = yPosLeftPenaltyArea;
    00258
    00259   lCrossings[5].x = xPosOpponentPenaltyArea;
    00260   lCrossings[5].y = yPosRightPenaltyArea;
    00261
    00262   lCrossings[6].x = xPosOpponentGroundline;
    00263   lCrossings[6].y = yPosLeftGroundline;
    00264
    00265   lCrossings[7].x = xPosOpponentGroundline;
    00266   lCrossings[7].y = yPosRightGroundline;
    00267
    00268 }
    00269
    00270
    00271 void SlamLineCrossingsTable::initFalseCrossings()
    00272 {
    00273   lCrossings[0].x = xPosHalfWayLine;
    00274   lCrossings[0].y = yPosLeftPenaltyArea;
    00275
    00276   lCrossings[1].x = xPosHalfWayLine;
    00277   lCrossings[1].y = yPosRightPenaltyArea;
    00278 }