From 95641d026924d2446e388c232c47a12213eb37cb Mon Sep 17 00:00:00 2001
From: Vojtech Bubnik <bubnikv@gmail.com>
Date: Mon, 24 Jul 2023 17:18:20 +0200
Subject: [PATCH] ArcWelder: Fixed arc_angle() in case the two arc points are
 further than a circle diameter. This may happen for a perfectly valid 180
 degrees circle due to numerical errors.

Added some asserts and comments elsewhere.
---
 src/libslic3r/GCode/GCodeWriter.cpp  |  8 ++++++++
 src/libslic3r/Geometry/ArcWelder.hpp |  6 ++++--
 src/libslic3r/Geometry/Circle.cpp    | 11 ++++++++---
 3 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/src/libslic3r/GCode/GCodeWriter.cpp b/src/libslic3r/GCode/GCodeWriter.cpp
index 4375e2e1de..2dad407caa 100644
--- a/src/libslic3r/GCode/GCodeWriter.cpp
+++ b/src/libslic3r/GCode/GCodeWriter.cpp
@@ -375,6 +375,9 @@ bool GCodeWriter::will_move_z(double z) const
 
 std::string GCodeWriter::extrude_to_xy(const Vec2d &point, double dE, const std::string_view comment)
 {
+    assert(dE != 0);
+    assert(std::abs(dE) < 1000.0);
+
     m_pos.head<2>() = point.head<2>();
 
     GCodeG1Formatter w;
@@ -386,6 +389,7 @@ std::string GCodeWriter::extrude_to_xy(const Vec2d &point, double dE, const std:
 
 std::string GCodeWriter::extrude_to_xy_G2G3IJ(const Vec2d &point, const Vec2d &ij, const bool ccw, double dE, const std::string_view comment)
 {
+    assert(std::abs(dE) < 1000.0);
     assert(dE != 0);
     assert(std::abs(point.x()) < 1200.);
     assert(std::abs(point.y()) < 1200.);
@@ -404,6 +408,7 @@ std::string GCodeWriter::extrude_to_xy_G2G3IJ(const Vec2d &point, const Vec2d &i
 std::string GCodeWriter::extrude_to_xy_G2G3R(const Vec2d &point, const double radius, const bool ccw, double dE, const std::string_view comment)
 {
     assert(dE != 0);
+    assert(std::abs(dE) < 1000.0);
     assert(std::abs(point.x()) < 1200.);
     assert(std::abs(point.y()) < 1200.);
     assert(std::abs(radius) < 1800.);
@@ -456,6 +461,9 @@ std::string GCodeWriter::retract_for_toolchange(bool before_wipe)
 
 std::string GCodeWriter::_retract(double length, double restart_extra, const std::string_view comment)
 {
+    assert(std::abs(length) < 1000.0);
+    assert(std::abs(restart_extra) < 1000.0);
+
     /*  If firmware retraction is enabled, we use a fake value of 1
         since we ignore the actual configured retract_length which 
         might be 0, in which case the retraction logic gets skipped. */
diff --git a/src/libslic3r/Geometry/ArcWelder.hpp b/src/libslic3r/Geometry/ArcWelder.hpp
index 5bf806c702..b328c12786 100644
--- a/src/libslic3r/Geometry/ArcWelder.hpp
+++ b/src/libslic3r/Geometry/ArcWelder.hpp
@@ -52,8 +52,10 @@ inline typename Derived::Scalar arc_angle(
     static_assert(std::is_same<typename Derived::Scalar, typename Derived2::Scalar>::value, "arc_angle(): Both vectors must be of the same type.");
     assert(radius != 0);
     using Float = typename Derived::Scalar;
-    Float a     = Float(2.) * asin(Float(0.5) * (end_pos - start_pos).norm() / radius);
-    return radius > Float(0) ? a : Float(2. * M_PI) + a;
+    Float a = Float(0.5) * (end_pos - start_pos).norm() / radius;
+    return radius > Float(0) ?
+        (a > Float( 1.) ? Float(   M_PI) : Float(2.) * std::asin(a)) :
+        (a < Float(-1.) ? Float( - M_PI) : Float(2. * M_PI) + Float(2.) * std::asin(a));
 }
 
 // Calculate positive length of an arc given two points and a radius.
diff --git a/src/libslic3r/Geometry/Circle.cpp b/src/libslic3r/Geometry/Circle.cpp
index 6796671954..cc222aa155 100644
--- a/src/libslic3r/Geometry/Circle.cpp
+++ b/src/libslic3r/Geometry/Circle.cpp
@@ -17,9 +17,14 @@ Point circle_center_taubin_newton(const Points::const_iterator& input_begin, con
 	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.
+// Robust and accurate algebraic circle fit, which works well even if data points are observed only within a small arc.
+// The method was proposed by G. Taubin in
+//      "Estimation Of Planar Curves, Surfaces And Nonplanar Space Curves Defined By Implicit Equations, 
+//       With Applications To Edge And Range Image Segmentation", IEEE Trans. PAMI, Vol. 13, pages 1115-1138, (1991)."
+// This particular implementation was adapted from
+//      "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