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