diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index 6a5328af31..b1c4d8e77e 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -247,6 +247,213 @@ double ConstraintCenterOfGravity::grad(double *param) return scale * deriv; } +// Slope at B-spline knot + +ConstraintSlopeAtBSplineKnot::ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, size_t knotindex) +{ + // set up pvec: pole x-coords, pole y-coords, pole weights, + // line point 1 coords, line point 2 coords + + numpoles = b.degree - b.mult[knotindex] + 1; + // slope at knot doesn't make sense if there's only C0 continuity + assert(numpoles >= 2); + + pvec.reserve(3*numpoles + 4); + + size_t startpole = 0; + // TODO: Adjust for periodic knot + for (size_t j = 1; j <= knotindex; ++j) + startpole += b.mult[j]; + if (!b.periodic && startpole >= b.poles.size()) + startpole = b.poles.size() - 1; + + for (size_t i = 0; i < numpoles; ++i) + pvec.push_back(b.poles[(startpole + i) % b.poles.size()].x); + for (size_t i = 0; i < numpoles; ++i) + pvec.push_back(b.poles[(startpole + i) % b.poles.size()].y); + for (size_t i = 0; i < numpoles; ++i) + pvec.push_back(b.weights[(startpole + i) % b.weights.size()]); + pvec.push_back(l.p1.x); + pvec.push_back(l.p1.y); + pvec.push_back(l.p2.x); + pvec.push_back(l.p2.y); + + // TODO: Set up factors to get slope at knot point + std::vector tempfactors((numpoles + 1), 1.0 / (numpoles + 1)); + factors.resize(numpoles); + slopefactors.resize(numpoles); + for (size_t i = 0; i < numpoles + 1; ++i) { + // FIXME: `getLinCombFactor` seg-faults for the last knot so this safeguard exists + if (numpoles > 2) { + tempfactors[i] = + b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i, b.degree - 1) / + (b.flattenedknots[startpole + b.degree + i] - b.flattenedknots[startpole + i]); + } + } + for (size_t i = 0; i < numpoles; ++i) { + factors[i] = + b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i); + slopefactors[i] = b.degree * (tempfactors[i] - tempfactors[i+1]); + } + + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintSlopeAtBSplineKnot::getTypeId() +{ + return SlopeAtBSplineKnot; +} + +void ConstraintSlopeAtBSplineKnot::rescale(double coef) +{ + double slopex = 0., slopey = 0.; + double linex = *linep2x() - *linep1x(); + double liney = *linep2y() - *linep1y(); + + for (size_t i = 0; i < numpoles; ++i) { + slopex += *polexat(i) * slopefactors[i]; + slopey += *poleyat(i) * slopefactors[i]; + } + + scale = coef / sqrt((slopex*slopex + slopey*slopey) * + (linex*linex + liney*liney)); +} + +double ConstraintSlopeAtBSplineKnot::error() +{ + // TODO: Fill up + double xsum = 0., xslopesum = 0.; + double ysum = 0., yslopesum = 0.; + double wsum = 0., wslopesum = 0.; + + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; + double wslopecontrib = *weightat(i) * slopefactors[i]; + wsum += wcontrib; + xsum += *polexat(i) * wcontrib; + ysum += *poleyat(i) * wcontrib; + wslopesum += wslopecontrib; + xslopesum += *polexat(i) * wslopecontrib; + yslopesum += *poleyat(i) * wslopecontrib; + } + + // This is actually wsum^2 * the respective slopes + double slopex = wsum*xslopesum - wslopesum*xsum; + double slopey = wsum*yslopesum - wslopesum*ysum; + double linex = *linep2x() - *linep1x(); + double liney = *linep2y() - *linep1y(); + + // error is the cross product + return scale * (slopex*liney - slopey*linex); +} + +double ConstraintSlopeAtBSplineKnot::grad(double *param) +{ + double result = 0.0; + double linex = *linep2x() - *linep1x(); + double liney = *linep2y() - *linep1y(); + + // TODO: Fill up + for (size_t i = 0; i < numpoles; ++i) { + if (param == polexat(i)) { + double wsum = 0., wslopesum = 0.; + for (size_t j = 0; j < numpoles; ++j) { + double wcontrib = *weightat(j) * factors[j]; + double wslopecontrib = *weightat(j) * slopefactors[j]; + wsum += wcontrib; + wslopesum += wslopecontrib; + } + result += (wsum*slopefactors[i] - wslopesum*factors[i]) * liney; + } + if (param == poleyat(i)) { + double wsum = 0., wslopesum = 0.; + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; + double wslopecontrib = *weightat(i) * slopefactors[i]; + wsum += wcontrib; + wslopesum += wslopecontrib; + } + result += - (wsum*slopefactors[i] - wslopesum*factors[i]) * linex; + } + if (param == weightat(i)) { + double xsum = 0., xslopesum = 0.; + double ysum = 0., yslopesum = 0.; + for (size_t j = 0; j < numpoles; ++j) { + double wcontrib = *weightat(j) * factors[j]; + double wslopecontrib = *weightat(j) * slopefactors[j]; + xsum += wcontrib * (*polexat(j) - *polexat(i)); + xslopesum += wslopecontrib * (*polexat(j) - *polexat(i)); + ysum += wcontrib * (*poleyat(j) - *poleyat(i)); + yslopesum += wslopecontrib * (*poleyat(j) - *poleyat(i)); + } + result += + (factors[i]*xslopesum - slopefactors[i]*xsum) * liney - + (factors[i]*yslopesum - slopefactors[i]*ysum) * linex; + } + } + + double slopex = 0., slopey = 0.; + bool slopexcomputed = false, slopeycomputed = false; + + auto getSlopeX = [&]() { + if (slopexcomputed) + return slopex; + + double xsum = 0., xslopesum = 0.; + double wsum = 0., wslopesum = 0.; + + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; + double wslopecontrib = *weightat(i) * slopefactors[i]; + wsum += wcontrib; + xsum += *polexat(i) * wcontrib; + wslopesum += wslopecontrib; + xslopesum += *polexat(i) * wslopecontrib; + } + + // This is actually wsum^2 * the respective slopes + slopex = wsum*xslopesum - wslopesum*xsum; + + slopexcomputed = true; + return slopex; + }; + + auto getSlopeY = [&]() { + if (slopeycomputed) + return slopey; + + double ysum = 0., yslopesum = 0.; + double wsum = 0., wslopesum = 0.; + + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; + double wslopecontrib = *weightat(i) * slopefactors[i]; + wsum += wcontrib; + ysum += *poleyat(i) * wcontrib; + wslopesum += wslopecontrib; + yslopesum += *poleyat(i) * wslopecontrib; + } + + // this is actually wsum^2 * the respective slopes + slopey = wsum*yslopesum - wslopesum*ysum; + + slopeycomputed = true; + return slopey; + }; + + if (param == linep1x()) + result += getSlopeY(); + if (param == linep2x()) + result += -getSlopeY(); + if (param == linep1y()) + result += -getSlopeX(); + if (param == linep2y()) + result += getSlopeX(); + + return scale * result; +} + // 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 d971d72750..df6328a229 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -70,7 +70,8 @@ namespace GCS EqualFocalDistance = 24, EqualLineLength = 25, CenterOfGravity = 26, - WeightedLinearCombination = 27 + WeightedLinearCombination = 27, + SlopeAtBSplineKnot = 28 }; enum InternalAlignmentType { @@ -206,6 +207,31 @@ namespace GCS size_t numpoles; }; + // Slope at knot + class ConstraintSlopeAtBSplineKnot : public Constraint + { + private: + inline double* polexat(size_t i) { return pvec[i]; } + inline double* poleyat(size_t i) { return pvec[numpoles + i]; } + inline double* weightat(size_t i) { return pvec[2*numpoles + i]; } + inline double* linep1x() { return pvec[3*numpoles + 0]; } + inline double* linep1y() { return pvec[3*numpoles + 1]; } + inline double* linep2x() { return pvec[3*numpoles + 2]; } + inline double* linep2y() { return pvec[3*numpoles + 3]; } + public: + // TODO: Should be able to make the geometries passed const + // Constrains the slope at a (C1 continuous) knot of the b-spline + ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, size_t knotindex); + ConstraintType getTypeId() override; + void rescale(double coef=1.) override; + double error() override; + double grad(double *) override; + private: + std::vector factors; + std::vector slopefactors; + size_t numpoles; + }; + // Difference class ConstraintDifference : public Constraint { diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index 89a4a64052..85ac511c78 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -812,6 +812,14 @@ int System::addConstraintTangentCircumf(Point &p1, Point &p2, double *rad1, doub return addConstraint(constr); } +int System::addConstraintTangentAtBSplineKnot(BSpline &b, Line &l, size_t knotindex, int tagId, bool driving) +{ + Constraint *constr = new ConstraintSlopeAtBSplineKnot(b, l, knotindex); + constr->setTag(tagId); + constr->setDriving(driving); + return addConstraint(constr); +} + // derived constraints int System::addConstraintP2PCoincident(Point &p1, Point &p2, int tagId, bool driving) diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index 460135f680..aa55774cdd 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -257,6 +257,8 @@ namespace GCS int tagId=0, bool driving = true); int addConstraintTangentCircumf(Point &p1, Point &p2, double *rd1, double *rd2, bool internal=false, int tagId=0, bool driving = true); + int addConstraintTangentAtBSplineKnot(BSpline &b, Line &l, size_t knotindex, + int tagId=0, bool driving = true); // derived constraints int addConstraintP2PCoincident(Point &p1, Point &p2, int tagId=0, bool driving = true);