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