From 4f3f5664232e8a4d9babea7e1201e763bffbe20a Mon Sep 17 00:00:00 2001 From: Ajinkya Dahale Date: Tue, 4 Oct 2022 13:42:20 +0530 Subject: [PATCH] [Sketcher] [planegcs] Add point-on-bspline constraint in planegcs Only non-rational B-spline for now --- src/Mod/Sketcher/App/planegcs/Constraints.cpp | 96 +++++++++++++++++++ src/Mod/Sketcher/App/planegcs/Constraints.h | 26 ++++- src/Mod/Sketcher/App/planegcs/GCS.cpp | 13 +++ src/Mod/Sketcher/App/planegcs/GCS.h | 1 + 4 files changed, 135 insertions(+), 1 deletion(-) diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index a583f3bc19..0b64ed7461 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/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) { diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index df6328a229..cf5e2b435c 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/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 factors; + size_t numpoints; + BSpline& bsp; + unsigned int startpole; + }; + // Difference class ConstraintDifference : public Constraint { diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index f1f925b7a6..952f0349f8 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/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); diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index 6de364afa6..29e9162f18 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/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);