c41d9a7efb63ce4aaa64f491f4993a25d2a64b30
[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 <GeomAdaptor_HSurface.hxx>
18 #include <CSLib.hxx>
19 #include <CSLib_NormalStatus.hxx>
20 #include <Geom_BezierSurface.hxx>
21 #include <Geom_BSplineSurface.hxx>
22 #include <Geom_UndefinedValue.hxx>
23 #include <GeomAdaptor_HSurface.hxx>
24 #include <gp_Vec.hxx>
25 #include <Standard_RangeError.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_HSurface)& 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       for (j = i; j <= theMaxOrder + theNV + 1; j++)
195         if (i + j > theMinOrder)
196         {
197           theDerSurf.SetValue(i, j, theBasisSurf->DN(theU, theV, i, j));
198           if (i != j)
199             theDerSurf.SetValue(j, i, theBasisSurf->DN(theU, theV, j, i));
200         }
201     for (i = 0; i <= theMaxOrder + theNU; i++)
202       for (j = 0; j <= theMaxOrder + theNV; j++)
203         theDerNUV.SetValue(i, j, CSLib::DNNUV(i, j, theDerSurf));
204   }
205 }
206
207 } // end of anonymous namespace
208
209 GeomEvaluator_OffsetSurface::GeomEvaluator_OffsetSurface(
210         const Handle(Geom_Surface)& theBase,
211         const Standard_Real theOffset,
212         const Handle(Geom_OsculatingSurface)& theOscSurf)
213   : GeomEvaluator_Surface(),
214     myBaseSurf(theBase),
215     myOffset(theOffset),
216     myOscSurf(theOscSurf)
217 {
218   if (!myOscSurf.IsNull())
219     return; // osculating surface already exists
220
221   // Create osculating surface for B-spline and Besier surfaces only
222   if (myBaseSurf->IsKind(STANDARD_TYPE(Geom_BSplineSurface)) ||
223       myBaseSurf->IsKind(STANDARD_TYPE(Geom_BezierSurface)))
224     myOscSurf = new Geom_OsculatingSurface(myBaseSurf, Precision::Confusion());
225 }
226
227 GeomEvaluator_OffsetSurface::GeomEvaluator_OffsetSurface(
228         const Handle(GeomAdaptor_HSurface)& theBase,
229         const Standard_Real theOffset,
230         const Handle(Geom_OsculatingSurface)& theOscSurf)
231   : GeomEvaluator_Surface(),
232     myBaseAdaptor(theBase),
233     myOffset(theOffset),
234     myOscSurf(theOscSurf)
235 {
236 }
237
238 void GeomEvaluator_OffsetSurface::D0(
239     const Standard_Real theU, const Standard_Real theV, gp_Pnt& theValue) const
240 {
241   Standard_Real aU = theU, aV = theV;
242   for (;;)
243   {
244     gp_Vec aD1U, aD1V;
245     BaseD1 (aU, aV, theValue, aD1U, aD1V);
246     try 
247     {
248       CalculateD0(aU, aV, theValue, aD1U, aD1V);
249       break;
250     } 
251     catch (Geom_UndefinedValue&)
252     {
253       // if failed at parametric boundary, try taking derivative at shifted point
254       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, aD1U, aD1V))
255       {
256         throw;
257       }
258     }
259   }
260 }
261
262 void GeomEvaluator_OffsetSurface::D1(
263     const Standard_Real theU, const Standard_Real theV,
264     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V) const
265 {
266   Standard_Real aU = theU, aV = theV;
267   for (;;)
268   {
269     gp_Vec aD2U, aD2V, aD2UV;
270     BaseD2 (aU, aV, theValue, theD1U, theD1V, aD2U, aD2V, aD2UV);
271     try 
272     {
273       CalculateD1(aU, aV, theValue, theD1U, theD1V, aD2U, aD2V, aD2UV);
274       break;
275     } 
276     catch (Geom_UndefinedValue&)
277     {
278       // if failed at parametric boundary, try taking derivative at shifted point
279       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, theD1U, theD1V))
280       {
281         throw;
282       }
283     }
284   }
285 }
286
287 void GeomEvaluator_OffsetSurface::D2(
288     const Standard_Real theU, const Standard_Real theV,
289     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
290     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV) const
291 {
292   Standard_Real aU = theU, aV = theV;
293   for (;;)
294   {
295     gp_Vec aD3U, aD3V, aD3UUV, aD3UVV;
296     BaseD3 (aU, aV, theValue, theD1U, theD1V,
297             theD2U, theD2V, theD2UV, aD3U, aD3V, aD3UUV, aD3UVV);
298     try
299     {
300       CalculateD2 (aU, aV, theValue, theD1U, theD1V,
301                    theD2U, theD2V, theD2UV, aD3U, aD3V, aD3UUV, aD3UVV);
302       break;
303     } 
304     catch (Geom_UndefinedValue&)
305     {
306       // if failed at parametric boundary, try taking derivative at shifted point
307       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, theD1U, theD1V))
308       {
309         throw;
310       }
311     }
312   }
313 }
314
315 void GeomEvaluator_OffsetSurface::D3(
316     const Standard_Real theU, const Standard_Real theV,
317     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
318     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV,
319     gp_Vec& theD3U, gp_Vec& theD3V, gp_Vec& theD3UUV, gp_Vec& theD3UVV) const
320 {
321   Standard_Real aU = theU, aV = theV;
322   for (;;)
323   {
324     BaseD3 (aU, aV, theValue, theD1U, theD1V,
325             theD2U, theD2V, theD2UV, theD3U, theD3V, theD3UUV, theD3UVV);
326     try
327     {
328       CalculateD3 (aU, aV, theValue, theD1U, theD1V,
329                    theD2U, theD2V, theD2UV, theD3U, theD3V, theD3UUV, theD3UVV);
330       break;
331     } 
332     catch (Geom_UndefinedValue&)
333     {
334       // if failed at parametric boundary, try taking derivative at shifted point
335       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, theD1U, theD1V))
336       {
337         throw;
338       }
339     }
340   }
341 }
342
343 gp_Vec GeomEvaluator_OffsetSurface::DN(
344     const Standard_Real theU, const Standard_Real theV,
345     const Standard_Integer theDerU, const Standard_Integer theDerV) const
346 {
347   Standard_RangeError_Raise_if(theDerU < 0, "GeomEvaluator_OffsetSurface::DN(): theDerU < 0");
348   Standard_RangeError_Raise_if(theDerV < 0, "GeomEvaluator_OffsetSurface::DN(): theDerV < 0");
349   Standard_RangeError_Raise_if(theDerU + theDerV < 1,
350       "GeomEvaluator_OffsetSurface::DN(): theDerU + theDerV < 1");
351
352   Standard_Real aU = theU, aV = theV;
353   for (;;)
354   {
355     gp_Pnt aP;
356     gp_Vec aD1U, aD1V;
357     BaseD1 (aU, aV, aP, aD1U, aD1V);
358     try
359     {
360       return CalculateDN (aU, aV, theDerU, theDerV, aD1U, aD1V);
361     } 
362     catch (Geom_UndefinedValue&)
363     {
364       // if failed at parametric boundary, try taking derivative at shifted point
365       if (! shiftPoint (theU, theV, aU, aV, myBaseSurf, myBaseAdaptor, aD1U, aD1V))
366       {
367         throw;
368       }
369     }
370   }
371 }
372
373
374 void GeomEvaluator_OffsetSurface::BaseD0(const Standard_Real theU, const Standard_Real theV,
375                                          gp_Pnt& theValue) const
376 {
377   if (!myBaseAdaptor.IsNull())
378     myBaseAdaptor->D0(theU, theV, theValue);
379   else
380     myBaseSurf->D0(theU, theV, theValue);
381 }
382
383 void GeomEvaluator_OffsetSurface::BaseD1(const Standard_Real theU, const Standard_Real theV,
384                                          gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V) const
385 {
386   if (!myBaseAdaptor.IsNull())
387     myBaseAdaptor->D1(theU, theV, theValue, theD1U, theD1V);
388   else
389     myBaseSurf->D1(theU, theV, theValue, theD1U, theD1V);
390 }
391
392 void GeomEvaluator_OffsetSurface::BaseD2(const Standard_Real theU, const Standard_Real theV,
393                                          gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
394                                          gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV) const
395 {
396   if (!myBaseAdaptor.IsNull())
397     myBaseAdaptor->D2(theU, theV, theValue, theD1U, theD1V, theD2U, theD2V, theD2UV);
398   else
399     myBaseSurf->D2(theU, theV, theValue, theD1U, theD1V, theD2U, theD2V, theD2UV);
400 }
401
402 void GeomEvaluator_OffsetSurface::BaseD3(
403     const Standard_Real theU, const Standard_Real theV,
404     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
405     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV,
406     gp_Vec& theD3U, gp_Vec& theD3V, gp_Vec& theD3UUV, gp_Vec& theD3UVV) const
407 {
408   if (!myBaseAdaptor.IsNull())
409     myBaseAdaptor->D3(theU, theV, theValue, theD1U, theD1V,
410     theD2U, theD2V, theD2UV, theD3U, theD3V, theD3UUV, theD3UVV);
411   else
412     myBaseSurf->D3(theU, theV, theValue, theD1U, theD1V,
413     theD2U, theD2V, theD2UV, theD3U, theD3V, theD3UUV, theD3UVV);
414 }
415
416
417 void GeomEvaluator_OffsetSurface::CalculateD0(
418     const Standard_Real theU, const Standard_Real theV,
419     gp_Pnt& theValue,
420     const gp_Vec& theD1U, const gp_Vec& theD1V) const
421 {
422   // Normalize derivatives before normal calculation because it gives more stable result.
423   // There will be normalized only derivatives greater than 1.0 to avoid differences in last significant digit
424   gp_Vec aD1U(theD1U);
425   gp_Vec aD1V(theD1V);
426   Standard_Real aD1UNorm2 = aD1U.SquareMagnitude();
427   Standard_Real aD1VNorm2 = aD1V.SquareMagnitude();
428   if (aD1UNorm2 > 1.0)
429     aD1U /= Sqrt(aD1UNorm2);
430   if (aD1VNorm2 > 1.0)
431     aD1V /= Sqrt(aD1VNorm2);
432
433   gp_Vec aNorm = aD1U.Crossed(aD1V);
434   if (aNorm.SquareMagnitude() > the_D1MagTol * the_D1MagTol)
435   {
436     // Non singular case. Simple computations.
437     aNorm.Normalize();
438     theValue.SetXYZ(theValue.XYZ() + myOffset * aNorm.XYZ());
439   }
440   else
441   {
442     const Standard_Integer MaxOrder = 3;
443
444     Handle(Geom_BSplineSurface) L;
445     Standard_Boolean isOpposite = Standard_False;
446     Standard_Boolean AlongU = Standard_False;
447     Standard_Boolean AlongV = Standard_False;
448     if (!myOscSurf.IsNull())
449     {
450       AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
451       AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
452     }
453     const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
454
455     TColgp_Array2OfVec DerNUV(0, MaxOrder, 0, MaxOrder);
456     TColgp_Array2OfVec DerSurf(0, MaxOrder + 1, 0, MaxOrder + 1);
457     Standard_Integer OrderU, OrderV;
458     Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
459     Bounds(Umin, Umax, Vmin, Vmax);
460
461     DerSurf.SetValue(1, 0, theD1U);
462     DerSurf.SetValue(0, 1, theD1V);
463     if (!myBaseSurf.IsNull())
464       derivatives(MaxOrder, 1, theU, theV, myBaseSurf, 0, 0, AlongU, AlongV, L, DerNUV, DerSurf);
465     else
466       derivatives(MaxOrder, 1, theU, theV, myBaseAdaptor, 0, 0, AlongU, AlongV, L, DerNUV, DerSurf);
467
468     gp_Dir Normal;
469     CSLib_NormalStatus NStatus;
470     CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
471                   NStatus, Normal, OrderU, OrderV);
472     if (NStatus == CSLib_InfinityOfSolutions)
473     {
474       // Replace zero derivative and try to calculate normal
475       gp_Vec aNewDU = theD1U;
476       gp_Vec aNewDV = theD1V;
477       if (ReplaceDerivative(theU, theV, aNewDU, aNewDV, the_D1MagTol * the_D1MagTol))
478         CSLib::Normal(aNewDU, aNewDV, the_D1MagTol, NStatus, Normal);
479     }
480
481     if (NStatus != CSLib_Defined)
482       throw Geom_UndefinedValue(
483           "GeomEvaluator_OffsetSurface::CalculateD0(): Unable to calculate normal");
484
485     theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * Normal.XYZ());
486   }
487 }
488
489 void GeomEvaluator_OffsetSurface::CalculateD1(
490     const Standard_Real theU, const Standard_Real theV,
491     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
492     const gp_Vec& theD2U, const gp_Vec& theD2V, const gp_Vec& theD2UV) const
493 {
494   // Check offset side.
495   Handle(Geom_BSplineSurface) L;
496   Standard_Boolean isOpposite = Standard_False;
497   Standard_Boolean AlongU = Standard_False;
498   Standard_Boolean AlongV = Standard_False;
499
500   // Normalize derivatives before normal calculation because it gives more stable result.
501   // There will be normalized only derivatives greater than 1.0 to avoid differences in last significant digit
502   gp_Vec aD1U(theD1U);
503   gp_Vec aD1V(theD1V);
504   Standard_Real aD1UNorm2 = aD1U.SquareMagnitude();
505   Standard_Real aD1VNorm2 = aD1V.SquareMagnitude();
506   if (aD1UNorm2 > 1.0)
507     aD1U /= Sqrt(aD1UNorm2);
508   if (aD1VNorm2 > 1.0)
509     aD1V /= Sqrt(aD1VNorm2);
510
511   Standard_Boolean isSingular = Standard_False;
512   Standard_Integer MaxOrder = 0;
513   gp_Vec aNorm = aD1U.Crossed(aD1V);
514   if (aNorm.SquareMagnitude() <= the_D1MagTol * the_D1MagTol)
515   {
516     if (!myOscSurf.IsNull())
517     {
518       AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
519       AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
520     }
521
522     MaxOrder = 3;
523     isSingular = Standard_True;
524   }
525
526   const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
527   
528   if (!isSingular)
529   {
530     // AlongU or AlongV leads to more complex D1 computation
531     // Try to compute D0 and D1 much simpler
532     aNorm.Normalize();
533     theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * aNorm.XYZ());
534
535     gp_Vec aN0(aNorm.XYZ()), aN1U, aN1V;
536     Standard_Real aScale = (theD1U^theD1V).Dot(aN0);
537     aN1U.SetX(theD2U.Y() * theD1V.Z() + theD1U.Y() * theD2UV.Z()
538       - theD2U.Z() * theD1V.Y() - theD1U.Z() * theD2UV.Y());
539     aN1U.SetY((theD2U.X() * theD1V.Z() + theD1U.X() * theD2UV.Z()
540       - theD2U.Z() * theD1V.X() - theD1U.Z() * theD2UV.X()) * -1.0);
541     aN1U.SetZ(theD2U.X() * theD1V.Y() + theD1U.X() * theD2UV.Y()
542       - theD2U.Y() * theD1V.X() - theD1U.Y() * theD2UV.X());
543     Standard_Real aScaleU = aN1U.Dot(aN0);
544     aN1U.Subtract(aScaleU * aN0);
545     aN1U /= aScale;
546
547     aN1V.SetX(theD2UV.Y() * theD1V.Z() + theD2V.Z() * theD1U.Y()
548       - theD2UV.Z() * theD1V.Y() - theD2V.Y() * theD1U.Z());
549     aN1V.SetY((theD2UV.X() * theD1V.Z() + theD2V.Z() * theD1U.X()
550       - theD2UV.Z() * theD1V.X() - theD2V.X() * theD1U.Z()) * -1.0);
551     aN1V.SetZ(theD2UV.X() * theD1V.Y() + theD2V.Y() * theD1U.X()
552       - theD2UV.Y() * theD1V.X() - theD2V.X() * theD1U.Y());
553     Standard_Real aScaleV = aN1V.Dot(aN0);
554     aN1V.Subtract(aScaleV * aN0);
555     aN1V /= aScale;
556
557     theD1U += myOffset * aSign * aN1U;
558     theD1V += myOffset * aSign * aN1V;
559
560     return;
561   }
562   
563   Standard_Integer OrderU, OrderV;
564   TColgp_Array2OfVec DerNUV(0, MaxOrder + 1, 0, MaxOrder + 1);
565   TColgp_Array2OfVec DerSurf(0, MaxOrder + 2, 0, MaxOrder + 2);
566   Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
567   Bounds(Umin, Umax, Vmin, Vmax);
568
569   DerSurf.SetValue(1, 0, theD1U);
570   DerSurf.SetValue(0, 1, theD1V);
571   DerSurf.SetValue(1, 1, theD2UV);
572   DerSurf.SetValue(2, 0, theD2U);
573   DerSurf.SetValue(0, 2, theD2V);
574   if (!myBaseSurf.IsNull())
575     derivatives(MaxOrder, 2, theU, theV, myBaseSurf, 1, 1, AlongU, AlongV, L, DerNUV, DerSurf);
576   else
577     derivatives(MaxOrder, 2, theU, theV, myBaseAdaptor, 1, 1, AlongU, AlongV, L, DerNUV, DerSurf);
578
579   gp_Dir Normal;
580   CSLib_NormalStatus NStatus;
581   CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax, NStatus, Normal, OrderU, OrderV);
582   if (NStatus == CSLib_InfinityOfSolutions)
583   {
584     gp_Vec aNewDU = theD1U;
585     gp_Vec aNewDV = theD1V;
586     // Replace zero derivative and try to calculate normal
587     if (ReplaceDerivative(theU, theV, aNewDU, aNewDV, the_D1MagTol * the_D1MagTol))
588     {
589       DerSurf.SetValue(1, 0, aNewDU);
590       DerSurf.SetValue(0, 1, aNewDV);
591       if (!myBaseSurf.IsNull())
592         derivatives(MaxOrder, 2, theU, theV, myBaseSurf, 1, 1, AlongU, AlongV, L, DerNUV, DerSurf);
593       else
594         derivatives(MaxOrder, 2, theU, theV, myBaseAdaptor, 1, 1, AlongU, AlongV, L, DerNUV, DerSurf);
595       CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
596                     NStatus, Normal, OrderU, OrderV);
597     }
598   }
599
600   if (NStatus != CSLib_Defined)
601     throw Geom_UndefinedValue(
602         "GeomEvaluator_OffsetSurface::CalculateD1(): Unable to calculate normal");
603
604   theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * Normal.XYZ());
605
606   theD1U = DerSurf(1, 0) + myOffset * aSign * CSLib::DNNormal(1, 0, DerNUV, OrderU, OrderV);
607   theD1V = DerSurf(0, 1) + myOffset * aSign * CSLib::DNNormal(0, 1, DerNUV, OrderU, OrderV);
608 }
609
610 void GeomEvaluator_OffsetSurface::CalculateD2(
611     const Standard_Real theU, const Standard_Real theV,
612     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
613     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV,
614     const gp_Vec& theD3U, const gp_Vec& theD3V, const gp_Vec& theD3UUV, const gp_Vec& theD3UVV) const
615 {
616   gp_Dir Normal;
617   CSLib_NormalStatus NStatus;
618   CSLib::Normal(theD1U, theD1V, the_D1MagTol, NStatus, Normal);
619
620   const Standard_Integer MaxOrder = (NStatus == CSLib_Defined) ? 0 : 3;
621   Standard_Integer OrderU, OrderV;
622   TColgp_Array2OfVec DerNUV(0, MaxOrder + 2, 0, MaxOrder + 2);
623   TColgp_Array2OfVec DerSurf(0, MaxOrder + 3, 0, MaxOrder + 3);
624
625   Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
626   Bounds(Umin, Umax, Vmin, Vmax);
627
628   DerSurf.SetValue(1, 0, theD1U);
629   DerSurf.SetValue(0, 1, theD1V);
630   DerSurf.SetValue(1, 1, theD2UV);
631   DerSurf.SetValue(2, 0, theD2U);
632   DerSurf.SetValue(0, 2, theD2V);
633   DerSurf.SetValue(3, 0, theD3U);
634   DerSurf.SetValue(2, 1, theD3UUV);
635   DerSurf.SetValue(1, 2, theD3UVV);
636   DerSurf.SetValue(0, 3, theD3V);
637   //*********************
638
639   Handle(Geom_BSplineSurface) L;
640   Standard_Boolean isOpposite = Standard_False;
641   Standard_Boolean AlongU = Standard_False;
642   Standard_Boolean AlongV = Standard_False;
643   if ((NStatus != CSLib_Defined) && !myOscSurf.IsNull())
644   {
645     AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
646     AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
647   }
648   const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
649
650   if (!myBaseSurf.IsNull())
651     derivatives(MaxOrder, 3, theU, theV, myBaseSurf, 2, 2, AlongU, AlongV, L, DerNUV, DerSurf);
652   else
653     derivatives(MaxOrder, 3, theU, theV, myBaseAdaptor, 2, 2, AlongU, AlongV, L, DerNUV, DerSurf);
654
655   CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
656                 NStatus, Normal, OrderU, OrderV);
657   if (NStatus != CSLib_Defined)
658     throw Geom_UndefinedValue(
659         "GeomEvaluator_OffsetSurface::CalculateD2(): Unable to calculate normal");
660
661   theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * Normal.XYZ());
662
663   theD1U = DerSurf(1, 0) + myOffset * aSign * CSLib::DNNormal(1, 0, DerNUV, OrderU, OrderV);
664   theD1V = DerSurf(0, 1) + myOffset * aSign * CSLib::DNNormal(0, 1, DerNUV, OrderU, OrderV);
665
666   if (!myBaseSurf.IsNull())
667   {
668     theD2U = myBaseSurf->DN(theU, theV, 2, 0);
669     theD2V = myBaseSurf->DN(theU, theV, 0, 2);
670     theD2UV = myBaseSurf->DN(theU, theV, 1, 1);
671   }
672   else
673   {
674     theD2U = myBaseAdaptor->DN(theU, theV, 2, 0);
675     theD2V = myBaseAdaptor->DN(theU, theV, 0, 2);
676     theD2UV = myBaseAdaptor->DN(theU, theV, 1, 1);
677   }
678
679   theD2U += aSign * myOffset * CSLib::DNNormal(2, 0, DerNUV, OrderU, OrderV);
680   theD2V += aSign * myOffset * CSLib::DNNormal(0, 2, DerNUV, OrderU, OrderV);
681   theD2UV += aSign * myOffset * CSLib::DNNormal(1, 1, DerNUV, OrderU, OrderV);
682 }
683
684 void GeomEvaluator_OffsetSurface::CalculateD3(
685     const Standard_Real theU, const Standard_Real theV,
686     gp_Pnt& theValue, gp_Vec& theD1U, gp_Vec& theD1V,
687     gp_Vec& theD2U, gp_Vec& theD2V, gp_Vec& theD2UV,
688     gp_Vec& theD3U, gp_Vec& theD3V, gp_Vec& theD3UUV, gp_Vec& theD3UVV) const
689 {
690   gp_Dir Normal;
691   CSLib_NormalStatus NStatus;
692   CSLib::Normal(theD1U, theD1V, the_D1MagTol, NStatus, Normal);
693   const Standard_Integer MaxOrder = (NStatus == CSLib_Defined) ? 0 : 3;
694   Standard_Integer OrderU, OrderV;
695   TColgp_Array2OfVec DerNUV(0, MaxOrder + 3, 0, MaxOrder + 3);
696   TColgp_Array2OfVec DerSurf(0, MaxOrder + 4, 0, MaxOrder + 4);
697   Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
698   Bounds(Umin, Umax, Vmin, Vmax);
699
700   DerSurf.SetValue(1, 0, theD1U);
701   DerSurf.SetValue(0, 1, theD1V);
702   DerSurf.SetValue(1, 1, theD2UV);
703   DerSurf.SetValue(2, 0, theD2U);
704   DerSurf.SetValue(0, 2, theD2V);
705   DerSurf.SetValue(3, 0, theD3U);
706   DerSurf.SetValue(2, 1, theD3UUV);
707   DerSurf.SetValue(1, 2, theD3UVV);
708   DerSurf.SetValue(0, 3, theD3V);
709
710
711   //*********************
712   Handle(Geom_BSplineSurface) L;
713   Standard_Boolean isOpposite = Standard_False;
714   Standard_Boolean AlongU = Standard_False;
715   Standard_Boolean AlongV = Standard_False;
716   if ((NStatus != CSLib_Defined) && !myOscSurf.IsNull())
717   {
718     AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
719     AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
720   }
721   const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
722
723   if (!myBaseSurf.IsNull())
724     derivatives(MaxOrder, 3, theU, theV, myBaseSurf, 3, 3, AlongU, AlongV, L, DerNUV, DerSurf);
725   else
726     derivatives(MaxOrder, 3, theU, theV, myBaseAdaptor, 3, 3, AlongU, AlongV, L, DerNUV, DerSurf);
727
728   CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
729                 NStatus, Normal, OrderU, OrderV);
730   if (NStatus != CSLib_Defined)
731     throw Geom_UndefinedValue(
732         "GeomEvaluator_OffsetSurface::CalculateD3(): Unable to calculate normal");
733
734   theValue.SetXYZ(theValue.XYZ() + myOffset * aSign * Normal.XYZ());
735
736   theD1U = DerSurf(1, 0) + myOffset * aSign * CSLib::DNNormal(1, 0, DerNUV, OrderU, OrderV);
737   theD1V = DerSurf(0, 1) + myOffset * aSign * CSLib::DNNormal(0, 1, DerNUV, OrderU, OrderV);
738
739   if (!myBaseSurf.IsNull())
740   {
741     theD2U   = myBaseSurf->DN(theU, theV, 2, 0);
742     theD2V   = myBaseSurf->DN(theU, theV, 0, 2);
743     theD2UV  = myBaseSurf->DN(theU, theV, 1, 1);
744     theD3U   = myBaseSurf->DN(theU, theV, 3, 0);
745     theD3V   = myBaseSurf->DN(theU, theV, 0, 3);
746     theD3UUV = myBaseSurf->DN(theU, theV, 2, 1);
747     theD3UVV = myBaseSurf->DN(theU, theV, 1, 2);
748   }
749   else
750   {
751     theD2U   = myBaseAdaptor->DN(theU, theV, 2, 0);
752     theD2V   = myBaseAdaptor->DN(theU, theV, 0, 2);
753     theD2UV  = myBaseAdaptor->DN(theU, theV, 1, 1);
754     theD3U   = myBaseAdaptor->DN(theU, theV, 3, 0);
755     theD3V   = myBaseAdaptor->DN(theU, theV, 0, 3);
756     theD3UUV = myBaseAdaptor->DN(theU, theV, 2, 1);
757     theD3UVV = myBaseAdaptor->DN(theU, theV, 1, 2);
758   }
759
760   theD2U   += aSign * myOffset * CSLib::DNNormal(2, 0, DerNUV, OrderU, OrderV);
761   theD2V   += aSign * myOffset * CSLib::DNNormal(0, 2, DerNUV, OrderU, OrderV);
762   theD2UV  += aSign * myOffset * CSLib::DNNormal(1, 1, DerNUV, OrderU, OrderV);
763   theD3U   += aSign * myOffset * CSLib::DNNormal(3, 0, DerNUV, OrderU, OrderV);
764   theD3V   += aSign * myOffset * CSLib::DNNormal(0, 3, DerNUV, OrderU, OrderV);
765   theD3UUV += aSign * myOffset * CSLib::DNNormal(2, 1, DerNUV, OrderU, OrderV);
766   theD3UVV += aSign * myOffset * CSLib::DNNormal(1, 2, DerNUV, OrderU, OrderV);
767 }
768
769 gp_Vec GeomEvaluator_OffsetSurface::CalculateDN(
770     const Standard_Real theU, const Standard_Real theV,
771     const Standard_Integer theNu, const Standard_Integer theNv,
772     const gp_Vec& theD1U, const gp_Vec& theD1V) const
773 {
774   gp_Dir Normal;
775   CSLib_NormalStatus NStatus;
776   CSLib::Normal(theD1U, theD1V, the_D1MagTol, NStatus, Normal);
777   const Standard_Integer MaxOrder = (NStatus == CSLib_Defined) ? 0 : 3;
778   Standard_Integer OrderU, OrderV;
779   TColgp_Array2OfVec DerNUV(0, MaxOrder + theNu, 0, MaxOrder + theNu);
780   TColgp_Array2OfVec DerSurf(0, MaxOrder + theNu + 1, 0, MaxOrder + theNv + 1);
781
782   Standard_Real Umin = 0, Umax = 0, Vmin = 0, Vmax = 0;
783   Bounds(Umin, Umax, Vmin, Vmax);
784
785   DerSurf.SetValue(1, 0, theD1U);
786   DerSurf.SetValue(0, 1, theD1V);
787
788   //*********************
789   Handle(Geom_BSplineSurface) L;
790   //   Is there any osculatingsurface along U or V;
791   Standard_Boolean isOpposite = Standard_False;
792   Standard_Boolean AlongU = Standard_False;
793   Standard_Boolean AlongV = Standard_False;
794   if ((NStatus != CSLib_Defined) && !myOscSurf.IsNull())
795   {
796     AlongU = myOscSurf->UOscSurf(theU, theV, isOpposite, L);
797     AlongV = myOscSurf->VOscSurf(theU, theV, isOpposite, L);
798   }
799   const Standard_Real aSign = ((AlongV || AlongU) && isOpposite) ? -1. : 1.;
800
801   if (!myBaseSurf.IsNull())
802     derivatives(MaxOrder, 1, theU, theV, myBaseSurf, theNu, theNv, AlongU, AlongV, L, DerNUV, DerSurf);
803   else
804     derivatives(MaxOrder, 1, theU, theV, myBaseAdaptor, theNu, theNv, AlongU, AlongV, L, DerNUV, DerSurf);
805
806   CSLib::Normal(MaxOrder, DerNUV, the_D1MagTol, theU, theV, Umin, Umax, Vmin, Vmax,
807                 NStatus, Normal, OrderU, OrderV);
808   if (NStatus != CSLib_Defined)
809     throw Geom_UndefinedValue(
810         "GeomEvaluator_OffsetSurface::CalculateDN(): Unable to calculate normal");
811
812   gp_Vec D;
813   if (!myBaseSurf.IsNull())
814     D = myBaseSurf->DN(theU, theV, theNu, theNv);
815   else
816     D = myBaseAdaptor->DN(theU, theV, theNu, theNv);
817
818   D += aSign * myOffset * CSLib::DNNormal(theNu, theNv, DerNUV, OrderU, OrderV);
819   return D;
820 }
821
822
823 void GeomEvaluator_OffsetSurface::Bounds(Standard_Real& theUMin, Standard_Real& theUMax,
824                                          Standard_Real& theVMin, Standard_Real& theVMax) const
825 {
826   if (!myBaseSurf.IsNull())
827     myBaseSurf->Bounds(theUMin, theUMax, theVMin, theVMax);
828   else
829   {
830     theUMin = myBaseAdaptor->FirstUParameter();
831     theUMax = myBaseAdaptor->LastUParameter();
832     theVMin = myBaseAdaptor->FirstVParameter();
833     theVMax = myBaseAdaptor->LastVParameter();
834   }
835 }
836
837
838 Standard_Boolean GeomEvaluator_OffsetSurface::ReplaceDerivative(
839     const Standard_Real theU, const Standard_Real theV,
840     gp_Vec& theDU, gp_Vec& theDV,
841     const Standard_Real theSquareTol) const
842 {
843   Standard_Boolean isReplaceDU = theDU.SquareMagnitude() < theSquareTol;
844   Standard_Boolean isReplaceDV = theDV.SquareMagnitude() < theSquareTol;
845   Standard_Boolean isReplaced = Standard_False;
846   if (isReplaceDU ^ isReplaceDV)
847   {
848     Standard_Real aU = theU;
849     Standard_Real aV = theV;
850     Standard_Real aUMin = 0, aUMax = 0, aVMin = 0, aVMax = 0;
851     Bounds(aUMin, aUMax, aVMin, aVMax);
852
853     // Calculate step along non-zero derivative
854     Standard_Real aStep;
855     Handle(Adaptor3d_HSurface) aSurfAdapt;
856     if (!myBaseAdaptor.IsNull())
857       aSurfAdapt = myBaseAdaptor;
858     else
859       aSurfAdapt = new GeomAdaptor_HSurface(myBaseSurf);
860     if (isReplaceDV)
861     {
862       aStep = Precision::Confusion() * theDU.Magnitude();
863       if (aStep > aUMax - aUMin)
864         aStep = (aUMax - aUMin) / 100.;
865     }
866     else
867     {
868       aStep = Precision::Confusion() * theDV.Magnitude();
869       if (aStep > aVMax - aVMin)
870         aStep = (aVMax - aVMin) / 100.;
871     }
872
873     gp_Pnt aP;
874     gp_Vec aDU, aDV;
875     // Step away from currect parametric coordinates and calculate derivatives once again.
876     // Replace zero derivative by the obtained.
877     for (Standard_Real aStepSign = -1.0; aStepSign <= 1.0 && !isReplaced; aStepSign += 2.0)
878     {
879       if (isReplaceDV)
880       {
881         aU = theU + aStepSign * aStep;
882         if (aU < aUMin || aU > aUMax)
883           continue;
884       }
885       else
886       {
887         aV = theV + aStepSign * aStep;
888         if (aV < aVMin || aV > aVMax)
889           continue;
890       }
891
892       BaseD1(aU, aV, aP, aDU, aDV);
893
894       if (isReplaceDU && aDU.SquareMagnitude() > theSquareTol)
895       {
896         theDU = aDU;
897         isReplaced = Standard_True;
898       }
899       if (isReplaceDV && aDV.SquareMagnitude() > theSquareTol)
900       {
901         theDV = aDV;
902         isReplaced = Standard_True;
903       }
904     }
905   }
906   return isReplaced;
907 }