src/GeomFill/GeomFill_CorrectedFrenet.cxx

   1 // Created on: 1997-12-19
   2 // Created by: Roman BORISOV /Philippe MANGIN
   3 // Copyright (c) 1997-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 <Adaptor3d_HCurve.hxx>
  19 #include <Bnd_Box.hxx>
  20 #include <BndLib_Add3dCurve.hxx>
  21 #include <Geom_BezierCurve.hxx>
  22 #include <Geom_BSplineCurve.hxx>
  23 #include <Geom_Plane.hxx>
  24 #include <GeomAbs_CurveType.hxx>
  25 #include <GeomFill_CorrectedFrenet.hxx>
  26 #include <GeomFill_Frenet.hxx>
  27 #include <GeomFill_SnglrFunc.hxx>
  28 #include <GeomFill_TrihedronLaw.hxx>
  29 #include <GeomLib.hxx>
  30 #include <gp_Trsf.hxx>
  31 #include <gp_Vec.hxx>
  32 #include <gp_Vec2d.hxx>
  33 #include <Law_BSpFunc.hxx>
  34 #include <Law_BSpline.hxx>
  35 #include <Law_Composite.hxx>
  36 #include <Law_Constant.hxx>
  37 #include <Law_Function.hxx>
  38 #include <Law_Interpolate.hxx>
  39 #include <Precision.hxx>
  40 #include <Standard_ConstructionError.hxx>
  41 #include <Standard_OutOfRange.hxx>
  42 #include <Standard_Type.hxx>
  43 #include <TColgp_HArray1OfPnt.hxx>
  44 #include <TColStd_HArray1OfReal.hxx>
  45 #include <TColStd_SequenceOfReal.hxx>
  46
  47 #include <stdio.h>
  48 //Patch
  49 #ifdef OCCT_DEBUG
  50 static Standard_Boolean Affich=0;
  51 #endif
  52
  53 #ifdef DRAW
  54 static Standard_Integer CorrNumber = 0;
  55 #include <Draw_Appli.hxx>
  56 #include <DrawTrSurf.hxx>
  57 #include <Draw_Segment2D.hxx>
  58 //#include <Draw.hxx>
  59 #include <TColgp_Array1OfPnt.hxx>
  60 #include <TColStd_Array1OfReal.hxx>
  61 #include <TColStd_HArray1OfInteger.hxx>
  62 #endif
  63
  64 #ifdef DRAW
  65 static void draw(const Handle(Law_Function)& law)
  66 {
  67   Standard_Real Step, u, v, tmin;
  68   Standard_Integer NbInt, i, j, jmax;
  69   NbInt = law->NbIntervals(GeomAbs_C3);
  70   TColStd_Array1OfReal Int(1, NbInt+1);
  71   law->Intervals(Int, GeomAbs_C3);
  72   gp_Pnt2d old;
  73   Handle(Draw_Segment2D) tg2d;
  74
  75   for(i = 1; i <= NbInt; i++){
  76     tmin = Int(i);
  77     Step = (Int(i+1)-Int(i))/4;
  78     if (i == NbInt) jmax = 4;
  79     else jmax = 3;
  80     for (j=1; j<=jmax; j++) {
  81       u =  tmin + (j-1)*Step;
  82       v = law->Value(u);
  83       gp_Pnt2d point2d(u,v);
  84       if ((i>1)||(j>1)) {
  85         tg2d = new Draw_Segment2D(old, point2d,Draw_kaki);
  86         dout << tg2d;
  87       }
  88       old = point2d;
  89     }
  90   }
  91   dout.Flush();
  92 }
  93 #endif
  94
  95
  96 static Standard_Real ComputeTorsion(const Standard_Real Param,
  97                                     const Handle(Adaptor3d_HCurve)& aCurve)
  98 {
  99   Standard_Real Torsion;
 100
 101   gp_Pnt aPoint;
 102   gp_Vec DC1, DC2, DC3;
 103   aCurve->D3(Param, aPoint, DC1, DC2, DC3);
 104   gp_Vec DC1crossDC2 = DC1 ^ DC2;
 105   Standard_Real Norm_DC1crossDC2 = DC1crossDC2.Magnitude();
 106
 107   Standard_Real DC1DC2DC3 = DC1crossDC2 * DC3 ; //mixed product
 108
 109   Standard_Real Tol = gp::Resolution();
 110   Standard_Real SquareNorm_DC1crossDC2 = Norm_DC1crossDC2 * Norm_DC1crossDC2;
 111   if (SquareNorm_DC1crossDC2 <= Tol)
 112     Torsion = 0.;
 113   else
 114     Torsion = DC1DC2DC3 / SquareNorm_DC1crossDC2 ;
 115
 116   return Torsion;
 117 }
 118
 119 //===============================================================
 120 // Function : smoothlaw
 121 // Purpose : to smooth a law : Reduce the number of knots
 122 //===============================================================
 123 static void smoothlaw(Handle(Law_BSpline)& Law,
 124                       const Handle(TColStd_HArray1OfReal)& Points,
 125                       const Handle(TColStd_HArray1OfReal)& Param,
 126                       const Standard_Real Tol)
 127 {
 128   Standard_Real tol, d;
 129   Standard_Integer ii, Nbk;
 130   Standard_Boolean B, Ok;
 131   Handle(Law_BSpline) BS = Law->Copy();
 132
 133   Nbk = BS->NbKnots();
 134   tol = Tol/10;
 135   Ok = Standard_False;
 136
 137   for (ii=Nbk-1; ii>1; ii--) { // Une premiere passe tolerance serres
 138     B = BS->RemoveKnot(ii, 0, tol);
 139     if (B) Ok = Standard_True;
 140   }
 141
 142   if (Ok) { // controle
 143     tol = 0.;
 144     for (ii=1; ii<=Param->Length() && Ok; ii++) {
 145       d = Abs(BS->Value(Param->Value(ii))-Points->Value(ii));
 146       if (d > tol) tol = d;
 147       Ok = (tol <= Tol);
 148     }
 149     if (Ok)
 150       tol = (Tol-tol)/2;
 151     else {
 152 #ifdef OCCT_DEBUG
 153       cout << "smooth law echec" << endl;
 154 #endif
 155       return; // Echec
 156     }
 157   }
 158   else {
 159     tol = Tol/2;
 160   }
 161
 162
 163  if (Ok) Law = BS;
 164
 165   Ok = Standard_False; // Une deuxieme passe tolerance desserre
 166   Nbk = BS->NbKnots();
 167   for (ii=Nbk-1; ii>1; ii--) {
 168     B = BS->RemoveKnot(ii, 0, tol);
 169     if (B) Ok = Standard_True;
 170   }
 171
 172   if (Ok) { // controle
 173     tol = 0.;
 174     for (ii=1; ii<=Param->Length() && Ok; ii++) {
 175       d = Abs(BS->Value(Param->Value(ii))-Points->Value(ii));
 176       if (d > tol) tol = d;
 177       Ok = (tol <= Tol);
 178     }
 179     if (!Ok) {
 180 #ifdef OCCT_DEBUG
 181       cout << "smooth law echec" << endl;
 182 #endif
 183     }
 184   }
 185   if (Ok) Law = BS;
 186
 187 #ifdef OCCT_DEBUG
 188   if (Affich) {
 189     cout << "Knots Law : " << endl;
 190     for (ii=1; ii<=BS->NbKnots(); ii++) {
 191       cout << ii << " : " << BS->Knot(ii) << endl;
 192     }
 193   }
 194 #endif
 195 }
 196
 197 //===============================================================
 198 // Function : FindPlane
 199 // Purpose :
 200 //===============================================================
 201 static Standard_Boolean FindPlane ( const Handle(Adaptor3d_HCurve)& c,
 202                                     Handle( Geom_Plane )& P )
 203 {
 204   Standard_Boolean found = Standard_True;
 205   Handle(TColgp_HArray1OfPnt) TabP;
 206
 207   switch (c->GetType()) {
 208
 209   case GeomAbs_Line:
 210     {
 211       found = Standard_False;
 212     }
 213     break;
 214
 215   case GeomAbs_Circle:
 216     P = new Geom_Plane(gp_Ax3(c->Circle().Position()));
 217     break;
 218
 219   case GeomAbs_Ellipse:
 220     P = new Geom_Plane(gp_Ax3(c->Ellipse().Position()));
 221     break;
 222
 223   case GeomAbs_Hyperbola:
 224     P = new Geom_Plane(gp_Ax3(c->Hyperbola().Position()));
 225     break;
 226
 227   case GeomAbs_Parabola:
 228     P = new Geom_Plane(gp_Ax3(c->Parabola().Position()));
 229     break;
 230
 231   case GeomAbs_BezierCurve:
 232     {
 233       Handle(Geom_BezierCurve) GC = c->Bezier();
 234       Standard_Integer nbp = GC->NbPoles();
 235       if ( nbp < 2)
 236         found = Standard_False;
 237       else if ( nbp == 2) {
 238         found = Standard_False;
 239       }
 240       else {
 241         TabP = new (TColgp_HArray1OfPnt) (1, nbp);
 242         GC->Poles(TabP->ChangeArray1());
 243       }
 244     }
 245     break;
 246
 247   case GeomAbs_BSplineCurve:
 248     {
 249       Handle(Geom_BSplineCurve) GC = c->BSpline();
 250       Standard_Integer nbp = GC->NbPoles();
 251       if ( nbp < 2)
 252         found = Standard_False;
 253       else if ( nbp == 2) {
 254         found = Standard_False;
 255       }
 256       else {
 257         TabP = new (TColgp_HArray1OfPnt) (1, nbp);
 258         GC->Poles(TabP->ChangeArray1());
 259       }
 260     }
 261     break;
 262
 263   default:
 264     { // On utilise un echantillonage
 265       Standard_Integer nbp = 15 + c->NbIntervals(GeomAbs_C3);
 266       Standard_Real f, l, t, inv;
 267       Standard_Integer ii;
 268       f = c->FirstParameter();
 269       l = c->LastParameter();
 270       inv = 1./(nbp-1);
 271       for (ii=1; ii<=nbp; ii++) {
 272         t = ( f*(nbp-ii) + l*(ii-1));
 273         t *= inv;
 274         TabP->SetValue(ii, c->Value(t));
 275       }
 276     }
 277   }
 278
 279   if (! TabP.IsNull()) { // Recherche d'un plan moyen et controle
 280     Standard_Boolean issingular;
 281     gp_Ax2 inertia;
 282     GeomLib::AxeOfInertia(TabP->Array1(), inertia, issingular);
 283     if (issingular) {
 284       found = Standard_False;
 285     }
 286     else {
 287       P = new Geom_Plane(inertia);
 288     }
 289     if (found)
 290       {
 291         //control = Controle(TabP->Array1(), P,  myTolerance);
 292 //      Standard_Boolean isOnPlane;
 293         Standard_Real a,b,c,d, dist;
 294         Standard_Integer ii;
 295         P->Coefficients(a,b,c,d);
 296         for (ii=1; ii<=TabP->Length() && found; ii++) {
 297           const gp_XYZ& xyz = TabP->Value(ii).XYZ();
 298           dist = a*xyz.X() + b*xyz.Y() + c*xyz.Z() + d;
 299           found = (Abs(dist) <= Precision::Confusion());
 300         }
 301         return found;
 302       }
 303   }
 304
 305   return found;
 306 }
 307
 308 //===============================================================
 309 // Function : Constructor
 310 // Purpose :
 311 //===============================================================
 312 GeomFill_CorrectedFrenet::GeomFill_CorrectedFrenet()
 313  : isFrenet(Standard_False)
 314 {
 315   frenet = new GeomFill_Frenet();
 316   myForEvaluation = Standard_False;
 317 }
 318
 319 //===============================================================
 320 // Function : Constructor
 321 // Purpose :
 322 //===============================================================
 323 GeomFill_CorrectedFrenet::GeomFill_CorrectedFrenet(const Standard_Boolean ForEvaluation)
 324  : isFrenet(Standard_False)
 325 {
 326   frenet = new GeomFill_Frenet();
 327   myForEvaluation = ForEvaluation;
 328 }
 329
 330 Handle(GeomFill_TrihedronLaw) GeomFill_CorrectedFrenet::Copy() const
 331 {
 332   Handle(GeomFill_CorrectedFrenet) copy = new (GeomFill_CorrectedFrenet)();
 333   if (!myCurve.IsNull()) copy->SetCurve(myCurve);
 334   return copy;
 335 }
 336
 337  void GeomFill_CorrectedFrenet::SetCurve(const Handle(Adaptor3d_HCurve)& C)
 338 {
 339
 340   GeomFill_TrihedronLaw::SetCurve(C);
 341   if (! C.IsNull()) {
 342     frenet->SetCurve(C);
 343
 344     GeomAbs_CurveType type;
 345     type = C->GetType();
 346     switch  (type) {
 347     case GeomAbs_Circle:
 348     case GeomAbs_Ellipse:
 349     case GeomAbs_Hyperbola:
 350     case GeomAbs_Parabola:
 351     case GeomAbs_Line:
 352       {
 353         // No probleme isFrenet
 354         isFrenet = Standard_True;
 355         break;
 356       }
 357      default :
 358        {
 359          // We have to search singulaties
 360          isFrenet = Standard_True;
 361          Init();
 362        }
 363     }
 364   }
 365 }
 366
 367
 368 //===============================================================
 369 // Function : Init
 370 // Purpose : Compute angle's law
 371 //===============================================================
 372  void GeomFill_CorrectedFrenet::Init()
 373 {
 374   EvolAroundT = new Law_Composite();
 375   Standard_Integer NbI = frenet->NbIntervals(GeomAbs_C0), i;
 376   TColStd_Array1OfReal T(1, NbI + 1);
 377   frenet->Intervals(T, GeomAbs_C0);
 378   Handle(Law_Function) Func;
 379   //OCC78
 380   TColStd_SequenceOfReal SeqPoles, SeqAngle;
 381   TColgp_SequenceOfVec SeqTangent, SeqNormal;
 382
 383   gp_Vec Tangent, Normal, BN;
 384   frenet->D0(myTrimmed->FirstParameter(), Tangent, Normal, BN);
 385   Standard_Integer NbStep;
 386 //  Standard_Real StartAng = 0, AvStep, Step, t;
 387   Standard_Real StartAng = 0, AvStep, Step;
 388
 389 #if DRAW
 390   Standard_Real t;
 391
 392   if (Affich) { // Display the curve C'^C''(t)
 393     GeomFill_SnglrFunc CS(myCurve);
 394     NbStep = 99;
 395     AvStep = (myTrimmed->LastParameter() -
 396               myTrimmed->FirstParameter())/NbStep;
 397     TColgp_Array1OfPnt TabP(1, NbStep+1);
 398
 399     TColStd_Array1OfReal TI(1, NbStep+1);
 400     TColStd_Array1OfInteger M(1,NbStep+1);
 401     M.Init(1);
 402     M(1) = M(NbStep+1) = 2;
 403     for (i=1; i<=NbStep+1; i++) {
 404       t = (myTrimmed->FirstParameter()+ (i-1)*AvStep);
 405       CS.D0(t, TabP(i));
 406       TI(i) = t;
 407     }
 408     char tname[100];
 409     Standard_CString name = tname ;
 410     sprintf(name,"Binorm_%d", ++CorrNumber);
 411     Handle(Geom_BSplineCurve) BS = new
 412       (Geom_BSplineCurve) (TabP, TI, M, 1);
 413 //    DrawTrSurf::Set(&name[0], BS);
 414     DrawTrSurf::Set(name, BS);
 415   }
 416 #endif
 417
 418
 419   NbStep = 10;
 420   AvStep = (myTrimmed->LastParameter() - myTrimmed->FirstParameter())/NbStep;
 421   for(i = 1; i <= NbI; i++) {
 422     NbStep = Max(Standard_Integer((T(i+1) - T(i))/AvStep), 3);
 423     Step = (T(i+1) - T(i))/NbStep;
 424     if(!InitInterval(T(i), T(i+1), Step, StartAng, Tangent, Normal, AT, AN, Func,
 425                      SeqPoles, SeqAngle, SeqTangent, SeqNormal))
 426     {
 427       if(isFrenet)
 428         isFrenet = Standard_False;
 429     }
 430     Handle(Law_Composite)::DownCast(EvolAroundT)->ChangeLaws().Append(Func);
 431   }
 432   if(myTrimmed->IsPeriodic())
 433     Handle(Law_Composite)::DownCast(EvolAroundT)->SetPeriodic();
 434
 435   TLaw = EvolAroundT;
 436   //OCC78
 437   Standard_Integer iEnd = SeqPoles.Length();
 438   HArrPoles = new TColStd_HArray1OfReal(1, iEnd);
 439   HArrAngle = new TColStd_HArray1OfReal(1, iEnd);
 440   HArrTangent = new TColgp_HArray1OfVec(1, iEnd);
 441   HArrNormal = new TColgp_HArray1OfVec(1, iEnd);
 442   for(i = 1; i <= iEnd; i++){
 443     HArrPoles->ChangeValue(i) = SeqPoles(i);
 444     HArrAngle->ChangeValue(i) = SeqAngle(i);
 445     HArrTangent->ChangeValue(i) = SeqTangent(i);
 446     HArrNormal->ChangeValue(i) = SeqNormal(i);
 447   };
 448
 449 #if DRAW
 450   if (Affich) {
 451     draw(EvolAroundT);
 452   }
 453 #endif
 454 }
 455
 456 //===============================================================
 457 // Function : InitInterval
 458 // Purpose : Compute the angle law on a span
 459 //===============================================================
 460  Standard_Boolean GeomFill_CorrectedFrenet::
 461  InitInterval(const Standard_Real First, const Standard_Real Last,
 462               const Standard_Real Step,
 463               Standard_Real& startAng, gp_Vec& prevTangent,
 464               gp_Vec& prevNormal, gp_Vec& aT, gp_Vec& aN,
 465               Handle(Law_Function)& FuncInt,
 466               TColStd_SequenceOfReal& SeqPoles,
 467               TColStd_SequenceOfReal& SeqAngle,
 468               TColgp_SequenceOfVec& SeqTangent,
 469               TColgp_SequenceOfVec& SeqNormal) const
 470 {
 471   Bnd_Box Boite;
 472   gp_Vec Tangent, Normal, BN, cross;
 473   TColStd_SequenceOfReal parameters;
 474   TColStd_SequenceOfReal EvolAT;
 475   Standard_Real Param = First, L, norm;
 476   Standard_Boolean isZero = Standard_True, isConst = Standard_True;
 477   const Standard_Real minnorm = 1.e-16;
 478   Standard_Integer i;
 479   gp_Pnt PonC;
 480   gp_Vec D1;
 481
 482   frenet->SetInterval(First, Last); //To have the rigth evaluation at bounds
 483   GeomFill_SnglrFunc CS(myCurve);
 484   BndLib_Add3dCurve::Add(CS, First, Last, 1.e-2, Boite);
 485
 486   aT = gp_Vec(0, 0, 0);
 487   aN = gp_Vec(0, 0, 0);
 488
 489   Standard_Real angleAT = 0., currParam, currStep = Step;
 490
 491   Handle( Geom_Plane ) aPlane;
 492   Standard_Boolean isPlanar = Standard_False;
 493   if (!myForEvaluation)
 494     isPlanar = FindPlane( myCurve, aPlane );
 495
 496   i = 1;
 497   currParam = Param;
 498   Standard_Real DLast = Last - Precision::PConfusion();
 499
 500   while (Param < Last) {
 501     if (currParam > DLast) {
 502       currStep = DLast - Param;
 503       currParam = Last;
 504     }
 505     if (isPlanar)
 506       currParam = Last;
 507
 508     frenet->D0(currParam, Tangent, Normal, BN);
 509     if (prevTangent.Angle(Tangent) < M_PI/3 || i == 1) {
 510       parameters.Append(currParam);
 511       //OCC78
 512       SeqPoles.Append(Param);
 513       SeqAngle.Append(i > 1? EvolAT(i-1) : startAng);
 514       SeqTangent.Append(prevTangent);
 515       SeqNormal.Append(prevNormal);
 516       angleAT = CalcAngleAT(Tangent,Normal,prevTangent,prevNormal);
 517
 518       if(isConst && i > 1)
 519         if(Abs(angleAT) > Precision::PConfusion())
 520           isConst = Standard_False;
 521
 522       angleAT += (i > 1) ? EvolAT(i-1) : startAng;
 523       EvolAT.Append(angleAT);
 524       prevNormal = Normal;
 525
 526       if(isZero)
 527         if(Abs(angleAT) > Precision::PConfusion())
 528           isZero = Standard_False;
 529
 530       aT += Tangent;
 531       cross = Tangent.Crossed(Normal);
 532       aN.SetLinearForm(Sin(angleAT), cross,
 533                        1 - Cos(angleAT), Tangent.Crossed(cross),
 534                        Normal+aN);
 535       prevTangent = Tangent;
 536       Param = currParam;
 537       i++;
 538
 539       //Evaluate the Next step
 540       CS.D1(Param, PonC, D1);
 541
 542       L = PonC.XYZ().Modulus()/2;
 543       norm = D1.Magnitude();
 544       if (norm <= gp::Resolution())
 545       {
 546         //norm = 2.*gp::Resolution();
 547         norm = minnorm;
 548       }
 549       currStep = L / norm;
 550       if (currStep <= gp::Resolution()) //L = 0 => curvature = 0, linear segment
 551         currStep = Step;
 552       if (currStep < Precision::Confusion()) //too small step
 553         currStep = Precision::Confusion();
 554       if  (currStep > Step) //too big step
 555         currStep = Step;//default value
 556     }
 557     else
 558       currStep /= 2; // Step too long !
 559
 560     currParam = Param + currStep;
 561   }
 562
 563   if (! isPlanar)
 564     {
 565       aT /= parameters.Length() - 1;
 566       aN /= parameters.Length() - 1;
 567     }
 568   startAng = angleAT;
 569
 570 // Interpolation
 571   if (isConst || isPlanar) {
 572     FuncInt = new Law_Constant();
 573     Handle(Law_Constant)::DownCast(FuncInt)->Set( angleAT, First, Last );
 574   }
 575
 576   else {
 577     Standard_Integer Length = parameters.Length();
 578     Handle(TColStd_HArray1OfReal) pararr =
 579       new TColStd_HArray1OfReal(1, Length);
 580     Handle(TColStd_HArray1OfReal) angleATarr =
 581       new TColStd_HArray1OfReal(1, Length);
 582
 583
 584     for (i = 1; i <= Length; i++) {
 585       pararr->ChangeValue(i) = parameters(i);
 586       angleATarr->ChangeValue(i) = EvolAT(i);
 587     }
 588
 589 #ifdef OCCT_DEBUG
 590     if (Affich) {
 591       cout<<"NormalEvolution"<<endl;
 592       for (i = 1; i <= Length; i++) {
 593         cout<<"("<<pararr->Value(i)<<", "<<angleATarr->Value(i)<<")" << endl;
 594       }
 595       cout<<endl;
 596     }
 597 #endif
 598
 599     Law_Interpolate lawAT(angleATarr, pararr,
 600                           Standard_False, Precision::PConfusion());
 601     lawAT.Perform();
 602     Handle(Law_BSpline) BS = lawAT.Curve();
 603     smoothlaw(BS, angleATarr, pararr, 0.1);
 604
 605     FuncInt = new Law_BSpFunc(BS, First, Last);
 606   }
 607   return isZero;
 608 }
 609 //===============================================================
 610 // Function : CalcAngleAT (OCC78)
 611 // Purpose : Calculate angle of rotation of trihedron normal and its derivatives relative
 612 //           at any position on his curve
 613 //===============================================================
 614 Standard_Real GeomFill_CorrectedFrenet::CalcAngleAT(const gp_Vec& Tangent, const gp_Vec& Normal,
 615                                                     const gp_Vec& prevTangent, const gp_Vec& prevNormal) const
 616 {
 617   Standard_Real angle;
 618   gp_Vec Normal_rot, cross;
 619   angle = Tangent.Angle(prevTangent);
 620   if (Abs(angle) > Precision::Angular()) {
 621     cross = Tangent.Crossed(prevTangent).Normalized();
 622     Normal_rot = Normal + sin(angle)*cross.Crossed(Normal) +
 623       (1 - cos(angle))*cross.Crossed(cross.Crossed(Normal));
 624   }
 625   else
 626     Normal_rot = Normal;
 627   Standard_Real angleAT = Normal_rot.Angle(prevNormal);
 628   if(angleAT > Precision::Angular() && M_PI - angleAT > Precision::Angular())
 629     if (Normal_rot.Crossed(prevNormal).IsOpposite(prevTangent, Precision::Angular()))
 630       angleAT = -angleAT;
 631   return angleAT;
 632 };
 633 //===============================================================
 634 // Function : ... (OCC78)
 635 // Purpose : This family of functions produce conversion of angle utility
 636 //===============================================================
 637 static Standard_Real corr2PI_PI(Standard_Real Ang){
 638   return Ang = (Ang < M_PI? Ang: Ang-2*M_PI);
 639 };
 640 static Standard_Real diffAng(Standard_Real A, Standard_Real Ao){
 641   Standard_Real dA = (A-Ao) - Floor((A-Ao)/2.0/M_PI)*2.0*M_PI;
 642   return dA = dA >= 0? corr2PI_PI(dA): -corr2PI_PI(-dA);
 643 };
 644 //===============================================================
 645 // Function : CalcAngleAT (OCC78)
 646 // Purpose : Calculate angle of rotation of trihedron normal and its derivatives relative
 647 //           at any position on his curve
 648 //===============================================================
 649 Standard_Real GeomFill_CorrectedFrenet::GetAngleAT(const Standard_Real Param) const{
 650   // Search index of low margin from poles of TLaw by bisection method
 651   Standard_Integer iB = 1, iE = HArrPoles->Length(), iC = (iE+iB)/2;
 652   if(Param == HArrPoles->Value(iB)) return TLaw->Value(Param);
 653   if(Param > HArrPoles->Value(iE)) iC = iE;
 654   if(iC < iE){
 655     while(!(HArrPoles->Value(iC) <= Param && Param <= HArrPoles->Value(iC+1))){
 656       if(HArrPoles->Value(iC) < Param) iB = iC; else iE = iC;
 657       iC = (iE+iB)/2;
 658     };
 659     if(HArrPoles->Value(iC) == Param || Param == HArrPoles->Value(iC+1)) return TLaw->Value(Param);
 660   };
 661   //  Calculate differenciation between apporoximated and local values of AngleAT
 662   Standard_Real AngP = TLaw->Value(Param), AngPo = HArrAngle->Value(iC), dAng = AngP - AngPo;
 663   gp_Vec Tangent, Normal, BN;
 664   frenet->D0(Param, Tangent, Normal, BN);
 665   Standard_Real DAng = CalcAngleAT(Tangent, Normal, HArrTangent->Value(iC), HArrNormal->Value(iC));
 666   Standard_Real DA = diffAng(DAng,dAng);
 667   // The correction (there is core of OCC78 bug)
 668   if(Abs(DA) > M_PI/2.0){
 669     AngP = AngPo + DAng;
 670   };
 671   return AngP;
 672 };
 673 //===============================================================
 674 // Function : D0
 675 // Purpose :
 676 //===============================================================
 677  Standard_Boolean GeomFill_CorrectedFrenet::D0(const Standard_Real Param,
 678                                                gp_Vec& Tangent,
 679                                                gp_Vec& Normal,
 680                                                gp_Vec& BiNormal)
 681 {
 682   frenet->D0(Param, Tangent, Normal, BiNormal);
 683   if (isFrenet) return Standard_True;
 684
 685   Standard_Real angleAT;
 686   //angleAT = TLaw->Value(Param);
 687   angleAT = GetAngleAT(Param); //OCC78
 688
 689 // rotation around Tangent
 690   gp_Vec cross;
 691   cross =  Tangent.Crossed(Normal);
 692   Normal.SetLinearForm(Sin(angleAT), cross,
 693                        (1 - Cos(angleAT)), Tangent.Crossed(cross),
 694                        Normal);
 695   BiNormal = Tangent.Crossed(Normal);
 696
 697   return Standard_True;
 698 }
 699
 700 //===============================================================
 701 // Function : D1
 702 // Purpose :
 703 //===============================================================
 704
 705  Standard_Boolean GeomFill_CorrectedFrenet::D1(const Standard_Real Param,
 706                                                gp_Vec& Tangent,
 707                                                gp_Vec& DTangent,
 708                                                gp_Vec& Normal,
 709                                                gp_Vec& DNormal,
 710                                                gp_Vec& BiNormal,
 711                                                gp_Vec& DBiNormal)
 712 {
 713   frenet->D1(Param, Tangent, DTangent, Normal, DNormal, BiNormal, DBiNormal);
 714   if (isFrenet) return Standard_True;
 715
 716   Standard_Real angleAT, d_angleAT;
 717   Standard_Real sina, cosa;
 718
 719   TLaw->D1(Param, angleAT, d_angleAT);
 720   angleAT = GetAngleAT(Param); //OCC78
 721
 722   gp_Vec cross, dcross, tcross, dtcross, aux;
 723   sina = Sin(angleAT);
 724   cosa = Cos(angleAT);
 725
 726   cross =  Tangent.Crossed(Normal);
 727   dcross.SetLinearForm(1, DTangent.Crossed(Normal),
 728                        Tangent.Crossed(DNormal));
 729
 730   tcross = Tangent.Crossed(cross);
 731   dtcross.SetLinearForm(1, DTangent.Crossed(cross),
 732                         Tangent.Crossed(dcross));
 733
 734   aux.SetLinearForm(sina, dcross,
 735                     cosa*d_angleAT, cross);
 736   aux.SetLinearForm(1 - cosa, dtcross,
 737                     sina*d_angleAT, tcross,
 738                     aux);
 739   DNormal+=aux;
 740
 741   Normal.SetLinearForm( sina, cross,
 742                        (1 - cosa), tcross,
 743                        Normal);
 744
 745   BiNormal = Tangent.Crossed(Normal);
 746
 747   DBiNormal.SetLinearForm(1, DTangent.Crossed(Normal),
 748                           Tangent.Crossed(DNormal));
 749
 750 // for test
 751 /*  gp_Vec FDN, Tf, Nf, BNf;
 752   Standard_Real h;
 753   h = 1.0e-8;
 754   if (Param + h > myTrimmed->LastParameter()) h = -h;
 755   D0(Param + h, Tf, Nf, BNf);
 756   FDN = (Nf - Normal)/h;
 757   cout<<"Param = "<<Param<<endl;
 758   cout<<"DN = ("<<DNormal.X()<<", "<<DNormal.Y()<<", "<<DNormal.Z()<<")"<<endl;
 759   cout<<"FDN = ("<<FDN.X()<<", "<<FDN.Y()<<", "<<FDN.Z()<<")"<<endl;
 760 */
 761
 762   return Standard_True;
 763 }
 764
 765 //===============================================================
 766 // Function : D2
 767 // Purpose :
 768 //===============================================================
 769  Standard_Boolean GeomFill_CorrectedFrenet::D2(const Standard_Real Param,
 770                                                gp_Vec& Tangent,
 771                                                gp_Vec& DTangent,
 772                                                gp_Vec& D2Tangent,
 773                                                gp_Vec& Normal,
 774                                                gp_Vec& DNormal,
 775                                                gp_Vec& D2Normal,
 776                                                gp_Vec& BiNormal,
 777                                                gp_Vec& DBiNormal,
 778                                                gp_Vec& D2BiNormal)
 779 {
 780   frenet->D2(Param, Tangent, DTangent, D2Tangent,
 781              Normal, DNormal, D2Normal,
 782              BiNormal, DBiNormal, D2BiNormal);
 783   if (isFrenet) return Standard_True;
 784
 785   Standard_Real angleAT, d_angleAT, d2_angleAT;
 786   Standard_Real sina, cosa;
 787   TLaw->D2(Param, angleAT, d_angleAT, d2_angleAT);
 788   angleAT = GetAngleAT(Param); //OCC78
 789
 790   gp_Vec cross, dcross, d2cross, tcross, dtcross, d2tcross, aux;
 791   sina = Sin(angleAT);
 792   cosa = Cos(angleAT);
 793   cross =  Tangent.Crossed(Normal);
 794   dcross.SetLinearForm(1, DTangent.Crossed(Normal),
 795                        Tangent.Crossed(DNormal));
 796   d2cross.SetLinearForm(1, D2Tangent.Crossed(Normal),
 797                         2, DTangent.Crossed(DNormal),
 798                         Tangent.Crossed(D2Normal));
 799
 800
 801   tcross = Tangent.Crossed(cross);
 802   dtcross.SetLinearForm(1, DTangent.Crossed(cross),
 803                         Tangent.Crossed(dcross));
 804   d2tcross.SetLinearForm(1, D2Tangent.Crossed(cross),
 805                          2, DTangent.Crossed(dcross),
 806                         Tangent.Crossed(d2cross));
 807
 808
 809   aux.SetLinearForm(sina, d2cross,
 810                     2*cosa*d_angleAT, dcross,
 811                     cosa*d2_angleAT - sina*d_angleAT*d_angleAT, cross);
 812
 813   aux.SetLinearForm(1 - cosa, d2tcross,
 814                     2*sina*d_angleAT, dtcross,
 815                     cosa*d_angleAT*d_angleAT + sina*d2_angleAT, tcross,
 816                     aux);
 817   D2Normal += aux;
 818
 819 /*  D2Normal += sina*(D2Tangent.Crossed(Normal) + 2*DTangent.Crossed(DNormal) + Tangent.Crossed(D2Normal)) +
 820                     2*cosa*d_angleAT*(DTangent.Crossed(Normal) + Tangent.Crossed(DNormal)) +
 821                     (cosa*d2_angleAT - sina*d_angleAT*d_angleAT)*Tangent.Crossed(Normal) +
 822 2*sina*d_angleAT*(DTangent.Crossed(Tangent.Crossed(Normal)) + Tangent.Crossed(DTangent.Crossed(Normal)) + Tangent.Crossed(Tangent.Crossed(DNormal))) +
 823 (1 - cosa)*(D2Tangent.Crossed(Tangent.Crossed(Normal)) + Tangent.Crossed(D2Tangent.Crossed(Normal)) + Tangent.Crossed(Tangent.Crossed(D2Normal)) + 2*DTangent.Crossed(DTangent.Crossed(Normal)) + 2*DTangent.Crossed(Tangent.Crossed(DNormal)) + 2*Tangent.Crossed(DTangent.Crossed(DNormal)))
 824 +
 825 (cosa*d_angleAT*d_angleAT + sina*d2_angleAT)*Tangent.Crossed(Tangent.Crossed(Normal));*/
 826
 827
 828   aux.SetLinearForm(sina, dcross,
 829                     cosa*d_angleAT, cross);
 830   aux.SetLinearForm(1 - cosa, dtcross,
 831                     sina*d_angleAT, tcross,
 832                     aux);
 833   DNormal+=aux;
 834
 835
 836   Normal.SetLinearForm( sina, cross,
 837                        (1 - cosa), tcross,
 838                        Normal);
 839
 840   BiNormal = Tangent.Crossed(Normal);
 841
 842   DBiNormal.SetLinearForm(1, DTangent.Crossed(Normal),
 843                           Tangent.Crossed(DNormal));
 844
 845   D2BiNormal.SetLinearForm(1, D2Tangent.Crossed(Normal),
 846                            2, DTangent.Crossed(DNormal),
 847                            Tangent.Crossed(D2Normal));
 848
 849 // for test
 850 /*  gp_Vec FD2N, FD2T, FD2BN, Tf, DTf, Nf, DNf, BNf, DBNf;
 851   Standard_Real h;
 852   h = 1.0e-8;
 853   if (Param + h > myTrimmed->LastParameter()) h = -h;
 854   D1(Param + h, Tf, DTf, Nf, DNf, BNf, DBNf);
 855   FD2N = (DNf - DNormal)/h;
 856   FD2T = (DTf - DTangent)/h;
 857   FD2BN = (DBNf - DBiNormal)/h;
 858   cout<<"Param = "<<Param<<endl;
 859   cout<<"D2N = ("<<D2Normal.X()<<", "<<D2Normal.Y()<<", "<<D2Normal.Z()<<")"<<endl;
 860   cout<<"FD2N = ("<<FD2N.X()<<", "<<FD2N.Y()<<", "<<FD2N.Z()<<")"<<endl<<endl;
 861   cout<<"D2T = ("<<D2Tangent.X()<<", "<<D2Tangent.Y()<<", "<<D2Tangent.Z()<<")"<<endl;
 862   cout<<"FD2T = ("<<FD2T.X()<<", "<<FD2T.Y()<<", "<<FD2T.Z()<<")"<<endl<<endl;
 863   cout<<"D2BN = ("<<D2BiNormal.X()<<", "<<D2BiNormal.Y()<<", "<<D2BiNormal.Z()<<")"<<endl;
 864   cout<<"FD2BN = ("<<FD2BN.X()<<", "<<FD2BN.Y()<<", "<<FD2BN.Z()<<")"<<endl<<endl;
 865 */
 866 //
 867   return Standard_True;
 868 }
 869
 870 //===============================================================
 871 // Function : NbIntervals
 872 // Purpose :
 873 //===============================================================
 874  Standard_Integer GeomFill_CorrectedFrenet::NbIntervals(const GeomAbs_Shape S) const
 875 {
 876   Standard_Integer NbFrenet, NbLaw;
 877   NbFrenet = frenet->NbIntervals(S);
 878   if (isFrenet) return NbFrenet;
 879
 880   NbLaw = EvolAroundT->NbIntervals(S);
 881   if (NbFrenet == 1)
 882     return  NbLaw;
 883
 884   TColStd_Array1OfReal FrenetInt(1, NbFrenet + 1);
 885   TColStd_Array1OfReal LawInt(1, NbLaw + 1);
 886   TColStd_SequenceOfReal Fusion;
 887
 888   frenet->Intervals(FrenetInt, S);
 889   EvolAroundT->Intervals(LawInt, S);
 890   GeomLib::FuseIntervals(FrenetInt, LawInt, Fusion);
 891
 892   return Fusion.Length()-1;
 893 }
 894
 895 //===============================================================
 896 // Function : Intervals
 897 // Purpose :
 898 //===============================================================
 899  void GeomFill_CorrectedFrenet::Intervals(TColStd_Array1OfReal& T,
 900                                           const GeomAbs_Shape S) const
 901 {
 902   Standard_Integer NbFrenet, NbLaw;
 903   if (isFrenet) {
 904     frenet->Intervals(T, S);
 905     return;
 906   }
 907
 908   NbFrenet = frenet->NbIntervals(S);
 909   if(NbFrenet==1) {
 910     EvolAroundT->Intervals(T, S);
 911   }
 912
 913   NbLaw = EvolAroundT->NbIntervals(S);
 914
 915   TColStd_Array1OfReal FrenetInt(1, NbFrenet + 1);
 916   TColStd_Array1OfReal LawInt(1, NbLaw + 1);
 917   TColStd_SequenceOfReal Fusion;
 918
 919   frenet->Intervals(FrenetInt, S);
 920   EvolAroundT->Intervals(LawInt, S);
 921   GeomLib::FuseIntervals(FrenetInt, LawInt, Fusion);
 922
 923   for(Standard_Integer i = 1; i <= Fusion.Length(); i++)
 924     T.ChangeValue(i) = Fusion.Value(i);
 925 }
 926
 927 //===============================================================
 928 // Function : SetInterval
 929 // Purpose :
 930 //===============================================================
 931  void GeomFill_CorrectedFrenet::SetInterval(const Standard_Real First,
 932                                             const Standard_Real Last)
 933 {
 934  GeomFill_TrihedronLaw::SetInterval(First, Last);
 935  frenet->SetInterval(First, Last);
 936  if (!isFrenet) TLaw =  EvolAroundT->Trim(First, Last,
 937                                           Precision::PConfusion()/2);
 938 }
 939
 940 //===============================================================
 941 // Function : EvaluateBestMode
 942 // Purpose :
 943 //===============================================================
 944 GeomFill_Trihedron GeomFill_CorrectedFrenet::EvaluateBestMode()
 945 {
 946   if (EvolAroundT.IsNull())
 947     return GeomFill_IsFrenet; //Frenet
 948
 949   const Standard_Real MaxAngle = 3.*M_PI/4.;
 950   const Standard_Real MaxTorsion = 100.;
 951
 952   Standard_Real Step, u, v, tmin, tmax;
 953   Standard_Integer NbInt, i, j, k = 1;
 954   NbInt = EvolAroundT->NbIntervals(GeomAbs_CN);
 955   TColStd_Array1OfReal Int(1, NbInt+1);
 956   EvolAroundT->Intervals(Int, GeomAbs_CN);
 957   gp_Pnt2d old;
 958   gp_Vec2d aVec, PrevVec;
 959
 960   Standard_Integer NbSamples = 10;
 961   for(i = 1; i <= NbInt; i++){
 962     tmin = Int(i);
 963     tmax = Int(i+1);
 964     Standard_Real Torsion = ComputeTorsion(tmin, myTrimmed);
 965     if (Abs(Torsion) > MaxTorsion)
 966       return GeomFill_IsDiscreteTrihedron; //DiscreteTrihedron
 967
 968     Handle(Law_Function) trimmedlaw = EvolAroundT->Trim(tmin, tmax, Precision::PConfusion()/2);
 969     Step = (Int(i+1)-Int(i))/NbSamples;
 970     for (j = 0; j <= NbSamples; j++) {
 971       u = tmin + j*Step;
 972       v = trimmedlaw->Value(u);
 973       gp_Pnt2d point2d(u,v);
 974       if (j != 0)
 975       {
 976         aVec.SetXY(point2d.XY() - old.XY());
 977         if (k > 2)
 978         {
 979           Standard_Real theAngle = PrevVec.Angle(aVec);
 980           if (Abs(theAngle) > MaxAngle)
 981             return GeomFill_IsDiscreteTrihedron; //DiscreteTrihedron
 982         }
 983         PrevVec = aVec;
 984       }
 985       old = point2d;
 986       k++;
 987     }
 988   }
 989
 990   return GeomFill_IsCorrectedFrenet; //CorrectedFrenet
 991 }
 992
 993 //===============================================================
 994 // Function : GetAverageLaw
 995 // Purpose :
 996 //===============================================================
 997  void GeomFill_CorrectedFrenet::GetAverageLaw(gp_Vec& ATangent,
 998                                               gp_Vec& ANormal,
 999                                               gp_Vec& ABiNormal)
1000 {
1001   if (isFrenet) frenet->GetAverageLaw(ATangent, ANormal, ABiNormal);
1002   else {
1003     ATangent = AT;
1004     ANormal = AN;
1005     ABiNormal = ATangent;
1006     ABiNormal.Cross(ANormal);
1007   }
1008 }
1009
1010 //===============================================================
1011 // Function : IsConstant
1012 // Purpose :
1013 //===============================================================
1014  Standard_Boolean GeomFill_CorrectedFrenet::IsConstant() const
1015 {
1016  return (myCurve->GetType() == GeomAbs_Line);
1017 }
1018
1019 //===============================================================
1020 // Function : IsOnlyBy3dCurve
1021 // Purpose :
1022 //===============================================================
1023  Standard_Boolean GeomFill_CorrectedFrenet::IsOnlyBy3dCurve() const
1024 {
1025  return Standard_True;
1026 }