From 5ba050b4674464ff3b5df3d86f9390089da8b39e Mon Sep 17 00:00:00 2001 From: Ajinkya Dahale Date: Wed, 5 Apr 2023 17:45:25 +0530 Subject: [PATCH] [Sketcher][planegcs] Implement parametric `BSpline::CalculateNormal` As opposed to "punctual" that already exists for curves. --- src/Mod/Sketcher/App/planegcs/Geo.cpp | 153 ++++++++++++++++++++++++++ src/Mod/Sketcher/App/planegcs/Geo.h | 7 +- 2 files changed, 157 insertions(+), 3 deletions(-) diff --git a/src/Mod/Sketcher/App/planegcs/Geo.cpp b/src/Mod/Sketcher/App/planegcs/Geo.cpp index b085384fd6..46872cb2dd 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.cpp +++ b/src/Mod/Sketcher/App/planegcs/Geo.cpp @@ -773,6 +773,159 @@ DeriVector2 BSpline::CalculateNormal(const Point& p, const double* derivparam) c return ret; } +DeriVector2 BSpline::CalculateNormal(const double* param, const double* derivparam) const +{ + // TODO: is there any advantage in making this a `static`? + size_t startpole = 0; + for (size_t j = 1; j < mult.size() && *(knots[j]) <= *param; ++j) + startpole += mult[j]; + if (!periodic && startpole >= poles.size()) + startpole = poles.size() - degree - 1; + + // double xsum = 0., xslopesum = 0.; + // double ysum = 0., yslopesum = 0.; + // double wsum = 0., wslopesum = 0.; + + auto polexat = [&](size_t i) { return poles[(startpole + i) % poles.size()].x; }; + auto poleyat = [&](size_t i) { return poles[(startpole + i) % poles.size()].y; }; + auto weightat = [&](size_t i) { return weights[(startpole + i) % weights.size()]; }; + + size_t numpoints = degree + 1; + // Tangent vector + // This should in principle be identical to error gradient wrt curve parameter in point-on-object + VEC_D d(numpoints); + for (size_t i = 0; i < numpoints; ++i) + d[i] = *polexat(i) * *weightat(i); + double xsum = BSpline::splineValue(*param, startpole + degree, degree, d, flattenedknots); + for (size_t i = 0; i < numpoints; ++i) + d[i] = *poleyat(i) * *weightat(i); + double ysum = BSpline::splineValue(*param, startpole + degree, degree, d, flattenedknots); + for (size_t i = 0; i < numpoints; ++i) + d[i] = *weightat(i); + double wsum = BSpline::splineValue(*param, startpole + degree, degree, d, flattenedknots); + + d.resize(numpoints - 1); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*polexat(i) * *weightat(i) - *polexat(i - 1) * *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double xslopesum = degree * BSpline::splineValue( + *param, startpole + degree, degree - 1, d, flattenedknots); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*poleyat(i) * *weightat(i) - *poleyat(i - 1) * *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double yslopesum = degree * BSpline::splineValue( + *param, startpole + degree, degree - 1, d, flattenedknots); + for (size_t i = 1; i < numpoints; ++i) { + d[i - 1] = (*weightat(i) - *weightat(i - 1)) + / (flattenedknots[startpole + i + degree] - flattenedknots[startpole + i]); + } + double wslopesum = degree * BSpline::splineValue( + *param, startpole + degree, degree - 1, d, flattenedknots); + + DeriVector2 result(wsum*xslopesum - wslopesum*xsum, wsum*yslopesum - wslopesum*ysum); + + // get dx, dy of the normal as well + bool dpfound = false; + for (size_t i = 0; i < numpoints; ++i) { + if (derivparam == polexat(i)) { + VEC_D d(numpoints); + d[i] = 1; + double factor = BSpline::splineValue(*param, startpole + degree, degree, d, flattenedknots); + VEC_D sd(numpoints-1); + if (i > 0) + sd[i-1] = 1.0 / (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + if (i < numpoints - 1) + sd[i] = -1.0 / (flattenedknots[startpole+i+1+degree] - flattenedknots[startpole+i+1]); + double slopefactor = BSpline::splineValue(*param, startpole + degree, degree-1, sd, flattenedknots); + result.dx = *weightat(i) * (wsum*slopefactor - wslopesum*factor); + dpfound = true; + break; + } + if (derivparam == poleyat(i)) { + VEC_D d(numpoints); + d[i] = 1; + double factor = BSpline::splineValue(*param, startpole + degree, degree, d, flattenedknots); + VEC_D sd(numpoints-1); + if (i > 0) + sd[i-1] = 1.0 / (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + if (i < numpoints - 1) + sd[i] = -1.0 / (flattenedknots[startpole+i+1+degree] - flattenedknots[startpole+i+1]); + double slopefactor = BSpline::splineValue(*param, startpole + degree, degree-1, sd, flattenedknots); + result.dy = *weightat(i) * (wsum*slopefactor - wslopesum*factor); + dpfound = true; + break; + } + if (derivparam == weightat(i)) { + VEC_D d(numpoints); + d[i] = 1; + double factor = BSpline::splineValue(*param, startpole + degree, degree, d, flattenedknots); + VEC_D sd(numpoints-1); + if (i > 0) + sd[i-1] = 1.0 / (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + if (i < numpoints - 1) + sd[i] = -1.0 / (flattenedknots[startpole+i+1+degree] - flattenedknots[startpole+i+1]); + double slopefactor = BSpline::splineValue(*param, startpole + degree, degree-1, sd, flattenedknots); + result.dx = degree * + (factor * (xslopesum - wslopesum*(*polexat(i))) - slopefactor * (xsum - wsum*(*polexat(i)))); + result.dy = degree * + (factor * (yslopesum - wslopesum*(*poleyat(i))) - slopefactor * (ysum - wsum*(*poleyat(i)))); + dpfound = true; + break; + } + } + + // the curve parameter being used by the constraint is not known to the geometry (there can be + // many tangent constraints on the same curve after all). Assume that this is the param provided. + if (derivparam == param) { + VEC_D sd(numpoints-1), ssd(numpoints-2); + for (size_t i = 1; i < numpoints; ++i) { + sd[i-1] = + (*weightat(i) - *weightat(i-1)) / + (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + } + for (size_t i = 1; i < numpoints-1; ++i) { + ssd[i-1] = + (sd[i] - sd[i-1]) / + (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + } + double wslopeslopesum = degree * (degree - 1) * + BSpline::splineValue(*param, startpole + degree, degree-2, ssd, flattenedknots); + + for (size_t i = 1; i < numpoints; ++i) { + sd[i-1] = + (*polexat(i) * *weightat(i) - *polexat(i-1) * *weightat(i-1)) / + (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + } + for (size_t i = 1; i < numpoints-1; ++i) { + ssd[i-1] = + (sd[i] - sd[i-1]) / + (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + } + double xslopeslopesum = degree * (degree - 1) * + BSpline::splineValue(*param, startpole + degree, degree-2, ssd, flattenedknots); + + for (size_t i = 1; i < numpoints; ++i) { + sd[i-1] = + (*poleyat(i) * *weightat(i) - *poleyat(i-1) * *weightat(i-1)) / + (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + } + for (size_t i = 1; i < numpoints-1; ++i) { + ssd[i-1] = + (sd[i] - sd[i-1]) / + (flattenedknots[startpole+i+degree] - flattenedknots[startpole+i]); + } + double yslopeslopesum = degree * (degree - 1) * + BSpline::splineValue(*param, startpole + degree, degree-2, ssd, flattenedknots); + + result.dx = wsum*xslopeslopesum - wslopeslopesum*xsum; + result.dy = wsum*yslopeslopesum - wslopeslopesum*ysum; + } + + return result.rotate90ccw(); +} + DeriVector2 BSpline::Value(double /*u*/, double /*du*/, const double* /*derivparam*/) const { // place holder diff --git a/src/Mod/Sketcher/App/planegcs/Geo.h b/src/Mod/Sketcher/App/planegcs/Geo.h index fa7f6c0455..f65bdfdfd1 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.h +++ b/src/Mod/Sketcher/App/planegcs/Geo.h @@ -162,7 +162,8 @@ public: const double* derivparam = nullptr) const = 0; // returns normal vector at parameter instead of at the given point. - virtual DeriVector2 CalculateNormal(const double* param, const double* derivparam = nullptr) + virtual DeriVector2 CalculateNormal(const double* param, + const double* derivparam = nullptr) const { DeriVector2 pointDV = Value(*param, 0.0); Point p(&pointDV.x, &pointDV.y); @@ -423,8 +424,8 @@ public: VEC_D flattenedknots; DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; // TODO: override parametric version - // DeriVector2 CalculateNormal(const double* param, const double* derivparam = nullptr) const - // override; + DeriVector2 CalculateNormal(const double* param, + const double* derivparam = nullptr) const override; DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; int PushOwnParams(VEC_pD& pvec) override; void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override;