0027048: BSpline cache is always wrong outside of surface
[occt.git] / src / BSplCLib / BSplCLib_Cache.cxx
1 // Copyright (c) 2014 OPEN CASCADE SAS
2 //
3 // This file is part of Open CASCADE Technology software library.
4 //
5 // This library is free software; you can redistribute it and/or modify it under
6 // the terms of the GNU Lesser General Public License version 2.1 as published
7 // by the Free Software Foundation, with special exception defined in the file
8 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
9 // distribution for complete text of the license and disclaimer of any warranty.
10 //
11 // Alternatively, this file may be used under the terms of Open CASCADE
12 // commercial license or contractual agreement.
13
14 #include <BSplCLib_Cache.hxx>
15 #include <BSplCLib.hxx>
16
17 #include <NCollection_LocalArray.hxx>
18
19 #include <TColgp_HArray1OfPnt.hxx>
20 #include <TColgp_HArray1OfPnt2d.hxx>
21 #include <TColStd_HArray1OfReal.hxx>
22 #include <TColStd_HArray2OfReal.hxx>
23
24
25 IMPLEMENT_STANDARD_RTTIEXT(BSplCLib_Cache,Standard_Transient)
26
27 //! Converts handle of array of Standard_Real into the pointer to Standard_Real
28 static Standard_Real* ConvertArray(const Handle(TColStd_HArray2OfReal)& theHArray)
29 {
30   const TColStd_Array2OfReal& anArray = theHArray->Array2();
31   return (Standard_Real*) &(anArray(anArray.LowerRow(), anArray.LowerCol()));
32 }
33
34
35 BSplCLib_Cache::BSplCLib_Cache()
36 {
37   myPolesWeights.Nullify();
38   myIsRational = Standard_False;
39   mySpanStart = 0.0;
40   mySpanLength = 0.0;
41   mySpanIndex = 0;
42   myDegree = 0;
43   myFlatKnots.Nullify();
44 }
45
46 BSplCLib_Cache::BSplCLib_Cache(const Standard_Integer&        theDegree,
47                                const Standard_Boolean&        thePeriodic,
48                                const TColStd_Array1OfReal&    theFlatKnots,
49                                const TColgp_Array1OfPnt2d&    thePoles2d,
50                                const TColStd_Array1OfReal*    theWeights)
51 {
52   Standard_Real aCacheParam = theFlatKnots.Value(theFlatKnots.Lower() + theDegree);
53   BuildCache(aCacheParam, theDegree, thePeriodic, 
54              theFlatKnots, thePoles2d, theWeights);
55 }
56
57 BSplCLib_Cache::BSplCLib_Cache(const Standard_Integer&        theDegree,
58                                const Standard_Boolean&        thePeriodic,
59                                const TColStd_Array1OfReal&    theFlatKnots,
60                                const TColgp_Array1OfPnt&      thePoles,
61                                const TColStd_Array1OfReal*    theWeights)
62 {
63   Standard_Real aCacheParam = theFlatKnots.Value(theFlatKnots.Lower() + theDegree);
64   BuildCache(aCacheParam, theDegree, thePeriodic, 
65              theFlatKnots, thePoles, theWeights);
66 }
67
68
69 Standard_Boolean BSplCLib_Cache::IsCacheValid(Standard_Real theParameter) const
70 {
71   Standard_Real aNewParam = theParameter;
72   if (!myFlatKnots.IsNull())
73     PeriodicNormalization(myFlatKnots->Array1(), aNewParam);
74
75   Standard_Real aDelta = aNewParam - mySpanStart;
76   return ((aDelta >= 0.0 || mySpanIndex == mySpanIndexMin) &&
77           (aDelta < mySpanLength || mySpanIndex == mySpanIndexMax));
78 }
79
80 void BSplCLib_Cache::PeriodicNormalization(const TColStd_Array1OfReal& theFlatKnots, 
81                                            Standard_Real& theParameter) const
82 {
83   Standard_Real aPeriod = theFlatKnots.Value(theFlatKnots.Upper() - myDegree) - 
84                           theFlatKnots.Value(myDegree + 1) ;
85   if (theParameter < theFlatKnots.Value(myDegree + 1))
86   {
87     Standard_Real aScale = IntegerPart(
88         (theFlatKnots.Value(myDegree + 1) - theParameter) / aPeriod);
89     theParameter += aPeriod * (aScale + 1.0);
90   }
91   if (theParameter > theFlatKnots.Value(theFlatKnots.Upper() - myDegree))
92   {
93     Standard_Real aScale = IntegerPart(
94         (theParameter - theFlatKnots.Value(theFlatKnots.Upper() - myDegree)) / aPeriod);
95     theParameter -= aPeriod * (aScale + 1.0);
96   }
97 }
98
99
100 void BSplCLib_Cache::BuildCache(const Standard_Real&           theParameter,
101                                 const Standard_Integer&        theDegree,
102                                 const Standard_Boolean&        thePeriodic,
103                                 const TColStd_Array1OfReal&    theFlatKnots,
104                                 const TColgp_Array1OfPnt2d&    thePoles2d,
105                                 const TColStd_Array1OfReal*    theWeights)
106 {
107   // Normalize theParameter for periodical B-splines
108   Standard_Real aNewParam = theParameter;
109   if (thePeriodic)
110   {
111     PeriodicNormalization(theFlatKnots, aNewParam);
112     myFlatKnots = new TColStd_HArray1OfReal(1, theFlatKnots.Length());
113     myFlatKnots->ChangeArray1() = theFlatKnots;
114   }
115   else if (!myFlatKnots.IsNull()) // Periodical curve became non-periodical
116     myFlatKnots.Nullify();
117
118   // Change the size of cached data if needed
119   myIsRational = (theWeights != NULL);
120   Standard_Integer aPWColNumber = myIsRational ? 3 : 2;
121   if (theDegree > myDegree)
122     myPolesWeights = new TColStd_HArray2OfReal(1, theDegree + 1, 1, aPWColNumber);
123
124   myDegree = theDegree;
125   mySpanIndex = 0;
126   BSplCLib::LocateParameter(theDegree, theFlatKnots, BSplCLib::NoMults(), 
127                             aNewParam, thePeriodic, mySpanIndex, aNewParam);
128   mySpanStart  = theFlatKnots.Value(mySpanIndex);
129   mySpanLength = theFlatKnots.Value(mySpanIndex + 1) - mySpanStart;
130   mySpanIndexMin = thePeriodic ? 0 : myDegree + 1;
131   mySpanIndexMax = theFlatKnots.Length() - 1 - theDegree;
132
133   // Calculate new cache data
134   BSplCLib::BuildCache(mySpanStart, mySpanLength, thePeriodic, theDegree, 
135                        theFlatKnots, thePoles2d, theWeights, 
136                        myPolesWeights->ChangeArray2());
137 }
138
139 void BSplCLib_Cache::BuildCache(const Standard_Real&           theParameter,
140                                 const Standard_Integer&        theDegree,
141                                 const Standard_Boolean&        thePeriodic,
142                                 const TColStd_Array1OfReal&    theFlatKnots,
143                                 const TColgp_Array1OfPnt&      thePoles,
144                                 const TColStd_Array1OfReal*    theWeights)
145 {
146   // Create list of knots with repetitions and normalize theParameter for periodical B-splines
147   Standard_Real aNewParam = theParameter;
148   if (thePeriodic)
149   {
150     PeriodicNormalization(theFlatKnots, aNewParam);
151     myFlatKnots = new TColStd_HArray1OfReal(1, theFlatKnots.Length());
152     myFlatKnots->ChangeArray1() = theFlatKnots;
153   }
154   else if (!myFlatKnots.IsNull()) // Periodical curve became non-periodical
155     myFlatKnots.Nullify();
156
157   // Change the size of cached data if needed
158   myIsRational = (theWeights != NULL);
159   Standard_Integer aPWColNumber = myIsRational ? 4 : 3;
160   if (theDegree > myDegree)
161     myPolesWeights = new TColStd_HArray2OfReal(1, theDegree + 1, 1, aPWColNumber);
162
163   myDegree = theDegree;
164   mySpanIndex = 0;
165   BSplCLib::LocateParameter(theDegree, theFlatKnots, BSplCLib::NoMults(), 
166                             aNewParam, thePeriodic, mySpanIndex, aNewParam);
167   mySpanStart  = theFlatKnots.Value(mySpanIndex);
168   mySpanLength = theFlatKnots.Value(mySpanIndex + 1) - mySpanStart;
169   mySpanIndexMin = thePeriodic ? 0 : myDegree + 1;
170   mySpanIndexMax = theFlatKnots.Length() - 1 - theDegree;
171
172   // Calculate new cache data
173   BSplCLib::BuildCache(mySpanStart, mySpanLength, thePeriodic, theDegree, 
174                        theFlatKnots, thePoles, theWeights, 
175                        myPolesWeights->ChangeArray2());
176 }
177
178
179 void BSplCLib_Cache::CalculateDerivative(const Standard_Real&    theParameter, 
180                                          const Standard_Integer& theDerivative, 
181                                                Standard_Real&    theDerivArray) const
182 {
183   Standard_Real aNewParameter = theParameter;
184   if (!myFlatKnots.IsNull()) // B-spline is periodical
185     PeriodicNormalization(myFlatKnots->Array1(), aNewParameter);
186   aNewParameter = (aNewParameter - mySpanStart) / mySpanLength;
187
188   Standard_Real* aPolesArray = ConvertArray(myPolesWeights);
189   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
190
191   // Temporary container. The maximal size of this container is defined by:
192   //    1) maximal derivative for cache evaluation, which is 3, plus one row for function values, 
193   //    2) and maximal dimension of the point, which is 3, plus one column for weights.
194   Standard_Real aTmpContainer[16];
195
196   // When the PLib::RationaDerivative needs to be called, use temporary container
197   Standard_Real* aPntDeriv = myIsRational ? aTmpContainer : &theDerivArray;
198
199   // When the degree of curve is lesser than the requested derivative,
200   // nullify array cells corresponding to greater derivatives
201   Standard_Integer aDerivative = theDerivative;
202   if (myDegree < theDerivative)
203   {
204     aDerivative = myDegree;
205     for (Standard_Integer ind = myDegree * aDimension; ind < (theDerivative + 1) * aDimension; ind++)
206     {
207       aPntDeriv[ind] = 0.0;
208       (&theDerivArray)[ind] = 0.0; // should be cleared separately, because aPntDeriv may look to another memory area
209     }
210   }
211
212   PLib::EvalPolynomial(aNewParameter, aDerivative, myDegree, aDimension, 
213                        aPolesArray[0], aPntDeriv[0]);
214   // Unnormalize derivatives since those are computed normalized
215   Standard_Real aFactor = 1.0;
216   for (Standard_Integer deriv = 1; deriv <= aDerivative; deriv++)
217   {
218     aFactor /= mySpanLength;
219     for (Standard_Integer ind = 0; ind < aDimension; ind++)
220       aPntDeriv[aDimension * deriv + ind] *= aFactor;
221   }
222
223   if (myIsRational) // calculate derivatives divided by weights derivatives
224     PLib::RationalDerivative(aDerivative, aDerivative, aDimension - 1, aPntDeriv[0], theDerivArray);
225 }
226
227
228 void BSplCLib_Cache::D0(const Standard_Real& theParameter, gp_Pnt2d& thePoint) const
229 {
230   Standard_Real aNewParameter = theParameter;
231   if (!myFlatKnots.IsNull()) // B-spline is periodical
232     PeriodicNormalization(myFlatKnots->Array1(), aNewParameter);
233   aNewParameter = (aNewParameter - mySpanStart) / mySpanLength;
234
235   Standard_Real* aPolesArray = ConvertArray(myPolesWeights);
236   Standard_Real aPoint[4];
237   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
238
239   PLib::NoDerivativeEvalPolynomial(aNewParameter, myDegree,
240                                    aDimension, myDegree * aDimension,
241                                    aPolesArray[0], aPoint[0]);
242
243   thePoint.SetCoord(aPoint[0], aPoint[1]);
244   if (myIsRational)
245     thePoint.ChangeCoord().Divide(aPoint[2]);
246 }
247
248 void BSplCLib_Cache::D0(const Standard_Real& theParameter, gp_Pnt& thePoint) const
249 {
250   Standard_Real aNewParameter = theParameter;
251   if (!myFlatKnots.IsNull()) // B-spline is periodical
252     PeriodicNormalization(myFlatKnots->Array1(), aNewParameter);
253   aNewParameter = (aNewParameter - mySpanStart) / mySpanLength;
254
255   Standard_Real* aPolesArray = ConvertArray(myPolesWeights);
256   Standard_Real aPoint[4];
257   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
258
259   PLib::NoDerivativeEvalPolynomial(aNewParameter, myDegree,
260                                    aDimension, myDegree * aDimension,
261                                    aPolesArray[0], aPoint[0]);
262
263   thePoint.SetCoord(aPoint[0], aPoint[1], aPoint[2]);
264   if (myIsRational)
265     thePoint.ChangeCoord().Divide(aPoint[3]);
266 }
267
268
269 void BSplCLib_Cache::D1(const Standard_Real& theParameter, gp_Pnt2d& thePoint, gp_Vec2d& theTangent) const
270 {
271   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
272   Standard_Real aPntDeriv[8]; // result storage (point and derivative coordinates)
273
274   this->CalculateDerivative(theParameter, 1, aPntDeriv[0]);
275   if (myIsRational) // the size of aPntDeriv was changed by PLib::RationalDerivative
276     aDimension -= 1;
277
278   thePoint.SetCoord(aPntDeriv[0], aPntDeriv[1]);
279   theTangent.SetCoord(aPntDeriv[aDimension], aPntDeriv[aDimension + 1]);
280 }
281
282 void BSplCLib_Cache::D1(const Standard_Real& theParameter, gp_Pnt& thePoint, gp_Vec& theTangent) const
283 {
284   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
285   Standard_Real aPntDeriv[8]; // result storage (point and derivative coordinates)
286
287   this->CalculateDerivative(theParameter, 1, aPntDeriv[0]);
288   if (myIsRational) // the size of aPntDeriv was changed by PLib::RationalDerivative
289     aDimension -= 1;
290
291   thePoint.SetCoord(aPntDeriv[0], aPntDeriv[1], aPntDeriv[2]);
292   theTangent.SetCoord(aPntDeriv[aDimension], aPntDeriv[aDimension + 1], aPntDeriv[aDimension + 2]);
293 }
294
295 void BSplCLib_Cache::D2(const Standard_Real& theParameter, gp_Pnt2d& thePoint, gp_Vec2d& theTangent, gp_Vec2d& theCurvature) const
296 {
297   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
298   Standard_Real aPntDeriv[12]; // result storage (point and derivatives coordinates)
299
300   this->CalculateDerivative(theParameter, 2, aPntDeriv[0]);
301   if (myIsRational) // the size of aPntDeriv was changed by PLib::RationalDerivative
302     aDimension -= 1;
303
304   thePoint.SetCoord(aPntDeriv[0], aPntDeriv[1]);
305   theTangent.SetCoord(aPntDeriv[aDimension], aPntDeriv[aDimension + 1]);
306   theCurvature.SetCoord(aPntDeriv[aDimension<<1], aPntDeriv[(aDimension<<1) + 1]);
307 }
308
309 void BSplCLib_Cache::D2(const Standard_Real& theParameter, gp_Pnt& thePoint, gp_Vec& theTangent, gp_Vec& theCurvature) const
310 {
311   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
312   Standard_Real aPntDeriv[12]; // result storage (point and derivatives coordinates)
313
314   this->CalculateDerivative(theParameter, 2, aPntDeriv[0]);
315   if (myIsRational) // the size of aPntDeriv was changed by PLib::RationalDerivative
316     aDimension -= 1;
317
318   thePoint.SetCoord(aPntDeriv[0], aPntDeriv[1], aPntDeriv[2]);
319   theTangent.SetCoord(aPntDeriv[aDimension], aPntDeriv[aDimension + 1], aPntDeriv[aDimension + 2]);
320   theCurvature.SetCoord(aPntDeriv[aDimension<<1], aPntDeriv[(aDimension<<1) + 1], aPntDeriv[(aDimension<<1) + 2]);
321 }
322
323
324 void BSplCLib_Cache::D3(const Standard_Real& theParameter, 
325                               gp_Pnt2d&      thePoint, 
326                               gp_Vec2d&      theTangent, 
327                               gp_Vec2d&      theCurvature,
328                               gp_Vec2d&      theTorsion) const
329 {
330   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
331   Standard_Real aPntDeriv[16]; // result storage (point and derivatives coordinates)
332
333   this->CalculateDerivative(theParameter, 3, aPntDeriv[0]);
334   if (myIsRational) // the size of aPntDeriv was changed by PLib::RationalDerivative
335     aDimension -= 1;
336
337   thePoint.SetCoord(aPntDeriv[0], aPntDeriv[1]);
338   theTangent.SetCoord(aPntDeriv[aDimension], aPntDeriv[aDimension + 1]);
339   Standard_Integer aShift = aDimension << 1;
340   theCurvature.SetCoord(aPntDeriv[aShift], aPntDeriv[aShift + 1]);
341   aShift += aDimension;
342   theTorsion.SetCoord(aPntDeriv[aShift], aPntDeriv[aShift + 1]);
343 }
344
345 void BSplCLib_Cache::D3(const Standard_Real& theParameter, 
346                               gp_Pnt&        thePoint, 
347                               gp_Vec&        theTangent, 
348                               gp_Vec&        theCurvature,
349                               gp_Vec&        theTorsion) const
350 {
351   Standard_Integer aDimension = myPolesWeights->RowLength(); // number of columns
352   Standard_Real aPntDeriv[16]; // result storage (point and derivatives coordinates)
353
354   this->CalculateDerivative(theParameter, 3, aPntDeriv[0]);
355   if (myIsRational) // the size of aPntDeriv was changed by PLib::RationalDerivative
356     aDimension -= 1;
357
358   thePoint.SetCoord(aPntDeriv[0], aPntDeriv[1], aPntDeriv[2]);
359   theTangent.SetCoord(aPntDeriv[aDimension], aPntDeriv[aDimension + 1], aPntDeriv[aDimension + 2]);
360   Standard_Integer aShift = aDimension << 1;
361   theCurvature.SetCoord(aPntDeriv[aShift], aPntDeriv[aShift + 1], aPntDeriv[aShift + 2]);
362   aShift += aDimension;
363   theTorsion.SetCoord(aPntDeriv[aShift], aPntDeriv[aShift + 1], aPntDeriv[aShift + 2]);
364 }
365