diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index 6dbb94a139..17b85b1c7b 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -131,40 +131,13 @@ double ConstraintEqual::grad(double *param) // Weighted Linear Combination -const std::vector< std::vector > ConstraintWeightedLinearCombination::defaultfactors = -{{}, - {1.0}, - {0.5, 0.5}, - {0.16666666666666666, 0.6666666666666667, 0.16666666666666669}, - {0.041666666666666664, 0.45833333333333337, 0.4583333333333333, 0.04166666666666667}, - {0.008333333333333333, 0.21666666666666667, 0.55, 0.2166666666666667, 0.008333333333333333}, - {0.0013888888888888887, 0.07916666666666666, 0.41944444444444445, 0.41944444444444445, 0.07916666666666669, 0.0013888888888888885}, - {0.0001984126984126984, 0.023809523809523815, 0.2363095238095238, 0.4793650793650793, 0.2363095238095238, 0.02380952380952381, 0.0001984126984126984}, - {2.48015873015873e-05, 0.006125992063492064, 0.10647321428571428, 0.3873759920634921, 0.38737599206349216, 0.10647321428571431, 0.006125992063492064, 2.48015873015873e-05}, - {2.7557319223985893e-06, 0.0013833774250440916, 0.04025573192239859, 0.24314925044091712, 0.43041776895943573, 0.24314925044091715, 0.04025573192239859, 0.0013833774250440916, 2.7557319223985905e-06}}; - - -ConstraintWeightedLinearCombination::ConstraintWeightedLinearCombination(size_t givennumpoints, const std::vector& givenpvec) - : numpoints(givennumpoints) -{ - pvec = givenpvec; - setupInputs(); -} - -ConstraintWeightedLinearCombination::ConstraintWeightedLinearCombination(size_t givennumpoints, const std::vector& givenpvec, const std::vector& givenfactors) +ConstraintWeightedLinearCombination::ConstraintWeightedLinearCombination(size_t givennumpoles, const std::vector& givenpvec, const std::vector& givenfactors) : factors(givenfactors) - , numpoints(givennumpoints) + , numpoles(givennumpoles) { pvec = givenpvec; - setupInputs(); -} - -void ConstraintWeightedLinearCombination::setupInputs() -{ - assert(pvec.size() == 2*numpoints + 1); - if (factors.empty()) - factors = defaultfactors[numpoints]; - assert(factors.size() == numpoints); + assert(pvec.size() == 2*numpoles + 1); + assert(factors.size() == numpoles); origpvec = pvec; rescale(); } @@ -181,43 +154,48 @@ void ConstraintWeightedLinearCombination::rescale(double coef) double ConstraintWeightedLinearCombination::error() { - double offset = 1; - double woffset = 1 + numpoints; + // Explanation of the math here: + // https://forum.freecadweb.org/viewtopic.php?f=9&t=71130&start=120#p635538 double sum = 0; double wsum = 0; - for (size_t i = 0; i < numpoints; ++i) { - double wcontrib = *pvec[woffset + i] * factors[i]; + for (size_t i = 0; i < numpoles; ++i) { + double wcontrib = *weightat(i) * factors[i]; wsum += wcontrib; - sum += *pvec[offset + i] * wcontrib; + sum += *poleat(i) * wcontrib; } - return scale * ((*pvec[0]) * wsum - sum); + return scale * ((*thepoint()) * wsum - sum); } double ConstraintWeightedLinearCombination::grad(double *param) { - double offset = 1; - double woffset = 1 + numpoints; - - // TODO: Do we assume pvec are different so param can only be one? + // Equations are from here: + // https://forum.freecadweb.org/viewtopic.php?f=9&t=71130&start=120#p635538 double deriv=0.; - if (param == pvec[0]) { + + if (param == thepoint()) { + // Eq. (11) double wsum = 0; - for (size_t i = 0; i < numpoints; ++i) { - wsum += *pvec[woffset + i] * factors[i]; + for (size_t i = 0; i < numpoles; ++i) { + wsum += *weightat(i) * factors[i]; } - deriv += wsum; + deriv = wsum; + return scale * deriv; } - for (size_t i = 0; i < numpoints; ++i) { - if (param == pvec[offset + i]) { - deriv += -(*pvec[woffset + i] * factors[i]); + for (size_t i = 0; i < numpoles; ++i) { + if (param == poleat(i)) { + // Eq. (12) + deriv = -(*weightat(i) * factors[i]); + return scale * deriv; } - if (param == pvec[woffset + i]) { - deriv += (*pvec[0] - *pvec[offset + i]) * factors[i]; + if (param == weightat(i)) { + // Eq. (13) + deriv = (*thepoint() - *poleat(i)) * factors[i]; + return scale * deriv; } } @@ -226,39 +204,12 @@ double ConstraintWeightedLinearCombination::grad(double *param) // Center of Gravity -const std::vector< std::vector > ConstraintCenterOfGravity::defaultweights = -{{}, - {1.0}, - {0.5, 0.5}, - {0.16666666666666666, 0.6666666666666667, 0.16666666666666669}, - {0.041666666666666664, 0.45833333333333337, 0.4583333333333333, 0.04166666666666667}, - {0.008333333333333333, 0.21666666666666667, 0.55, 0.2166666666666667, 0.008333333333333333}, - {0.0013888888888888887, 0.07916666666666666, 0.41944444444444445, 0.41944444444444445, 0.07916666666666669, 0.0013888888888888885}, - {0.0001984126984126984, 0.023809523809523815, 0.2363095238095238, 0.4793650793650793, 0.2363095238095238, 0.02380952380952381, 0.0001984126984126984}, - {2.48015873015873e-05, 0.006125992063492064, 0.10647321428571428, 0.3873759920634921, 0.38737599206349216, 0.10647321428571431, 0.006125992063492064, 2.48015873015873e-05}, - {2.7557319223985893e-06, 0.0013833774250440916, 0.04025573192239859, 0.24314925044091712, 0.43041776895943573, 0.24314925044091715, 0.04025573192239859, 0.0013833774250440916, 2.7557319223985905e-06}}; - - -ConstraintCenterOfGravity::ConstraintCenterOfGravity(const std::vector& givenpvec) -{ - pvec = givenpvec; - numpoints = pvec.size() - 1; - setupInputs(); -} - ConstraintCenterOfGravity::ConstraintCenterOfGravity(const std::vector& givenpvec, const std::vector& givenweights) : weights(givenweights) { pvec = givenpvec; numpoints = pvec.size() - 1; - setupInputs(); -} - -void ConstraintCenterOfGravity::setupInputs() -{ assert(pvec.size() > 1); - if (weights.empty()) - weights = defaultweights[numpoints]; assert(weights.size() == numpoints); origpvec = pvec; rescale(); @@ -277,19 +228,21 @@ void ConstraintCenterOfGravity::rescale(double coef) double ConstraintCenterOfGravity::error() { double sum = 0; - for (size_t i = 1; i < pvec.size(); ++i) - sum += *pvec[i] * weights[i-1]; + for (size_t i = 0; i < numpoints; ++i) + sum += *pointat(i) * weights[i]; - return scale * (*pvec[0] - sum); + return scale * (*thecenter() - sum); } double ConstraintCenterOfGravity::grad(double *param) { double deriv=0.; - if (param == pvec[0]) deriv += 1; + if (param == thecenter()) + deriv = 1; - for (size_t i = 1; i < pvec.size(); ++i) - if (param == pvec[i]) deriv += -weights[i-1]; + for (size_t i = 0; i < numpoints; ++i) + if (param == pointat(i)) + deriv = -weights[i]; return scale * deriv; } diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index 31c7f95b1b..685f5fcfa3 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -160,17 +160,19 @@ namespace GCS // Center of Gravity class ConstraintCenterOfGravity : public Constraint { + inline double* thecenter() { return pvec[0]; } + inline double* pointat(size_t i) { return pvec[1 + i]; } public: /// Constrains that the first parameter is center of gravity of rest - ConstraintCenterOfGravity(const std::vector& givenpvec); + /// Let `pvec = [q, p_1, p_2,...]`, and + /// `givenweights = [f_1, f_2,...]`, then this constraint ensures + /// `q = sum(p_i*f_i)`. ConstraintCenterOfGravity(const std::vector& givenpvec, const std::vector& givenweights); ConstraintType getTypeId() override; void rescale(double coef=1.) override; double error() override; double grad(double *) override; private: - void setupInputs(); - static const std::vector< std::vector > defaultweights; std::vector weights; double numpoints; }; @@ -178,24 +180,30 @@ namespace GCS // Weighted Linear Combination class ConstraintWeightedLinearCombination : public Constraint { + inline double* thepoint() { return pvec[0]; } + inline double* poleat(size_t i) { return pvec[1 + i]; } + inline double* weightat(size_t i) { return pvec[1 + numpoles + i]; } public: /// Constrains that the first element in pvec is a linear combination /// of the next numpoints elements in homogeneous coordinates with /// weights given in the last numpoints elements. - /// Let pvec = [q, p_1, p_2,... w_1, w_2,...], and - /// givenfactors = [f_1, f_2,...], then this constraint ensures - /// q*sum(w_i*f_i) = sum(p_i*w_i*f_i). - ConstraintWeightedLinearCombination(size_t givennumpoints, const std::vector& givenpvec); + /// Let `pvec = [q, p_1, p_2,... w_1, w_2,...]`, and + /// `givenfactors = [f_1, f_2,...]`, then this constraint ensures + /// `q*sum(w_i*f_i) = sum(p_i*w_i*f_i)`. + /// + /// This constraint is currently used to ensure that a B-spline knot + /// point remains at the position determined by it's poles. + /// In that case, `q` is the x (or y) coordinate of the knot, `p_i` are + /// the x (or y) coordinates of the poles, and `w_i` are their weights. + /// Finally, `f_i` are obtained using `BSpline::getLinCombFactor()`. ConstraintWeightedLinearCombination(size_t givennumpoints, const std::vector& givenpvec, const std::vector& givenfactors); ConstraintType getTypeId() override; void rescale(double coef=1.) override; double error() override; double grad(double *) override; private: - void setupInputs(); - static const std::vector< std::vector > defaultfactors; std::vector factors; - size_t numpoints; + size_t numpoles; }; // Difference diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index 3ee7f0dfa0..77c5a09ce3 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -1345,27 +1345,46 @@ int System::addConstraintInternalAlignmentKnotPoint(BSpline &b, Point &p, unsign if (numpoles == 0) numpoles = 1; + // `startpole` is the first pole affecting the knot with `knotindex` size_t startpole = 0; std::vector pvec; pvec.push_back(p.x); - std::vector weights(numpoles, 1.0 / numpoles); + std::vector factors(numpoles, 1.0 / numpoles); - // TODO: Adjust for periodic knot + // Only poles with indices `[i, i+1,... i+b.degree]` affect the interval + // `flattenedknots[b.degree+i]` to `flattenedknots[b.degree+i+1]`. + // When a knot has higher multiplicity, it can be seen as spanning + // multiple of these intervals, and thus is only affected by an + // intersection of the poles that affect it. + // The `knotindex` gives us the intervals, so work backwards and find + // the affecting poles. + // Note that this works also for periodic B-splines, just that the poles wrap around if needed. for (size_t j = 1; j <= knotindex; ++j) startpole += b.mult[j]; + // For the last knot, even the upper limit of the last interval range + // is included. So offset for that. + // For periodic B-splines the `flattenedknots` are defined differently, + // so this is not needed for them. if (!b.periodic && startpole >= b.poles.size()) startpole = b.poles.size() - 1; - for (size_t i = 0; i < numpoles; ++i) { + + // Calculate the factors to be passed to weighted linear combination constraint. + // The if condition has a small performance benefit, but that is not why it is here. + // One case when numpoles <= 2 is for the last knot of a non-periodic B-spline. + // In this case, the interval `k` passed to `getLinCombFactor` is degenerate, and this is the cleanest way to handle it. + if (numpoles > 2) + for (size_t i = 0; i < numpoles; ++i) + factors[i] = b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i); + + // The mod operation is to adjust for periodic B-splines. + // This can be separated for performance reasons but it will be less readable. + for (size_t i = 0; i < numpoles; ++i) pvec.push_back(b.poles[(startpole + i) % b.poles.size()].x); - // FIXME: `getLinCombFactor` seg-faults for the last knot so this safeguard exists - if (numpoles > 2) - weights[i] = b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i); - } for (size_t i = 0; i < numpoles; ++i) pvec.push_back(b.weights[(startpole + i) % b.poles.size()]); - Constraint *constr = new ConstraintWeightedLinearCombination(numpoles, pvec, weights); + Constraint *constr = new ConstraintWeightedLinearCombination(numpoles, pvec, factors); constr->setTag(tagId); constr->setDriving(driving); constr->setInternalAlignment(Constraint::Alignment::InternalAlignment); @@ -1378,7 +1397,7 @@ int System::addConstraintInternalAlignmentKnotPoint(BSpline &b, Point &p, unsign for (size_t i = 0; i < numpoles; ++i) pvec.push_back(b.weights[(startpole + i) % b.poles.size()]); - constr = new ConstraintWeightedLinearCombination(numpoles, pvec, weights); + constr = new ConstraintWeightedLinearCombination(numpoles, pvec, factors); constr->setTag(tagId); constr->setDriving(driving); constr->setInternalAlignment(Constraint::Alignment::InternalAlignment); diff --git a/src/Mod/Sketcher/App/planegcs/Geo.cpp b/src/Mod/Sketcher/App/planegcs/Geo.cpp index 0c053cf982..d765429fea 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.cpp +++ b/src/Mod/Sketcher/App/planegcs/Geo.cpp @@ -718,12 +718,21 @@ double BSpline::getLinCombFactor(double x, size_t k, size_t i) // https://en.wikipedia.org/wiki/De_Boor%27s_algorithm. // FIXME: This should probably be guaranteed by now, and done somewhere else + // To elaborate: `flattenedknots` should be set up as soon as `knots` + // and `mult` have been defined after creating the B-spline. + // However, in the future the values of `knots` could go into the solver + // as well, when alternatives may be needed to keep `flattenedknots` updated. + // Slightly more detailed discussion here: + // https://github.com/FreeCAD/FreeCAD/pull/7484#discussion_r1020858392 if (flattenedknots.empty()) setupFlattenedKnots(); std::vector d(degree + 1, 0.0); - // TODO: Ensure this is within range - d[i + degree - k] = 1.0; + // Ensure this is within range + int idxOfPole = i + degree - k; + if (idxOfPole < 0 || idxOfPole > degree) + return 0.0; + d[idxOfPole] = 1.0; for (size_t r = 1; static_cast(r) < degree + 1; ++r) { for (size_t j = degree; j > r - 1; --j) {