src/CSLib/CSLib.cxx

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