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