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