0025418: Debug output to be limited to OCC development environment
[occt.git] / src / GeomFill / GeomFill_GuideTrihedronPlan.cxx
1 // Created on: 1998-07-02
2 // Created by: Stephanie HUMEAU
3 // Copyright (c) 1998-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_GuideTrihedronPlan.ixx>
18
19 #include <gp_Pnt.hxx>
20 #include <gp_Pnt2d.hxx>
21 //#include <gp_Trsf2d.hxx>
22 //#include <Bnd_Box2d.hxx>
23 #include <ElCLib.hxx>
24
25 #include <Adaptor3d_Curve.hxx>
26 #include <GeomAdaptor_HCurve.hxx>
27 #include <GeomAdaptor_HSurface.hxx>
28
29 #include <Geom_Plane.hxx>
30
31 #include <IntCurveSurface_IntersectionPoint.hxx>
32 #include <IntCurveSurface_HInter.hxx>
33
34 #include <GeomFill_Frenet.hxx>
35 #include <GeomFill_PlanFunc.hxx>
36
37 #include <math_Vector.hxx>
38
39 #include <math_FunctionRoot.hxx>
40 #include <math_Matrix.hxx>
41
42 #include <Precision.hxx>
43
44 #if DRAW
45 #include <DrawTrSurf.hxx>
46 #endif
47
48 #ifdef OCCT_DEBUG
49 static void TracePlan(const Handle(Geom_Surface)& /*Plan*/)
50 {
51   cout << "Pas d'intersection Guide/Plan" << endl;      
52 #if DRAW
53   char* Temp = "ThePlan" ;
54   DrawTrSurf::Set(Temp, Plan);
55 //  DrawTrSurf::Set("ThePlan", Plan);
56 #endif 
57 }
58 #endif
59
60 //==================================================================
61 //Function: InGoodPeriod
62 //Purpose : Recadre un paramtere
63 //==================================================================
64 static void InGoodPeriod(const Standard_Real Prec,
65                          const Standard_Real  Period,
66                          Standard_Real& Current)
67 {
68   Standard_Real Diff=Current-Prec;
69   Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period);
70   Current -= nb*Period;
71   Diff = Current-Prec;
72   if (Diff > Period/2) Current -= Period;
73   else if (Diff < -Period/2) Current += Period;
74 }
75
76 //=======================================================================
77 //function : GuideTrihedronPlan
78 //purpose  : Constructor
79 //=======================================================================
80 GeomFill_GuideTrihedronPlan::GeomFill_GuideTrihedronPlan (const Handle(Adaptor3d_HCurve)& theGuide) :
81                                                           X(1,1),  
82                                                           XTol(1,1),
83                                                           Inf(1,1), Sup(1,1),
84                                                           myStatus(GeomFill_PipeOk)
85 {
86   myCurve.Nullify();
87   myGuide = theGuide; // guide
88   myTrimG = theGuide;
89   myNbPts = 20;     // nb points pour calculs
90   Pole = new (TColgp_HArray2OfPnt2d)(1,1,1,myNbPts);//tab pr stocker Pprime (pt sur guide)
91   frenet = new (GeomFill_Frenet)();
92   XTol.Init(1.e-6);
93   XTol(1) = myGuide->Resolution(1.e-6);
94 }
95
96 //=======================================================================
97 //function : Init
98 //purpose  : calcule myNbPts points sur la courbe guide (<=> normale)
99 //=======================================================================
100  void GeomFill_GuideTrihedronPlan::Init()
101 {
102   myStatus = GeomFill_PipeOk;
103   gp_Pnt P;
104 //  Bnd_Box2d Box;
105 //  Box.Update(-0.1, -0.1, 0.1, 0.1); // Taille minimal
106   gp_Vec Tangent,Normal,BiNormal;
107   Standard_Integer ii;
108   Standard_Real t, DeltaG, w = 0.;
109   Standard_Real f = myCurve->FirstParameter();
110   Standard_Real l = myCurve->LastParameter();
111   
112
113
114   Handle(Geom_Plane) Plan;
115   Handle(GeomAdaptor_HSurface) Pl;
116   IntCurveSurface_IntersectionPoint PInt;
117   IntCurveSurface_HInter Int;
118   frenet->SetCurve(myCurve);
119   DeltaG = (myGuide->LastParameter() -  myGuide->FirstParameter())/2;
120   
121   Inf(1) = myGuide->FirstParameter() - DeltaG;
122   Sup(1) = myGuide->LastParameter() + DeltaG;
123
124   if (!myGuide->IsPeriodic()) {
125     myTrimG = myGuide->Trim(myGuide->FirstParameter()- DeltaG/100, 
126                             myGuide->LastParameter() + DeltaG/100, 
127                             DeltaG*1.e-7);
128   }
129   else {
130     myTrimG = myGuide; 
131   }
132 //  Standard_Real Step = DeltaG/100;
133   DeltaG /= 3;
134   for (ii=1; ii<=myNbPts; ii++) 
135     {
136       t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
137       t /= (myNbPts-1);
138       myCurve->D0(t, P); 
139       frenet->D0(t, Tangent, Normal, BiNormal);
140       Plan = new (Geom_Plane) (P, Tangent);
141       Pl = new(GeomAdaptor_HSurface) (Plan);
142
143       Int.Perform(myTrimG, Pl); // intersection plan / guide 
144       if (Int.NbPoints() == 0) {
145 #ifdef OCCT_DEBUG
146         TracePlan(Plan);
147 #endif
148         w = (fabs(myGuide->LastParameter() -w) > fabs(myGuide->FirstParameter()-w) ? myGuide->FirstParameter() : myGuide->LastParameter());
149                                                                                    
150         myStatus = GeomFill_PlaneNotIntersectGuide;
151         //return;
152       }
153       else 
154         {
155           gp_Pnt Pmin;
156           PInt = Int.Point(1);
157           Pmin = PInt.Pnt();
158           Standard_Real Dmin = P.Distance(Pmin);
159           for (Standard_Integer jj=2;jj<=Int.NbPoints();jj++)
160             {
161               Pmin = Int.Point(jj).Pnt();
162               if (P.Distance(Pmin) < Dmin) 
163                 {
164                   PInt = Int.Point(jj);
165                   Dmin = P.Distance(Pmin);
166                 }
167             }//for_jj
168           
169           w = PInt.W();
170         }
171       if (ii>1) {
172         Standard_Real Diff = w -  Pole->Value(1, ii-1).Y();
173         if (Abs(Diff) > DeltaG) {
174           if (myGuide->IsPeriodic()) {
175             InGoodPeriod (Pole->Value(1, ii-1).Y(),
176                           myGuide->Period(), w);
177             
178             Diff =  w -  Pole->Value(1, ii-1).Y();
179           }
180         }
181         
182 #ifdef OCCT_DEBUG
183         if (Abs(Diff) > DeltaG) {
184           cout << "Trihedron Plan Diff on Guide : " << 
185             Diff << endl;
186         }
187 #endif
188       }
189       
190       gp_Pnt2d p1(t, w); // on stocke les parametres
191       Pole->SetValue(1, ii, p1);
192       
193     }// for_ii
194 }
195
196 //=======================================================================
197 //function : SetCurve
198 //purpose  : calculation of trihedron
199 //=======================================================================
200 void GeomFill_GuideTrihedronPlan::SetCurve(const Handle(Adaptor3d_HCurve)& C)
201 {
202   myCurve = C;
203   if (!myCurve.IsNull()) Init();
204 }
205
206 //=======================================================================
207 //function : Guide
208 //purpose  : calculation of trihedron
209 //=======================================================================
210
211  Handle(Adaptor3d_HCurve) GeomFill_GuideTrihedronPlan::Guide()const
212 {
213   return myGuide;
214 }
215
216 //=======================================================================
217 //function : D0
218 //purpose  : calculation of trihedron
219 //=======================================================================
220  Standard_Boolean GeomFill_GuideTrihedronPlan::D0(const Standard_Real Param,
221                                                   gp_Vec& Tangent,
222                                                   gp_Vec& Normal,
223                                                   gp_Vec& BiNormal) 
224
225   gp_Pnt P, Pprime;
226 //  gp_Vec To;
227
228   myCurve->D0(Param, P); 
229
230   frenet->D0(Param,Tangent,Normal,BiNormal);
231   
232   //initialisation de la recherche
233   InitX(Param);
234       
235   Standard_Integer Iter = 50;
236   
237   // fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
238   GeomFill_PlanFunc E(P, Tangent, myGuide);
239   
240   // resolution
241   math_FunctionRoot Result(E, X(1), XTol(1), 
242                            Inf(1), Sup(1), Iter); 
243   
244   if (Result.IsDone()) 
245     {
246       Standard_Real Res = Result.Root();
247 //      R = Result.Root();    // solution    
248    
249       Pprime = myTrimG->Value(Res); // pt sur courbe guide 
250       gp_Vec n (P, Pprime); // vecteur definissant la normale du triedre 
251             
252       Normal = n.Normalized();
253       BiNormal = Tangent.Crossed(Normal);
254       BiNormal.Normalized();   
255     }
256   else { // Erreur...
257 #ifdef OCCT_DEBUG
258     cout << "D0 :";
259     // plan ortho a la trajectoire pour determiner Pprime
260     Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
261     TracePlan(Plan);
262 #endif
263     myStatus = GeomFill_PlaneNotIntersectGuide;
264     return Standard_False;
265   }
266   
267   return Standard_True;
268 }
269
270 //=======================================================================
271 //function : D1
272 //purpose  : calculation of trihedron and first derivative
273 //=======================================================================
274  Standard_Boolean GeomFill_GuideTrihedronPlan::D1(const Standard_Real Param,
275                                                   gp_Vec& Tangent,
276                                                   gp_Vec& DTangent,
277                                                   gp_Vec& Normal,
278                                                   gp_Vec& DNormal,
279                                                   gp_Vec& BiNormal,        
280                                                   gp_Vec& DBiNormal) 
281
282 //  return Standard_False;
283   gp_Pnt P, PG;
284   gp_Vec To,TG;
285
286
287   
288   // triedre de frenet sur la trajectoire
289   myCurve->D1(Param, P, To);
290   frenet->D1(Param,Tangent,DTangent,Normal,DNormal,BiNormal,DBiNormal);
291
292  
293   // tolerance sur E
294   Standard_Integer Iter = 50;
295
296   // fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
297   InitX(Param);
298   GeomFill_PlanFunc E(P, Tangent, myGuide);
299
300   // resolution
301   math_FunctionRoot Result(E, X(1), XTol(1), 
302                            Inf(1), Sup(1), Iter);
303
304   if (Result.IsDone()) 
305     {
306       Standard_Real Res =  Result.Root();
307 //      R = Result.Root();    // solution  
308       myTrimG->D1(Res, PG, TG);
309       gp_Vec n (P, PG), dn; // vecteur definissant la normale du triedre
310       Standard_Real Norm = n.Magnitude();
311       if (Norm < 1.e-12) {
312         Norm = 1.0;
313       }
314       n /=Norm;
315
316       
317       Normal = n; 
318       BiNormal = Tangent.Crossed(Normal);
319
320 // derivee premiere du triedre
321       Standard_Real dedx, dedt, dtg_dt;
322       E.Derivative(Res, dedx);
323       E.DEDT(Res, To, DTangent, dedt);
324       dtg_dt = -dedt/dedx;
325
326
327 /*      Standard_Real h=1.e-7, e, etg, etc;
328       E.Value(Res, e);
329       E.Value(Res+h, etg);
330       if ( Abs( (etg-e)/h - dedx) > 1.e-4) {
331         cout << "err :" <<  (etg-e)/h - dedx << endl;
332       }
333       gp_Pnt pdbg;
334       gp_Vec td, nb, bnb;
335       myCurve->D0(Param+h, pdbg);      
336       frenet->D0(Param+h,td, nb, bnb);
337
338       GeomFill_PlanFunc Edeb(pdbg, td, myGuide); 
339       Edeb.Value(Res, etc);
340       if ( Abs( (etc-e)/h - dedt) > 1.e-4) {
341         cout << "err :" <<  (etc-e)/h - dedt << endl;
342       } */           
343
344       dn.SetLinearForm(dtg_dt, TG, -1, To);
345       
346       DNormal.SetLinearForm(-(n*dn), n, dn);
347       DNormal /= Norm;
348       DBiNormal.SetLinearForm(Tangent.Crossed(DNormal),
349                               DTangent.Crossed(Normal));
350     }
351   else {// Erreur...
352 #ifdef OCCT_DEBUG
353     cout << "D1 :";
354     // plan ortho a la trajectoire
355     Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
356     TracePlan(Plan);
357 #endif
358     myStatus = GeomFill_PlaneNotIntersectGuide;
359     return Standard_False;
360   }
361  
362   return Standard_True; 
363 }
364
365
366 //=======================================================================
367 //function : D2
368 //purpose  : calculation of trihedron and derivatives
369 //=======================================================================
370  Standard_Boolean GeomFill_GuideTrihedronPlan::D2(const Standard_Real Param,
371                                                   gp_Vec& Tangent,
372                                                   gp_Vec& DTangent,
373                                                   gp_Vec& D2Tangent,
374                                                   gp_Vec& Normal,
375                                                   gp_Vec& DNormal,
376                                                   gp_Vec& D2Normal,
377                                                   gp_Vec& BiNormal,                       
378                                                   gp_Vec& DBiNormal,              
379                                                   gp_Vec& D2BiNormal) 
380
381 //  gp_Pnt P, PG;
382   gp_Pnt P;
383 //  gp_Vec To,DTo,TG,DTG;
384   gp_Vec To,DTo;
385
386   myCurve->D2(Param, P, To, DTo); 
387
388   // triedre de Frenet sur la trajectoire
389   frenet->D2(Param,Tangent,DTangent,D2Tangent,
390              Normal,DNormal,D2Normal,
391              BiNormal,DBiNormal,D2BiNormal);
392
393 /*
394   // plan ortho a Tangent pour trouver la pt Pprime sur le guide
395   Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent); 
396   Handle(GeomAdaptor_HSurface) Pl= new(GeomAdaptor_HSurface)(Plan);
397   
398
399   Standard_Integer Iter = 50;
400   // fonction dont il faut trouver la racine : G(W) - Pl(U,V)=0
401   GeomFill_FunctionPipe E(Pl , myGuide);
402   InitX(Param);
403   
404   // resolution
405   math_FunctionSetRoot Result(E, X, XTol, 
406                                     Inf, Sup, Iter); 
407   if (Result.IsDone()) 
408     {
409       math_Vector R(1,3); 
410       R = Result.Root();    // solution 
411       myTrimG->D2(R(1), PG, TG, DTG); 
412
413       gp_Vec n (P, PG); // vecteur definissant la normale du triedre
414       Standard_Real Norm = n.Magnitude();
415       n /= Norm;      
416       Normal = n.Normalized(); 
417       BiNormal = Tangent.Crossed(Normal);
418
419  
420
421    // derivee premiere du triedre 
422       Standard_Real dtp_dt;
423       dtp_dt = (To*Tangent - Norm*(n*DTangent))/(Tangent*TG);
424       gp_Vec dn, d2n;
425       dn.SetLinearForm(dtp_dt, TG, -1,  To);
426       
427       DNormal.SetLinearForm(-(n*dn), n, dn);
428       DNormal /= Norm;  
429       DBiNormal = Tangent.Crossed(DNormal) + DTangent.Crossed(Normal);
430   
431     // derivee seconde du triedre
432       Standard_Real d2tp_dt2;
433       d2tp_dt2 = (DTo*Tangent+To*DTangent - dn*DTangent-Norm*n*D2Tangent)/(TG*Tangent)
434         - (To*Tangent-Norm*n*DTangent) * (DTG*dtp_dt*Tangent+TG*DTangent)
435           / ((TG*Tangent)*(TG*Tangent));
436
437
438       d2n.SetLinearForm(dtp_dt*dtp_dt, DTG, d2tp_dt2, TG, -DTo);
439       dn/=Norm;
440       d2n/=Norm;
441
442       D2Normal.SetLinearForm(3*Pow(n*dn,2)- (dn.SquareMagnitude() + n*d2n), n,
443                              -2*(n*dn), dn,
444                              d2n);
445  
446       D2BiNormal.SetLinearForm(1, D2Tangent.Crossed(Normal),
447                                2, DTangent.Crossed(DNormal), 
448                                Tangent.Crossed(D2Normal));
449     }
450   else {// Erreur...
451 #ifdef OCCT_DEBUG
452     cout << "D2 :";
453     TracePlan(Plan);
454 #endif
455     myStatus = GeomFill_PlaneNotIntersectGuide;
456     return Standard_False;
457    }
458 */
459 //  return Standard_True;
460  return Standard_False;
461 }
462
463
464 //=======================================================================
465 //function : Copy
466 //purpose  : 
467 //=======================================================================
468  Handle(GeomFill_TrihedronLaw) GeomFill_GuideTrihedronPlan::Copy() const
469 {
470  Handle(GeomFill_GuideTrihedronPlan) copy = 
471    new (GeomFill_GuideTrihedronPlan) (myGuide);
472  copy->SetCurve(myCurve);
473  return copy;
474 }
475
476 //=======================================================================
477 //function : ErrorStatus
478 //purpose  : 
479 //=======================================================================
480  GeomFill_PipeError GeomFill_GuideTrihedronPlan::ErrorStatus() const
481 {
482   return myStatus;
483 }
484  
485
486 //=======================================================================
487 //function : NbIntervals
488 //purpose  : Version provisoire : Il faut tenir compte du guide
489 //=======================================================================
490  Standard_Integer GeomFill_GuideTrihedronPlan::NbIntervals(const GeomAbs_Shape S)const
491 {
492   Standard_Integer Nb;
493   GeomAbs_Shape tmpS;
494   switch (S) {
495   case GeomAbs_C0: tmpS = GeomAbs_C1; break;
496   case GeomAbs_C1: tmpS = GeomAbs_C2; break;
497   case GeomAbs_C2: tmpS = GeomAbs_C3; break;
498   default: tmpS = GeomAbs_CN;
499   }  
500
501   Nb = myCurve->NbIntervals(tmpS);
502   return Nb;
503 }
504 //======================================================================
505 //function :Intervals
506 //purpose  : 
507 //=======================================================================
508  void GeomFill_GuideTrihedronPlan::Intervals(TColStd_Array1OfReal& TT,
509                                             const GeomAbs_Shape S) const
510 {
511   GeomAbs_Shape tmpS;
512   switch (S) {
513   case GeomAbs_C0: tmpS = GeomAbs_C1; break;
514   case GeomAbs_C1: tmpS = GeomAbs_C2; break;
515   case GeomAbs_C2: tmpS = GeomAbs_C3; break;
516   default: tmpS = GeomAbs_CN;
517   }
518   myCurve->Intervals(TT, tmpS);
519 }
520
521 //======================================================================
522 //function :SetInterval
523 //purpose  : 
524 //=======================================================================
525 void GeomFill_GuideTrihedronPlan::SetInterval(const Standard_Real First,
526                                               const Standard_Real Last) 
527 {
528   myTrimmed = myCurve->Trim(First, Last, Precision::Confusion());  
529 }
530
531
532 //=======================================================================
533 //function : GetAverageLaw
534 //purpose  : 
535 //=======================================================================
536  void GeomFill_GuideTrihedronPlan::GetAverageLaw(gp_Vec& ATangent,
537                                     gp_Vec& ANormal,
538                                     gp_Vec& ABiNormal) 
539 {
540   Standard_Integer ii;
541   Standard_Real t, Delta = (myCurve->LastParameter() - 
542                             myCurve->FirstParameter())/20.001;
543
544   ATangent.SetCoord(0.,0.,0.);
545   ANormal.SetCoord(0.,0.,0.);
546   ABiNormal.SetCoord(0.,0.,0.);
547   gp_Vec T, N, B;
548   
549   for (ii=1; ii<=20; ii++) {
550     t = myCurve->FirstParameter() +(ii-1)*Delta;
551     D0(t, T, N, B);
552     ATangent +=T;
553     ANormal  +=N;
554     ABiNormal+=B;
555   }
556   ATangent  /= 20;
557   ANormal   /= 20;
558   ABiNormal /= 20; 
559 }
560
561 //=======================================================================
562 //function : IsConstant
563 //purpose  : 
564 //=======================================================================
565  Standard_Boolean GeomFill_GuideTrihedronPlan::IsConstant() const
566 {
567   if ((myCurve->GetType() == GeomAbs_Line) &&
568       (myGuide->GetType() == GeomAbs_Line)) {
569     Standard_Real Angle;
570     Angle = myCurve->Line().Angle(myGuide->Line());
571     if ((Angle<1.e-12) || ((2*M_PI-Angle)<1.e-12) )
572       return Standard_True;
573   }
574
575   return Standard_False;
576 }
577
578 //=======================================================================
579 //function : IsOnlyBy3dCurve
580 //purpose  : 
581 //=======================================================================
582  Standard_Boolean GeomFill_GuideTrihedronPlan::IsOnlyBy3dCurve() const
583 {
584   return Standard_False;
585 }
586
587 //=======================================================================
588 //function : Origine
589 //purpose  : Nothing!!
590 //=======================================================================
591 void  GeomFill_GuideTrihedronPlan::Origine(const Standard_Real ,
592                                            const Standard_Real  )
593 {
594 }
595
596 //==================================================================
597 //Function : InitX
598 //Purpose : recherche par interpolation d'une valeur initiale
599 //==================================================================
600 void GeomFill_GuideTrihedronPlan::InitX(const Standard_Real Param)
601 {
602
603   Standard_Integer Ideb = 1, Ifin =  Pole->RowLength(), Idemi;
604   Standard_Real Valeur, t1, t2;
605
606   
607   Valeur = Pole->Value(1, Ideb).X();
608   if (Param == Valeur) {
609     Ifin = Ideb+1; 
610   }
611   
612   Valeur =  Pole->Value(1, Ifin).X();
613   if (Param == Valeur) {
614     Ideb = Ifin-1; 
615   } 
616   
617   while ( Ideb+1 != Ifin) {
618     Idemi = (Ideb+Ifin)/2;
619     Valeur = Pole->Value(1, Idemi).X();
620     if (Valeur < Param) {
621       Ideb = Idemi;
622     }
623     else { 
624       if ( Valeur > Param) { Ifin = Idemi;}
625       else { 
626         Ideb = Idemi;                
627         Ifin = Ideb+1;
628       }
629     }
630   }
631
632   t1 = Pole->Value(1,Ideb).X();
633   t2 = Pole->Value(1,Ifin).X();
634   Standard_Real diff = t2-t1;
635   if (diff > 1.e-7) {
636     Standard_Real b = (Param-t1) / diff,
637     a = (t2-Param) / diff;
638
639     X(1) = Pole->Value(1,Ideb).Coord(2) * a 
640       + Pole->Value(1,Ifin).Coord(2) * b; //param guide 
641   }
642   else {
643     X(1) = (Pole->Value(1, Ideb).Coord(2) + 
644             Pole->Value(1, Ifin).Coord(2)) / 2;
645   }
646   if (myGuide->IsPeriodic()) {
647     X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(), 
648                                   myGuide->LastParameter());
649   }
650 }