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