Added implementation of Taubin fit cribbed from "Circular and Linear Regression: Fitting circles and lines by least squares", p126 by Nikolai Chernov for both Pointf and Points,

Added tests for fit and extended some operators to function on Pointf

src/test/libslic3r/test_geometry.cpp

@@ -161,6 +161,98 @@ TEST_CASE("Offseting a line generates a polygon correctly"){
     REQUIRE(area.area() == Polygon(std::vector<Point>({Point(10,5),Point(20,5),Point(20,15),Point(10,15)})).area());
 }
 
+SCENARIO("Circle Fit, TaubinFit with Newton's method") {
+    GIVEN("A vector of Pointfs arranged in a half-circle with approximately the same distance R from some point") {
+        Pointf expected_center(-6, 0);
+        Pointfs sample {Pointf(6.0, 0), Pointf(5.1961524, 3), Pointf(3 ,5.1961524), Pointf(0, 6.0), Pointf(3, 5.1961524), Pointf(-5.1961524, 3), Pointf(-6.0, 0)};
+        std::transform(sample.begin(), sample.end(), sample.begin(), [expected_center] (const Pointf& a) { return a + expected_center;});
+
+        WHEN("Circle fit is called on the entire array") {
+            Pointf result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample);
+            THEN("A center point of -6,0 is returned.") {
+                REQUIRE(result_center == expected_center);
+            }
+        }
+        WHEN("Circle fit is called on the first four points") {
+            Pointf result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample.cbegin(), sample.cbegin()+4);
+            THEN("A center point of -6,0 is returned.") {
+                REQUIRE(result_center == expected_center);
+            }
+        }
+        WHEN("Circle fit is called on the middle four points") {
+            Pointf result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample.cbegin()+2, sample.cbegin()+6);
+            THEN("A center point of -6,0 is returned.") {
+                REQUIRE(result_center == expected_center);
+            }
+        }
+    }
+    GIVEN("A vector of Pointfs arranged in a half-circle with approximately the same distance R from some point") {
+        Pointf expected_center(-3, 9);
+        Pointfs sample {Pointf(6.0, 0), Pointf(5.1961524, 3), Pointf(3 ,5.1961524), 
+                        Pointf(0, 6.0), 
+                        Pointf(3, 5.1961524), Pointf(-5.1961524, 3), Pointf(-6.0, 0)};
+
+        std::transform(sample.begin(), sample.end(), sample.begin(), [expected_center] (const Pointf& a) { return a + expected_center;});
+
+
+        WHEN("Circle fit is called on the entire array") {
+            Pointf result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample);
+            THEN("A center point of 3,9 is returned.") {
+                REQUIRE(result_center == expected_center);
+            }
+        }
+        WHEN("Circle fit is called on the first four points") {
+            Pointf result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample.cbegin(), sample.cbegin()+4);
+            THEN("A center point of 3,9 is returned.") {
+                REQUIRE(result_center == expected_center);
+            }
+        }
+        WHEN("Circle fit is called on the middle four points") {
+            Pointf result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample.cbegin()+2, sample.cbegin()+6);
+            THEN("A center point of 3,9 is returned.") {
+                REQUIRE(result_center == expected_center);
+            }
+        }
+    }
+    GIVEN("A vector of Points arranged in a half-circle with approximately the same distance R from some point") {
+        Point expected_center { Point::new_scale(-3, 9)};
+        Points sample {Point::new_scale(6.0, 0), Point::new_scale(5.1961524, 3), Point::new_scale(3 ,5.1961524), 
+                        Point::new_scale(0, 6.0), 
+                        Point::new_scale(3, 5.1961524), Point::new_scale(-5.1961524, 3), Point::new_scale(-6.0, 0)};
+
+        std::transform(sample.begin(), sample.end(), sample.begin(), [expected_center] (const Point& a) { return a + expected_center;});
+
+
+        WHEN("Circle fit is called on the entire array") {
+            Point result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample);
+            THEN("A center point of scaled 3,9 is returned.") {
+                REQUIRE(result_center.coincides_with_epsilon(expected_center));
+            }
+        }
+        WHEN("Circle fit is called on the first four points") {
+            Point result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample.cbegin(), sample.cbegin()+4);
+            THEN("A center point of scaled 3,9 is returned.") {
+                REQUIRE(result_center.coincides_with_epsilon(expected_center));
+            }
+        }
+        WHEN("Circle fit is called on the middle four points") {
+            Point result_center(0,0);
+            result_center = Geometry::circle_taubin_newton(sample.cbegin()+2, sample.cbegin()+6);
+            THEN("A center point of scaled 3,9 is returned.") {
+                REQUIRE(result_center.coincides_with_epsilon(expected_center));
+            }
+        }
+    }
+}
+
 
 TEST_CASE("Chained path working correctly"){
     // if chained_path() works correctly, these points should be joined with no diagonal paths

xs/src/libslic3r/Geometry.cpp

@@ -2,9 +2,11 @@
 #include "ClipperUtils.hpp"
 #include "ExPolygon.hpp"
 #include "Line.hpp"
+#include "Log.hpp"
 #include "PolylineCollection.hpp"
 #include "clipper.hpp"
 #include <algorithm>
+#include <numeric>
 #include <cassert>
 #include <cmath>
 #include <list>
@@ -343,6 +345,109 @@ linint(double value, double oldmin, double oldmax, double newmin, double newmax)
     return (value - oldmin) * (newmax - newmin) / (oldmax - oldmin) + newmin;
 }
 
+Point
+circle_taubin_newton(const Points& input, size_t cycles)
+{
+    return circle_taubin_newton(input.cbegin(), input.cend(), cycles);
+}
+
+Point
+circle_taubin_newton(const Points::const_iterator& input_begin, const Points::const_iterator& input_end, size_t cycles)
+{
+    Pointfs tmp;
+    tmp.reserve(std::distance(input_begin, input_end));
+    std::transform(input_begin, input_end, std::back_inserter(tmp), [] (const Point& in) {return Pointf::new_unscale(in); } );
+    return Point::new_scale(circle_taubin_newton(tmp.cbegin(), tmp.end(), cycles));
+}
+
+Pointf
+circle_taubin_newton(const Pointfs& input, size_t cycles)
+{
+    return circle_taubin_newton(input.cbegin(), input.cend(), cycles);
+}
+
+
+/// 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.
+Pointf
+circle_taubin_newton(const Pointfs::const_iterator& input_begin, const Pointfs::const_iterator& input_end, size_t cycles)
+{
+    // calculate the centroid of the data set
+    const Pointf sum = std::accumulate(input_begin, input_end, Pointf(0,0));
+    const size_t n = std::distance(input_begin, input_end);
+    const double n_flt = static_cast<double>(n);
+    const Pointf 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)) {
+            Slic3r::Log::error("Geometry") << "Fit is going in the wrong direction.\n";
+            return Pointf(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;
+
+    Pointf center {Pointf(Mxz * (Myy - xnew) - Myz * Mxy, Myz * (Mxx - xnew) - Mxz*Mxy)};
+    center = center / DET / 2;
+
+    return center + centroid;
+    
+}
+
 /*
 == Perl implementations for methods tested in geometry.t but not translated.  ==
 == The first three are unreachable in the current perl code and the fourth is ==

xs/src/libslic3r/Geometry.hpp

@@ -25,6 +25,14 @@ double rad2deg(double angle);
 double rad2deg_dir(double angle);
 double deg2rad(double angle);
 
+/// Find the center of the circle corresponding to the vector of Points as an arc.
+Point circle_taubin_newton(const Points& input, size_t cycles = 20);
+Point circle_taubin_newton(const Points::const_iterator& input_start, const Points::const_iterator& input_end, size_t cycles = 20);
+
+/// Find the center of the circle corresponding to the vector of Pointfs as an arc.
+Pointf circle_taubin_newton(const Pointfs& input, size_t cycles = 20);
+Pointf circle_taubin_newton(const Pointfs::const_iterator& input_start, const Pointfs::const_iterator& input_end, size_t cycles = 20);
+
 /// Epsilon value
 constexpr double epsilon { 1e-4 };
 constexpr coord_t scaled_epsilon { static_cast<coord_t>(epsilon / SCALING_FACTOR) };

xs/src/libslic3r/Point.cpp

@@ -337,6 +337,9 @@ operator+(const Point& point1, const Point& point2)
     return Point(point1.x + point2.x, point1.y + point2.y);
 }
 
+
+
+
 Point
 operator-(const Point& point1, const Point& point2)
 {
@@ -371,6 +374,18 @@ Pointf::dump_perl() const
     return ss.str();
 }
 
+Pointf
+operator+(const Pointf& point1, const Pointf& point2)
+{
+    return Pointf(point1.x + point2.x, point1.y + point2.y);
+}
+
+Pointf
+operator/(const Pointf& point1, const double& scalar)
+{
+    return Pointf(point1.x / scalar, point1.y / scalar);
+}
+
 void
 Pointf::scale(double factor)
 {
@@ -425,6 +440,14 @@ Pointf::vector_to(const Pointf &point) const
     return Vectorf(point.x - this->x, point.y - this->y);
 }
 
+Pointf&
+Pointf::operator/=(const double& scalar)
+{
+    this->x /= scalar;
+    this->y /= scalar;
+    return *this;
+}
+
 std::ostream&
 operator<<(std::ostream &stm, const Pointf3 &pointf3)
 {
@@ -437,11 +460,15 @@ Pointf3::scale(double factor)
     Pointf::scale(factor);
     this->z *= factor;
 }
+bool Pointf::coincides_with_epsilon(const Pointf& rhs) const { 
+    return std::abs(this->x - rhs.x) < EPSILON && std::abs(this->y - rhs.y) < EPSILON ;
+}
 
 bool Pointf::operator==(const Pointf& rhs) const { 
-    return Point::new_scale(*this) == Point::new_scale(rhs);
+    return this->coincides_with_epsilon(rhs);
 }
 
+
 void
 Pointf3::translate(const Vectorf3 &vector)
 {

xs/src/libslic3r/Point.hpp

@@ -122,8 +122,10 @@ class Pointf
         return Pointf(unscale(p.x), unscale(p.y));
     };
 
-    // equality operator based on the scaled coordinates
+    // equality operator based on coincides_with_epsilon
     bool operator==(const Pointf& rhs) const;
+    bool coincides_with_epsilon(const Pointf& rhs) const;
+    Pointf& operator/=(const double& scalar); 
 
     std::string wkt() const;
     std::string dump_perl() const;
@@ -136,6 +138,9 @@ class Pointf
     Vectorf vector_to(const Pointf &point) const;
 };
 
+Pointf operator+(const Pointf& point1, const Pointf& point2);
+Pointf operator/(const Pointf& point1, const double& scalar);
+
 std::ostream& operator<<(std::ostream &stm, const Pointf3 &pointf3);
 
 class Pointf3 : public Pointf