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.
18 #include <Adaptor3d_HCurve.hxx>
19 #include <Adaptor3d_HSurface.hxx>
20 #include <Adaptor3d_Surface.hxx>
22 #include <Extrema_ExtCS.hxx>
23 #include <Extrema_POnSurf.hxx>
24 #include <Geom_BSplineCurve.hxx>
25 #include <Geom_Curve.hxx>
26 #include <Geom_Surface.hxx>
27 #include <Geom_SurfaceOfRevolution.hxx>
28 #include <Geom_TrimmedCurve.hxx>
29 #include <GeomAdaptor.hxx>
30 #include <GeomAdaptor_HCurve.hxx>
31 #include <GeomAdaptor_HSurface.hxx>
32 #include <GeomFill_FunctionGuide.hxx>
33 #include <GeomFill_LocationGuide.hxx>
34 #include <GeomFill_LocationLaw.hxx>
35 #include <GeomFill_SectionLaw.hxx>
36 #include <GeomFill_SectionPlacement.hxx>
37 #include <GeomFill_TrihedronWithGuide.hxx>
38 #include <GeomFill_UniformSection.hxx>
39 #include <GeomLib.hxx>
43 #include <gp_GTrsf.hxx>
46 #include <gp_Pnt2d.hxx>
47 #include <gp_Trsf.hxx>
50 #include <IntCurveSurface_IntersectionPoint.hxx>
51 #include <math_FunctionSetRoot.hxx>
52 #include <math_Gauss.hxx>
53 #include <math_Matrix.hxx>
54 #include <math_Vector.hxx>
55 #include <Precision.hxx>
56 #include <Standard_ConstructionError.hxx>
57 #include <Standard_NotImplemented.hxx>
58 #include <Standard_OutOfRange.hxx>
59 #include <Standard_Type.hxx>
60 #include <TColgp_HArray1OfPnt.hxx>
61 #include <TColStd_HArray1OfInteger.hxx>
62 #include <TColStd_HArray1OfReal.hxx>
64 IMPLEMENT_STANDARD_RTTIEXT(GeomFill_LocationGuide,GeomFill_LocationLaw)
67 static Standard_Integer Affich = 0;
68 #include <Approx_Curve3d.hxx>
69 #include <DrawTrSurf.hxx>
70 #include <GeomFill_TrihedronWithGuide.hxx>
73 //=======================================================================
74 //function : TraceRevol
75 //purpose : Trace la surface de revolution (Debug)
76 //=======================================================================
78 static void TraceRevol(const Standard_Real t,
79 const Standard_Real s,
80 const Handle(GeomFill_TrihedronWithGuide)& Law,
81 const Handle(GeomFill_SectionLaw)& Section,
82 const Handle(Adaptor3d_HCurve)& Curve,
88 gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX());
93 gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ());
96 gp_Dir D = M.Column(3);
97 gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion
99 // calculer transfo entre triedre et Oxyz
103 Transfo.SetTransformation(N3, Rep);
105 // transformer la section
106 Standard_Real f, l,e=1.e-7;
107 Handle (Geom_Curve) S, C;
109 if (Section->IsConstant(e)) {
110 C = Section->ConstantSection();
113 Standard_Integer NbPoles, NbKnots, Deg;
114 Section->SectionShape(NbPoles, NbKnots, Deg);
115 TColStd_Array1OfInteger Mult(1,NbKnots);
116 Section->Mults( Mult);
117 TColStd_Array1OfReal Knots(1,NbKnots);
118 Section->Knots(Knots);
119 TColgp_Array1OfPnt Poles(1, NbPoles);
120 TColStd_Array1OfReal Weights(1, NbPoles);
121 Section->D0(s, Poles, Weights);
122 if (Section->IsRational())
123 C = new (Geom_BSplineCurve)
124 (Poles, Weights, Knots, Mult ,
125 Deg, Section->IsUPeriodic());
127 C = new (Geom_BSplineCurve)
129 Deg, Section->IsUPeriodic());
133 f = C->FirstParameter();
134 l = C->LastParameter();
135 S = new (Geom_TrimmedCurve) (C, f, l);
136 S->Transform(Transfo);
138 // Surface de revolution
139 Handle (Geom_Surface) Revol = new(Geom_SurfaceOfRevolution) (S, Ax);
140 cout << "Surf Revol at parameter t = " << t << endl;
143 Standard_CString aName = "TheRevol" ;
144 DrawTrSurf::Set(aName,Revol);
149 //==================================================================
150 //Function: InGoodPeriod
151 //Purpose : Recadre un paramtere
152 //==================================================================
153 static void InGoodPeriod(const Standard_Real Prec,
154 const Standard_Real Period,
155 Standard_Real& Current)
157 Standard_Real Diff=Current-Prec;
158 Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period);
159 Current -= nb*Period;
161 if (Diff > Period/2) Current -= Period;
162 else if (Diff < -Period/2) Current += Period;
165 //==================================================================
166 //Function: GeomFill_LocationGuide
167 //Purpose : constructor
168 //==================================================================
169 GeomFill_LocationGuide::
170 GeomFill_LocationGuide (const Handle(GeomFill_TrihedronWithGuide)& Triedre)
171 : TolRes(1,3), Inf(1,3,0.), Sup(1,3,0.),
172 X(1,3), R(1,3), myStatus(GeomFill_PipeOk)
175 myLaw = Triedre; // loi de triedre
176 mySec.Nullify(); // loi de section
178 myFirstS = myLastS = -505e77;
180 myNbPts = 21; // nb points pour les calculs
181 myGuide = myLaw->Guide(); // courbe guide
182 if (!myGuide->IsPeriodic()) {
183 Standard_Real f, l, delta;
184 f = myGuide->FirstParameter();
185 l = myGuide->LastParameter();
189 myGuide = myGuide->Trim(f,l,delta*1.e-7); // courbe guide
192 myPoles2d = new (TColgp_HArray2OfPnt2d)(1, 2, 1, myNbPts);
193 rotation = Standard_False; // contact ou non
194 OrigParam1 = 0; // param pour ACR quand trajectoire
195 OrigParam2 = 1; // et guide pas meme sens de parcourt
197 WithTrans = Standard_False;
201 Approx_Curve3d approx(myGuide, 1.e-4,
203 15+myGuide->NbIntervals(GeomAbs_CN),
205 if (approx.HasResult()) {
206 Standard_CString aName = "TheGuide" ;
207 DrawTrSurf::Set(aName, approx.Curve());
213 //==================================================================
214 //Function: SetRotation
215 //Purpose : init et force la Rotation
216 //==================================================================
217 void GeomFill_LocationGuide::SetRotation(const Standard_Real PrecAngle,
218 Standard_Real& LastAngle)
220 if (myCurve.IsNull())
221 throw Standard_ConstructionError(
222 "GeomFill_LocationGuide::The path is not setted !!");
225 gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX());
229 Standard_Integer ii, Deg;
230 Standard_Boolean isconst, israt=Standard_False;
231 Standard_Real t, v,w, OldAngle=0, Angle, DeltaG, Diff;
232 Standard_Real CurAngle = PrecAngle, a1/*, a2*/;
234 Handle(Geom_SurfaceOfRevolution) Revol; // surface de revolution
235 Handle(GeomAdaptor_HSurface) Pl; // = Revol
236 Handle(Geom_TrimmedCurve) S;
237 IntCurveSurface_IntersectionPoint PInt; // intersection guide/Revol
238 Handle(TColStd_HArray1OfInteger) Mult;
239 Handle(TColStd_HArray1OfReal) Knots, Weights;
240 Handle(TColgp_HArray1OfPnt) Poles;
243 Standard_Real U=0, UPeriod=0;
244 Standard_Real f = myCurve->FirstParameter();
245 Standard_Real l = myCurve->LastParameter();
246 Standard_Boolean Ok, uperiodic = mySec->IsUPeriodic();
248 DeltaG = (myGuide->LastParameter() - myGuide->FirstParameter())/5;
249 Handle(Geom_Curve) mySection;
250 Standard_Real Tol =1.e-9;
252 Standard_Integer NbPoles, NbKnots;
253 mySec->SectionShape(NbPoles, NbKnots, Deg);
256 if (mySec->IsConstant(Tol)) {
257 mySection = mySec->ConstantSection();
258 Uf = mySection->FirstParameter();
259 Ul = mySection->LastParameter();
261 isconst = Standard_True;
264 isconst = Standard_False;
265 israt = mySec->IsRational();
266 Mult = new (TColStd_HArray1OfInteger) (1, NbKnots);
267 mySec->Mults( Mult->ChangeArray1());
268 Knots = new (TColStd_HArray1OfReal) (1, NbKnots);
269 mySec->Knots(Knots->ChangeArray1());
270 Poles = new (TColgp_HArray1OfPnt) (1, NbPoles);
271 Weights = new (TColStd_HArray1OfReal) (1, NbPoles);
272 Uf = Knots->Value(1);
273 Ul = Knots->Value(NbKnots);
278 // Standard_Integer bid1, bid2, NbK;
279 Delta = myGuide->LastParameter() - myGuide->FirstParameter();
280 Inf(1) = myGuide->FirstParameter() - Delta/10;
281 Sup(1) = myGuide->LastParameter() + Delta/10;
287 Inf(3) = Uf - Delta/10;
288 Sup(3) = Ul + Delta/10;
291 if (uperiodic) UPeriod = Ul-Uf;
293 for (ii=1; ii<=myNbPts; ii++) {
294 t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
297 Ok = myLaw->D0(t, T, N, B);
299 myStatus = myLaw->ErrorStatus();
300 return; //Y a rien a faire.
304 gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ());
308 gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion
310 // calculer transfo entre triedre et Oxyz
314 Transfo.SetTransformation(N3, Rep);
316 // transformer la section
318 U = myFirstS + (t-myCurve->FirstParameter())*ratio;
319 mySec->D0(U, Poles->ChangeArray1(), Weights->ChangeArray1());
321 mySection = new (Geom_BSplineCurve)
326 Deg, mySec->IsUPeriodic());
328 mySection = new (Geom_BSplineCurve)
332 Deg, mySec->IsUPeriodic());
333 S = new (Geom_TrimmedCurve) (mySection, Uf, Ul);
336 S = new (Geom_TrimmedCurve)
337 (Handle(Geom_Curve)::DownCast(mySection->Copy()), Uf, Ul);
339 S->Transform(Transfo);
341 // Surface de revolution
342 Revol = new(Geom_SurfaceOfRevolution) (S, Ax);
344 GeomAdaptor_Surface GArevol(Revol);
345 Extrema_ExtCS DistMini(myGuide->Curve(), GArevol,
346 Precision::Confusion(), Precision::Confusion());
349 Standard_Real theU = 0., theV = 0.;
351 if (!DistMini.IsDone() || DistMini.NbExt() == 0) {
353 cout <<"LocationGuide : Pas d'intersection"<<endl;
354 TraceRevol(t, U, myLaw, mySec, myCurve, Trans);
356 Standard_Boolean SOS=Standard_False;
358 // Intersection de secour entre surf revol et guide
360 X(1) = myPoles2d->Value(1,ii-1).Y();
361 X(2) = myPoles2d->Value(2,ii-1).X();
362 X(3) = myPoles2d->Value(2,ii-1).Y();
363 GeomFill_FunctionGuide E (mySec, myGuide, U);
364 E.SetParam(U, P, T.XYZ(), N.XYZ());
365 // resolution => angle
366 math_FunctionSetRoot Result(E, TolRes);
367 Result.Perform(E, X, Inf, Sup);
369 if (Result.IsDone() &&
370 (Result.FunctionSetErrors().Norm() < TolRes(1)*TolRes(1)) ) {
372 cout << "Ratrappage Reussi !" << endl;
377 PInt.SetValues(P, RR(2), RR(3), RR(1), IntCurveSurface_Out);
383 cout << "Echec du Ratrappage !" << endl;
388 myStatus = GeomFill_ImpossibleContact;
392 else { // on prend le point d'intersection
393 // d'angle le plus proche de P
395 Standard_Real MinDist = RealLast();
396 Standard_Integer jref = 0;
397 for (Standard_Integer j = 1; j <= DistMini.NbExt(); j++)
399 Standard_Real aDist = DistMini.SquareDistance(j);
406 MinDist = Sqrt(MinDist);
407 DistMini.Points(jref, Pc, Ps);
409 Ps.Parameter(theU, theV);
412 InGoodPeriod (CurAngle, 2*M_PI, a1);
419 Diff = w - myPoles2d->Value(1, ii-1).Y();
420 if (Abs(Diff) > DeltaG) {
421 if (myGuide->IsPeriodic()) {
422 InGoodPeriod (myPoles2d->Value(1, ii-1).Y(),
423 myGuide->Period(), w);
424 Diff = w - myPoles2d->Value(1, ii-1).Y();
429 if (Abs(Diff) > DeltaG) {
430 cout << "Location :: Diff on Guide : " <<
435 //Recadrage de l'angle.
439 Diff = Angle - OldAngle;
440 if (Abs(Diff) > M_PI) {
441 InGoodPeriod (OldAngle, 2*M_PI, Angle);
442 Diff = Angle - OldAngle;
445 if (Abs(Diff) > M_PI/4) {
446 cout << "Diff d'angle trop grand !!" << endl;
457 InGoodPeriod (myPoles2d->Value(2, ii-1).Y(), UPeriod, v);
459 Diff = v - myPoles2d->Value(2, ii-1).Y();
461 if (Abs(Diff) > (Ul-Uf)/(2+NbKnots)) {
462 cout << "Diff sur section trop grand !!" << endl;
467 p1.SetCoord(t, w); // on stocke les parametres
468 p2.SetCoord(Angle , v);
470 myPoles2d->SetValue(1, ii, p1);
471 myPoles2d->SetValue(2, ii, p2);
475 LastAngle = CurAngle;
476 rotation = Standard_True; //C'est pret !
480 //==================================================================
482 //Purpose : init loi de section et force la Rotation
483 //==================================================================
484 void GeomFill_LocationGuide::Set(const Handle(GeomFill_SectionLaw)& Section,
485 const Standard_Boolean rotat,
486 const Standard_Real SFirst,
487 const Standard_Real SLast,
488 const Standard_Real PrecAngle,
489 Standard_Real& LastAngle)
491 myStatus = GeomFill_PipeOk;
494 LastAngle = PrecAngle;
495 if (myCurve.IsNull())
498 ratio = (SLast-SFirst) / (myCurve->LastParameter() -
499 myCurve->FirstParameter());
502 if (rotat) SetRotation(PrecAngle, LastAngle);
503 else rotation = Standard_False;
506 //==================================================================
507 //Function: EraseRotation
508 //Purpose : Supprime la Rotation
509 //==================================================================
510 void GeomFill_LocationGuide:: EraseRotation()
512 rotation = Standard_False;
513 if (myStatus == GeomFill_ImpossibleContact) myStatus = GeomFill_PipeOk;
516 //==================================================================
519 //==================================================================
520 Handle(GeomFill_LocationLaw) GeomFill_LocationGuide::Copy() const
523 Handle(GeomFill_TrihedronWithGuide) L;
524 L = Handle(GeomFill_TrihedronWithGuide)::DownCast(myLaw->Copy());
525 Handle(GeomFill_LocationGuide) copy = new
526 (GeomFill_LocationGuide) (L);
527 copy->SetOrigine(OrigParam1, OrigParam2);
528 copy->Set(mySec, rotation, myFirstS, myLastS,
529 myPoles2d->Value(1,1).X(), la);
530 copy->SetTrsf(Trans);
536 //==================================================================
538 //Purpose : Calcul des poles sur la surface d'arret (intersection
539 // courbe guide / surface de revolution en myNbPts points)
540 //==================================================================
541 void GeomFill_LocationGuide::SetCurve(const Handle(Adaptor3d_HCurve)& C)
543 Standard_Real LastAngle;
547 if (!myCurve.IsNull()){
549 myLaw->Origine(OrigParam1, OrigParam2);
550 myStatus = myLaw->ErrorStatus();
552 if (rotation) SetRotation(myPoles2d->Value(1,1).X(), LastAngle);
556 //==================================================================
558 //Purpose : return the trajectoire
559 //==================================================================
560 const Handle(Adaptor3d_HCurve)& GeomFill_LocationGuide::GetCurve() const
565 //==================================================================
568 //==================================================================
569 void GeomFill_LocationGuide::SetTrsf(const gp_Mat& Transfo)
575 WithTrans = Standard_False; // Au cas ou Trans = I
576 for (Standard_Integer ii=1; ii<=3 && !WithTrans ; ii++)
577 for (Standard_Integer jj=1; jj<=3 && !WithTrans; jj++)
578 if (Abs(Aux.Value(ii, jj)) > 1.e-14) WithTrans = Standard_True;
581 //==================================================================
584 //==================================================================
585 Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param,
593 myCurve->D0(Param, P);
595 Ok = myLaw->D0(Param, T, N, B);
597 myStatus = myLaw->ErrorStatus();
600 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
607 Standard_Real U = myFirstS +
608 (Param-myCurve->FirstParameter())*ratio;
609 // initialisations germe
612 Standard_Integer Iter = 100;
618 // Intersection entre surf revol et guide
620 GeomFill_FunctionGuide E (mySec, myGuide, U);
621 E.SetParam(Param, P, t, n);
622 // resolution => angle
623 math_FunctionSetRoot Result(E, TolRes, Iter);
624 Result.Perform(E, X, Inf, Sup);
626 if (Result.IsDone()) {
632 Rot.SetRotation(t, R(2));
640 cout << "LocationGuide::D0 : No Result !"<<endl;
641 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
643 myStatus = GeomFill_ImpossibleContact;
644 return Standard_False;
648 return Standard_True;
651 //==================================================================
653 //Purpose : calcul de l'intersection (C0) surface revol / guide
654 //==================================================================
655 Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param,
658 // TColgp_Array1OfPnt2d& Poles2d)
659 TColgp_Array1OfPnt2d& )
665 myCurve->D0(Param, P);
667 Ok = myLaw->D0(Param, T, N, B);
669 myStatus = myLaw->ErrorStatus();
672 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
679 //initialisation du germe
681 Standard_Integer Iter = 100;
687 // equation d'intersection entre surf revol et guide => angle
688 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
689 (Param-myCurve->FirstParameter())*ratio);
690 E.SetParam(Param, P, t, n);
693 math_FunctionSetRoot Result(E, TolRes, Iter);
694 Result.Perform(E, X, Inf, Sup);
696 if (Result.IsDone()) {
702 Rot.SetRotation(t, R(2));
712 Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter());
713 cout << "LocationGuide::D0 : No Result !"<<endl;
714 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
716 myStatus = GeomFill_ImpossibleContact;
717 return Standard_False;
721 return Standard_True;
725 //==================================================================
727 //Purpose : calcul de l'intersection (C1) surface revol / guide
728 //==================================================================
729 Standard_Boolean GeomFill_LocationGuide::D1(const Standard_Real Param,
734 // TColgp_Array1OfPnt2d& Poles2d,
735 TColgp_Array1OfPnt2d& ,
736 // TColgp_Array1OfVec2d& DPoles2d)
737 TColgp_Array1OfVec2d& )
739 // gp_Vec T, N, B, DT, DN, DB, T0, N0, B0;
740 gp_Vec T, N, B, DT, DN, DB;
745 myCurve->D1(Param, P, DV);
747 Ok = myLaw->D1(Param, T, DT, N, DN, B, DB);
749 myStatus = myLaw->ErrorStatus();
752 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
753 DM.SetCols(DN.XYZ() , DB.XYZ(), DT.XYZ());
761 return Standard_False;
764 Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter());
766 myCurve->FirstParameter() ;
769 // initialisation du germe
772 Standard_Integer Iter = 100;
773 gp_XYZ t,b,n, dt, db, dn;
781 // equation d'intersection surf revol / guide => angle
782 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
783 (Param-myCurve->FirstParameter())*ratio);
784 E.SetParam(Param, P, t, n);
787 math_FunctionSetRoot Result(E, X, TolRes,
792 // solution de la fonction
795 // derivee de la fonction
796 math_Vector DEDT(1,3);
797 E.DerivT(R, DV.XYZ(), dt, DEDT); // dE/dt => DEDT
799 math_Vector DSDT (1,3,0);
800 math_Matrix DEDX (1,3,1,3,0);
801 E.Derivatives(R, DEDX); // dE/dx au point R => DEDX
803 // resolution du syst. : DEDX*DSDT = -DEDT
807 Ga.Solve (DEDT.Opposite(), DSDT);// resolution du syst.
811 cout << "DEDX = " << DEDX << endl;
812 cout << "DEDT = " << DEDT << endl;
814 throw Standard_ConstructionError(
815 "LocationGuide::D1 : No Result dans la derivee");
818 // transformation = rotation
820 Rot.SetRotation(t, R(2));
824 M.SetCols(n*Rot, b*Rot, t);
826 // transfo entre triedre (en Q) et Oxyz
827 gp_Ax3 Rep(gp::Origin(),gp::DZ(), gp::DX());
828 gp_Ax3 RepTriedre(gp::Origin(),t,n);
830 Transfo3.SetTransformation(Rep,RepTriedre);
831 // on se place dans Oxyz
832 Transfo3.Transforms(n);
833 Transfo3.Transforms(b);
834 Transfo3.Transforms(dn);
835 Transfo3.Transforms(db);
837 // matrices de rotation et derivees
838 Standard_Real A = R(2);
839 Standard_Real Aprim = DSDT(2);
842 gp_Mat M2 (Cos(A), -Sin(A),0, // rotation autour de T
847 gp_Mat M2prim (-Sin(A), -Cos(A), 0, // derivee rotation autour de T
850 M2prim.Multiply(Aprim);
864 // on repasse dans repere triedre
866 InvTrsf = Transfo3.Inverted();
867 InvTrsf.Transforms(dn);
868 InvTrsf.Transforms(db);
870 DM.SetCols(dn , db , dt);
875 cout << "LocationGuide::D1 : No Result !!"<<endl;
876 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
878 myStatus = GeomFill_ImpossibleContact;
879 return Standard_False;
885 return Standard_True;
889 //==================================================================
891 //Purpose : calcul de l'intersection (C2) surface revol / guide
892 //==================================================================
893 Standard_Boolean GeomFill_LocationGuide::D2(const Standard_Real Param,
900 // TColgp_Array1OfPnt2d& Poles2d,
901 TColgp_Array1OfPnt2d& ,
902 // TColgp_Array1OfVec2d& DPoles2d,
903 TColgp_Array1OfVec2d& ,
904 // TColgp_Array1OfVec2d& D2Poles2d)
905 TColgp_Array1OfVec2d& )
907 gp_Vec T, N, B, DT, DN, DB, D2T, D2N, D2B;
908 // gp_Vec T0, N0, B0, T1, N1, B1;
913 myCurve->D2(Param, P, DV, D2V);
915 Ok = myLaw->D2(Param, T, DT, D2T, N, DN, D2N, B, DB, D2B);
917 myStatus = myLaw->ErrorStatus();
929 return Standard_False;
931 Standard_Real U = myFirstS +
932 (Param-myCurve->FirstParameter())*ratio;
934 math_Vector X(1,3,0);
940 // Standard_Real ETol = 1.e-6;
941 Standard_Integer Iter = 100;
944 // resoudre equation d'intersection entre surf revol et guide => angle
945 GeomFill_FunctionGuide E (mySec, myGuide, myFirstS +
946 (Param-myCurve->FirstParameter())*ratio);
947 E.SetParam(Param, P, T, N);
950 math_FunctionSetRoot Result(E, X, TolRes,
955 Result.Root(R); // solution
957 //gp_Pnt2d p (R(2), R(3)); // point sur la surface (angle, v)
958 //Poles2d.SetValue(1,p);
960 // derivee de la fonction
961 math_Vector DEDT(1,3,0);
962 E.DerivT(Param, Param0, R, R0, DEDT); // dE/dt => DEDT
963 math_Vector DSDT (1,3,0);
964 math_Matrix DEDX (1,3,1,3,0);
965 E.Derivatives(R, DEDX); // dE/dx au point R => DEDX
967 // resolution du syst. lin. : DEDX*DSDT = -DEDT
971 Ga.Solve (DEDT.Opposite(), DSDT); // resolution du syst. lin.
972 //gp_Vec2d dp (DSDT(2), DSDT(3)); // surface
973 //DPoles2d.SetValue(1, dp);
975 else cout <<"LocationGuide::D2 : No Result dans la derivee premiere"<<endl;
978 GeomFill_Tensor D2EDX2(3,3,3);
979 E.Deriv2X(R, D2EDX2); // d2E/dx2
981 math_Vector D2EDT2(1,3,0);
983 // if(Param1 < Param && Param < Param0)
984 E.Deriv2T(Param1, Param, Param0, R1, R, R0, D2EDT2); // d2E/dt2
985 // else if (Param < Param0 && Param0 < Param1)
986 // E.Deriv2T(Param, Param0, Param1, R, R0, R1, D2EDT2); // d2E/dt2
988 // E.Deriv2T(Param0, Param1, Param, R0, R1, R, D2EDT2); // d2E/dt2
990 math_Matrix D2EDTDX(1,3,1,3,0);
991 E.DerivTX(Param, Param0, R, R0, D2EDTDX); // d2E/dtdx
993 math_Vector D2SDT2(1,3,0); // d2s/dt2
994 math_Matrix M1(1,3,1,3,0);
995 D2EDX2.Multiply(DSDT,M1);
997 // resolution du syst. lin.
998 math_Gauss Ga1 (DEDX);
1001 Ga1.Solve ( - M1*DSDT - 2*D2EDTDX*DSDT - D2EDT2 , D2SDT2);
1002 //gp_Vec2d d2p (D2SDT2(2), D2SDT2(3)); // surface
1003 //D2Poles2d.SetValue(1, d2p);
1006 cout <<"LocationGuide::D2 : No Result dans la derivee seconde"<<endl;
1007 myStatus = GeomFill_ImpossibleContact;
1010 //------------------------------------------
1012 //------------------------------------------
1017 Tr.SetRotation(Axe, R(2));
1027 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
1029 //------------------------------------------
1030 // derivees de la rotation
1032 //-----------------------------------------
1033 gp_Vec db,dn,db3,dn3;
1037 gp_Vec db1,dn1,db2,dn2;
1039 //transfo entre triedre et Oxyz
1040 gp_Ax3 RepTriedre4(Q,D,B2);
1042 Transfo3.SetTransformation(Rep,RepTriedre4);
1044 //on passe dans le repere du triedre
1045 n.Transform(Transfo3);
1046 b.Transform(Transfo3);
1047 n2.Transform(Transfo3);
1048 b2.Transform(Transfo3);
1049 dn.Transform(Transfo3);
1050 db.Transform(Transfo3);
1051 dn3.Transform(Transfo3);
1052 db3.Transform(Transfo3);
1053 D2N.Transform(Transfo3);
1054 D2B.Transform(Transfo3);
1056 //matrices de rotation et derivees
1057 Standard_Real A = R(2);
1058 Standard_Real Aprim = DSDT(2);
1059 Standard_Real Asec = D2SDT2(2);
1061 gp_Mat M2 (Cos(A),-Sin(A),0, // rotation autour de T
1065 gp_Mat M2prim (-Sin(A),-Cos(A),0, // derivee 1ere rotation autour de T
1069 gp_Mat M2sec (-Cos(A), Sin(A), 0, // derivee 2nde rotation autour de T
1070 -Sin(A), -Cos(A), 0,
1072 M2sec.Multiply(Aprim*Aprim);
1073 gp_Mat M2p = M2prim.Multiplied(Asec);
1076 M2prim.Multiply(Aprim);
1080 Rot.SetValues(M2(1,1),M2(1,2),M2(1,3),0,
1081 M2(2,1),M2(2,2),M2(2,3),0,
1082 M2(3,1),M2(3,2),M2(3,3),0,
1085 DRot.SetValues(M2prim(1,1),M2prim(1,2),M2prim(1,3),0,
1086 M2prim(2,1),M2prim(2,2),M2prim(2,3),0,
1087 M2prim(3,1),M2prim(3,2),M2prim(3,3),0,
1091 D2Rot.SetValues(M2sec(1,1),M2sec(1,2),M2sec(1,3),0,
1092 M2sec(2,1),M2sec(2,2),M2sec(2,3),0,
1093 M2sec(3,1),M2sec(3,2),M2sec(3,3),0,
1104 dn1.Transform(Transfo3.Inverted());
1105 db1.Transform(Transfo3.Inverted());
1107 DM.SetCols(dn1.XYZ(), db1.XYZ(), DT.XYZ());
1112 dn3.Transform(DRot);
1113 db3.Transform(DRot);
1114 n2.Transform(D2Rot);
1115 b2.Transform(D2Rot);
1116 dn2 = n2 + 2*dn3 + D2N;
1117 db2 = b2 + 2*db3 + D2B;
1118 dn2.Transform(Transfo3.Inverted());
1119 db2.Transform(Transfo3.Inverted());
1121 D2M.SetCols(dn2.XYZ(), db2.XYZ(), D2T.XYZ());
1126 cout << "LocationGuide::D2 : No Result !!" <<endl;
1127 TraceRevol(Param, U, myLaw, mySec, myCurve, Trans);
1129 return Standard_False;
1135 M.SetCols(N.XYZ(), B.XYZ(), T.XYZ());
1136 DM.SetCols(DN.XYZ(), DB.XYZ(), DT.XYZ());
1137 D2M.SetCols(D2N.XYZ(), D2B.XYZ(), D2T.XYZ());
1140 return Standard_True;
1141 // return Standard_False;
1144 //==================================================================
1145 //Function : HasFirstRestriction
1147 //==================================================================
1148 Standard_Boolean GeomFill_LocationGuide::HasFirstRestriction() const
1150 return Standard_False;
1153 //==================================================================
1154 //Function : HasLastRestriction
1156 //==================================================================
1157 Standard_Boolean GeomFill_LocationGuide::HasLastRestriction() const
1159 return Standard_False;
1162 //==================================================================
1163 //Function : TraceNumber
1165 //==================================================================
1166 Standard_Integer GeomFill_LocationGuide::TraceNumber() const
1171 //==================================================================
1172 //Function : ErrorStatus
1174 //==================================================================
1175 GeomFill_PipeError GeomFill_LocationGuide::ErrorStatus() const
1180 //==================================================================
1181 //Function:NbIntervals
1183 //==================================================================
1184 Standard_Integer GeomFill_LocationGuide::NbIntervals
1185 (const GeomAbs_Shape S) const
1187 Standard_Integer Nb_Sec, Nb_Law;
1188 Nb_Sec = myTrimmed->NbIntervals(S);
1189 Nb_Law = myLaw->NbIntervals(S);
1194 else if (Nb_Law==1) {
1198 TColStd_Array1OfReal IntC(1, Nb_Sec+1);
1199 TColStd_Array1OfReal IntL(1, Nb_Law+1);
1200 TColStd_SequenceOfReal Inter;
1201 myTrimmed->Intervals(IntC, S);
1202 myLaw->Intervals(IntL, S);
1204 GeomLib::FuseIntervals( IntC, IntL, Inter, Precision::PConfusion()*0.99);
1205 return Inter.Length()-1;
1209 //==================================================================
1210 //Function:Intervals
1212 //==================================================================
1213 void GeomFill_LocationGuide::Intervals(TColStd_Array1OfReal& T,
1214 const GeomAbs_Shape S) const
1216 Standard_Integer Nb_Sec, Nb_Law;
1217 Nb_Sec = myTrimmed->NbIntervals(S);
1218 Nb_Law = myLaw->NbIntervals(S);
1221 myLaw->Intervals(T, S);
1224 else if (Nb_Law==1) {
1225 myTrimmed->Intervals(T, S);
1229 TColStd_Array1OfReal IntC(1, Nb_Sec+1);
1230 TColStd_Array1OfReal IntL(1, Nb_Law+1);
1231 TColStd_SequenceOfReal Inter;
1232 myTrimmed->Intervals(IntC, S);
1233 myLaw->Intervals(IntL, S);
1235 GeomLib::FuseIntervals(IntC, IntL, Inter, Precision::PConfusion()*0.99);
1236 for (Standard_Integer ii=1; ii<=Inter.Length(); ii++)
1240 //==================================================================
1241 //Function:SetInterval
1243 //==================================================================
1244 void GeomFill_LocationGuide::SetInterval(const Standard_Real First,
1245 const Standard_Real Last)
1247 myLaw->SetInterval(First, Last);
1248 myTrimmed = myCurve->Trim(First, Last, 0);
1250 //==================================================================
1251 //Function: GetInterval
1253 //==================================================================
1254 void GeomFill_LocationGuide::GetInterval(Standard_Real& First,
1255 Standard_Real& Last) const
1257 First = myTrimmed->FirstParameter();
1258 Last = myTrimmed->LastParameter();
1261 //==================================================================
1262 //Function: GetDomain
1264 //==================================================================
1265 void GeomFill_LocationGuide::GetDomain(Standard_Real& First,
1266 Standard_Real& Last) const
1268 First = myCurve->FirstParameter();
1269 Last = myCurve->LastParameter();
1272 //==================================================================
1273 //function : SetTolerance
1275 //==================================================================
1276 void GeomFill_LocationGuide::SetTolerance(const Standard_Real Tol3d,
1277 const Standard_Real )
1279 TolRes(1) = myGuide->Resolution(Tol3d);
1280 Resolution(1, Tol3d, TolRes(2), TolRes(3));
1284 //==================================================================
1285 //function : Resolution
1286 //purpose : A definir
1287 //==================================================================
1288 //void GeomFill_LocationGuide::Resolution (const Standard_Integer Index,
1289 void GeomFill_LocationGuide::Resolution (const Standard_Integer ,
1290 const Standard_Real Tol,
1291 Standard_Real& TolU,
1292 Standard_Real& TolV) const
1298 //==================================================================
1299 //Function:GetMaximalNorm
1300 //Purpose : On suppose les triedres normes => return 1
1301 //==================================================================
1302 Standard_Real GeomFill_LocationGuide::GetMaximalNorm()
1307 //==================================================================
1308 //Function:GetAverageLaw
1310 //==================================================================
1311 void GeomFill_LocationGuide::GetAverageLaw(gp_Mat& AM,
1314 Standard_Integer ii;
1315 Standard_Real U, delta;
1316 gp_Vec V, V1, V2, V3;
1318 myLaw->GetAverageLaw(V1, V2, V3);
1319 AM.SetCols(V1.XYZ(), V2.XYZ(), V3.XYZ());
1321 AV.SetCoord(0., 0., 0.);
1322 delta = (myTrimmed->LastParameter() - myTrimmed->FirstParameter())/10;
1323 U = myTrimmed->FirstParameter();
1324 for (ii=0; ii<=myNbPts; ii++, U+=delta) {
1325 V.SetXYZ( myTrimmed->Value(U).XYZ() );
1328 AV = AV/(myNbPts+1);
1332 //==================================================================
1333 //Function : Section
1335 //==================================================================
1336 Handle(Geom_Curve) GeomFill_LocationGuide::Section() const
1338 return mySec->ConstantSection();
1341 //==================================================================
1344 //==================================================================
1345 Handle(Adaptor3d_HCurve) GeomFill_LocationGuide::Guide() const
1350 //==================================================================
1351 //Function : IsRotation
1353 //==================================================================
1354 // Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& Error) const
1355 Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& ) const
1357 return Standard_False;
1360 //==================================================================
1361 //Function : Rotation
1363 //==================================================================
1364 // void GeomFill_LocationGuide::Rotation(gp_Pnt& Centre) const
1365 void GeomFill_LocationGuide::Rotation(gp_Pnt& ) const
1367 throw Standard_NotImplemented("GeomFill_LocationGuide::Rotation");
1370 //==================================================================
1371 //Function : IsTranslation
1373 //==================================================================
1374 // Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& Error) const
1375 Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& ) const
1377 return Standard_False;
1380 //==================================================================
1382 //Purpose : recherche par interpolation d'une valeur initiale
1383 //==================================================================
1384 void GeomFill_LocationGuide::InitX(const Standard_Real Param)
1387 Standard_Integer Ideb = 1, Ifin = myPoles2d->RowLength(), Idemi;
1388 Standard_Real Valeur, t1, t2;
1391 Valeur = myPoles2d->Value(1, Ideb).X();
1392 if (Param == Valeur) {
1396 Valeur = myPoles2d->Value(1, Ifin).X();
1397 if (Param == Valeur) {
1401 while ( Ideb+1 != Ifin) {
1402 Idemi = (Ideb+Ifin)/2;
1403 Valeur = myPoles2d->Value(1, Idemi).X();
1404 if (Valeur < Param) {
1408 if ( Valeur > Param) { Ifin = Idemi;}
1416 t1 = myPoles2d->Value(1,Ideb).X();
1417 t2 = myPoles2d->Value(1,Ifin).X();
1418 Standard_Real diff = t2-t1;
1420 Standard_Real W1, W2;
1421 W1 = myPoles2d->Value(1,Ideb).Coord(2);
1422 W2 = myPoles2d->Value(1,Ifin).Coord(2);
1423 const gp_Pnt2d& P1 = myPoles2d->Value(2, Ideb);
1424 const gp_Pnt2d& P2 = myPoles2d->Value(2, Ifin);
1427 Standard_Real b = (Param-t1) / diff,
1428 a = (t2-Param) / diff;
1429 X(1) = a * W1 + b * W2;
1430 X(2) = a * P1.Coord(1) + b * P2.Coord(1); // angle
1431 X(3) = a * P1.Coord(2) + b * P2.Coord(2); // param isov
1435 X(2) = (P1.Coord(1) + P2.Coord(1)) /2;
1436 X(3) = (P1.Coord(2) + P2.Coord(2)) /2;
1439 if (myGuide->IsPeriodic()) {
1440 X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(),
1441 myGuide->LastParameter());
1443 X(2) = ElCLib::InPeriod(X(2), 0, 2*M_PI);
1444 if (mySec->IsUPeriodic()) {
1445 X(3) = ElCLib::InPeriod(X(3), Uf, Ul);
1450 //==================================================================
1451 //Function : SetOrigine
1452 //Purpose : utilise pour ACR dans le cas ou la trajectoire est multi-edges
1453 //==================================================================
1454 void GeomFill_LocationGuide::SetOrigine(const Standard_Real Param1,
1455 const Standard_Real Param2)
1457 OrigParam1 = Param1;
1458 OrigParam2 = Param2;
1461 //==================================================================
1462 //Function : ComputeAutomaticLaw
1464 //==================================================================
1465 GeomFill_PipeError GeomFill_LocationGuide::ComputeAutomaticLaw(Handle(TColgp_HArray1OfPnt2d)& ParAndRad) const
1469 Standard_Integer ii;
1472 GeomFill_PipeError theStatus = GeomFill_PipeOk;
1474 Standard_Real f = myCurve->FirstParameter();
1475 Standard_Real l = myCurve->LastParameter();
1477 ParAndRad = new TColgp_HArray1OfPnt2d(1, myNbPts);
1478 for (ii = 1; ii <= myNbPts; ii++)
1480 t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l;
1483 Standard_Boolean Ok = myLaw->D0(t, T, N, B);
1486 theStatus = myLaw->ErrorStatus();
1489 gp_Pnt PointOnGuide = myLaw->CurrentPointOnGuide();
1490 Standard_Real CurWidth = P.Distance(PointOnGuide);
1492 gp_Pnt2d aParamWithRadius(t, CurWidth);
1493 ParAndRad->SetValue(ii, aParamWithRadius);