diff --git a/src/Mod/Sketcher/App/Sketch.cpp b/src/Mod/Sketcher/App/Sketch.cpp index e4424c8f82..30f2414f6f 100644 --- a/src/Mod/Sketcher/App/Sketch.cpp +++ b/src/Mod/Sketcher/App/Sketch.cpp @@ -617,12 +617,7 @@ int Sketch::addGeometry(const Part::Geometry *geo, bool fixed) const GeomPoint *point = static_cast(geo); auto pointf = GeometryFacade::getFacade(point); // create the definition struct for that geom - if( pointf->getInternalType() == InternalType::BSplineKnotPoint ) { - return addPoint(*point, true); - } - else { - return addPoint(*point, fixed); - } + return addPoint(*point, fixed); } else if (geo->getTypeId() == GeomLineSegment::getClassTypeId()) { // add a line const GeomLineSegment *lineSeg = static_cast(geo); @@ -1257,12 +1252,10 @@ int Sketch::addBSpline(const Part::GeomBSplineCurve &bspline, bool fixed) double * p1x = new double(startPnt.x); double * p1y = new double(startPnt.y); - // if periodic, startpoint and endpoint do not play a role in the solver, this removes unnecessary DoF of determining where in the curve - // the start and the stop should be - if(!periodic) { - params.push_back(p1x); - params.push_back(p1y); - } + // If periodic, startpoint and endpoint do not play a role in the solver, this can remove unnecessary DoF of determining where in the curve + // the start and the stop should be. However, since start and end points are placed above knots, removing them leads to that knot being unusable. + params.push_back(p1x); + params.push_back(p1y); p1.x = p1x; p1.y = p1y; @@ -1270,12 +1263,11 @@ int Sketch::addBSpline(const Part::GeomBSplineCurve &bspline, bool fixed) double * p2x = new double(endPnt.x); double * p2y = new double(endPnt.y); - // if periodic, startpoint and endpoint do not play a role in the solver, this removes unnecessary DoF of determining where in the curve - // the start and the stop should be - if(!periodic) { - params.push_back(p2x); - params.push_back(p2y); - } + // If periodic, startpoint and endpoint do not play a role in the solver, this can remove unnecessary DoF of determining where in the curve + // the start and the stop should be. However, since start and end points are placed above knots, removing them leads to that knot being unusable. + params.push_back(p2x); + params.push_back(p2y); + p2.x = p2x; p2.y = p2y; @@ -3311,16 +3303,14 @@ int Sketch::addInternalAlignmentKnotPoint(int geoId1, int geoId2, int knotindex) int pointId1 = getPointId(geoId2, PointPos::start); if (pointId1 >= 0 && pointId1 < int(Points.size())) { - // GCS::Point &p = Points[pointId1]; - + GCS::Point &p = Points[pointId1]; GCS::BSpline &b = BSplines[Geoms[geoId1].index]; - // no constraint is actually added, as knots are fixed geometry in this implementation - // indexing is added here. - // However, we need to advance the tag, so that the index after a knot constraint is accurate - ConstraintsCounter++; + assert(knotindex < static_cast(b.knots.size()) && knotindex >= 0); b.knotpointGeoids[knotindex] = geoId2; + int tag = ++ConstraintsCounter; + GCSsys.addConstraintInternalAlignmentKnotPoint(b, p, knotindex, tag); return ConstraintsCounter; } @@ -3374,12 +3364,9 @@ bool Sketch::updateGeometry() GeomPoint *point = static_cast(it->geo); auto pointf = GeometryFacade::getFacade(point); - if(!(pointf->getInternalType() == InternalType::BSplineKnotPoint)) { - point->setPoint(Vector3d(*Points[it->startPointId].x, + point->setPoint(Vector3d(*Points[it->startPointId].x, *Points[it->startPointId].y, - 0.0) - ); - } + 0.0)); } else if (it->type == Line) { GeomLineSegment *lineSeg = static_cast(it->geo); @@ -3527,38 +3514,6 @@ bool Sketch::updateGeometry() } bsp->setKnots(knots,mult);*/ - - // This is the code that needs to be here to take advantage of the current OCCT reliant implementation - // The current B-Spline implementation relies on OCCT for pole calculation, so the knots are set by the OCCT calculated values - auto occtknots = bsp->getKnots(); - - for(auto it3 = occtknots.begin() ; it3 != occtknots.end(); ++it3) - knots.push_back(*it3); - - int index = 0; - for(std::vector::const_iterator it5 = mybsp.knotpointGeoids.begin(); it5 != mybsp.knotpointGeoids.end(); ++it5, index++) { - if( *it5 != GeoEnum::GeoUndef) { - if (Geoms[*it5].type == Point) { - GeomPoint *point = static_cast(Geoms[*it5].geo); - - auto pointf = GeometryFacade::getFacade(point); - - if(pointf->getInternalType() == InternalType::BSplineKnotPoint) { - auto pointcoords = bsp->pointAtParameter(knots[index]); - point->setPoint(pointcoords); // update the geompoint of the knot (geometry update) - // Now we update the position of the points in the solver, so that any call to solve() - // calculates constraints and positions based on the actual position of the knots. - auto pointindex = getPointId(*it5, PointPos::start); - - if(pointindex >= 0) { - auto solverpoint = Points[pointindex]; - *(solverpoint.x) = pointcoords.x; - *(solverpoint.y) = pointcoords.y; - } - } - } - } - } } } catch (Base::Exception &e) { Base::Console().Error("Updating geometry: Error build geometry(%d): %s\n", diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index c506e7b5ac..6dbb94a139 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -129,6 +129,171 @@ double ConstraintEqual::grad(double *param) return scale * deriv; } +// 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) + : factors(givenfactors) + , numpoints(givennumpoints) +{ + pvec = givenpvec; + setupInputs(); +} + +void ConstraintWeightedLinearCombination::setupInputs() +{ + assert(pvec.size() == 2*numpoints + 1); + if (factors.empty()) + factors = defaultfactors[numpoints]; + assert(factors.size() == numpoints); + origpvec = pvec; + rescale(); +} + +ConstraintType ConstraintWeightedLinearCombination::getTypeId() +{ + return WeightedLinearCombination; +} + +void ConstraintWeightedLinearCombination::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintWeightedLinearCombination::error() +{ + double offset = 1; + double woffset = 1 + numpoints; + + double sum = 0; + double wsum = 0; + + for (size_t i = 0; i < numpoints; ++i) { + double wcontrib = *pvec[woffset + i] * factors[i]; + wsum += wcontrib; + sum += *pvec[offset + i] * wcontrib; + } + + return scale * ((*pvec[0]) * 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? + + double deriv=0.; + if (param == pvec[0]) { + double wsum = 0; + for (size_t i = 0; i < numpoints; ++i) { + wsum += *pvec[woffset + i] * factors[i]; + } + deriv += wsum; + } + + for (size_t i = 0; i < numpoints; ++i) { + if (param == pvec[offset + i]) { + deriv += -(*pvec[woffset + i] * factors[i]); + } + if (param == pvec[woffset + i]) { + deriv += (*pvec[0] - *pvec[offset + i]) * factors[i]; + } + } + + return scale * deriv; +} + +// 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(); +} + +ConstraintType ConstraintCenterOfGravity::getTypeId() +{ + return CenterOfGravity; +} + +void ConstraintCenterOfGravity::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintCenterOfGravity::error() +{ + double sum = 0; + for (size_t i = 1; i < pvec.size(); ++i) + sum += *pvec[i] * weights[i-1]; + + return scale * (*pvec[0] - sum); +} + +double ConstraintCenterOfGravity::grad(double *param) +{ + double deriv=0.; + if (param == pvec[0]) deriv += 1; + + for (size_t i = 1; i < pvec.size(); ++i) + if (param == pvec[i]) deriv += -weights[i-1]; + + 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 8fd56e5eed..31c7f95b1b 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -68,7 +68,9 @@ namespace GCS InternalAlignmentPoint2Hyperbola = 22, PointOnParabola = 23, EqualFocalDistance = 24, - EqualLineLength = 25 + EqualLineLength = 25, + CenterOfGravity = 26, + WeightedLinearCombination = 27 }; enum InternalAlignmentType { @@ -155,6 +157,47 @@ namespace GCS double grad(double *) override; }; + // Center of Gravity + class ConstraintCenterOfGravity : public Constraint + { + public: + /// Constrains that the first parameter is center of gravity of rest + ConstraintCenterOfGravity(const std::vector& givenpvec); + 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; + }; + + // Weighted Linear Combination + class ConstraintWeightedLinearCombination : public Constraint + { + 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); + 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; + }; + // Difference class ConstraintDifference : public Constraint { diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index 5286e959a1..810673717a 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -1326,13 +1326,63 @@ int System::addConstraintInternalAlignmentParabolaFocus(Parabola &e, Point &p1, return addConstraintEqual(e.focus1.y, p1.y, tagId, driving, Constraint::Alignment::InternalAlignment); } -int System::addConstraintInternalAlignmentBSplineControlPoint(BSpline &b, Circle &c, int poleindex, int tagId, bool driving) +int System::addConstraintInternalAlignmentBSplineControlPoint(BSpline &b, Circle &c, unsigned int poleindex, int tagId, bool driving) { addConstraintEqual(b.poles[poleindex].x, c.center.x, tagId, driving, Constraint::Alignment::InternalAlignment); addConstraintEqual(b.poles[poleindex].y, c.center.y, tagId, driving, Constraint::Alignment::InternalAlignment); return addConstraintEqual(b.weights[poleindex], c.rad, tagId, driving, Constraint::Alignment::InternalAlignment); } +int System::addConstraintInternalAlignmentKnotPoint(BSpline &b, Point &p, unsigned int knotindex, int tagId, bool driving) +{ + if (b.periodic && knotindex == 0) { + // This is done here since knotpoints themselves aren't stored + addConstraintP2PCoincident(p, b.start, tagId, driving); + addConstraintP2PCoincident(p, b.end, tagId, driving); + } + + size_t numpoles = b.degree - b.mult[knotindex] + 1; + if (numpoles == 0) + numpoles = 1; + + size_t startpole = 0; + std::vector pvec; + pvec.push_back(p.x); + + std::vector weights(numpoles, 1.0 / numpoles); + + // 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); + // 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); + constr->setTag(tagId); + constr->setDriving(driving); + addConstraint(constr); + + pvec.clear(); + pvec.push_back(p.y); + 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.poles.size()]); + + constr = new ConstraintWeightedLinearCombination(numpoles, pvec, weights); + constr->setTag(tagId); + constr->setDriving(driving); + return addConstraint(constr); +} + //calculates angle between two curves at point of their intersection p. If two //points are supplied, p is used for first curve and p2 for second, yielding a //remote angle computation (this is useful when the endpoints haven't) been diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index 8944f835c3..460135f680 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -323,7 +323,8 @@ namespace GCS int addConstraintInternalAlignmentHyperbolaMinorDiameter(Hyperbola &e, Point &p1, Point &p2, int tagId=0, bool driving = true); int addConstraintInternalAlignmentHyperbolaFocus(Hyperbola &e, Point &p1, int tagId=0, bool driving = true); int addConstraintInternalAlignmentParabolaFocus(Parabola &e, Point &p1, int tagId=0, bool driving = true); - int addConstraintInternalAlignmentBSplineControlPoint(BSpline &b, Circle &c, int poleindex, int tag=0, bool driving = true); + int addConstraintInternalAlignmentBSplineControlPoint(BSpline &b, Circle &c, unsigned int poleindex, int tag=0, bool driving = true); + int addConstraintInternalAlignmentKnotPoint(BSpline &b, Point &p, unsigned int knotindex, int tagId=0, bool driving=true); double calculateAngleViaPoint(const Curve &crv1, const Curve &crv2, Point &p) const; double calculateAngleViaPoint(const Curve &crv1, const Curve &crv2, Point &p1, Point &p2) const; diff --git a/src/Mod/Sketcher/App/planegcs/Geo.cpp b/src/Mod/Sketcher/App/planegcs/Geo.cpp index 9b1fc8d61c..0c053cf982 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.cpp +++ b/src/Mod/Sketcher/App/planegcs/Geo.cpp @@ -704,7 +704,6 @@ void BSpline::ReconstructOnNewPvec(VEC_pD &pvec, int &cnt) start.y=pvec[cnt]; cnt++; end.x=pvec[cnt]; cnt++; end.y=pvec[cnt]; cnt++; - } BSpline* BSpline::Copy() @@ -713,4 +712,64 @@ BSpline* BSpline::Copy() return crv; } +double BSpline::getLinCombFactor(double x, size_t k, size_t i) +{ + // Adapted to C++ from the python implementation in the Wikipedia page for de Boor algorithm + // https://en.wikipedia.org/wiki/De_Boor%27s_algorithm. + + // FIXME: This should probably be guaranteed by now, and done somewhere else + if (flattenedknots.empty()) + setupFlattenedKnots(); + + std::vector d(degree + 1, 0.0); + // TODO: Ensure this is within range + d[i + degree - k] = 1.0; + + for (size_t r = 1; static_cast(r) < degree + 1; ++r) { + for (size_t j = degree; j > r - 1; --j) { + double alpha = + (x - flattenedknots[j + k - degree]) / + (flattenedknots[j + 1 + k - r] - flattenedknots[j + k - degree]); + d[j] = (1.0 - alpha) * d[j-1] + alpha * d[j]; + } + } + + return d[degree]; +} + +void BSpline::setupFlattenedKnots() +{ + flattenedknots.clear(); + + for (size_t i = 0; i < knots.size(); ++i) + flattenedknots.insert(flattenedknots.end(), mult[i], *knots[i]); + + // Adjust for periodic: see OCC documentation for explanation + if (periodic) { + double period = *knots.back() - *knots.front(); + int c = degree + 1 - mult[0]; // number of knots to pad + + // Add capacity so that iterators remain valid + flattenedknots.reserve(flattenedknots.size() + 2*c); + + // get iterators first for convenience + auto frontStart = flattenedknots.end() - mult.back() - c; + auto frontEnd = flattenedknots.end() - mult.back(); + auto backStart = flattenedknots.begin() + mult.front(); + auto backEnd = flattenedknots.begin() + mult.front() + c; + + // creating new vectors because above iterators can become invalidated upon insert + std::vector frontNew(frontStart, frontEnd); + std::vector backNew(backStart, backEnd); + + flattenedknots.insert(flattenedknots.end(), backNew.begin(), backNew.end()); + flattenedknots.insert(flattenedknots.begin(), frontNew.begin(), frontNew.end()); + + for (int i = 0; i < c; ++i) { + *(flattenedknots.begin() + i) -= period; + *(flattenedknots.end() - 1 - i) += period; + } + } +} + }//namespace GCS diff --git a/src/Mod/Sketcher/App/planegcs/Geo.h b/src/Mod/Sketcher/App/planegcs/Geo.h index f22bce938a..13ad0d4969 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.h +++ b/src/Mod/Sketcher/App/planegcs/Geo.h @@ -279,7 +279,7 @@ namespace GCS BSpline(){periodic=false;degree=2;} ~BSpline() override{} // parameters - VEC_P poles; + VEC_P poles; // TODO: use better data structures so poles.x and poles.y VEC_pD weights; VEC_pD knots; // dependent parameters (depends on previous parameters, @@ -292,12 +292,19 @@ namespace GCS int degree; bool periodic; VEC_I knotpointGeoids; // geoids of knotpoints as to index Geom array + VEC_D flattenedknots; // knot vector with repetitions for multiplicity and "padding" for periodic spline // interface helpers DeriVector2 CalculateNormal(const Point &p, 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; BSpline* Copy() override; + /// finds the value B_i(x) such that spline(x) = sum(poles[i] * B_i(x)) + /// i is index of control point + /// x is the point at which combination is needed + /// k is the range in `flattenedknots` that contains x + double getLinCombFactor(double x, size_t k, size_t i); + void setupFlattenedKnots(); }; } //namespace GCS