00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "Field.h"
00010 #include "Tools/Debugging/Debugging.h"
00011 #include "Tools/FieldDimensions.h"
00012 #include "Tools/Player.h"
00013 #include "Tools/RobotConfiguration.h"
00014 #include "Platform/GTAssert.h"
00015
00016 Field::Field()
00017 : Boundary<double>(0,0)
00018 {
00019 add(Vector2<double>(xPosOpponentGoal,yPosLeftSideline));
00020 add(Vector2<double>(xPosOwnGoal,yPosRightSideline));
00021 initBoundary(boundary);
00022 initLines(lines[LinesPercept::field], lines[LinesPercept::numberOfLineTypes + 2], lines[LinesPercept::numberOfLineTypes + 3]);
00023 initBorder(lines[LinesPercept::border]);
00024 initOwnGoal(lines[LinesPercept::yellowGoal]);
00025 initOpponentGoal(lines[LinesPercept::skyblueGoal]);
00026 initSimpleLines(lines[LinesPercept::numberOfLineTypes]);
00027 }
00028
00029 void Field::initBoundary(Table& table)
00030 {
00031 double x[] =
00032 {
00033 xPosOwnGroundline,
00034 xPosOwnSideCorner,
00035 xPosOpponentSideCorner,
00036 xPosOpponentGroundline,
00037 xPosOpponentGroundline,
00038 xPosOpponentGoal - 100,
00039 xPosOpponentGoal - 100,
00040 xPosOpponentGroundline,
00041 xPosOpponentGroundline,
00042 xPosOpponentSideCorner,
00043 xPosOwnSideCorner,
00044 xPosOwnGroundline,
00045 xPosOwnGroundline,
00046 xPosOwnGoal + 100,
00047 xPosOwnGoal + 100,
00048 xPosOwnGroundline,
00049 xPosOwnGroundline
00050 },
00051 y[] =
00052 {
00053 yPosLeftGroundline,
00054 yPosLeftSideline,
00055 yPosLeftSideline,
00056 yPosLeftGroundline,
00057 yPosLeftGoal,
00058 yPosLeftGoal,
00059 yPosRightGoal,
00060 yPosRightGoal,
00061 yPosRightGroundline,
00062 yPosRightSideline,
00063 yPosRightSideline,
00064 yPosRightGroundline,
00065 yPosRightGoal,
00066 yPosRightGoal,
00067 yPosLeftGoal,
00068 yPosLeftGoal,
00069 yPosLeftGroundline
00070 };
00071
00072 table.setSize(sizeof(x) / sizeof(x[0]) - 1);
00073 addCoords(table,table.numberOfEntries,x,y);
00074 }
00075
00076 void Field::initLines(Table& table, Table& xTable, Table& yTable)
00077 {
00078 double x[] =
00079 {
00080
00081 xPosOwnGroundline,
00082 xPosOwnGroundline,
00083 xPosOwnGroundline - 25,
00084 xPosOwnGroundline - 25,
00085
00086
00087 xPosOpponentGroundline,
00088 xPosOpponentGroundline,
00089 xPosOpponentGroundline + 25,
00090 xPosOpponentGroundline + 25,
00091
00092
00093 xPosOwnGroundline,
00094 xPosOwnPenaltyArea - 25,
00095 xPosOwnPenaltyArea - 25,
00096 xPosOwnGroundline,
00097
00098
00099 xPosOwnGroundline,
00100 xPosOwnPenaltyArea,
00101 xPosOwnPenaltyArea,
00102 xPosOwnGroundline,
00103
00104
00105 xPosOpponentGroundline,
00106 xPosOpponentPenaltyArea + 25,
00107 xPosOpponentPenaltyArea + 25,
00108 xPosOpponentGroundline,
00109
00110
00111 xPosOpponentGroundline,
00112 xPosOpponentPenaltyArea,
00113 xPosOpponentPenaltyArea,
00114 xPosOpponentGroundline,
00115
00116
00117 -12.5,-12.5,-52.6,-97.2,-127,-137.5,-127,-97.2,-52.6,-12.5,
00118 12.5,12.5,52.6,97.2,127,137.5,127,97.2,52.6,12.5,
00119
00120
00121 -12.5,-12.5,-62.2,-114.9,-150.1,-162.5,-150.1,-114.9,-62.2,-12.5,-12.5,
00122 12.5,12.5,62.2,114.9,150.1,162.5,150.1,114.9,62.2,12.5,12.5
00123 },
00124 y[] =
00125 {
00126
00127 yPosLeftGoal,
00128 yPosRightGoal,
00129 yPosLeftGoal,
00130 yPosRightGoal,
00131
00132
00133 yPosLeftGoal,
00134 yPosRightGoal,
00135 yPosLeftGoal,
00136 yPosRightGoal,
00137
00138
00139 yPosLeftPenaltyArea - 25,
00140 yPosLeftPenaltyArea - 25,
00141 yPosRightPenaltyArea + 25,
00142 yPosRightPenaltyArea + 25,
00143
00144
00145 yPosLeftPenaltyArea,
00146 yPosLeftPenaltyArea,
00147 yPosRightPenaltyArea,
00148 yPosRightPenaltyArea,
00149
00150
00151 yPosLeftPenaltyArea - 25,
00152 yPosLeftPenaltyArea - 25,
00153 yPosRightPenaltyArea + 25,
00154 yPosRightPenaltyArea + 25,
00155
00156
00157 yPosLeftPenaltyArea,
00158 yPosLeftPenaltyArea,
00159 yPosRightPenaltyArea,
00160 yPosRightPenaltyArea,
00161
00162
00163 137.5,-137.5,-127,-97.2,-52.6,0,52.6,97.2,127,137.5,
00164 137.5,-137.5,-127,-97.2,-52.6,0,52.6,97.2,127,137.5,
00165
00166
00167 yPosRightSideline,-162.5,-150.1,-114.9,-62.2,0,62.2,114.9,150.1,162.5,yPosLeftSideline,
00168 yPosRightSideline,-162.5,-150.1,-114.9,-62.2,0,62.2,114.9,150.1,162.5,yPosLeftSideline
00169 };
00170
00171 table.setSize(54);
00172 xTable.setSize(54);
00173 yTable.setSize(54);
00174
00175
00176 for(int i = 0; i < 4; ++i)
00177 addCoords(table,xTable,yTable,1,x + 2 * i,y + 2 * i);
00178
00179
00180 addCoords(table,xTable,yTable,3,x + 8,y + 8);
00181 addCoords(table,xTable,yTable,3,x + 12,y + 12);
00182
00183
00184 addCoords(table,xTable,yTable,3,x + 16,y + 16);
00185 addCoords(table,xTable,yTable,3,x + 20,y + 20);
00186
00187
00188 addCoords(table,xTable,yTable,9,x + 24,y + 24);
00189 addCoords(table,xTable,yTable,9,x + 34,y + 34);
00190
00191
00192 addCoords(table,xTable,yTable,10,x + 44,y + 44);
00193 addCoords(table,xTable,yTable,10,x + 55,y + 55);
00194
00195 xTable.numberOfEntries = xTable.index;
00196 yTable.numberOfEntries = yTable.index;
00197 }
00198
00199 void Field::initSimpleLines(Table& table)
00200 {
00201 double x[] =
00202 {
00203 xPosOwnGroundline,
00204 xPosOwnGroundline,
00205
00206 xPosOpponentGroundline,
00207 xPosOpponentGroundline,
00208
00209 xPosOwnGroundline + 12,
00210 xPosOwnPenaltyArea,
00211 xPosOwnPenaltyArea,
00212 xPosOwnGroundline + 12,
00213
00214 xPosOpponentGroundline - 12,
00215 xPosOpponentPenaltyArea,
00216 xPosOpponentPenaltyArea,
00217 xPosOpponentGroundline - 12,
00218
00219 xPosHalfWayLine,
00220 xPosHalfWayLine
00221 },
00222 y[] =
00223 {
00224 yPosLeftGoal,
00225 yPosRightGoal,
00226
00227 yPosLeftGoal,
00228 yPosRightGoal,
00229
00230 yPosLeftPenaltyArea,
00231 yPosLeftPenaltyArea,
00232 yPosRightPenaltyArea,
00233 yPosRightPenaltyArea,
00234
00235 yPosLeftPenaltyArea,
00236 yPosLeftPenaltyArea,
00237 yPosRightPenaltyArea,
00238 yPosRightPenaltyArea,
00239
00240 yPosRightSideline,yPosLeftSideline
00241 };
00242
00243 table.setSize(9);
00244
00245
00246 addCoords(table,1,x,y);
00247 addCoords(table,1,x + 2,y + 2);
00248
00249
00250 addCoords(table,3,x + 4,y + 4);
00251 addCoords(table,3,x + 8,y + 8);
00252
00253
00254 addCoords(table,1,x + 12,y + 12);
00255 }
00256
00257 void Field::initBorder(Table& table)
00258 {
00259 double x[] =
00260 {
00261 xPosOwnGoalpost,
00262 xPosOwnGroundline,
00263 xPosOwnGroundline,
00264 xPosOwnSideCorner,
00265 xPosOpponentSideCorner,
00266 xPosOpponentGroundline,
00267 xPosOpponentGroundline,
00268 xPosOpponentGoalpost,
00269
00270 xPosOpponentGoalpost,
00271 xPosOpponentGroundline,
00272 xPosOpponentGroundline,
00273 xPosOpponentSideCorner,
00274 xPosOwnSideCorner,
00275 xPosOwnGroundline,
00276 xPosOwnGroundline,
00277 xPosOwnGoalpost
00278
00279 },
00280 y[] =
00281 {
00282 yPosLeftGoal,
00283 yPosLeftGoal,
00284 yPosLeftGroundline,
00285 yPosLeftSideline,
00286 yPosLeftSideline,
00287 yPosLeftGroundline,
00288 yPosLeftGoal,
00289 yPosLeftGoal,
00290
00291 yPosRightGoal,
00292 yPosRightGoal,
00293 yPosRightGroundline,
00294 yPosRightSideline,
00295 yPosRightSideline,
00296 yPosRightGroundline,
00297 yPosRightGoal,
00298 yPosRightGoal
00299
00300 };
00301
00302 table.setSize(sizeof(x) / sizeof(x[0]) - 2);
00303 addCoords(table,7,x,y);
00304 addCoords(table,7,x+8,y+8);
00305 }
00306
00307 void Field::initOpponentGoal(Table& table)
00308 {
00309 double x[] =
00310 {
00311 xPosOpponentGoalpost,
00312 xPosOpponentGoal,
00313 xPosOpponentGoal,
00314 xPosOpponentGoalpost
00315 },
00316 y[] =
00317 {
00318 yPosLeftGoal,
00319 yPosLeftGoal,
00320 yPosRightGoal,
00321 yPosRightGoal
00322 };
00323
00324 table.setSize(sizeof(x) / sizeof(x[0]) - 1);
00325 addCoords(table,table.numberOfEntries,x,y);
00326 }
00327
00328 void Field::initOwnGoal(Table& table)
00329 {
00330 double x[] =
00331 {
00332 xPosOwnGoalpost,
00333 xPosOwnGoal,
00334 xPosOwnGoal,
00335 xPosOwnGoalpost
00336 },
00337 y[] =
00338 {
00339 yPosRightGoal,
00340 yPosRightGoal,
00341 yPosLeftGoal,
00342 yPosLeftGoal
00343 };
00344
00345 table.setSize(sizeof(x) / sizeof(x[0]) - 1);
00346 addCoords(table,table.numberOfEntries,x,y);
00347 }
00348
00349 void Field::addCoords(Table& table,int number,double* x,double* y)
00350 {
00351 Vector2<double> v1(x[0],y[0]);
00352 for(int i = 1; i <= number; ++i)
00353 {
00354 Vector2<double> v2(x[i],y[i]),
00355 v3 = v2 - v1;
00356 table.push(Pose2D(atan2(v3.y,v3.x),v1), v3.abs());
00357 v1 = v2;
00358 }
00359 }
00360
00361 void Field::addCoords(Table& table,Table& xTable,Table& yTable,int number,double* x,double* y)
00362 {
00363 Vector2<double> v1(x[0],y[0]);
00364 for(int i = 1; i <= number; ++i)
00365 {
00366 Vector2<double> v2(x[i],y[i]),
00367 v3 = v2 - v1;
00368 if(fabs(v3.x) > 1)
00369 xTable.push(Pose2D(atan2(v3.y,v3.x),v1), v3.abs());
00370 if(fabs(v3.y) > 1)
00371 yTable.push(Pose2D(atan2(v3.y,v3.x),v1), v3.abs());
00372 table.push(Pose2D(atan2(v3.y,v3.x),v1), v3.abs());
00373 v1 = v2;
00374 }
00375 }
00376
00377 bool Field::isReallyInside(const Vector2<double>& v) const
00378 {
00379 if(!isInside(v))
00380 return false;
00381 for(int i = 0; i < boundary.numberOfEntries; ++i)
00382 {
00383 Pose2D diff = Pose2D(v) - boundary.corner[i];
00384 if(diff.translation.y > 0 && diff.translation.x >= 0 && diff.translation.x < boundary.length[i])
00385 return false;
00386 }
00387 return true;
00388 }
00389
00390 double Field::clip(Vector2<double>& v) const
00391 {
00392 if(isReallyInside(v))
00393 return 0;
00394 else
00395 {
00396 Vector2<double> old = v,
00397 v2;
00398 double minDist = 100000;
00399 for(int i = 0; i < boundary.numberOfEntries; ++i)
00400 {
00401 Vector2<double> diff = (Pose2D(old) - boundary.corner[i]).translation;
00402 if(diff.x < 0)
00403 v2 = boundary.corner[i].translation;
00404 else if(diff.x > boundary.length[i])
00405 v2 = (boundary.corner[i] + Pose2D(Vector2<double>(boundary.length[i],0))).translation;
00406 else
00407 v2 = (boundary.corner[i] + Pose2D(Vector2<double>(diff.x,0))).translation;
00408 double dist = (old - v2).abs();
00409 if(minDist > dist)
00410 {
00411 minDist = dist;
00412 v = v2;
00413 }
00414 }
00415 return (v - old).abs();
00416 }
00417 }
00418
00419 Vector2<double> Field::getClosestPoint(const Vector2<double>& v,
00420 LinesPercept::LineType type) const
00421 {
00422 const Table& table = lines[type];
00423 Vector2<double> vMin,
00424 v2;
00425 double minDist = 100000;
00426 for(int i = 0; i < table.numberOfEntries; ++i)
00427 {
00428 Vector2<double> diff = (Pose2D(v) - table.corner[i]).translation;
00429 if(diff.x < 0)
00430 v2 = table.corner[i].translation;
00431 else if(diff.x > table.length[i])
00432 v2 = (table.corner[i] + Pose2D(Vector2<double>(table.length[i],0))).translation;
00433 else
00434 v2 = (table.corner[i] + Pose2D(Vector2<double>(diff.x,0))).translation;
00435 Vector2<double> vDiff = v2 - v;
00436 double dist = vDiff.abs();
00437 if(minDist > dist)
00438 {
00439 minDist = dist;
00440 vMin = v2;
00441 }
00442 }
00443 return vMin;
00444 }
00445
00446 Vector2<double> Field::getClosestPoint(const Pose2D& p, int numberOfRotations,
00447 LinesPercept::LineType type) const
00448 {
00449
00450 double r = p.rotation / pi2 * numberOfRotations + 0.5;
00451 if(r < 0)
00452 r += numberOfRotations;
00453 int targetRot = int(r);
00454 ASSERT(targetRot >= 0 && targetRot < numberOfRotations);
00455 int targetRot2 = targetRot - numberOfRotations / 2;
00456 if(targetRot2 < 0)
00457 targetRot2 += numberOfRotations;
00458 const Table& table = lines[type];
00459 Vector2<double> vMin,
00460 v2;
00461 double minDist = 100000;
00462 for(int i = 0; i < table.numberOfEntries; ++i)
00463 {
00464
00465 double r = (table.corner[i].rotation - pi_2) / pi2 * numberOfRotations + 0.5;
00466 if(r < 0)
00467 r += numberOfRotations;
00468 int rot = int(r);
00469 ASSERT(rot >= 0 && rot < numberOfRotations);
00470
00471
00472 if(rot == targetRot || type == LinesPercept::field && rot == targetRot2)
00473 {
00474 double a = normalize(table.corner[i].rotation + pi_2);
00475 Vector2<double> diff = (p - table.corner[i]).translation;
00476 if(diff.x < 0)
00477 v2 = table.corner[i].translation;
00478 else if(diff.x > table.length[i])
00479 v2 = (table.corner[i] + Pose2D(Vector2<double>(table.length[i],0))).translation;
00480 else
00481 v2 = (table.corner[i] + Pose2D(Vector2<double>(diff.x,0))).translation;
00482 Vector2<double> vDiff = v2 - p.translation;
00483 double dist = vDiff.abs();
00484 if(minDist > dist)
00485 {
00486 minDist = dist;
00487 vMin = v2;
00488 }
00489 }
00490 }
00491 return vMin;
00492 }
00493
00494 double Field::getClosestDistance(const Vector2<double>& v,
00495 LinesPercept::LineType type) const
00496 {
00497 return (v - getClosestPoint(v,type)).abs();
00498 }
00499
00500 double Field::getDistance(const Pose2D& pose,
00501 LinesPercept::LineType type) const
00502 {
00503 const Table& table = lines[type];
00504 double minDist = 100000;
00505 for(int i = 0; i < table.numberOfEntries; ++i)
00506 {
00507 Vector2<double> v1 = (table.corner[i] - pose).translation,
00508 v2 = (table.corner[i] + Pose2D(Vector2<double>(table.length[i],0))
00509 - pose).translation;
00510 if(v1.y < 0 && v2.y > 0 ||
00511 v1.y > 0 && v2.y < 0)
00512 {
00513 double dist = v1.x + (v2.x - v1.x) * -v1.y / (v2.y - v1.y);
00514 if(dist >= 0 && dist < minDist)
00515 minDist = dist;
00516 }
00517 }
00518 return minDist == 100000 ? -1 : minDist;
00519 }
00520
00521 double Field::getDistance(const Pose2D& pose,bool ignoreFieldLines) const
00522 {
00523 double minDist = 100000;
00524 for(int i = LinesPercept::numberOfLineTypes - (ignoreFieldLines ? 1 : 0);
00525 i > 0; --i)
00526 {
00527 double dist = getDistance(pose,LinesPercept::LineType(i));
00528 if(dist != -1 && dist < minDist)
00529 minDist = dist;
00530 }
00531 return minDist == 100000 ? -1 : minDist;
00532 }
00533
00534
00535
00536
00537 double Field::getDistanceToOwnPenaltyArea(const Pose2D& pose) const
00538 {
00539 const Table& table = lines[LinesPercept::numberOfLineTypes];
00540 double minDist = 100000;
00541 for(int i = 2; i < 5; ++i)
00542 {
00543 Vector2<double> v1 = (table.corner[i] - pose).translation,
00544 v2 = (table.corner[i] + Pose2D(Vector2<double>(table.length[i],0))
00545 - pose).translation;
00546 if(v1.y < 0 && v2.y > 0 ||
00547 v1.y > 0 && v2.y < 0)
00548 {
00549 double dist = v1.x + (v2.x - v1.x) * -v1.y / (v2.y - v1.y);
00550 if(dist >= 0 && dist < minDist)
00551 minDist = dist;
00552 }
00553 }
00554 return minDist == 100000 ? -1 : minDist;
00555 }
00556
00557
00558 double Field::getObstacleDistance(const Pose2D& pose, ObstaclesPercept::ObstacleType& obstacleType) const
00559 {
00560 double minDist = getDistance(pose,LinesPercept::LineType(LinesPercept::numberOfLineTypes + 1));
00561 LinesPercept::LineType minDistType = LinesPercept::LineType(LinesPercept::numberOfLineTypes + 1);
00562 if(minDist == -1)
00563 minDist = 100000;
00564
00565 for(int i = LinesPercept::numberOfLineTypes - 1;
00566 i > 0;
00567 --i)
00568 {
00569 double dist = getDistance(pose,LinesPercept::LineType(i));
00570 if(dist != -1 && dist < minDist)
00571 {
00572 minDist = dist;
00573 minDistType = LinesPercept::LineType(i);
00574 }
00575 }
00576
00577
00578
00579 switch(minDistType)
00580 {
00581 case LinesPercept::border: obstacleType = ObstaclesPercept::border; break;
00582 case LinesPercept::redRobot: obstacleType = ObstaclesPercept::opponent; break;
00583 case LinesPercept::blueRobot: obstacleType = ObstaclesPercept::teammate; break;
00584 case LinesPercept::yellowGoal: obstacleType = ObstaclesPercept::goal; break;
00585 case LinesPercept::skyblueGoal: obstacleType = ObstaclesPercept::goal; break;
00586 default: obstacleType = ObstaclesPercept::unknown;
00587 }
00588 return minDist == 100000 ? -1 : minDist;
00589 }
00590
00591 void Field::placePlayers(const PlayerPoseCollection& ppc)
00592 {
00593 lines[LinesPercept::numberOfLineTypes + 1].setSize(
00594 (ppc.numberOfOwnPlayers + ppc.numberOfOpponentPlayers) * 4);
00595 for(int i = 0; i < ppc.numberOfOwnPlayers; ++i)
00596 addPlayer(ppc.getOwnPlayerPose(i).getPose());
00597 for(int j = 0; j < ppc.numberOfOpponentPlayers; ++j)
00598 addPlayer(ppc.getOpponentPlayerPose(j).getPose());
00599 }
00600
00601 void Field::addPlayer(const Pose2D& pose)
00602 {
00603
00604 const RobotDimensions& robotDimensions = getRobotConfiguration().getRobotDimensions();
00605 double front = robotDimensions.bodyLength / 2 - robotDimensions.lengthNeckToBodyCenter,
00606 back = front - robotDimensions.bodyLength,
00607 side = robotDimensions.overallBodyWidth / 2;
00608 Vector2<double> fl = (pose + Pose2D(Vector2<double>(front, side))).translation,
00609 fr = (pose + Pose2D(Vector2<double>(front, -side))).translation,
00610 bl = (pose + Pose2D(Vector2<double>(back, side))).translation,
00611 br = (pose + Pose2D(Vector2<double>(back, -side))).translation;
00612 double x[] = {fl.x, fr.x, br.x, bl.x, fl.x},
00613 y[] = {fl.y, fr.y, br.y, bl.y, fl.y};
00614 addCoords(lines[LinesPercept::numberOfLineTypes + 1], 4, x, y);
00615 }
00616
00617 Pose2D Field::randomPose() const
00618 {
00619 Pose2D pose;
00620 do
00621 pose = Pose2D::random(x,y,Range<double>(-pi,pi));
00622 while(!isReallyInside(pose.translation));
00623 return pose;
00624 }
00625
00626 void Field::draw(const Drawings::Color color, LinesPercept::LineType type) const
00627 {
00628 const Table& table = type == LinesPercept::numberOfLineTypes ? boundary : lines[type];
00629 for(int i = 0; i < table.numberOfEntries; ++i)
00630 {
00631 Vector2<double> p = (table.corner[i] +
00632 Pose2D(Vector2<double>(table.length[i],0))).translation;
00633 LINE(selfLocatorField,
00634 (int) table.corner[i].translation.x,
00635 (int) table.corner[i].translation.y,
00636 (int) p.x,
00637 (int) p.y,
00638 1, Drawings::ps_solid, color);
00639 }
00640 }
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760