1 // File: GeomFill_GuideTrihedronPlan.cxx
2 // Created: Thu Jul 02 15:39:24 1998
3 // Author: Stephanie HUMEAU
7 #include <GeomFill_GuideTrihedronPlan.ixx>
10 #include <gp_Pnt2d.hxx>
11 //#include <gp_Trsf2d.hxx>
12 //#include <Bnd_Box2d.hxx>
15 #include <Adaptor3d_Curve.hxx>
16 #include <GeomAdaptor_HCurve.hxx>
17 #include <GeomAdaptor_HSurface.hxx>
19 #include <Geom_Plane.hxx>
21 #include <IntCurveSurface_IntersectionPoint.hxx>
22 #include <IntCurveSurface_HInter.hxx>
24 #include <GeomFill_Frenet.hxx>
25 #include <GeomFill_PlanFunc.hxx>
27 #include <math_Vector.hxx>
29 #include <math_FunctionRoot.hxx>
30 #include <math_Matrix.hxx>
32 #include <Precision.hxx>
35 #include <DrawTrSurf.hxx>
39 static void TracePlan(const Handle(Geom_Surface)& Plan)
41 cout << "Pas d'intersection Guide/Plan" << endl;
43 char* Temp = "ThePlan" ;
44 DrawTrSurf::Set(Temp, Plan);
45 // DrawTrSurf::Set("ThePlan", Plan);
50 //==================================================================
51 //Function: InGoodPeriod
52 //Purpose : Recadre un paramtere
53 //==================================================================
54 static void InGoodPeriod(const Standard_Real Prec,
55 const Standard_Real Period,
56 Standard_Real& Current)
58 Standard_Real Diff=Current-Prec;
59 Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period);
62 if (Diff > Period/2) Current -= Period;
63 else if (Diff < -Period/2) Current += Period;
66 //=======================================================================
67 //function : GuideTrihedronPlan
68 //purpose : Constructor
69 //=======================================================================
70 GeomFill_GuideTrihedronPlan::GeomFill_GuideTrihedronPlan (const Handle(Adaptor3d_HCurve)& theGuide) :
74 myStatus(GeomFill_PipeOk)
77 myGuide = theGuide; // guide
79 myNbPts = 20; // nb points pour calculs
80 Pole = new (TColgp_HArray2OfPnt2d)(1,1,1,myNbPts);//tab pr stocker Pprime (pt sur guide)
81 frenet = new (GeomFill_Frenet)();
83 XTol(1) = myGuide->Resolution(1.e-6);
86 //=======================================================================
88 //purpose : calcule myNbPts points sur la courbe guide (<=> normale)
89 //=======================================================================
90 void GeomFill_GuideTrihedronPlan::Init()
92 myStatus = GeomFill_PipeOk;
95 // Box.Update(-0.1, -0.1, 0.1, 0.1); // Taille minimal
96 gp_Vec Tangent,Normal,BiNormal;
98 Standard_Real t, DeltaG, w;
99 Standard_Real f = myCurve->FirstParameter();
100 Standard_Real l = myCurve->LastParameter();
104 Handle(Geom_Plane) Plan;
105 Handle(GeomAdaptor_HSurface) Pl;
106 IntCurveSurface_IntersectionPoint PInt;
107 IntCurveSurface_HInter Int;
108 frenet->SetCurve(myCurve);
109 DeltaG = (myGuide->LastParameter() - myGuide->FirstParameter())/2;
111 Inf(1) = myGuide->FirstParameter() - DeltaG;
112 Sup(1) = myGuide->LastParameter() + DeltaG;
114 if (!myGuide->IsPeriodic()) {
115 myTrimG = myGuide->Trim(myGuide->FirstParameter()- DeltaG/100,
116 myGuide->LastParameter() + DeltaG/100,
122 // Standard_Real Step = DeltaG/100;
124 for (ii=1; ii<=myNbPts; ii++)
126 t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
129 frenet->D0(t, Tangent, Normal, BiNormal);
130 Plan = new (Geom_Plane) (P, Tangent);
131 Pl = new(GeomAdaptor_HSurface) (Plan);
133 Int.Perform(myTrimG, Pl); // intersection plan / guide
134 if (Int.NbPoints() == 0) {
138 w = (fabs(myGuide->LastParameter() -w) > fabs(myGuide->FirstParameter()-w) ? myGuide->FirstParameter() : myGuide->LastParameter());
140 myStatus = GeomFill_PlaneNotIntersectGuide;
148 Standard_Real Dmin = P.Distance(Pmin);
149 for (Standard_Integer jj=2;jj<=Int.NbPoints();jj++)
151 Pmin = Int.Point(jj).Pnt();
152 if (P.Distance(Pmin) < Dmin)
154 PInt = Int.Point(jj);
155 Dmin = P.Distance(Pmin);
162 Standard_Real Diff = w - Pole->Value(1, ii-1).Y();
163 if (Abs(Diff) > DeltaG) {
164 if (myGuide->IsPeriodic()) {
165 InGoodPeriod (Pole->Value(1, ii-1).Y(),
166 myGuide->Period(), w);
168 Diff = w - Pole->Value(1, ii-1).Y();
173 if (Abs(Diff) > DeltaG) {
174 cout << "Trihedron Plan Diff on Guide : " <<
180 gp_Pnt2d p1(t, w); // on stocke les parametres
181 Pole->SetValue(1, ii, p1);
186 //=======================================================================
187 //function : SetCurve
188 //purpose : calculation of trihedron
189 //=======================================================================
190 void GeomFill_GuideTrihedronPlan::SetCurve(const Handle(Adaptor3d_HCurve)& C)
193 if (!myCurve.IsNull()) Init();
196 //=======================================================================
198 //purpose : calculation of trihedron
199 //=======================================================================
201 Handle(Adaptor3d_HCurve) GeomFill_GuideTrihedronPlan::Guide()const
206 //=======================================================================
208 //purpose : calculation of trihedron
209 //=======================================================================
210 Standard_Boolean GeomFill_GuideTrihedronPlan::D0(const Standard_Real Param,
218 myCurve->D0(Param, P);
220 frenet->D0(Param,Tangent,Normal,BiNormal);
222 //initialisation de la recherche
225 Standard_Integer Iter = 50;
227 // fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
228 GeomFill_PlanFunc E(P, Tangent, myGuide);
231 math_FunctionRoot Result(E, X(1), XTol(1),
232 Inf(1), Sup(1), Iter);
236 Standard_Real Res = Result.Root();
237 // R = Result.Root(); // solution
239 Pprime = myTrimG->Value(Res); // pt sur courbe guide
240 gp_Vec n (P, Pprime); // vecteur definissant la normale du triedre
242 Normal = n.Normalized();
243 BiNormal = Tangent.Crossed(Normal);
244 BiNormal.Normalized();
249 // plan ortho a la trajectoire pour determiner Pprime
250 Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
253 myStatus = GeomFill_PlaneNotIntersectGuide;
254 return Standard_False;
257 return Standard_True;
260 //=======================================================================
262 //purpose : calculation of trihedron and first derivative
263 //=======================================================================
264 Standard_Boolean GeomFill_GuideTrihedronPlan::D1(const Standard_Real Param,
272 // return Standard_False;
278 // triedre de frenet sur la trajectoire
279 myCurve->D1(Param, P, To);
280 frenet->D1(Param,Tangent,DTangent,Normal,DNormal,BiNormal,DBiNormal);
284 Standard_Integer Iter = 50;
286 // fonction dont il faut trouver la racine : G(W)-Pl(U,V)=0
288 GeomFill_PlanFunc E(P, Tangent, myGuide);
291 math_FunctionRoot Result(E, X(1), XTol(1),
292 Inf(1), Sup(1), Iter);
296 Standard_Real Res = Result.Root();
297 // R = Result.Root(); // solution
298 myTrimG->D1(Res, PG, TG);
299 gp_Vec n (P, PG), dn; // vecteur definissant la normale du triedre
300 Standard_Real Norm = n.Magnitude();
308 BiNormal = Tangent.Crossed(Normal);
310 // derivee premiere du triedre
311 Standard_Real dedx, dedt, dtg_dt;
312 E.Derivative(Res, dedx);
313 E.DEDT(Res, To, DTangent, dedt);
317 /* Standard_Real h=1.e-7, e, etg, etc;
320 if ( Abs( (etg-e)/h - dedx) > 1.e-4) {
321 cout << "err :" << (etg-e)/h - dedx << endl;
325 myCurve->D0(Param+h, pdbg);
326 frenet->D0(Param+h,td, nb, bnb);
328 GeomFill_PlanFunc Edeb(pdbg, td, myGuide);
329 Edeb.Value(Res, etc);
330 if ( Abs( (etc-e)/h - dedt) > 1.e-4) {
331 cout << "err :" << (etc-e)/h - dedt << endl;
334 dn.SetLinearForm(dtg_dt, TG, -1, To);
336 DNormal.SetLinearForm(-(n*dn), n, dn);
338 DBiNormal.SetLinearForm(Tangent.Crossed(DNormal),
339 DTangent.Crossed(Normal));
344 // plan ortho a la trajectoire
345 Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
348 myStatus = GeomFill_PlaneNotIntersectGuide;
349 return Standard_False;
352 return Standard_True;
356 //=======================================================================
358 //purpose : calculation of trihedron and derivatives
359 //=======================================================================
360 Standard_Boolean GeomFill_GuideTrihedronPlan::D2(const Standard_Real Param,
373 // gp_Vec To,DTo,TG,DTG;
376 myCurve->D2(Param, P, To, DTo);
378 // triedre de Frenet sur la trajectoire
379 frenet->D2(Param,Tangent,DTangent,D2Tangent,
380 Normal,DNormal,D2Normal,
381 BiNormal,DBiNormal,D2BiNormal);
384 // plan ortho a Tangent pour trouver la pt Pprime sur le guide
385 Handle(Geom_Plane) Plan = new (Geom_Plane)(P, Tangent);
386 Handle(GeomAdaptor_HSurface) Pl= new(GeomAdaptor_HSurface)(Plan);
389 Standard_Integer Iter = 50;
390 // fonction dont il faut trouver la racine : G(W) - Pl(U,V)=0
391 GeomFill_FunctionPipe E(Pl , myGuide);
395 math_FunctionSetRoot Result(E, X, XTol,
400 R = Result.Root(); // solution
401 myTrimG->D2(R(1), PG, TG, DTG);
403 gp_Vec n (P, PG); // vecteur definissant la normale du triedre
404 Standard_Real Norm = n.Magnitude();
406 Normal = n.Normalized();
407 BiNormal = Tangent.Crossed(Normal);
411 // derivee premiere du triedre
412 Standard_Real dtp_dt;
413 dtp_dt = (To*Tangent - Norm*(n*DTangent))/(Tangent*TG);
415 dn.SetLinearForm(dtp_dt, TG, -1, To);
417 DNormal.SetLinearForm(-(n*dn), n, dn);
419 DBiNormal = Tangent.Crossed(DNormal) + DTangent.Crossed(Normal);
421 // derivee seconde du triedre
422 Standard_Real d2tp_dt2;
423 d2tp_dt2 = (DTo*Tangent+To*DTangent - dn*DTangent-Norm*n*D2Tangent)/(TG*Tangent)
424 - (To*Tangent-Norm*n*DTangent) * (DTG*dtp_dt*Tangent+TG*DTangent)
425 / ((TG*Tangent)*(TG*Tangent));
428 d2n.SetLinearForm(dtp_dt*dtp_dt, DTG, d2tp_dt2, TG, -DTo);
432 D2Normal.SetLinearForm(3*Pow(n*dn,2)- (dn.SquareMagnitude() + n*d2n), n,
436 D2BiNormal.SetLinearForm(1, D2Tangent.Crossed(Normal),
437 2, DTangent.Crossed(DNormal),
438 Tangent.Crossed(D2Normal));
445 myStatus = GeomFill_PlaneNotIntersectGuide;
446 return Standard_False;
449 // return Standard_True;
450 return Standard_False;
454 //=======================================================================
457 //=======================================================================
458 Handle(GeomFill_TrihedronLaw) GeomFill_GuideTrihedronPlan::Copy() const
460 Handle(GeomFill_GuideTrihedronPlan) copy =
461 new (GeomFill_GuideTrihedronPlan) (myGuide);
462 copy->SetCurve(myCurve);
466 //=======================================================================
467 //function : ErrorStatus
469 //=======================================================================
470 GeomFill_PipeError GeomFill_GuideTrihedronPlan::ErrorStatus() const
476 //=======================================================================
477 //function : NbIntervals
478 //purpose : Version provisoire : Il faut tenir compte du guide
479 //=======================================================================
480 Standard_Integer GeomFill_GuideTrihedronPlan::NbIntervals(const GeomAbs_Shape S)const
485 case GeomAbs_C0: tmpS = GeomAbs_C1; break;
486 case GeomAbs_C1: tmpS = GeomAbs_C2; break;
487 case GeomAbs_C2: tmpS = GeomAbs_C3; break;
488 default: tmpS = GeomAbs_CN;
491 Nb = myCurve->NbIntervals(tmpS);
494 //======================================================================
495 //function :Intervals
497 //=======================================================================
498 void GeomFill_GuideTrihedronPlan::Intervals(TColStd_Array1OfReal& TT,
499 const GeomAbs_Shape S) const
503 case GeomAbs_C0: tmpS = GeomAbs_C1; break;
504 case GeomAbs_C1: tmpS = GeomAbs_C2; break;
505 case GeomAbs_C2: tmpS = GeomAbs_C3; break;
506 default: tmpS = GeomAbs_CN;
508 myCurve->Intervals(TT, tmpS);
511 //======================================================================
512 //function :SetInterval
514 //=======================================================================
515 void GeomFill_GuideTrihedronPlan::SetInterval(const Standard_Real First,
516 const Standard_Real Last)
518 myTrimmed = myCurve->Trim(First, Last, Precision::Confusion());
522 //=======================================================================
523 //function : GetAverageLaw
525 //=======================================================================
526 void GeomFill_GuideTrihedronPlan::GetAverageLaw(gp_Vec& ATangent,
531 Standard_Real t, Delta = (myCurve->LastParameter() -
532 myCurve->FirstParameter())/20.001;
534 ATangent.SetCoord(0.,0.,0.);
535 ANormal.SetCoord(0.,0.,0.);
536 ABiNormal.SetCoord(0.,0.,0.);
539 for (ii=1, T; ii<=20; ii++) {
540 t = myCurve->FirstParameter() +(ii-1)*Delta;
551 //=======================================================================
552 //function : IsConstant
554 //=======================================================================
555 Standard_Boolean GeomFill_GuideTrihedronPlan::IsConstant() const
557 if ((myCurve->GetType() == GeomAbs_Line) &&
558 (myGuide->GetType() == GeomAbs_Line)) {
560 Angle = myCurve->Line().Angle(myGuide->Line());
561 if ((Angle<1.e-12) || ((2*M_PI-Angle)<1.e-12) )
562 return Standard_True;
565 return Standard_False;
568 //=======================================================================
569 //function : IsOnlyBy3dCurve
571 //=======================================================================
572 Standard_Boolean GeomFill_GuideTrihedronPlan::IsOnlyBy3dCurve() const
574 return Standard_False;
577 //=======================================================================
579 //purpose : Nothing!!
580 //=======================================================================
581 void GeomFill_GuideTrihedronPlan::Origine(const Standard_Real ,
582 const Standard_Real )
586 //==================================================================
588 //Purpose : recherche par interpolation d'une valeur initiale
589 //==================================================================
590 void GeomFill_GuideTrihedronPlan::InitX(const Standard_Real Param)
593 Standard_Integer Ideb = 1, Ifin = Pole->RowLength(), Idemi;
594 Standard_Real Valeur, t1, t2;
597 Valeur = Pole->Value(1, Ideb).X();
598 if (Param == Valeur) {
602 Valeur = Pole->Value(1, Ifin).X();
603 if (Param == Valeur) {
607 while ( Ideb+1 != Ifin) {
608 Idemi = (Ideb+Ifin)/2;
609 Valeur = Pole->Value(1, Idemi).X();
610 if (Valeur < Param) {
614 if ( Valeur > Param) { Ifin = Idemi;}
622 t1 = Pole->Value(1,Ideb).X();
623 t2 = Pole->Value(1,Ifin).X();
624 Standard_Real diff = t2-t1;
626 Standard_Real b = (Param-t1) / diff,
627 a = (t2-Param) / diff;
629 X(1) = Pole->Value(1,Ideb).Coord(2) * a
630 + Pole->Value(1,Ifin).Coord(2) * b; //param guide
633 X(1) = (Pole->Value(1, Ideb).Coord(2) +
634 Pole->Value(1, Ifin).Coord(2)) / 2;
636 if (myGuide->IsPeriodic()) {
637 X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(),
638 myGuide->LastParameter());