From 133dd1cae555330ef845205243a1a99768da9e6c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Jun 2013 15:10:51 +0000 Subject: [PATCH] circle-circle intersection --- gtsam/geometry/Point2.cpp | 52 ++++++++++++++- gtsam/geometry/Point2.h | 18 +++++ gtsam/geometry/tests/testPoint2.cpp | 100 ++++++++++++++++++++-------- 3 files changed, 141 insertions(+), 29 deletions(-) diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 051d33279..e0f3baec9 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -17,6 +17,8 @@ #include #include +#include +#include using namespace std; @@ -42,7 +44,7 @@ double Point2::norm(boost::optional H) const { double r = sqrt(x_ * x_ + y_ * y_); if (H) { Matrix D_result_d; - if (std::abs(r) > 1e-10) + if (fabs(r) > 1e-10) D_result_d = Matrix_(1, 2, x_ / r, y_ / r); else D_result_d = oneOne; // TODO: really infinity, why 1 here?? @@ -66,6 +68,54 @@ double Point2::distance(const Point2& point, boost::optional H1, return d.norm(); } +/* ************************************************************************* */ +list Point2::CircleCircleIntersection(double R, Point2 c, double r) { + + list 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(); + + // Check for solutions + double R2 = R*R; + double b = R2 - r*r + d2; + double b2 = b*b; + + // Return empty list if no solution + // test below is equivalent to h2>0 == !(d > (R + r) || d < (R - r)) + if (4*R2*d2 >= b2) { + // 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 touching, just return one point + double h2 = R2 - 0.25*b2*i2; + 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()); + + // Determine the absolute intersection points. + solutions.push_back(p2 + offset); + solutions.push_back(p2 - offset); + } + } + return solutions; +} + +/* ************************************************************************* */ +list Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2) { + list solutions = Point2::CircleCircleIntersection(r1,c2-c1,r2); + BOOST_FOREACH(Point2& p, solutions) p+= c1; + return solutions; +} + /* ************************************************************************* */ ostream &operator<<(ostream &os, const Point2& p) { os << '(' << p.x() << ", " << p.y() << ')'; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index f2f0a0fef..05a47e379 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -65,6 +65,24 @@ public: y_ = v(1); } + /** + * @brief Intersect Circle with radius R at origin, with circle of radius r at c + * @param R radius of circle at origin + * @param center center of second circle + * @param r radius of second circle + * @return list of solutions (0,1, or 2 points) + */ + static std::list CircleCircleIntersection(double R, Point2 c, double r); + + /** + * @brief Intersect 2 circles + * @param c1 center of first circle + * @param r1 radius of first circle + * @param c2 center of second circle + * @param r2 radius of second circle + */ + static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2); + /// @} /// @name Testable /// @{ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index cb06598c6..61aad1ce5 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -19,7 +19,6 @@ #include #include #include - #include using namespace std; @@ -30,13 +29,13 @@ GTSAM_CONCEPT_LIE_INST(Point2) /* ************************************************************************* */ TEST(Point2, constructor) { - Point2 p1(1,2), p2 = p1; + Point2 p1(1, 2), p2 = p1; EXPECT(assert_equal(p1, p2)); } /* ************************************************************************* */ TEST(Point2, Lie) { - Point2 p1(1,2), p2(4,5); + Point2 p1(1, 2), p2(4, 5); Matrix H1, H2; EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2))); @@ -52,8 +51,7 @@ TEST(Point2, Lie) { } /* ************************************************************************* */ -TEST( Point2, expmap) -{ +TEST( Point2, expmap) { Vector d(2); d(0) = 1; d(1) = -1; @@ -62,8 +60,7 @@ TEST( Point2, expmap) } /* ************************************************************************* */ -TEST( Point2, arithmetic) -{ +TEST( Point2, arithmetic) { EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); @@ -73,9 +70,8 @@ TEST( Point2, arithmetic) } /* ************************************************************************* */ -TEST( Point2, unit) -{ - Point2 p0(10, 0), p1(0,-10), p2(10, 10); +TEST( Point2, unit) { + Point2 p0(10, 0), p1(0, -10), p2(10, 10); EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6)); EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6)); EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6)); @@ -90,25 +86,24 @@ Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); LieVector norm_proxy(const Point2& point) { return LieVector(point.norm()); } -TEST( Point2, norm ) -{ +TEST( Point2, norm ) { Point2 p0(cos(5.0), sin(5.0)); - DOUBLES_EQUAL(1,p0.norm(),1e-6); + DOUBLES_EQUAL(1, p0.norm(), 1e-6); Point2 p1(4, 5), p2(1, 1); - DOUBLES_EQUAL( 5,p1.distance(p2),1e-6); - DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6); + DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6); + DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6); Matrix expectedH, actualH; - double actual ; + double actual; // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1] actual = x1.norm(actualH); - EXPECT_DOUBLES_EQUAL(0,actual,1e-9); + EXPECT_DOUBLES_EQUAL(0, actual, 1e-9); expectedH = Matrix_(1, 2, 1.0, 1.0); EXPECT(assert_equal(expectedH,actualH)); actual = x2.norm(actualH); - EXPECT_DOUBLES_EQUAL(sqrt(2),actual,1e-9); + EXPECT_DOUBLES_EQUAL(sqrt(2), actual, 1e-9); expectedH = numericalDerivative11(norm_proxy, x2); EXPECT(assert_equal(expectedH,actualH)); } @@ -117,19 +112,18 @@ TEST( Point2, norm ) LieVector distance_proxy(const Point2& location, const Point2& point) { return LieVector(location.distance(point)); } -TEST( Point2, distance ) -{ +TEST( Point2, distance ) { Matrix expectedH1, actualH1, expectedH2, actualH2; // establish distance is indeed zero - EXPECT_DOUBLES_EQUAL(1,x1.distance(l1),1e-9); + EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9); // establish distance is indeed 45 degrees - EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.distance(l2),1e-9); + EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9); // Another pair double actual23 = x2.distance(l3, actualH1, actualH2); - EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9); + EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9); // Check numerical derivatives expectedH1 = numericalDerivative21(distance_proxy, x2, l3); @@ -139,7 +133,7 @@ TEST( Point2, distance ) // Another test double actual34 = x3.distance(l4, actualH1, actualH2); - EXPECT_DOUBLES_EQUAL(2,actual34,1e-9); + EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9); // Check numerical derivatives expectedH1 = numericalDerivative21(distance_proxy, x3, l4); @@ -149,15 +143,65 @@ TEST( Point2, distance ) } /* ************************************************************************* */ -TEST( Point2, stream) -{ - Point2 p(1,2); +TEST( Point2, circleCircleIntersection) { + + double offset = 0.994987; + // Test intersections of circle moving from inside to outside + + list inside = Point2::CircleCircleIntersection(5,Point2(0,0),1); + EXPECT_LONGS_EQUAL(0,inside.size()); + + list touching1 = Point2::CircleCircleIntersection(5,Point2(4,0),1); + EXPECT_LONGS_EQUAL(1,touching1.size()); + EXPECT(assert_equal(Point2(5,0), touching1.front())); + + list common = Point2::CircleCircleIntersection(5,Point2(5,0),1); + EXPECT_LONGS_EQUAL(2,common.size()); + EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6)); + EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6)); + + list touching2 = Point2::CircleCircleIntersection(5,Point2(6,0),1); + EXPECT_LONGS_EQUAL(1,touching2.size()); + EXPECT(assert_equal(Point2(5,0), touching2.front())); + + // test rotated case + list rotated = Point2::CircleCircleIntersection(5,Point2(0,5),1); + EXPECT_LONGS_EQUAL(2,rotated.size()); + EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6)); + EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6)); + + // test r1 smaller = Point2::CircleCircleIntersection(1,Point2(5,0),5); + EXPECT_LONGS_EQUAL(2,smaller.size()); + EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6)); + EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6)); + + // test offset case, r1>r2 + list offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1); + EXPECT_LONGS_EQUAL(2,offset1.size()); + EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6)); + EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6)); + + // test offset case, r1 offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5); + EXPECT_LONGS_EQUAL(2,offset2.size()); + EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6)); + EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6)); + +} + +/* ************************************************************************* */ +TEST( Point2, stream) { + Point2 p(1, 2); std::ostringstream os; os << p; EXPECT(os.str() == "(1, 2)"); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main () { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */