0026936: Drawbacks of inlining in new type system in OCCT 7.0 -- automatic
[occt.git] / src / GeomEvaluator / GeomEvaluator_OffsetCurve.cxx
1 // Created on: 2015-09-21
2 // Copyright (c) 2015 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
6 // This library is free software; you can redistribute it and/or modify it under
7 // the terms of the GNU Lesser General Public License version 2.1 as published
8 // by the Free Software Foundation, with special exception defined in the file
9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
10 // distribution for complete text of the license and disclaimer of any warranty.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 #include <GeomEvaluator_OffsetCurve.hxx>
16
17 #include <GeomAdaptor_HCurve.hxx>
18 #include <Standard_NullValue.hxx>
19
20
21 IMPLEMENT_STANDARD_RTTIEXT(GeomEvaluator_OffsetCurve,GeomEvaluator_Curve)
22
23 GeomEvaluator_OffsetCurve::GeomEvaluator_OffsetCurve(
24         const Handle(Geom_Curve)& theBase,
25         const Standard_Real theOffset,
26         const gp_Dir& theDirection)
27   : GeomEvaluator_Curve(),
28     myBaseCurve(theBase),
29     myOffset(theOffset),
30     myOffsetDir(theDirection)
31 {
32 }
33
34 GeomEvaluator_OffsetCurve::GeomEvaluator_OffsetCurve(
35         const Handle(GeomAdaptor_HCurve)& theBase,
36         const Standard_Real theOffset,
37         const gp_Dir& theDirection)
38   : GeomEvaluator_Curve(),
39     myBaseAdaptor(theBase),
40     myOffset(theOffset),
41     myOffsetDir(theDirection)
42 {
43 }
44
45 void GeomEvaluator_OffsetCurve::D0(const Standard_Real theU,
46                                          gp_Pnt& theValue) const
47 {
48   gp_Vec aD1;
49   BaseD1(theU, theValue, aD1);
50   CalculateD0(theValue, aD1);
51 }
52
53 void GeomEvaluator_OffsetCurve::D1(const Standard_Real theU,
54                                          gp_Pnt& theValue,
55                                          gp_Vec& theD1) const
56 {
57   gp_Vec aD2;
58   BaseD2(theU, theValue, theD1, aD2);
59   CalculateD1(theValue, theD1, aD2);
60 }
61
62 void GeomEvaluator_OffsetCurve::D2(const Standard_Real theU,
63                                          gp_Pnt& theValue,
64                                          gp_Vec& theD1,
65                                          gp_Vec& theD2) const
66 {
67   gp_Vec aD3;
68   BaseD3(theU, theValue, theD1, theD2, aD3);
69
70   Standard_Boolean isDirectionChange = Standard_False;
71   if (theD1.SquareMagnitude() <= gp::Resolution())
72   {
73     gp_Vec aDummyD4;
74     isDirectionChange = AdjustDerivative(3, theU, theD1, theD2, aD3, aDummyD4);
75   }
76
77   CalculateD2(theValue, theD1, theD2, aD3, isDirectionChange);
78 }
79
80 void GeomEvaluator_OffsetCurve::D3(const Standard_Real theU,
81                                          gp_Pnt& theValue,
82                                          gp_Vec& theD1,
83                                          gp_Vec& theD2,
84                                          gp_Vec& theD3) const
85 {
86   gp_Vec aD4;
87   BaseD4(theU, theValue, theD1, theD2, theD3, aD4);
88
89   Standard_Boolean isDirectionChange = Standard_False;
90   if (theD1.SquareMagnitude() <= gp::Resolution())
91     isDirectionChange = AdjustDerivative(4, theU, theD1, theD2, theD3, aD4);
92
93   CalculateD3(theValue, theD1, theD2, theD3, aD4, isDirectionChange);
94 }
95
96 gp_Vec GeomEvaluator_OffsetCurve::DN(const Standard_Real theU,
97                                      const Standard_Integer theDeriv) const
98 {
99   Standard_RangeError_Raise_if(theDeriv < 1, "GeomEvaluator_OffsetCurve::DN(): theDeriv < 1");
100
101   gp_Pnt aPnt;
102   gp_Vec aDummy, aDN;
103   switch (theDeriv)
104   {
105   case 1:
106     D1(theU, aPnt, aDN);
107     break;
108   case 2:
109     D2(theU, aPnt, aDummy, aDN);
110     break;
111   case 3:
112     D3(theU, aPnt, aDummy, aDummy, aDN);
113     break;
114   default:
115     aDN = BaseDN(theU, theDeriv);
116   }
117   return aDN;
118 }
119
120
121 void GeomEvaluator_OffsetCurve::BaseD0(const Standard_Real theU,
122                                              gp_Pnt& theValue) const
123 {
124   if (!myBaseAdaptor.IsNull())
125     myBaseAdaptor->D0(theU, theValue);
126   else
127     myBaseCurve->D0(theU, theValue);
128 }
129
130 void GeomEvaluator_OffsetCurve::BaseD1(const Standard_Real theU,
131                                              gp_Pnt& theValue,
132                                              gp_Vec& theD1) const
133 {
134   if (!myBaseAdaptor.IsNull())
135     myBaseAdaptor->D1(theU, theValue, theD1);
136   else
137     myBaseCurve->D1(theU, theValue, theD1);
138 }
139
140 void GeomEvaluator_OffsetCurve::BaseD2(const Standard_Real theU,
141                                              gp_Pnt& theValue,
142                                              gp_Vec& theD1,
143                                              gp_Vec& theD2) const
144 {
145   if (!myBaseAdaptor.IsNull())
146     myBaseAdaptor->D2(theU, theValue, theD1, theD2);
147   else
148     myBaseCurve->D2(theU, theValue, theD1, theD2);
149 }
150
151 void GeomEvaluator_OffsetCurve::BaseD3(const Standard_Real theU,
152                                              gp_Pnt& theValue,
153                                              gp_Vec& theD1,
154                                              gp_Vec& theD2,
155                                              gp_Vec& theD3) const
156 {
157   if (!myBaseAdaptor.IsNull())
158     myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
159   else
160     myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
161 }
162
163 void GeomEvaluator_OffsetCurve::BaseD4(const Standard_Real theU,
164                                              gp_Pnt& theValue,
165                                              gp_Vec& theD1,
166                                              gp_Vec& theD2,
167                                              gp_Vec& theD3,
168                                              gp_Vec& theD4) const
169 {
170   if (!myBaseAdaptor.IsNull())
171   {
172     myBaseAdaptor->D3(theU, theValue, theD1, theD2, theD3);
173     theD4 = myBaseAdaptor->DN(theU, 4);
174   }
175   else
176   {
177     myBaseCurve->D3(theU, theValue, theD1, theD2, theD3);
178     theD4 = myBaseCurve->DN(theU, 4);
179   }
180 }
181
182 gp_Vec GeomEvaluator_OffsetCurve::BaseDN(const Standard_Real theU,
183                                          const Standard_Integer theDeriv) const
184 {
185   if (!myBaseAdaptor.IsNull())
186     return myBaseAdaptor->DN(theU, theDeriv);
187   return myBaseCurve->DN(theU, theDeriv);
188 }
189
190
191 void GeomEvaluator_OffsetCurve::CalculateD0(      gp_Pnt& theValue,
192                                             const gp_Vec& theD1) const
193 {
194   gp_XYZ Ndir = (theD1.XYZ()).Crossed(myOffsetDir.XYZ());
195   Standard_Real R = Ndir.Modulus();
196   if (R <= gp::Resolution())
197     Standard_NullValue::Raise("GeomEvaluator_OffsetCurve: Undefined normal vector "
198                               "because tangent vector has zero-magnitude!");
199
200   Ndir.Multiply(myOffset / R);
201   theValue.ChangeCoord().Add(Ndir);
202 }
203
204 void GeomEvaluator_OffsetCurve::CalculateD1(      gp_Pnt& theValue,
205                                                   gp_Vec& theD1,
206                                             const gp_Vec& theD2) const
207 {
208   // P(u) = p(u) + Offset * Ndir / R
209   // with R = || p' ^ V|| and Ndir = P' ^ direction (local normal direction)
210
211   // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
212
213   gp_XYZ Ndir = (theD1.XYZ()).Crossed(myOffsetDir.XYZ());
214   gp_XYZ DNdir = (theD2.XYZ()).Crossed(myOffsetDir.XYZ());
215   Standard_Real R2 = Ndir.SquareModulus();
216   Standard_Real R = Sqrt(R2);
217   Standard_Real R3 = R * R2;
218   Standard_Real Dr = Ndir.Dot(DNdir);
219   if (R3 <= gp::Resolution()) {
220     if (R2 <= gp::Resolution())
221       Standard_NullValue::Raise("GeomEvaluator_OffsetCurve: Null derivative");
222     //We try another computation but the stability is not very good.
223     DNdir.Multiply(R);
224     DNdir.Subtract(Ndir.Multiplied(Dr / R));
225     DNdir.Multiply(myOffset / R2);
226   }
227   else {
228     // Same computation as IICURV in EUCLID-IS because the stability is better
229     DNdir.Multiply(myOffset / R);
230     DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
231   }
232
233   Ndir.Multiply(myOffset / R);
234   // P(u)
235   theValue.ChangeCoord().Add(Ndir);
236   // P'(u)
237   theD1.Add(gp_Vec(DNdir));
238 }
239
240 void GeomEvaluator_OffsetCurve::CalculateD2(      gp_Pnt& theValue,
241                                                   gp_Vec& theD1,
242                                                   gp_Vec& theD2,
243                                             const gp_Vec& theD3,
244                                             const Standard_Boolean theIsDirChange) const
245 {
246   // P(u) = p(u) + Offset * Ndir / R
247   // with R = || p' ^ V|| and Ndir = P' ^ direction (local normal direction)
248
249   // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
250
251   // P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
252   //         Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))
253
254   gp_XYZ Ndir = (theD1.XYZ()).Crossed(myOffsetDir.XYZ());
255   gp_XYZ DNdir = (theD2.XYZ()).Crossed(myOffsetDir.XYZ());
256   gp_XYZ D2Ndir = (theD3.XYZ()).Crossed(myOffsetDir.XYZ());
257   Standard_Real R2 = Ndir.SquareModulus();
258   Standard_Real R = Sqrt(R2);
259   Standard_Real R3 = R2 * R;
260   Standard_Real R4 = R2 * R2;
261   Standard_Real R5 = R3 * R2;
262   Standard_Real Dr = Ndir.Dot(DNdir);
263   Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
264
265   if (R5 <= gp::Resolution()) {
266     if (R4 <= gp::Resolution())
267       Standard_NullValue::Raise("GeomEvaluator_OffsetCurve: Null derivative");
268     //We try another computation but the stability is not very good
269     //dixit ISG.
270     // V2 = P" (U) :
271     R4 = R2 * R2;
272     D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
273     D2Ndir.Add(Ndir.Multiplied(((3.0 * Dr * Dr) / R4) - (D2r / R2)));
274     D2Ndir.Multiply(myOffset / R);
275
276     // V1 = P' (U) :
277     DNdir.Multiply(R);
278     DNdir.Subtract(Ndir.Multiplied(Dr / R));
279     DNdir.Multiply(myOffset / R2);
280   }
281   else {
282     // Same computation as IICURV in EUCLID-IS because the stability is better.
283     // V2 = P" (U) :
284     D2Ndir.Multiply(myOffset / R);
285     D2Ndir.Subtract(DNdir.Multiplied(2.0 * myOffset * Dr / R3));
286     D2Ndir.Add(Ndir.Multiplied(myOffset * (((3.0 * Dr * Dr) / R5) - (D2r / R3))));
287
288     // V1 = P' (U) :
289     DNdir.Multiply(myOffset / R);
290     DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
291   }
292
293   Ndir.Multiply(myOffset / R);
294   // P(u)
295   theValue.ChangeCoord().Add(Ndir);
296   // P'(u) :
297   theD1.Add(gp_Vec(DNdir));
298   // P"(u) :
299   if (theIsDirChange)
300     theD2.Reverse();
301   theD2.Add(gp_Vec(D2Ndir));
302 }
303
304 void GeomEvaluator_OffsetCurve::CalculateD3(      gp_Pnt& theValue,
305                                                   gp_Vec& theD1,
306                                                   gp_Vec& theD2,
307                                                   gp_Vec& theD3,
308                                             const gp_Vec& theD4,
309                                             const Standard_Boolean theIsDirChange) const
310 {
311   // P(u) = p(u) + Offset * Ndir / R
312   // with R = || p' ^ V|| and Ndir = P' ^ direction (local normal direction)
313
314   // P'(u) = p'(u) + (Offset / R**2) * (DNdir/DU * R -  Ndir * (DR/R))
315
316   // P"(u) = p"(u) + (Offset / R) * (D2Ndir/DU - DNdir * (2.0 * Dr/ R**2) +
317   //         Ndir * ( (3.0 * Dr**2 / R**4) - (D2r / R**2)))
318
319   //P"'(u) = p"'(u) + (Offset / R) * (D3Ndir - (3.0 * Dr/R**2) * D2Ndir -
320   //         (3.0 * D2r / R2) * DNdir + (3.0 * Dr * Dr / R4) * DNdir -
321   //         (D3r/R2) * Ndir + (6.0 * Dr * Dr / R4) * Ndir +
322   //         (6.0 * Dr * D2r / R4) * Ndir - (15.0 * Dr* Dr* Dr /R6) * Ndir
323
324   gp_XYZ Ndir = (theD1.XYZ()).Crossed(myOffsetDir.XYZ());
325   gp_XYZ DNdir = (theD2.XYZ()).Crossed(myOffsetDir.XYZ());
326   gp_XYZ D2Ndir = (theD3.XYZ()).Crossed(myOffsetDir.XYZ());
327   gp_XYZ D3Ndir = (theD4.XYZ()).Crossed(myOffsetDir.XYZ());
328   Standard_Real R2 = Ndir.SquareModulus();
329   Standard_Real R = Sqrt(R2);
330   Standard_Real R3 = R2 * R;
331   Standard_Real R4 = R2 * R2;
332   Standard_Real R5 = R3 * R2;
333   Standard_Real R6 = R3 * R3;
334   Standard_Real R7 = R5 * R2;
335   Standard_Real Dr = Ndir.Dot(DNdir);
336   Standard_Real D2r = Ndir.Dot(D2Ndir) + DNdir.Dot(DNdir);
337   Standard_Real D3r = Ndir.Dot(D3Ndir) + 3.0 * DNdir.Dot(D2Ndir);
338   if (R7 <= gp::Resolution()) {
339     if (R6 <= gp::Resolution())
340       Standard_NullValue::Raise("CSLib_Offset: Null derivative");
341     // V3 = P"' (U) :
342     D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * Dr / R2));
343     D3Ndir.Subtract(DNdir.Multiplied(3.0 * ((D2r / R2) + (Dr*Dr / R4))));
344     D3Ndir.Add(Ndir.Multiplied(6.0*Dr*Dr / R4 + 6.0*Dr*D2r / R4 - 15.0*Dr*Dr*Dr / R6 - D3r));
345     D3Ndir.Multiply(myOffset / R);
346     // V2 = P" (U) :
347     R4 = R2 * R2;
348     D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R2));
349     D2Ndir.Subtract(Ndir.Multiplied((3.0 * Dr * Dr / R4) - (D2r / R2)));
350     D2Ndir.Multiply(myOffset / R);
351     // V1 = P' (U) :
352     DNdir.Multiply(R);
353     DNdir.Subtract(Ndir.Multiplied(Dr / R));
354     DNdir.Multiply(myOffset / R2);
355   }
356   else {
357     // V3 = P"' (U) :
358     D3Ndir.Divide(R);
359     D3Ndir.Subtract(D2Ndir.Multiplied(3.0 * Dr / R3));
360     D3Ndir.Subtract(DNdir.Multiplied((3.0 * ((D2r / R3) + (Dr*Dr) / R5))));
361     D3Ndir.Add(Ndir.Multiplied(6.0*Dr*Dr / R5 + 6.0*Dr*D2r / R5 - 15.0*Dr*Dr*Dr / R7 - D3r));
362     D3Ndir.Multiply(myOffset);
363     // V2 = P" (U) :
364     D2Ndir.Divide(R);
365     D2Ndir.Subtract(DNdir.Multiplied(2.0 * Dr / R3));
366     D2Ndir.Subtract(Ndir.Multiplied((3.0 * Dr * Dr / R5) - (D2r / R3)));
367     D2Ndir.Multiply(myOffset);
368     // V1 = P' (U) :
369     DNdir.Multiply(myOffset / R);
370     DNdir.Subtract(Ndir.Multiplied(myOffset * Dr / R3));
371   }
372
373   Ndir.Multiply(myOffset / R);
374   // P(u)
375   theValue.ChangeCoord().Add(Ndir);
376   // P'(u) :
377   theD1.Add(gp_Vec(DNdir));
378   // P"(u)
379   theD2.Add(gp_Vec(D2Ndir));
380   // P"'(u)
381   if (theIsDirChange)
382     theD3.Reverse();
383   theD3.Add(gp_Vec(D2Ndir));
384 }
385
386
387 Standard_Boolean GeomEvaluator_OffsetCurve::AdjustDerivative(
388     const Standard_Integer theMaxDerivative, const Standard_Real theU,
389     gp_Vec& theD1, gp_Vec& theD2, gp_Vec& theD3, gp_Vec& theD4) const
390 {
391   static const Standard_Real aTol = gp::Resolution();
392   static const Standard_Real aMinStep = 1e-7;
393   static const Standard_Integer aMaxDerivOrder = 3;
394
395   Standard_Boolean isDirectionChange = Standard_False;
396   Standard_Real anUinfium;
397   Standard_Real anUsupremum;
398   if (!myBaseAdaptor.IsNull())
399   {
400     anUinfium = myBaseAdaptor->FirstParameter();
401     anUsupremum = myBaseAdaptor->LastParameter();
402   }
403   else
404   {
405     anUinfium = myBaseCurve->FirstParameter();
406     anUsupremum = myBaseCurve->LastParameter();
407   }
408
409   static const Standard_Real DivisionFactor = 1.e-3;
410   Standard_Real du;
411   if ((anUsupremum >= RealLast()) || (anUinfium <= RealFirst()))
412     du = 0.0;
413   else
414     du = anUsupremum - anUinfium;
415
416   const Standard_Real aDelta = Max(du * DivisionFactor, aMinStep);
417
418   //Derivative is approximated by Taylor-series
419   Standard_Integer anIndex = 1; //Derivative order
420   gp_Vec V;
421
422   do
423   {
424     V = BaseDN(theU, ++anIndex);
425   } while ((V.SquareMagnitude() <= aTol) && anIndex < aMaxDerivOrder);
426
427   Standard_Real u;
428
429   if (theU - anUinfium < aDelta)
430     u = theU + aDelta;
431   else
432     u = theU - aDelta;
433
434   gp_Pnt P1, P2;
435   BaseD0(Min(theU, u), P1);
436   BaseD0(Max(theU, u), P2);
437
438   gp_Vec V1(P1, P2);
439   isDirectionChange = V.Dot(V1) < 0.0;
440   Standard_Real aSign = isDirectionChange ? -1.0 : 1.0;
441
442   theD1 = V * aSign;
443   gp_Vec* aDeriv[3] = { &theD2, &theD3, &theD4 };
444   for (Standard_Integer i = 1; i < theMaxDerivative; i++)
445     *(aDeriv[i - 1]) = BaseDN(theU, anIndex + i) * aSign;
446
447   return isDirectionChange;
448 }
449