On the way to quality of circle intersections

gtsam/geometry/Point2.cpp

@@ -69,37 +69,58 @@ double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
 }
 
 /* ************************************************************************* */
+// Calculate h, the distance of the intersections of two circles from the center line.
+// This is a dimensionless fraction of the distance d between the circle centers,
+// and also determines how "good" the intersection is. If the circles do not intersect
+// or they are identical, returns boost::none. If one solution, h -> 0.
+// @param R_d : R/d, ratio of radius of first circle to distance between centers
+// @param r_d : r/d, ratio of radius of second circle to distance between centers
+// @param tol: absolute tolerance below which we consider touching circles
+// Math inspired by http://paulbourke.net/geometry/circlesphere/
+static boost::optional<double> circleCircleQuality(double R_d, double r_d, double tol=1e-9) {
+
+  double R2_d2 = R_d*R_d; // Yes, RD-D2 !
+  double f = 0.5 + 0.5*(R2_d2 - r_d*r_d);
+  double h2 = R2_d2 - f*f;
+
+  // h^2<0 is equivalent to (d > (R + r) || d < (R - r))
+  // Hence, there are only solutions if >=0
+  if (h2<-tol) return boost::none; // allow *slightly* negative
+  else if (h2<tol) return 0.0; // one solution
+  else return sqrt(h2); // two solutions
+}
+
+/* ************************************************************************* */
+// Math inspired by http://paulbourke.net/geometry/circlesphere/
 list<Point2> Point2::CircleCircleIntersection(double R, Point2 c, double r) {
 
   list<Point2> solutions;
 
-  // Math inspired by http://paulbourke.net/geometry/circlesphere/
+  // distance between circle centers.
-  // Changed to avoid sqrt in case there are 0 or 1 intersections, and only one div
+  double d2 = c.x() * c.x() + c.y() * c.y(), d = sqrt(d2);
 
-  // squared distance between circle centers.
+  // circles coincide, either no solution or infinite number of solutions.
-  double d2 = c.x() * c.x() + c.y() * c.y();
+  if (d2<1e-9) return solutions;
 
-  // Check for solutions
+  // Calculate h, the distance of the intersections from the center line,
-  double R2 = R*R;
+  // as a dimensionless fraction of  the distance d.
-  double b = R2 - r*r + d2;
+  // It is the solution of a quadratic, so it has either 2 solutions, is 0, or none
-  double b2 = b*b;
+  double _d = 1.0/d, R_d = R*_d, r_d=r*_d;
+  boost::optional<double> h = circleCircleQuality(R_d,r_d);
 
-  // Return empty list if no solution
+  // If h== boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r)
-  // test below is equivalent to h2>0 == !(d > (R + r) || d < (R - r))
+  if (h) {
-  if (4*R2*d2 >= b2) {
     // Determine p2, the point where the line through the circle
-    // intersection points crosses the Line between the circle centers.
+    // intersection points crosses the line between the circle centers.
-    double i2 = 1.0/d2;
+    double f = 0.5 + 0.5*(R_d*R_d - r_d*r_d);
-    double f = 0.5*b*i2;
     Point2 p2 = f * c;
 
-    // if touching, just return one point
+    // If h == 0, the circles are touching, so just return one point
-    double h2 = R2 - 0.25*b2*i2;
+    if (h==0.0)
-    if (h2 < 1e-9)
       solutions.push_back(p2);
     else {
       // determine the offsets of the intersection points from p
-      Point2 offset = sqrt(h2*i2) * Point2(-c.y(), c.x());
+      Point2 offset = (*h) * Point2(-c.y(), c.x());
 
       // Determine the absolute intersection points.
       solutions.push_back(p2 + offset);
@@ -109,6 +130,51 @@ list<Point2> Point2::CircleCircleIntersection(double R, Point2 c, double r) {
   return solutions;
 }
 
+//list<Point2> Point2::CircleCircleIntersection(double R, Point2 c, double r) {
+//
+//  list<Point2> solutions;
+//
+//  // Math inspired by http://paulbourke.net/geometry/circlesphere/
+//  // Changed to avoid sqrt in case there are 0 or 1 intersections, and only one div
+//
+//  // squared distance between circle centers.
+//  double d2 = c.x() * c.x() + c.y() * c.y();
+//
+//  // A crucial quantity we compute is h, a the distance of the intersections
+//  // from the center line, as a dimensionless fraction of  the distance d.
+//  // It is the solution of a quadratic, so it has either 2 solutions, is 0, or none
+//  // We calculate it as sqrt(h^2*d^4)/d^2, but first check whether h^2*d^4>=0
+//  double R2 = R*R;
+//  double R2d2 = R2*d2; // yes, R2-D2!
+//  double b = R2 + d2 - r*r;
+//  double b2 = b*b;
+//  double h2d4 = R2d2 - 0.25*b2; // h^2*d^4
+//
+//  // h^2*d^4<0 is equivalent to (d > (R + r) || d < (R - r))
+//  // Hence, there are only solutions if >=0
+//  if (h2d4>=0) {
+//    // Determine p2, the point where the line through the circle
+//    // intersection points crosses the line between the circle centers.
+//    double i2 = 1.0/d2;
+//    double f = 0.5*b*i2;
+//    Point2 p2 = f * c;
+//
+//    // If h^2*d^4 == 0, the circles are touching, so just return one point
+//    if (h2d4 < 1e-9)
+//      solutions.push_back(p2);
+//    else {
+//      // determine the offsets of the intersection points from p
+//      double h = sqrt(h2d4)*i2; // h = sqrt(h^2*d^4)/d^2
+//      Point2 offset = h * Point2(-c.y(), c.x());
+//
+//      // Determine the absolute intersection points.
+//      solutions.push_back(p2 + offset);
+//      solutions.push_back(p2 - offset);
+//    }
+//  }
+//  return solutions;
+//}
+//
 /* ************************************************************************* */
 list<Point2> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2) {
   list<Point2> solutions = Point2::CircleCircleIntersection(r1,c2-c1,r2);