From 3df8da662edb98554fbf5567594bea58cb3d4d25 Mon Sep 17 00:00:00 2001 From: Vojtech Bubnik Date: Fri, 14 Jul 2023 11:20:55 +0200 Subject: [PATCH] WIP Arc discretization, arc interpolation and unit tests. --- src/libslic3r/Geometry/ArcWelder.cpp | 126 ++++++++++++++++----------- src/libslic3r/Geometry/ArcWelder.hpp | 4 + src/libslic3r/Geometry/Circle.hpp | 39 ++++++++- tests/libslic3r/test_arc_welder.cpp | 114 +++++++++++++++++++++++- tests/libslic3r/test_geometry.cpp | 35 ++++++++ 5 files changed, 261 insertions(+), 57 deletions(-) diff --git a/src/libslic3r/Geometry/ArcWelder.cpp b/src/libslic3r/Geometry/ArcWelder.cpp index f0b38a497e..51c9ffe1f5 100644 --- a/src/libslic3r/Geometry/ArcWelder.cpp +++ b/src/libslic3r/Geometry/ArcWelder.cpp @@ -26,6 +26,7 @@ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// #include "ArcWelder.hpp" +#include "Circle.hpp" #include "../MultiPoint.hpp" #include "../Polygon.hpp" @@ -36,11 +37,25 @@ namespace Slic3r { namespace Geometry { namespace ArcWelder { -// Area of a parallelogram of two vectors to be considered collinear. -static constexpr const double Parallel_area_threshold = 0.0001; -static constexpr const auto Parallel_area_threshold_scaled = int64_t(Parallel_area_threshold / sqr(SCALING_FACTOR)); -// FIXME do we want to use EPSILON here? -static constexpr const double epsilon = 0.000005; +Points arc_discretize(const Point &p1, const Point &p2, const double radius, const bool ccw, const double deviation) +{ + Vec2d center = arc_center(p1.cast(), p2.cast(), radius, ccw); + double angle = arc_angle(p1.cast(), p2.cast(), radius); + + double r = std::abs(radius); + double angle_step = 2. * acos((r - deviation) / r); + size_t num_steps = size_t(ceil(angle / angle_step)); + + Points out; + out.reserve(num_steps + 1); + out.emplace_back(p1); + if (! ccw) + angle_step *= -1.; + for (size_t i = 1; i < num_steps; ++ i) + out.emplace_back(p1.rotated(angle_step * i, center.cast())); + out.emplace_back(p2); + return out; +} struct Circle { @@ -50,37 +65,26 @@ struct Circle // Interpolate three points with a circle. // Returns false if the three points are collinear or if the radius is bigger than maximum allowed radius. -//FIXME unit test! static std::optional try_create_circle(const Point &p1, const Point &p2, const Point &p3, const double max_radius) { - // Use area of triangle to judge whether three points are considered collinear. - Vec2i64 v2 = (p2 - p1).cast(); - Vec2i64 v3 = (p3 - p2).cast(); - if (std::abs(cross2(v2, v3)) <= Parallel_area_threshold_scaled) - return {}; - - int64_t det = cross2(p2.cast(), p3.cast()) - cross2(p1.cast(), v3); - if (std::abs(det) < int64_t(SCALED_EPSILON)) - return {}; - - Point center = ((1. / 2.0 * double(det)) * - (double(p1.cast().squaredNorm()) * perp(v3).cast() + - double(p2.cast().squaredNorm()) * perp(p1 - p3).cast() + - double(p3.cast().squaredNorm()) * perp(v2).cast())).cast(); - double r = sqrt(double((center - p1).squaredNorm())); - return r > max_radius ? std::make_optional() : std::make_optional({ center, r }); + if (auto center = Slic3r::Geometry::try_circle_center(p1.cast(), p2.cast(), p3.cast(), SCALED_EPSILON); center) { + Point c = center->cast(); + if (double r = sqrt(double((c - p1).cast().squaredNorm())); r <= max_radius) + return std::make_optional({ c, float(r) }); + } + return {}; } // Returns a closest point on the segment. // Returns false if the closest point is not inside the segment, but at its boundary. static bool foot_pt_on_segment(const Point &p1, const Point &p2, const Point &c, Point &out) { - Vec2i64 v21 = (p2 - p1).cast(); - int64_t denom = v21.squaredNorm(); - if (denom > epsilon) { - if (double t = double((c - p1).cast().dot(v21)) / double(denom); - t >= epsilon && t < 1. - epsilon) { - out = p1 + (t * v21.cast()).cast(); + Vec2i64 v21 = (p2 - p1).cast(); + int64_t l2 = v21.squaredNorm(); + if (l2 > int64_t(SCALED_EPSILON)) { + if (int64_t t = (c - p1).cast().dot(v21); + t >= int64_t(SCALED_EPSILON) && t < l2 - int64_t(SCALED_EPSILON)) { + out = p1 + ((double(t) / double(l2)) * v21.cast()).cast(); return true; } } @@ -91,16 +95,19 @@ static bool foot_pt_on_segment(const Point &p1, const Point &p2, const Point &c, static inline bool circle_approximation_sufficient(const Circle &circle, const Points::const_iterator begin, const Points::const_iterator end, const double tolerance) { // The circle was calculated from the 1st and last point of the point sequence, thus the fitting of those points does not need to be evaluated. - assert(std::abs((*begin - circle.center).cast().norm() - circle.radius) < epsilon); - assert(std::abs((*std::prev(end) - circle.center).cast().norm() - circle.radius) < epsilon); + assert(std::abs((*begin - circle.center).cast().norm() - circle.radius) < SCALED_EPSILON); + assert(std::abs((*std::prev(end) - circle.center).cast().norm() - circle.radius) < SCALED_EPSILON); assert(end - begin >= 3); - for (auto it = begin; std::next(it) != end; ++ it) { - if (it != begin) { - if (double distance_from_center = (*it - circle.center).cast().norm(); - std::abs(distance_from_center - circle.radius) > tolerance) - return false; - } + // Test the 1st point. + if (double distance_from_center = (*begin - circle.center).cast().norm(); + std::abs(distance_from_center - circle.radius) > tolerance) + return false; + + for (auto it = std::next(begin); std::next(it) != end; ++ it) { + if (double distance_from_center = (*it - circle.center).cast().norm(); + std::abs(distance_from_center - circle.radius) > tolerance) + return false; Point closest_point; if (foot_pt_on_segment(*it, *std::next(it), circle.center, closest_point)) { if (double distance_from_center = (closest_point - circle.center).cast().norm(); @@ -114,8 +121,8 @@ static inline bool circle_approximation_sufficient(const Circle &circle, const P static inline bool get_deviation_sum_squared(const Circle &circle, const Points::const_iterator begin, const Points::const_iterator end, const double tolerance, double &total_deviation) { // The circle was calculated from the 1st and last point of the point sequence, thus the fitting of those points does not need to be evaluated. - assert(std::abs((*begin - circle.center).cast().norm() - circle.radius) < epsilon); - assert(std::abs((*std::prev(end) - circle.center).cast().norm() - circle.radius) < epsilon); + assert(std::abs((*begin - circle.center).cast().norm() - circle.radius) < SCALED_EPSILON); + assert(std::abs((*std::prev(end) - circle.center).cast().norm() - circle.radius) < SCALED_EPSILON); assert(end - begin >= 3); total_deviation = 0; @@ -146,18 +153,19 @@ static std::optional try_create_circle(const Points::const_iterator begi size_t size = end - begin; if (size == 3) { out = try_create_circle(*begin, *std::next(begin), *std::prev(end), max_radius); - if (! circle_approximation_sufficient(*out, begin, end, tolerance)) + if (out && ! circle_approximation_sufficient(*out, begin, end, tolerance)) out.reset(); } else { +#if 0 size_t ipivot = size / 2; // Take a center difference of points at the center of the path. //FIXME does it really help? For short arches, the linear interpolation may be Point pivot = (size % 2 == 0) ? (*(begin + ipivot) + *(begin + ipivot - 1)) / 2 : (*(begin + ipivot - 1) + *(begin + ipivot + 1)) / 2; if (std::optional circle = try_create_circle(*begin, pivot, *std::prev(end), max_radius); - circle_approximation_sufficient(*circle, begin, end, tolerance)) + circle && circle_approximation_sufficient(*circle, begin, end, tolerance)) return circle; - +#endif // Find the circle with the least deviation, if one exists. double least_deviation; double current_deviation; @@ -335,19 +343,24 @@ Path fit_path(const Points &src, double tolerance, double fit_circle_percent_tol out.erase(douglas_peucker_in_place(out.begin(), out.end(), tolerance), out.end()); } else { // Perform simplification & fitting. + // Index of the start of a last polyline, which has not yet been decimated. int begin_pl_idx = 0; - for (auto begin = src.begin(); begin < src.end();) { - // Minimum 3 points required for circle fitting. - auto end = begin + 3; + out.push_back({ src.front(), 0.f }); + for (auto it = std::next(src.begin()); it != src.end();) { + // Minimum 2 additional points required for circle fitting. + auto begin = std::prev(it); + auto end = std::next(it); + assert(end <= src.end()); std::optional arc; - while (end <= src.end()) { + while (end != src.end()) { + auto next_end = std::next(end); if (std::optional this_arc = ArcWelder::Arc::try_create_arc( - begin, end, + begin, next_end, ArcWelder::default_scaled_max_radius, tolerance, fit_circle_percent_tolerance); this_arc) { arc = this_arc; - ++ end; + end = next_end; } else break; } @@ -355,13 +368,22 @@ Path fit_path(const Points &src, double tolerance, double fit_circle_percent_tol // If there is a trailing polyline, decimate it first before saving a new arc. if (out.size() - begin_pl_idx > 2) out.erase(douglas_peucker_in_place(out.begin() + begin_pl_idx, out.end(), tolerance), out.end()); - // Save the end of the last circle segment, which may become the first point of a possible future polyline. + // Save the index of an end of the new circle segment, which may become the first point of a possible future polyline. begin_pl_idx = int(out.size()); - -- end; - out.push_back({ arc->end_point, float(arc->direction == Orientation::CCW ? arc->radius : - arc->radius) }); - } else - out.push_back({ arc->end_point, 0.f }); + // This will be the next point to try to add. + it = end; + // Add the new arc. + assert(*begin == arc->start_point); + assert(*std::prev(it) == arc->end_point); + out.push_back({ arc->end_point, float(arc->radius), arc->direction }); + } else { + // Arc is not valid, append a linear segment. + out.push_back({ *it ++ }); + } } + if (out.size() - begin_pl_idx > 2) + // Do the final polyline decimation. + out.erase(douglas_peucker_in_place(out.begin() + begin_pl_idx, out.end(), tolerance), out.end()); } return out; diff --git a/src/libslic3r/Geometry/ArcWelder.hpp b/src/libslic3r/Geometry/ArcWelder.hpp index 4974060c55..00a1cc4397 100644 --- a/src/libslic3r/Geometry/ArcWelder.hpp +++ b/src/libslic3r/Geometry/ArcWelder.hpp @@ -97,6 +97,10 @@ inline typename Derived::Scalar arc_length( return arc_angle(start_pos, end_pos, radius) * std::abs(radius); } +// Discretize arc given the radius, orientation and maximum deviation from the arc. +// Returned polygon starts with p1, ends with p2 and it is discretized to guarantee the maximum deviation. +Points arc_discretize(const Point &p1, const Point &p2, const double radius, const bool ccw, const double deviation); + // 1.2m diameter, maximum given by coord_t static constexpr const double default_scaled_max_radius = scaled(600.); // 0.05mm diff --git a/src/libslic3r/Geometry/Circle.hpp b/src/libslic3r/Geometry/Circle.hpp index ba048dcd63..7b85916f5a 100644 --- a/src/libslic3r/Geometry/Circle.hpp +++ b/src/libslic3r/Geometry/Circle.hpp @@ -9,10 +9,17 @@ namespace Slic3r { namespace Geometry { // https://en.wikipedia.org/wiki/Circumscribed_circle // Circumcenter coordinates, Cartesian coordinates -template -Vector circle_center(const Vector &a, const Vector &bsrc, const Vector &csrc, typename Vector::Scalar epsilon) +// In case the three points are collinear, returns their centroid. +template +Eigen::Matrix circle_center(const Derived &a, const Derived2 &bsrc, const Derived3 &csrc, typename typename Derived::Scalar epsilon) { - using Scalar = typename Vector::Scalar; + static_assert(Derived ::IsVectorAtCompileTime && int(Derived ::SizeAtCompileTime) == 2, "circle_center(): 1st point is not a 2D vector"); + static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "circle_center(): 2nd point is not a 2D vector"); + static_assert(Derived3::IsVectorAtCompileTime && int(Derived3::SizeAtCompileTime) == 2, "circle_center(): 3rd point is not a 2D vector"); + static_assert(std::is_same::value && std::is_same::value, + "circle_center(): All three points must be of the same type."); + using Scalar = typename Derived::Scalar; + using Vector = Eigen::Matrix; Vector b = bsrc - a; Vector c = csrc - a; Scalar lb = b.squaredNorm(); @@ -30,6 +37,32 @@ Vector circle_center(const Vector &a, const Vector &bsrc, const Vector &csrc, ty } } +// https://en.wikipedia.org/wiki/Circumscribed_circle +// Circumcenter coordinates, Cartesian coordinates +// Returns no value if the three points are collinear. +template +std::optional> try_circle_center(const Derived &a, const Derived2 &bsrc, const Derived3 &csrc, typename typename Derived::Scalar epsilon) +{ + static_assert(Derived ::IsVectorAtCompileTime && int(Derived ::SizeAtCompileTime) == 2, "try_circle_center(): 1st point is not a 2D vector"); + static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "try_circle_center(): 2nd point is not a 2D vector"); + static_assert(Derived3::IsVectorAtCompileTime && int(Derived3::SizeAtCompileTime) == 2, "try_circle_center(): 3rd point is not a 2D vector"); + static_assert(std::is_same::value && std::is_same::value, + "try_circle_center(): All three points must be of the same type."); + using Scalar = typename Derived::Scalar; + using Vector = Eigen::Matrix; + 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. + return {}; + } else { + Vector v = lc * b - lb * c; + return std::make_optional(a + Vector(- v.y(), v.x()) / (2 * d)); + } +} + // 2D circle defined by its center and squared radius template struct CircleSq { diff --git a/tests/libslic3r/test_arc_welder.cpp b/tests/libslic3r/test_arc_welder.cpp index 8a700a24af..6c4b58e00f 100644 --- a/tests/libslic3r/test_arc_welder.cpp +++ b/tests/libslic3r/test_arc_welder.cpp @@ -2,9 +2,11 @@ #include #include +#include -TEST_CASE("arc_center", "[ArcWelder]") { - using namespace Slic3r; +using namespace Slic3r; + +TEST_CASE("arc basics", "[ArcWelder]") { using namespace Slic3r::Geometry; WHEN("arc from { 2000.f, 1000.f } to { 1000.f, 2000.f }") { @@ -89,3 +91,111 @@ TEST_CASE("arc_center", "[ArcWelder]") { } } } + +TEST_CASE("arc discretization", "[ArcWelder]") { + using namespace Slic3r::Geometry; + WHEN("arc from { 2, 1 } to { 1, 2 }") { + const Point p1 = Point::new_scale(2., 1.); + const Point p2 = Point::new_scale(1., 2.); + const Point center = Point::new_scale(1., 1.); + const float radius = scaled(1.); + const float resolution = scaled(0.002); + auto test = [center, resolution, radius](const Point &p1, const Point &p2, const float r, const bool ccw) { + Vec2f c = ArcWelder::arc_center(p1.cast(), p2.cast(), r, ccw); + REQUIRE((p1.cast() - c).norm() == Approx(radius)); + REQUIRE((c - center.cast()).norm() == Approx(0.)); + Points pts = ArcWelder::arc_discretize(p1, p2, r, ccw, resolution); + REQUIRE(pts.size() >= 2); + REQUIRE(pts.front() == p1); + REQUIRE(pts.back() == p2); + for (const Point &p : pts) + REQUIRE(std::abs((p.cast() - c.cast()).norm() - double(radius)) < double(resolution + SCALED_EPSILON)); + }; + THEN("90 degrees arc, CCW") { + test(p1, p2, radius, true); + } + THEN("270 degrees arc, CCW") { + test(p2, p1, - radius, true); + } + THEN("90 degrees arc, CW") { + test(p2, p1, radius, false); + } + THEN("270 degrees arc, CW") { + test(p1, p2, - radius, false); + } + } +} + +TEST_CASE("arc fitting", "[ArcWelder]") { + using namespace Slic3r::Geometry; + + WHEN("arc from { 2, 1 } to { 1, 2 }") { + const Point p1 = Point::new_scale(2., 1.); + const Point p2 = Point::new_scale(1., 2.); + const Point center = Point::new_scale(1., 1.); + const float radius = scaled(1.); + const float resolution = scaled(0.002); + auto test = [center, resolution, radius](const Point &p1, const Point &p2, const float r, const bool ccw) { + Points pts = ArcWelder::arc_discretize(p1, p2, r, ccw, resolution); + ArcWelder::Path path = ArcWelder::fit_path(pts, resolution + SCALED_EPSILON, ArcWelder::default_scaled_resolution); + REQUIRE(path.size() == 2); + REQUIRE(path.front().point == p1); + REQUIRE(path.front().radius == 0.f); + REQUIRE(path.back().point == p2); + REQUIRE(path.back().radius == Approx(radius)); + REQUIRE(path.back().ccw() == ccw); + }; + THEN("90 degrees arc, CCW is fitted") { + test(p1, p2, radius, true); + } + THEN("270 degrees arc, CCW is fitted") { + test(p2, p1, - radius, true); + } + THEN("90 degrees arc, CW is fitted") { + test(p2, p1, radius, false); + } + THEN("270 degrees arc, CW is fitted") { + test(p1, p2, - radius, false); + } + } + + WHEN("arc from { 2, 1 } to { 1, 2 }, another arc from { 2, 1 } to { 0, 2 }, tangentially connected") { + const Point p1 = Point::new_scale(2., 1.); + const Point p2 = Point::new_scale(1., 2.); + const Point p3 = Point::new_scale(0., 3.); + const Point center1 = Point::new_scale(1., 1.); + const Point center2 = Point::new_scale(1., 3.); + const float radius = scaled(1.); + const float resolution = scaled(0.002); + auto test = [center1, center2, resolution, radius](const Point &p1, const Point &p2, const Point &p3, const float r, const bool ccw) { + Points pts = ArcWelder::arc_discretize(p1, p2, r, ccw, resolution); + { + Points pts2 = ArcWelder::arc_discretize(p2, p3, - r, ! ccw, resolution); + REQUIRE(pts.back() == pts2.front()); + pts.insert(pts.end(), std::next(pts2.begin()), pts2.end()); + } + ArcWelder::Path path = ArcWelder::fit_path(pts, resolution + SCALED_EPSILON, ArcWelder::default_scaled_resolution); + REQUIRE(path.size() == 3); + REQUIRE(path.front().point == p1); + REQUIRE(path.front().radius == 0.f); + REQUIRE(path[1].point == p2); + REQUIRE(path[1].radius == Approx(radius)); + REQUIRE(path[1].ccw() == ccw); + REQUIRE(path.back().point == p3); + REQUIRE(path.back().radius == Approx(radius)); + REQUIRE(path.back().ccw() == ! ccw); + }; + THEN("90 degrees arches, CCW are fitted") { + test(p1, p2, p3, radius, true); + } + THEN("270 degrees arc, CCW is fitted") { + test(p3, p2, p1, -radius, true); + } + THEN("90 degrees arc, CW is fitted") { + test(p3, p2, p1, radius, false); + } + THEN("270 degrees arc, CW is fitted") { + test(p1, p2, p3, -radius, false); + } + } +} diff --git a/tests/libslic3r/test_geometry.cpp b/tests/libslic3r/test_geometry.cpp index 48fc8fd72e..c828465292 100644 --- a/tests/libslic3r/test_geometry.cpp +++ b/tests/libslic3r/test_geometry.cpp @@ -196,6 +196,41 @@ TEST_CASE("Offseting a line generates a polygon correctly", "[Geometry]"){ REQUIRE(area.area() == Slic3r::Polygon(Points({Point(10,5),Point(20,5),Point(20,15),Point(10,15)})).area()); } +SCENARIO("Circle Fit, 3 points", "[Geometry]") { + WHEN("Three points make a circle") { + double s1 = scaled(1.); + THEN("circle_center(): A center point { 0, 0 } is returned") { + Vec2d center = Geometry::circle_center(Vec2d{ s1, 0. }, Vec2d{ 0, s1 }, Vec2d{ -s1, 0. }, SCALED_EPSILON); + REQUIRE(is_approx(center, Vec2d(0, 0))); + } + THEN("circle_center(): A center point { 0, 0 } is returned for points in reverse") { + Vec2d center = Geometry::circle_center(Vec2d{ -s1, 0. }, Vec2d{ 0, s1 }, Vec2d{ s1, 0. }, SCALED_EPSILON); + REQUIRE(is_approx(center, Vec2d(0, 0))); + } + THEN("try_circle_center(): A center point { 0, 0 } is returned") { + std::optional center = Geometry::try_circle_center(Vec2d{ s1, 0. }, Vec2d{ 0, s1 }, Vec2d{ -s1, 0. }, SCALED_EPSILON); + REQUIRE(center); + REQUIRE(is_approx(*center, Vec2d(0, 0))); + } + THEN("try_circle_center(): A center point { 0, 0 } is returned for points in reverse") { + std::optional center = Geometry::try_circle_center(Vec2d{ -s1, 0. }, Vec2d{ 0, s1 }, Vec2d{ s1, 0. }, SCALED_EPSILON); + REQUIRE(center); + REQUIRE(is_approx(*center, Vec2d(0, 0))); + } + } + WHEN("Three points are collinear") { + double s1 = scaled(1.); + THEN("circle_center(): A center point { 2, 0 } is returned") { + Vec2d center = Geometry::circle_center(Vec2d{ s1, 0. }, Vec2d{ 2. * s1, 0. }, Vec2d{ 3. * s1, 0. }, SCALED_EPSILON); + REQUIRE(is_approx(center, Vec2d(2. * s1, 0))); + } + THEN("try_circle_center(): Fails for collinear points") { + std::optional center = Geometry::try_circle_center(Vec2d{ s1, 0. }, Vec2d{ 2. * s1, 0. }, Vec2d{ 3. * s1, 0. }, SCALED_EPSILON); + REQUIRE(! center); + } + } +} + SCENARIO("Circle Fit, TaubinFit with Newton's method", "[Geometry]") { GIVEN("A vector of Vec2ds arranged in a half-circle with approximately the same distance R from some point") { Vec2d expected_center(-6, 0);