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