f526dd54984396e6666c6e912ab44359ec96a9dc
[occt.git] / src / CSLib / CSLib.cxx
1 // Created on: 1991-09-09
2 // Created by: Michel Chauvat
3 // Copyright (c) 1991-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
5 //
6 // This file is part of Open CASCADE Technology software library.
7 //
8 // This library is free software; you can redistribute it and/or modify it under
9 // the terms of the GNU Lesser General Public License version 2.1 as published
10 // by the Free Software Foundation, with special exception defined in the file
11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
12 // distribution for complete text of the license and disclaimer of any warranty.
13 //
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
16
17
18 #include <CSLib.hxx>
19 #include <CSLib_NormalPolyDef.hxx>
20 #include <gp.hxx>
21 #include <gp_Dir.hxx>
22 #include <gp_Vec.hxx>
23 #include <math_FunctionRoots.hxx>
24 #include <PLib.hxx>
25 #include <Precision.hxx>
26 #include <TColgp_Array2OfVec.hxx>
27 #include <TColStd_Array1OfReal.hxx>
28 #include <TColStd_Array2OfReal.hxx>
29
30 void CSLib::Normal (
31
32 const gp_Vec&        D1U, 
33 const gp_Vec&        D1V,
34 const Standard_Real        SinTol, 
35 CSLib_DerivativeStatus& theStatus,
36 gp_Dir&              Normal
37 ) {
38
39 // Function: Calculation of the normal from tangents by u and by v.
40
41   Standard_Real D1UMag = D1U.SquareMagnitude(); 
42   Standard_Real D1VMag = D1V.SquareMagnitude();
43   gp_Vec D1UvD1V = D1U.Crossed(D1V);
44
45   if (D1UMag <= gp::Resolution() && D1VMag <= gp::Resolution()) {
46      theStatus = CSLib_D1IsNull;
47   }
48   else if (D1UMag <= gp::Resolution())           theStatus = CSLib_D1uIsNull;
49   else if (D1VMag <= gp::Resolution())           theStatus = CSLib_D1vIsNull;
50 //  else if ((D1VMag / D1UMag) <= RealEpsilon())   theStatus = CSLib_D1vD1uRatioIsNull;
51 //  else if ((D1UMag / D1VMag) <= RealEpsilon())   theStatus = CSLib_D1uD1vRatioIsNull;
52   else  {
53     Standard_Real Sin2 = 
54     D1UvD1V.SquareMagnitude() / (D1UMag * D1VMag);
55     
56     if (Sin2 < (SinTol * SinTol))  { theStatus = CSLib_D1uIsParallelD1v; }
57     else { Normal = gp_Dir (D1UvD1V);   theStatus = CSLib_Done; }
58   }
59 }
60
61 void CSLib::Normal (
62
63 const gp_Vec&    D1U,
64 const gp_Vec&    D1V,
65 const gp_Vec&    D2U,
66 const gp_Vec&    D2V, 
67 const gp_Vec&    DUV,
68 const Standard_Real    SinTol,
69 Standard_Boolean&      Done,
70 CSLib_NormalStatus& theStatus,
71 gp_Dir&          Normal
72 ) {
73
74 //  Calculation of an approximate normale in case of a null normal.
75 //  Use limited development of the normal of order 1:
76 //     N(u0+du,v0+dv) = N0 + dN/du(u0,v0) * du + dN/dv(u0,v0) * dv + epsilon
77 //  -> N ~ dN/du + dN/dv.
78
79
80
81   gp_Vec D1Nu = D2U.Crossed (D1V);
82   D1Nu.Add (D1U.Crossed (DUV));
83
84   gp_Vec D1Nv = DUV.Crossed (D1V);
85   D1Nv.Add (D1U.Crossed (D2V));
86
87   Standard_Real LD1Nu = D1Nu.SquareMagnitude();
88   Standard_Real LD1Nv = D1Nv.SquareMagnitude();
89
90
91   if (LD1Nu <= RealEpsilon() && LD1Nv <= RealEpsilon())  { 
92       theStatus = CSLib_D1NIsNull;
93       Done = Standard_False;
94   }
95   else if (LD1Nu < RealEpsilon()) {
96       theStatus = CSLib_D1NuIsNull;
97       Done = Standard_True;
98       Normal = gp_Dir (D1Nv);
99   }
100   else if (LD1Nv < RealEpsilon()) {
101       theStatus = CSLib_D1NvIsNull;
102       Done = Standard_True;
103       Normal = gp_Dir (D1Nu);
104   }
105   else if ((LD1Nv / LD1Nu) <= RealEpsilon()) { 
106       theStatus = CSLib_D1NvNuRatioIsNull;
107       Done = Standard_False;
108   }
109   else if ((LD1Nu / LD1Nv) <= RealEpsilon()) { 
110       theStatus = CSLib_D1NuNvRatioIsNull;
111       Done = Standard_False;
112   }
113   else {
114     gp_Vec D1NCross = D1Nu.Crossed (D1Nv);
115     Standard_Real Sin2 = D1NCross.SquareMagnitude() / (LD1Nu * LD1Nv);
116
117     if (Sin2 < (SinTol * SinTol))  { 
118       theStatus = CSLib_D1NuIsParallelD1Nv;
119       Done = Standard_True;
120       Normal = gp_Dir (D1Nu);
121     }    
122     else { 
123       theStatus = CSLib_InfinityOfSolutions;
124       Done = Standard_False;
125     }
126   }
127
128 }
129 void CSLib::Normal (
130
131 const gp_Vec&        D1U, 
132 const gp_Vec&        D1V,
133 const Standard_Real        MagTol, 
134 CSLib_NormalStatus& theStatus,
135 gp_Dir&              Normal
136 ) {
137 // Function: Calculate the normal from tangents by u and by v.
138
139   Standard_Real D1UMag = D1U.Magnitude(); 
140   Standard_Real D1VMag = D1V.Magnitude();
141   gp_Vec D1UvD1V = D1U.Crossed(D1V);
142   Standard_Real NMag =D1UvD1V .Magnitude();
143
144   if (NMag <= MagTol || D1UMag <= MagTol || D1VMag <= MagTol ) {
145
146      theStatus = CSLib_Singular;
147 //     if (D1UMag <= MagTol || D1VMag <= MagTol && NMag > MagTol) MagTol = 2* NMag;
148 }
149   else
150   {
151     // Firstly normalize tangent vectors D1U and D1V (this method is more stable)
152     gp_Dir aD1U(D1U);
153     gp_Dir aD1V(D1V);
154     Normal = gp_Dir(aD1U.Crossed(aD1V));
155     theStatus = CSLib_Defined;
156   }
157   
158
159 }
160 // Calculate normal vector in singular cases
161 //
162 void CSLib::Normal(const Standard_Integer MaxOrder, 
163                    const TColgp_Array2OfVec& DerNUV, 
164                    const Standard_Real SinTol, 
165                    const Standard_Real U,
166                    const Standard_Real V,
167                    const Standard_Real Umin,
168                    const Standard_Real Umax,
169                    const Standard_Real Vmin,
170                    const Standard_Real Vmax,
171                    CSLib_NormalStatus& theStatus,
172                    gp_Dir& Normal, 
173                    Standard_Integer& OrderU, 
174                    Standard_Integer& OrderV)
175 {
176 //  Standard_Integer i,l,Order=-1;
177   Standard_Integer i=0,Order=-1;
178   Standard_Boolean Trouve=Standard_False;
179 //  theStatus = Singular;
180   Standard_Real Norme;
181   gp_Vec D;
182   //Find k0 such that all derivatives N=dS/du ^ dS/dv are null
183   //till order k0-1
184   while(!Trouve && Order < MaxOrder)
185   {
186     Order++;
187     i=Order;
188     while((i>=0) && (!Trouve))
189     {
190       Standard_Integer j=Order-i;
191       D=DerNUV(i,j);
192       Norme=D.Magnitude();
193       Trouve=(Trouve ||(Norme>=SinTol));
194       i--;
195     }
196   }
197   OrderU=i+1;
198   OrderV=Order-OrderU;
199   //Vko first non null derivative of N : reference
200   if(Trouve)
201   {
202      if(Order == 0) 
203      {
204          theStatus = CSLib_Defined;
205          Normal=D.Normalized();
206      }
207      else
208      {
209       gp_Vec Vk0;  
210       Vk0=DerNUV(OrderU,OrderV);
211       TColStd_Array1OfReal Ratio(0,Order);
212       //Calculate lambda i
213       i=0;
214       Standard_Boolean definie=Standard_False;
215       while(i<=Order && !definie)
216       {
217          if(DerNUV(i,Order-i).Magnitude()<=SinTol) Ratio(i)=0;
218          else
219          {
220             if(DerNUV(i,Order-i).IsParallel(Vk0,1e-6)) 
221             {
222 //            Ratio(i) = DerNUV(i,Order-i).Magnitude() / Vk0.Magnitude();
223 //            if(DerNUV(i,Order-i).IsOpposite(Vk0,1e-6)) Ratio(i)=-Ratio(i);
224                Standard_Real r = DerNUV(i,Order-i).Magnitude() / Vk0.Magnitude();
225                if(DerNUV(i,Order-i).IsOpposite(Vk0,1e-6)) r=-r;
226                Ratio(i)=r;
227           
228             }
229             else
230             {
231               definie=Standard_True;
232 //
233             }
234          }
235          i++;
236       }//end while
237       if(!definie)
238       {  //All lambda i exist
239          Standard_Integer SP;
240          Standard_Real inf,sup;
241          inf = 0.0 - M_PI;
242          sup = 0.0 + M_PI;
243          Standard_Boolean FU,LU,FV,LV;
244
245          //Creation of the domain of definition depending on the position
246          //of a single point (medium, border, corner).
247          FU=(Abs(U-Umin) < Precision::PConfusion());
248          LU=(Abs(U-Umax) < Precision::PConfusion() );
249          FV=(Abs(V-Vmin) < Precision::PConfusion() );
250          LV=(Abs(V-Vmax) < Precision::PConfusion() );
251          if (LU)
252          {
253             inf = M_PI / 2;
254             sup = 3 * inf;
255             if (LV)
256             {
257               inf = M_PI;
258             }
259             if (FV)
260             {
261               sup = M_PI;
262             }
263          }
264          else if (FU)
265          {
266             sup = M_PI / 2;
267             inf = -sup;
268             if (LV)
269             { 
270               sup = 0;
271             }
272             if (FV)
273             {
274               inf = 0;
275             }
276          }
277          else if (LV)
278          {
279             inf = 0.0 - M_PI;
280             sup = 0;
281          }
282          else if (FV)
283          {
284             inf = 0;
285             sup = M_PI;
286          }
287          Standard_Boolean CS=0;
288          Standard_Real Vprec = 0., Vsuiv = 0.;
289          //Creation of the polynom
290          CSLib_NormalPolyDef  Poly(Order,Ratio);
291          //Find zeros of SAPS
292          math_FunctionRoots FindRoots(Poly,inf,sup,200,1e-5,
293                                    Precision::Confusion(),
294                                    Precision::Confusion());
295          //If there are zeros
296          if (FindRoots.IsDone() && FindRoots.NbSolutions() > 0)
297             {
298                //ranking by increasing order of roots of SAPS in Sol0
299
300                TColStd_Array1OfReal Sol0(0,FindRoots.NbSolutions()+1);
301                Sol0(1)=FindRoots.Value(1);
302                Standard_Integer n=1;
303                while(n<=FindRoots.NbSolutions())
304                {
305                   Standard_Real ASOL=FindRoots.Value(n);
306                   Standard_Integer j=n-1;
307                   while((j>=1) && (Sol0(j)> ASOL))
308                   {
309                      Sol0(j+1)=Sol0(j);
310                      j--;
311                   }
312                   Sol0(j+1)=ASOL;
313                   n++;
314                }//end while(n
315                //Add limits of the domains 
316                Sol0(0)=inf;
317                Sol0(FindRoots.NbSolutions()+1)=sup;
318                //Find change of sign of SAPS in comparison with its
319                //values to the left and right of each root
320                Standard_Integer ifirst=0;
321                for (i=0;i<=FindRoots.NbSolutions();i++) 
322                {
323                    if(Abs(Sol0(i+1)-Sol0(i)) > Precision::PConfusion())
324                    {
325                       Poly.Value((Sol0(i)+Sol0(i+1))/2.0,Vsuiv);
326                       if(ifirst == 0) 
327                       {
328                          ifirst=i;
329                          CS=Standard_False;
330                          Vprec=Vsuiv;
331                       }
332                       else 
333                       {
334                          CS=(Vprec*Vsuiv)<0;
335                          Vprec=Vsuiv;
336                       }
337                    }
338                }
339             }
340          else
341             {
342                //SAPS has no root, so forcedly do not change the sign
343                CS=Standard_False;
344                Poly.Value(inf,Vsuiv);
345             }
346          //fin if(MFR.IsDone() && MFR.NbSolutions()>0)
347
348          if(CS)
349          //Polynom changes the sign
350             SP=0;
351          else if(Vsuiv>0)
352                  //Polynom is always positive
353                  SP=1;
354               else
355                  //Polynom is always negative
356                  SP=-1;
357          if(SP==0)
358              theStatus = CSLib_InfinityOfSolutions;
359          else
360          {
361             theStatus = CSLib_Defined;
362             Normal=SP*Vk0.Normalized();
363          }
364        }
365        else 
366        {
367          theStatus = CSLib_Defined;
368          Normal=D.Normalized();
369        }
370     }
371    }
372 }
373 //
374 // Calculate the derivative of the non-normed normal vector
375 //
376 gp_Vec CSLib::DNNUV(const Standard_Integer Nu, 
377                     const Standard_Integer Nv,
378                     const TColgp_Array2OfVec& DerSurf)
379 {
380   Standard_Integer i,j;
381   gp_Vec D(0,0,0),VG,VD,PV;
382   for(i=0;i<=Nu;i++)
383     for(j=0;j<=Nv;j++){
384       VG=DerSurf.Value(i+1,j);
385       VD=DerSurf.Value(Nu-i,Nv+1-j);
386       PV=VG^VD;
387       D=D+PLib::Bin(Nu,i)*PLib::Bin(Nv,j)*PV;
388     }
389   return D;
390 }
391
392 //=======================================================================
393 //function : DNNUV
394 //purpose  : 
395 //=======================================================================
396
397 gp_Vec CSLib::DNNUV(const Standard_Integer Nu,
398                     const Standard_Integer Nv,
399                     const TColgp_Array2OfVec& DerSurf1,
400                     const TColgp_Array2OfVec& DerSurf2) 
401 {
402   Standard_Integer i,j;
403   gp_Vec D(0,0,0),VG,VD,PV;
404   for(i=0;i<=Nu;i++)
405     for(j=0;j<=Nv;j++){
406       VG=DerSurf1.Value(i+1,j);
407       VD=DerSurf2.Value(Nu-i,Nv+1-j);
408       PV=VG^VD;
409       D=D+PLib::Bin(Nu,i)*PLib::Bin(Nv,j)*PV;
410     }
411   return D;
412 }
413
414 //
415 // Calculate the derivatives of the normed normal vector depending on the  derivatives
416 // of the non-normed normal vector
417 //
418 gp_Vec CSLib::DNNormal(const Standard_Integer Nu,
419                        const Standard_Integer Nv,
420                        const TColgp_Array2OfVec& DerNUV,
421                        const Standard_Integer Iduref,
422                        const Standard_Integer Idvref)
423 {
424   const Standard_Integer Kderiv = Nu + Nv;
425   TColgp_Array2OfVec DerVecNor(0,Kderiv,0,Kderiv);
426   TColStd_Array2OfReal TabScal(0,Kderiv,0,Kderiv);
427   TColStd_Array2OfReal TabNorm(0,Kderiv,0,Kderiv);
428   gp_Vec DerNor = (DerNUV.Value (Iduref, Idvref)).Normalized();
429   DerVecNor.SetValue(0,0,DerNor);
430   Standard_Real Dnorm = DerNUV.Value (Iduref, Idvref) * DerVecNor.Value (0, 0);
431   TabNorm.SetValue(0,0,Dnorm);
432   TabScal.SetValue(0,0,0.);
433
434   for (Standard_Integer Mderiv = 1; Mderiv <= Kderiv; Mderiv++)
435   {
436     for (Standard_Integer Pderiv = 0; Pderiv <= Mderiv; Pderiv++)
437     {
438       const Standard_Integer Qderiv = Mderiv - Pderiv;
439       if (Pderiv > Nu || Qderiv > Nv)
440       {
441         continue;
442       }
443
444       //  Compute n . derivee(p,q) of n
445       Standard_Real Scal = 0.;
446       if (Pderiv > Qderiv)
447       {
448         for (Standard_Integer Jderiv = 1; Jderiv <= Qderiv; Jderiv++)
449         {
450           Scal = Scal - PLib::Bin (Qderiv, Jderiv)
451                       * (DerVecNor.Value (0, Jderiv) * DerVecNor.Value (Pderiv, Qderiv - Jderiv));
452         }
453
454         for (Standard_Integer Jderiv = 0; Jderiv < Qderiv; Jderiv++)
455         {
456           Scal = Scal - PLib::Bin (Qderiv, Jderiv)
457                       * (DerVecNor.Value (Pderiv, Jderiv) * DerVecNor.Value (0, Qderiv - Jderiv));
458         }
459
460         for (Standard_Integer Ideriv = 1; Ideriv < Pderiv; Ideriv++)
461         {
462           for (Standard_Integer Jderiv = 0; Jderiv <= Qderiv; Jderiv++)
463           {
464             Scal = Scal - PLib::Bin (Pderiv, Ideriv)
465                         * PLib::Bin (Qderiv, Jderiv)
466                         * (DerVecNor.Value (Ideriv, Jderiv) * DerVecNor.Value (Pderiv - Ideriv, Qderiv - Jderiv));
467           }
468         }
469       }
470       else
471       {
472         for (Standard_Integer Ideriv = 1; Ideriv <= Pderiv; Ideriv++)
473         {
474           Scal = Scal - PLib::Bin (Pderiv, Ideriv)
475                       * DerVecNor.Value (Ideriv, 0)
476                       * DerVecNor.Value (Pderiv - Ideriv, Qderiv);
477         }
478
479         for (Standard_Integer Ideriv = 0; Ideriv < Pderiv; Ideriv++)
480         {
481           Scal = Scal - PLib::Bin (Pderiv, Ideriv)
482                       * DerVecNor.Value (Ideriv, Qderiv)
483                       * DerVecNor.Value (Pderiv - Ideriv, 0);
484         }
485
486         for (Standard_Integer Ideriv = 0; Ideriv <= Pderiv; Ideriv++)
487         {
488           for (Standard_Integer Jderiv = 1; Jderiv < Qderiv; Jderiv++)
489           {
490             Scal = Scal - PLib::Bin (Pderiv, Ideriv)
491                         * PLib::Bin (Qderiv, Jderiv)
492                         * (DerVecNor.Value (Ideriv, Jderiv) * DerVecNor.Value (Pderiv - Ideriv, Qderiv - Jderiv));
493           }
494         }
495       }
496       TabScal.SetValue (Pderiv, Qderiv, Scal/2.);
497
498       // Compute the derivative (n,p) of NUV Length
499       Dnorm = (DerNUV.Value (Pderiv + Iduref, Qderiv + Idvref)) * DerVecNor.Value (0, 0);
500       for (Standard_Integer Jderiv = 0; Jderiv < Qderiv; Jderiv++)
501       {
502         Dnorm = Dnorm - PLib::Bin (Qderiv + Idvref, Jderiv + Idvref)
503                       * TabNorm.Value (Pderiv, Jderiv)
504                       * TabScal.Value (0, Qderiv - Jderiv);
505       }
506
507       for (Standard_Integer Ideriv = 0; Ideriv < Pderiv; Ideriv++)
508       {
509         for (Standard_Integer Jderiv = 0; Jderiv <= Qderiv; Jderiv++)
510         {
511           Dnorm = Dnorm - PLib::Bin (Pderiv + Iduref, Ideriv + Iduref)
512                         * PLib::Bin (Qderiv + Idvref, Jderiv + Idvref)
513                         * TabNorm.Value (Ideriv, Jderiv)
514                         * TabScal.Value (Pderiv - Ideriv, Qderiv - Jderiv);
515         }
516       }
517       TabNorm.SetValue (Pderiv, Qderiv, Dnorm);
518
519       // Compute derivative (p,q) of n
520       DerNor = DerNUV.Value (Pderiv + Iduref, Qderiv + Idvref);
521       for (Standard_Integer Jderiv = 1; Jderiv <= Qderiv; Jderiv++)
522       {
523         DerNor = DerNor - PLib::Bin (Pderiv + Iduref, Iduref)
524                         * PLib::Bin (Qderiv + Idvref, Jderiv + Idvref)
525                         * TabNorm.Value (0, Jderiv)
526                         * DerVecNor.Value (Pderiv, Qderiv - Jderiv);
527       }
528
529       for (Standard_Integer Ideriv = 1; Ideriv <= Pderiv; Ideriv++)
530       {
531         for (Standard_Integer Jderiv = 0; Jderiv <= Qderiv; Jderiv++)
532         {
533           DerNor = DerNor - PLib::Bin (Pderiv + Iduref, Ideriv + Iduref)
534                           * PLib::Bin (Qderiv + Idvref, Jderiv + Idvref)
535                           * TabNorm.Value (Ideriv, Jderiv)
536                           * DerVecNor.Value (Pderiv - Ideriv, Qderiv - Jderiv);
537         }
538       }
539       DerNor = DerNor / PLib::Bin (Pderiv + Iduref, Iduref)
540                       / PLib::Bin (Qderiv + Idvref, Idvref)
541                       / TabNorm.Value (0, 0);
542       DerVecNor.SetValue (Pderiv, Qderiv, DerNor);
543     }
544   }
545   return DerVecNor.Value(Nu,Nv);
546 }