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

# Tools/FieldDimensions.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file FieldDimensions.cpp
    00003 *
    00004 * Some useful functions regarding field dimensions.
    00005 *
    00006 * @author Max Risler
    00007 */
    00008
    00009 #include "FieldDimensions.h"
    00010
    00011 double FieldDimensions::distanceToOpponentPenaltyArea(const Vector2<double> &p)
    00012 {
    00013   double distanceX, distanceY;
    00014   double minDistance = 0;
    00015
    00016   if ((p.x <= xPosOpponentPenaltyArea) ||
    00017     (p.y < yPosRightPenaltyArea) ||
    00018     (p.y > yPosLeftPenaltyArea))
    00019   {
    00020     if (p.x > xPosOpponentPenaltyArea)
    00021       distanceX = 0;
    00022     else
    00023       distanceX = xPosOpponentPenaltyArea - p.x;
    00024
    00025     if (p.y < yPosRightPenaltyArea)
    00026       distanceY = yPosRightPenaltyArea - p.y;
    00027     else if (p.y > yPosLeftPenaltyArea)
    00028       distanceY = p.y - yPosLeftPenaltyArea;
    00029     else
    00030       distanceY = 0;
    00031     return sqrt(distanceX*distanceX+distanceY*distanceY);
    00032   }
    00033   else
    00034   {
    00035     if ((yPosRightPenaltyArea - p.y) > (p.y - yPosLeftPenaltyArea))
    00036     {
    00037       minDistance = yPosRightPenaltyArea - p.y;
    00038     }
    00039     else
    00040     {
    00041       minDistance = p.y - yPosLeftPenaltyArea;
    00042     }
    00043     if ((xPosOpponentPenaltyArea - p.x) > minDistance)
    00044     {
    00045       minDistance = xPosOpponentPenaltyArea - p.x;
    00046     }
    00047     return minDistance;
    00048   }
    00049 }
    00050
    00051 double FieldDimensions::distanceToOwnPenaltyArea(const Vector2<double> &p)
    00052 {
    00053   Pose2D pose(p);
    00054   return distanceToOwnPenaltyArea(pose);
    00055 }
    00056
    00057 double FieldDimensions::distanceToOwnPenaltyArea(const Pose2D &p)
    00058 {
    00059   double distanceX, distanceY;
    00060   double minDistance = 0;
    00061
    00062
    00063   if ((p.translation.x >= xPosOwnPenaltyArea) ||
    00064     (p.translation.y < yPosRightPenaltyArea) ||
    00065     (p.translation.y > yPosLeftPenaltyArea))
    00066   {
    00067     if (p.translation.x < xPosOwnPenaltyArea)
    00068       distanceX = 0;
    00069     else
    00070       distanceX = p.translation.x - xPosOwnPenaltyArea;
    00071
    00072     if (p.translation.y < yPosRightPenaltyArea)
    00073       distanceY = yPosRightPenaltyArea - p.translation.y;
    00074     else if (p.translation.y > yPosLeftPenaltyArea)
    00075       distanceY = p.translation.y - yPosLeftPenaltyArea;
    00076     else
    00077       distanceY = 0;
    00078
    00079     return sqrt(distanceX*distanceX+distanceY*distanceY);
    00080   }
    00081   else
    00082   {
    00083     if ((yPosRightPenaltyArea - p.translation.y) >
    00084       (p.translation.y - yPosLeftPenaltyArea))
    00085     {
    00086       minDistance = yPosRightPenaltyArea - p.translation.y;
    00087     }
    00088     else
    00089     {
    00090       minDistance = p.translation.y - yPosLeftPenaltyArea;
    00091     }
    00092     if ((p.translation.x - xPosOwnPenaltyArea) > minDistance)
    00093     {
    00094       minDistance = p.translation.x - xPosOwnPenaltyArea;
    00095     }
    00096     return minDistance;
    00097   }
    00098 }
    00099
    00100
    00101 bool FieldDimensions::isInsideField(const Vector2<int> &p)
    00102 {
    00103   if (
    00104     p.x > xPosOwnGoal && p.x < xPosOpponentGoal &&
    00105     p.y > yPosRightGoal && p.y < yPosLeftGoal)
    00106     //inside rectangle between goals
    00107     return true;
    00108
    00109   if (
    00110     p.x <= xPosOwnGroundline || p.x >= xPosOpponentGroundline ||
    00111     p.y <= yPosRightSideline || p.y >= yPosLeftSideline)
    00112     // outside the field rectancle
    00113     return false;
    00114
    00115   // check corners
    00116   if (Geometry::getDistanceToLine(ownLeftCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
    00117   if (Geometry::getDistanceToLine(opponentLeftCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
    00118   if (Geometry::getDistanceToLine(ownRightCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
    00119   if (Geometry::getDistanceToLine(opponentRightCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
    00120
    00121   return true;
    00122 }
    00123
    00124 bool FieldDimensions::isInsideGoal(const Vector2<int> &p)
    00125 {
    00126   return (
    00127     p.y > yPosRightGoal && p.y < yPosLeftGoal &&
    00128     (
    00129     p.x > xPosOwnGoal && p.x < xPosOwnGroundline
    00130     ||
    00131     p.x > xPosOpponentGoal && p.x < xPosOpponentGroundline
    00132     )
    00133     );
    00134 }
    00135
    00136 bool FieldDimensions::isInsideOwnGoal(const Vector2<int> &p)
    00137 {
    00138   return (
    00139     p.y > yPosRightGoal && p.y < yPosLeftGoal &&
    00140     p.x > xPosOwnGoal && p.x < xPosOwnGroundline
    00141     );
    00142 }
    00143
    00144 bool FieldDimensions::isInsideOpponentGoal(const Vector2<int> &p)
    00145 {
    00146   return (
    00147     p.y > yPosRightGoal && p.y < yPosLeftGoal &&
    00148     p.x < xPosOpponentGoal && p.x > xPosOpponentGroundline
    00149     );
    00150 }
    00151
    00152
    00153 double FieldDimensions::distanceToBorder(const Vector2<double> &p)
    00154 {
    00155   if (!isInsideField(Vector2<int>((int)p.x, (int)p.y))) return 0.0;
    00156
    00157   double distance;
    00158   Vector2<double> pAbs(fabs(p.x), fabs(p.y));
    00159
    00160   distance = yPosLeftSideline - pAbs.y;
    00161   distance = min(distance, Geometry::getDistanceToLine(opponentLeftCorner, pAbs));
    00162   distance = min(distance, Geometry::getDistanceToEdge(opponentLeftGroundline, pAbs));
    00163   distance = min(distance, Geometry::getDistanceToEdge(opponentGoalLeftWall, pAbs));
    00164   distance = min(distance, xPosOpponentGoal - pAbs.x);
    00165
    00166   return distance;
    00167 }
    00168
    00169 void FieldDimensions::vectorToBorder(const Vector2<double> &p, Vector2<double> &result)
    00170 {
    00171   if (!isInsideField(Vector2<int>((int)p.x, (int)p.y))) return;
    00172
    00173   double minDistance;
    00174   double distance;
    00175   const Geometry::Line *pNearestBorder;
    00176
    00177   minDistance = Geometry::getDistanceToLine(leftWall, p);
    00178   pNearestBorder = &leftWall;
    00179
    00180   distance = Geometry::getDistanceToLine(opponentLeftCorner, p);
    00181   if ( distance < minDistance )
    00182   {
    00183     minDistance = distance;
    00184     pNearestBorder = &opponentLeftCorner;
    00185   }
    00186
    00187   distance = Geometry::getDistanceToLine(opponentLeftGroundline, p);
    00188   if ( distance < minDistance )
    00189   {
    00190     minDistance = distance;
    00191     pNearestBorder = &opponentLeftGroundline;
    00192   }
    00193
    00194   distance = Geometry::getDistanceToLine(opponentRightCorner, p);
    00195   if ( distance < minDistance )
    00196   {
    00197     minDistance = distance;
    00198     pNearestBorder = &opponentRightCorner;
    00199   }
    00200
    00201   distance = Geometry::getDistanceToLine(rightWall, p);
    00202   if ( distance < minDistance )
    00203   {
    00204     minDistance = distance;
    00205     pNearestBorder = &rightWall;
    00206   }
    00207
    00208   distance = Geometry::getDistanceToLine(ownRightCorner, p);
    00209   if ( distance < minDistance )
    00210   {
    00211     minDistance = distance;
    00212     pNearestBorder = &ownRightCorner;
    00213   }
    00214
    00215   distance = Geometry::getDistanceToLine(ownLeftGroundline, p);
    00216   if ( distance < minDistance )
    00217   {
    00218     minDistance = distance;
    00219     pNearestBorder = &ownLeftGroundline;
    00220   }
    00221
    00222   distance = Geometry::getDistanceToLine(ownLeftCorner, p);
    00223   if ( distance < minDistance )
    00224   {
    00225     minDistance = distance;
    00226     pNearestBorder = &ownLeftCorner;
    00227   }
    00228
    00229   result.x = -pNearestBorder->direction.y;
    00230   result.y = pNearestBorder->direction.x;
    00231   result.normalize();
    00232   result *= minDistance;
    00233
    00234   return;
    00235 }
    00236
    00237 void FieldDimensions::vectorToBorderIncludingGoals(const Vector2<double> &p, Vector2<double> &result)
    00238 {
    00239   if (!isInsideField(Vector2<int>((int)p.x, (int)p.y))) return;
    00240
    00241   double minDistance;
    00242   double distance;
    00243   const Geometry::Line *pNearestBorder;
    00244
    00245   minDistance = Geometry::getDistanceToLine(leftWall, p);
    00246   pNearestBorder = &leftWall;
    00247
    00248   distance = Geometry::getDistanceToLine(opponentLeftCorner, p);
    00249   if ( distance < minDistance )
    00250   {
    00251     minDistance = distance;
    00252     pNearestBorder = &opponentLeftCorner;
    00253   }
    00254
    00255   distance = Geometry::getDistanceToLine(opponentLeftGroundline, p);
    00256   if ( distance < minDistance )
    00257   {
    00258     minDistance = distance;
    00259     pNearestBorder = &opponentLeftGroundline;
    00260   }
    00261
    00262   distance = Geometry::getDistanceToLine(opponentRightCorner, p);
    00263   if ( distance < minDistance )
    00264   {
    00265     minDistance = distance;
    00266     pNearestBorder = &opponentRightCorner;
    00267   }
    00268
    00269   distance = Geometry::getDistanceToLine(rightWall, p);
    00270   if ( distance < minDistance )
    00271   {
    00272     minDistance = distance;
    00273     pNearestBorder = &rightWall;
    00274   }
    00275
    00276   distance = Geometry::getDistanceToLine(ownRightCorner, p);
    00277   if ( distance < minDistance )
    00278   {
    00279     minDistance = distance;
    00280     pNearestBorder = &ownRightCorner;
    00281   }
    00282
    00283   distance = Geometry::getDistanceToLine(ownLeftGroundline, p);
    00284   if ( distance < minDistance )
    00285   {
    00286     minDistance = distance;
    00287     pNearestBorder = &ownLeftGroundline;
    00288   }
    00289
    00290   distance = Geometry::getDistanceToLine(ownLeftCorner, p);
    00291   if ( distance < minDistance )
    00292   {
    00293     minDistance = distance;
    00294     pNearestBorder = &ownLeftCorner;
    00295   }
    00296
    00297   if(isInsideOwnGoal(Vector2<int>((int)p.x, (int)p.y)))
    00298   {
    00299     minDistance = 2000;
    00300     distance = Geometry::getDistanceToLine(ownGoalRightWall, p);
    00301     if ( distance < minDistance )
    00302     {
    00303       minDistance = distance;
    00304       pNearestBorder = &ownGoalRightWall;
    00305     }
    00306
    00307     distance = Geometry::getDistanceToLine(ownGoalRearWall, p);
    00308     if ( distance < minDistance )
    00309     {
    00310       minDistance = distance;
    00311       pNearestBorder = &ownGoalRearWall;
    00312     }
    00313
    00314     distance = Geometry::getDistanceToLine(ownGoalLeftWall, p);
    00315     if ( distance < minDistance )
    00316     {
    00317       minDistance = distance;
    00318       pNearestBorder = &ownGoalLeftWall;
    00319     }
    00320   }
    00321   if(isInsideOpponentGoal(Vector2<int>((int)p.x, (int)p.y)))
    00322   {
    00323     minDistance = 2000;
    00324     distance = Geometry::getDistanceToLine(opponentGoalRightWall, p);
    00325     if ( distance < minDistance )
    00326     {
    00327       minDistance = distance;
    00328       pNearestBorder = &opponentGoalRightWall;
    00329     }
    00330
    00331     distance = Geometry::getDistanceToLine(opponentGoalRearWall, p);
    00332     if ( distance < minDistance )
    00333     {
    00334       minDistance = distance;
    00335       pNearestBorder = &opponentGoalRearWall;
    00336     }
    00337
    00338     distance = Geometry::getDistanceToLine(opponentGoalLeftWall, p);
    00339     if ( distance < minDistance )
    00340     {
    00341       minDistance = distance;
    00342       pNearestBorder = &opponentGoalLeftWall;
    00343     }
    00344   }
    00345   result.x = -pNearestBorder->direction.y;
    00346   result.y = pNearestBorder->direction.x;
    00347   result.normalize();
    00348   result *= minDistance;
    00349
    00350   return;
    00351 }
    00352
    00353
    00354 bool FieldDimensions::isOnOwnGoalGroundline(const Vector2<int> &p)
    00355 {
    00356   return (
    00357     p.x == xPosOwnGroundline &&
    00358     p.y < yPosLeftGoal &&
    00359     p.y > yPosRightGoal
    00360     );
    00361 }
    00362
    00363 bool FieldDimensions::isOnOpponentGoalGroundline(const Vector2<int> &p)
    00364 {
    00365   return (
    00366     p.x == xPosOpponentGroundline &&
    00367     p.y < yPosLeftGoal &&
    00368     p.y > yPosRightGoal
    00369     );
    00370 }
    00371
    00372 bool FieldDimensions::clipLineWithField
    00373 (
    00374  Vector2<int>& point1,
    00375  Vector2<int>& point2
    00376  )
    00377 {
    00378   bool clipped;
    00379
    00380   Vector2<int> newPoint1 = point1;
    00381   Vector2<int> newPoint2 = point2;
    00382
    00383   Vector2<int> bottomLeftField(xPosOwnGroundline, yPosRightSideline);
    00384   Vector2<int> topRightField(xPosOpponentGroundline, yPosLeftSideline);
    00385
    00386   clipped = Geometry::clipLineWithRectangleCohenSutherland(bottomLeftField, topRightField, newPoint1, newPoint2);
    00387
    00388   Geometry::Line lineToClip(point1, point2 - point1);
    00389   /*  if () return false;
    00390   if (Geometry::getDistanceToLine(opponentLeftCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
    00391   if (Geometry::getDistanceToLine(ownRightCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
    00392   if (Geometry::getDistanceToLine(opponentRightCorner,Vector2<double>(p.x, p.y)) <= 0) return false;
    00393   */
    00394   //point1 inside own corners
    00395   //right
    00396   if(Geometry::getDistanceToLine(ownRightCorner,Vector2<double>(newPoint1.x, newPoint1.y)) <= 0)
    00397   {
    00398     Geometry::getIntersectionOfLines(lineToClip, ownRightCorner, newPoint1);
    00399     clipped = true;
    00400   }
    00401   //left
    00402   else if(Geometry::getDistanceToLine(ownLeftCorner,Vector2<double>(newPoint1.x, newPoint1.y)) <= 0)
    00403   {
    00404     Geometry::getIntersectionOfLines(lineToClip, ownLeftCorner, newPoint1);
    00405     clipped = true;
    00406   }
    00407   //point inside opponent corners
    00408   //right
    00409   else if(Geometry::getDistanceToLine(opponentRightCorner,Vector2<double>(newPoint1.x, newPoint1.y)) <= 0)
    00410   {
    00411     Geometry::getIntersectionOfLines(lineToClip, opponentRightCorner, newPoint1);
    00412     clipped = true;
    00413   }
    00414   //left
    00415   else if(Geometry::getDistanceToLine(opponentLeftCorner,Vector2<double>(newPoint1.x, newPoint1.y)) <= 0)
    00416   {
    00417     Geometry::getIntersectionOfLines(lineToClip, opponentLeftCorner, newPoint1);
    00418     clipped = true;
    00419   }
    00420
    00421   //point2  inside own corners
    00422   //right
    00423   if(Geometry::getDistanceToLine(ownRightCorner,Vector2<double>(newPoint2.x, newPoint2.y)) <= 0)
    00424   {
    00425     Geometry::getIntersectionOfLines(lineToClip, ownRightCorner, newPoint2);
    00426     clipped = true;
    00427   }
    00428   //left
    00429   else if(Geometry::getDistanceToLine(ownLeftCorner,Vector2<double>(newPoint2.x, newPoint2.y)) <= 0)
    00430   {
    00431     Geometry::getIntersectionOfLines(lineToClip, ownLeftCorner, newPoint2);
    00432     clipped = true;
    00433   }
    00434
    00435   //point inside opponent corners
    00436   //right
    00437   else if(Geometry::getDistanceToLine(opponentRightCorner,Vector2<double>(newPoint2.x, newPoint2.y)) <= 0)
    00438   {
    00439     Geometry::getIntersectionOfLines(lineToClip, opponentRightCorner, newPoint2);
    00440     clipped = true;
    00441   }
    00442   //left
    00443   else if(Geometry::getDistanceToLine(opponentLeftCorner,Vector2<double>(newPoint2.x, newPoint2.y)) <= 0)
    00444   {
    00445     Geometry::getIntersectionOfLines(lineToClip, opponentLeftCorner, newPoint2);
    00446     clipped = true;
    00447   }
    00448
    00449   point1 = newPoint1;
    00450   point2 = newPoint2;
    00451   return clipped;
    00452 }
    00453
    00454 bool FieldDimensions::clipLineWithFieldAndGoalAreas
    00455 (
    00456  Vector2<int>& point1,
    00457  Vector2<int>& point2
    00458  )
    00459 {
    00460   bool clipped = false;
    00461
    00462   Vector2<int> newPoint1 = point1;
    00463   Vector2<int> newPoint2 = point2;
    00464
    00465
    00466   // point 1 is inside a goal
    00467   if(isInsideOwnGoal(point1))
    00468   {
    00469     Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, newPoint1, newPoint2);
    00470     if(isOnOwnGoalGroundline(newPoint2))
    00471     {
    00472       Vector2<int> point2InField = point2;
    00473       clipLineWithField(newPoint2, point2InField);
    00474       if(isOnOpponentGoalGroundline(point2InField))
    00475       {
    00476         Vector2<int> point2InGoal = point2;
    00477         Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, point2InField, point2InGoal);
    00478         newPoint2 = point2InGoal;
    00479       }
    00480       else
    00481       {
    00482         newPoint2 = point2InField;
    00483       }
    00484     }
    00485     clipped = true;
    00486   }
    00487   else if(isInsideOpponentGoal(point1))
    00488   {
    00489     Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, newPoint1, newPoint2);
    00490     if(isOnOpponentGoalGroundline(newPoint2))
    00491     {
    00492       Vector2<int> point2InField = point2;
    00493       clipLineWithField(newPoint2, point2InField);
    00494       if(isOnOwnGoalGroundline(point2InField))
    00495       {
    00496         Vector2<int> point2InGoal = point2;
    00497         Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, point2InField, point2InGoal);
    00498         newPoint2 = point2InGoal;
    00499       }
    00500       else
    00501       {
    00502         newPoint2 = point2InField;
    00503       }
    00504     }
    00505     clipped = true;
    00506   }
    00507
    00508   // point 2 is inside a goal
    00509   if(isInsideOwnGoal(point2))
    00510   {
    00511     Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, newPoint1, newPoint2);
    00512     if(isOnOwnGoalGroundline(newPoint1))
    00513     {
    00514       Vector2<int> point1InField = point1;
    00515       clipLineWithField(newPoint1, point1InField);
    00516       if(isOnOpponentGoalGroundline(point1InField))
    00517       {
    00518         Vector2<int> point1InGoal = point1;
    00519         Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, point1InField, point1InGoal);
    00520         newPoint1 = point1InGoal;
    00521       }
    00522       else
    00523       {
    00524         newPoint1 = point1InField;
    00525       }
    00526     }
    00527     clipped = true;
    00528   }
    00529   else if(isInsideOpponentGoal(point2))
    00530   {
    00531     Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, newPoint1, newPoint2);
    00532     if(isOnOpponentGoalGroundline(newPoint1))
    00533     {
    00534       Vector2<int> point1InField = point1;
    00535       clipLineWithField(newPoint1, point1InField);
    00536       if(isOnOwnGoalGroundline(point1InField))
    00537       {
    00538         Vector2<int> point1InGoal = point1;
    00539         Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, point1InField, point1InGoal);
    00540         newPoint1 = point1InGoal;
    00541       }
    00542       else
    00543       {
    00544         newPoint1 = point1InField;
    00545       }
    00546     }
    00547     clipped = true;
    00548   }
    00549
    00550   if(!clipped)
    00551   {
    00552     Vector2<int> bottomLeftField(xPosOwnGroundline, yPosRightSideline);
    00553     Vector2<int> topRightField(xPosOpponentGroundline, yPosLeftSideline);
    00554
    00555     clipped = clipLineWithField(newPoint1, newPoint2);
    00556
    00557     // Line intersects groudline at own goal
    00558     if(newPoint1.x == xPosOwnGroundline && newPoint1.y < yPosLeftGoal && newPoint1.y > yPosRightGoal)
    00559     {
    00560       Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, newPoint1, point1);
    00561       newPoint1 = point1;
    00562       clipped = true;
    00563     }
    00564     if(newPoint2.x == xPosOwnGroundline && newPoint2.y < yPosLeftGoal && newPoint2.y > yPosRightGoal )
    00565     {
    00566       Geometry::clipLineWithRectangleCohenSutherland(ownGoalRightRearPanel, ownGoalLeftPost, newPoint2, point2);
    00567       newPoint2 = point2;
    00568       clipped = true;
    00569     }
    00570     // Line intersects groudline at opponent goal
    00571     if(newPoint1.x == xPosOpponentGroundline && newPoint1.y < yPosLeftGoal && newPoint1.y > yPosRightGoal)
    00572     {
    00573       Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, newPoint1, point1);
    00574       newPoint1 = point1;
    00575       clipped = true;
    00576     }
    00577     if(isOnOpponentGoalGroundline(newPoint2))
    00578     {
    00579       Geometry::clipLineWithRectangleCohenSutherland(opponentGoalRightPost, opponentGoalLeftRearPanel, newPoint2, point2);
    00580       newPoint2 = point2;
    00581       clipped = true;
    00582     }
    00583
    00584   }
    00585
    00586   point1 = newPoint1;
    00587   point2 = newPoint2;
    00588   return clipped;
    00589 }
    00590