src/SketchSolver/PlaneGCSSolver/PlaneGCSSolver_GeoExtensions.cpp

   1 // Copyright (C) 2019-2022  CEA/DEN, EDF R&D
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Lesser General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2.1 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Lesser General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Lesser General Public
  14 // License along with this library; if not, write to the Free Software
  15 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  16 //
  17 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
  18 //
  19
  20 #include <PlaneGCSSolver_GeoExtensions.h>
  21 #include <PlaneGCSSolver_Tools.h>
  22
  23 #include <GeomAPI_BSpline2d.h>
  24 #include <GeomAPI_Pnt2d.h>
  25
  26 #include <cmath>
  27
  28 namespace GCS
  29 {
  30
  31   static void periodicNormalization(double& theParam, double thePeriodStart, double thePeriodEnd)
  32   {
  33     double aPeriod = thePeriodEnd - thePeriodStart;
  34     if (aPeriod > tolerance) {
  35       theParam = std::max(thePeriodStart,
  36                           theParam + aPeriod * std::ceil((thePeriodStart - theParam) / aPeriod));
  37     }
  38   }
  39
  40
  41 DeriVector2 BSplineImpl::Value(double u, double du, double* derivparam)
  42 {
  43   DeriVector2 value, deriv;
  44   d1(u, derivparam, value, deriv);
  45   return value.sum(GCS::DeriVector2(0., 0., deriv.x, deriv.y).mult(du));
  46 }
  47
  48 DeriVector2 BSplineImpl::CalculateNormal(Point &p, double* derivparam)
  49 {
  50   double u = 0.0;
  51   if (!parameter(p, u))
  52     return DeriVector2();
  53
  54   DeriVector2 value, deriv;
  55   d1(u, derivparam, value, deriv);
  56   return deriv.rotate90ccw();
  57 }
  58
  59 BSplineImpl* BSplineImpl::Copy()
  60 {
  61   return new BSplineImpl(*this);
  62 }
  63
  64 void BSplineImpl::d1(double theU,
  65                      double* theDerivParam,
  66                      GCS::DeriVector2& theValue,
  67                      GCS::DeriVector2& theDerivative)
  68 {
  69   int aSpan = spanIndex(theU);
  70   std::vector<GCS::DeriVector2> aPoles;
  71   std::vector<double> aWeights;
  72   spanPolesAndWeights(aSpan, theDerivParam, aPoles, aWeights);
  73   performDeBoor(theU, aSpan, aPoles, aWeights, theValue, theDerivative);
  74 }
  75
  76 int BSplineImpl::spanIndex(double& u)
  77 {
  78   if (myFlatKnots.empty()) {
  79     // fill flat knots indices
  80     for (int i = 0; i < (int)mult.size(); ++i)
  81       myFlatKnots.resize(myFlatKnots.size() + mult[i], *knots[i]);
  82     if (periodic) {
  83       // additional knots at the beginning and the end to complete periodity
  84       int anExtraBegin = degree + 1 - mult.front();
  85       int anExtraEnd = degree + 1 - mult.back();
  86       double aPeriod = *knots.back() - *knots.front();
  87       VEC_D aNewFlatKnots;
  88       aNewFlatKnots.reserve(myFlatKnots.size() + (size_t)(anExtraBegin + anExtraEnd));
  89       auto it = myFlatKnots.end() - mult.back() - anExtraBegin;
  90       while (anExtraBegin > 0) {
  91         aNewFlatKnots.push_back(*(it++) - aPeriod);
  92         --anExtraBegin;
  93       }
  94       aNewFlatKnots.insert(aNewFlatKnots.end(), myFlatKnots.begin(), myFlatKnots.end());
  95       it = myFlatKnots.begin() + mult.front();
  96       while (anExtraEnd > 0) {
  97         aNewFlatKnots.push_back(*(it++) + aPeriod);
  98         --anExtraEnd;
  99       }
 100       myFlatKnots = aNewFlatKnots;
 101     }
 102   }
 103
 104   if (periodic)
 105     periodicNormalization(u, *knots.front(), *knots.back());
 106
 107   int anIndex = 0;
 108   for (int i = 1; i < (int)knots.size() - 1; ++i) {
 109     if (u <= *knots[i])
 110       break;
 111     anIndex += mult[i];
 112   }
 113   return anIndex;
 114 }
 115
 116 void BSplineImpl::spanPolesAndWeights(int theSpanIndex,
 117                                       double* theDerivParam,
 118                                       std::vector<GCS::DeriVector2>& thePoles,
 119                                       std::vector<double>& theWeights) const
 120 {
 121   thePoles.reserve(degree + 1);
 122   theWeights.reserve(degree + 1);
 123   for (int i = theSpanIndex; i <= theSpanIndex + degree; ++i) {
 124     // optimization: weighted pole
 125     int idx = i % (int)poles.size();
 126     thePoles.push_back(GCS::DeriVector2(poles[idx], theDerivParam).mult(*weights[idx]));
 127     theWeights.push_back(*weights[idx]);
 128   }
 129 }
 130
 131 void BSplineImpl::performDeBoor(double theU,
 132                                 int theSpanIndex,
 133                                 std::vector<GCS::DeriVector2>& thePoles,
 134                                 std::vector<double>& theWeights,
 135                                 GCS::DeriVector2& theValue,
 136                                 GCS::DeriVector2& theDerivative) const
 137 {
 138   std::vector<GCS::DeriVector2> aPDeriv(thePoles.size(), DeriVector2());
 139   std::vector<double> aWDeriv(theWeights.size(), 0.0);
 140   for (int i = 0; i < degree; ++i) {
 141     for (int j = degree; j > i; --j) {
 142       double denom = (myFlatKnots[theSpanIndex + j + degree - i] -
 143                       myFlatKnots[theSpanIndex + j]);
 144       double a = (theU - myFlatKnots[theSpanIndex + j]) / denom;
 145       aPDeriv[j] = aPDeriv[j].linCombi(a, aPDeriv[j - 1], 1.0 - a).sum(
 146                    thePoles[j].subtr(thePoles[j - 1]).mult(1.0 / denom));
 147       aWDeriv[j] = aWDeriv[j] * a + aWDeriv[j - 1] * (1.0 - a)
 148                  + (theWeights[j] - theWeights[j - 1]) / denom;
 149       thePoles[j] = thePoles[j].linCombi(a, thePoles[j - 1], 1.0 - a);
 150       theWeights[j] = theWeights[j] * a + theWeights[j - 1] * (1.0 - a);
 151     }
 152   }
 153   double w = 1 / theWeights[degree];
 154   theValue = thePoles[degree].mult(w);
 155   theDerivative = aPDeriv[degree].subtr(theValue.mult(aWDeriv[degree])).mult(w);
 156 }
 157
 158 bool BSplineImpl::parameter(const Point& thePoint, double& theParam) const
 159 {
 160   std::list<GeomPnt2dPtr> aPoles;
 161   std::list<double> aWeights;
 162   std::list<double> aKnots;
 163   std::list<int> aMults;
 164
 165   for (GCS::VEC_P::const_iterator anIt = poles.begin(); anIt != poles.end(); ++anIt)
 166     aPoles.push_back(GeomPnt2dPtr(new GeomAPI_Pnt2d(*anIt->x, *anIt->y)));
 167   for (GCS::VEC_pD::const_iterator anIt = weights.begin(); anIt != weights.end(); ++anIt)
 168     aWeights.push_back(**anIt);
 169   for (GCS::VEC_pD::const_iterator anIt = knots.begin(); anIt != knots.end(); ++anIt)
 170     aKnots.push_back(**anIt);
 171   aMults.assign(mult.begin(), mult.end());
 172
 173   std::shared_ptr<GeomAPI_BSpline2d> aCurve(
 174       new GeomAPI_BSpline2d(degree, aPoles, aWeights, aKnots, aMults, periodic));
 175
 176   GeomPnt2dPtr aPoint(new GeomAPI_Pnt2d(*thePoint.x, *thePoint.y));
 177   return aCurve->parameter(aPoint, 1e100, theParam);
 178 }
 179
 180 } // namespace GCS