0031939: Coding - correction of spelling errors in comments [part 3]
[occt.git] / src / GeomEvaluator / GeomEvaluator_OffsetSurface.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_OffsetSurface.hxx>
16
17 #include <CSLib.hxx>
18 #include <CSLib_NormalStatus.hxx>
19 #include <Geom_BezierSurface.hxx>
20 #include <Geom_BSplineSurface.hxx>
21 #include <Geom_UndefinedValue.hxx>
22 #include <GeomAdaptor_Surface.hxx>
23 #include <gp_Vec.hxx>
24 #include <Standard_RangeError.hxx>
25 #include <Standard_NumericError.hxx>
26
27 IMPLEMENT_STANDARD_RTTIEXT(GeomEvaluator_OffsetSurface,GeomEvaluator_Surface)
28
29 namespace {
30
31 // tolerance for considering derivative to be null
32 const Standard_Real the_D1MagTol = 1.e-9;
33
34 // If calculation of normal fails, try shifting the point towards the center 
35 // of the parametric space of the surface, in the hope that derivatives 
36 // are better defined there.
37 //
38 // This shift is iterative, starting with Precision::PConfusion()
39 // and increasing by multiple of 2 on each step.
40 //
41 // NB: temporarily this is made as static function and not class method, 
42 // hence code duplications
43 static Standard_Boolean shiftPoint (const Standard_Real theUStart, const Standard_Real theVStart,
44                                     Standard_Real& theU, Standard_Real& theV, 
45                                     const Handle(Geom_Surface)& theSurf,
46                                     const Handle(GeomAdaptor_Surface)& theAdaptor,
47                                     const gp_Vec& theD1U, const gp_Vec& theD1V)
48 {
49   // Get parametric bounds and closure status
50   Standard_Real aUMin, aUMax, aVMin, aVMax;
51   Standard_Boolean isUPeriodic, isVPeriodic;
52   if (! theSurf.IsNull())
53   {
54     theSurf->Bounds (aUMin, aUMax, aVMin, aVMax);
55     isUPeriodic = theSurf->IsUPeriodic();
56     isVPeriodic = theSurf->IsVPeriodic();
57   }
58   else
59   {
60     aUMin = theAdaptor->FirstUParameter();
61     aUMax = theAdaptor->LastUParameter();
62     aVMin = theAdaptor->FirstVParameter();
63     aVMax = theAdaptor->LastVParameter();
64     isUPeriodic = theAdaptor->IsUPeriodic();
65     isVPeriodic = theAdaptor->IsVPeriodic();
66   }
67
68   // check if either U or V is singular (normally one of them is)
69   Standard_Boolean isUSingular = (theD1U.SquareMagnitude() < the_D1MagTol * the_D1MagTol);
70   Standard_Boolean isVSingular = (theD1V.SquareMagnitude() < the_D1MagTol * the_D1MagTol);
71
72   // compute vector to shift from start point to center of the surface;
73   // if surface is periodic or singular in some direction, take shift in that direction zero
74   Standard_Real aDirU = (isUPeriodic || (isUSingular && ! isVSingular) ? 0. : 0.5 * (aUMin + aUMax) - theUStart);
75   Standard_Real aDirV = (isVPeriodic || (isVSingular && ! isUSingular) ? 0. : 0.5 * (aVMin + aVMax) - theVStart);
76   Standard_Real aDist = Sqrt (aDirU * aDirU + aDirV * aDirV);
77
78   // shift current point from its current position towards center, by value of twice
79   // current distance from it to start (but not less than Precision::PConfusion());
80   // fail if center is overpassed.
81   Standard_Real aDU = theU - theUStart;
82   Standard_Real aDV = theV - theVStart;
83   Standard_Real aStep = Max (2. * Sqrt (aDU * aDU + aDV * aDV), Precision::PConfusion());
84   if (aStep >= aDist)
85   {
86     return Standard_False;
87   }
88
89   aStep /= aDist;
90   theU += aDirU * aStep;
91   theV += aDirV * aStep;
92
93 //  std::cout << "Normal calculation failed at (" << theUStart << ", " << theVStart << "), shifting to (" << theU << ", " << theV << ")" << std::endl;
94
95   return Standard_True;
96 }
97
98 // auxiliary function
99 template<class SurfOrAdapt>
100 static void derivatives(Standard_Integer theMaxOrder,
101                         Standard_Integer theMinOrder,
102                         const Standard_Real theU,
103                         const Standard_Real theV,
104                         const SurfOrAdapt& theBasisSurf,
105                         const Standard_Integer theNU,
106                         const Standard_Integer theNV,
107                         const Standard_Boolean theAlongU,
108                         const Standard_Boolean theAlongV,
109                         const Handle(Geom_BSplineSurface)& theL,
110                         TColgp_Array2OfVec& theDerNUV,
111                         TColgp_Array2OfVec& theDerSurf)
112 {
113   Standard_Integer i, j;
114   gp_Pnt P;
115   gp_Vec DL1U, DL1V, DL2U, DL2V, DL2UV, DL3U, DL3UUV, DL3UVV, DL3V;
116
117   if (theAlongU || theAlongV)
118   {
119     theMaxOrder = 0;
120     TColgp_Array2OfVec DerSurfL(0, theMaxOrder + theNU + 1, 0, theMaxOrder + theNV + 1);
121     switch (theMinOrder)
122     {
123     case 1:
124       theL->D1(theU, theV, P, DL1U, DL1V);
125       DerSurfL.SetValue(1, 0, DL1U);
126       DerSurfL.SetValue(0, 1, DL1V);
127       break;
128     case 2:
129       theL->D2(theU, theV, P, DL1U, DL1V, DL2U, DL2V, DL2UV);
130       DerSurfL.SetValue(1, 0, DL1U);
131       DerSurfL.SetValue(0, 1, DL1V);
132       DerSurfL.SetValue(1, 1, DL2UV);
133       DerSurfL.SetValue(2, 0, DL2U);
134       DerSurfL.SetValue(0, 2, DL2V);
135       break;
136     case 3:
137       theL->D3(theU, theV, P, DL1U, DL1V, DL2U, DL2V, DL2UV, DL3U, DL3V, DL3UUV, DL3UVV);
138       DerSurfL.SetValue(1, 0, DL1U);
139       DerSurfL.SetValue(0, 1, DL1V);
140       DerSurfL.SetValue(1, 1, DL2UV);
141       DerSurfL.SetValue(2, 0, DL2U);
142       DerSurfL.SetValue(0, 2, DL2V);
143       DerSurfL.SetValue(3, 0, DL3U);
144       DerSurfL.SetValue(2, 1, DL3UUV);
145       DerSurfL.SetValue(1, 2, DL3UVV);
146       DerSurfL.SetValue(0, 3, DL3V);
147       break;
148     default:
149       break;
150     }
151
152     if (theNU <= theNV)
153     {
154       for (i = 0; i <= theMaxOrder + 1 + theNU; i++)
155         for (j = i; j <= theMaxOrder + theNV + 1; j++)
156           if (i + j > theMinOrder)
157           {
158             DerSurfL.SetValue(i, j, theL->DN(theU, theV, i, j));
159             theDerSurf.SetValue(i, j, theBasisSurf->DN(theU, theV, i, j));
160             if (i != j && j <= theNU + 1)
161             {
162               theDerSurf.SetValue(j, i, theBasisSurf->DN(theU, theV, j, i));
163               DerSurfL.SetValue(j, i, theL->DN(theU, theV, j, i));
164             }
165           }
166     }
167     else
168     {
169       for (j = 0; j <= theMaxOrder + 1 + theNV; j++)
170         for (i = j; i <= theMaxOrder + theNU + 1; i++)
171           if (i + j > theMinOrder)
172           {
173             DerSurfL.SetValue(i, j, theL->DN(theU, theV, i, j));
174             theDerSurf.SetValue(i, j, theBasisSurf->DN(theU, theV, i, j));
175             if (i != j && i <= theNV + 1)
176             {
177               theDerSurf.SetValue(j, i, theBasisSurf->DN(theU, theV, j, i));
178               DerSurfL.SetValue(j, i, theL->DN(theU, theV, j, i));
179             }
180           }
181     }
182     for (i = 0; i <= theMaxOrder + theNU; i++)
183       for (j = 0; j <= theMaxOrder + theNV; j++)
184       {
185         if (theAlongU)
186           theDerNUV.SetValue(i, j, CSLib::DNNUV(i, j, DerSurfL, theDerSurf));
187         if (theAlongV)
188           theDerNUV.SetValue(i, j, CSLib::DNNUV(i, j, theDerSurf, DerSurfL));
189       }
190   }
191   else
192   {
193     for (i = 0; i <= theMaxOrder + theNU+ 1; i++)
194     {
195       for (j = i; j <= theMaxOrder + theNV + 1; j++)
196       {
197         if (i + j > theMinOrder)
198         {
199           theDerSurf.SetValue(i, j, theBasisSurf->DN(theU, theV, i, j));
200           if (i != j
201            && j <= theDerSurf.UpperRow()
202            && i <= theDerSurf.UpperCol())
203           {
204             theDerSurf.SetValue(j, i, theBasisSurf->DN(theU, theV, j, i));
205           }
206         }
207       }
208     }
209     for (i = 0; i <= theMaxOrder + theNU; i++)
210       for (j = 0; j <= theMaxOrder + theNV; j++)
211         theDerNUV.SetValue(i, j, CSLib::DNNUV(i, j, theDerSurf));
212   }
213 }
214
215 inline Standard_Boolean IsInfiniteCoord (const gp_Vec& theVec)
216 {
217   return Precision::IsInfinite (theVec.X()) ||
218          Precision::IsInfinite (theVec.Y()) ||
219          Precision::IsInfinite (theVec.Z());
220 }
221
222 inline void CheckInfinite (const gp_Vec& theVecU, const gp_Vec& theVecV)
223 {
224   if (IsInfiniteCoord (theVecU) || IsInfiniteCoord (theVecV))
225   {
226     throw Standard_NumericError ("GeomEvaluator_OffsetSurface: Evaluation of infinite parameters");
227   }
228 }
229
230 } // end of anonymous namespace
231
232 GeomEvaluator_OffsetSurface::GeomEvaluator_OffsetSurface(
233         const Handle(Geom_Surface)& theBase,
234         const Standard_Real theOffset,
235         const Handle(Geom_OsculatingSurface)& theOscSurf)
236   : GeomEvaluator_Surface(),
237     myBaseSurf(theBase),
238     myOffset(theOffset),
239     myOscSurf(theOscSurf)
240 {
241   if (!myOscSurf.IsNull())
242     return; // osculating surface already exists
243
244   // Create osculating surface for B-spline and Besier surfaces only
245   if (myBaseSurf->IsKind(STANDARD_TYPE(Geom_BSplineSurface)) ||
246       myBaseSurf->IsKind(STANDARD_TYPE(Geom_BezierSurface)))
247     myOscSurf = new Geom_OsculatingSurface(myBaseSurf, Precision::Confusion());
248 }
249
250 GeomEvaluator_OffsetSurface::GeomEvaluator_OffsetSurface(
251         const Handle(GeomAdaptor_Surface)& theBase,
252         const Standard_Real theOffset,
253         const Handle(Geom_OsculatingSurface)& theOscSurf)
254   : GeomEvaluator_Surface(),
255     myBaseAdaptor(theBase),
256     myOffset(theOffset),
257     myOscSurf(theOscSurf)
258 {
259 }
260
261 void GeomEvaluator_OffsetSurface::D0(
262     const Standard_Real theU, const Standard_Real theV, gp_Pnt& theValue) const
263 {
264   Standard_Real aU = theU, aV = theV;
265   for (;;)
266   {
267     gp_Vec aD1U, aD1V;
268     BaseD1 (aU, aV, theValue, aD1U, aD1V);
269
270     CheckInfinite (aD1U, aD1V);
271
272     try 
273     {
274       CalculateD0(aU, aV, theValue, aD1U, aD1V);
275       break;
276     } 
277     catch (Geom_UndefinedValue&)
278     {
279       // if failed at parametric boundary, try taking derivative at shifted point
280       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, aD1U, aD1V))
281       {
282         throw;
283       }
284     }
285   }
286 }
287
288 void GeomEvaluator_OffsetSurface::D1(
289     const Standard_Real theU, const Standard_Real theV,
290     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V) const
291 {
292   Standard_Real aU = theU, aV = theV;
293   for (;;)
294   {
295     gp_Vec aD2U, aD2V, aD2UV;
296     BaseD2 (aU, aV, theValue, theD1U, theD1V, aD2U, aD2V, aD2UV);
297
298     CheckInfinite (theD1U, theD1V);
299
300     try 
301     {
302       CalculateD1(aU, aV, theValue, theD1U, theD1V, aD2U, aD2V, aD2UV);
303       break;
304     } 
305     catch (Geom_UndefinedValue&)
306     {
307       // if failed at parametric boundary, try taking derivative at shifted point
308       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, theD1U, theD1V))
309       {
310         throw;
311       }
312     }
313   }
314 }
315
316 void GeomEvaluator_OffsetSurface::D2(
317     const Standard_Real theU, const Standard_Real theV,
318     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
319     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV) const
320 {
321   Standard_Real aU = theU, aV = theV;
322   for (;;)
323   {
324     gp_Vec aD3U, aD3V, aD3UUV, aD3UVV;
325     BaseD3 (aU, aV, theValue, theD1U, theD1V,
326             theD2U, theD2V, theD2UV, aD3U, aD3V, aD3UUV, aD3UVV);
327
328     CheckInfinite (theD1U, theD1V);
329
330     try
331     {
332       CalculateD2 (aU, aV, theValue, theD1U, theD1V,
333                    theD2U, theD2V, theD2UV, aD3U, aD3V, aD3UUV, aD3UVV);
334       break;
335     } 
336     catch (Geom_UndefinedValue&)
337     {
338       // if failed at parametric boundary, try taking derivative at shifted point
339       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, theD1U, theD1V))
340       {
341         throw;
342       }
343     }
344   }
345 }
346
347 void GeomEvaluator_OffsetSurface::D3(
348     const Standard_Real theU, const Standard_Real theV,
349     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
350     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV,
351     gp_Vec& theD3U, gp_Vec& theD3V, gp_Vec& theD3UUV, gp_Vec& theD3UVV) const
352 {
353   Standard_Real aU = theU, aV = theV;
354   for (;;)
355   {
356     BaseD3 (aU, aV, theValue, theD1U, theD1V,
357             theD2U, theD2V, theD2UV, theD3U, theD3V, theD3UUV, theD3UVV);
358
359     CheckInfinite (theD1U, theD1V);
360
361     try
362     {
363       CalculateD3 (aU, aV, theValue, theD1U, theD1V,
364                    theD2U, theD2V, theD2UV, theD3U, theD3V, theD3UUV, theD3UVV);
365       break;
366     } 
367     catch (Geom_UndefinedValue&)
368     {
369       // if failed at parametric boundary, try taking derivative at shifted point
370       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, theD1U, theD1V))
371       {
372         throw;
373       }
374     }
375   }
376 }
377
378 gp_Vec GeomEvaluator_OffsetSurface::DN(
379     const Standard_Real theU, const Standard_Real theV,
380     const Standard_Integer theDerU, const Standard_Integer theDerV) const
381 {
382   Standard_RangeError_Raise_if(theDerU < 0, "GeomEvaluator_OffsetSurface::DN(): theDerU < 0");
383   Standard_RangeError_Raise_if(theDerV < 0, "GeomEvaluator_OffsetSurface::DN(): theDerV < 0");
384   Standard_RangeError_Raise_if(theDerU + theDerV < 1,
385       "GeomEvaluator_OffsetSurface::DN(): theDerU + theDerV < 1");
386
387   Standard_Real aU = theU, aV = theV;
388   for (;;)
389   {
390     gp_Pnt aP;
391     gp_Vec aD1U, aD1V;
392     BaseD1 (aU, aV, aP, aD1U, aD1V);
393
394     CheckInfinite (aD1U, aD1V);
395
396     try
397     {
398       return CalculateDN (aU, aV, theDerU, theDerV, aD1U, aD1V);
399     } 
400     catch (Geom_UndefinedValue&)
401     {
402       // if failed at parametric boundary, try taking derivative at shifted point
403       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, aD1U, aD1V))
404       {
405         throw;
406       }
407     }
408   }
409 }
410
411
412 void GeomEvaluator_OffsetSurface::BaseD0(const Standard_Real theU, const Standard_Real theV,
413                                          gp_Pnt& theValue) const
414 {
415   if (!myBaseAdaptor.IsNull())
416     myBaseAdaptor->D0(theU, theV, theValue);
417   else
418     myBaseSurf->D0(theU, theV, theValue);
419 }
420
421 void GeomEvaluator_OffsetSurface::BaseD1(const Standard_Real theU, const Standard_Real theV,
422                                          gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V) const
423 {
424   if (!myBaseAdaptor.IsNull())
425     myBaseAdaptor->D1(theU, theV, theValue, theD1U, theD1V);
426   else
427     myBaseSurf->D1(theU, theV, theValue, theD1U, theD1V);
428 }
429
430 void GeomEvaluator_OffsetSurface::BaseD2(const Standard_Real theU, const Standard_Real theV,
431                                          gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
432                                          gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV) const
433 {
434   if (!myBaseAdaptor.IsNull())
435     myBaseAdaptor->D2(theU, theV, theValue, theD1U, theD1V, theD2U, theD2V, theD2UV);
436   else
437     myBaseSurf->D2(theU, theV, theValue, theD1U, theD1V, theD2U, theD2V, theD2UV);
438 }
439
440 void GeomEvaluator_OffsetSurface::BaseD3(
441     const Standard_Real theU, const Standard_Real theV,
442     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
443     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV,
444     gp_Vec& theD3U, gp_Vec& theD3V, gp_Vec& theD3UUV, gp_Vec& theD3UVV) const
445 {
446   if (!myBaseAdaptor.IsNull())
447     myBaseAdaptor->D3(theU, theV, theValue, theD1U, theD1V,
448     theD2U, theD2V, theD2UV, theD3U, theD3V, theD3UUV, theD3UVV);
449   else
450     myBaseSurf->D3(theU, theV, theValue, theD1U, theD1V,
451     theD2U, theD2V, theD2UV, theD3U, theD3V, theD3UUV, theD3UVV);
452 }
453
454
455 void GeomEvaluator_OffsetSurface::CalculateD0(
456     const Standard_Real theU, const Standard_Real theV,
457     gp_Pnt& theValue,
458     const gp_Vec& theD1U, const gp_Vec& theD1V) const
459 {
460   // Normalize derivatives before normal calculation because it gives more stable result.
461   // There will be normalized only derivatives greater than 1.0 to avoid differences in last significant digit
462   gp_Vec aD1U(theD1U);
463   gp_Vec aD1V(theD1V);
464   Standard_Real aD1UNorm2 = aD1U.SquareMagnitude();
465   Standard_Real aD1VNorm2 = aD1V.SquareMagnitude();
466   if (aD1UNorm2 > 1.0)
467     aD1U /= Sqrt(aD1UNorm2);
468   if (aD1VNorm2 > 1.0)
469     aD1V /= Sqrt(aD1VNorm2);
470
471   gp_Vec aNorm = aD1U.Crossed(aD1V);
472   if (aNorm.SquareMagnitude() > the_D1MagTol * the_D1MagTol)
473   {
474     // Non singular case. Simple computations.
475     aNorm.Normalize();
476     theValue.SetXYZ(theValue.XYZ() + myOffset * aNorm.XYZ());
477   }
478   else
479   {
480     const Standard_Integer MaxOrder = 3;
481
482     Handle(Geom_BSplineSurface) L;
483     Standard_Boolean isOpposite = Standard_False;
484     Standard_Boolean AlongU = Standard_False;
485     Standard_Boolean AlongV = Standard_False;
486     if (!myOscSurf.IsNull())
487     {
488       AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
489       AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
490     }
491     const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
492
493     TColgp_Array2OfVec DerNUV(0, MaxOrder, 0, MaxOrder);
494     TColgp_Array2OfVec DerSurf(0, MaxOrder + 1, 0, MaxOrder + 1);
495     Standard_Integer OrderU, OrderV;
496     Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
497     Bounds(Umin, Umax, Vmin, Vmax);
498
499     DerSurf.SetValue(1, 0, theD1U);
500     DerSurf.SetValue(0, 1, theD1V);
501     if (!myBaseSurf.IsNull())
502       derivatives(MaxOrder, 1, theU, theV, myBaseSurf, 0, 0, AlongU, AlongV, L, DerNUV, DerSurf);
503     else
504       derivatives(MaxOrder, 1, theU, theV, myBaseAdaptor, 0, 0, AlongU, AlongV, L, DerNUV, DerSurf);
505
506     gp_Dir Normal;
507     CSLib_NormalStatus NStatus = CSLib_Singular;
508     CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
509                   NStatus, Normal, OrderU, OrderV);
510     if (NStatus == CSLib_InfinityOfSolutions)
511     {
512       // Replace zero derivative and try to calculate normal
513       gp_Vec aNewDU = theD1U;
514       gp_Vec aNewDV = theD1V;
515       if (ReplaceDerivative(theU, theV, aNewDU, aNewDV, the_D1MagTol * the_D1MagTol))
516         CSLib::Normal(aNewDU, aNewDV, the_D1MagTol, NStatus, Normal);
517     }
518
519     if (NStatus != CSLib_Defined)
520       throw Geom_UndefinedValue(
521           "GeomEvaluator_OffsetSurface::CalculateD0(): Unable to calculate normal");
522
523     theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * Normal.XYZ());
524   }
525 }
526
527 void GeomEvaluator_OffsetSurface::CalculateD1(
528     const Standard_Real theU, const Standard_Real theV,
529     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
530     const gp_Vec& theD2U, const gp_Vec& theD2V, const gp_Vec& theD2UV) const
531 {
532   // Check offset side.
533   Handle(Geom_BSplineSurface) L;
534   Standard_Boolean isOpposite = Standard_False;
535   Standard_Boolean AlongU = Standard_False;
536   Standard_Boolean AlongV = Standard_False;
537
538   // Normalize derivatives before normal calculation because it gives more stable result.
539   // There will be normalized only derivatives greater than 1.0 to avoid differences in last significant digit
540   gp_Vec aD1U(theD1U);
541   gp_Vec aD1V(theD1V);
542   Standard_Real aD1UNorm2 = aD1U.SquareMagnitude();
543   Standard_Real aD1VNorm2 = aD1V.SquareMagnitude();
544   if (aD1UNorm2 > 1.0)
545     aD1U /= Sqrt(aD1UNorm2);
546   if (aD1VNorm2 > 1.0)
547     aD1V /= Sqrt(aD1VNorm2);
548
549   Standard_Boolean isSingular = Standard_False;
550   Standard_Integer MaxOrder = 0;
551   gp_Vec aNorm = aD1U.Crossed(aD1V);
552   if (aNorm.SquareMagnitude() <= the_D1MagTol * the_D1MagTol)
553   {
554     if (!myOscSurf.IsNull())
555     {
556       AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
557       AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
558     }
559
560     MaxOrder = 3;
561     isSingular = Standard_True;
562   }
563
564   const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
565   
566   if (!isSingular)
567   {
568     // AlongU or AlongV leads to more complex D1 computation
569     // Try to compute D0 and D1 much simpler
570     aNorm.Normalize();
571     theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * aNorm.XYZ());
572
573     gp_Vec aN0(aNorm.XYZ()), aN1U, aN1V;
574     Standard_Real aScale = (theD1U^theD1V).Dot(aN0);
575     aN1U.SetX(theD2U.Y() * theD1V.Z() + theD1U.Y() * theD2UV.Z()
576       - theD2U.Z() * theD1V.Y() - theD1U.Z() * theD2UV.Y());
577     aN1U.SetY((theD2U.X() * theD1V.Z() + theD1U.X() * theD2UV.Z()
578       - theD2U.Z() * theD1V.X() - theD1U.Z() * theD2UV.X()) * -1.0);
579     aN1U.SetZ(theD2U.X() * theD1V.Y() + theD1U.X() * theD2UV.Y()
580       - theD2U.Y() * theD1V.X() - theD1U.Y() * theD2UV.X());
581     Standard_Real aScaleU = aN1U.Dot(aN0);
582     aN1U.Subtract(aScaleU * aN0);
583     aN1U /= aScale;
584
585     aN1V.SetX(theD2UV.Y() * theD1V.Z() + theD2V.Z() * theD1U.Y()
586       - theD2UV.Z() * theD1V.Y() - theD2V.Y() * theD1U.Z());
587     aN1V.SetY((theD2UV.X() * theD1V.Z() + theD2V.Z() * theD1U.X()
588       - theD2UV.Z() * theD1V.X() - theD2V.X() * theD1U.Z()) * -1.0);
589     aN1V.SetZ(theD2UV.X() * theD1V.Y() + theD2V.Y() * theD1U.X()
590       - theD2UV.Y() * theD1V.X() - theD2V.X() * theD1U.Y());
591     Standard_Real aScaleV = aN1V.Dot(aN0);
592     aN1V.Subtract(aScaleV * aN0);
593     aN1V /= aScale;
594
595     theD1U += myOffset * aSign * aN1U;
596     theD1V += myOffset * aSign * aN1V;
597
598     return;
599   }
600   
601   Standard_Integer OrderU, OrderV;
602   TColgp_Array2OfVec DerNUV(0, MaxOrder + 1, 0, MaxOrder + 1);
603   TColgp_Array2OfVec DerSurf(0, MaxOrder + 2, 0, MaxOrder + 2);
604   Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
605   Bounds(Umin, Umax, Vmin, Vmax);
606
607   DerSurf.SetValue(1, 0, theD1U);
608   DerSurf.SetValue(0, 1, theD1V);
609   DerSurf.SetValue(1, 1, theD2UV);
610   DerSurf.SetValue(2, 0, theD2U);
611   DerSurf.SetValue(0, 2, theD2V);
612   if (!myBaseSurf.IsNull())
613     derivatives(MaxOrder, 2, theU, theV, myBaseSurf, 1, 1, AlongU, AlongV, L, DerNUV, DerSurf);
614   else
615     derivatives(MaxOrder, 2, theU, theV, myBaseAdaptor, 1, 1, AlongU, AlongV, L, DerNUV, DerSurf);
616
617   gp_Dir Normal;
618   CSLib_NormalStatus NStatus;
619   CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax, NStatus, Normal, OrderU, OrderV);
620   if (NStatus == CSLib_InfinityOfSolutions)
621   {
622     gp_Vec aNewDU = theD1U;
623     gp_Vec aNewDV = theD1V;
624     // Replace zero derivative and try to calculate normal
625     if (ReplaceDerivative(theU, theV, aNewDU, aNewDV, the_D1MagTol * the_D1MagTol))
626     {
627       DerSurf.SetValue(1, 0, aNewDU);
628       DerSurf.SetValue(0, 1, aNewDV);
629       if (!myBaseSurf.IsNull())
630         derivatives(MaxOrder, 2, theU, theV, myBaseSurf, 1, 1, AlongU, AlongV, L, DerNUV, DerSurf);
631       else
632         derivatives(MaxOrder, 2, theU, theV, myBaseAdaptor, 1, 1, AlongU, AlongV, L, DerNUV, DerSurf);
633       CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
634                     NStatus, Normal, OrderU, OrderV);
635     }
636   }
637
638   if (NStatus != CSLib_Defined)
639     throw Geom_UndefinedValue(
640         "GeomEvaluator_OffsetSurface::CalculateD1(): Unable to calculate normal");
641
642   theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * Normal.XYZ());
643
644   theD1U = DerSurf(1, 0) + myOffset * aSign * CSLib::DNNormal(1, 0, DerNUV, OrderU, OrderV);
645   theD1V = DerSurf(0, 1) + myOffset * aSign * CSLib::DNNormal(0, 1, DerNUV, OrderU, OrderV);
646 }
647
648 void GeomEvaluator_OffsetSurface::CalculateD2(
649     const Standard_Real theU, const Standard_Real theV,
650     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
651     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV,
652     const gp_Vec& theD3U, const gp_Vec& theD3V, const gp_Vec& theD3UUV, const gp_Vec& theD3UVV) const
653 {
654   gp_Dir Normal;
655   CSLib_NormalStatus NStatus;
656   CSLib::Normal(theD1U, theD1V, the_D1MagTol, NStatus, Normal);
657
658   const Standard_Integer MaxOrder = (NStatus == CSLib_Defined) ? 0 : 3;
659   Standard_Integer OrderU, OrderV;
660   TColgp_Array2OfVec DerNUV(0, MaxOrder + 2, 0, MaxOrder + 2);
661   TColgp_Array2OfVec DerSurf(0, MaxOrder + 3, 0, MaxOrder + 3);
662
663   Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
664   Bounds(Umin, Umax, Vmin, Vmax);
665
666   DerSurf.SetValue(1, 0, theD1U);
667   DerSurf.SetValue(0, 1, theD1V);
668   DerSurf.SetValue(1, 1, theD2UV);
669   DerSurf.SetValue(2, 0, theD2U);
670   DerSurf.SetValue(0, 2, theD2V);
671   DerSurf.SetValue(3, 0, theD3U);
672   DerSurf.SetValue(2, 1, theD3UUV);
673   DerSurf.SetValue(1, 2, theD3UVV);
674   DerSurf.SetValue(0, 3, theD3V);
675   //*********************
676
677   Handle(Geom_BSplineSurface) L;
678   Standard_Boolean isOpposite = Standard_False;
679   Standard_Boolean AlongU = Standard_False;
680   Standard_Boolean AlongV = Standard_False;
681   if ((NStatus != CSLib_Defined) && !myOscSurf.IsNull())
682   {
683     AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
684     AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
685   }
686   const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
687
688   if (!myBaseSurf.IsNull())
689     derivatives(MaxOrder, 3, theU, theV, myBaseSurf, 2, 2, AlongU, AlongV, L, DerNUV, DerSurf);
690   else
691     derivatives(MaxOrder, 3, theU, theV, myBaseAdaptor, 2, 2, AlongU, AlongV, L, DerNUV, DerSurf);
692
693   CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
694                 NStatus, Normal, OrderU, OrderV);
695   if (NStatus != CSLib_Defined)
696     throw Geom_UndefinedValue(
697         "GeomEvaluator_OffsetSurface::CalculateD2(): Unable to calculate normal");
698
699   theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * Normal.XYZ());
700
701   theD1U = DerSurf(1, 0) + myOffset * aSign * CSLib::DNNormal(1, 0, DerNUV, OrderU, OrderV);
702   theD1V = DerSurf(0, 1) + myOffset * aSign * CSLib::DNNormal(0, 1, DerNUV, OrderU, OrderV);
703
704   if (!myBaseSurf.IsNull())
705   {
706     theD2U = myBaseSurf->DN(theU, theV, 2, 0);
707     theD2V = myBaseSurf->DN(theU, theV, 0, 2);
708     theD2UV = myBaseSurf->DN(theU, theV, 1, 1);
709   }
710   else
711   {
712     theD2U = myBaseAdaptor->DN(theU, theV, 2, 0);
713     theD2V = myBaseAdaptor->DN(theU, theV, 0, 2);
714     theD2UV = myBaseAdaptor->DN(theU, theV, 1, 1);
715   }
716
717   theD2U += aSign * myOffset * CSLib::DNNormal(2, 0, DerNUV, OrderU, OrderV);
718   theD2V += aSign * myOffset * CSLib::DNNormal(0, 2, DerNUV, OrderU, OrderV);
719   theD2UV += aSign * myOffset * CSLib::DNNormal(1, 1, DerNUV, OrderU, OrderV);
720 }
721
722 void GeomEvaluator_OffsetSurface::CalculateD3(
723     const Standard_Real theU, const Standard_Real theV,
724     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
725     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV,
726     gp_Vec& theD3U, gp_Vec& theD3V, gp_Vec& theD3UUV, gp_Vec& theD3UVV) const
727 {
728   gp_Dir Normal;
729   CSLib_NormalStatus NStatus;
730   CSLib::Normal(theD1U, theD1V, the_D1MagTol, NStatus, Normal);
731   const Standard_Integer MaxOrder = (NStatus == CSLib_Defined) ? 0 : 3;
732   Standard_Integer OrderU, OrderV;
733   TColgp_Array2OfVec DerNUV(0, MaxOrder + 3, 0, MaxOrder + 3);
734   TColgp_Array2OfVec DerSurf(0, MaxOrder + 4, 0, MaxOrder + 4);
735   Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
736   Bounds(Umin, Umax, Vmin, Vmax);
737
738   DerSurf.SetValue(1, 0, theD1U);
739   DerSurf.SetValue(0, 1, theD1V);
740   DerSurf.SetValue(1, 1, theD2UV);
741   DerSurf.SetValue(2, 0, theD2U);
742   DerSurf.SetValue(0, 2, theD2V);
743   DerSurf.SetValue(3, 0, theD3U);
744   DerSurf.SetValue(2, 1, theD3UUV);
745   DerSurf.SetValue(1, 2, theD3UVV);
746   DerSurf.SetValue(0, 3, theD3V);
747
748
749   //*********************
750   Handle(Geom_BSplineSurface) L;
751   Standard_Boolean isOpposite = Standard_False;
752   Standard_Boolean AlongU = Standard_False;
753   Standard_Boolean AlongV = Standard_False;
754   if ((NStatus != CSLib_Defined) && !myOscSurf.IsNull())
755   {
756     AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
757     AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
758   }
759   const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
760
761   if (!myBaseSurf.IsNull())
762     derivatives(MaxOrder, 3, theU, theV, myBaseSurf, 3, 3, AlongU, AlongV, L, DerNUV, DerSurf);
763   else
764     derivatives(MaxOrder, 3, theU, theV, myBaseAdaptor, 3, 3, AlongU, AlongV, L, DerNUV, DerSurf);
765
766   CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
767                 NStatus, Normal, OrderU, OrderV);
768   if (NStatus != CSLib_Defined)
769     throw Geom_UndefinedValue(
770         "GeomEvaluator_OffsetSurface::CalculateD3(): Unable to calculate normal");
771
772   theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * Normal.XYZ());
773
774   theD1U = DerSurf(1, 0) + myOffset * aSign * CSLib::DNNormal(1, 0, DerNUV, OrderU, OrderV);
775   theD1V = DerSurf(0, 1) + myOffset * aSign * CSLib::DNNormal(0, 1, DerNUV, OrderU, OrderV);
776
777   if (!myBaseSurf.IsNull())
778   {
779     theD2U   = myBaseSurf->DN(theU, theV, 2, 0);
780     theD2V   = myBaseSurf->DN(theU, theV, 0, 2);
781     theD2UV  = myBaseSurf->DN(theU, theV, 1, 1);
782     theD3U   = myBaseSurf->DN(theU, theV, 3, 0);
783     theD3V   = myBaseSurf->DN(theU, theV, 0, 3);
784     theD3UUV = myBaseSurf->DN(theU, theV, 2, 1);
785     theD3UVV = myBaseSurf->DN(theU, theV, 1, 2);
786   }
787   else
788   {
789     theD2U   = myBaseAdaptor->DN(theU, theV, 2, 0);
790     theD2V   = myBaseAdaptor->DN(theU, theV, 0, 2);
791     theD2UV  = myBaseAdaptor->DN(theU, theV, 1, 1);
792     theD3U   = myBaseAdaptor->DN(theU, theV, 3, 0);
793     theD3V   = myBaseAdaptor->DN(theU, theV, 0, 3);
794     theD3UUV = myBaseAdaptor->DN(theU, theV, 2, 1);
795     theD3UVV = myBaseAdaptor->DN(theU, theV, 1, 2);
796   }
797
798   theD2U   += aSign * myOffset * CSLib::DNNormal(2, 0, DerNUV, OrderU, OrderV);
799   theD2V   += aSign * myOffset * CSLib::DNNormal(0, 2, DerNUV, OrderU, OrderV);
800   theD2UV  += aSign * myOffset * CSLib::DNNormal(1, 1, DerNUV, OrderU, OrderV);
801   theD3U   += aSign * myOffset * CSLib::DNNormal(3, 0, DerNUV, OrderU, OrderV);
802   theD3V   += aSign * myOffset * CSLib::DNNormal(0, 3, DerNUV, OrderU, OrderV);
803   theD3UUV += aSign * myOffset * CSLib::DNNormal(2, 1, DerNUV, OrderU, OrderV);
804   theD3UVV += aSign * myOffset * CSLib::DNNormal(1, 2, DerNUV, OrderU, OrderV);
805 }
806
807 gp_Vec GeomEvaluator_OffsetSurface::CalculateDN(
808     const Standard_Real theU, const Standard_Real theV,
809     const Standard_Integer theNu, const Standard_Integer theNv,
810     const gp_Vec& theD1U, const gp_Vec& theD1V) const
811 {
812   gp_Dir Normal;
813   CSLib_NormalStatus NStatus;
814   CSLib::Normal(theD1U, theD1V, the_D1MagTol, NStatus, Normal);
815   const Standard_Integer MaxOrder = (NStatus == CSLib_Defined) ? 0 : 3;
816   Standard_Integer OrderU, OrderV;
817   TColgp_Array2OfVec DerNUV(0, MaxOrder + theNu, 0, MaxOrder + theNv);
818   TColgp_Array2OfVec DerSurf(0, MaxOrder + theNu + 1, 0, MaxOrder + theNv + 1);
819
820   Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
821   Bounds(Umin, Umax, Vmin, Vmax);
822
823   DerSurf.SetValue(1, 0, theD1U);
824   DerSurf.SetValue(0, 1, theD1V);
825
826   //*********************
827   Handle(Geom_BSplineSurface) L;
828   //   Is there any osculatingsurface along U or V;
829   Standard_Boolean isOpposite = Standard_False;
830   Standard_Boolean AlongU = Standard_False;
831   Standard_Boolean AlongV = Standard_False;
832   if ((NStatus != CSLib_Defined) && !myOscSurf.IsNull())
833   {
834     AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
835     AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
836   }
837   const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
838
839   if (!myBaseSurf.IsNull())
840     derivatives(MaxOrder, 1, theU, theV, myBaseSurf, theNu, theNv, AlongU, AlongV, L, DerNUV, DerSurf);
841   else
842     derivatives(MaxOrder, 1, theU, theV, myBaseAdaptor, theNu, theNv, AlongU, AlongV, L, DerNUV, DerSurf);
843
844   CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
845                 NStatus, Normal, OrderU, OrderV);
846   if (NStatus != CSLib_Defined)
847     throw Geom_UndefinedValue(
848         "GeomEvaluator_OffsetSurface::CalculateDN(): Unable to calculate normal");
849
850   gp_Vec D;
851   if (!myBaseSurf.IsNull())
852     D = myBaseSurf->DN(theU, theV, theNu, theNv);
853   else
854     D = myBaseAdaptor->DN(theU, theV, theNu, theNv);
855
856   D += aSign * myOffset * CSLib::DNNormal(theNu, theNv, DerNUV, OrderU, OrderV);
857   return D;
858 }
859
860
861 void GeomEvaluator_OffsetSurface::Bounds(Standard_Real& theUMin, Standard_Real& theUMax,
862                                          Standard_Real& theVMin, Standard_Real& theVMax) const
863 {
864   if (!myBaseSurf.IsNull())
865     myBaseSurf->Bounds(theUMin, theUMax, theVMin, theVMax);
866   else
867   {
868     theUMin = myBaseAdaptor->FirstUParameter();
869     theUMax = myBaseAdaptor->LastUParameter();
870     theVMin = myBaseAdaptor->FirstVParameter();
871     theVMax = myBaseAdaptor->LastVParameter();
872   }
873 }
874
875
876 Standard_Boolean GeomEvaluator_OffsetSurface::ReplaceDerivative(
877     const Standard_Real theU, const Standard_Real theV,
878     gp_Vec& theDU, gp_Vec& theDV,
879     const Standard_Real theSquareTol) const
880 {
881   Standard_Boolean isReplaceDU = theDU.SquareMagnitude() < theSquareTol;
882   Standard_Boolean isReplaceDV = theDV.SquareMagnitude() < theSquareTol;
883   Standard_Boolean isReplaced = Standard_False;
884   if (isReplaceDU ^ isReplaceDV)
885   {
886     Standard_Real aU = theU;
887     Standard_Real aV = theV;
888     Standard_Real aUMin = 0, aUMax = 0, aVMin = 0, aVMax = 0;
889     Bounds(aUMin, aUMax, aVMin, aVMax);
890
891     // Calculate step along non-zero derivative
892     Standard_Real aStep;
893     Handle(Adaptor3d_Surface) aSurfAdapt;
894     if (!myBaseAdaptor.IsNull())
895       aSurfAdapt = myBaseAdaptor;
896     else
897       aSurfAdapt = new GeomAdaptor_Surface(myBaseSurf);
898     if (isReplaceDV)
899     {
900       aStep = Precision::Confusion() * theDU.Magnitude();
901       if (aStep > aUMax - aUMin)
902         aStep = (aUMax - aUMin) / 100.;
903     }
904     else
905     {
906       aStep = Precision::Confusion() * theDV.Magnitude();
907       if (aStep > aVMax - aVMin)
908         aStep = (aVMax - aVMin) / 100.;
909     }
910
911     gp_Pnt aP;
912     gp_Vec aDU, aDV;
913     // Step away from current parametric coordinates and calculate derivatives once again.
914     // Replace zero derivative by the obtained.
915     for (Standard_Real aStepSign = -1.0; aStepSign <= 1.0 && !isReplaced; aStepSign += 2.0)
916     {
917       if (isReplaceDV)
918       {
919         aU = theU + aStepSign * aStep;
920         if (aU < aUMin || aU > aUMax)
921           continue;
922       }
923       else
924       {
925         aV = theV + aStepSign * aStep;
926         if (aV < aVMin || aV > aVMax)
927           continue;
928       }
929
930       BaseD1(aU, aV, aP, aDU, aDV);
931
932       if (isReplaceDU && aDU.SquareMagnitude() > theSquareTol)
933       {
934         theDU = aDU;
935         isReplaced = Standard_True;
936       }
937       if (isReplaceDV && aDV.SquareMagnitude() > theSquareTol)
938       {
939         theDV = aDV;
940         isReplaced = Standard_True;
941       }
942     }
943   }
944   return isReplaced;
945 }