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