1 // File: GeomFill_LocationGuide.cxx
2 // Created: Wed Jul 8 15:16:45 1998
3 // Author: Stephanie HUMEAU
7 #include <GeomFill_LocationGuide.ixx>
12 #include <gp_Trsf.hxx>
13 #include <gp_GTrsf.hxx>
16 #include <gp_Pnt2d.hxx>
18 #include <math_Vector.hxx>
19 #include <math_Gauss.hxx>
20 #include <math_FunctionSetRoot.hxx>
21 #include <Precision.hxx>
23 #include <Geom_SurfaceOfRevolution.hxx>
24 #include <Geom_BSplineCurve.hxx>
25 #include <Geom_Curve.hxx>
27 #include <Adaptor3d_SurfaceOfRevolution.hxx>
28 #include <Adaptor3d_HSurface.hxx>
30 #include <IntCurveSurface_IntersectionPoint.hxx>
31 #include <IntCurveSurface_HInter.hxx>
32 #include <Adaptor3d_Surface.hxx>
33 #include <GeomAdaptor.hxx>
34 #include <GeomAdaptor_HSurface.hxx>
35 #include <GeomAdaptor_HCurve.hxx>
38 #include <GeomFill_FunctionGuide.ixx>
39 #include <GeomFill_UniformSection.hxx>
40 #include <GeomFill_SectionPlacement.hxx>
41 #include <Geom_TrimmedCurve.hxx>
42 #include <GeomLib.hxx>
45 #include <TColStd_HArray1OfInteger.hxx>
46 #include <TColStd_HArray1OfReal.hxx>
47 #include <TColgp_HArray1OfPnt.hxx>
50 static Standard_Integer Affich = 0;
51 #include <Approx_Curve3d.hxx>
52 #include <DrawTrSurf.hxx>
55 //=======================================================================
56 //function : TraceRevol
57 //purpose : Trace la surface de revolution (Debug)
58 //=======================================================================
60 static void TraceRevol(const Standard_Real t,
61 const Standard_Real s,
62 const Handle(GeomFill_TrihedronWithGuide)& Law,
63 const Handle(GeomFill_SectionLaw)& Section,
64 const Handle(Adaptor3d_HCurve)& Curve,
71 gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX());
74 Ok = Law->D0(t, T, N, B);
76 gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ());
79 gp_Dir D = M.Column(3);
80 gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion
82 // calculer transfo entre triedre et Oxyz
86 Transfo.SetTransformation(N3, Rep);
88 // transformer la section
89 Standard_Real f, l,e=1.e-7;
90 Handle (Geom_Curve) S, C;
92 if (Section->IsConstant(e)) {
93 C = Section->ConstantSection();
96 Standard_Integer NbPoles, NbKnots, Deg;
97 Section->SectionShape(NbPoles, NbKnots, Deg);
98 TColStd_Array1OfInteger Mult(1,NbKnots);
99 Section->Mults( Mult);
100 TColStd_Array1OfReal Knots(1,NbKnots);
101 Section->Knots(Knots);
102 TColgp_Array1OfPnt Poles(1, NbPoles);
103 TColStd_Array1OfReal Weights(1, NbPoles);
104 Section->D0(s, Poles, Weights);
105 if (Section->IsRational())
106 C = new (Geom_BSplineCurve)
107 (Poles, Weights, Knots, Mult ,
108 Deg, Section->IsUPeriodic());
110 C = new (Geom_BSplineCurve)
112 Deg, Section->IsUPeriodic());
116 f = C->FirstParameter();
117 l = C->LastParameter();
118 S = new (Geom_TrimmedCurve) (C, f, l);
119 S->Transform(Transfo);
121 // Surface de revolution
122 Handle (Geom_Surface) Revol = new(Geom_SurfaceOfRevolution) (S, Ax);
123 cout << "Surf Revol at parameter t = " << t << endl;
126 Standard_CString aName = "TheRevol" ;
127 DrawTrSurf::Set(aName,Revol);
132 //==================================================================
133 //Function: InGoodPeriod
134 //Purpose : Recadre un paramtere
135 //==================================================================
136 static void InGoodPeriod(const Standard_Real Prec,
137 const Standard_Real Period,
138 Standard_Real& Current)
140 Standard_Real Diff=Current-Prec;
141 Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period);
142 Current -= nb*Period;
144 if (Diff > Period/2) Current -= Period;
145 else if (Diff < -Period/2) Current += Period;
148 //==================================================================
149 //Function: GeomFill_LocationGuide
150 //Purpose : constructor
151 //==================================================================
152 GeomFill_LocationGuide::
153 GeomFill_LocationGuide (const Handle(GeomFill_TrihedronWithGuide)& Triedre)
154 : TolRes(1,3), Inf(1,3,0.), Sup(1,3,0.),
155 X(1,3), R(1,3), myStatus(GeomFill_PipeOk)
158 myLaw = Triedre; // loi de triedre
159 mySec.Nullify(); // loi de section
161 myFirstS = myLastS = -505e77;
163 myNbPts = 21; // nb points pour les calculs
164 myGuide = myLaw->Guide(); // courbe guide
165 if (!myGuide->IsPeriodic()) {
166 Standard_Real f, l, delta;
167 f = myGuide->FirstParameter();
168 l = myGuide->LastParameter();
172 myGuide = myGuide->Trim(f,l,delta*1.e-7); // courbe guide
175 myPoles2d = new (TColgp_HArray2OfPnt2d)(1, 2, 1, myNbPts);
176 rotation = Standard_False; // contact ou non
177 OrigParam1 = 0; // param pour ACR quand trajectoire
178 OrigParam2 = 1; // et guide pas meme sens de parcourt
180 WithTrans = Standard_False;
184 Approx_Curve3d approx(myGuide, 1.e-4,
186 15+myGuide->NbIntervals(GeomAbs_CN),
188 if (approx.HasResult()) {
189 Standard_CString aName = "TheGuide" ;
190 DrawTrSurf::Set(aName, approx.Curve());
196 //==================================================================
197 //Function: SetRotation
198 //Purpose : init et force la Rotation
199 //==================================================================
200 void GeomFill_LocationGuide::SetRotation(const Standard_Real PrecAngle,
201 Standard_Real& LastAngle)
203 if (myCurve.IsNull())
204 Standard_ConstructionError::Raise(
205 "GeomFill_LocationGuide::The path is not setted !!");
208 gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX());
212 Standard_Integer ii, Deg;
213 Standard_Boolean isconst, israt=Standard_False;
214 Standard_Real t, v,w, OldAngle=0, Angle, DeltaG, DeltaU, Diff;
215 Standard_Real CurAngle = PrecAngle, a1, a2;
217 Handle(Geom_SurfaceOfRevolution) Revol; // surface de revolution
218 Handle(GeomAdaptor_HSurface) Pl; // = Revol
219 Handle(Geom_TrimmedCurve) S;
220 IntCurveSurface_IntersectionPoint PInt; // intersection guide/Revol
221 IntCurveSurface_HInter Int;
222 Handle(TColStd_HArray1OfInteger) Mult;
223 Handle(TColStd_HArray1OfReal) Knots, Weights;
224 Handle(TColgp_HArray1OfPnt) Poles;
227 Standard_Real U=0, UPeriod=0;
228 Standard_Real f = myCurve->FirstParameter();
229 Standard_Real l = myCurve->LastParameter();
230 Standard_Boolean Ok, uperiodic = mySec->IsUPeriodic();
232 DeltaG = (myGuide->LastParameter() - myGuide->FirstParameter())/5;
233 Handle(Geom_Curve) mySection;
234 Standard_Real Tol =1.e-9;
236 Standard_Integer NbPoles, NbKnots;
237 mySec->SectionShape(NbPoles, NbKnots, Deg);
240 if (mySec->IsConstant(Tol)) {
241 mySection = mySec->ConstantSection();
242 Uf = mySection->FirstParameter();
243 Ul = mySection->LastParameter();
245 isconst = Standard_True;
248 isconst = Standard_False;
249 israt = mySec->IsRational();
250 Mult = new (TColStd_HArray1OfInteger) (1, NbKnots);
251 mySec->Mults( Mult->ChangeArray1());
252 Knots = new (TColStd_HArray1OfReal) (1, NbKnots);
253 mySec->Knots(Knots->ChangeArray1());
254 Poles = new (TColgp_HArray1OfPnt) (1, NbPoles);
255 Weights = new (TColStd_HArray1OfReal) (1, NbPoles);
256 Uf = Knots->Value(1);
257 Ul = Knots->Value(NbKnots);
262 // Standard_Integer bid1, bid2, NbK;
263 Delta = myGuide->LastParameter() - myGuide->FirstParameter();
264 Inf(1) = myGuide->FirstParameter() - Delta/10;
265 Sup(1) = myGuide->LastParameter() + Delta/10;
271 Inf(3) = Uf - Delta/10;
272 Sup(3) = Ul + Delta/10;
275 DeltaU = (Ul-Uf)/(2+NbKnots);
276 if (uperiodic) UPeriod = Ul-Uf;
278 for (ii=1; ii<=myNbPts; ii++) {
279 t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
282 Ok = myLaw->D0(t, T, N, B);
284 myStatus = myLaw->ErrorStatus();
285 return; //Y a rien a faire.
289 gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ());
293 gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion
295 // calculer transfo entre triedre et Oxyz
299 Transfo.SetTransformation(N3, Rep);
301 // transformer la section
303 U = myFirstS + (t-myCurve->FirstParameter())*ratio;
304 mySec->D0(U, Poles->ChangeArray1(), Weights->ChangeArray1());
306 mySection = new (Geom_BSplineCurve)
311 Deg, mySec->IsUPeriodic());
313 mySection = new (Geom_BSplineCurve)
317 Deg, mySec->IsUPeriodic());
318 S = new (Geom_TrimmedCurve) (mySection, Uf, Ul);
321 S = new (Geom_TrimmedCurve)
322 (Handle(Geom_Curve)::DownCast(mySection->Copy()), Uf, Ul);
324 S->Transform(Transfo);
326 // Surface de revolution
327 Revol = new(Geom_SurfaceOfRevolution) (S, Ax);
329 Pl = new (GeomAdaptor_HSurface)(Revol);
330 Int.Perform(myGuide, Pl); // intersection surf. revol / guide
331 if (Int.NbPoints() == 0) {
333 cout <<"LocationGuide : Pas d'intersection"<<endl;
334 TraceRevol(t, U, myLaw, mySec, myCurve, Trans);
336 Standard_Boolean SOS=Standard_False;
338 // Intersection de secour entre surf revol et guide
340 X(1) = myPoles2d->Value(1,ii-1).Y();
341 X(2) = myPoles2d->Value(2,ii-1).X();
342 X(3) = myPoles2d->Value(2,ii-1).Y();
343 GeomFill_FunctionGuide E (mySec, myGuide, U);
344 E.SetParam(U, P, T.XYZ(), N.XYZ());
345 // resolution => angle
346 math_FunctionSetRoot Result(E, X, TolRes,
349 if (Result.IsDone() &&
350 (Result.FunctionSetErrors().Norm() < TolRes(1)*TolRes(1)) ) {
352 cout << "Ratrappage Reussi !" << endl;
357 PInt.SetValues(P, RR(2), RR(3), RR(1), IntCurveSurface_Out);
361 cout << "Echec du Ratrappage !" << endl;
366 myStatus = GeomFill_ImpossibleContact;
370 else { // on prend le point d'intersection
371 // d'angle le plus proche de P
374 InGoodPeriod (CurAngle, 2*PI, a1);
375 Standard_Real Dmin = Abs(a1-CurAngle);
376 for (Standard_Integer jj=2;jj<=Int.NbPoints();jj++) {
377 a2 = Int.Point(jj).U();
378 InGoodPeriod (CurAngle, 2*PI, a2);
379 if (Abs(a2-CurAngle) < Dmin) {
380 PInt = Int.Point(jj);
381 Dmin = Abs(a2-CurAngle);
389 Diff = w - myPoles2d->Value(1, ii-1).Y();
390 if (Abs(Diff) > DeltaG) {
391 if (myGuide->IsPeriodic()) {
392 InGoodPeriod (myPoles2d->Value(1, ii-1).Y(),
393 myGuide->Period(), w);
394 Diff = w - myPoles2d->Value(1, ii-1).Y();
399 if (Abs(Diff) > DeltaG) {
400 cout << "Location :: Diff on Guide : " <<
405 //Recadrage de l'angle.
408 Diff = Angle - OldAngle;
409 if (Abs(Diff) > PI) {
410 InGoodPeriod (OldAngle, 2*PI, Angle);
411 Diff = Angle - OldAngle;
414 if (Abs(Diff) > PI/4) {
415 cout << "Diff d'angle trop grand !!" << endl;
425 InGoodPeriod (myPoles2d->Value(2, ii-1).Y(), UPeriod, v);
427 Diff = v - myPoles2d->Value(2, ii-1).Y();
429 if (Abs(Diff) > DeltaU) {
430 cout << "Diff sur section trop grand !!" << endl;
435 p1.SetCoord(t, w); // on stocke les parametres
436 p2.SetCoord(Angle , v);
438 myPoles2d->SetValue(1, ii, p1);
439 myPoles2d->SetValue(2, ii, p2);
443 LastAngle = CurAngle;
444 rotation = Standard_True; //C'est pret !
448 //==================================================================
450 //Purpose : init loi de section et force la Rotation
451 //==================================================================
452 void GeomFill_LocationGuide::Set(const Handle(GeomFill_SectionLaw)& Section,
453 const Standard_Boolean rotat,
454 const Standard_Real SFirst,
455 const Standard_Real SLast,
456 const Standard_Real PrecAngle,
457 Standard_Real& LastAngle)
459 myStatus = GeomFill_PipeOk;
462 LastAngle = PrecAngle;
463 if (myCurve.IsNull())
466 ratio = (SLast-SFirst) / (myCurve->LastParameter() -
467 myCurve->FirstParameter());
470 if (rotat) SetRotation(PrecAngle, LastAngle);
471 else rotation = Standard_False;
474 //==================================================================
475 //Function: EraseRotation
476 //Purpose : Supprime la Rotation
477 //==================================================================
478 void GeomFill_LocationGuide:: EraseRotation()
480 rotation = Standard_False;
481 if (myStatus == GeomFill_ImpossibleContact) myStatus = GeomFill_PipeOk;
484 //==================================================================
487 //==================================================================
488 Handle(GeomFill_LocationLaw) GeomFill_LocationGuide::Copy() const
491 Handle(GeomFill_TrihedronWithGuide) L;
492 L = Handle(GeomFill_TrihedronWithGuide)::DownCast(myLaw->Copy());
493 Handle(GeomFill_LocationGuide) copy = new
494 (GeomFill_LocationGuide) (L);
495 copy->SetOrigine(OrigParam1, OrigParam2);
496 copy->Set(mySec, rotation, myFirstS, myLastS,
497 myPoles2d->Value(1,1).X(), la);
498 copy->SetTrsf(Trans);
504 //==================================================================
506 //Purpose : Calcul des poles sur la surface d'arret (intersection
507 // courbe guide / surface de revolution en myNbPts points)
508 //==================================================================
509 void GeomFill_LocationGuide::SetCurve(const Handle(Adaptor3d_HCurve)& C)
511 Standard_Real LastAngle;
515 if (!myCurve.IsNull()){
517 myLaw->Origine(OrigParam1, OrigParam2);
518 myStatus = myLaw->ErrorStatus();
520 if (rotation) SetRotation(myPoles2d->Value(1,1).X(), LastAngle);
524 //==================================================================
526 //Purpose : return the trajectoire
527 //==================================================================
528 const Handle(Adaptor3d_HCurve)& GeomFill_LocationGuide::GetCurve() const
533 //==================================================================
536 //==================================================================
537 void GeomFill_LocationGuide::SetTrsf(const gp_Mat& Transfo)
543 WithTrans = Standard_False; // Au cas ou Trans = I
544 for (Standard_Integer ii=1; ii<=3 && !WithTrans ; ii++)
545 for (Standard_Integer jj=1; jj<=3 && !WithTrans; jj++)
546 if (Abs(Aux.Value(ii, jj)) > 1.e-14) WithTrans = Standard_True;
549 //==================================================================
552 //==================================================================
553 Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param,
561 myCurve->D0(Param, P);
563 Ok = myLaw->D0(Param, T, N, B);
565 myStatus = myLaw->ErrorStatus();
568 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
575 Standard_Real U = myFirstS +
576 (Param-myCurve->FirstParameter())*ratio;
577 // initialisations germe
580 Standard_Integer Iter = 100;
586 // Intersection entre surf revol et guide
588 GeomFill_FunctionGuide E (mySec, myGuide, U);
589 E.SetParam(Param, P, t, n);
590 // resolution => angle
591 math_FunctionSetRoot Result(E, X, TolRes,
594 if (Result.IsDone()) {
600 Rot.SetRotation(t, R(2));
608 cout << "LocationGuide::D0 : No Result !"<<endl;
609 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
611 myStatus = GeomFill_ImpossibleContact;
612 return Standard_False;
616 return Standard_True;
619 //==================================================================
621 //Purpose : calcul de l'intersection (C0) surface revol / guide
622 //==================================================================
623 Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param,
626 // TColgp_Array1OfPnt2d& Poles2d)
627 TColgp_Array1OfPnt2d& )
633 Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter());
635 myCurve->FirstParameter() ;
639 myCurve->D0(Param, P);
641 Ok = myLaw->D0(Param, T, N, B);
643 myStatus = myLaw->ErrorStatus();
646 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
654 Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter());
656 myCurve->FirstParameter() ;
659 //initialisation du germe
661 Standard_Integer Iter = 100;
667 // equation d'intersection entre surf revol et guide => angle
668 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
669 (Param-myCurve->FirstParameter())*ratio);
670 E.SetParam(Param, P, t, n);
673 math_FunctionSetRoot Result(E, X, TolRes,
676 if (Result.IsDone()) {
682 Rot.SetRotation(t, R(2));
692 cout << "LocationGuide::D0 : No Result !"<<endl;
693 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
695 myStatus = GeomFill_ImpossibleContact;
696 return Standard_False;
700 return Standard_True;
704 //==================================================================
706 //Purpose : calcul de l'intersection (C1) surface revol / guide
707 //==================================================================
708 Standard_Boolean GeomFill_LocationGuide::D1(const Standard_Real Param,
713 // TColgp_Array1OfPnt2d& Poles2d,
714 TColgp_Array1OfPnt2d& ,
715 // TColgp_Array1OfVec2d& DPoles2d)
716 TColgp_Array1OfVec2d& )
718 // gp_Vec T, N, B, DT, DN, DB, T0, N0, B0;
719 gp_Vec T, N, B, DT, DN, DB;
724 myCurve->D1(Param, P, DV);
726 Ok = myLaw->D1(Param, T, DT, N, DN, B, DB);
728 myStatus = myLaw->ErrorStatus();
731 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
732 DM.SetCols(DN.XYZ() , DB.XYZ(), DT.XYZ());
740 return Standard_False;
743 Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter());
745 myCurve->FirstParameter() ;
748 // initialisation du germe
751 Standard_Integer Iter = 100;
752 gp_XYZ t,b,n, dt, db, dn;
760 // equation d'intersection surf revol / guide => angle
761 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
762 (Param-myCurve->FirstParameter())*ratio);
763 E.SetParam(Param, P, t, n);
766 math_FunctionSetRoot Result(E, X, TolRes,
771 // solution de la fonction
774 // derivee de la fonction
775 math_Vector DEDT(1,3);
776 E.DerivT(R, DV.XYZ(), dt, DEDT); // dE/dt => DEDT
778 math_Vector DSDT (1,3,0);
779 math_Matrix DEDX (1,3,1,3,0);
780 E.Derivatives(R, DEDX); // dE/dx au point R => DEDX
782 // resolution du syst. : DEDX*DSDT = -DEDT
786 Ga.Solve (DEDT.Opposite(), DSDT);// resolution du syst.
790 cout << "DEDX = " << DEDX << endl;
791 cout << "DEDT = " << DEDT << endl;
793 Standard_ConstructionError::Raise(
794 "LocationGuide::D1 : No Result dans la derivee");
797 // transformation = rotation
799 Rot.SetRotation(t, R(2));
803 M.SetCols(n*Rot, b*Rot, t);
805 // transfo entre triedre (en Q) et Oxyz
806 gp_Ax3 Rep(gp::Origin(),gp::DZ(), gp::DX());
807 gp_Ax3 RepTriedre(gp::Origin(),t,n);
809 Transfo3.SetTransformation(Rep,RepTriedre);
810 // on se place dans Oxyz
811 Transfo3.Transforms(n);
812 Transfo3.Transforms(b);
813 Transfo3.Transforms(dn);
814 Transfo3.Transforms(db);
816 // matrices de rotation et derivees
817 Standard_Real A = R(2);
818 Standard_Real Aprim = DSDT(2);
821 gp_Mat M2 (Cos(A), -Sin(A),0, // rotation autour de T
826 gp_Mat M2prim (-Sin(A), -Cos(A), 0, // derivee rotation autour de T
829 M2prim.Multiply(Aprim);
843 // on repasse dans repere triedre
845 InvTrsf = Transfo3.Inverted();
846 InvTrsf.Transforms(dn);
847 InvTrsf.Transforms(db);
849 DM.SetCols(dn , db , dt);
854 cout << "LocationGuide::D1 : No Result !!"<<endl;
855 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
857 myStatus = GeomFill_ImpossibleContact;
858 return Standard_False;
864 return Standard_True;
868 //==================================================================
870 //Purpose : calcul de l'intersection (C2) surface revol / guide
871 //==================================================================
872 Standard_Boolean GeomFill_LocationGuide::D2(const Standard_Real Param,
879 // TColgp_Array1OfPnt2d& Poles2d,
880 TColgp_Array1OfPnt2d& ,
881 // TColgp_Array1OfVec2d& DPoles2d,
882 TColgp_Array1OfVec2d& ,
883 // TColgp_Array1OfVec2d& D2Poles2d)
884 TColgp_Array1OfVec2d& )
886 gp_Vec T, N, B, DT, DN, DB, D2T, D2N, D2B;
887 // gp_Vec T0, N0, B0, T1, N1, B1;
892 myCurve->D2(Param, P, DV, D2V);
894 Ok = myLaw->D2(Param, T, DT, D2T, N, DN, D2N, B, DB, D2B);
896 myStatus = myLaw->ErrorStatus();
908 return Standard_False;
910 Standard_Real U = myFirstS +
911 (Param-myCurve->FirstParameter())*ratio;
913 math_Vector X(1,3,0);
919 // Standard_Real ETol = 1.e-6;
920 Standard_Integer Iter = 100;
923 // resoudre equation d'intersection entre surf revol et guide => angle
924 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
925 (Param-myCurve->FirstParameter())*ratio);
926 E.SetParam(Param, P, T, N);
929 math_FunctionSetRoot Result(E, X, TolRes,
934 Result.Root(R); // solution
936 //gp_Pnt2d p (R(2), R(3)); // point sur la surface (angle, v)
937 //Poles2d.SetValue(1,p);
939 // derivee de la fonction
940 math_Vector DEDT(1,3,0);
941 E.DerivT(Param, Param0, R, R0, DEDT); // dE/dt => DEDT
942 math_Vector DSDT (1,3,0);
943 math_Matrix DEDX (1,3,1,3,0);
944 E.Derivatives(R, DEDX); // dE/dx au point R => DEDX
946 // resolution du syst. lin. : DEDX*DSDT = -DEDT
950 Ga.Solve (DEDT.Opposite(), DSDT); // resolution du syst. lin.
951 //gp_Vec2d dp (DSDT(2), DSDT(3)); // surface
952 //DPoles2d.SetValue(1, dp);
954 else cout <<"LocationGuide::D2 : No Result dans la derivee premiere"<<endl;
957 GeomFill_Tensor D2EDX2(3,3,3);
958 E.Deriv2X(R, D2EDX2); // d2E/dx2
960 math_Vector D2EDT2(1,3,0);
962 // if(Param1 < Param && Param < Param0)
963 E.Deriv2T(Param1, Param, Param0, R1, R, R0, D2EDT2); // d2E/dt2
964 // else if (Param < Param0 && Param0 < Param1)
965 // E.Deriv2T(Param, Param0, Param1, R, R0, R1, D2EDT2); // d2E/dt2
967 // E.Deriv2T(Param0, Param1, Param, R0, R1, R, D2EDT2); // d2E/dt2
969 math_Matrix D2EDTDX(1,3,1,3,0);
970 E.DerivTX(Param, Param0, R, R0, D2EDTDX); // d2E/dtdx
972 math_Vector D2SDT2(1,3,0); // d2s/dt2
973 math_Matrix M1(1,3,1,3,0);
974 D2EDX2.Multiply(DSDT,M1);
976 // resolution du syst. lin.
977 math_Gauss Ga1 (DEDX);
980 Ga1.Solve ( - M1*DSDT - 2*D2EDTDX*DSDT - D2EDT2 , D2SDT2);
981 //gp_Vec2d d2p (D2SDT2(2), D2SDT2(3)); // surface
982 //D2Poles2d.SetValue(1, d2p);
985 cout <<"LocationGuide::D2 : No Result dans la derivee seconde"<<endl;
986 myStatus = GeomFill_ImpossibleContact;
989 //------------------------------------------
991 //------------------------------------------
996 Tr.SetRotation(Axe, R(2));
1006 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
1008 //------------------------------------------
1009 // derivees de la rotation
1011 //-----------------------------------------
1012 gp_Vec db,dn,db3,dn3;
1016 gp_Vec db1,dn1,db2,dn2;
1018 //transfo entre triedre et Oxyz
1019 gp_Ax3 RepTriedre4(Q,D,B2);
1021 Transfo3.SetTransformation(Rep,RepTriedre4);
1023 //on passe dans le repere du triedre
1024 n.Transform(Transfo3);
1025 b.Transform(Transfo3);
1026 n2.Transform(Transfo3);
1027 b2.Transform(Transfo3);
1028 dn.Transform(Transfo3);
1029 db.Transform(Transfo3);
1030 dn3.Transform(Transfo3);
1031 db3.Transform(Transfo3);
1032 D2N.Transform(Transfo3);
1033 D2B.Transform(Transfo3);
1035 //matrices de rotation et derivees
1036 Standard_Real A = R(2);
1037 Standard_Real Aprim = DSDT(2);
1038 Standard_Real Asec = D2SDT2(2);
1040 gp_Mat M2 (Cos(A),-Sin(A),0, // rotation autour de T
1044 gp_Mat M2prim (-Sin(A),-Cos(A),0, // derivee 1ere rotation autour de T
1048 gp_Mat M2sec (-Cos(A), Sin(A), 0, // derivee 2nde rotation autour de T
1049 -Sin(A), -Cos(A), 0,
1051 M2sec.Multiply(Aprim*Aprim);
1052 gp_Mat M2p = M2prim.Multiplied(Asec);
1055 M2prim.Multiply(Aprim);
1059 Rot.SetValues(M2(1,1),M2(1,2),M2(1,3),0,
1060 M2(2,1),M2(2,2),M2(2,3),0,
1061 M2(3,1),M2(3,2),M2(3,3),0,
1064 DRot.SetValues(M2prim(1,1),M2prim(1,2),M2prim(1,3),0,
1065 M2prim(2,1),M2prim(2,2),M2prim(2,3),0,
1066 M2prim(3,1),M2prim(3,2),M2prim(3,3),0,
1070 D2Rot.SetValues(M2sec(1,1),M2sec(1,2),M2sec(1,3),0,
1071 M2sec(2,1),M2sec(2,2),M2sec(2,3),0,
1072 M2sec(3,1),M2sec(3,2),M2sec(3,3),0,
1083 dn1.Transform(Transfo3.Inverted());
1084 db1.Transform(Transfo3.Inverted());
1086 DM.SetCols(dn1.XYZ(), db1.XYZ(), DT.XYZ());
1091 dn3.Transform(DRot);
1092 db3.Transform(DRot);
1093 n2.Transform(D2Rot);
1094 b2.Transform(D2Rot);
1095 dn2 = n2 + 2*dn3 + D2N;
1096 db2 = b2 + 2*db3 + D2B;
1097 dn2.Transform(Transfo3.Inverted());
1098 db2.Transform(Transfo3.Inverted());
1100 D2M.SetCols(dn2.XYZ(), db2.XYZ(), D2T.XYZ());
1105 cout << "LocationGuide::D2 : No Result !!" <<endl;
1106 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
1108 return Standard_False;
1114 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
1115 DM.SetCols(DN.XYZ(), DB.XYZ(), DT.XYZ());
1116 D2M.SetCols(D2N.XYZ(), D2B.XYZ(), D2T.XYZ());
1119 return Standard_True;
1120 // return Standard_False;
1123 //==================================================================
1124 //Function : HasFirstRestriction
1126 //==================================================================
1127 Standard_Boolean GeomFill_LocationGuide::HasFirstRestriction() const
1129 return Standard_False;
1132 //==================================================================
1133 //Function : HasLastRestriction
1135 //==================================================================
1136 Standard_Boolean GeomFill_LocationGuide::HasLastRestriction() const
1138 return Standard_False;
1141 //==================================================================
1142 //Function : TraceNumber
1144 //==================================================================
1145 Standard_Integer GeomFill_LocationGuide::TraceNumber() const
1150 //==================================================================
1151 //Function : ErrorStatus
1153 //==================================================================
1154 GeomFill_PipeError GeomFill_LocationGuide::ErrorStatus() const
1159 //==================================================================
1160 //Function:NbIntervals
1162 //==================================================================
1163 Standard_Integer GeomFill_LocationGuide::NbIntervals
1164 (const GeomAbs_Shape S) const
1166 Standard_Integer Nb_Sec, Nb_Law;
1167 Nb_Sec = myTrimmed->NbIntervals(S);
1168 Nb_Law = myLaw->NbIntervals(S);
1173 else if (Nb_Law==1) {
1177 TColStd_Array1OfReal IntC(1, Nb_Sec+1);
1178 TColStd_Array1OfReal IntL(1, Nb_Law+1);
1179 TColStd_SequenceOfReal Inter;
1180 myTrimmed->Intervals(IntC, S);
1181 myLaw->Intervals(IntL, S);
1183 GeomLib::FuseIntervals( IntC, IntL, Inter, Precision::PConfusion()*0.99);
1184 return Inter.Length()-1;
1188 //==================================================================
1189 //Function:Intervals
1191 //==================================================================
1192 void GeomFill_LocationGuide::Intervals(TColStd_Array1OfReal& T,
1193 const GeomAbs_Shape S) const
1195 Standard_Integer Nb_Sec, Nb_Law;
1196 Nb_Sec = myTrimmed->NbIntervals(S);
1197 Nb_Law = myLaw->NbIntervals(S);
1200 myLaw->Intervals(T, S);
1203 else if (Nb_Law==1) {
1204 myTrimmed->Intervals(T, S);
1208 TColStd_Array1OfReal IntC(1, Nb_Sec+1);
1209 TColStd_Array1OfReal IntL(1, Nb_Law+1);
1210 TColStd_SequenceOfReal Inter;
1211 myTrimmed->Intervals(IntC, S);
1212 myLaw->Intervals(IntL, S);
1214 GeomLib::FuseIntervals(IntC, IntL, Inter, Precision::PConfusion()*0.99);
1215 for (Standard_Integer ii=1; ii<=Inter.Length(); ii++)
1219 //==================================================================
1220 //Function:SetInterval
1222 //==================================================================
1223 void GeomFill_LocationGuide::SetInterval(const Standard_Real First,
1224 const Standard_Real Last)
1226 myLaw->SetInterval(First, Last);
1227 myTrimmed = myCurve->Trim(First, Last, 0);
1229 //==================================================================
1230 //Function: GetInterval
1232 //==================================================================
1233 void GeomFill_LocationGuide::GetInterval(Standard_Real& First,
1234 Standard_Real& Last) const
1236 First = myTrimmed->FirstParameter();
1237 Last = myTrimmed->LastParameter();
1240 //==================================================================
1241 //Function: GetDomain
1243 //==================================================================
1244 void GeomFill_LocationGuide::GetDomain(Standard_Real& First,
1245 Standard_Real& Last) const
1247 First = myCurve->FirstParameter();
1248 Last = myCurve->LastParameter();
1251 //==================================================================
1252 //function : SetTolerance
1254 //==================================================================
1255 void GeomFill_LocationGuide::SetTolerance(const Standard_Real Tol3d,
1256 const Standard_Real )
1258 TolRes(1) = myGuide->Resolution(Tol3d);
1259 Resolution(1, Tol3d, TolRes(2), TolRes(3));
1263 //==================================================================
1264 //function : Resolution
1265 //purpose : A definir
1266 //==================================================================
1267 //void GeomFill_LocationGuide::Resolution (const Standard_Integer Index,
1268 void GeomFill_LocationGuide::Resolution (const Standard_Integer ,
1269 const Standard_Real Tol,
1270 Standard_Real& TolU,
1271 Standard_Real& TolV) const
1277 //==================================================================
1278 //Function:GetMaximalNorm
1279 //Purpose : On suppose les triedres normes => return 1
1280 //==================================================================
1281 Standard_Real GeomFill_LocationGuide::GetMaximalNorm()
1286 //==================================================================
1287 //Function:GetAverageLaw
1289 //==================================================================
1290 void GeomFill_LocationGuide::GetAverageLaw(gp_Mat& AM,
1293 Standard_Integer ii;
1294 Standard_Real U, delta;
1295 gp_Vec V, V1, V2, V3;
1297 myLaw->GetAverageLaw(V1, V2, V3);
1298 AM.SetCols(V1.XYZ(), V2.XYZ(), V3.XYZ());
1300 AV.SetCoord(0., 0., 0.);
1301 delta = (myTrimmed->LastParameter() - myTrimmed->FirstParameter())/10;
1302 U = myTrimmed->FirstParameter();
1303 for (ii=0; ii<=myNbPts; ii++, U+=delta) {
1304 V.SetXYZ( myTrimmed->Value(U).XYZ() );
1307 AV = AV/(myNbPts+1);
1311 //==================================================================
1312 //Function : Section
1314 //==================================================================
1315 Handle(Geom_Curve) GeomFill_LocationGuide::Section() const
1317 return mySec->ConstantSection();
1320 //==================================================================
1323 //==================================================================
1324 Handle(Adaptor3d_HCurve) GeomFill_LocationGuide::Guide() const
1329 //==================================================================
1330 //Function : IsRotation
1332 //==================================================================
1333 // Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& Error) const
1334 Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& ) const
1336 return Standard_False;
1339 //==================================================================
1340 //Function : Rotation
1342 //==================================================================
1343 // void GeomFill_LocationGuide::Rotation(gp_Pnt& Centre) const
1344 void GeomFill_LocationGuide::Rotation(gp_Pnt& ) const
1346 Standard_NotImplemented::Raise("GeomFill_LocationGuide::Rotation");
1349 //==================================================================
1350 //Function : IsTranslation
1352 //==================================================================
1353 // Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& Error) const
1354 Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& ) const
1356 return Standard_False;
1359 //==================================================================
1361 //Purpose : recherche par interpolation d'une valeur initiale
1362 //==================================================================
1363 void GeomFill_LocationGuide::InitX(const Standard_Real Param) const
1366 Standard_Integer Ideb = 1, Ifin = myPoles2d->RowLength(), Idemi;
1367 Standard_Real Valeur, t1, t2;
1370 Valeur = myPoles2d->Value(1, Ideb).X();
1371 if (Param == Valeur) {
1375 Valeur = myPoles2d->Value(1, Ifin).X();
1376 if (Param == Valeur) {
1380 while ( Ideb+1 != Ifin) {
1381 Idemi = (Ideb+Ifin)/2;
1382 Valeur = myPoles2d->Value(1, Idemi).X();
1383 if (Valeur < Param) {
1387 if ( Valeur > Param) { Ifin = Idemi;}
1395 t1 = myPoles2d->Value(1,Ideb).X();
1396 t2 = myPoles2d->Value(1,Ifin).X();
1397 Standard_Real diff = t2-t1;
1399 Standard_Real W1, W2;
1400 W1 = myPoles2d->Value(1,Ideb).Coord(2);
1401 W2 = myPoles2d->Value(1,Ifin).Coord(2);
1402 const gp_Pnt2d& P1 = myPoles2d->Value(2, Ideb);
1403 const gp_Pnt2d& P2 = myPoles2d->Value(2, Ifin);
1406 Standard_Real b = (Param-t1) / diff,
1407 a = (t2-Param) / diff;
1408 X(1) = a * W1 + b * W2;
1409 X(2) = a * P1.Coord(1) + b * P2.Coord(1); // angle
1410 X(3) = a * P1.Coord(2) + b * P2.Coord(2); // param isov
1414 X(2) = (P1.Coord(1) + P2.Coord(1)) /2;
1415 X(3) = (P1.Coord(2) + P2.Coord(2)) /2;
1418 if (myGuide->IsPeriodic()) {
1419 X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(),
1420 myGuide->LastParameter());
1422 X(2) = ElCLib::InPeriod(X(2), 0, 2*PI);
1423 if (mySec->IsUPeriodic()) {
1424 X(3) = ElCLib::InPeriod(X(3), Uf, Ul);
1429 //==================================================================
1430 //Function : SetOrigine
1431 //Purpose : utilise pour ACR dans le cas ou la trajectoire est multi-edges
1432 //==================================================================
1433 void GeomFill_LocationGuide::SetOrigine(const Standard_Real Param1,
1434 const Standard_Real Param2)
1436 OrigParam1 = Param1;
1437 OrigParam2 = Param2;