[Sketcher] [planegcs] Add point-on-bspline constraint in planegcs

Only non-rational B-spline for now

src/Mod/Sketcher/App/planegcs/Constraints.cpp

@@ -462,6 +462,102 @@ double ConstraintSlopeAtBSplineKnot::grad(double *param)
     return scale * result;
 }
 
+// Point On BSpline
+
+ConstraintPointOnBSpline::ConstraintPointOnBSpline(double* point, double* initparam, int coordidx, BSpline& b)
+    : bsp(b)
+{
+    // This is always going to be true
+    numpoints = b.degree + 1;
+
+    pvec.reserve(2 + b.degree + 1);
+    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 < b.mult.size() && *(b.knots[j]) <= *initparam; ++j)
+        startpole += b.mult[j];
+    if (!b.periodic && startpole >= b.poles.size())
+        startpole = b.poles.size() - 1;
+
+    for (size_t i = 0; i < numpoints; ++i) {
+        if (coordidx == 0)
+            pvec.push_back(b.poles[(startpole + i) % b.poles.size()].x);
+        else
+            pvec.push_back(b.poles[(startpole + i) % b.poles.size()].y);
+    }
+
+    factors.resize(numpoints);
+    if (b.flattenedknots.empty())
+        b.setupFlattenedKnots();
+
+    origpvec = pvec;
+    rescale();
+}
+
+ConstraintType ConstraintPointOnBSpline::getTypeId()
+{
+    return PointOnBSpline;
+}
+
+void ConstraintPointOnBSpline::rescale(double coef)
+{
+    scale = coef * 1.0;
+}
+
+double ConstraintPointOnBSpline::error()
+{
+    // We need to find factors over and over again because param can change.
+    // TODO: We don't need to find the factors here, just for grad.
+    // Calculate position with factors because we have the method.
+    double sum = 0;
+
+    // TODO: maybe make it global so it doesn't have to be created every time
+    VEC_D d(numpoints);
+    // TODO: Also support rational b-splines
+    for (size_t i = 0; i < numpoints; ++i)
+        d[i] = *poleat(i);
+    sum += BSpline::splineValue(*theparam(), startpole + bsp.degree, bsp.degree, d, bsp.flattenedknots);
+
+    // TODO: Change the poles as the point moves between pieces
+
+    return scale * (*thepoint() - sum);
+}
+
+double ConstraintPointOnBSpline::grad(double *gcsparam)
+{
+    double deriv=0.;
+    if (gcsparam == thepoint()) {
+        // TODO: Change this for rational splines
+        deriv += 1.;
+    }
+
+    if (gcsparam == theparam()) {
+        // TODO: Implement derivative and return value
+        VEC_D d(numpoints - 1);
+        for (size_t i = 1; i < numpoints; ++i) {
+            d[i-1] =
+                (*poleat(i) - *poleat(i-1)) /
+                (bsp.flattenedknots[startpole+i+bsp.degree] - bsp.flattenedknots[startpole+i]);
+        }
+        double slopevalue = BSpline::splineValue(*theparam(), startpole + bsp.degree, bsp.degree-1, d, bsp.flattenedknots);
+        deriv -= bsp.degree * slopevalue;
+    }
+
+    for (size_t i = 0; i < numpoints; ++i) {
+        if (gcsparam == poleat(i)) {
+            factors[i] = bsp.getLinCombFactor(*theparam(), startpole + bsp.degree, startpole + i);
+            deriv += -factors[i];
+        }
+    }
+
+    return scale * deriv;
+}
+
 // Difference
 ConstraintDifference::ConstraintDifference(double *p1, double *p2, double *d)
 {

src/Mod/Sketcher/App/planegcs/Constraints.h

@@ -71,7 +71,8 @@ namespace GCS
         EqualLineLength = 25,
         CenterOfGravity = 26,
         WeightedLinearCombination = 27,
-        SlopeAtBSplineKnot = 28
+        SlopeAtBSplineKnot = 28,
+        PointOnBSpline = 29
     };
 
     enum InternalAlignmentType {
@@ -232,6 +233,29 @@ namespace GCS
         size_t numpoles;
     };
 
+    // Point On BSpline
+    class ConstraintPointOnBSpline : public Constraint
+    {
+    private:
+        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]; }
+    public:
+        /// TODO: Explain how it's provided
+        /// coordidx = 0 if x, 1 if y
+        ConstraintPointOnBSpline(double* point, double* initparam, int coordidx, BSpline& b);
+        ConstraintType getTypeId() override;
+        void rescale(double coef=1.) override;
+        double error() override;
+        double grad(double *) override;
+        void setupInputs();
+        std::vector<double> factors;
+        size_t numpoints;
+        BSpline& bsp;
+        unsigned int startpole;
+    };
+
     // Difference
     class ConstraintDifference : public Constraint
     {

src/Mod/Sketcher/App/planegcs/GCS.cpp

@@ -893,6 +893,19 @@ int System::addConstraintPointOnParabolicArc(Point &p, ArcOfParabola &e, int tag
     return addConstraint(constr);
 }
 
+int System::addConstraintPointOnBSpline(Point &p, BSpline &b, double* pointparam, int tagId, bool driving)
+{
+    Constraint *constr = new ConstraintPointOnBSpline(p.x, pointparam, 0, b);
+    constr->setTag(tagId);
+    constr->setDriving(driving);
+    addConstraint(constr);
+
+    constr = new ConstraintPointOnBSpline(p.y, pointparam, 1, b);
+    constr->setTag(tagId);
+    constr->setDriving(driving);
+    return addConstraint(constr);
+}
+
 int System::addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId, bool driving)
 {
     addConstraintCurveValue(a.start,a,a.startAngle, tagId, driving);

src/Mod/Sketcher/App/planegcs/GCS.h

@@ -273,6 +273,7 @@ namespace GCS
         int addConstraintPointOnEllipse(Point &p, Ellipse &e, int tagId=0, bool driving = true);
         int addConstraintPointOnHyperbolicArc(Point &p, ArcOfHyperbola &e, int tagId=0, bool driving = true);
         int addConstraintPointOnParabolicArc(Point &p, ArcOfParabola &e, int tagId=0, bool driving = true);
+        int addConstraintPointOnBSpline(Point &p, BSpline &b, double* pointparam, int tagId, bool driving = true);
         int addConstraintArcOfEllipseRules(ArcOfEllipse &a, int tagId=0, bool driving = true);
         int addConstraintCurveValue(Point &p, Curve &a, double *u, int tagId=0, bool driving = true);
         int addConstraintArcOfHyperbolaRules(ArcOfHyperbola &a, int tagId=0, bool driving = true);