From a95248288b88e13be3215d4ced5676998dd59de8 Mon Sep 17 00:00:00 2001
From: stribor14 <mirko.kokot@romb-technologies.hr>
Date: Tue, 1 Oct 2024 13:59:38 +0200
Subject: [PATCH] Rework polynomial solving and refactor a bit

---
 include/Bezier/utils.h | 22 ++++++----------------
 src/bezier.cpp         | 25 ++++---------------------
 src/utils.cpp          | 23 +++++++++++++++++++++++
 3 files changed, 33 insertions(+), 37 deletions(-)

diff --git a/include/Bezier/utils.h b/include/Bezier/utils.h
index c17c4e8..3245bfb 100644
--- a/include/Bezier/utils.h
+++ b/include/Bezier/utils.h
@@ -15,17 +15,6 @@ namespace Utils
  */
 const double _epsilon = std::sqrt(std::numeric_limits<double>::epsilon());
 
-struct _PolynomialRoots : public std::vector<double>
-{
-  explicit _PolynomialRoots(unsigned reserve) { std::vector<double>::reserve(reserve); }
-  void clear() {}          // no-op so that PolynomialSolver::RealRoots() doesn't clear it
-  void push_back(double t) // only allow valid roots
-  {
-    if (t >= 0 && t <= 1)
-      std::vector<double>::push_back(t);
-  }
-};
-
 inline unsigned _exp2(unsigned exp) { return 1 << exp; }
 
 template <typename T> inline T _pow(T base, unsigned exp)
@@ -49,12 +38,11 @@ inline Eigen::RowVectorXd _powSeries(double base, unsigned exp)
   return power_series;
 }
 
-inline Eigen::VectorXd _trimZeroes(const Eigen::VectorXd& vec)
+template <typename T> inline std::vector<T> _concatenate(std::vector<T>&& v1, std::vector<T>&& v2)
 {
-  auto idx = vec.size();
-  while (idx && std::abs(vec(idx - 1)) < _epsilon)
-    --idx;
-  return vec.head(idx);
+  v1.reserve(v1.size() + v2.size());
+  v1.insert(v1.end(), std::make_move_iterator(v2.begin()), std::make_move_iterator(v2.end()));
+  return std::move(v1);
 }
 
 inline double _polylineLength(const PointVector& polyline)
@@ -86,6 +74,8 @@ inline double _polylineDist(const PointVector& polyline, const Point& point)
 
 PointVector _polylineSimplify(const PointVector& polyline, unsigned N);
 
+std::vector<double> _solvePolynomial(const Eigen::VectorXd& polynomial);
+
 } // namespace Utils
 } // namespace Bezier
 
diff --git a/src/bezier.cpp b/src/bezier.cpp
index add9c87..4b0539f 100644
--- a/src/bezier.cpp
+++ b/src/bezier.cpp
@@ -7,7 +7,6 @@
 #include <unsupported/Eigen/LevenbergMarquardt>
 #include <unsupported/Eigen/MatrixFunctions>
 #include <unsupported/Eigen/NumericalDiff>
-#include <unsupported/Eigen/Polynomials>
 
 using namespace Bezier;
 using namespace Bezier::Utils;
@@ -320,14 +319,7 @@ std::vector<double> Curve::roots() const
   if (!cached_roots_)
     cached_roots_ = N_ < 2 ? std::vector<double>{} : [this] {
       Eigen::MatrixXd bezier_polynomial = bernsteinCoeffs(N_) * control_points_;
-      auto trimmed_x = _trimZeroes(bezier_polynomial.col(0));
-      auto trimmed_y = _trimZeroes(bezier_polynomial.col(1));
-      _PolynomialRoots roots(trimmed_x.size() + trimmed_y.size());
-      if (trimmed_x.size() > 1)
-        Eigen::PolynomialSolver<double, Eigen::Dynamic>(trimmed_x).realRoots(roots);
-      if (trimmed_y.size() > 1)
-        Eigen::PolynomialSolver<double, Eigen::Dynamic>(trimmed_y).realRoots(roots);
-      return roots;
+      return _concatenate(_solvePolynomial(bezier_polynomial.col(0)), _solvePolynomial(bezier_polynomial.col(1)));
     }();
 
   return cached_roots_.value();
@@ -448,14 +440,10 @@ double Curve::projectPoint(const Point& point) const
   polynomial.topRows(cached_projection_polynomial_derivative_->rows()) -=
       cached_projection_polynomial_derivative_.value() * point;
 
-  auto trimmed = _trimZeroes(polynomial);
-  _PolynomialRoots candidates(trimmed.size());
-  if (trimmed.size() > 1)
-    Eigen::PolynomialSolver<double, Eigen::Dynamic>(trimmed).realRoots(candidates);
-
   double min_t = (point - valueAt(0.0)).norm() < (point - valueAt(1.0)).norm() ? 0.0 : 1.0;
   double min_dist = (point - valueAt(min_t)).norm();
 
+  auto candidates = _solvePolynomial(polynomial);
   return std::accumulate(candidates.begin(), candidates.end(), std::make_pair(min_t, min_dist),
                          [this, &point](std::pair<double, double> min, double t) {
                            double dist = (point - valueAt(t)).norm();
@@ -524,13 +512,8 @@ Curve Curve::joinCurves(const Curve& curve1, const Curve& curve2, unsigned int o
 {
   if (order == 1)
     return Curve(PointVector{curve1.control_points_.row(0), curve2.control_points_.row(curve2.N_ - 1)});
-
-  auto polyline = curve1.polyline();
-  auto polyline2 = curve2.polyline();
-  polyline.reserve(polyline.size() + polyline2.size());
-  polyline.insert(polyline.end(), std::make_move_iterator(polyline2.begin()), std::make_move_iterator(polyline2.end()));
-
-  return fromPolyline(polyline, order ? order : curve1.order() + curve2.order());
+  return fromPolyline(_concatenate(curve1.polyline(), curve2.polyline()),
+                      order ? order : curve1.order() + curve2.order());
 }
 
 Curve Curve::fromPolyline(const PointVector& polyline, unsigned int order)
diff --git a/src/utils.cpp b/src/utils.cpp
index b3d5c8a..800cfdd 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -2,6 +2,8 @@
 
 #include <numeric>
 
+#include <unsupported/Eigen/Polynomials>
+
 using namespace Bezier;
 using namespace Bezier::Utils;
 
@@ -72,3 +74,24 @@ PointVector Bezier::Utils::_polylineSimplify(const PointVector& polyline, unsign
     simplified.push_back(polyline[k]);
   return simplified;
 }
+
+std::vector<double> Bezier::Utils::_solvePolynomial(const Eigen::VectorXd& polynomial)
+{
+  auto idx = polynomial.size();
+  while (idx && std::abs(polynomial(idx - 1)) < _epsilon)
+    --idx;
+
+  struct PolynomialRoots : public std::vector<double>
+  {
+    void push_back(double t) // only allow valid roots
+    {
+      if (t >= 0 && t <= 1)
+        std::vector<double>::push_back(t);
+    }
+  } roots;
+  roots.reserve(idx);
+
+  if (idx > 1)
+    Eigen::PolynomialSolver<double, Eigen::Dynamic>(polynomial.head(idx)).realRoots(roots);
+  return roots;
+}