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

# Modules/SelfLocator/GT2005StableSelfLocator/GT2005StableLineCrossingsTable.cpp

Go to the documentation of this file.

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