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