src/GeomFill/GeomFill_CorrectedFrenet.cxx

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