From dcd869bb5d807cbe6849837d7d6d43e802c26cd7 Mon Sep 17 00:00:00 2001
From: Ajinkya Dahale <AjinkyaDahale@users.noreply.github.com>
Date: Wed, 18 Jan 2023 22:44:06 +0530
Subject: [PATCH] [Sketcher][planegcs] Dynamically change piece in
 point-on-BSpline

---
 src/Mod/Sketcher/App/planegcs/Constraints.cpp | 38 +++++++++++--------
 src/Mod/Sketcher/App/planegcs/Constraints.h   |  5 ++-
 2 files changed, 26 insertions(+), 17 deletions(-)

diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp
index f7bdff6cbd..d7ec7cf8a8 100644
--- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp
+++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp
@@ -470,28 +470,20 @@ ConstraintPointOnBSpline::ConstraintPointOnBSpline(double* point, double* initpa
     // This is always going to be true
     numpoints = bsp.degree + 1;
 
-    pvec.reserve(2 + 2*numpoints);
+    pvec.reserve(2 + 2*b.poles.size());
     pvec.push_back(point);
     pvec.push_back(initparam);
 
-    // The startpole logic is repeated in a lot of places,
-    // for example in GCS and slope at knot
-    // find relevant poles
-    startpole = 0;
-    // TODO: Adjust for periodic knot
-    for (size_t j = 1; j < bsp.mult.size() && *(bsp.knots[j]) <= *initparam; ++j)
-        startpole += bsp.mult[j];
-    if (!bsp.periodic && startpole >= bsp.poles.size())
-        startpole = bsp.poles.size() - bsp.degree - 1;
+    setStartPole(*initparam);
 
-    for (size_t i = 0; i < numpoints; ++i) {
+    for (size_t i = 0; i < b.poles.size(); ++i) {
         if (coordidx == 0)
-            pvec.push_back(bsp.poles[(startpole + i) % bsp.poles.size()].x);
+            pvec.push_back(b.poles[i].x);
         else
-            pvec.push_back(bsp.poles[(startpole + i) % bsp.poles.size()].y);
+            pvec.push_back(b.poles[i].y);
     }
-    for (size_t i = 0; i < numpoints; ++i)
-        pvec.push_back(bsp.weights[(startpole + i) % bsp.weights.size()]);
+    for (size_t i = 0; i < b.weights.size(); ++i)
+        pvec.push_back(b.weights[i]);
 
     if (bsp.flattenedknots.empty())
         bsp.setupFlattenedKnots();
@@ -505,6 +497,18 @@ ConstraintType ConstraintPointOnBSpline::getTypeId()
     return PointOnBSpline;
 }
 
+void ConstraintPointOnBSpline::setStartPole(double u)
+{
+    // The startpole logic is repeated in a lot of places,
+    // for example in GCS and slope at knot
+    // find relevant poles
+    startpole = 0;
+    for (size_t j = 1; j < bsp.mult.size() && *(bsp.knots[j]) <= u; ++j)
+        startpole += bsp.mult[j];
+    if (!bsp.periodic && startpole >= bsp.poles.size())
+        startpole = bsp.poles.size() - bsp.degree - 1;
+}
+
 void ConstraintPointOnBSpline::rescale(double coef)
 {
     scale = coef * 1.0;
@@ -512,6 +516,10 @@ void ConstraintPointOnBSpline::rescale(double coef)
 
 double ConstraintPointOnBSpline::error()
 {
+    if (*theparam() < bsp.flattenedknots[startpole + bsp.degree] ||
+        *theparam() > bsp.flattenedknots[startpole + bsp.degree + 1])
+        setStartPole(*theparam());
+
     double sum = 0;
     double wsum = 0;
 
diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h
index db9ef2fc45..7359d3e4fd 100644
--- a/src/Mod/Sketcher/App/planegcs/Constraints.h
+++ b/src/Mod/Sketcher/App/planegcs/Constraints.h
@@ -240,8 +240,9 @@ namespace GCS
         inline double* thepoint() { return pvec[0]; }
         // TODO: better name because param has a different meaning here?
         inline double* theparam() { return pvec[1]; }
-        inline double* poleat(size_t i) { return pvec[2 + i]; }
-        inline double* weightat(size_t i) { return pvec[2 + numpoints + i]; }
+        inline double* poleat(size_t i) { return pvec[2 + (startpole + i) % bsp.poles.size()]; }
+        inline double* weightat(size_t i) { return pvec[2 + bsp.poles.size() + (startpole + i) % bsp.weights.size()]; }
+        void setStartPole(double u);
     public:
         /// TODO: Explain how it's provided
         /// coordidx = 0 if x, 1 if y