From 497905406b34067226d28b75f99ee4294e9458cb Mon Sep 17 00:00:00 2001 From: Vojtech Bubnik Date: Wed, 27 Oct 2021 15:08:28 +0200 Subject: [PATCH] New code for minimum enclosing circle by randomized Welzl algorithm. Split the circle code from Geometry.cpp/hpp to Geometry/Circle.cpp,hpp --- src/libslic3r/CMakeLists.txt | 2 + src/libslic3r/Fill/FillBase.cpp | 1 + src/libslic3r/Geometry.cpp | 87 --------------- src/libslic3r/Geometry.hpp | 40 ------- src/libslic3r/Geometry/Circle.cpp | 94 ++++++++++++++++ src/libslic3r/Geometry/Circle.hpp | 173 ++++++++++++++++++++++++++++++ src/slic3r/GUI/3DBed.cpp | 2 +- tests/libslic3r/test_geometry.cpp | 18 ++++ 8 files changed, 289 insertions(+), 128 deletions(-) create mode 100644 src/libslic3r/Geometry/Circle.cpp create mode 100644 src/libslic3r/Geometry/Circle.hpp diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index d42077b9c..646a64d51 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -114,6 +114,8 @@ add_library(libslic3r STATIC GCodeWriter.hpp Geometry.cpp Geometry.hpp + Geometry/Circle.cpp + Geometry/Circle.hpp Int128.hpp KDTreeIndirect.hpp Layer.cpp diff --git a/src/libslic3r/Fill/FillBase.cpp b/src/libslic3r/Fill/FillBase.cpp index a41a11cb7..52653f975 100644 --- a/src/libslic3r/Fill/FillBase.cpp +++ b/src/libslic3r/Fill/FillBase.cpp @@ -4,6 +4,7 @@ #include "../ClipperUtils.hpp" #include "../EdgeGrid.hpp" #include "../Geometry.hpp" +#include "../Geometry/Circle.hpp" #include "../Point.hpp" #include "../PrintConfig.hpp" #include "../Surface.hpp" diff --git a/src/libslic3r/Geometry.cpp b/src/libslic3r/Geometry.cpp index dfc463dde..a999f248b 100644 --- a/src/libslic3r/Geometry.cpp +++ b/src/libslic3r/Geometry.cpp @@ -337,93 +337,6 @@ double rad2deg_dir(double angle) return rad2deg(angle); } -Point circle_center_taubin_newton(const Points::const_iterator& input_begin, const Points::const_iterator& input_end, size_t cycles) -{ - Vec2ds tmp; - tmp.reserve(std::distance(input_begin, input_end)); - std::transform(input_begin, input_end, std::back_inserter(tmp), [] (const Point& in) { return unscale(in); } ); - Vec2d center = circle_center_taubin_newton(tmp.cbegin(), tmp.end(), cycles); - return Point::new_scale(center.x(), center.y()); -} - -/// Adapted from work in "Circular and Linear Regression: Fitting circles and lines by least squares", pg 126 -/// Returns a point corresponding to the center of a circle for which all of the points from input_begin to input_end -/// lie on. -Vec2d circle_center_taubin_newton(const Vec2ds::const_iterator& input_begin, const Vec2ds::const_iterator& input_end, size_t cycles) -{ - // calculate the centroid of the data set - const Vec2d sum = std::accumulate(input_begin, input_end, Vec2d(0,0)); - const size_t n = std::distance(input_begin, input_end); - const double n_flt = static_cast(n); - const Vec2d centroid { sum / n_flt }; - - // Compute the normalized moments of the data set. - double Mxx = 0, Myy = 0, Mxy = 0, Mxz = 0, Myz = 0, Mzz = 0; - for (auto it = input_begin; it < input_end; ++it) { - // center/normalize the data. - double Xi {it->x() - centroid.x()}; - double Yi {it->y() - centroid.y()}; - double Zi {Xi*Xi + Yi*Yi}; - Mxy += (Xi*Yi); - Mxx += (Xi*Xi); - Myy += (Yi*Yi); - Mxz += (Xi*Zi); - Myz += (Yi*Zi); - Mzz += (Zi*Zi); - } - - // divide by number of points to get the moments - Mxx /= n_flt; - Myy /= n_flt; - Mxy /= n_flt; - Mxz /= n_flt; - Myz /= n_flt; - Mzz /= n_flt; - - // Compute the coefficients of the characteristic polynomial for the circle - // eq 5.60 - const double Mz {Mxx + Myy}; // xx + yy = z - const double Cov_xy {Mxx*Myy - Mxy*Mxy}; // this shows up a couple times so cache it here. - const double C3 {4.0*Mz}; - const double C2 {-3.0*(Mz*Mz) - Mzz}; - const double C1 {Mz*(Mzz - (Mz*Mz)) + 4.0*Mz*Cov_xy - (Mxz*Mxz) - (Myz*Myz)}; - const double C0 {(Mxz*Mxz)*Myy + (Myz*Myz)*Mxx - 2.0*Mxz*Myz*Mxy - Cov_xy*(Mzz - (Mz*Mz))}; - - const double C22 = {C2 + C2}; - const double C33 = {C3 + C3 + C3}; - - // solve the characteristic polynomial with Newton's method. - double xnew = 0.0; - double ynew = 1e20; - - for (size_t i = 0; i < cycles; ++i) { - const double yold {ynew}; - ynew = C0 + xnew * (C1 + xnew*(C2 + xnew * C3)); - if (std::abs(ynew) > std::abs(yold)) { - BOOST_LOG_TRIVIAL(error) << "Geometry: Fit is going in the wrong direction.\n"; - return Vec2d(std::nan(""), std::nan("")); - } - const double Dy {C1 + xnew*(C22 + xnew*C33)}; - - const double xold {xnew}; - xnew = xold - (ynew / Dy); - - if (std::abs((xnew-xold) / xnew) < 1e-12) i = cycles; // converged, we're done here - - if (xnew < 0) { - // reset, we went negative - xnew = 0.0; - } - } - - // compute the determinant and the circle's parameters now that we've solved. - double DET = xnew*xnew - xnew*Mz + Cov_xy; - - Vec2d center(Mxz * (Myy - xnew) - Myz * Mxy, Myz * (Mxx - xnew) - Mxz*Mxy); - center /= (DET * 2.); - return center + centroid; -} - void simplify_polygons(const Polygons &polygons, double tolerance, Polygons* retval) { Polygons pp; diff --git a/src/libslic3r/Geometry.hpp b/src/libslic3r/Geometry.hpp index d98cab9c7..5ecfcaeff 100644 --- a/src/libslic3r/Geometry.hpp +++ b/src/libslic3r/Geometry.hpp @@ -325,38 +325,6 @@ bool liang_barsky_line_clipping( return liang_barsky_line_clipping(x0clip, x1clip, bbox); } -// Ugly named variant, that accepts the squared line -// Don't call me with a nearly zero length vector! -// sympy: -// factor(solve([a * x + b * y + c, x**2 + y**2 - r**2], [x, y])[0]) -// factor(solve([a * x + b * y + c, x**2 + y**2 - r**2], [x, y])[1]) -template -int ray_circle_intersections_r2_lv2_c(T r2, T a, T b, T lv2, T c, std::pair, Eigen::Matrix> &out) -{ - T x0 = - a * c; - T y0 = - b * c; - T d2 = r2 * lv2 - c * c; - if (d2 < T(0)) - return 0; - T d = sqrt(d2); - out.first.x() = (x0 + b * d) / lv2; - out.first.y() = (y0 - a * d) / lv2; - out.second.x() = (x0 - b * d) / lv2; - out.second.y() = (y0 + a * d) / lv2; - return d == T(0) ? 1 : 2; -} -template -int ray_circle_intersections(T r, T a, T b, T c, std::pair, Eigen::Matrix> &out) -{ - T lv2 = a * a + b * b; - if (lv2 < T(SCALED_EPSILON * SCALED_EPSILON)) { - //FIXME what is the correct epsilon? - // What if the line touches the circle? - return false; - } - return ray_circle_intersections_r2_lv2_c2(r * r, a, b, a * a + b * b, c, out); -} - Pointf3s convex_hull(Pointf3s points); Polygon convex_hull(Points points); Polygon convex_hull(const Polygons &polygons); @@ -384,14 +352,6 @@ template T angle_to_0_2PI(T angle) return angle; } -/// Find the center of the circle corresponding to the vector of Points as an arc. -Point circle_center_taubin_newton(const Points::const_iterator& input_start, const Points::const_iterator& input_end, size_t cycles = 20); -inline Point circle_center_taubin_newton(const Points& input, size_t cycles = 20) { return circle_center_taubin_newton(input.cbegin(), input.cend(), cycles); } - -/// Find the center of the circle corresponding to the vector of Pointfs as an arc. -Vec2d circle_center_taubin_newton(const Vec2ds::const_iterator& input_start, const Vec2ds::const_iterator& input_end, size_t cycles = 20); -inline Vec2d circle_center_taubin_newton(const Vec2ds& input, size_t cycles = 20) { return circle_center_taubin_newton(input.cbegin(), input.cend(), cycles); } - void simplify_polygons(const Polygons &polygons, double tolerance, Polygons* retval); double linint(double value, double oldmin, double oldmax, double newmin, double newmax); diff --git a/src/libslic3r/Geometry/Circle.cpp b/src/libslic3r/Geometry/Circle.cpp new file mode 100644 index 000000000..3d7fc0223 --- /dev/null +++ b/src/libslic3r/Geometry/Circle.cpp @@ -0,0 +1,94 @@ +#include "Circle.hpp" + +#include "../Polygon.hpp" + +namespace Slic3r { namespace Geometry { + +Point circle_center_taubin_newton(const Points::const_iterator& input_begin, const Points::const_iterator& input_end, size_t cycles) +{ + Vec2ds tmp; + tmp.reserve(std::distance(input_begin, input_end)); + std::transform(input_begin, input_end, std::back_inserter(tmp), [] (const Point& in) { return unscale(in); } ); + Vec2d center = circle_center_taubin_newton(tmp.cbegin(), tmp.end(), cycles); + return Point::new_scale(center.x(), center.y()); +} + +/// Adapted from work in "Circular and Linear Regression: Fitting circles and lines by least squares", pg 126 +/// Returns a point corresponding to the center of a circle for which all of the points from input_begin to input_end +/// lie on. +Vec2d circle_center_taubin_newton(const Vec2ds::const_iterator& input_begin, const Vec2ds::const_iterator& input_end, size_t cycles) +{ + // calculate the centroid of the data set + const Vec2d sum = std::accumulate(input_begin, input_end, Vec2d(0,0)); + const size_t n = std::distance(input_begin, input_end); + const double n_flt = static_cast(n); + const Vec2d centroid { sum / n_flt }; + + // Compute the normalized moments of the data set. + double Mxx = 0, Myy = 0, Mxy = 0, Mxz = 0, Myz = 0, Mzz = 0; + for (auto it = input_begin; it < input_end; ++it) { + // center/normalize the data. + double Xi {it->x() - centroid.x()}; + double Yi {it->y() - centroid.y()}; + double Zi {Xi*Xi + Yi*Yi}; + Mxy += (Xi*Yi); + Mxx += (Xi*Xi); + Myy += (Yi*Yi); + Mxz += (Xi*Zi); + Myz += (Yi*Zi); + Mzz += (Zi*Zi); + } + + // divide by number of points to get the moments + Mxx /= n_flt; + Myy /= n_flt; + Mxy /= n_flt; + Mxz /= n_flt; + Myz /= n_flt; + Mzz /= n_flt; + + // Compute the coefficients of the characteristic polynomial for the circle + // eq 5.60 + const double Mz {Mxx + Myy}; // xx + yy = z + const double Cov_xy {Mxx*Myy - Mxy*Mxy}; // this shows up a couple times so cache it here. + const double C3 {4.0*Mz}; + const double C2 {-3.0*(Mz*Mz) - Mzz}; + const double C1 {Mz*(Mzz - (Mz*Mz)) + 4.0*Mz*Cov_xy - (Mxz*Mxz) - (Myz*Myz)}; + const double C0 {(Mxz*Mxz)*Myy + (Myz*Myz)*Mxx - 2.0*Mxz*Myz*Mxy - Cov_xy*(Mzz - (Mz*Mz))}; + + const double C22 = {C2 + C2}; + const double C33 = {C3 + C3 + C3}; + + // solve the characteristic polynomial with Newton's method. + double xnew = 0.0; + double ynew = 1e20; + + for (size_t i = 0; i < cycles; ++i) { + const double yold {ynew}; + ynew = C0 + xnew * (C1 + xnew*(C2 + xnew * C3)); + if (std::abs(ynew) > std::abs(yold)) { + BOOST_LOG_TRIVIAL(error) << "Geometry: Fit is going in the wrong direction.\n"; + return Vec2d(std::nan(""), std::nan("")); + } + const double Dy {C1 + xnew*(C22 + xnew*C33)}; + + const double xold {xnew}; + xnew = xold - (ynew / Dy); + + if (std::abs((xnew-xold) / xnew) < 1e-12) i = cycles; // converged, we're done here + + if (xnew < 0) { + // reset, we went negative + xnew = 0.0; + } + } + + // compute the determinant and the circle's parameters now that we've solved. + double DET = xnew*xnew - xnew*Mz + Cov_xy; + + Vec2d center(Mxz * (Myy - xnew) - Myz * Mxy, Myz * (Mxx - xnew) - Mxz*Mxy); + center /= (DET * 2.); + return center + centroid; +} + +} } // namespace Slic3r::Geometry diff --git a/src/libslic3r/Geometry/Circle.hpp b/src/libslic3r/Geometry/Circle.hpp new file mode 100644 index 000000000..b2e38fbf1 --- /dev/null +++ b/src/libslic3r/Geometry/Circle.hpp @@ -0,0 +1,173 @@ +#ifndef slic3r_Geometry_Circle_hpp_ +#define slic3r_Geometry_Circle_hpp_ + +namespace Slic3r { namespace Geometry { + +/// Find the center of the circle corresponding to the vector of Points as an arc. +Point circle_center_taubin_newton(const Points::const_iterator& input_start, const Points::const_iterator& input_end, size_t cycles = 20); +inline Point circle_center_taubin_newton(const Points& input, size_t cycles = 20) { return circle_center_taubin_newton(input.cbegin(), input.cend(), cycles); } + +/// Find the center of the circle corresponding to the vector of Pointfs as an arc. +Vec2d circle_center_taubin_newton(const Vec2ds::const_iterator& input_start, const Vec2ds::const_iterator& input_end, size_t cycles = 20); +inline Vec2d circle_center_taubin_newton(const Vec2ds& input, size_t cycles = 20) { return circle_center_taubin_newton(input.cbegin(), input.cend(), cycles); } + +// https://en.wikipedia.org/wiki/Circumscribed_circle +// Circumcenter coordinates, Cartesian coordinates +template +bool circle_center(const Vector &a, Vector b, Vector c, Vector ¢er) +{ + 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; + } +} + +/* +// 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 { + using Scalar = typename Vector::Scalar; + + Vector center; + Scalar 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(); + } + + 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; } + + static CircleSq make_invalid() { return Circle { { 0, 0 }, -1 }; } +}; + +// 2D circle defined by its center and radius +template +struct Circle { + using Scalar = typename Vector::Scalar; + + Vector center; + Scalar 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(); + } + template + explicit Circle(const CircleSq &c) : center(c.center), radius(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; } + + static Circle make_invalid() { return Circle { { 0, 0 }, -1 }; } +}; + +// Randomized algorithm by Emo Welzl +template +CircleSq smallest_enclosing_circle2_welzl(const Points &points) +{ + using Scalar = typename Vector::Scalar; + + const auto &p0 = points[0].cast(); + auto circle = CircleSq(p0, points[1].cast()); + for (size_t i = 2; i < points.size(); ++ i) + if (const Vector &p = points[i].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].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 (int k = 0; k < j; ++ k) + if (const Vector &r = points[k].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; + } + return circle; +} + +template +Circle smallest_enclosing_circle_welzl(const Points &points) +{ + return Circle(smallest_enclosing_circle2_welzl(points)); +} + +// Ugly named variant, that accepts the squared line +// Don't call me with a nearly zero length vector! +// sympy: +// factor(solve([a * x + b * y + c, x**2 + y**2 - r**2], [x, y])[0]) +// factor(solve([a * x + b * y + c, x**2 + y**2 - r**2], [x, y])[1]) +template +int ray_circle_intersections_r2_lv2_c(T r2, T a, T b, T lv2, T c, std::pair, Eigen::Matrix> &out) +{ + T x0 = - a * c; + T y0 = - b * c; + T d2 = r2 * lv2 - c * c; + if (d2 < T(0)) + return 0; + T d = sqrt(d2); + out.first.x() = (x0 + b * d) / lv2; + out.first.y() = (y0 - a * d) / lv2; + out.second.x() = (x0 - b * d) / lv2; + out.second.y() = (y0 + a * d) / lv2; + return d == T(0) ? 1 : 2; +} +template +int ray_circle_intersections(T r, T a, T b, T c, std::pair, Eigen::Matrix> &out) +{ + T lv2 = a * a + b * b; + if (lv2 < T(SCALED_EPSILON * SCALED_EPSILON)) { + //FIXME what is the correct epsilon? + // What if the line touches the circle? + return false; + } + return ray_circle_intersections_r2_lv2_c2(r * r, a, b, a * a + b * b, c, out); +} + +} } // namespace Slic3r::Geometry + +#endif slic3r_Geometry_Circle_hpp_ diff --git a/src/slic3r/GUI/3DBed.cpp b/src/slic3r/GUI/3DBed.cpp index 8e6867311..ee2fb2e69 100644 --- a/src/slic3r/GUI/3DBed.cpp +++ b/src/slic3r/GUI/3DBed.cpp @@ -5,7 +5,7 @@ #include "libslic3r/Polygon.hpp" #include "libslic3r/ClipperUtils.hpp" #include "libslic3r/BoundingBox.hpp" -#include "libslic3r/Geometry.hpp" +#include "libslic3r/Geometry/Circle.hpp" #include "libslic3r/Tesselate.hpp" #include "libslic3r/PresetBundle.hpp" diff --git a/tests/libslic3r/test_geometry.cpp b/tests/libslic3r/test_geometry.cpp index 8261fe249..6e13e518c 100644 --- a/tests/libslic3r/test_geometry.cpp +++ b/tests/libslic3r/test_geometry.cpp @@ -6,6 +6,7 @@ #include "libslic3r/Polyline.hpp" #include "libslic3r/Line.hpp" #include "libslic3r/Geometry.hpp" +#include "libslic3r/Geometry/Circle.hpp" #include "libslic3r/ClipperUtils.hpp" #include "libslic3r/ShortestPath.hpp" @@ -320,6 +321,23 @@ SCENARIO("Circle Fit, TaubinFit with Newton's method", "[Geometry]") { } } +TEST_CASE("smallest_enclosing_circle_welzl", "[Geometry]") { + // Some random points in plane. + Points pts { + { 89243, 4359 }, { 763465, 59687 }, { 3245, 734987 }, { 2459867, 987634 }, { 759866, 67843982 }, { 9754687, 9834658 }, { 87235089, 743984373 }, + { 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()); }); + 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()); }); + + REQUIRE(all_inside); + REQUIRE(num_on_boundary == 3); +} + SCENARIO("Path chaining", "[Geometry]") { GIVEN("A path") { std::vector points = { Point(26,26),Point(52,26),Point(0,26),Point(26,52),Point(26,0),Point(0,52),Point(52,52),Point(52,0) };