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

# Tools/Field.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file Tools/Field.cpp
    00003  *
    00004  * This file contains the implementation of a class representing the field boundary.
    00005  *
    00006  * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00007  */
    00008
    00009 #include "Field.h"
    00010 #include "Tools/Debugging/Debugging.h"
    00011 #include "Tools/Player.h"
    00012 #include "Tools/RobotConfiguration.h"
    00013 #include "Platform/GTAssert.h"
    00014
    00015 Field::Field()
    00016 : Boundary<double>(0,0)
    00017 {
    00018 #ifdef HUGE_FIELD
    00019   add(Vector2<double>(xPosOpponentGoal,yPosLeftFlags + flagRadius));
    00020   add(Vector2<double>(xPosOwnGoal,yPosRightFlags - flagRadius));
    00021   initBoundaryForHugeField(lines[LinesPercept::boundary]);
    00022   initBorderForHugeField(lines[LinesPercept::border]); // carpet borders
    00023 #else
    00024   add(Vector2<double>(xPosOpponentGoal,yPosLeftSideline));
    00025   add(Vector2<double>(xPosOwnGoal,yPosRightSideline));
    00026   initBoundary(lines[LinesPercept::boundary]);
    00027   initBorder(lines[LinesPercept::border]); // field borders
    00028 #endif
    00029   initLines(lines[LinesPercept::field], lines[LinesPercept::xField], lines[LinesPercept::yField]);
    00030   initOwnGoal(lines[LinesPercept::yellowGoal]); // for red player
    00031   initOpponentGoal(lines[LinesPercept::skyblueGoal]); // for red player
    00032 #ifdef HUGE_FIELD
    00033   initSimpleLinesForHugeField(lines[LinesPercept::numberOfLineTypes]);
    00034 #else
    00035   initSimpleLines(lines[LinesPercept::numberOfLineTypes]);
    00036 #endif
    00037 }
    00038
    00039 void Field::initBoundary(Table& table)
    00040 {
    00041   double x[] =
    00042   {
    00043     xPosOwnGroundline,
    00044     xPosOwnSideCorner,
    00045     xPosOpponentSideCorner,
    00046     xPosOpponentGroundline,
    00047     xPosOpponentGroundline,
    00048     xPosOpponentGoal - 100,
    00049     xPosOpponentGoal - 100,
    00050     xPosOpponentGroundline,
    00051     xPosOpponentGroundline,
    00052     xPosOpponentSideCorner,
    00053     xPosOwnSideCorner,
    00054     xPosOwnGroundline,
    00055     xPosOwnGroundline,
    00056     xPosOwnGoal + 100,
    00057     xPosOwnGoal + 100,
    00058     xPosOwnGroundline,
    00059     xPosOwnGroundline
    00060   },
    00061          y[] =
    00062   {
    00063     yPosLeftGroundline,
    00064     yPosLeftSideline,
    00065     yPosLeftSideline,
    00066     yPosLeftGroundline,
    00067     yPosLeftGoal,
    00068     yPosLeftGoal,
    00069     yPosRightGoal,
    00070     yPosRightGoal,
    00071     yPosRightGroundline,
    00072     yPosRightSideline,
    00073     yPosRightSideline,
    00074     yPosRightGroundline,
    00075     yPosRightGoal,
    00076     yPosRightGoal,
    00077     yPosLeftGoal,
    00078     yPosLeftGoal,
    00079     yPosLeftGroundline
    00080   };
    00081
    00082   table.setSize(sizeof(x) / sizeof(x[0]) - 1);
    00083   addCoords(table,table.maxNumberOfEntries,x,y);
    00084 }
    00085
    00086 void Field::initBoundaryForHugeField(Table& table)
    00087 {
    00088   double x[] =
    00089   {
    00090     xPosOwnGoal,
    00091     xPosOwnGoal,
    00092     xPosOpponentGoal,
    00093     xPosOpponentGoal,
    00094     xPosOwnGoal
    00095   },
    00096          y[] =
    00097   {
    00098     yPosLeftFlags + (int) flagRadius,
    00099     yPosRightFlags - (int) flagRadius,
    00100     yPosRightFlags - (int) flagRadius,
    00101     yPosLeftFlags + (int) flagRadius,
    00102     yPosLeftFlags + (int) flagRadius
    00103   };
    00104
    00105   table.setSize(4);
    00106   addCoords(table,4,x,y);
    00107 }
    00108
    00109 void Field::initLines(Table& table, Table& xTable, Table& yTable)
    00110 {
    00111   double x[] =
    00112   {
    00113     // own ground line
    00114     xPosOwnGroundline,
    00115     xPosOwnGroundline,
    00116     xPosOwnGroundline - fieldLinesWidth,
    00117     xPosOwnGroundline - fieldLinesWidth,
    00118
    00119     // opponent ground line
    00120     xPosOpponentGroundline,
    00121     xPosOpponentGroundline,
    00122     xPosOpponentGroundline + fieldLinesWidth,
    00123     xPosOpponentGroundline + fieldLinesWidth,
    00124
    00125     // own inner penalty line border
    00126     xPosOwnGroundline,
    00127     xPosOwnPenaltyArea - fieldLinesWidth,
    00128     xPosOwnPenaltyArea - fieldLinesWidth,
    00129     xPosOwnGroundline,
    00130
    00131     // own outer penalty line border
    00132     xPosOwnGroundline,
    00133     xPosOwnPenaltyArea,
    00134     xPosOwnPenaltyArea,
    00135     xPosOwnGroundline,
    00136
    00137     // opponent inner penalty line border
    00138     xPosOpponentGroundline,
    00139     xPosOpponentPenaltyArea + fieldLinesWidth,
    00140     xPosOpponentPenaltyArea + fieldLinesWidth,
    00141     xPosOpponentGroundline,
    00142
    00143     // opponent outer penalty line border
    00144     xPosOpponentGroundline,
    00145     xPosOpponentPenaltyArea,
    00146     xPosOpponentPenaltyArea,
    00147     xPosOpponentGroundline,
    00148
    00149     //                                     _
    00150     // inner center, opponent half-disc:  /_\ .
    00151     -fieldLinesWidth / 2,
    00152     -fieldLinesWidth / 2,
    00153     int(-sin(pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00154     int(-sin(pi_2 / 2) * (centerCircleRadius - fieldLinesWidth / 2)),
    00155     int(-sin(3 * pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00156     -centerCircleRadius + fieldLinesWidth / 2,
    00157     int(-sin(3 * pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00158     int(-sin(pi_2 / 2) * (centerCircleRadius - fieldLinesWidth / 2)),
    00159     int(-sin(pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00160     -fieldLinesWidth / 2,
    00161
    00162     //                                     _
    00163     // inner center, opponent half-disc:  /_\ .
    00164     fieldLinesWidth / 2,
    00165     fieldLinesWidth / 2,
    00166     int(sin(pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00167     int(sin(pi_2 / 2) * (centerCircleRadius - fieldLinesWidth / 2)),
    00168     int(sin(3 * pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00169     centerCircleRadius - fieldLinesWidth / 2,
    00170     int(sin(3 * pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00171     int(sin(pi_2 / 2) * (centerCircleRadius - fieldLinesWidth / 2)),
    00172     int(sin(pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00173     fieldLinesWidth / 2,
    00174
    00175     //                                          __   __
    00176     // outer circle and center line, own half:    \_/
    00177     -fieldLinesWidth / 2,
    00178     -fieldLinesWidth / 2,
    00179     int(-sin(pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00180     int(-sin(pi_2 / 2) * (centerCircleRadius + fieldLinesWidth / 2)),
    00181     int(-sin(3 * pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00182     -centerCircleRadius - fieldLinesWidth / 2,
    00183     int(-sin(3 * pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00184     int(-sin(pi_2 / 2) * (centerCircleRadius + fieldLinesWidth / 2)),
    00185     int(-sin(pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00186     -fieldLinesWidth / 2,
    00187     -fieldLinesWidth / 2,
    00188
    00189     //                                                  _
    00190     // outer circle and center line, opponent half:  __/ \__
    00191     fieldLinesWidth / 2,
    00192     fieldLinesWidth / 2,
    00193     int(sin(pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00194     int(sin(pi_2 / 2) * (centerCircleRadius + fieldLinesWidth / 2)),
    00195     int(sin(3 * pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00196     centerCircleRadius + fieldLinesWidth / 2,
    00197     int(sin(3 * pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00198     int(sin(pi_2 / 2) * (centerCircleRadius + fieldLinesWidth / 2)),
    00199     int(sin(pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00200     fieldLinesWidth / 2,
    00201     fieldLinesWidth / 2,
    00202
    00203     // outer border
    00204     xPosOwnGroundline - fieldLinesWidth,
    00205     xPosOwnGroundline - fieldLinesWidth,
    00206     xPosOpponentGroundline + fieldLinesWidth,
    00207     xPosOpponentGroundline + fieldLinesWidth,
    00208     xPosOwnGroundline - fieldLinesWidth,
    00209
    00210     // goal lines between penalty lines
    00211     xPosOwnGroundline,
    00212     xPosOwnGroundline,
    00213     xPosOpponentGroundline,
    00214     xPosOpponentGroundline,
    00215
    00216     // corners
    00217     xPosOwnGroundline,
    00218     xPosOwnGroundline,
    00219     -fieldLinesWidth / 2,
    00220
    00221     xPosOwnGroundline,
    00222     xPosOwnGroundline,
    00223     -fieldLinesWidth / 2,
    00224
    00225     xPosOpponentGroundline,
    00226     xPosOpponentGroundline,
    00227     fieldLinesWidth / 2,
    00228
    00229     xPosOpponentGroundline,
    00230     xPosOpponentGroundline,
    00231     fieldLinesWidth / 2,
    00232 },
    00233          y[] =
    00234   {
    00235     // own ground line
    00236     yPosLeftGoal,
    00237     yPosRightGoal,
    00238     yPosLeftGoal,
    00239     yPosRightGoal,
    00240
    00241     // opponent ground line
    00242     yPosLeftGoal,
    00243     yPosRightGoal,
    00244     yPosLeftGoal,
    00245     yPosRightGoal,
    00246
    00247     // own inner penalty line border
    00248     yPosLeftPenaltyArea - fieldLinesWidth,
    00249     yPosLeftPenaltyArea - fieldLinesWidth,
    00250     yPosRightPenaltyArea + fieldLinesWidth,
    00251     yPosRightPenaltyArea + fieldLinesWidth,
    00252
    00253     // own outer penalty line border
    00254     yPosLeftPenaltyArea,
    00255     yPosLeftPenaltyArea,
    00256     yPosRightPenaltyArea,
    00257     yPosRightPenaltyArea,
    00258
    00259     // opponent inner penalty line border
    00260     yPosLeftPenaltyArea - fieldLinesWidth,
    00261     yPosLeftPenaltyArea - fieldLinesWidth,
    00262     yPosRightPenaltyArea + fieldLinesWidth,
    00263     yPosRightPenaltyArea + fieldLinesWidth,
    00264
    00265     // opponent outer penalty line border
    00266     yPosLeftPenaltyArea,
    00267     yPosLeftPenaltyArea,
    00268     yPosRightPenaltyArea,
    00269     yPosRightPenaltyArea,
    00270
    00271     //                                _
    00272     // inner center, own half-disc:  \_/
    00273     centerCircleRadius - fieldLinesWidth / 2,
    00274     -centerCircleRadius + fieldLinesWidth / 2,
    00275     int(-cos(pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00276     int(-cos(pi_2 / 2) * (centerCircleRadius - fieldLinesWidth / 2)),
    00277     int(-cos(3 * pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00278     0,
    00279     int(cos(3 * pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00280     int(cos(pi_2 / 2) * (centerCircleRadius - fieldLinesWidth / 2)),
    00281     int(cos(pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00282     centerCircleRadius - fieldLinesWidth / 2,
    00283
    00284     //                                     _
    00285     // inner center, opponent half-disc:  /_\ .
    00286     centerCircleRadius - fieldLinesWidth / 2,
    00287     -centerCircleRadius + fieldLinesWidth / 2,
    00288     int(-cos(pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00289     int(-cos(pi_2 / 2) * (centerCircleRadius - fieldLinesWidth / 2)),
    00290     int(-cos(3 * pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00291     0,
    00292     int(cos(3 * pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00293     int(cos(pi_2 / 2) * (centerCircleRadius - fieldLinesWidth / 2)),
    00294     int(cos(pi_2 / 4) * (centerCircleRadius - fieldLinesWidth / 2)),
    00295     centerCircleRadius - fieldLinesWidth / 2,
    00296
    00297     //                                          __   __
    00298     // outer circle and center line, own half:    \_/
    00299     yPosRightSideline,
    00300     -centerCircleRadius - fieldLinesWidth / 2,
    00301     int(-cos(pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00302     int(-cos(pi_2 / 2) * (centerCircleRadius + fieldLinesWidth / 2)),
    00303     int(-cos(3 * pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00304     0,
    00305     int(cos(3 * pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00306     int(cos(pi_2 / 2) * (centerCircleRadius + fieldLinesWidth / 2)),
    00307     int(cos(pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00308     centerCircleRadius + fieldLinesWidth / 2,
    00309     yPosLeftSideline,
    00310
    00311     //                                                  _
    00312     // outer circle and center line, opponent half:  __/ \__
    00313     yPosRightSideline,
    00314     -centerCircleRadius - fieldLinesWidth / 2,
    00315     int(-cos(pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00316     int(-cos(pi_2 / 2) * (centerCircleRadius + fieldLinesWidth / 2)),
    00317     int(-cos(3 * pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00318     0,
    00319     int(cos(3 * pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00320     int(cos(pi_2 / 2) * (centerCircleRadius + fieldLinesWidth / 2)),
    00321     int(cos(pi_2 / 4) * (centerCircleRadius + fieldLinesWidth / 2)),
    00322     centerCircleRadius + fieldLinesWidth / 2,
    00323     yPosLeftSideline,
    00324
    00325     // outer border
    00326     yPosLeftSideline + fieldLinesWidth,
    00327     yPosRightSideline - fieldLinesWidth,
    00328     yPosRightSideline - fieldLinesWidth,
    00329     yPosLeftSideline + fieldLinesWidth,
    00330     yPosLeftSideline + fieldLinesWidth,
    00331
    00332     // goal lines between penalty lines
    00333     yPosLeftPenaltyArea - fieldLinesWidth,
    00334     yPosRightPenaltyArea + fieldLinesWidth,
    00335     yPosLeftPenaltyArea - fieldLinesWidth,
    00336     yPosRightPenaltyArea + fieldLinesWidth,
    00337
    00338     // corners
    00339     yPosLeftPenaltyArea,
    00340     yPosLeftSideline,
    00341     yPosLeftSideline,
    00342
    00343     yPosRightPenaltyArea,
    00344     yPosRightSideline,
    00345     yPosRightSideline,
    00346
    00347     yPosLeftPenaltyArea,
    00348     yPosLeftSideline,
    00349     yPosLeftSideline,
    00350
    00351     yPosRightPenaltyArea,
    00352     yPosRightSideline,
    00353     yPosRightSideline,
    00354   };
    00355
    00356 #ifdef HUGE_FIELD
    00357   table.setSize(64);
    00358   xTable.setSize(64);
    00359   yTable.setSize(64);
    00360
    00361   // outer border
    00362   addCoords(table,xTable,yTable,4,x + 66,y + 66, true);
    00363
    00364   // goal lines between penalty lines
    00365   addCoords(table,xTable,yTable,1,x + 71,y + 71);
    00366   addCoords(table,xTable,yTable,1,x + 73,y + 73, true);
    00367
    00368   // corners
    00369   addCoords(table,xTable,yTable,2,x + 75,y + 75, true);
    00370   addCoords(table,xTable,yTable,2,x + 78,y + 78);
    00371   addCoords(table,xTable,yTable,2,x + 81,y + 81);
    00372   addCoords(table,xTable,yTable,2,x + 84,y + 84, true);
    00373 #else
    00374   table.setSize(54);
    00375   xTable.setSize(54);
    00376   yTable.setSize(54);
    00377
    00378   // ground lines
    00379   for(int i = 0; i < 4; ++i)
    00380     addCoords(table,xTable,yTable,1,x + 2 * i,y + 2 * i, i==1 || i==2);
    00381 #endif
    00382
    00383   // own penalty
    00384   addCoords(table,xTable,yTable,3,x + 8,y + 8, true);
    00385   addCoords(table,xTable,yTable,3,x + 12,y + 12);
    00386
    00387   // opponent penalty
    00388   addCoords(table,xTable,yTable,3,x + 16,y + 16);
    00389   addCoords(table,xTable,yTable,3,x + 20,y + 20, true);
    00390
    00391   // inner center
    00392   addCoords(table,xTable,yTable,9,x + 24,y + 24, true);
    00393   addCoords(table,xTable,yTable,9,x + 34,y + 34);
    00394
    00395   // outer center and center line
    00396   addCoords(table,xTable,yTable,10,x + 44,y + 44);
    00397   addCoords(table,xTable,yTable,10,x + 55,y + 55, true);
    00398 }
    00399
    00400 void Field::initSimpleLines(Table& table)
    00401 {
    00402   double x[] =
    00403   {
    00404     xPosOwnGroundline,
    00405     xPosOwnGroundline,
    00406
    00407     xPosOpponentGroundline,
    00408     xPosOpponentGroundline,
    00409
    00410     xPosOwnGroundline + fieldLinesWidth / 2,
    00411     xPosOwnPenaltyArea,
    00412     xPosOwnPenaltyArea,
    00413     xPosOwnGroundline + fieldLinesWidth / 2,
    00414
    00415     xPosOpponentGroundline - fieldLinesWidth / 2,
    00416     xPosOpponentPenaltyArea,
    00417     xPosOpponentPenaltyArea,
    00418     xPosOpponentGroundline - fieldLinesWidth / 2,
    00419
    00420     xPosHalfWayLine,
    00421     xPosHalfWayLine
    00422   },
    00423          y[] =
    00424   {
    00425     yPosLeftGoal,
    00426     yPosRightGoal,
    00427
    00428     yPosLeftGoal,
    00429     yPosRightGoal,
    00430
    00431     yPosLeftPenaltyArea,
    00432     yPosLeftPenaltyArea,
    00433     yPosRightPenaltyArea,
    00434     yPosRightPenaltyArea,
    00435
    00436     yPosLeftPenaltyArea,
    00437     yPosLeftPenaltyArea,
    00438     yPosRightPenaltyArea,
    00439     yPosRightPenaltyArea,
    00440
    00441     yPosRightSideline,yPosLeftSideline
    00442   };
    00443
    00444   table.setSize(9);
    00445
    00446   // ground lines
    00447   addCoords(table,1,x,y);
    00448   addCoords(table,1,x + 2,y + 2);
    00449
    00450   // penalty area
    00451   addCoords(table,3,x + 4,y + 4);
    00452   addCoords(table,3,x + 8,y + 8);
    00453
    00454   // halfWay line
    00455   addCoords(table,1,x + 12,y + 12);
    00456 }
    00457
    00458 void Field::initSimpleLinesForHugeField(Table& table)
    00459 {
    00460   double x[] =
    00461   {
    00462     xPosOwnGroundline - fieldLinesWidth / 2,
    00463     xPosOwnGroundline - fieldLinesWidth / 2,
    00464     xPosOpponentGroundline + fieldLinesWidth / 2,
    00465     xPosOpponentGroundline + fieldLinesWidth / 2,
    00466     xPosOwnGroundline - fieldLinesWidth / 2,
    00467
    00468     xPosOwnGroundline - fieldLinesWidth / 2,
    00469     xPosOwnPenaltyArea - fieldLinesWidth / 2,
    00470     xPosOwnPenaltyArea - fieldLinesWidth / 2,
    00471     xPosOwnGroundline - fieldLinesWidth / 2,
    00472
    00473     xPosOpponentGroundline + fieldLinesWidth / 2,
    00474     xPosOpponentPenaltyArea + fieldLinesWidth / 2,
    00475     xPosOpponentPenaltyArea + fieldLinesWidth / 2,
    00476     xPosOpponentGroundline + fieldLinesWidth / 2,
    00477
    00478     xPosHalfWayLine,
    00479     xPosHalfWayLine
    00480   },
    00481          y[] =
    00482   {
    00483     yPosLeftSideline + fieldLinesWidth / 2,
    00484     yPosRightSideline - fieldLinesWidth / 2,
    00485     yPosRightSideline - fieldLinesWidth / 2,
    00486     yPosLeftSideline + fieldLinesWidth / 2,
    00487     yPosLeftSideline + fieldLinesWidth / 2,
    00488
    00489     yPosLeftPenaltyArea - fieldLinesWidth / 2,
    00490     yPosLeftPenaltyArea - fieldLinesWidth / 2,
    00491     yPosRightPenaltyArea + fieldLinesWidth / 2,
    00492     yPosRightPenaltyArea + fieldLinesWidth / 2,
    00493
    00494     yPosLeftPenaltyArea - fieldLinesWidth / 2,
    00495     yPosLeftPenaltyArea - fieldLinesWidth / 2,
    00496     yPosRightPenaltyArea + fieldLinesWidth / 2,
    00497     yPosRightPenaltyArea + fieldLinesWidth / 2,
    00498
    00499     yPosRightSideline - fieldLinesWidth / 2,
    00500     yPosLeftSideline + fieldLinesWidth / 2
    00501   };
    00502
    00503   table.setSize(11);
    00504
    00505   // border lines
    00506   addCoords(table,4,x,y);
    00507
    00508   // penalty area
    00509   addCoords(table,3,x + 5,y + 5);
    00510   addCoords(table,3,x + 9,y + 9);
    00511
    00512   // halfWay line
    00513   addCoords(table,1,x + 13,y + 13);
    00514 }
    00515
    00516 void Field::initBorder(Table& table)
    00517 {
    00518   double x[] =
    00519   {
    00520     xPosOwnGoalpost,
    00521     xPosOwnGroundline,
    00522     xPosOwnGroundline,
    00523     xPosOwnSideCorner,
    00524     xPosOpponentSideCorner,
    00525     xPosOpponentGroundline,
    00526     xPosOpponentGroundline,
    00527     xPosOpponentGoalpost,
    00528     // opponent goal
    00529     xPosOpponentGoalpost,
    00530     xPosOpponentGroundline,
    00531     xPosOpponentGroundline,
    00532     xPosOpponentSideCorner,
    00533     xPosOwnSideCorner,
    00534     xPosOwnGroundline,
    00535     xPosOwnGroundline,
    00536     xPosOwnGoalpost
    00537     // own goal
    00538   },
    00539          y[] =
    00540   {
    00541     yPosLeftGoal,
    00542     yPosLeftGoal,
    00543     yPosLeftGroundline,
    00544     yPosLeftSideline,
    00545     yPosLeftSideline,
    00546     yPosLeftGroundline,
    00547     yPosLeftGoal,
    00548     yPosLeftGoal,
    00549     // opponent goal
    00550     yPosRightGoal,
    00551     yPosRightGoal,
    00552     yPosRightGroundline,
    00553     yPosRightSideline,
    00554     yPosRightSideline,
    00555     yPosRightGroundline,
    00556     yPosRightGoal,
    00557     yPosRightGoal
    00558     // own goal
    00559   };
    00560
    00561   table.setSize(sizeof(x) / sizeof(x[0]) - 2);
    00562   addCoords(table,7,x,y);
    00563   addCoords(table,7,x+8,y+8);
    00564 }
    00565
    00566 void Field::initBorderForHugeField(Table& table)
    00567 {
    00568   double x[] =
    00569   {
    00570     xPosOwnGroundline,
    00571     xPosOwnGoal,
    00572     xPosOwnGoal,
    00573     xPosOpponentGoal,
    00574     xPosOpponentGoal,
    00575     xPosOpponentGroundline,
    00576
    00577     xPosOpponentGroundline,
    00578     xPosOpponentGoal,
    00579     xPosOpponentGoal,
    00580     xPosOwnGoal,
    00581     xPosOwnGoal,
    00582     xPosOwnGroundline
    00583   },
    00584          y[] =
    00585   {
    00586     yPosRightGoal,
    00587     yPosRightGoal,
    00588     yPosRightFlags - (int) flagRadius,
    00589     yPosRightFlags - (int) flagRadius,
    00590     yPosRightGoal,
    00591     yPosRightGoal,
    00592
    00593     yPosLeftGoal,
    00594     yPosLeftGoal,
    00595     yPosLeftFlags + (int) flagRadius,
    00596     yPosLeftFlags + (int) flagRadius,
    00597     yPosLeftGoal,
    00598     yPosLeftGoal
    00599   };
    00600
    00601   table.setSize(10 + 4*16);
    00602   addCoords(table, 5, x, y);
    00603   addCoords(table, 5, x + 6, y + 6);
    00604
    00605   // landmarks
    00606   for (int lx = 0; lx < 2; lx++)
    00607     for (int ly = 0; ly < 2; ly++)
    00608     {
    00609       double centerX = (lx == 0) ? xPosFrontFlags : xPosBackFlags;
    00610       double centerY = (ly == 0) ? yPosLeftFlags : yPosRightFlags;
    00611       double x[2], y[2];
    00612       for (double r = 0; r < pi2; r += pi/8)
    00613       {
    00614         if (r > 0)
    00615         {
    00616           x[0] = x[1];
    00617           y[0] = y[1];
    00618         }
    00619         x[1] = centerX + sin(r) * flagRadius;
    00620         y[1] = centerY + cos(r) * flagRadius;
    00621         if (r > 0)
    00622           addCoords(table, 1, x, y);
    00623       }
    00624     }
    00625 }
    00626
    00627 void Field::initOpponentGoal(Table& table)
    00628 {
    00629   double x[] =
    00630   {
    00631     xPosOpponentGoalpost,
    00632     xPosOpponentGoal,
    00633     xPosOpponentGoal,
    00634     xPosOpponentGoalpost
    00635   },
    00636          y[] =
    00637   {
    00638     yPosLeftGoal,
    00639     yPosLeftGoal,
    00640     yPosRightGoal,
    00641     yPosRightGoal
    00642   };
    00643
    00644   table.setSize(sizeof(x) / sizeof(x[0]) - 1);
    00645   addCoords(table,table.maxNumberOfEntries,x,y,true);
    00646 }
    00647
    00648 void Field::initOwnGoal(Table& table)
    00649 {
    00650   double x[] =
    00651   {
    00652     xPosOwnGoalpost,
    00653     xPosOwnGoal,
    00654     xPosOwnGoal,
    00655     xPosOwnGoalpost
    00656   },
    00657          y[] =
    00658   {
    00659     yPosRightGoal,
    00660     yPosRightGoal,
    00661     yPosLeftGoal,
    00662     yPosLeftGoal
    00663   };
    00664
    00665   table.setSize(sizeof(x) / sizeof(x[0]) - 1);
    00666   addCoords(table,table.maxNumberOfEntries,x,y,true);
    00667 }
    00668
    00669 void Field::addCoords(Table& table,int number,double* x,double* y, bool rotate)
    00670 {
    00671   Vector2<double> v1(x[0],y[0]);
    00672   for(int i = 1; i <= number; ++i)
    00673   {
    00674     Vector2<double> v2(x[i],y[i]),
    00675                     vs = (rotate) ? v2 : v1,
    00676                     vd = (rotate) ? v1 - v2 : v2 - v1;
    00677     if(vd.abs())
    00678       table.push(Pose2D(atan2(vd.y,vd.x),vs), vd.abs());
    00679     v1 = v2;
    00680   }
    00681 }
    00682
    00683 void Field::addCoords(Table& table,Table& xTable,Table& yTable,int number,double* x,double* y, bool rotate)
    00684 {
    00685   Vector2<double> v1(x[0],y[0]);
    00686   for(int i = 1; i <= number; ++i)
    00687   {
    00688     Vector2<double> v2(x[i],y[i]),
    00689                     vs = (rotate) ? v2 : v1,
    00690                     vd = (rotate) ? v1 - v2 : v2 - v1;
    00691     if(fabs(vd.x) > 1)
    00692       xTable.push(Pose2D(atan2(vd.y,vd.x),vs), vd.abs());
    00693     if(fabs(vd.y) > 1)
    00694       yTable.push(Pose2D(atan2(vd.y,vd.x),vs), vd.abs());
    00695     if(vd.abs())
    00696       table.push(Pose2D(atan2(vd.y,vd.x),vs), vd.abs());
    00697     v1 = v2;
    00698   }
    00699 }
    00700
    00701 bool Field::isReallyInside(const Vector2<double>& v) const
    00702 {
    00703   if(!isInside(v))
    00704     return false;
    00705 #ifndef HUGE_FIELD
    00706   for(int i = 0; i < lines[LinesPercept::boundary].numberOfEntries; ++i)
    00707   {
    00708     Pose2D diff = Pose2D(v) - lines[LinesPercept::boundary].corner[i];
    00709     if(diff.translation.y > 0 && diff.translation.x >= 0 && diff.translation.x < lines[LinesPercept::boundary].length[i])
    00710       return false;
    00711   }
    00712 #endif
    00713   return true;
    00714 }
    00715
    00716 double Field::clip(Vector2<double>& v) const
    00717 {
    00718   if(isReallyInside(v))
    00719     return 0;
    00720   else
    00721   {
    00722     Vector2<double> old = v,
    00723                           v2;
    00724     double minDist = 100000;
    00725     for(int i = 0; i < lines[LinesPercept::boundary].numberOfEntries; ++i)
    00726     {
    00727       Vector2<double> diff = (Pose2D(old) - lines[LinesPercept::boundary].corner[i]).translation;
    00728       if(diff.x < 0)
    00729         v2 = lines[LinesPercept::boundary].corner[i].translation;
    00730       else if(diff.x > lines[LinesPercept::boundary].length[i])
    00731         v2 = (lines[LinesPercept::boundary].corner[i] + Pose2D(Vector2<double>(lines[LinesPercept::boundary].length[i],0))).translation;
    00732       else
    00733         v2 = (lines[LinesPercept::boundary].corner[i] + Pose2D(Vector2<double>(diff.x,0))).translation;
    00734       double dist = (old - v2).abs();
    00735       if(minDist > dist)
    00736       {
    00737         minDist = dist;
    00738         v = v2;
    00739       }
    00740     }
    00741     return (v - old).abs();
    00742   }
    00743 }
    00744
    00745 Vector2<double> Field::getClosestPoint(const Vector2<double>& v,
    00746                                        LinesPercept::LineType type) const
    00747 {
    00748   const Table& table = lines[type];
    00749   Vector2<double> vMin,
    00750                   v2;
    00751   double minDist = 100000;
    00752   for(int i = 0; i < table.numberOfEntries; ++i)
    00753   {
    00754     Vector2<double> diff = (Pose2D(v) - table.corner[i]).translation;
    00755     if(diff.x < 0)
    00756       v2 = table.corner[i].translation;
    00757     else if(diff.x > table.length[i])
    00758       v2 = (table.corner[i] + Pose2D(Vector2<double>(table.length[i],0))).translation;
    00759     else
    00760       v2 = (table.corner[i] + Pose2D(Vector2<double>(diff.x,0))).translation;
    00761     Vector2<double> vDiff = v2 - v;
    00762     double dist = vDiff.abs();
    00763     if(minDist > dist)
    00764     {
    00765       minDist = dist;
    00766       vMin = v2;
    00767     }
    00768   }
    00769   return vMin;
    00770 }
    00771
    00772 bool Field::getClosestPoint(Vector2<double>& vMin, const Pose2D& p, int numberOfRotations,
    00773                                        LinesPercept::LineType type) const
    00774 {
    00775   // target angle -> target index
    00776   double r = p.rotation / pi2 * numberOfRotations + 0.5;
    00777   if(r < 0)
    00778     r += numberOfRotations;
    00779   int targetRot = int(r);
    00780   ASSERT(targetRot >= 0 && targetRot < numberOfRotations);
    00781   const Table& table = lines[type];
    00782   Vector2<double> v2;
    00783   double minDist = 100000;
    00784   for(int i = 0; i < table.numberOfEntries; ++i)
    00785   {
    00786     // angle -> index
    00787     double r = (table.corner[i].rotation + pi_2) / pi2 * numberOfRotations + 0.5;
    00788     if(r < 0)
    00789       r += numberOfRotations;
    00790     int rot = int(r);
    00791     ASSERT(rot >= 0 && rot < numberOfRotations);
    00792
    00793     // index must be target index
    00794     if(rot == targetRot)
    00795     {
    00796       Vector2<double> diff = (p - table.corner[i]).translation;
    00797       if(diff.x < 0)
    00798         v2 = table.corner[i].translation;
    00799       else if(diff.x > table.length[i])
    00800         v2 = (table.corner[i] + Pose2D(Vector2<double>(table.length[i],0))).translation;
    00801       else
    00802         v2 = (table.corner[i] + Pose2D(Vector2<double>(diff.x,0))).translation;
    00803       Vector2<double> vDiff = v2 - p.translation;
    00804       double dist = vDiff.abs();
    00805       if(minDist > dist)
    00806       {
    00807         minDist = dist;
    00808         vMin = v2;
    00809       }
    00810     }
    00811   }
    00812   return (minDist<100000);
    00813 }
    00814
    00815 double Field::getClosestDistance(const Vector2<double>& v,
    00816                                  LinesPercept::LineType type) const
    00817 {
    00818   return (v - getClosestPoint(v,type)).abs();
    00819 }
    00820
    00821 double Field::getDistance(const Pose2D& pose,
    00822                           LinesPercept::LineType type) const
    00823 {
    00824   const Table& table = lines[type];
    00825   double minDist = 100000;
    00826   for(int i = 0; i < table.numberOfEntries; ++i)
    00827   {
    00828     Vector2<double> v1 = (table.corner[i] - pose).translation,
    00829                     v2 = (table.corner[i] + Pose2D(Vector2<double>(table.length[i],0))
    00830                           - pose).translation;
    00831     if(v1.y < 0 && v2.y > 0 ||
    00832        v1.y > 0 && v2.y < 0)
    00833     {
    00834       double dist = v1.x + (v2.x - v1.x) * -v1.y / (v2.y - v1.y);
    00835       if(dist >= 0 && dist < minDist)
    00836         minDist = dist;
    00837     }
    00838   }
    00839   return minDist == 100000 ? -1 : minDist;
    00840 }
    00841
    00842 double Field::getDistance(const Pose2D& pose,bool ignoreFieldLines) const
    00843 {
    00844   double minDist = 100000;
    00845   for(int i = LinesPercept::numberOfLineTypes - (ignoreFieldLines ? 1 : 0);
    00846       i > 0; --i) // use simpleLines instead of lines
    00847   {
    00848     double dist = getDistance(pose,LinesPercept::LineType(i));
    00849     if(dist != -1 && dist < minDist)
    00850       minDist = dist;
    00851   }
    00852   return minDist == 100000 ? -1 : minDist;
    00853 }
    00854
    00855 double Field::getDistanceToOwnPenaltyArea(const Pose2D& pose) const
    00856 {
    00857   const Table& table = lines[LinesPercept::numberOfLineTypes];
    00858   double minDist = 100000;
    00859 // this ifdef is pretty ugly but necessary since huge field has sidelines in simple field line table
    00860 // accessing fixed indices in line table is a bad idea anyway...  MR
    00861 #ifdef HUGE_FIELD
    00862   for(int i = 4; i < 7; ++i) //own penalty area in simple field line table for huge field
    00863 #else
    00864   for(int i = 2; i < 5; ++i) //own penalty area in simple field line table for small field
    00865 #endif
    00866   {
    00867     Vector2<double> v1 = (table.corner[i] - pose).translation,
    00868                     v2 = (table.corner[i] + Pose2D(Vector2<double>(table.length[i],0))
    00869                           - pose).translation;
    00870     if(v1.y < 0 && v2.y > 0 ||
    00871        v1.y > 0 && v2.y < 0)
    00872     {
    00873       double dist = v1.x + (v2.x - v1.x) * -v1.y / (v2.y - v1.y);
    00874       if(dist >= 0 && dist < minDist)
    00875         minDist = dist;
    00876     }
    00877   }
    00878   return minDist == 100000 ? -1 : minDist;
    00879 }
    00880
    00881 double Field::getObstacleDistance(const Pose2D& pose, ObstaclesPercept::ObstacleType& obstacleType) const
    00882 {
    00883   double minDist = getDistance(pose,LinesPercept::LineType(LinesPercept::numberOfLineTypes + 1));
    00884   LinesPercept::LineType minDistType = LinesPercept::LineType(LinesPercept::numberOfLineTypes + 1);
    00885   if(minDist == -1)
    00886     minDist = 100000;
    00887
    00888   for(int i = LinesPercept::numberOfLineTypes - 1;
    00889     i > 0; // everything but field lines
    00890     --i)
    00891   {
    00892     double dist = getDistance(pose,LinesPercept::LineType(i));
    00893     if(dist != -1 && dist < minDist)
    00894     {
    00895       minDist = dist;
    00896       minDistType = LinesPercept::LineType(i);
    00897     }
    00898   }
    00899
    00900   //minDist = getDistanceToOwnPenaltyArea(pose);
    00901
    00902   switch(minDistType)
    00903   {
    00904   //case LinesPercept::border: obstacleType = ObstaclesPercept::border; break;
    00905   case LinesPercept::redRobot: obstacleType = ObstaclesPercept::opponent; break;
    00906   case LinesPercept::blueRobot: obstacleType = ObstaclesPercept::teammate; break;
    00907   case LinesPercept::yellowGoal: obstacleType = ObstaclesPercept::goal; break;
    00908   case LinesPercept::skyblueGoal: obstacleType = ObstaclesPercept::goal; break;
    00909   default: obstacleType = ObstaclesPercept::unknown;
    00910   }
    00911   return minDist == 100000 ? -1 : minDist;
    00912 }
    00913
    00914 void Field::placePlayers(const PlayerPoseCollection& ppc)
    00915 {
    00916   lines[LinesPercept::numberOfLineTypes + 1].setSize(
    00917     (ppc.numberOfOwnPlayers + ppc.numberOfOpponentPlayers) * 4); // for player boundaries
    00918   for(int i = 0; i < ppc.numberOfOwnPlayers; ++i)
    00919     addPlayer(ppc.getOwnPlayerPose(i).getPose());
    00920   for(int j = 0; j < ppc.numberOfOpponentPlayers; ++j)
    00921     addPlayer(ppc.getOpponentPlayerPose(j).getPose());
    00922 }
    00923
    00924 void Field::addPlayer(const Pose2D& pose)
    00925 {
    00926   // @todo These should be the dimensions of the other robot!
    00927   const RobotDimensions& robotDimensions = getRobotConfiguration().getRobotDimensions();
    00928   double front = robotDimensions.bodyLength / 2 - robotDimensions.lengthNeckToBodyCenter,
    00929          back = front - robotDimensions.bodyLength,
    00930          side = robotDimensions.overallBodyWidth / 2;
    00931   Vector2<double> fl = (pose + Pose2D(Vector2<double>(front, side))).translation,
    00932                   fr = (pose + Pose2D(Vector2<double>(front, -side))).translation,
    00933                   bl = (pose + Pose2D(Vector2<double>(back, side))).translation,
    00934                   br = (pose + Pose2D(Vector2<double>(back, -side))).translation;
    00935   double x[] = {fl.x, fr.x, br.x, bl.x, fl.x},
    00936          y[] = {fl.y, fr.y, br.y, bl.y, fl.y};
    00937   addCoords(lines[LinesPercept::numberOfLineTypes + 1], 4, x, y);
    00938 }
    00939
    00940 Pose2D Field::randomPose() const
    00941 {
    00942   Pose2D pose;
    00943   do
    00944     pose = Pose2D::random(x,y,Range<double>(-pi,pi));
    00945   while(!isReallyInside(pose.translation));
    00946   return pose;
    00947 }
    00948
    00949 void Field::draw(const Drawings::Color color, LinesPercept::LineType type) const
    00950 {
    00951   NDECLARE_DEBUGDRAWING("field", "drawingOnField", "displays the field tables");
    00952   const Table& table = lines[type];
    00953   for(int i = 0; i < table.numberOfEntries; ++i)
    00954   {
    00955     Vector2<double> p = (table.corner[i] +
    00956       Pose2D(Vector2<double>(table.length[i],0))).translation;
    00957     NLINE("field",
    00958        (int) table.corner[i].translation.x,
    00959        (int) table.corner[i].translation.y,
    00960        (int) p.x,
    00961        (int) p.y,
    00962        1, Drawings::ps_solid, color);
    00963     p = (table.corner[i] +
    00964       Pose2D(Vector2<double>(table.length[i]/2,0))).translation;
    00965     Vector2<double> p2 = (table.corner[i] +
    00966       Pose2D(Vector2<double>(table.length[i]/2,100))).translation;
    00967     NLINE("field",
    00968        (int) p2.x,
    00969        (int) p2.y,
    00970        (int) p.x,
    00971        (int) p.y,
    00972        1, Drawings::ps_solid, color);
    00973   }
    00974 }