1 // Created on: 1998-07-08
2 // Created by: Stephanie HUMEAU
3 // Copyright (c) 1998-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
6 // This file is part of Open CASCADE Technology software library.
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.
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
17 #include <GeomFill_LocationGuide.ixx>
22 #include <gp_Trsf.hxx>
23 #include <gp_GTrsf.hxx>
26 #include <gp_Pnt2d.hxx>
28 #include <math_Vector.hxx>
29 #include <math_Gauss.hxx>
30 #include <math_FunctionSetRoot.hxx>
31 #include <Precision.hxx>
33 #include <Geom_SurfaceOfRevolution.hxx>
34 #include <Geom_BSplineCurve.hxx>
35 #include <Geom_Curve.hxx>
37 #include <Adaptor3d_SurfaceOfRevolution.hxx>
38 #include <Adaptor3d_HSurface.hxx>
40 #include <IntCurveSurface_IntersectionPoint.hxx>
41 #include <Adaptor3d_Surface.hxx>
42 #include <GeomAdaptor.hxx>
43 #include <GeomAdaptor_HSurface.hxx>
44 #include <GeomAdaptor_HCurve.hxx>
47 #include <GeomFill_FunctionGuide.ixx>
48 #include <GeomFill_UniformSection.hxx>
49 #include <GeomFill_SectionPlacement.hxx>
50 #include <Geom_TrimmedCurve.hxx>
51 #include <GeomLib.hxx>
54 #include <TColStd_HArray1OfInteger.hxx>
55 #include <TColStd_HArray1OfReal.hxx>
56 #include <TColgp_HArray1OfPnt.hxx>
58 #include <Extrema_ExtCS.hxx>
59 #include <Extrema_POnSurf.hxx>
62 static Standard_Integer Affich = 0;
63 #include <Approx_Curve3d.hxx>
64 #include <DrawTrSurf.hxx>
65 #include <GeomFill_TrihedronWithGuide.hxx>
68 //=======================================================================
69 //function : TraceRevol
70 //purpose : Trace la surface de revolution (Debug)
71 //=======================================================================
73 static void TraceRevol(const Standard_Real t,
74 const Standard_Real s,
75 const Handle(GeomFill_TrihedronWithGuide)& Law,
76 const Handle(GeomFill_SectionLaw)& Section,
77 const Handle(Adaptor3d_HCurve)& Curve,
83 gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX());
88 gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ());
91 gp_Dir D = M.Column(3);
92 gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion
94 // calculer transfo entre triedre et Oxyz
98 Transfo.SetTransformation(N3, Rep);
100 // transformer la section
101 Standard_Real f, l,e=1.e-7;
102 Handle (Geom_Curve) S, C;
104 if (Section->IsConstant(e)) {
105 C = Section->ConstantSection();
108 Standard_Integer NbPoles, NbKnots, Deg;
109 Section->SectionShape(NbPoles, NbKnots, Deg);
110 TColStd_Array1OfInteger Mult(1,NbKnots);
111 Section->Mults( Mult);
112 TColStd_Array1OfReal Knots(1,NbKnots);
113 Section->Knots(Knots);
114 TColgp_Array1OfPnt Poles(1, NbPoles);
115 TColStd_Array1OfReal Weights(1, NbPoles);
116 Section->D0(s, Poles, Weights);
117 if (Section->IsRational())
118 C = new (Geom_BSplineCurve)
119 (Poles, Weights, Knots, Mult ,
120 Deg, Section->IsUPeriodic());
122 C = new (Geom_BSplineCurve)
124 Deg, Section->IsUPeriodic());
128 f = C->FirstParameter();
129 l = C->LastParameter();
130 S = new (Geom_TrimmedCurve) (C, f, l);
131 S->Transform(Transfo);
133 // Surface de revolution
134 Handle (Geom_Surface) Revol = new(Geom_SurfaceOfRevolution) (S, Ax);
135 cout << "Surf Revol at parameter t = " << t << endl;
138 Standard_CString aName = "TheRevol" ;
139 DrawTrSurf::Set(aName,Revol);
144 //==================================================================
145 //Function: InGoodPeriod
146 //Purpose : Recadre un paramtere
147 //==================================================================
148 static void InGoodPeriod(const Standard_Real Prec,
149 const Standard_Real Period,
150 Standard_Real& Current)
152 Standard_Real Diff=Current-Prec;
153 Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period);
154 Current -= nb*Period;
156 if (Diff > Period/2) Current -= Period;
157 else if (Diff < -Period/2) Current += Period;
160 //==================================================================
161 //Function: GeomFill_LocationGuide
162 //Purpose : constructor
163 //==================================================================
164 GeomFill_LocationGuide::
165 GeomFill_LocationGuide (const Handle(GeomFill_TrihedronWithGuide)& Triedre)
166 : TolRes(1,3), Inf(1,3,0.), Sup(1,3,0.),
167 X(1,3), R(1,3), myStatus(GeomFill_PipeOk)
170 myLaw = Triedre; // loi de triedre
171 mySec.Nullify(); // loi de section
173 myFirstS = myLastS = -505e77;
175 myNbPts = 21; // nb points pour les calculs
176 myGuide = myLaw->Guide(); // courbe guide
177 if (!myGuide->IsPeriodic()) {
178 Standard_Real f, l, delta;
179 f = myGuide->FirstParameter();
180 l = myGuide->LastParameter();
184 myGuide = myGuide->Trim(f,l,delta*1.e-7); // courbe guide
187 myPoles2d = new (TColgp_HArray2OfPnt2d)(1, 2, 1, myNbPts);
188 rotation = Standard_False; // contact ou non
189 OrigParam1 = 0; // param pour ACR quand trajectoire
190 OrigParam2 = 1; // et guide pas meme sens de parcourt
192 WithTrans = Standard_False;
196 Approx_Curve3d approx(myGuide, 1.e-4,
198 15+myGuide->NbIntervals(GeomAbs_CN),
200 if (approx.HasResult()) {
201 Standard_CString aName = "TheGuide" ;
202 DrawTrSurf::Set(aName, approx.Curve());
208 //==================================================================
209 //Function: SetRotation
210 //Purpose : init et force la Rotation
211 //==================================================================
212 void GeomFill_LocationGuide::SetRotation(const Standard_Real PrecAngle,
213 Standard_Real& LastAngle)
215 if (myCurve.IsNull())
216 Standard_ConstructionError::Raise(
217 "GeomFill_LocationGuide::The path is not setted !!");
220 gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX());
224 Standard_Integer ii, Deg;
225 Standard_Boolean isconst, israt=Standard_False;
226 Standard_Real t, v,w, OldAngle=0, Angle, DeltaG, Diff;
227 Standard_Real CurAngle = PrecAngle, a1/*, a2*/;
229 Handle(Geom_SurfaceOfRevolution) Revol; // surface de revolution
230 Handle(GeomAdaptor_HSurface) Pl; // = Revol
231 Handle(Geom_TrimmedCurve) S;
232 IntCurveSurface_IntersectionPoint PInt; // intersection guide/Revol
233 Handle(TColStd_HArray1OfInteger) Mult;
234 Handle(TColStd_HArray1OfReal) Knots, Weights;
235 Handle(TColgp_HArray1OfPnt) Poles;
238 Standard_Real U=0, UPeriod=0;
239 Standard_Real f = myCurve->FirstParameter();
240 Standard_Real l = myCurve->LastParameter();
241 Standard_Boolean Ok, uperiodic = mySec->IsUPeriodic();
243 DeltaG = (myGuide->LastParameter() - myGuide->FirstParameter())/5;
244 Handle(Geom_Curve) mySection;
245 Standard_Real Tol =1.e-9;
247 Standard_Integer NbPoles, NbKnots;
248 mySec->SectionShape(NbPoles, NbKnots, Deg);
251 if (mySec->IsConstant(Tol)) {
252 mySection = mySec->ConstantSection();
253 Uf = mySection->FirstParameter();
254 Ul = mySection->LastParameter();
256 isconst = Standard_True;
259 isconst = Standard_False;
260 israt = mySec->IsRational();
261 Mult = new (TColStd_HArray1OfInteger) (1, NbKnots);
262 mySec->Mults( Mult->ChangeArray1());
263 Knots = new (TColStd_HArray1OfReal) (1, NbKnots);
264 mySec->Knots(Knots->ChangeArray1());
265 Poles = new (TColgp_HArray1OfPnt) (1, NbPoles);
266 Weights = new (TColStd_HArray1OfReal) (1, NbPoles);
267 Uf = Knots->Value(1);
268 Ul = Knots->Value(NbKnots);
273 // Standard_Integer bid1, bid2, NbK;
274 Delta = myGuide->LastParameter() - myGuide->FirstParameter();
275 Inf(1) = myGuide->FirstParameter() - Delta/10;
276 Sup(1) = myGuide->LastParameter() + Delta/10;
282 Inf(3) = Uf - Delta/10;
283 Sup(3) = Ul + Delta/10;
286 if (uperiodic) UPeriod = Ul-Uf;
288 for (ii=1; ii<=myNbPts; ii++) {
289 t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
292 Ok = myLaw->D0(t, T, N, B);
294 myStatus = myLaw->ErrorStatus();
295 return; //Y a rien a faire.
299 gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ());
303 gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion
305 // calculer transfo entre triedre et Oxyz
309 Transfo.SetTransformation(N3, Rep);
311 // transformer la section
313 U = myFirstS + (t-myCurve->FirstParameter())*ratio;
314 mySec->D0(U, Poles->ChangeArray1(), Weights->ChangeArray1());
316 mySection = new (Geom_BSplineCurve)
321 Deg, mySec->IsUPeriodic());
323 mySection = new (Geom_BSplineCurve)
327 Deg, mySec->IsUPeriodic());
328 S = new (Geom_TrimmedCurve) (mySection, Uf, Ul);
331 S = new (Geom_TrimmedCurve)
332 (Handle(Geom_Curve)::DownCast(mySection->Copy()), Uf, Ul);
334 S->Transform(Transfo);
336 // Surface de revolution
337 Revol = new(Geom_SurfaceOfRevolution) (S, Ax);
339 GeomAdaptor_Surface GArevol(Revol);
340 Extrema_ExtCS DistMini(myGuide->Curve(), GArevol,
341 Precision::Confusion(), Precision::Confusion());
344 Standard_Real theU = 0., theV = 0.;
346 if (!DistMini.IsDone() || DistMini.NbExt() == 0) {
348 cout <<"LocationGuide : Pas d'intersection"<<endl;
349 TraceRevol(t, U, myLaw, mySec, myCurve, Trans);
351 Standard_Boolean SOS=Standard_False;
353 // Intersection de secour entre surf revol et guide
355 X(1) = myPoles2d->Value(1,ii-1).Y();
356 X(2) = myPoles2d->Value(2,ii-1).X();
357 X(3) = myPoles2d->Value(2,ii-1).Y();
358 GeomFill_FunctionGuide E (mySec, myGuide, U);
359 E.SetParam(U, P, T.XYZ(), N.XYZ());
360 // resolution => angle
361 math_FunctionSetRoot Result(E, TolRes);
362 Result.Perform(E, X, Inf, Sup);
364 if (Result.IsDone() &&
365 (Result.FunctionSetErrors().Norm() < TolRes(1)*TolRes(1)) ) {
367 cout << "Ratrappage Reussi !" << endl;
372 PInt.SetValues(P, RR(2), RR(3), RR(1), IntCurveSurface_Out);
378 cout << "Echec du Ratrappage !" << endl;
383 myStatus = GeomFill_ImpossibleContact;
387 else { // on prend le point d'intersection
388 // d'angle le plus proche de P
390 Standard_Real MinDist = RealLast();
391 Standard_Integer jref = 0;
392 for (Standard_Integer j = 1; j <= DistMini.NbExt(); j++)
394 Standard_Real aDist = DistMini.SquareDistance(j);
401 MinDist = Sqrt(MinDist);
402 DistMini.Points(jref, Pc, Ps);
404 Ps.Parameter(theU, theV);
407 InGoodPeriod (CurAngle, 2*M_PI, a1);
414 Diff = w - myPoles2d->Value(1, ii-1).Y();
415 if (Abs(Diff) > DeltaG) {
416 if (myGuide->IsPeriodic()) {
417 InGoodPeriod (myPoles2d->Value(1, ii-1).Y(),
418 myGuide->Period(), w);
419 Diff = w - myPoles2d->Value(1, ii-1).Y();
424 if (Abs(Diff) > DeltaG) {
425 cout << "Location :: Diff on Guide : " <<
430 //Recadrage de l'angle.
434 Diff = Angle - OldAngle;
435 if (Abs(Diff) > M_PI) {
436 InGoodPeriod (OldAngle, 2*M_PI, Angle);
437 Diff = Angle - OldAngle;
440 if (Abs(Diff) > M_PI/4) {
441 cout << "Diff d'angle trop grand !!" << endl;
452 InGoodPeriod (myPoles2d->Value(2, ii-1).Y(), UPeriod, v);
454 Diff = v - myPoles2d->Value(2, ii-1).Y();
456 if (Abs(Diff) > (Ul-Uf)/(2+NbKnots)) {
457 cout << "Diff sur section trop grand !!" << endl;
462 p1.SetCoord(t, w); // on stocke les parametres
463 p2.SetCoord(Angle , v);
465 myPoles2d->SetValue(1, ii, p1);
466 myPoles2d->SetValue(2, ii, p2);
470 LastAngle = CurAngle;
471 rotation = Standard_True; //C'est pret !
475 //==================================================================
477 //Purpose : init loi de section et force la Rotation
478 //==================================================================
479 void GeomFill_LocationGuide::Set(const Handle(GeomFill_SectionLaw)& Section,
480 const Standard_Boolean rotat,
481 const Standard_Real SFirst,
482 const Standard_Real SLast,
483 const Standard_Real PrecAngle,
484 Standard_Real& LastAngle)
486 myStatus = GeomFill_PipeOk;
489 LastAngle = PrecAngle;
490 if (myCurve.IsNull())
493 ratio = (SLast-SFirst) / (myCurve->LastParameter() -
494 myCurve->FirstParameter());
497 if (rotat) SetRotation(PrecAngle, LastAngle);
498 else rotation = Standard_False;
501 //==================================================================
502 //Function: EraseRotation
503 //Purpose : Supprime la Rotation
504 //==================================================================
505 void GeomFill_LocationGuide:: EraseRotation()
507 rotation = Standard_False;
508 if (myStatus == GeomFill_ImpossibleContact) myStatus = GeomFill_PipeOk;
511 //==================================================================
514 //==================================================================
515 Handle(GeomFill_LocationLaw) GeomFill_LocationGuide::Copy() const
518 Handle(GeomFill_TrihedronWithGuide) L;
519 L = Handle(GeomFill_TrihedronWithGuide)::DownCast(myLaw->Copy());
520 Handle(GeomFill_LocationGuide) copy = new
521 (GeomFill_LocationGuide) (L);
522 copy->SetOrigine(OrigParam1, OrigParam2);
523 copy->Set(mySec, rotation, myFirstS, myLastS,
524 myPoles2d->Value(1,1).X(), la);
525 copy->SetTrsf(Trans);
531 //==================================================================
533 //Purpose : Calcul des poles sur la surface d'arret (intersection
534 // courbe guide / surface de revolution en myNbPts points)
535 //==================================================================
536 void GeomFill_LocationGuide::SetCurve(const Handle(Adaptor3d_HCurve)& C)
538 Standard_Real LastAngle;
542 if (!myCurve.IsNull()){
544 myLaw->Origine(OrigParam1, OrigParam2);
545 myStatus = myLaw->ErrorStatus();
547 if (rotation) SetRotation(myPoles2d->Value(1,1).X(), LastAngle);
551 //==================================================================
553 //Purpose : return the trajectoire
554 //==================================================================
555 const Handle(Adaptor3d_HCurve)& GeomFill_LocationGuide::GetCurve() const
560 //==================================================================
563 //==================================================================
564 void GeomFill_LocationGuide::SetTrsf(const gp_Mat& Transfo)
570 WithTrans = Standard_False; // Au cas ou Trans = I
571 for (Standard_Integer ii=1; ii<=3 && !WithTrans ; ii++)
572 for (Standard_Integer jj=1; jj<=3 && !WithTrans; jj++)
573 if (Abs(Aux.Value(ii, jj)) > 1.e-14) WithTrans = Standard_True;
576 //==================================================================
579 //==================================================================
580 Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param,
588 myCurve->D0(Param, P);
590 Ok = myLaw->D0(Param, T, N, B);
592 myStatus = myLaw->ErrorStatus();
595 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
602 Standard_Real U = myFirstS +
603 (Param-myCurve->FirstParameter())*ratio;
604 // initialisations germe
607 Standard_Integer Iter = 100;
613 // Intersection entre surf revol et guide
615 GeomFill_FunctionGuide E (mySec, myGuide, U);
616 E.SetParam(Param, P, t, n);
617 // resolution => angle
618 math_FunctionSetRoot Result(E, TolRes, Iter);
619 Result.Perform(E, X, Inf, Sup);
621 if (Result.IsDone()) {
627 Rot.SetRotation(t, R(2));
635 cout << "LocationGuide::D0 : No Result !"<<endl;
636 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
638 myStatus = GeomFill_ImpossibleContact;
639 return Standard_False;
643 return Standard_True;
646 //==================================================================
648 //Purpose : calcul de l'intersection (C0) surface revol / guide
649 //==================================================================
650 Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param,
653 // TColgp_Array1OfPnt2d& Poles2d)
654 TColgp_Array1OfPnt2d& )
660 myCurve->D0(Param, P);
662 Ok = myLaw->D0(Param, T, N, B);
664 myStatus = myLaw->ErrorStatus();
667 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
674 //initialisation du germe
676 Standard_Integer Iter = 100;
682 // equation d'intersection entre surf revol et guide => angle
683 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
684 (Param-myCurve->FirstParameter())*ratio);
685 E.SetParam(Param, P, t, n);
688 math_FunctionSetRoot Result(E, TolRes, Iter);
689 Result.Perform(E, X, Inf, Sup);
691 if (Result.IsDone()) {
697 Rot.SetRotation(t, R(2));
707 Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter());
708 cout << "LocationGuide::D0 : No Result !"<<endl;
709 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
711 myStatus = GeomFill_ImpossibleContact;
712 return Standard_False;
716 return Standard_True;
720 //==================================================================
722 //Purpose : calcul de l'intersection (C1) surface revol / guide
723 //==================================================================
724 Standard_Boolean GeomFill_LocationGuide::D1(const Standard_Real Param,
729 // TColgp_Array1OfPnt2d& Poles2d,
730 TColgp_Array1OfPnt2d& ,
731 // TColgp_Array1OfVec2d& DPoles2d)
732 TColgp_Array1OfVec2d& )
734 // gp_Vec T, N, B, DT, DN, DB, T0, N0, B0;
735 gp_Vec T, N, B, DT, DN, DB;
740 myCurve->D1(Param, P, DV);
742 Ok = myLaw->D1(Param, T, DT, N, DN, B, DB);
744 myStatus = myLaw->ErrorStatus();
747 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
748 DM.SetCols(DN.XYZ() , DB.XYZ(), DT.XYZ());
756 return Standard_False;
759 Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter());
761 myCurve->FirstParameter() ;
764 // initialisation du germe
767 Standard_Integer Iter = 100;
768 gp_XYZ t,b,n, dt, db, dn;
776 // equation d'intersection surf revol / guide => angle
777 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
778 (Param-myCurve->FirstParameter())*ratio);
779 E.SetParam(Param, P, t, n);
782 math_FunctionSetRoot Result(E, X, TolRes,
787 // solution de la fonction
790 // derivee de la fonction
791 math_Vector DEDT(1,3);
792 E.DerivT(R, DV.XYZ(), dt, DEDT); // dE/dt => DEDT
794 math_Vector DSDT (1,3,0);
795 math_Matrix DEDX (1,3,1,3,0);
796 E.Derivatives(R, DEDX); // dE/dx au point R => DEDX
798 // resolution du syst. : DEDX*DSDT = -DEDT
802 Ga.Solve (DEDT.Opposite(), DSDT);// resolution du syst.
806 cout << "DEDX = " << DEDX << endl;
807 cout << "DEDT = " << DEDT << endl;
809 Standard_ConstructionError::Raise(
810 "LocationGuide::D1 : No Result dans la derivee");
813 // transformation = rotation
815 Rot.SetRotation(t, R(2));
819 M.SetCols(n*Rot, b*Rot, t);
821 // transfo entre triedre (en Q) et Oxyz
822 gp_Ax3 Rep(gp::Origin(),gp::DZ(), gp::DX());
823 gp_Ax3 RepTriedre(gp::Origin(),t,n);
825 Transfo3.SetTransformation(Rep,RepTriedre);
826 // on se place dans Oxyz
827 Transfo3.Transforms(n);
828 Transfo3.Transforms(b);
829 Transfo3.Transforms(dn);
830 Transfo3.Transforms(db);
832 // matrices de rotation et derivees
833 Standard_Real A = R(2);
834 Standard_Real Aprim = DSDT(2);
837 gp_Mat M2 (Cos(A), -Sin(A),0, // rotation autour de T
842 gp_Mat M2prim (-Sin(A), -Cos(A), 0, // derivee rotation autour de T
845 M2prim.Multiply(Aprim);
859 // on repasse dans repere triedre
861 InvTrsf = Transfo3.Inverted();
862 InvTrsf.Transforms(dn);
863 InvTrsf.Transforms(db);
865 DM.SetCols(dn , db , dt);
870 cout << "LocationGuide::D1 : No Result !!"<<endl;
871 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
873 myStatus = GeomFill_ImpossibleContact;
874 return Standard_False;
880 return Standard_True;
884 //==================================================================
886 //Purpose : calcul de l'intersection (C2) surface revol / guide
887 //==================================================================
888 Standard_Boolean GeomFill_LocationGuide::D2(const Standard_Real Param,
895 // TColgp_Array1OfPnt2d& Poles2d,
896 TColgp_Array1OfPnt2d& ,
897 // TColgp_Array1OfVec2d& DPoles2d,
898 TColgp_Array1OfVec2d& ,
899 // TColgp_Array1OfVec2d& D2Poles2d)
900 TColgp_Array1OfVec2d& )
902 gp_Vec T, N, B, DT, DN, DB, D2T, D2N, D2B;
903 // gp_Vec T0, N0, B0, T1, N1, B1;
908 myCurve->D2(Param, P, DV, D2V);
910 Ok = myLaw->D2(Param, T, DT, D2T, N, DN, D2N, B, DB, D2B);
912 myStatus = myLaw->ErrorStatus();
924 return Standard_False;
926 Standard_Real U = myFirstS +
927 (Param-myCurve->FirstParameter())*ratio;
929 math_Vector X(1,3,0);
935 // Standard_Real ETol = 1.e-6;
936 Standard_Integer Iter = 100;
939 // resoudre equation d'intersection entre surf revol et guide => angle
940 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
941 (Param-myCurve->FirstParameter())*ratio);
942 E.SetParam(Param, P, T, N);
945 math_FunctionSetRoot Result(E, X, TolRes,
950 Result.Root(R); // solution
952 //gp_Pnt2d p (R(2), R(3)); // point sur la surface (angle, v)
953 //Poles2d.SetValue(1,p);
955 // derivee de la fonction
956 math_Vector DEDT(1,3,0);
957 E.DerivT(Param, Param0, R, R0, DEDT); // dE/dt => DEDT
958 math_Vector DSDT (1,3,0);
959 math_Matrix DEDX (1,3,1,3,0);
960 E.Derivatives(R, DEDX); // dE/dx au point R => DEDX
962 // resolution du syst. lin. : DEDX*DSDT = -DEDT
966 Ga.Solve (DEDT.Opposite(), DSDT); // resolution du syst. lin.
967 //gp_Vec2d dp (DSDT(2), DSDT(3)); // surface
968 //DPoles2d.SetValue(1, dp);
970 else cout <<"LocationGuide::D2 : No Result dans la derivee premiere"<<endl;
973 GeomFill_Tensor D2EDX2(3,3,3);
974 E.Deriv2X(R, D2EDX2); // d2E/dx2
976 math_Vector D2EDT2(1,3,0);
978 // if(Param1 < Param && Param < Param0)
979 E.Deriv2T(Param1, Param, Param0, R1, R, R0, D2EDT2); // d2E/dt2
980 // else if (Param < Param0 && Param0 < Param1)
981 // E.Deriv2T(Param, Param0, Param1, R, R0, R1, D2EDT2); // d2E/dt2
983 // E.Deriv2T(Param0, Param1, Param, R0, R1, R, D2EDT2); // d2E/dt2
985 math_Matrix D2EDTDX(1,3,1,3,0);
986 E.DerivTX(Param, Param0, R, R0, D2EDTDX); // d2E/dtdx
988 math_Vector D2SDT2(1,3,0); // d2s/dt2
989 math_Matrix M1(1,3,1,3,0);
990 D2EDX2.Multiply(DSDT,M1);
992 // resolution du syst. lin.
993 math_Gauss Ga1 (DEDX);
996 Ga1.Solve ( - M1*DSDT - 2*D2EDTDX*DSDT - D2EDT2 , D2SDT2);
997 //gp_Vec2d d2p (D2SDT2(2), D2SDT2(3)); // surface
998 //D2Poles2d.SetValue(1, d2p);
1001 cout <<"LocationGuide::D2 : No Result dans la derivee seconde"<<endl;
1002 myStatus = GeomFill_ImpossibleContact;
1005 //------------------------------------------
1007 //------------------------------------------
1012 Tr.SetRotation(Axe, R(2));
1022 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
1024 //------------------------------------------
1025 // derivees de la rotation
1027 //-----------------------------------------
1028 gp_Vec db,dn,db3,dn3;
1032 gp_Vec db1,dn1,db2,dn2;
1034 //transfo entre triedre et Oxyz
1035 gp_Ax3 RepTriedre4(Q,D,B2);
1037 Transfo3.SetTransformation(Rep,RepTriedre4);
1039 //on passe dans le repere du triedre
1040 n.Transform(Transfo3);
1041 b.Transform(Transfo3);
1042 n2.Transform(Transfo3);
1043 b2.Transform(Transfo3);
1044 dn.Transform(Transfo3);
1045 db.Transform(Transfo3);
1046 dn3.Transform(Transfo3);
1047 db3.Transform(Transfo3);
1048 D2N.Transform(Transfo3);
1049 D2B.Transform(Transfo3);
1051 //matrices de rotation et derivees
1052 Standard_Real A = R(2);
1053 Standard_Real Aprim = DSDT(2);
1054 Standard_Real Asec = D2SDT2(2);
1056 gp_Mat M2 (Cos(A),-Sin(A),0, // rotation autour de T
1060 gp_Mat M2prim (-Sin(A),-Cos(A),0, // derivee 1ere rotation autour de T
1064 gp_Mat M2sec (-Cos(A), Sin(A), 0, // derivee 2nde rotation autour de T
1065 -Sin(A), -Cos(A), 0,
1067 M2sec.Multiply(Aprim*Aprim);
1068 gp_Mat M2p = M2prim.Multiplied(Asec);
1071 M2prim.Multiply(Aprim);
1075 Rot.SetValues(M2(1,1),M2(1,2),M2(1,3),0,
1076 M2(2,1),M2(2,2),M2(2,3),0,
1077 M2(3,1),M2(3,2),M2(3,3),0,
1080 DRot.SetValues(M2prim(1,1),M2prim(1,2),M2prim(1,3),0,
1081 M2prim(2,1),M2prim(2,2),M2prim(2,3),0,
1082 M2prim(3,1),M2prim(3,2),M2prim(3,3),0,
1086 D2Rot.SetValues(M2sec(1,1),M2sec(1,2),M2sec(1,3),0,
1087 M2sec(2,1),M2sec(2,2),M2sec(2,3),0,
1088 M2sec(3,1),M2sec(3,2),M2sec(3,3),0,
1099 dn1.Transform(Transfo3.Inverted());
1100 db1.Transform(Transfo3.Inverted());
1102 DM.SetCols(dn1.XYZ(), db1.XYZ(), DT.XYZ());
1107 dn3.Transform(DRot);
1108 db3.Transform(DRot);
1109 n2.Transform(D2Rot);
1110 b2.Transform(D2Rot);
1111 dn2 = n2 + 2*dn3 + D2N;
1112 db2 = b2 + 2*db3 + D2B;
1113 dn2.Transform(Transfo3.Inverted());
1114 db2.Transform(Transfo3.Inverted());
1116 D2M.SetCols(dn2.XYZ(), db2.XYZ(), D2T.XYZ());
1121 cout << "LocationGuide::D2 : No Result !!" <<endl;
1122 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
1124 return Standard_False;
1130 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
1131 DM.SetCols(DN.XYZ(), DB.XYZ(), DT.XYZ());
1132 D2M.SetCols(D2N.XYZ(), D2B.XYZ(), D2T.XYZ());
1135 return Standard_True;
1136 // return Standard_False;
1139 //==================================================================
1140 //Function : HasFirstRestriction
1142 //==================================================================
1143 Standard_Boolean GeomFill_LocationGuide::HasFirstRestriction() const
1145 return Standard_False;
1148 //==================================================================
1149 //Function : HasLastRestriction
1151 //==================================================================
1152 Standard_Boolean GeomFill_LocationGuide::HasLastRestriction() const
1154 return Standard_False;
1157 //==================================================================
1158 //Function : TraceNumber
1160 //==================================================================
1161 Standard_Integer GeomFill_LocationGuide::TraceNumber() const
1166 //==================================================================
1167 //Function : ErrorStatus
1169 //==================================================================
1170 GeomFill_PipeError GeomFill_LocationGuide::ErrorStatus() const
1175 //==================================================================
1176 //Function:NbIntervals
1178 //==================================================================
1179 Standard_Integer GeomFill_LocationGuide::NbIntervals
1180 (const GeomAbs_Shape S) const
1182 Standard_Integer Nb_Sec, Nb_Law;
1183 Nb_Sec = myTrimmed->NbIntervals(S);
1184 Nb_Law = myLaw->NbIntervals(S);
1189 else if (Nb_Law==1) {
1193 TColStd_Array1OfReal IntC(1, Nb_Sec+1);
1194 TColStd_Array1OfReal IntL(1, Nb_Law+1);
1195 TColStd_SequenceOfReal Inter;
1196 myTrimmed->Intervals(IntC, S);
1197 myLaw->Intervals(IntL, S);
1199 GeomLib::FuseIntervals( IntC, IntL, Inter, Precision::PConfusion()*0.99);
1200 return Inter.Length()-1;
1204 //==================================================================
1205 //Function:Intervals
1207 //==================================================================
1208 void GeomFill_LocationGuide::Intervals(TColStd_Array1OfReal& T,
1209 const GeomAbs_Shape S) const
1211 Standard_Integer Nb_Sec, Nb_Law;
1212 Nb_Sec = myTrimmed->NbIntervals(S);
1213 Nb_Law = myLaw->NbIntervals(S);
1216 myLaw->Intervals(T, S);
1219 else if (Nb_Law==1) {
1220 myTrimmed->Intervals(T, S);
1224 TColStd_Array1OfReal IntC(1, Nb_Sec+1);
1225 TColStd_Array1OfReal IntL(1, Nb_Law+1);
1226 TColStd_SequenceOfReal Inter;
1227 myTrimmed->Intervals(IntC, S);
1228 myLaw->Intervals(IntL, S);
1230 GeomLib::FuseIntervals(IntC, IntL, Inter, Precision::PConfusion()*0.99);
1231 for (Standard_Integer ii=1; ii<=Inter.Length(); ii++)
1235 //==================================================================
1236 //Function:SetInterval
1238 //==================================================================
1239 void GeomFill_LocationGuide::SetInterval(const Standard_Real First,
1240 const Standard_Real Last)
1242 myLaw->SetInterval(First, Last);
1243 myTrimmed = myCurve->Trim(First, Last, 0);
1245 //==================================================================
1246 //Function: GetInterval
1248 //==================================================================
1249 void GeomFill_LocationGuide::GetInterval(Standard_Real& First,
1250 Standard_Real& Last) const
1252 First = myTrimmed->FirstParameter();
1253 Last = myTrimmed->LastParameter();
1256 //==================================================================
1257 //Function: GetDomain
1259 //==================================================================
1260 void GeomFill_LocationGuide::GetDomain(Standard_Real& First,
1261 Standard_Real& Last) const
1263 First = myCurve->FirstParameter();
1264 Last = myCurve->LastParameter();
1267 //==================================================================
1268 //function : SetTolerance
1270 //==================================================================
1271 void GeomFill_LocationGuide::SetTolerance(const Standard_Real Tol3d,
1272 const Standard_Real )
1274 TolRes(1) = myGuide->Resolution(Tol3d);
1275 Resolution(1, Tol3d, TolRes(2), TolRes(3));
1279 //==================================================================
1280 //function : Resolution
1281 //purpose : A definir
1282 //==================================================================
1283 //void GeomFill_LocationGuide::Resolution (const Standard_Integer Index,
1284 void GeomFill_LocationGuide::Resolution (const Standard_Integer ,
1285 const Standard_Real Tol,
1286 Standard_Real& TolU,
1287 Standard_Real& TolV) const
1293 //==================================================================
1294 //Function:GetMaximalNorm
1295 //Purpose : On suppose les triedres normes => return 1
1296 //==================================================================
1297 Standard_Real GeomFill_LocationGuide::GetMaximalNorm()
1302 //==================================================================
1303 //Function:GetAverageLaw
1305 //==================================================================
1306 void GeomFill_LocationGuide::GetAverageLaw(gp_Mat& AM,
1309 Standard_Integer ii;
1310 Standard_Real U, delta;
1311 gp_Vec V, V1, V2, V3;
1313 myLaw->GetAverageLaw(V1, V2, V3);
1314 AM.SetCols(V1.XYZ(), V2.XYZ(), V3.XYZ());
1316 AV.SetCoord(0., 0., 0.);
1317 delta = (myTrimmed->LastParameter() - myTrimmed->FirstParameter())/10;
1318 U = myTrimmed->FirstParameter();
1319 for (ii=0; ii<=myNbPts; ii++, U+=delta) {
1320 V.SetXYZ( myTrimmed->Value(U).XYZ() );
1323 AV = AV/(myNbPts+1);
1327 //==================================================================
1328 //Function : Section
1330 //==================================================================
1331 Handle(Geom_Curve) GeomFill_LocationGuide::Section() const
1333 return mySec->ConstantSection();
1336 //==================================================================
1339 //==================================================================
1340 Handle(Adaptor3d_HCurve) GeomFill_LocationGuide::Guide() const
1345 //==================================================================
1346 //Function : IsRotation
1348 //==================================================================
1349 // Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& Error) const
1350 Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& ) const
1352 return Standard_False;
1355 //==================================================================
1356 //Function : Rotation
1358 //==================================================================
1359 // void GeomFill_LocationGuide::Rotation(gp_Pnt& Centre) const
1360 void GeomFill_LocationGuide::Rotation(gp_Pnt& ) const
1362 Standard_NotImplemented::Raise("GeomFill_LocationGuide::Rotation");
1365 //==================================================================
1366 //Function : IsTranslation
1368 //==================================================================
1369 // Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& Error) const
1370 Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& ) const
1372 return Standard_False;
1375 //==================================================================
1377 //Purpose : recherche par interpolation d'une valeur initiale
1378 //==================================================================
1379 void GeomFill_LocationGuide::InitX(const Standard_Real Param) const
1382 Standard_Integer Ideb = 1, Ifin = myPoles2d->RowLength(), Idemi;
1383 Standard_Real Valeur, t1, t2;
1386 Valeur = myPoles2d->Value(1, Ideb).X();
1387 if (Param == Valeur) {
1391 Valeur = myPoles2d->Value(1, Ifin).X();
1392 if (Param == Valeur) {
1396 while ( Ideb+1 != Ifin) {
1397 Idemi = (Ideb+Ifin)/2;
1398 Valeur = myPoles2d->Value(1, Idemi).X();
1399 if (Valeur < Param) {
1403 if ( Valeur > Param) { Ifin = Idemi;}
1411 t1 = myPoles2d->Value(1,Ideb).X();
1412 t2 = myPoles2d->Value(1,Ifin).X();
1413 Standard_Real diff = t2-t1;
1415 Standard_Real W1, W2;
1416 W1 = myPoles2d->Value(1,Ideb).Coord(2);
1417 W2 = myPoles2d->Value(1,Ifin).Coord(2);
1418 const gp_Pnt2d& P1 = myPoles2d->Value(2, Ideb);
1419 const gp_Pnt2d& P2 = myPoles2d->Value(2, Ifin);
1422 Standard_Real b = (Param-t1) / diff,
1423 a = (t2-Param) / diff;
1424 X(1) = a * W1 + b * W2;
1425 X(2) = a * P1.Coord(1) + b * P2.Coord(1); // angle
1426 X(3) = a * P1.Coord(2) + b * P2.Coord(2); // param isov
1430 X(2) = (P1.Coord(1) + P2.Coord(1)) /2;
1431 X(3) = (P1.Coord(2) + P2.Coord(2)) /2;
1434 if (myGuide->IsPeriodic()) {
1435 X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(),
1436 myGuide->LastParameter());
1438 X(2) = ElCLib::InPeriod(X(2), 0, 2*M_PI);
1439 if (mySec->IsUPeriodic()) {
1440 X(3) = ElCLib::InPeriod(X(3), Uf, Ul);
1445 //==================================================================
1446 //Function : SetOrigine
1447 //Purpose : utilise pour ACR dans le cas ou la trajectoire est multi-edges
1448 //==================================================================
1449 void GeomFill_LocationGuide::SetOrigine(const Standard_Real Param1,
1450 const Standard_Real Param2)
1452 OrigParam1 = Param1;
1453 OrigParam2 = Param2;
1456 //==================================================================
1457 //Function : ComputeAutomaticLaw
1459 //==================================================================
1460 GeomFill_PipeError GeomFill_LocationGuide::ComputeAutomaticLaw(Handle(TColgp_HArray1OfPnt2d)& ParAndRad) const
1464 Standard_Integer ii;
1467 GeomFill_PipeError theStatus = GeomFill_PipeOk;
1469 Standard_Real f = myCurve->FirstParameter();
1470 Standard_Real l = myCurve->LastParameter();
1472 ParAndRad = new TColgp_HArray1OfPnt2d(1, myNbPts);
1473 for (ii = 1; ii <= myNbPts; ii++)
1475 t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
1478 Standard_Boolean Ok = myLaw->D0(t, T, N, B);
1481 theStatus = myLaw->ErrorStatus();
1484 gp_Pnt PointOnGuide = myLaw->CurrentPointOnGuide();
1485 Standard_Real CurWidth = P.Distance(PointOnGuide);
1487 gp_Pnt2d aParamWithRadius(t, CurWidth);
1488 ParAndRad->SetValue(ii, aParamWithRadius);