3DPrinting/PrusaSlicer-NonPlainar
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

New code for minimum enclosing circle by randomized Welzl algorithm.

Browse Source 
Split the circle code from Geometry.cpp/hpp to Geometry/Circle.cpp,hpp
This commit is contained in:
Vojtech Bubnik 2021-10-27 15:08:28 +02:00
parent 77548df00f
commit 497905406b
8 changed files with 289 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/libslic3r/CMakeLists.txt
View File

@ -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

1
src/libslic3r/Fill/FillBase.cpp
View File

@ -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"

87
src/libslic3r/Geometry.cpp
View File

@ -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<double>(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;

40
src/libslic3r/Geometry.hpp
View File

@ -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<typename T>
int ray_circle_intersections_r2_lv2_c(T r2, T a, T b, T lv2, T c, std::pair<Eigen::Matrix<T, 2, 1, Eigen::DontAlign>, Eigen::Matrix<T, 2, 1, Eigen::DontAlign>> &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<typename T>
int ray_circle_intersections(T r, T a, T b, T c, std::pair<Eigen::Matrix<T, 2, 1, Eigen::DontAlign>, Eigen::Matrix<T, 2, 1, Eigen::DontAlign>> &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<typename T> 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);

94
src/libslic3r/Geometry/Circle.cpp Normal file
View File

@ -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<double>(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

173
src/libslic3r/Geometry/Circle.hpp Normal file
View File

@ -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<typename Vector>
bool circle_center(const Vector &a, Vector b, Vector c, Vector &center)
{
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<typename Vector>
bool circle_center_centered(Vector a, Vector b, Vector c, Vector &center)
{
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<typename Vector>
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<typename Vector>
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<typename Vector2>
explicit Circle(const CircleSq<Vector2> &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<typename Vector, typename Points>
CircleSq<Vector> smallest_enclosing_circle2_welzl(const Points &points)
{
using Scalar = typename Vector::Scalar;
const auto &p0 = points[0].cast<Scalar>();
auto circle = CircleSq<Vector>(p0, points[1].cast<Scalar>());
for (size_t i = 2; i < points.size(); ++ i)
if (const Vector &p = points[i].cast<Scalar>(); ! circle.contains_with_eps(p)) {
// p is the first point on the smallest circle enclosing points[0..i]
auto c = CircleSq<Vector>(p0, p);
for (size_t j = 1; j < i; ++ j)
if (const Vector &q = points[j].cast<Scalar>(); ! c.contains_with_eps(q)) {
// q is the second point on the smallest circle enclosing points[0..i]
auto c2 = CircleSq<Vector>(p, q);
for (int k = 0; k < j; ++ k)
if (const Vector &r = points[k].cast<Scalar>(); ! c2.contains_with_eps(r)) {
if (Vector center; circle_center<Vector>(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<typename Vector, typename Points>
Circle<Vector> smallest_enclosing_circle_welzl(const Points &points)
{
return Circle<Vector>(smallest_enclosing_circle2_welzl<Vector, Points>(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<typename T>
int ray_circle_intersections_r2_lv2_c(T r2, T a, T b, T lv2, T c, std::pair<Eigen::Matrix<T, 2, 1, Eigen::DontAlign>, Eigen::Matrix<T, 2, 1, Eigen::DontAlign>> &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<typename T>
int ray_circle_intersections(T r, T a, T b, T c, std::pair<Eigen::Matrix<T, 2, 1, Eigen::DontAlign>, Eigen::Matrix<T, 2, 1, Eigen::DontAlign>> &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_

2
src/slic3r/GUI/3DBed.cpp
View File

@ -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"

18
tests/libslic3r/test_geometry.cpp
View File

@ -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<Vec2d>(pts);
bool all_inside = std::all_of(pts.begin(), pts.end(), [c](const Point &pt){ return c.contains_with_eps(pt.cast<double>()); });
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<double>()); });
REQUIRE(all_inside);
REQUIRE(num_on_boundary == 3);
}
SCENARIO("Path chaining", "[Geometry]") {
GIVEN("A path") {
std::vector<Point> 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) };