diff --git a/src/libslic3r/Geometry/ArcWelder.hpp b/src/libslic3r/Geometry/ArcWelder.hpp index dfc6d886e6..8ef7d8585a 100644 --- a/src/libslic3r/Geometry/ArcWelder.hpp +++ b/src/libslic3r/Geometry/ArcWelder.hpp @@ -7,7 +7,7 @@ namespace Slic3r { namespace Geometry { namespace ArcWelder { -// Calculate center point of an arc given two points and a radius. +// Calculate center point (center of a circle) of an arc given two points and a radius. // positive radius: take shorter arc // negative radius: take longer arc // radius must NOT be zero! @@ -36,6 +36,37 @@ inline Eigen::Matrix arc_center( return (radius > Float(0)) == is_ccw ? (mid + vp).eval() : (mid - vp).eval(); } +// Calculate middle sample point (point on an arc) of an arc given two points and a radius. +// positive radius: take shorter arc +// negative radius: take longer arc +// radius must NOT be zero! +// Taking a sample at the middle of a convex arc (less than PI) is likely much more +// useful than taking a sample at the middle of a concave arc (more than PI). +template +inline Eigen::Matrix arc_middle_point( + const Eigen::MatrixBase &start_pos, + const Eigen::MatrixBase &end_pos, + const Float radius, + const bool is_ccw) +{ + static_assert(Derived::IsVectorAtCompileTime && int(Derived::SizeAtCompileTime) == 2, "arc_center(): first parameter is not a 2D vector"); + static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_center(): second parameter is not a 2D vector"); + static_assert(std::is_same::value, "arc_center(): Both vectors must be of the same type."); + static_assert(std::is_same::value, "arc_center(): Radius must be of the same type as the vectors."); + assert(radius != 0); + using Vector = Eigen::Matrix; + auto v = end_pos - start_pos; + Float q2 = v.squaredNorm(); + assert(q2 > 0); + Float t2 = sqr(radius) / q2 - Float(.25f); + // If the start_pos and end_pos are nearly antipodal, t2 may become slightly negative. + // In that case return a centroid of start_point & end_point. + Float t = (t2 > 0 ? sqrt(t2) : Float(0)) - radius / sqrt(q2); + auto mid = Float(0.5) * (start_pos + end_pos); + Vector vp{ -v.y() * t, v.x() * t }; + return (radius > Float(0)) == is_ccw ? (mid + vp).eval() : (mid - vp).eval(); +} + // Calculate angle of an arc given two points and a radius. // Returned angle is in the range <0, 2 PI) // positive radius: take shorter arc @@ -87,7 +118,7 @@ inline typename Derived::Scalar arc_length( { static_assert(Derived::IsVectorAtCompileTime && int(Derived::SizeAtCompileTime) == 2, "arc_length(): first parameter is not a 2D vector"); static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_length(): second parameter is not a 2D vector"); - static_assert(Derived3::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_length(): third parameter is not a 2D vector"); + static_assert(Derived3::IsVectorAtCompileTime && int(Derived3::SizeAtCompileTime) == 2, "arc_length(): third parameter is not a 2D vector"); static_assert(std::is_same::value && std::is_same::value, "arc_length(): All third points must be of the same type."); using Float = typename Derived::Scalar; @@ -103,6 +134,145 @@ inline typename Derived::Scalar arc_length( return angle * radius; } +// Be careful! This version has a strong bias towards small circles with small radii +// for small angle (nearly straight) arches! +// One should rather use arc_fit_center_gauss_newton_ls(), which solves a non-linear least squares problem. +// +// Calculate center point (center of a circle) of an arc given two fixed points to interpolate +// and an additional list of points to fit by least squares. +// The circle fitting problem is non-linear, it was linearized by taking difference of squares of radii as a residual. +// Therefore the center point is required as a point to linearize at. +// Start & end point must be different and the interpolated points must not be collinear with input points. +template +inline typename Eigen::Matrix arc_fit_center_algebraic_ls( + const Eigen::MatrixBase &start_pos, + const Eigen::MatrixBase &end_pos, + const Eigen::MatrixBase ¢er_pos, + const Iterator it_begin, + const Iterator it_end) +{ + static_assert(Derived::IsVectorAtCompileTime && int(Derived::SizeAtCompileTime) == 2, "arc_fit_center_algebraic_ls(): start_pos is not a 2D vector"); + static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_fit_center_algebraic_ls(): end_pos is not a 2D vector"); + static_assert(Derived3::IsVectorAtCompileTime && int(Derived3::SizeAtCompileTime) == 2, "arc_fit_center_algebraic_ls(): third parameter is not a 2D vector"); + static_assert(std::is_same::value && + std::is_same::value, "arc_fit_center_algebraic_ls(): All third points must be of the same type."); + using Float = typename Derived::Scalar; + using Vector = Eigen::Matrix; + // Prepare a vector space to transform the fitting into: + // center_pos, dir_x, dir_y + Vector v = end_pos - start_pos; + Vector c = Float(.5) * (start_pos + end_pos); + Float lv = v.norm(); + assert(lv > 0); + Vector dir_y = v / lv; + Vector dir_x = perp(dir_y); + // Center of the new system: + // Center X at the projection of center_pos + Float offset_x = dir_x.dot(center_pos); + // Center is supposed to lie on bisector of the arc end points. + // Center Y at the mid point of v. + Float offset_y = dir_y.dot(c); + assert(std::abs(dir_y.dot(center_pos) - offset_y) < SCALED_EPSILON); + assert((dir_x * offset_x + dir_y * offset_y - center_pos).norm() < SCALED_EPSILON); + // Solve the least squares fitting in a transformed space. + Float a = Float(0.5) * lv; + Float b = c.dot(dir_x) - offset_x; + Float ab2 = sqr(a) + sqr(b); + Float num = Float(0); + Float denom = Float(0); + const Float w = it_end - it_begin; + for (Iterator it = it_begin; it != it_end; ++ it) { + Vector p = *it; + Float x_i = dir_x.dot(p) - offset_x; + Float y_i = dir_y.dot(p) - offset_y; + Float x_i2 = sqr(x_i); + Float y_i2 = sqr(y_i); + num += (x_i - b) * (x_i2 + y_i2 - ab2); + denom += b * (b - Float(2) * x_i) + sqr(x_i) + Float(0.25) * w; + } + assert(denom != 0); + Float c_x = Float(0.5) * num / denom; + // Transform the center back. + Vector out = dir_x * (c_x + offset_x) + dir_y * offset_y; + return out; +} + +// Calculate center point (center of a circle) of an arc given two fixed points to interpolate +// and an additional list of points to fit by non-linear least squares. +// The non-linear least squares problem is solved by a Gauss-Newton iterative method. +// Start & end point must be different and the interpolated points must not be collinear with input points. +// Center position is used to calculate the initial solution of the Gauss-Newton method. +// +// In case the input points are collinear or close to collinear (such as a small angle arc), +// the solution may not converge and an error is indicated. +template +inline std::optional> arc_fit_center_gauss_newton_ls( + const Eigen::MatrixBase &start_pos, + const Eigen::MatrixBase &end_pos, + const Eigen::MatrixBase ¢er_pos, + const Iterator it_begin, + const Iterator it_end, + const size_t num_iterations) +{ + static_assert(Derived::IsVectorAtCompileTime && int(Derived::SizeAtCompileTime) == 2, "arc_fit_center_gauss_newton_ls(): start_pos is not a 2D vector"); + static_assert(Derived2::IsVectorAtCompileTime && int(Derived2::SizeAtCompileTime) == 2, "arc_fit_center_gauss_newton_ls(): end_pos is not a 2D vector"); + static_assert(Derived3::IsVectorAtCompileTime && int(Derived3::SizeAtCompileTime) == 2, "arc_fit_center_gauss_newton_ls(): third parameter is not a 2D vector"); + static_assert(std::is_same::value && + std::is_same::value, "arc_fit_center_gauss_newton_ls(): All third points must be of the same type."); + using Float = typename Derived::Scalar; + using Vector = Eigen::Matrix; + // Prepare a vector space to transform the fitting into: + // center_pos, dir_x, dir_y + Vector v = end_pos - start_pos; + Vector c = Float(.5) * (start_pos + end_pos); + Float lv = v.norm(); + assert(lv > 0); + Vector dir_y = v / lv; + Vector dir_x = perp(dir_y); + // Center is supposed to lie on bisector of the arc end points. + // Center Y at the mid point of v. + Float offset_y = dir_y.dot(c); + // Initial value of the parameter to be optimized iteratively. + Float c_x = dir_x.dot(center_pos); + // Solve the least squares fitting in a transformed space. + Float a = Float(0.5) * lv; + Float a2 = sqr(a); + Float b = c.dot(dir_x); + for (size_t iter = 0; iter < num_iterations; ++ iter) { + Float num = Float(0); + Float denom = Float(0); + Float u = b - c_x; + // Current estimate of the circle radius. + Float r = sqrt(a2 + sqr(u)); + assert(r > 0); + for (Iterator it = it_begin; it != it_end; ++ it) { + Vector p = *it; + Float x_i = dir_x.dot(p); + Float y_i = dir_y.dot(p) - offset_y; + Float v = x_i - c_x; + Float y_i2 = sqr(y_i); + // Distance of i'th sample from the current circle center. + Float r_i = sqrt(sqr(v) + y_i2); + if (r_i >= EPSILON) { + // Square of residual is differentiable at the current c_x and current sample. + // Jacobian: diff(residual, c_x) + Float j_i = u / r - v / r_i; + num += j_i * (r_i - r); + denom += sqr(j_i); + } else { + // Sample point is on current center of the circle, + // therefore the gradient is not defined. + } + } + if (denom == 0) + // Fitting diverged, the input points are likely nearly collinear with the arch end points. + return std::optional(); + c_x -= num / denom; + } + // Transform the center back. + return std::optional(dir_x * c_x + dir_y * offset_y); +} + // Test whether a point is inside a wedge of an arc. template inline bool inside_arc_wedge_vectors(