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