circle-circle intersection
Frank Dellaert
parent
e618df7bfa
commit
133dd1cae5
|
|
@ -17,6 +17,8 @@
|
|||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/base/Lie-inl.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -42,7 +44,7 @@ double Point2::norm(boost::optional<Matrix&> 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<Matrix&> H1,
|
|||
return d.norm();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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();
|
||||
|
||||
// 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> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2) {
|
||||
list<Point2> 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() << ')';
|
||||
|
|
|
|||
|
|
@ -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<Point2> 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<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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<Point2> inside = Point2::CircleCircleIntersection(5,Point2(0,0),1);
|
||||
EXPECT_LONGS_EQUAL(0,inside.size());
|
||||
|
||||
list<Point2> touching1 = Point2::CircleCircleIntersection(5,Point2(4,0),1);
|
||||
EXPECT_LONGS_EQUAL(1,touching1.size());
|
||||
EXPECT(assert_equal(Point2(5,0), touching1.front()));
|
||||
|
||||
list<Point2> 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<Point2> 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<Point2> 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<r2
|
||||
list<Point2> 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<Point2> 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<r2
|
||||
list<Point2> 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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue