From b21f5d5916c6e0137b7e5699ec786f86966d507f Mon Sep 17 00:00:00 2001
From: tomate44 <cg@grellier.fr>
Date: Mon, 28 Sep 2020 08:52:26 +0200
Subject: [PATCH] Part: add BezierCurve constrained interpolation

---
 src/Mod/Part/App/BezierCurvePy.xml    |  10 +++
 src/Mod/Part/App/BezierCurvePyImp.cpp | 100 ++++++++++++++++++++++++++
 2 files changed, 110 insertions(+)

diff --git a/src/Mod/Part/App/BezierCurvePy.xml b/src/Mod/Part/App/BezierCurvePy.xml
index 466b89c40c4a..7256e64e025f 100644
--- a/src/Mod/Part/App/BezierCurvePy.xml
+++ b/src/Mod/Part/App/BezierCurvePy.xml
@@ -156,5 +156,15 @@ ensures that:
 |t1-t0| &lt; UTolerance =""==&gt; |f(t1)-f(t0)| &lt; Tolerance3D</UserDocu>
 			</Documentation>
 		</Methode>
+        <Methode Name="interpolate">
+			<Documentation>
+				<UserDocu>Interpolates a list of constraints.
+				Each constraint is a list of a point and some optional derivatives
+				An optional list of parameters can be passed. It must be of same size as constraint list.
+				Otherwise, a simple uniform parametrisation is used.
+				Example :
+				bezier.interpolate([[pt1, deriv11, deriv12], [pt2,], [pt3, deriv31]], [0, 0.5, 1.0])</UserDocu>
+			</Documentation>
+		</Methode>
 	</PythonExport>
 </GenerateModel>
diff --git a/src/Mod/Part/App/BezierCurvePyImp.cpp b/src/Mod/Part/App/BezierCurvePyImp.cpp
index 32275a5b8932..93779a43661a 100644
--- a/src/Mod/Part/App/BezierCurvePyImp.cpp
+++ b/src/Mod/Part/App/BezierCurvePyImp.cpp
@@ -27,6 +27,9 @@
 # include <gp_Pnt.hxx>
 # include <TColStd_Array1OfReal.hxx>
 # include <TColgp_Array1OfPnt.hxx>
+# include <math_Matrix.hxx>
+# include <math_Gauss.hxx>
+# include <BSplCLib.hxx>
 #endif
 
 #include <Base/VectorPy.h>
@@ -380,6 +383,103 @@ Py::Object BezierCurvePy::getEndPoint(void) const
     return Py::Vector(Base::Vector3d(pnt.X(), pnt.Y(), pnt.Z()));
 }
 
+PyObject* BezierCurvePy::interpolate(PyObject * args)
+{
+    PyObject* obj;
+    PyObject* par=0;
+    if (!PyArg_ParseTuple(args, "O|O", &obj, &par))
+        return 0;
+    try {
+        Handle(Geom_BezierCurve) curve = Handle(Geom_BezierCurve)::DownCast
+        (getGeometryPtr()->handle());
+        Py::Sequence constraints(obj);
+        int nb_pts = constraints.size();
+        if (nb_pts < 2)
+            Standard_Failure::Raise("not enough points given");
+        
+        TColStd_Array1OfReal params(1, nb_pts);
+        if (par) {
+            Py::Sequence plist(par);
+            int param_size = plist.size();
+            if (param_size != nb_pts)
+                Standard_Failure::Raise("number of points and parameters don't match");
+            int idx=1;
+            for (Py::Sequence::iterator pit = plist.begin(); pit != plist.end(); ++pit) {
+                Py::Float val(*pit);
+                params(idx++) = (double)val;
+            }
+        }
+        else {
+            for (int idx=0; idx<nb_pts; ++idx) {
+                params(idx+1) = (double)idx/((double)nb_pts-1);
+            }
+        }
+        
+        int num_poles = 0;
+        for (Py::Sequence::iterator it1 = constraints.begin(); it1 != constraints.end(); ++it1) {
+            Py::Sequence row(*it1);
+            for (Py::Sequence::iterator it2 = row.begin(); it2 != row.end(); ++it2) {
+                num_poles++;
+            }
+        }
+        if (num_poles > curve->MaxDegree())
+            Standard_Failure::Raise("number of constraints exceeds bezier curve capacity");
+        // create a bezier-type knot sequence
+        TColStd_Array1OfReal knots(1, 2*num_poles);
+        for (int idx=1; idx<=num_poles; ++idx) {
+            knots(idx) = params(1);
+            knots(num_poles+idx) = params(nb_pts);
+        }
+        math_Matrix OCCmatrix(1, num_poles, 1, num_poles, 0.0);
+        math_Vector res_x(1, num_poles, 0.0);
+        math_Vector res_y(1, num_poles, 0.0);
+        math_Vector res_z(1, num_poles, 0.0);
+        int row_idx = 1;
+        int cons_idx = 1;
+        for (Py::Sequence::iterator it1 = constraints.begin(); it1 != constraints.end(); ++it1) {
+            Py::Sequence row(*it1);
+            math_Matrix bezier_eval(1, row.size(), 1, num_poles, 0.0);
+            Standard_Integer first_non_zero;
+            Standard_Integer error_code = BSplCLib::EvalBsplineBasis(row.size()-1, num_poles, knots, params(cons_idx), first_non_zero, bezier_eval, Standard_False);
+            int idx2 = 1;
+            for (Py::Sequence::iterator it2 = row.begin(); it2 != row.end(); ++it2) {
+                OCCmatrix.SetRow(row_idx, bezier_eval.Row(idx2));
+                Py::Vector v(*it2);
+                Base::Vector3d pnt = v.toVector();
+                res_x(row_idx) = pnt.x;
+                res_y(row_idx) = pnt.y;
+                res_z(row_idx) = pnt.z;
+                idx2++;
+                row_idx++;
+            }
+            cons_idx++;
+        }
+        math_Gauss gauss(OCCmatrix);
+        gauss.Solve(res_x);
+        if (!gauss.IsDone())
+            Standard_Failure::Raise("Failed to solve equations");
+        gauss.Solve(res_y);
+        if (!gauss.IsDone())
+            Standard_Failure::Raise("Failed to solve equations");
+        gauss.Solve(res_z);
+        if (!gauss.IsDone())
+            Standard_Failure::Raise("Failed to solve equations");
+
+        TColgp_Array1OfPnt poles(1,num_poles);
+        for (int idx=1; idx<=num_poles; ++idx) {
+            poles.SetValue(idx, gp_Pnt(res_x(idx),res_y(idx),res_z(idx)));
+        }
+
+        Handle(Geom_BezierCurve) bezier = new Geom_BezierCurve(poles);
+        this->getGeomBezierCurvePtr()->setHandle(bezier);
+        Py_Return;
+    }
+    catch (Standard_Failure& e) {
+        PyErr_SetString(PartExceptionOCCError, e.GetMessageString());
+        return 0;
+    }
+}
+
 PyObject *BezierCurvePy::getCustomAttributes(const char* /*attr*/) const
 {
     return 0;