From d78a5acba9d4c10809af0c098a9bad1f3b837f85 Mon Sep 17 00:00:00 2001 From: Vojtech Bubnik Date: Sat, 30 Oct 2021 11:18:23 +0200 Subject: [PATCH] Numerical improvements to Welzel minimum enclosing circle algorithm --- src/libslic3r/Geometry/Circle.hpp | 150 ++++++++++++++++-------------- tests/libslic3r/test_geometry.cpp | 9 +- 2 files changed, 83 insertions(+), 76 deletions(-) diff --git a/src/libslic3r/Geometry/Circle.hpp b/src/libslic3r/Geometry/Circle.hpp index b2c27e636..b02a887c1 100644 --- a/src/libslic3r/Geometry/Circle.hpp +++ b/src/libslic3r/Geometry/Circle.hpp @@ -14,41 +14,26 @@ inline Vec2d circle_center_taubin_newton(const Vec2ds& input, size_t cycles = 20 // https://en.wikipedia.org/wiki/Circumscribed_circle // Circumcenter coordinates, Cartesian coordinates template -bool circle_center(const Vector &a, Vector b, Vector c, Vector ¢er) +Vector circle_center(const Vector &a, const Vector &bsrc, const Vector &csrc, typename Vector::Scalar epsilon) { using Scalar = typename Vector::Scalar; - b -= a; - c -= a; - if (Scalar d = b.x() * c.y() - b.y() * c.x(); d == 0) - return false; - else { - Vector v = c.squaredNorm() * b - b.squaredNorm() * c; - center = a + Vector(- v.y(), v.x()) / (2 * d); - return true; + Vector b = bsrc - a; + Vector c = csrc - a; + Scalar lb = b.squaredNorm(); + Scalar lc = c.squaredNorm(); + if (Scalar d = b.x() * c.y() - b.y() * c.x(); std::abs(d) < epsilon) { + // The three points are collinear. Take the center of the two points + // furthest away from each other. + Scalar lbc = (csrc - bsrc).squaredNorm(); + return Scalar(0.5) * ( + lb > lc && lb > lbc ? a + bsrc : + lc > lb && lc > lbc ? a + csrc : bsrc + csrc); + } else { + Vector v = lc * b - lb * c; + return a + Vector(- v.y(), v.x()) / (2 * d); } } -/* -// Likely a bit more accurate accurate version of circle_center() by centering. -template -bool circle_center_centered(Vector a, Vector b, Vector c, Vector ¢er) -{ - auto bbox_center = 0.5 * (a.cwiseMin(b).cwiseMin(c) + a.cwiseMax(b).cwiseMax(c)); - a -= bbox_center; - b -= bbox_center; - c -= bbox_center; - auto bc = b - c; - auto ca = c - a; - auto ab = a - b; - if (d = ao.x() * bc.y() + bo.x() * ca.y() + co.x() * ab.y(); d == 0) - return false; - else { - center = bbox_center - perp(ao.squaredNorm() * bc + bo.squaredNorm() * ca + co.squaredNorm() * ab) / (2 * d); - return true; - } -} -*/ - // 2D circle defined by its center and squared radius template struct CircleSq { @@ -57,19 +42,21 @@ struct CircleSq { Vector center; Scalar radius2; + CircleSq() {} + CircleSq(const Vector ¢er, const Scalar radius2) : center(center), radius2(radius2) {} CircleSq(const Vector &a, const Vector &b) : center(Scalar(0.5) * (a + b)) { radius2 = (a - center).squaredNorm(); } - CircleSq(const Vector &a, const Vector &b, const Vector &c) { - if (circle_center(a, b, c, this->center)) - this->radius = (a - this->center).squaredNorm(); - else - *this = make_invalid(); + CircleSq(const Vector &a, const Vector &b, const Vector &c, Scalar epsilon) { + this->center = circle_center(a, b, c, epsilon); + this->radius2 = (a - this->center).squaredNorm(); } bool invalid() const { return this->radius2 < 0; } bool valid() const { return ! this->invalid(); } bool contains(const Vector &p) const { return (p - this->center).squaredNorm() < this->radius2; } - bool contains_with_eps(const Vector &p, const Scalar relative_epsilon2 = Scalar((1 + 1e-14) * (1 + 1e-14))) const - { Scalar r2 = this->radius2 * relative_epsilon2; return (p - this->center).squaredNorm() < r2; } + bool contains(const Vector &p, const Scalar epsilon2) const { return (p - this->center).squaredNorm() < this->radius2 + epsilon2; } + + CircleSq inflated(Scalar epsilon) const + { assert(this->radius2 >= 0); Scalar r = sqrt(this->radius2) + epsilon; return { this->center, r * r }; } static CircleSq make_invalid() { return CircleSq { { 0, 0 }, -1 }; } }; @@ -82,58 +69,77 @@ struct Circle { Vector center; Scalar radius; + Circle() {} + Circle(const Vector ¢er, const Scalar radius) : center(center), radius(radius) {} Circle(const Vector &a, const Vector &b) : center(Scalar(0.5) * (a + b)) { radius = (a - center).norm(); } - Circle(const Vector &a, const Vector &b, const Vector &c) { - this->center = circle_center(a, b, c); - this->radius = (a - this->center).norm(); - } + Circle(const Vector &a, const Vector &b, const Vector &c, const Scalar epsilon) { *this = CircleSq(a, b, c, epsilon); } + + // Conversion from CircleSq template - explicit Circle(const CircleSq &c) : center(c.center), radius(sqrt(c.radius2)) {} + explicit Circle(const CircleSq &c) : center(c.center), radius(c.radius2 <= 0 ? c.radius2 : sqrt(c.radius2)) {} + template + Circle operator=(const CircleSq &c) { this->center = c.center; this->radius = c.radius2 <= 0 ? c.radius2 : sqrt(c.radius2) } bool invalid() const { return this->radius < 0; } bool valid() const { return ! this->invalid(); } - bool contains(const Vector &p) const { return (p - this->center).squaredNorm() < this->radius * this->radius; } - bool contains_with_eps(const Vector &p, const Scalar relative_epsilon = 1 + 1e-14) const { Scalar r = this->radius * relative_epsilon; return (p - this->center).squaredNorm() < r * r; } + bool contains(const Vector &p) const { return (p - this->center).squaredNorm() <= this->radius * this->radius; } + bool contains(const Vector &p, const Scalar epsilon) const + { Scalar re = this->radius + epsilon; return (p - this->center).squaredNorm() < re * re; } + + Circle inflated(Scalar epsilon) const { assert(this->radius >= 0); return { this->center, this->radius + epsilon }; } static Circle make_invalid() { return Circle { { 0, 0 }, -1 }; } }; -// Randomized algorithm by Emo Welzl +using Circlef = Circle; +using Circled = Circle; +using CircleSqf = CircleSq; +using CircleSqd = CircleSq; + +// Randomized algorithm by Emo Welzl, working with squared radii for efficiency. The returned circle radius is inflated by epsilon. template -CircleSq smallest_enclosing_circle2_welzl(const Points &points) +CircleSq smallest_enclosing_circle2_welzl(const Points &points, const typename Vector::Scalar epsilon) { using Scalar = typename Vector::Scalar; - - const auto &p0 = points[0].template cast(); - auto circle = CircleSq(p0, points[1].template cast()); - for (size_t i = 2; i < points.size(); ++ i) - if (const Vector &p = points[i].template cast(); ! circle.contains_with_eps(p)) { - // p is the first point on the smallest circle enclosing points[0..i] - auto c = CircleSq(p0, p); - for (size_t j = 1; j < i; ++ j) - if (const Vector &q = points[j].template cast(); ! c.contains_with_eps(q)) { - // q is the second point on the smallest circle enclosing points[0..i] - auto c2 = CircleSq(p, q); - for (size_t k = 0; k < j; ++ k) - if (const Vector &r = points[k].template cast(); ! c2.contains_with_eps(r)) { - if (Vector center; circle_center(p, q, r, center)) { - c2.center = center; - c2.radius2 = (center - p).squaredNorm(); - } - } - if (c2.radius2 > 0) - c = c2; - } - if (c.radius2 > 0) - circle = c; - } + CircleSq circle; + + if (! points.empty()) { + const auto &p0 = points[0].template cast(); + if (points.size() == 1) { + circle.center = p0; + circle.radius2 = epsilon * epsilon; + } else { + circle = CircleSq(p0, points[1].template cast()).inflated(epsilon); + for (size_t i = 2; i < points.size(); ++ i) + if (const Vector &p = points[i].template cast(); ! circle.contains(p)) { + // p is the first point on the smallest circle enclosing points[0..i] + circle = CircleSq(p0, p).inflated(epsilon); + for (size_t j = 1; j < i; ++ j) + if (const Vector &q = points[j].template cast(); ! circle.contains(q)) { + // q is the second point on the smallest circle enclosing points[0..i] + circle = CircleSq(p, q).inflated(epsilon); + for (size_t k = 0; k < j; ++ k) + if (const Vector &r = points[k].template cast(); ! circle.contains(r)) + circle = CircleSq(p, q, r, epsilon).inflated(epsilon); + } + } + } + } + return circle; } +// Randomized algorithm by Emo Welzl. The returned circle radius is inflated by epsilon. template -Circle smallest_enclosing_circle_welzl(const Points &points) +Circle smallest_enclosing_circle_welzl(const Points &points, const typename Vector::Scalar epsilon) { - return Circle(smallest_enclosing_circle2_welzl(points)); + return Circle(smallest_enclosing_circle2_welzl(points, epsilon)); +} + +// Randomized algorithm by Emo Welzl. The returned circle radius is inflated by SCALED_EPSILON. +inline Circled smallest_enclosing_circle_welzl(const Points &points) +{ + return smallest_enclosing_circle_welzl(points, SCALED_EPSILON); } // Ugly named variant, that accepts the squared line diff --git a/tests/libslic3r/test_geometry.cpp b/tests/libslic3r/test_geometry.cpp index 6e13e518c..6bec7af68 100644 --- a/tests/libslic3r/test_geometry.cpp +++ b/tests/libslic3r/test_geometry.cpp @@ -328,11 +328,12 @@ TEST_CASE("smallest_enclosing_circle_welzl", "[Geometry]") { { 65874456, 2987546 }, { 98234524, 657654873 }, { 786243598, 287934765 }, { 824356, 734265 }, { 82576449, 7864534 }, { 7826345, 3984765 } }; - const auto c = Slic3r::Geometry::smallest_enclosing_circle_welzl(pts); - bool all_inside = std::all_of(pts.begin(), pts.end(), [c](const Point &pt){ return c.contains_with_eps(pt.cast()); }); + const auto c = Slic3r::Geometry::smallest_enclosing_circle_welzl(pts); + // The radius returned is inflated by SCALED_EPSILON, thus all points should be inside. + bool all_inside = std::all_of(pts.begin(), pts.end(), [c](const Point &pt){ return c.contains(pt.cast()); }); auto c2(c); - c2.radius -= 1.; - auto num_on_boundary = std::count_if(pts.begin(), pts.end(), [c2](const Point& pt) { return ! c2.contains_with_eps(pt.cast()); }); + c2.radius -= SCALED_EPSILON * 2.1; + auto num_on_boundary = std::count_if(pts.begin(), pts.end(), [c2](const Point& pt) { return ! c2.contains(pt.cast(), SCALED_EPSILON); }); REQUIRE(all_inside); REQUIRE(num_on_boundary == 3);