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

# Tools/Math/Geometry.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Math/Geometry.cpp
    00003 * Implemets class Geometry
    00004 *
    00005 * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Jüngel</A>
    00006 * @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
    00007 */
    00008
    00009 #include "Geometry.h"
    00010 #include "Tools/FieldDimensions.h"
    00011 #include "Tools/Debugging/Debugging.h"
    00012 #include "Tools/RobotConfiguration.h"
    00013
    00014 Geometry::Circle::Circle() : radius(0)
    00015 {
    00016 }
    00017
    00018 double Geometry::angleTo(const Pose2D& from,
    00019                                 const Vector2<double>& to)
    00020 {
    00021   Pose2D relPos = Pose2D(to) - from;
    00022   return atan2(relPos.translation.y,relPos.translation.x);
    00023 }
    00024
    00025 double Geometry::distanceTo(const Pose2D& from,
    00026                                    const Vector2<double>& to)
    00027 {
    00028   return (Pose2D(to) - from).translation.abs();
    00029 }
    00030
    00031 Vector2<double> Geometry::vectorTo(const Pose2D& from, const Vector2<double>& to)
    00032 {
    00033   return (Pose2D(to) - from).translation;
    00034 }
    00035
    00036 void Geometry::Line::normalizeDirection()
    00037 {
    00038   double distance =
    00039     sqrt(sqr(direction.x) + sqr(direction.y));
    00040   direction.x = direction.x / distance;
    00041   direction.y = direction.y / distance;
    00042 }
    00043
    00044 Geometry::Circle Geometry::getCircle
    00045 (
    00046  const Vector2<int> point1,
    00047  const Vector2<int> point2,
    00048  const Vector2<int> point3
    00049  )
    00050 {
    00051   double x1 = point1.x;
    00052   double y1 = point1.y;
    00053   double x2 = point2.x;
    00054   double y2 = point2.y;
    00055   double x3 = point3.x;
    00056   double y3 = point3.y;
    00057
    00058   Circle circle;
    00059
    00060   if((x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3 == 0) )
    00061   {
    00062     circle.radius = 0;
    00063   }
    00064   else
    00065   {
    00066     circle.radius =
    00067       0.5 *
    00068       sqrt(
    00069       ( (sqr(x1 - x2) + sqr(y1 - y2) ) *
    00070       (sqr(x1 - x3) + sqr(y1 - y3) ) *
    00071       (sqr(x2 - x3) + sqr(y2 - y3) )
    00072       )
    00073       /
    00074       sqr(x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3)
    00075       );
    00076   }
    00077
    00078   if( (2 * (-x2*y1 + x3*y1 + x1*y2 - x3*y2 - x1*y3 + x2*y3) == 0) )
    00079   {
    00080     circle.center.x = 0;
    00081   }
    00082   else
    00083   {
    00084     circle.center.x =
    00085       (
    00086       sqr(x3) * (y1 - y2) +
    00087       (sqr(x1) + (y1 - y2) * (y1 - y3)) * (y2 - y3) +
    00088       sqr(x2) * (-y1 + y3)
    00089       )
    00090       /
    00091       (2 * (-x2*y1 + x3*y1 + x1*y2 - x3*y2 - x1*y3 + x2*y3) );
    00092   }
    00093
    00094   if((2 * (x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3) == 0) )
    00095   {
    00096     circle.center.y = 0;
    00097   }
    00098   else
    00099   {
    00100     circle.center.y =
    00101       (
    00102       sqr(x1) * (x2 - x3) +
    00103       sqr(x2) * x3 +
    00104       x3*(-sqr(y1) + sqr(y2) ) -
    00105       x2*(+sqr(x3) - sqr(y1) + sqr(y3) ) +
    00106       x1*(-sqr(x2) + sqr(x3) - sqr(y2) + sqr(y3) )
    00107       )
    00108       /
    00109       (2 * (x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3) );
    00110   }
    00111   return circle;
    00112 }
    00113
    00114 bool Geometry::getIntersectionOfLines
    00115 (
    00116  const Line line1,
    00117  const Line line2,
    00118  Vector2<int>& intersection
    00119  )
    00120 {
    00121  Vector2<double> intersectionDouble;
    00122  bool toReturn = getIntersectionOfLines(line1,line2,intersectionDouble);
    00123  intersection.x = (int)intersectionDouble.x;
    00124  intersection.y = (int)intersectionDouble.y;
    00125  return toReturn;
    00126 }
    00127
    00128 bool Geometry::getIntersectionOfLines
    00129 (
    00130  const Line line1,
    00131  const Line line2,
    00132  Vector2<double>& intersection
    00133  )
    00134 {
    00135   if(line1.direction.y * line2.direction.x == line1.direction.x * line2.direction.y)
    00136   {
    00137     return false;
    00138   }
    00139
    00140   intersection.x =
    00141     line1.base.x +
    00142     line1.direction.x *
    00143     (
    00144     line1.base.y * line2.direction.x -
    00145     line2.base.y * line2.direction.x +
    00146     (-line1.base.x + line2.base.x) * line2.direction.y
    00147     )
    00148     /
    00149     ( (-line1.direction.y) * line2.direction.x + line1.direction.x * line2.direction.y );
    00150
    00151   intersection.y =
    00152     line1.base.y +
    00153     line1.direction.y *
    00154     (
    00155     -line1.base.y * line2.direction.x +
    00156     line2.base.y * line2.direction.x +
    00157     (line1.base.x - line2.base.x) * line2.direction.y
    00158     )
    00159     /
    00160     (line1.direction.y * line2.direction.x - line1.direction.x * line2.direction.y);
    00161
    00162   return true;
    00163 }
    00164
    00165 bool Geometry::getIntersectionOfRaysFactor
    00166 (
    00167  const Line ray1,
    00168  const Line ray2,
    00169  double& factor
    00170  )
    00171 {
    00172   double divisor = ray2.direction.x * ray1.direction.y - ray1.direction.x * ray2.direction.y;
    00173   if(divisor==0)
    00174   {
    00175     return false;
    00176   }
    00177   double k=(ray2.direction.y*ray1.base.x - ray2.direction.y*ray2.base.x - ray2.direction.x*ray1.base.y + ray2.direction.x*ray2.base.y)/divisor;
    00178   double l=(ray1.direction.y*ray1.base.x - ray1.direction.y*ray2.base.x - ray1.direction.x*ray1.base.y + ray1.direction.x*ray2.base.y)/divisor;
    00179   if ((k>=0)&&(l>=0)&&(k<=1)&&(l<=1))
    00180   {
    00181     //intersection=ray1.base + ray1.direction*k;
    00182     factor=k;
    00183     return true;
    00184   }
    00185   return false;
    00186 }
    00187
    00188 double Geometry::getDistanceToLine
    00189 (
    00190  const Line line,
    00191  const Vector2<double>& point
    00192  )
    00193 {
    00194   if (line.direction.x == 0 && line.direction.y == 0)
    00195     return distance(point, line.base);
    00196
    00197   Vector2<double> normal;
    00198   normal.x = line.direction.y;
    00199   normal.y = -line.direction.x;
    00200   normal.normalize();
    00201
    00202   double c = normal * line.base;
    00203
    00204   return normal * point - c;
    00205 }
    00206
    00207 double Geometry::getDistanceToEdge
    00208 (
    00209  const Line line,
    00210  const Vector2<double>& point
    00211  )
    00212 {
    00213   if (line.direction.x == 0 && line.direction.y == 0)
    00214     return distance(point, line.base);
    00215
    00216   double c = line.direction * line.base;
    00217
    00218   double d = (line.direction * point - c) / (line.direction * line.direction);
    00219
    00220   if (d < 0)
    00221     return distance(point, line.base);
    00222   else if (d > 1.0)
    00223     return distance(point, line.base + line.direction);
    00224   else
    00225     return fabs(getDistanceToLine(line, point));
    00226 }
    00227
    00228
    00229 double Geometry::distance
    00230 (
    00231  const Vector2<double>& point1,
    00232  const Vector2<double>& point2
    00233  )
    00234 {
    00235   return (point2 - point1).abs();
    00236 }
    00237
    00238 double Geometry::distance
    00239 (
    00240  const Vector2<int>& point1,
    00241  const Vector2<int>& point2
    00242  )
    00243 {
    00244   return (point2 - point1).abs();
    00245 }
    00246
    00247 double Geometry::calculateScaleFactor(
    00248   int y,
    00249   unsigned long frameNumber,
    00250   unsigned long prevFrameNumber,
    00251   const CameraInfo& cameraInfo
    00252   )
    00253 {
    00254   if(frameNumber != prevFrameNumber)
    00255   {
    00256     const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00257     double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime; // in seconds
    00258     double imageDiff = timeDiff * r.imagesPerSecond; // how many images are between the old and the current one (should be 1)?
    00259     return (1 - double(1 + y) / cameraInfo.resolutionHeight) / imageDiff * 0.9; // interpolate, 10% less
    00260   }
    00261   else
    00262     return 0;
    00263 }
    00264
    00265 Vector3<double> Geometry::rayFromCamera(
    00266   int y,
    00267   const CameraMatrix& cameraMatrix,
    00268   const CameraMatrix& prevCameraMatrix,
    00269   const Vector3<double> vector,
    00270   const CameraInfo& cameraInfo
    00271   )
    00272 {
    00273   Vector3<double> v1 = cameraMatrix.rotation * vector;
    00274   if(!cameraInfo.simulated)
    00275   {
    00276     Vector3<double> v2 = prevCameraMatrix.rotation * vector;
    00277     // shift current percept towards old percept based on image row
    00278     v1 += (v2 - v1) * calculateScaleFactor(y, cameraMatrix.frameNumber, prevCameraMatrix.frameNumber, cameraInfo);
    00279   }
    00280   return v1;
    00281 }
    00282
    00283 void Geometry::calculateAnglesForPoint
    00284 (
    00285  const Vector2<int>& point,
    00286  const CameraMatrix& cameraMatrix,
    00287  const CameraInfo& cameraInfo,
    00288  Vector2<double>& angles
    00289  )
    00290 {
    00291   double factor = cameraInfo.focalLength;
    00292
    00293   Vector3<double> vectorToPoint(
    00294     factor,
    00295     cameraInfo.opticalCenter.x - point.x,
    00296     cameraInfo.opticalCenter.y - point.y);
    00297
    00298   Vector3<double> vectorToPointWorld =
    00299     cameraMatrix.rotation * vectorToPoint;
    00300
    00301   angles.x =
    00302     atan2(vectorToPointWorld.y,vectorToPointWorld.x);
    00303
    00304   angles.y =
    00305     atan2(
    00306     vectorToPointWorld.z,
    00307     sqrt(sqr(vectorToPointWorld.x) + sqr(vectorToPointWorld.y))
    00308     );
    00309 }
    00310
    00311 void Geometry::calculateAnglesForPoint
    00312 (
    00313  const Vector2<int>& point,
    00314  const CameraMatrix& cameraMatrix,
    00315  const CameraMatrix& prevCameraMatrix,
    00316  const CameraInfo& cameraInfo,
    00317  Vector2<double>& angles
    00318  )
    00319 {
    00320   calculateAnglesForPoint(point, cameraMatrix, cameraInfo, angles);
    00321   if(!cameraInfo.simulated)
    00322   {
    00323     Vector2<double> angles2;
    00324     calculateAnglesForPoint(point, prevCameraMatrix, cameraInfo, angles2);
    00325     // shift current percept towards old percept based on image row
    00326     angles += (angles2 - angles) * calculateScaleFactor(point.y, cameraMatrix.frameNumber, prevCameraMatrix.frameNumber, cameraInfo);
    00327   }
    00328 }
    00329
    00330 void Geometry::calculatePointByAngles
    00331 (
    00332  const Vector2<double>& angles,
    00333  const CameraMatrix& cameraMatrix,
    00334  const CameraInfo& cameraInfo,
    00335  Vector2<int>& point
    00336  )
    00337 {
    00338   Vector3<double> vectorToPointWorld, vectorToPoint;
    00339   vectorToPointWorld.x = cos(angles.x);
    00340   vectorToPointWorld.y = sin(angles.x);
    00341   vectorToPointWorld.z = tan(angles.y);
    00342
    00343   RotationMatrix rotationMatrix = cameraMatrix.rotation;
    00344   vectorToPoint = rotationMatrix.invert() * vectorToPointWorld;
    00345
    00346   double factor = cameraInfo.focalLength;
    00347
    00348   double scale = factor / vectorToPoint.x;
    00349
    00350   point.x = (int)(0.5 + cameraInfo.opticalCenter.x - vectorToPoint.y * scale);
    00351   point.y = (int)(0.5 + cameraInfo.opticalCenter.y  - vectorToPoint.z * scale);
    00352 }
    00353
    00354
    00355 bool Geometry::clipLineWithQuadrangle
    00356 (
    00357  const Line lineToClip,
    00358  const Vector2<double>& corner0,
    00359  const Vector2<double>& corner1,
    00360  const Vector2<double>& corner2,
    00361  const Vector2<double>& corner3,
    00362  Vector2<int>& clipPoint1,
    00363  Vector2<int>& clipPoint2
    00364  )
    00365 {
    00366   Vector2<double> point1, point2;
    00367   bool toReturn = clipLineWithQuadrangle(
    00368    lineToClip,
    00369    corner0, corner1, corner2, corner3,
    00370    point1, point2);
    00371    clipPoint1.x = (int)point1.x;
    00372    clipPoint1.y = (int)point1.y;
    00373    clipPoint2.x = (int)point2.x;
    00374    clipPoint2.y = (int)point2.y;
    00375    return toReturn;
    00376 }
    00377
    00378 bool Geometry::clipLineWithQuadrangle
    00379 (
    00380  const Line lineToClip,
    00381  const Vector2<double>& corner0,
    00382  const Vector2<double>& corner1,
    00383  const Vector2<double>& corner2,
    00384  const Vector2<double>& corner3,
    00385  Vector2<double>& clipPoint1,
    00386  Vector2<double>& clipPoint2
    00387  )
    00388 {
    00389   Geometry::Line side[4] , verticalLine;
    00390
    00391   verticalLine.base = lineToClip.base;
    00392
    00393   verticalLine.direction.x = -lineToClip.direction.y;
    00394   verticalLine.direction.y = lineToClip.direction.x;
    00395
    00396   Vector2<double> corner[4];
    00397   corner[0] = corner0;
    00398   corner[1] = corner1;
    00399   corner[2] = corner2;
    00400   corner[3] = corner3;
    00401
    00402   side[0].base = corner0;
    00403   side[0].direction = corner1;
    00404
    00405   side[1].base = corner1;
    00406   side[1].direction = corner3;
    00407
    00408   side[2].base = corner2;
    00409   side[2].direction = corner1;
    00410
    00411   side[3].base = corner3;
    00412   side[3].direction = corner3;
    00413
    00414   Vector2<double> point1, point2, point;
    00415   bool nextIsPoint1 = true;
    00416   /*
    00417   for(int i = 0; i < 4; i++)
    00418   {
    00419     if(Geometry::getIntersectionOfLines(side[i], lineToClip, point))
    00420     {
    00421       if(nextIsPoint1 && sign(point.x - corner[i]) != sign(point.x - corner[(i+1)%4])
    00422         )
    00423       {
    00424         point1 = point;
    00425         nextIsPoint1 = false;
    00426       }
    00427       else
    00428         point2 = point;
    00429     };
    00430   }*/
    00431
    00432   if(Geometry::getIntersectionOfLines(side[0], lineToClip, point))
    00433   {
    00434     if(corner[0].x < point.x && point.x < corner[1].x)
    00435     {
    00436       if(nextIsPoint1)
    00437       {
    00438         point1 = point;
    00439         nextIsPoint1 = false;
    00440       }
    00441     }
    00442   }
    00443
    00444   if(Geometry::getIntersectionOfLines(side[1], lineToClip, point))
    00445   {
    00446     if(corner[1].y < point.y && point.y < corner[2].y)
    00447     {
    00448       if(nextIsPoint1)
    00449       {
    00450         point1 = point;
    00451         nextIsPoint1 = false;
    00452       }
    00453       else
    00454       point2 = point;
    00455     }
    00456   }
    00457
    00458   if(Geometry::getIntersectionOfLines(side[2], lineToClip, point))
    00459   {
    00460     if(corner[2].x > point.x && point.x > corner[3].x)
    00461     {
    00462       if(nextIsPoint1)
    00463       {
    00464         point1 = point;
    00465         nextIsPoint1 = false;
    00466       }
    00467       else
    00468       point2 = point;
    00469     }
    00470   }
    00471
    00472   if(Geometry::getIntersectionOfLines(side[3], lineToClip, point))
    00473   {
    00474     if(corner[3].y > point.y && point.y > corner[0].y)
    00475     {
    00476       if(nextIsPoint1)
    00477       {
    00478         point1 = point;
    00479         nextIsPoint1 = false;
    00480       }
    00481       else
    00482       point2 = point;
    00483     }
    00484   }
    00485
    00486   if(nextIsPoint1)
    00487     return false;
    00488
    00489   if(getDistanceToLine(verticalLine, point1) < getDistanceToLine(verticalLine, point2) )
    00490   {
    00491     clipPoint1 = point1;
    00492     clipPoint2 = point2;
    00493   }
    00494   else
    00495   {
    00496     clipPoint1 = point2;
    00497     clipPoint2 = point1;
    00498   }
    00499   return true;
    00500 }
    00501
    00502 bool Geometry::isPointInsideRectangle
    00503 (
    00504  const Vector2<double>& bottomLeftCorner,
    00505  const Vector2<double>& topRightCorner,
    00506  const Vector2<double>& point
    00507 )
    00508 {
    00509   return(
    00510     bottomLeftCorner.x <= point.x && point.x <= topRightCorner.x &&
    00511     bottomLeftCorner.y <= point.y && point.y <= topRightCorner.y
    00512     );
    00513 }
    00514
    00515 bool Geometry::isPointInsideRectangle
    00516 (
    00517  const Vector2<int>& bottomLeftCorner,
    00518  const Vector2<int>& topRightCorner,
    00519  const Vector2<int>& point
    00520 )
    00521 {
    00522   return(
    00523     bottomLeftCorner.x <= point.x && point.x <= topRightCorner.x &&
    00524     bottomLeftCorner.y <= point.y && point.y <= topRightCorner.y
    00525     );
    00526 }
    00527
    00528 bool Geometry::clipPointInsideRectange(
    00529   const Vector2<int>& bottomLeftCorner,
    00530   const Vector2<int>& topRightCorner,
    00531   Vector2<int>& point
    00532 )
    00533 {
    00534   bool clipped = false;
    00535   if (point.x < bottomLeftCorner.x)
    00536   {
    00537     point.x = bottomLeftCorner.x;
    00538     clipped = true;
    00539   }
    00540   if (point.x > topRightCorner.x)
    00541   {
    00542     point.x = topRightCorner.x;
    00543     clipped = true;
    00544   }
    00545   if (point.y < bottomLeftCorner.y)
    00546   {
    00547     point.y = bottomLeftCorner.y;
    00548     clipped = true;
    00549   }
    00550   if (point.y > topRightCorner.y)
    00551   {
    00552     point.y = topRightCorner.y;
    00553     clipped = true;
    00554   }
    00555   return clipped;
    00556 }
    00557
    00558 bool Geometry::clipPointInsideRectange(
    00559   const Vector2<int>& bottomLeftCorner,
    00560   const Vector2<int>& topRightCorner,
    00561   Vector2<double>& point
    00562 )
    00563 {
    00564   bool clipped = false;
    00565   if (point.x < bottomLeftCorner.x)
    00566   {
    00567     point.x = (double)bottomLeftCorner.x;
    00568     clipped = true;
    00569   }
    00570   if (point.x > topRightCorner.x)
    00571   {
    00572     point.x = (double)topRightCorner.x;
    00573     clipped = true;
    00574   }
    00575   if (point.y < bottomLeftCorner.y)
    00576   {
    00577     point.y = (double)bottomLeftCorner.y;
    00578     clipped = true;
    00579   }
    00580   if (point.y > topRightCorner.y)
    00581   {
    00582     point.y = (double)topRightCorner.y;
    00583     clipped = true;
    00584   }
    00585   return clipped;
    00586 }
    00587
    00588 bool Geometry::calculatePointOnField
    00589 (
    00590  const int x,
    00591  const int y,
    00592  const CameraMatrix& cameraMatrix,
    00593  const CameraInfo& cameraInfo,
    00594  Vector2<double>& pointOnField
    00595  )
    00596 {
    00597 #ifndef _WIN32 // don't recalculate on real robot
    00598   static
    00599 #endif
    00600   double xFactor = cameraInfo.focalLengthInv,
    00601          yFactor = cameraInfo.focalLengthInv;
    00602
    00603   Vector3<double>
    00604     vectorToCenter(1, (cameraInfo.opticalCenter.x - x) * xFactor, (cameraInfo.opticalCenter.y - y) * yFactor);
    00605
    00606   Vector3<double>
    00607     vectorToCenterWorld = cameraMatrix.rotation * vectorToCenter;
    00608
    00609   //Is the point above the horizon ? - return
    00610   if(vectorToCenterWorld.z > -5 * yFactor) return false;
    00611
    00612   double a1 = cameraMatrix.translation.x,
    00613          a2 = cameraMatrix.translation.y,
    00614          a3 = cameraMatrix.translation.z,
    00615          b1 = vectorToCenterWorld.x,
    00616          b2 = vectorToCenterWorld.y,
    00617          b3 = vectorToCenterWorld.z;
    00618
    00619   pointOnField.x = (a1 * b3 - a3 * b1) / b3 + 0.5;
    00620   pointOnField.y = (a2 * b3 - a3 * b2) / b3 + 0.5;
    00621
    00622   return fabs(pointOnField.x) < 10000 && fabs(pointOnField.y) < 10000;
    00623 }
    00624
    00625 bool Geometry::calculatePointOnField
    00626 (
    00627  const int x,
    00628  const int y,
    00629  const CameraMatrix& cameraMatrix,
    00630  const CameraMatrix& prevCameraMatrix,
    00631  const CameraInfo& cameraInfo,
    00632  Vector2<double>& pointOnField
    00633  )
    00634 {
    00635   if(cameraInfo.simulated)
    00636     return calculatePointOnField(x, y, cameraMatrix, cameraInfo, pointOnField);
    00637   else
    00638   {
    00639     Vector2<double> pointOnField2;
    00640     if(calculatePointOnField(x, y, cameraMatrix, cameraInfo, pointOnField) &&
    00641        calculatePointOnField(x, y, prevCameraMatrix, cameraInfo, pointOnField2))
    00642     {
    00643       double scaleFactor = calculateScaleFactor(y, cameraMatrix.frameNumber, prevCameraMatrix.frameNumber, cameraInfo);
    00644       // shift current percept towards old percept based on image row
    00645       pointOnField.x += ((pointOnField2.x - pointOnField.x) * scaleFactor);
    00646       pointOnField.y += ((pointOnField2.y - pointOnField.y) * scaleFactor);
    00647       return true;
    00648     }
    00649     else
    00650       return false;
    00651   }
    00652 }
    00653
    00654 void Geometry::calculatePointInImage
    00655 (
    00656  const Vector2<int>& point,
    00657  const CameraMatrix& cameraMatrix,
    00658  const CameraInfo& cameraInfo,
    00659  Vector2<int>& pointInImage
    00660  )
    00661 {
    00662   Vector2<double> offset(point.x - cameraMatrix.translation.x,
    00663                          point.y - cameraMatrix.translation.y);
    00664   calculatePointByAngles(
    00665     Vector2<double>(atan2(offset.y,offset.x),
    00666                     -atan2(cameraMatrix.translation.z,offset.abs())),
    00667     cameraMatrix, cameraInfo,
    00668     pointInImage
    00669   );
    00670 }
    00671
    00672 /*
    00673 not tested
    00674 bool Geometry::ExpandOrClipLineWithRectangleCohenSutherland
    00675 (
    00676  const Vector2<int>& bottomLeft,
    00677  const Vector2<int>& topRight,
    00678  Vector2<int>& point1,
    00679  Vector2<int>& point2
    00680  )
    00681 {
    00682   while(!isPointInsideRectangle(bottomLeft, topRight, point1) )
    00683   {
    00684     point1 -= (point2 - point1);
    00685   }
    00686   while(!isPointInsideRectangle(bottomLeft, topRight, point2) )
    00687   {
    00688     point2 -= (point1 - point2);
    00689   }
    00690   return(clipLineWithRectangleCohenSutherland(bottomLeft, topRight, point1, point2));
    00691 }
    00692 */
    00693
    00694 bool Geometry::getIntersectionPointsOfLineAndRectangle(
    00695  const Vector2<int>& bottomLeft,
    00696  const Vector2<int>& topRight,
    00697  const Geometry::Line line,
    00698  Vector2<int>& point1,
    00699  Vector2<int>& point2
    00700 )
    00701 {
    00702   int foundPoints=0;
    00703   Vector2<double> point[2];
    00704   if (line.direction.x!=0)
    00705   {
    00706     double y1=line.base.y+(bottomLeft.x-line.base.x)*line.direction.y/line.direction.x;
    00707     if ((y1>=bottomLeft.y)&&(y1<=topRight.y))
    00708     {
    00709       point[foundPoints].x=bottomLeft.x;
    00710       point[foundPoints++].y=y1;
    00711     }
    00712     double y2=line.base.y+(topRight.x-line.base.x)*line.direction.y/line.direction.x;
    00713     if ((y2>=bottomLeft.y)&&(y2<=topRight.y))
    00714     {
    00715       point[foundPoints].x=topRight.x;
    00716       point[foundPoints++].y=y2;
    00717     }
    00718   }
    00719   if (line.direction.y!=0)
    00720   {
    00721     double x1=line.base.x+(bottomLeft.y-line.base.y)*line.direction.x/line.direction.y;
    00722     if ((x1>=bottomLeft.x)&&(x1<=topRight.x)&&(foundPoints<2))
    00723     {
    00724       point[foundPoints].x=x1;
    00725       point[foundPoints].y=bottomLeft.y;
    00726       if ((foundPoints==0)||((point[0]-point[1]).abs()>0.1))
    00727       {
    00728         foundPoints++;
    00729       }
    00730     }
    00731     double x2=line.base.x+(topRight.y-line.base.y)*line.direction.x/line.direction.y;
    00732     if ((x2>=bottomLeft.x)&&(x2<=topRight.x)&&(foundPoints<2))
    00733     {
    00734       point[foundPoints].x=x2;
    00735       point[foundPoints].y=topRight.y;
    00736       if ((foundPoints==0)||((point[0]-point[1]).abs()>0.1))
    00737       {
    00738         foundPoints++;
    00739       }
    00740     }
    00741   }
    00742   switch (foundPoints)
    00743   {
    00744   case 1:
    00745     point1.x=(int)point[0].x;
    00746     point2.x=point1.x;
    00747     point1.y=(int)point[0].y;
    00748     point2.y=point1.y;
    00749     foundPoints++;
    00750     return true;
    00751   case 2:
    00752     if ((point[1]-point[0])*line.direction >0)
    00753     {
    00754       point1.x=(int)point[0].x;
    00755       point1.y=(int)point[0].y;
    00756       point2.x=(int)point[1].x;
    00757       point2.y=(int)point[1].y;
    00758     }
    00759     else
    00760     {
    00761       point1.x=(int)point[1].x;
    00762       point1.y=(int)point[1].y;
    00763       point2.x=(int)point[0].x;
    00764       point2.y=(int)point[0].y;
    00765     }
    00766     return true;
    00767   default:
    00768     return false;
    00769   }
    00770 }
    00771
    00772 bool Geometry::clipLineWithRectangleCohenSutherland
    00773 (
    00774  const Vector2<int>& bottomLeft,
    00775  const Vector2<int>& topRight,
    00776  Vector2<int>& point1,
    00777  Vector2<int>& point2
    00778  )
    00779 {
    00780 //  bool point2WasRight = point2.x > point1.x;
    00781 //  bool point2WasTop = point2.y > point1.y;
    00782
    00783   int swap = 0;
    00784
    00785   Vector2<int> p;
    00786   int c1 = cohenSutherlandOutCode(bottomLeft, topRight, point1);
    00787   int c2 = cohenSutherlandOutCode(bottomLeft, topRight, point2);
    00788
    00789   if(c1 == 0 && c2 == 0) return false;
    00790
    00791   while ( ( !(c1 == 0) || !(c2 == 0) ) && ( (c1&c2) == 0 ) )
    00792   {
    00793     if ( c1 == 0 )
    00794     {
    00795       p = point1;
    00796       point1 = point2;
    00797       point2 = p;
    00798       c1 = c2;
    00799       swap++;
    00800     }
    00801     if (c1 & 1)
    00802     {
    00803       point1.y = intersection(point1.x,point1.y,point2.x,point2.y,bottomLeft.x) ;
    00804       point1.x = bottomLeft.x ;
    00805     }
    00806     else if (c1&2)
    00807     {
    00808       point1.y = intersection(point1.x,point1.y,point2.x,point2.y,topRight.x) ;
    00809       point1.x = topRight.x ;
    00810     }
    00811     else if (c1&4)
    00812     {
    00813       point1.x = intersection(point1.y,point1.x,point2.y,point2.x,bottomLeft.y) ;
    00814       point1.y = bottomLeft.y ;
    00815     }
    00816     else if (c1&8)
    00817     {
    00818       point1.x = intersection(point1.y,point1.x,point2.y,point2.x,topRight.y) ;
    00819       point1.y = topRight.y ;
    00820     }
    00821     c1 = cohenSutherlandOutCode(bottomLeft, topRight, point1);
    00822     c2 = cohenSutherlandOutCode(bottomLeft, topRight, point2);
    00823   }
    00824   if ( c1 == 0 && c2 == 0 )
    00825   {
    00826 /*    if(
    00827       (point2WasTop && point2.y <= point1.y) ||
    00828       (!point2WasTop && point2.y > point1.y) ||
    00829       (point2WasRight && point2.x <= point1.x) ||
    00830       (!point2WasRight && point2.x > point1.x)
    00831       )
    00832 */
    00833     if(swap % 2 == 1)
    00834     {
    00835       p = point1 ;
    00836       point1 = point2;
    00837       point2 = p ;
    00838     }
    00839     return true;
    00840   }
    00841   else
    00842     return false;
    00843 }
    00844
    00845 int Geometry::intersection(int a1, int b1, int a2, int b2, int value)
    00846 {
    00847   int result = 0 ;
    00848   if ( a2 - a1 != 0 )
    00849     result = (int) (b1 +(double) (value-a1) / (a2-a1) * (b2-b1));
    00850   else
    00851     result = 32767;
    00852   return(result);
    00853 }
    00854
    00855 Vector2<double> Geometry::relative2FieldCoord(RobotPose rp, double x, double y)
    00856 {
    00857   Vector3<double> relativePos(x, y, 0.0);
    00858   Vector2<double> fieldPos(0.0, 0.0);
    00859   RotationMatrix R;
    00860
    00861   R.fromKardanRPY(0,0, -rp.rotation);
    00862   relativePos = R * relativePos;
    00863
    00864   fieldPos.x = relativePos.x + rp.translation.x;
    00865   fieldPos.y = relativePos.y + rp.translation.y;
    00866
    00867   return fieldPos;
    00868 }
    00869
    00870 Vector2<double> Geometry::fieldCoord2Relative(RobotPose robotPose, Vector2<double> fieldCoord)
    00871 {
    00872   Vector2<double> relativeCoord;
    00873   double distance = Geometry::distanceTo(robotPose.getPose(), fieldCoord);
    00874   double angle = Geometry::angleTo(robotPose.getPose(), fieldCoord);
    00875   relativeCoord.x = distance * cos(angle);
    00876   relativeCoord.y = distance * sin(angle);
    00877   return relativeCoord;
    00878 }
    00879
    00880
    00881 bool Geometry::calculateBallInImage(const Vector2<double>& ballOffset,
    00882                                     const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo, Circle& circle)
    00883 {
    00884   Vector2<double> offset(ballOffset.x - cameraMatrix.translation.x,
    00885                          ballOffset.y - cameraMatrix.translation.y);
    00886   double distance = offset.abs(),
    00887          height = cameraMatrix.translation.z - ballRadius,
    00888          cameraDistance = sqrt(sqr(distance) + sqr(height));
    00889   circle.center = Vector2<double>(atan2(offset.y,offset.x),-atan2(height,distance));
    00890   if(cameraDistance >= ballRadius)
    00891   {
    00892     double alpha = pi_2 - circle.center.y - acos(ballRadius / cameraDistance),
    00893            yBottom = -atan2(height + cos(alpha) * ballRadius,
    00894                             distance - sin(alpha) * ballRadius),
    00895            beta = pi_2 - circle.center.y + acos(ballRadius / cameraDistance),
    00896            yTop = -atan2(height + cos(beta) * ballRadius,
    00897                          distance - sin(beta) * ballRadius);
    00898     Vector2<int> top,
    00899                  bottom;
    00900     calculatePointByAngles(Vector2<double>(circle.center.x,yTop),cameraMatrix, cameraInfo, top);
    00901     calculatePointByAngles(Vector2<double>(circle.center.x,yBottom),cameraMatrix, cameraInfo, bottom);
    00902     circle.center.x = (top.x + bottom.x) / 2.0;
    00903     circle.center.y = (top.y + bottom.y) / 2.0;
    00904     circle.radius = (top - bottom).abs() / 2.0;
    00905     return true;
    00906   }
    00907   else
    00908     return false;
    00909 }
    00910
    00911 double Geometry::angleSizeToPixelSize(double angleSize, const CameraInfo& cameraInfo)
    00912 {
    00913   return cameraInfo.focalLength * tan(angleSize);
    00914 }
    00915
    00916 double Geometry::pixelSizeToAngleSize(double pixelSize, const CameraInfo& cameraInfo)
    00917 {
    00918   return atan(pixelSize * cameraInfo.focalLengthInv);
    00919 }
    00920
    00921 double Geometry::getDistanceBySize
    00922 (
    00923  const CameraInfo& cameraInfo,
    00924  double sizeInReality,
    00925  double sizeInPixels
    00926  )
    00927 {
    00928   double xFactor = cameraInfo.focalLength;
    00929   return sizeInReality * xFactor / (sizeInPixels + 0.00000001);
    00930 }
    00931
    00932 double Geometry::getDistanceBySize
    00933 (
    00934  const CameraInfo& cameraInfo,
    00935  double sizeInReality,
    00936  double sizeInPixels,
    00937  double centerX,
    00938  double centerY
    00939  )
    00940 {
    00941   double mx = centerX;
    00942   double my = centerY;
    00943   double cx = cameraInfo.opticalCenter.x;
    00944   double cy = cameraInfo.opticalCenter.y;
    00945   double focalLenPow2 = cameraInfo.focalLenPow2;
    00946   double sqrImgRadius = (mx-cx)*(mx-cx) + (my-cy)*(my-cy);
    00947   double imgDistance = sqrt(focalLenPow2 + sqrImgRadius);
    00948   return imgDistance*sizeInReality/(sizeInPixels + 0.00000001);
    00949 }
    00950
    00951 double Geometry::getDistanceByAngleSize
    00952 (
    00953  double sizeInReality,
    00954  double sizeAsAngle
    00955  )
    00956 {
    00957   return (sizeInReality / 2.0) / tan(sizeAsAngle / 2.0 + 0.00000001);
    00958 }
    00959
    00960 double Geometry::getBallDistanceByAngleSize
    00961 (
    00962  double sizeInReality,
    00963  double sizeAsAngle
    00964  )
    00965 {
    00966   return (sizeInReality / 2.0) / sin(sizeAsAngle / 2.0 + 0.00000001);
    00967 }
    00968
    00969 double Geometry::getSizeByDistance
    00970 (
    00971  const CameraInfo& cameraInfo,
    00972  double sizeInReality,
    00973  double distance
    00974 )
    00975 {
    00976   double xFactor = cameraInfo.focalLength;
    00977   return sizeInReality / distance * xFactor;
    00978 }
    00979
    00980
    00981 double Geometry::getSizeByDistance
    00982 (
    00983  double sizeInReality,
    00984  double distance,
    00985  double imageWidthPixels,
    00986  double imageWidthAngle
    00987 )
    00988 {
    00989   double xFactor = imageWidthPixels / tan(imageWidthAngle / 2.0) / 2.0;
    00990 //  return int(sizeInReality / double(distance - distancePanCenterToCamera) * xFactor);
    00991   return sizeInReality / distance * xFactor;
    00992 }
    00993
    00994 Geometry::Line Geometry::calculateHorizon
    00995 (
    00996 const CameraMatrix& cameraMatrix,
    00997 const CameraInfo& cameraInfo
    00998 )
    00999 {
    01000   Line horizon;
    01001   double r31 = cameraMatrix.rotation.c[0].z;
    01002   double r32 = cameraMatrix.rotation.c[1].z;
    01003   double r33 = cameraMatrix.rotation.c[2].z;
    01004
    01005   if(r33 == 0)
    01006     r33 = 0.00001;
    01007
    01008   double x1 = 0,
    01009          x2 = cameraInfo.resolutionWidth - 1,
    01010          v1 = cameraInfo.focalLength,
    01011          v2 = cameraInfo.opticalCenter.x,
    01012          v3 = cameraInfo.opticalCenter.y,
    01013          y1 = (v3 * r33 + r31 * v1 + r32 * v2) / r33,
    01014          y2 = (v3 * r33 + r31 * v1 - r32 * v2) / r33;
    01015
    01016   // Mirror ends of horizon if Camera rotated to the left
    01017   if((cameraMatrix.rotation * Vector3<double>(0,0,1)).z < 0)
    01018   {
    01019     double t = x1;
    01020     x1 = x2;
    01021     x2 = t;
    01022     t = y1;
    01023     y1 = y2;
    01024     y2 = t;
    01025   }
    01026
    01027   horizon.base.x = (x1 + x2) / 2.0;
    01028   horizon.base.y = (y1 + y2) / 2.0;
    01029   horizon.direction.x = x2 - x1;
    01030   horizon.direction.y = y2 - y1;
    01031   horizon.normalizeDirection();
    01032   return horizon;
    01033 }
    01034
    01035
    01036 int Geometry::calculateLineSize
    01037 (
    01038  const Vector2<int>& pointInImage,
    01039  const CameraMatrix& cameraMatrix,
    01040  const CameraInfo& cameraInfo
    01041  )
    01042 {
    01043   Vector2<int> pointOnField; //position on field, relative to robot
    01044   if(Geometry::calculatePointOnField(pointInImage.x, pointInImage.y, cameraMatrix, cameraInfo, pointOnField))
    01045   {
    01046     int distance = (int) sqrt(sqr(cameraMatrix.translation.z) + sqr(pointOnField.abs()));
    01047     return (int)Geometry::getSizeByDistance(cameraInfo, 25, distance);
    01048   }
    01049   else
    01050   {
    01051     return 0;
    01052   }
    01053 }
    01054
    01055 Geometry::CorrectedCoords Geometry::radialCorrectionLUT[cameraResolutionHeight_ERS7][cameraResolutionWidth_ERS7];
    01056
    01057 void Geometry::setupRadialCorrection(const CameraInfo& cameraInfo)
    01058 {
    01059   double corrX, corrY;
    01060   unsigned char cX, cY;
    01061   for(int x=0; x<cameraInfo.resolutionWidth; x++)
    01062     for(int y=0; y<cameraInfo.resolutionHeight; y++)
    01063     {
    01064       radialDistortionCorrection(cameraInfo, x, y, corrX, corrY);
    01065       cX = (unsigned char) (corrX+0.5);
    01066       cY = (unsigned char) (corrY+0.5);
    01067       radialCorrectionLUT[y][x].x = cX;
    01068       radialCorrectionLUT[y][x].y = cY;
    01069     }
    01070 }