X-Git-Url: http://git.salome-platform.org/gitweb/?a=blobdiff_plain;f=src%2FSketchSolver%2FPlaneGCSSolver%2FPlaneGCSSolver_GeoExtensions.cpp;h=45ff24893d2909cfca6cd750b79b193dcb9da0c7;hb=06e7f5859095193fc7f498bd89a7d28009794f53;hp=480dbd58d07333c444db93468748032d4b79a8b0;hpb=645e2cb70c0e40290725f28fdc5fec8a93338d28;p=modules%2Fshaper.git diff --git a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_GeoExtensions.cpp b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_GeoExtensions.cpp index 480dbd58d..45ff24893 100644 --- a/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_GeoExtensions.cpp +++ b/src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_GeoExtensions.cpp @@ -1,4 +1,4 @@ -// Copyright (C) 2019-2020 CEA/DEN, EDF R&D +// Copyright (C) 2019-2023 CEA, EDF // // This library is free software; you can redistribute it and/or // modify it under the terms of the GNU Lesser General Public @@ -22,52 +22,38 @@ #include #include -#include #include namespace GCS { -DeriVector2 BSplineImpl::Value(double u, double du, double* derivparam) -{ - if (!isCacheValid()) - rebuildCache(); + static void periodicNormalization(double& theParam, double thePeriodStart, double thePeriodEnd) + { + double aPeriod = thePeriodEnd - thePeriodStart; + if (aPeriod > tolerance) { + theParam = std::max(thePeriodStart, + theParam + aPeriod * std::ceil((thePeriodStart - theParam) / aPeriod)); + } + } - std::shared_ptr aValue; - std::shared_ptr aDeriv; - myCurve->D1(u, aValue, aDeriv); - return DeriVector2(aValue->x(), aValue->y(), aDeriv->x() * du, aDeriv->y() * du); +DeriVector2 BSplineImpl::Value(double u, double du, double* derivparam) +{ + DeriVector2 value, deriv; + d1(u, derivparam, value, deriv); + return value.sum(GCS::DeriVector2(0., 0., deriv.x, deriv.y).mult(du)); } DeriVector2 BSplineImpl::CalculateNormal(Point &p, double* derivparam) { - if (!isCacheValid()) - rebuildCache(); - double u = 0.0; - if (!myCurve->parameter(GeomPnt2dPtr(new GeomAPI_Pnt2d(*p.x, *p.y)), 1e100, u)) { - // Sometimes OCCT's Extrema algorithm cannot find the parameter on B-spline curve - // (usually, if the point is near the curve extremity). - // Workaround: compute distance to each boundary point - double aDistPS = PlaneGCSSolver_Tools::distance(p, poles.front()); - double aDistPE = PlaneGCSSolver_Tools::distance(p, poles.back()); - static const double THE_TOLERANCE = 1.e-6; - if (aDistPS < aDistPE && aDistPS < THE_TOLERANCE) - u = *knots.front(); - else if (aDistPE < aDistPS && aDistPE < THE_TOLERANCE) - u = *knots.back(); - else - return DeriVector2(); - } - - std::shared_ptr aValue; - std::shared_ptr aDeriv; - myCurve->D1(u, aValue, aDeriv); + if (!parameter(p, u)) + return DeriVector2(); - DeriVector2 norm(aDeriv->x(), aDeriv->y(), 0.0, 0.0); - return norm.rotate90ccw(); + DeriVector2 value, deriv; + d1(u, derivparam, value, deriv); + return deriv.rotate90ccw(); } BSplineImpl* BSplineImpl::Copy() @@ -75,49 +61,120 @@ BSplineImpl* BSplineImpl::Copy() return new BSplineImpl(*this); } +void BSplineImpl::d1(double theU, + double* theDerivParam, + GCS::DeriVector2& theValue, + GCS::DeriVector2& theDerivative) +{ + int aSpan = spanIndex(theU); + std::vector aPoles; + std::vector aWeights; + spanPolesAndWeights(aSpan, theDerivParam, aPoles, aWeights); + performDeBoor(theU, aSpan, aPoles, aWeights, theValue, theDerivative); +} -bool BSplineImpl::isCacheValid() const +int BSplineImpl::spanIndex(double& u) { - // curve has to be initialized - bool isValid = myCurve.get() && !myCurve->isNull(); - - static const double THE_TOLERANCE = 1.e-7; - // compare poles - isValid = isValid && poles.size() == myCachedPoles.size(); - std::list::const_iterator aCachePIt = myCachedPoles.begin(); - GCS::VEC_P::const_iterator aPolesIt = poles.begin(); - for (; isValid && aPolesIt != poles.end(); ++aPolesIt, ++aCachePIt) { - isValid = isValid && fabs((*aCachePIt)->x() - *aPolesIt->x) < THE_TOLERANCE - && fabs((*aCachePIt)->y() - *aPolesIt->y) < THE_TOLERANCE; + if (myFlatKnots.empty()) { + // fill flat knots indices + for (int i = 0; i < (int)mult.size(); ++i) + myFlatKnots.resize(myFlatKnots.size() + mult[i], *knots[i]); + if (periodic) { + // additional knots at the beginning and the end to complete periodity + int anExtraBegin = degree + 1 - mult.front(); + int anExtraEnd = degree + 1 - mult.back(); + double aPeriod = *knots.back() - *knots.front(); + VEC_D aNewFlatKnots; + aNewFlatKnots.reserve(myFlatKnots.size() + (size_t)(anExtraBegin + anExtraEnd)); + auto it = myFlatKnots.end() - mult.back() - anExtraBegin; + while (anExtraBegin > 0) { + aNewFlatKnots.push_back(*(it++) - aPeriod); + --anExtraBegin; + } + aNewFlatKnots.insert(aNewFlatKnots.end(), myFlatKnots.begin(), myFlatKnots.end()); + it = myFlatKnots.begin() + mult.front(); + while (anExtraEnd > 0) { + aNewFlatKnots.push_back(*(it++) + aPeriod); + --anExtraEnd; + } + myFlatKnots = aNewFlatKnots; + } } - // compare weights - isValid = isValid && weights.size() == myCachedWeights.size(); - std::list::const_iterator aCacheWIt = myCachedWeights.begin(); - GCS::VEC_pD::const_iterator aWeightsIt = weights.begin(); - for (; isValid && aWeightsIt != weights.end(); ++aWeightsIt, ++aCacheWIt) - isValid = isValid && fabs(*aCacheWIt - **aWeightsIt) < THE_TOLERANCE; + if (periodic) + periodicNormalization(u, *knots.front(), *knots.back()); - return isValid; + int anIndex = 0; + for (int i = 1; i < (int)knots.size() - 1; ++i) { + if (u <= *knots[i]) + break; + anIndex += mult[i]; + } + return anIndex; +} + +void BSplineImpl::spanPolesAndWeights(int theSpanIndex, + double* theDerivParam, + std::vector& thePoles, + std::vector& theWeights) const +{ + thePoles.reserve(degree + 1); + theWeights.reserve(degree + 1); + for (int i = theSpanIndex; i <= theSpanIndex + degree; ++i) { + // optimization: weighted pole + int idx = i % (int)poles.size(); + thePoles.push_back(GCS::DeriVector2(poles[idx], theDerivParam).mult(*weights[idx])); + theWeights.push_back(*weights[idx]); + } +} + +void BSplineImpl::performDeBoor(double theU, + int theSpanIndex, + std::vector& thePoles, + std::vector& theWeights, + GCS::DeriVector2& theValue, + GCS::DeriVector2& theDerivative) const +{ + std::vector aPDeriv(thePoles.size(), DeriVector2()); + std::vector aWDeriv(theWeights.size(), 0.0); + for (int i = 0; i < degree; ++i) { + for (int j = degree; j > i; --j) { + double denom = (myFlatKnots[theSpanIndex + j + degree - i] - + myFlatKnots[theSpanIndex + j]); + double a = (theU - myFlatKnots[theSpanIndex + j]) / denom; + aPDeriv[j] = aPDeriv[j].linCombi(a, aPDeriv[j - 1], 1.0 - a).sum( + thePoles[j].subtr(thePoles[j - 1]).mult(1.0 / denom)); + aWDeriv[j] = aWDeriv[j] * a + aWDeriv[j - 1] * (1.0 - a) + + (theWeights[j] - theWeights[j - 1]) / denom; + thePoles[j] = thePoles[j].linCombi(a, thePoles[j - 1], 1.0 - a); + theWeights[j] = theWeights[j] * a + theWeights[j - 1] * (1.0 - a); + } + } + double w = 1 / theWeights[degree]; + theValue = thePoles[degree].mult(w); + theDerivative = aPDeriv[degree].subtr(theValue.mult(aWDeriv[degree])).mult(w); } -void BSplineImpl::rebuildCache() +bool BSplineImpl::parameter(const Point& thePoint, double& theParam) const { - myCachedPoles.clear(); - myCachedWeights.clear(); - myCachedKnots.clear(); - myCachedMultiplicities.clear(); - - for (GCS::VEC_P::iterator anIt = poles.begin(); anIt != poles.end(); ++anIt) - myCachedPoles.push_back(GeomPnt2dPtr(new GeomAPI_Pnt2d(*anIt->x, *anIt->y))); - for (GCS::VEC_pD::iterator anIt = weights.begin(); anIt != weights.end(); ++anIt) - myCachedWeights.push_back(**anIt); - for (GCS::VEC_pD::iterator anIt = knots.begin(); anIt != knots.end(); ++anIt) - myCachedKnots.push_back(**anIt); - myCachedMultiplicities.assign(mult.begin(), mult.end()); - - myCurve.reset(new GeomAPI_BSpline2d(degree, myCachedPoles, myCachedWeights, - myCachedKnots, myCachedMultiplicities, periodic)); + std::list aPoles; + std::list aWeights; + std::list aKnots; + std::list aMults; + + for (GCS::VEC_P::const_iterator anIt = poles.begin(); anIt != poles.end(); ++anIt) + aPoles.push_back(GeomPnt2dPtr(new GeomAPI_Pnt2d(*anIt->x, *anIt->y))); + for (GCS::VEC_pD::const_iterator anIt = weights.begin(); anIt != weights.end(); ++anIt) + aWeights.push_back(**anIt); + for (GCS::VEC_pD::const_iterator anIt = knots.begin(); anIt != knots.end(); ++anIt) + aKnots.push_back(**anIt); + aMults.assign(mult.begin(), mult.end()); + + std::shared_ptr aCurve( + new GeomAPI_BSpline2d(degree, aPoles, aWeights, aKnots, aMults, periodic)); + + GeomPnt2dPtr aPoint(new GeomAPI_Pnt2d(*thePoint.x, *thePoint.y)); + return aCurve->parameter(aPoint, 1e100, theParam); } } // namespace GCS