Commit | Line | Data |
---|---|---|
b311480e | 1 | // Created on: 1998-07-08 |
2 | // Created by: Stephanie HUMEAU | |
3 | // Copyright (c) 1998-1999 Matra Datavision | |
973c2be1 | 4 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e | 5 | // |
973c2be1 | 6 | // This file is part of Open CASCADE Technology software library. |
b311480e | 7 | // |
d5f74e42 | 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 | |
973c2be1 | 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. | |
b311480e | 13 | // |
973c2be1 | 14 | // Alternatively, this file may be used under the terms of Open CASCADE |
15 | // commercial license or contractual agreement. | |
7fd59977 | 16 | |
7fd59977 | 17 | |
42cf5bc1 | 18 | #include <Adaptor3d_HCurve.hxx> |
7fd59977 | 19 | #include <Adaptor3d_HSurface.hxx> |
7fd59977 | 20 | #include <Adaptor3d_Surface.hxx> |
42cf5bc1 | 21 | #include <ElCLib.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> | |
7fd59977 | 29 | #include <GeomAdaptor.hxx> |
7fd59977 | 30 | #include <GeomAdaptor_HCurve.hxx> |
42cf5bc1 | 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> | |
7fd59977 | 36 | #include <GeomFill_SectionPlacement.hxx> |
42cf5bc1 | 37 | #include <GeomFill_TrihedronWithGuide.hxx> |
38 | #include <GeomFill_UniformSection.hxx> | |
7fd59977 | 39 | #include <GeomLib.hxx> |
42cf5bc1 | 40 | #include <gp.hxx> |
41 | #include <gp_Ax1.hxx> | |
42 | #include <gp_Dir.hxx> | |
43 | #include <gp_GTrsf.hxx> | |
44 | #include <gp_Mat.hxx> | |
45 | #include <gp_Pnt.hxx> | |
46 | #include <gp_Pnt2d.hxx> | |
47 | #include <gp_Trsf.hxx> | |
48 | #include <gp_Vec.hxx> | |
49 | #include <gp_XYZ.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> | |
7fd59977 | 61 | #include <TColStd_HArray1OfInteger.hxx> |
62 | #include <TColStd_HArray1OfReal.hxx> | |
f9032cf2 | 63 | |
92efcf78 | 64 | IMPLEMENT_STANDARD_RTTIEXT(GeomFill_LocationGuide,GeomFill_LocationLaw) |
65 | ||
7fd59977 | 66 | #if DRAW |
67 | static Standard_Integer Affich = 0; | |
68 | #include <Approx_Curve3d.hxx> | |
69 | #include <DrawTrSurf.hxx> | |
ec357c5c | 70 | #include <GeomFill_TrihedronWithGuide.hxx> |
7fd59977 | 71 | #endif |
72 | ||
73 | //======================================================================= | |
74 | //function : TraceRevol | |
75 | //purpose : Trace la surface de revolution (Debug) | |
76 | //======================================================================= | |
0797d9d3 | 77 | #ifdef OCCT_DEBUG |
7fd59977 | 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, | |
83 | const gp_Mat& Trans) | |
84 | ||
85 | { | |
86 | gp_Vec T, N, B; | |
87 | gp_Pnt P; | |
7fd59977 | 88 | gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX()); |
89 | ||
90 | Curve->D0(t, P); | |
96a95605 | 91 | Law->D0(t, T, N, B); |
7fd59977 | 92 | |
93 | gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ()); | |
94 | M *= Trans; | |
95 | ||
96 | gp_Dir D = M.Column(3); | |
97 | gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion | |
98 | ||
99 | // calculer transfo entre triedre et Oxyz | |
100 | gp_Dir N2 = N; | |
101 | gp_Ax3 N3(P,D,N2); | |
102 | gp_Trsf Transfo; | |
103 | Transfo.SetTransformation(N3, Rep); | |
104 | ||
105 | // transformer la section | |
106 | Standard_Real f, l,e=1.e-7; | |
107 | Handle (Geom_Curve) S, C; | |
108 | ||
109 | if (Section->IsConstant(e)) { | |
110 | C = Section->ConstantSection(); | |
111 | } | |
112 | else { | |
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()); | |
126 | else | |
127 | C = new (Geom_BSplineCurve) | |
128 | (Poles, Knots, Mult, | |
129 | Deg, Section->IsUPeriodic()); | |
130 | ||
131 | } | |
132 | ||
133 | f = C->FirstParameter(); | |
134 | l = C->LastParameter(); | |
135 | S = new (Geom_TrimmedCurve) (C, f, l); | |
136 | S->Transform(Transfo); | |
137 | ||
138 | // Surface de revolution | |
139 | Handle (Geom_Surface) Revol = new(Geom_SurfaceOfRevolution) (S, Ax); | |
04232180 | 140 | std::cout << "Surf Revol at parameter t = " << t << std::endl; |
7fd59977 | 141 | |
142 | #if DRAW | |
143 | Standard_CString aName = "TheRevol" ; | |
144 | DrawTrSurf::Set(aName,Revol); | |
145 | #endif | |
146 | } | |
147 | #endif | |
148 | ||
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) | |
156 | { | |
157 | Standard_Real Diff=Current-Prec; | |
158 | Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period); | |
159 | Current -= nb*Period; | |
160 | Diff = Current-Prec; | |
161 | if (Diff > Period/2) Current -= Period; | |
162 | else if (Diff < -Period/2) Current += Period; | |
163 | } | |
164 | ||
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) | |
173 | { | |
174 | TolRes.Init(1.e-6); | |
175 | myLaw = Triedre; // loi de triedre | |
176 | mySec.Nullify(); // loi de section | |
177 | myCurve.Nullify(); | |
178 | myFirstS = myLastS = -505e77; | |
179 | ||
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(); | |
186 | delta = (l-f)/100; | |
187 | f-=delta; | |
188 | l+=delta; | |
189 | myGuide = myGuide->Trim(f,l,delta*1.e-7); // courbe guide | |
190 | }// if | |
191 | ||
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 | |
196 | Trans.SetIdentity(); | |
197 | WithTrans = Standard_False; | |
198 | ||
199 | #if DRAW | |
200 | if (Affich) { | |
201 | Approx_Curve3d approx(myGuide, 1.e-4, | |
202 | GeomAbs_C1, | |
203 | 15+myGuide->NbIntervals(GeomAbs_CN), | |
204 | 14); | |
205 | if (approx.HasResult()) { | |
206 | Standard_CString aName = "TheGuide" ; | |
207 | DrawTrSurf::Set(aName, approx.Curve()); | |
208 | } | |
209 | } | |
210 | #endif | |
211 | } | |
212 | ||
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) | |
219 | { | |
220 | if (myCurve.IsNull()) | |
9775fa61 | 221 | throw Standard_ConstructionError( |
7fd59977 | 222 | "GeomFill_LocationGuide::The path is not setted !!"); |
223 | ||
224 | //repere fixe | |
225 | gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX()); | |
226 | // gp_Pnt P,P1,P2; | |
227 | gp_Pnt P; | |
228 | gp_Vec T,N,B; | |
229 | Standard_Integer ii, Deg; | |
230 | Standard_Boolean isconst, israt=Standard_False; | |
96a95605 | 231 | Standard_Real t, v,w, OldAngle=0, Angle, DeltaG, Diff; |
f9032cf2 | 232 | Standard_Real CurAngle = PrecAngle, a1/*, a2*/; |
7fd59977 | 233 | gp_Pnt2d p1,p2; |
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 | |
7fd59977 | 238 | Handle(TColStd_HArray1OfInteger) Mult; |
239 | Handle(TColStd_HArray1OfReal) Knots, Weights; | |
240 | Handle(TColgp_HArray1OfPnt) Poles; | |
241 | ||
242 | ||
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(); | |
247 | ||
248 | DeltaG = (myGuide->LastParameter() - myGuide->FirstParameter())/5; | |
249 | Handle(Geom_Curve) mySection; | |
250 | Standard_Real Tol =1.e-9; | |
251 | ||
252 | Standard_Integer NbPoles, NbKnots; | |
253 | mySec->SectionShape(NbPoles, NbKnots, Deg); | |
254 | ||
255 | ||
256 | if (mySec->IsConstant(Tol)) { | |
257 | mySection = mySec->ConstantSection(); | |
258 | Uf = mySection->FirstParameter(); | |
259 | Ul = mySection->LastParameter(); | |
260 | ||
261 | isconst = Standard_True; | |
262 | } | |
263 | else { | |
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); | |
274 | } | |
275 | ||
276 | // Bornes de calculs | |
277 | Standard_Real Delta; | |
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; | |
282 | ||
c6541a0c D |
283 | Inf(2) = -M_PI; |
284 | Sup(2) = 3*M_PI; | |
7fd59977 | 285 | |
286 | Delta = Ul - Uf; | |
287 | Inf(3) = Uf - Delta/10; | |
288 | Sup(3) = Ul + Delta/10; | |
289 | ||
290 | // JALONNEMENT | |
7fd59977 | 291 | if (uperiodic) UPeriod = Ul-Uf; |
292 | ||
293 | for (ii=1; ii<=myNbPts; ii++) { | |
294 | t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l; | |
295 | t /= (myNbPts-1); | |
296 | myCurve->D0(t, P); | |
297 | Ok = myLaw->D0(t, T, N, B); | |
298 | if (!Ok) { | |
299 | myStatus = myLaw->ErrorStatus(); | |
300 | return; //Y a rien a faire. | |
301 | } | |
302 | gp_Dir D = T; | |
303 | if (WithTrans) { | |
304 | gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ()); | |
305 | M *= Trans; | |
306 | D = M.Column(3); | |
307 | } | |
308 | gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion | |
309 | ||
310 | // calculer transfo entre triedre et Oxyz | |
311 | gp_Dir N2 = N; | |
312 | gp_Ax3 N3(P,D,N2); | |
313 | gp_Trsf Transfo; | |
314 | Transfo.SetTransformation(N3, Rep); | |
315 | ||
316 | // transformer la section | |
317 | if (! isconst) { | |
318 | U = myFirstS + (t-myCurve->FirstParameter())*ratio; | |
319 | mySec->D0(U, Poles->ChangeArray1(), Weights->ChangeArray1()); | |
320 | if (israt) | |
321 | mySection = new (Geom_BSplineCurve) | |
322 | (Poles->Array1(), | |
323 | Weights->Array1(), | |
324 | Knots->Array1(), | |
325 | Mult->Array1(), | |
326 | Deg, mySec->IsUPeriodic()); | |
327 | else | |
328 | mySection = new (Geom_BSplineCurve) | |
329 | (Poles->Array1(), | |
330 | Knots->Array1(), | |
331 | Mult->Array1(), | |
332 | Deg, mySec->IsUPeriodic()); | |
333 | S = new (Geom_TrimmedCurve) (mySection, Uf, Ul); | |
334 | } | |
335 | else { | |
336 | S = new (Geom_TrimmedCurve) | |
337 | (Handle(Geom_Curve)::DownCast(mySection->Copy()), Uf, Ul); | |
338 | } | |
339 | S->Transform(Transfo); | |
f9032cf2 | 340 | |
7fd59977 | 341 | // Surface de revolution |
342 | Revol = new(Geom_SurfaceOfRevolution) (S, Ax); | |
343 | ||
f9032cf2 | 344 | GeomAdaptor_Surface GArevol(Revol); |
345 | Extrema_ExtCS DistMini(myGuide->Curve(), GArevol, | |
346 | Precision::Confusion(), Precision::Confusion()); | |
347 | Extrema_POnCurv Pc; | |
348 | Extrema_POnSurf Ps; | |
349 | Standard_Real theU = 0., theV = 0.; | |
350 | ||
351 | if (!DistMini.IsDone() || DistMini.NbExt() == 0) { | |
0797d9d3 | 352 | #ifdef OCCT_DEBUG |
04232180 | 353 | std::cout <<"LocationGuide : Pas d'intersection"<<std::endl; |
7fd59977 | 354 | TraceRevol(t, U, myLaw, mySec, myCurve, Trans); |
355 | #endif | |
356 | Standard_Boolean SOS=Standard_False; | |
357 | if (ii>1) { | |
859a47c3 | 358 | // Intersection de secour entre surf revol et guide |
359 | // equation | |
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); | |
368 | ||
369 | if (Result.IsDone() && | |
370 | (Result.FunctionSetErrors().Norm() < TolRes(1)*TolRes(1)) ) { | |
0797d9d3 | 371 | #ifdef OCCT_DEBUG |
04232180 | 372 | std::cout << "Ratrappage Reussi !" << std::endl; |
7fd59977 | 373 | #endif |
859a47c3 | 374 | SOS = Standard_True; |
375 | math_Vector RR(1,3); | |
376 | Result.Root(RR); | |
377 | PInt.SetValues(P, RR(2), RR(3), RR(1), IntCurveSurface_Out); | |
378 | theU = PInt.U(); | |
379 | theV = PInt.V(); | |
380 | } | |
381 | else { | |
0797d9d3 | 382 | #ifdef OCCT_DEBUG |
04232180 | 383 | std::cout << "Echec du Ratrappage !" << std::endl; |
7fd59977 | 384 | #endif |
859a47c3 | 385 | } |
7fd59977 | 386 | } |
387 | if (!SOS) { | |
388 | myStatus = GeomFill_ImpossibleContact; | |
389 | return; | |
390 | } | |
391 | } | |
392 | else { // on prend le point d'intersection | |
393 | // d'angle le plus proche de P | |
f9032cf2 | 394 | |
395 | Standard_Real MinDist = RealLast(); | |
396 | Standard_Integer jref = 0; | |
397 | for (Standard_Integer j = 1; j <= DistMini.NbExt(); j++) | |
398 | { | |
399 | Standard_Real aDist = DistMini.SquareDistance(j); | |
400 | if (aDist < MinDist) | |
401 | { | |
402 | MinDist = aDist; | |
403 | jref = j; | |
404 | } | |
405 | } | |
406 | MinDist = Sqrt(MinDist); | |
407 | DistMini.Points(jref, Pc, Ps); | |
408 | ||
409 | Ps.Parameter(theU, theV); | |
410 | a1 = theU; | |
411 | ||
c6541a0c | 412 | InGoodPeriod (CurAngle, 2*M_PI, a1); |
7fd59977 | 413 | }//else |
414 | ||
415 | // Controle de w | |
f9032cf2 | 416 | w = Pc.Parameter(); |
417 | ||
7fd59977 | 418 | if (ii>1) { |
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(); | |
425 | } | |
426 | } | |
427 | ||
0797d9d3 | 428 | #ifdef OCCT_DEBUG |
7fd59977 | 429 | if (Abs(Diff) > DeltaG) { |
04232180 | 430 | std::cout << "Location :: Diff on Guide : " << |
431 | Diff << std::endl; | |
7fd59977 | 432 | } |
433 | #endif | |
434 | } | |
435 | //Recadrage de l'angle. | |
f9032cf2 | 436 | Angle = theU; |
437 | ||
7fd59977 | 438 | if (ii > 1) { |
439 | Diff = Angle - OldAngle; | |
c6541a0c D |
440 | if (Abs(Diff) > M_PI) { |
441 | InGoodPeriod (OldAngle, 2*M_PI, Angle); | |
7fd59977 | 442 | Diff = Angle - OldAngle; |
443 | } | |
0797d9d3 | 444 | #ifdef OCCT_DEBUG |
c6541a0c | 445 | if (Abs(Diff) > M_PI/4) { |
04232180 | 446 | std::cout << "Diff d'angle trop grand !!" << std::endl; |
7fd59977 | 447 | } |
448 | #endif | |
449 | } | |
450 | ||
451 | ||
452 | //Recadrage du V | |
f9032cf2 | 453 | v = theV; |
454 | ||
7fd59977 | 455 | if (ii > 1) { |
456 | if (uperiodic) { | |
457 | InGoodPeriod (myPoles2d->Value(2, ii-1).Y(), UPeriod, v); | |
458 | } | |
459 | Diff = v - myPoles2d->Value(2, ii-1).Y(); | |
0797d9d3 | 460 | #ifdef OCCT_DEBUG |
96a95605 | 461 | if (Abs(Diff) > (Ul-Uf)/(2+NbKnots)) { |
04232180 | 462 | std::cout << "Diff sur section trop grand !!" << std::endl; |
7fd59977 | 463 | } |
464 | #endif | |
465 | } | |
466 | ||
467 | p1.SetCoord(t, w); // on stocke les parametres | |
468 | p2.SetCoord(Angle , v); | |
469 | CurAngle = Angle; | |
470 | myPoles2d->SetValue(1, ii, p1); | |
471 | myPoles2d->SetValue(2, ii, p2); | |
472 | OldAngle = Angle; | |
473 | } | |
474 | ||
475 | LastAngle = CurAngle; | |
476 | rotation = Standard_True; //C'est pret ! | |
477 | } | |
478 | ||
479 | ||
480 | //================================================================== | |
481 | //Function: Set | |
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) | |
490 | { | |
491 | myStatus = GeomFill_PipeOk; | |
492 | myFirstS = SFirst; | |
493 | myLastS = SLast; | |
494 | LastAngle = PrecAngle; | |
495 | if (myCurve.IsNull()) | |
496 | ratio = 0.; | |
497 | else | |
498 | ratio = (SLast-SFirst) / (myCurve->LastParameter() - | |
499 | myCurve->FirstParameter()); | |
500 | mySec = Section; | |
501 | ||
502 | if (rotat) SetRotation(PrecAngle, LastAngle); | |
503 | else rotation = Standard_False; | |
504 | } | |
505 | ||
506 | //================================================================== | |
507 | //Function: EraseRotation | |
508 | //Purpose : Supprime la Rotation | |
509 | //================================================================== | |
510 | void GeomFill_LocationGuide:: EraseRotation() | |
511 | { | |
512 | rotation = Standard_False; | |
513 | if (myStatus == GeomFill_ImpossibleContact) myStatus = GeomFill_PipeOk; | |
514 | } | |
515 | ||
516 | //================================================================== | |
517 | //Function: Copy | |
518 | //Purpose : | |
519 | //================================================================== | |
520 | Handle(GeomFill_LocationLaw) GeomFill_LocationGuide::Copy() const | |
521 | { | |
522 | Standard_Real la; | |
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); | |
531 | ||
532 | return copy; | |
533 | } | |
534 | ||
535 | ||
536 | //================================================================== | |
537 | //Function: SetCurve | |
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) | |
542 | { | |
543 | Standard_Real LastAngle; | |
544 | myCurve = C; | |
545 | myTrimmed = C; | |
546 | ||
547 | if (!myCurve.IsNull()){ | |
548 | myLaw->SetCurve(C); | |
549 | myLaw->Origine(OrigParam1, OrigParam2); | |
550 | myStatus = myLaw->ErrorStatus(); | |
551 | ||
552 | if (rotation) SetRotation(myPoles2d->Value(1,1).X(), LastAngle); | |
553 | } | |
554 | } | |
555 | ||
556 | //================================================================== | |
557 | //Function: GetCurve | |
558 | //Purpose : return the trajectoire | |
559 | //================================================================== | |
560 | const Handle(Adaptor3d_HCurve)& GeomFill_LocationGuide::GetCurve() const | |
561 | { | |
562 | return myCurve; | |
563 | } | |
564 | ||
565 | //================================================================== | |
566 | //Function: SetTrsf | |
567 | //Purpose : | |
568 | //================================================================== | |
569 | void GeomFill_LocationGuide::SetTrsf(const gp_Mat& Transfo) | |
570 | { | |
571 | Trans = Transfo; | |
572 | gp_Mat Aux; | |
573 | Aux.SetIdentity(); | |
574 | Aux -= Trans; | |
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; | |
579 | } | |
580 | ||
581 | //================================================================== | |
582 | //Function: D0 | |
583 | //Purpose : | |
584 | //================================================================== | |
585 | Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param, | |
586 | gp_Mat& M, | |
587 | gp_Vec& V) | |
588 | { | |
589 | Standard_Boolean Ok; | |
590 | gp_Vec T,N,B; | |
591 | gp_Pnt P; | |
592 | ||
593 | myCurve->D0(Param, P); | |
594 | V.SetXYZ(P.XYZ()); | |
595 | Ok = myLaw->D0(Param, T, N, B); | |
596 | if (!Ok) { | |
597 | myStatus = myLaw->ErrorStatus(); | |
598 | return Ok; | |
599 | } | |
600 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
601 | ||
602 | if (WithTrans) { | |
603 | M *= Trans; | |
604 | } | |
605 | ||
606 | if(rotation) { | |
607 | Standard_Real U = myFirstS + | |
608 | (Param-myCurve->FirstParameter())*ratio; | |
609 | // initialisations germe | |
610 | InitX(Param); | |
611 | ||
612 | Standard_Integer Iter = 100; | |
613 | gp_XYZ t,b,n; | |
614 | t = M.Column(3); | |
615 | b = M.Column(2); | |
616 | n = M.Column(1); | |
617 | ||
618 | // Intersection entre surf revol et guide | |
619 | // equation | |
620 | GeomFill_FunctionGuide E (mySec, myGuide, U); | |
621 | E.SetParam(Param, P, t, n); | |
622 | // resolution => angle | |
859a47c3 | 623 | math_FunctionSetRoot Result(E, TolRes, Iter); |
624 | Result.Perform(E, X, Inf, Sup); | |
7fd59977 | 625 | |
626 | if (Result.IsDone()) { | |
627 | // solution | |
628 | Result.Root(R); | |
629 | ||
630 | // rotation | |
631 | gp_Mat Rot; | |
632 | Rot.SetRotation(t, R(2)); | |
633 | b *= Rot; | |
634 | n *= Rot; | |
635 | ||
636 | M.SetCols(n, b, t); | |
637 | } | |
638 | else { | |
0797d9d3 | 639 | #ifdef OCCT_DEBUG |
04232180 | 640 | std::cout << "LocationGuide::D0 : No Result !"<<std::endl; |
7fd59977 | 641 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); |
642 | #endif | |
643 | myStatus = GeomFill_ImpossibleContact; | |
644 | return Standard_False; | |
645 | } | |
646 | } | |
647 | ||
648 | return Standard_True; | |
649 | } | |
650 | ||
651 | //================================================================== | |
652 | //Function: D0 | |
653 | //Purpose : calcul de l'intersection (C0) surface revol / guide | |
654 | //================================================================== | |
655 | Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param, | |
656 | gp_Mat& M, | |
657 | gp_Vec& V, | |
658 | // TColgp_Array1OfPnt2d& Poles2d) | |
659 | TColgp_Array1OfPnt2d& ) | |
660 | { | |
661 | gp_Vec T, N, B; | |
662 | gp_Pnt P; | |
663 | Standard_Boolean Ok; | |
7fd59977 | 664 | |
665 | myCurve->D0(Param, P); | |
666 | V.SetXYZ(P.XYZ()); | |
667 | Ok = myLaw->D0(Param, T, N, B); | |
668 | if (!Ok) { | |
669 | myStatus = myLaw->ErrorStatus(); | |
670 | return Ok; | |
671 | } | |
672 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
673 | ||
674 | if (WithTrans) { | |
675 | M *= Trans; | |
676 | } | |
677 | ||
678 | if (rotation) { | |
7fd59977 | 679 | //initialisation du germe |
680 | InitX(Param); | |
681 | Standard_Integer Iter = 100; | |
682 | gp_XYZ b, n, t; | |
683 | t = M.Column(3); | |
684 | b = M.Column(2); | |
685 | n = M.Column(1); | |
686 | ||
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); | |
859a47c3 | 691 | |
7fd59977 | 692 | // resolution |
859a47c3 | 693 | math_FunctionSetRoot Result(E, TolRes, Iter); |
694 | Result.Perform(E, X, Inf, Sup); | |
695 | ||
7fd59977 | 696 | if (Result.IsDone()) { |
697 | // solution | |
698 | Result.Root(R); | |
699 | ||
700 | // rotation | |
701 | gp_Mat Rot; | |
702 | Rot.SetRotation(t, R(2)); | |
703 | ||
704 | ||
705 | b *= Rot; | |
706 | n *= Rot; | |
707 | ||
708 | M.SetCols(n, b, t); | |
709 | } | |
710 | else { | |
0797d9d3 | 711 | #ifdef OCCT_DEBUG |
6e6cd5d9 | 712 | Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter()); |
04232180 | 713 | std::cout << "LocationGuide::D0 : No Result !"<<std::endl; |
7fd59977 | 714 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); |
715 | #endif | |
716 | myStatus = GeomFill_ImpossibleContact; | |
717 | return Standard_False; | |
718 | } | |
719 | } | |
720 | ||
721 | return Standard_True; | |
722 | } | |
723 | ||
724 | ||
725 | //================================================================== | |
726 | //Function: D1 | |
727 | //Purpose : calcul de l'intersection (C1) surface revol / guide | |
728 | //================================================================== | |
729 | Standard_Boolean GeomFill_LocationGuide::D1(const Standard_Real Param, | |
730 | gp_Mat& M, | |
731 | gp_Vec& V, | |
732 | gp_Mat& DM, | |
733 | gp_Vec& DV, | |
734 | // TColgp_Array1OfPnt2d& Poles2d, | |
735 | TColgp_Array1OfPnt2d& , | |
736 | // TColgp_Array1OfVec2d& DPoles2d) | |
737 | TColgp_Array1OfVec2d& ) | |
738 | { | |
739 | // gp_Vec T, N, B, DT, DN, DB, T0, N0, B0; | |
740 | gp_Vec T, N, B, DT, DN, DB; | |
741 | // gp_Pnt P, P0; | |
742 | gp_Pnt P; | |
743 | Standard_Boolean Ok; | |
744 | ||
745 | myCurve->D1(Param, P, DV); | |
746 | V.SetXYZ(P.XYZ()); | |
747 | Ok = myLaw->D1(Param, T, DT, N, DN, B, DB); | |
748 | if (!Ok) { | |
749 | myStatus = myLaw->ErrorStatus(); | |
750 | return Ok; | |
751 | } | |
752 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
753 | DM.SetCols(DN.XYZ() , DB.XYZ(), DT.XYZ()); | |
754 | ||
755 | if (WithTrans) { | |
756 | M *= Trans; | |
757 | DM *= Trans; | |
758 | } | |
759 | ||
760 | if (rotation) { | |
761 | return Standard_False; | |
4e76d93b | 762 | /* |
0797d9d3 | 763 | #ifdef OCCT_DEBUG |
7fd59977 | 764 | Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter()); |
765 | #else | |
766 | myCurve->FirstParameter() ; | |
767 | #endif | |
768 | ||
769 | // initialisation du germe | |
770 | InitX(Param); | |
771 | ||
772 | Standard_Integer Iter = 100; | |
773 | gp_XYZ t,b,n, dt, db, dn; | |
774 | t = M.Column(3); | |
775 | b = M.Column(2); | |
776 | n = M.Column(1); | |
777 | dt = M.Column(3); | |
778 | db = M.Column(2); | |
779 | dn = M.Column(1); | |
780 | ||
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); | |
785 | ||
786 | // resolution | |
787 | math_FunctionSetRoot Result(E, X, TolRes, | |
788 | Inf, Sup, Iter); | |
789 | ||
790 | if (Result.IsDone()) | |
791 | { | |
792 | // solution de la fonction | |
793 | Result.Root(R); | |
794 | ||
795 | // derivee de la fonction | |
796 | math_Vector DEDT(1,3); | |
797 | E.DerivT(R, DV.XYZ(), dt, DEDT); // dE/dt => DEDT | |
798 | ||
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 | |
802 | ||
803 | // resolution du syst. : DEDX*DSDT = -DEDT | |
804 | math_Gauss Ga(DEDX); | |
805 | if (Ga.IsDone()) | |
806 | { | |
807 | Ga.Solve (DEDT.Opposite(), DSDT);// resolution du syst. | |
808 | }//if | |
809 | else { | |
0797d9d3 | 810 | #ifdef OCCT_DEBUG |
04232180 | 811 | std::cout << "DEDX = " << DEDX << std::endl; |
812 | std::cout << "DEDT = " << DEDT << std::endl; | |
7fd59977 | 813 | #endif |
9775fa61 | 814 | throw Standard_ConstructionError( |
7fd59977 | 815 | "LocationGuide::D1 : No Result dans la derivee"); |
816 | } | |
817 | ||
818 | // transformation = rotation | |
819 | gp_Mat Rot, DRot; | |
820 | Rot.SetRotation(t, R(2)); | |
821 | ||
822 | ||
823 | ||
824 | M.SetCols(n*Rot, b*Rot, t); | |
825 | ||
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); | |
829 | gp_Trsf Transfo3; | |
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); | |
836 | ||
837 | // matrices de rotation et derivees | |
838 | Standard_Real A = R(2); | |
839 | Standard_Real Aprim = DSDT(2); | |
840 | ||
0797d9d3 | 841 | #ifdef OCCT_DEBUG |
7fd59977 | 842 | gp_Mat M2 (Cos(A), -Sin(A),0, // rotation autour de T |
843 | Sin(A), Cos(A),0, | |
844 | 0,0,1); | |
845 | #endif | |
846 | ||
847 | gp_Mat M2prim (-Sin(A), -Cos(A), 0, // derivee rotation autour de T | |
848 | Cos(A), -Sin(A), 0, | |
849 | 0, 0, 0); | |
850 | M2prim.Multiply(Aprim); | |
851 | ||
852 | // transformations | |
853 | ||
854 | ||
855 | dn *= Rot; | |
856 | db *= Rot; | |
857 | ||
858 | n *= DRot; | |
859 | b *= DRot; | |
860 | ||
861 | dn += n; | |
862 | db += b; | |
863 | ||
864 | // on repasse dans repere triedre | |
865 | gp_Trsf InvTrsf; | |
866 | InvTrsf = Transfo3.Inverted(); | |
867 | InvTrsf.Transforms(dn); | |
868 | InvTrsf.Transforms(db); | |
869 | ||
870 | DM.SetCols(dn , db , dt); | |
871 | }//if_Result | |
872 | ||
873 | else { | |
0797d9d3 | 874 | #ifdef OCCT_DEBUG |
04232180 | 875 | std::cout << "LocationGuide::D1 : No Result !!"<<std::endl; |
7fd59977 | 876 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); |
877 | #endif | |
878 | myStatus = GeomFill_ImpossibleContact; | |
879 | return Standard_False; | |
880 | } | |
4e76d93b | 881 | */ |
7fd59977 | 882 | }//if_rotation |
883 | ||
884 | ||
885 | return Standard_True; | |
886 | ||
887 | } | |
888 | ||
889 | //================================================================== | |
890 | //Function: D2 | |
891 | //Purpose : calcul de l'intersection (C2) surface revol / guide | |
892 | //================================================================== | |
893 | Standard_Boolean GeomFill_LocationGuide::D2(const Standard_Real Param, | |
894 | gp_Mat& M, | |
895 | gp_Vec& V, | |
896 | gp_Mat& DM, | |
897 | gp_Vec& DV, | |
898 | gp_Mat& D2M, | |
899 | gp_Vec& D2V, | |
900 | // TColgp_Array1OfPnt2d& Poles2d, | |
901 | TColgp_Array1OfPnt2d& , | |
902 | // TColgp_Array1OfVec2d& DPoles2d, | |
903 | TColgp_Array1OfVec2d& , | |
904 | // TColgp_Array1OfVec2d& D2Poles2d) | |
905 | TColgp_Array1OfVec2d& ) | |
906 | { | |
907 | gp_Vec T, N, B, DT, DN, DB, D2T, D2N, D2B; | |
908 | // gp_Vec T0, N0, B0, T1, N1, B1; | |
909 | // gp_Pnt P, P0, P1; | |
910 | gp_Pnt P; | |
911 | Standard_Boolean Ok; | |
912 | ||
913 | myCurve->D2(Param, P, DV, D2V); | |
914 | V.SetXYZ(P.XYZ()); | |
915 | Ok = myLaw->D2(Param, T, DT, D2T, N, DN, D2N, B, DB, D2B); | |
916 | if (!Ok) { | |
917 | myStatus = myLaw->ErrorStatus(); | |
918 | return Ok; | |
919 | } | |
920 | ||
921 | if (WithTrans) { | |
922 | M *= Trans; | |
923 | DM *= Trans; | |
924 | D2M *= Trans; | |
925 | } | |
926 | ||
927 | if (rotation) | |
928 | { | |
929 | return Standard_False; | |
930 | /* | |
931 | Standard_Real U = myFirstS + | |
932 | (Param-myCurve->FirstParameter())*ratio; | |
933 | // rotation | |
934 | math_Vector X(1,3,0); | |
935 | InitX(Param,X); | |
936 | // tolerance sur X | |
937 | ||
938 | TolRes.Init(1.e-6); | |
939 | // tolerance sur E | |
940 | // Standard_Real ETol = 1.e-6; | |
941 | Standard_Integer Iter = 100; | |
942 | ||
943 | ||
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); | |
948 | ||
949 | // resolution | |
950 | math_FunctionSetRoot Result(E, X, TolRes, | |
951 | Inf, Sup, Iter); | |
952 | ||
953 | if (Result.IsDone()) | |
954 | { | |
955 | Result.Root(R); // solution | |
956 | ||
957 | //gp_Pnt2d p (R(2), R(3)); // point sur la surface (angle, v) | |
958 | //Poles2d.SetValue(1,p); | |
959 | ||
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 | |
966 | ||
967 | // resolution du syst. lin. : DEDX*DSDT = -DEDT | |
968 | math_Gauss Ga(DEDX); | |
969 | if (Ga.IsDone()) | |
970 | { | |
971 | Ga.Solve (DEDT.Opposite(), DSDT); // resolution du syst. lin. | |
972 | //gp_Vec2d dp (DSDT(2), DSDT(3)); // surface | |
973 | //DPoles2d.SetValue(1, dp); | |
974 | }//if | |
04232180 | 975 | else std::cout <<"LocationGuide::D2 : No Result dans la derivee premiere"<<std::endl; |
7fd59977 | 976 | |
977 | // deuxieme derivee | |
978 | GeomFill_Tensor D2EDX2(3,3,3); | |
979 | E.Deriv2X(R, D2EDX2); // d2E/dx2 | |
980 | ||
981 | math_Vector D2EDT2(1,3,0); | |
982 | ||
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 | |
987 | // else | |
988 | // E.Deriv2T(Param0, Param1, Param, R0, R1, R, D2EDT2); // d2E/dt2 | |
989 | ||
990 | math_Matrix D2EDTDX(1,3,1,3,0); | |
991 | E.DerivTX(Param, Param0, R, R0, D2EDTDX); // d2E/dtdx | |
992 | ||
993 | math_Vector D2SDT2(1,3,0); // d2s/dt2 | |
994 | math_Matrix M1(1,3,1,3,0); | |
995 | D2EDX2.Multiply(DSDT,M1); | |
996 | ||
997 | // resolution du syst. lin. | |
998 | math_Gauss Ga1 (DEDX); | |
999 | if (Ga1.IsDone()) | |
1000 | { | |
1001 | Ga1.Solve ( - M1*DSDT - 2*D2EDTDX*DSDT - D2EDT2 , D2SDT2); | |
1002 | //gp_Vec2d d2p (D2SDT2(2), D2SDT2(3)); // surface | |
1003 | //D2Poles2d.SetValue(1, d2p); | |
1004 | }//if | |
1005 | else { | |
04232180 | 1006 | std::cout <<"LocationGuide::D2 : No Result dans la derivee seconde"<<std::endl; |
7fd59977 | 1007 | myStatus = GeomFill_ImpossibleContact; |
1008 | } | |
1009 | ||
1010 | //------------------------------------------ | |
1011 | // rotation | |
1012 | //------------------------------------------ | |
1013 | ||
1014 | gp_Trsf Tr; | |
1015 | gp_Pnt Q (0, 0 ,0); | |
1016 | gp_Ax1 Axe (Q, D); | |
1017 | Tr.SetRotation(Axe, R(2)); | |
1018 | ||
1019 | gp_Vec b,b2; | |
1020 | b = b2 = B; | |
1021 | gp_Vec n,n2; | |
1022 | n = n2 = N; | |
1023 | ||
1024 | B.Transform(Tr); | |
1025 | N.Transform(Tr); | |
1026 | ||
1027 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
1028 | ||
1029 | //------------------------------------------ | |
1030 | // derivees de la rotation | |
1031 | // A VERIFIER !!!! | |
1032 | //----------------------------------------- | |
1033 | gp_Vec db,dn,db3,dn3; | |
1034 | db = db3 = DB; | |
1035 | dn = dn3 = DN; | |
1036 | ||
1037 | gp_Vec db1,dn1,db2,dn2; | |
1038 | ||
1039 | //transfo entre triedre et Oxyz | |
1040 | gp_Ax3 RepTriedre4(Q,D,B2); | |
1041 | gp_Trsf Transfo3; | |
1042 | Transfo3.SetTransformation(Rep,RepTriedre4); | |
1043 | ||
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); | |
1055 | ||
1056 | //matrices de rotation et derivees | |
1057 | Standard_Real A = R(2); | |
1058 | Standard_Real Aprim = DSDT(2); | |
1059 | Standard_Real Asec = D2SDT2(2); | |
1060 | ||
1061 | gp_Mat M2 (Cos(A),-Sin(A),0, // rotation autour de T | |
1062 | Sin(A), Cos(A),0, | |
1063 | 0, 0, 1); | |
1064 | ||
1065 | gp_Mat M2prim (-Sin(A),-Cos(A),0, // derivee 1ere rotation autour de T | |
1066 | Cos(A), -Sin(A),0, | |
1067 | 0,0,0); | |
1068 | ||
1069 | gp_Mat M2sec (-Cos(A), Sin(A), 0, // derivee 2nde rotation autour de T | |
1070 | -Sin(A), -Cos(A), 0, | |
1071 | 0,0,0); | |
1072 | M2sec.Multiply(Aprim*Aprim); | |
1073 | gp_Mat M2p = M2prim.Multiplied(Asec); | |
1074 | M2sec.Add(M2p); | |
1075 | ||
1076 | M2prim.Multiply(Aprim); | |
1077 | ||
1078 | // transformation | |
1079 | gp_Trsf Rot; | |
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, | |
1083 | 1.e-8,1.e-8); | |
1084 | gp_Trsf DRot; | |
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, | |
1088 | 1.e-8,1.e-8); | |
1089 | ||
1090 | gp_Trsf D2Rot; | |
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, | |
1094 | 1.e-8,1.e-8); | |
1095 | ||
1096 | ||
1097 | //derivee premiere | |
1098 | dn.Transform(Rot); | |
1099 | db.Transform(Rot); | |
1100 | n.Transform(DRot); | |
1101 | b.Transform(DRot); | |
1102 | dn1 = dn + n; | |
1103 | db1 = db + b; | |
1104 | dn1.Transform(Transfo3.Inverted()); | |
1105 | db1.Transform(Transfo3.Inverted()); | |
1106 | ||
1107 | DM.SetCols(dn1.XYZ(), db1.XYZ(), DT.XYZ()); | |
1108 | ||
1109 | //derivee seconde | |
1110 | D2N.Transform(Rot); | |
1111 | D2B.Transform(Rot); | |
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()); | |
1120 | ||
1121 | D2M.SetCols(dn2.XYZ(), db2.XYZ(), D2T.XYZ()); | |
1122 | ||
1123 | }//if_result | |
1124 | else { | |
0797d9d3 | 1125 | #ifdef OCCT_DEBUG |
04232180 | 1126 | std::cout << "LocationGuide::D2 : No Result !!" <<std::endl; |
7fd59977 | 1127 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); |
1128 | #endif | |
1129 | return Standard_False; | |
1130 | }*/ | |
1131 | }//if_rotation | |
1132 | ||
1133 | else | |
1134 | { | |
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()); | |
1138 | } | |
1139 | ||
1140 | return Standard_True; | |
1141 | // return Standard_False; | |
1142 | } | |
1143 | ||
1144 | //================================================================== | |
1145 | //Function : HasFirstRestriction | |
1146 | //Purpose : | |
1147 | //================================================================== | |
1148 | Standard_Boolean GeomFill_LocationGuide::HasFirstRestriction() const | |
1149 | { | |
1150 | return Standard_False; | |
1151 | } | |
1152 | ||
1153 | //================================================================== | |
1154 | //Function : HasLastRestriction | |
1155 | //Purpose : | |
1156 | //================================================================== | |
1157 | Standard_Boolean GeomFill_LocationGuide::HasLastRestriction() const | |
1158 | { | |
1159 | return Standard_False; | |
1160 | } | |
1161 | ||
1162 | //================================================================== | |
1163 | //Function : TraceNumber | |
1164 | //Purpose : | |
1165 | //================================================================== | |
1166 | Standard_Integer GeomFill_LocationGuide::TraceNumber() const | |
1167 | { | |
1168 | return 0; | |
1169 | } | |
1170 | ||
1171 | //================================================================== | |
1172 | //Function : ErrorStatus | |
1173 | //Purpose : | |
1174 | //================================================================== | |
1175 | GeomFill_PipeError GeomFill_LocationGuide::ErrorStatus() const | |
1176 | { | |
1177 | return myStatus; | |
1178 | } | |
1179 | ||
1180 | //================================================================== | |
1181 | //Function:NbIntervals | |
1182 | //Purpose : | |
1183 | //================================================================== | |
1184 | Standard_Integer GeomFill_LocationGuide::NbIntervals | |
1185 | (const GeomAbs_Shape S) const | |
1186 | { | |
1187 | Standard_Integer Nb_Sec, Nb_Law; | |
1188 | Nb_Sec = myTrimmed->NbIntervals(S); | |
1189 | Nb_Law = myLaw->NbIntervals(S); | |
1190 | ||
1191 | if (Nb_Sec==1) { | |
1192 | return Nb_Law; | |
1193 | } | |
1194 | else if (Nb_Law==1) { | |
1195 | return Nb_Sec; | |
1196 | } | |
1197 | ||
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); | |
1203 | ||
1204 | GeomLib::FuseIntervals( IntC, IntL, Inter, Precision::PConfusion()*0.99); | |
1205 | return Inter.Length()-1; | |
1206 | ||
1207 | } | |
1208 | ||
1209 | //================================================================== | |
1210 | //Function:Intervals | |
1211 | //Purpose : | |
1212 | //================================================================== | |
1213 | void GeomFill_LocationGuide::Intervals(TColStd_Array1OfReal& T, | |
1214 | const GeomAbs_Shape S) const | |
1215 | { | |
1216 | Standard_Integer Nb_Sec, Nb_Law; | |
1217 | Nb_Sec = myTrimmed->NbIntervals(S); | |
1218 | Nb_Law = myLaw->NbIntervals(S); | |
1219 | ||
1220 | if (Nb_Sec==1) { | |
1221 | myLaw->Intervals(T, S); | |
1222 | return; | |
1223 | } | |
1224 | else if (Nb_Law==1) { | |
1225 | myTrimmed->Intervals(T, S); | |
1226 | return; | |
1227 | } | |
1228 | ||
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); | |
1234 | ||
1235 | GeomLib::FuseIntervals(IntC, IntL, Inter, Precision::PConfusion()*0.99); | |
1236 | for (Standard_Integer ii=1; ii<=Inter.Length(); ii++) | |
1237 | T(ii) = Inter(ii); | |
1238 | } | |
1239 | ||
1240 | //================================================================== | |
1241 | //Function:SetInterval | |
1242 | //Purpose : | |
1243 | //================================================================== | |
1244 | void GeomFill_LocationGuide::SetInterval(const Standard_Real First, | |
1245 | const Standard_Real Last) | |
1246 | { | |
1247 | myLaw->SetInterval(First, Last); | |
1248 | myTrimmed = myCurve->Trim(First, Last, 0); | |
1249 | } | |
1250 | //================================================================== | |
1251 | //Function: GetInterval | |
1252 | //Purpose : | |
1253 | //================================================================== | |
1254 | void GeomFill_LocationGuide::GetInterval(Standard_Real& First, | |
1255 | Standard_Real& Last) const | |
1256 | { | |
1257 | First = myTrimmed->FirstParameter(); | |
1258 | Last = myTrimmed->LastParameter(); | |
1259 | } | |
1260 | ||
1261 | //================================================================== | |
1262 | //Function: GetDomain | |
1263 | //Purpose : | |
1264 | //================================================================== | |
1265 | void GeomFill_LocationGuide::GetDomain(Standard_Real& First, | |
1266 | Standard_Real& Last) const | |
1267 | { | |
1268 | First = myCurve->FirstParameter(); | |
1269 | Last = myCurve->LastParameter(); | |
1270 | } | |
1271 | ||
1272 | //================================================================== | |
1273 | //function : SetTolerance | |
1274 | //purpose : | |
1275 | //================================================================== | |
1276 | void GeomFill_LocationGuide::SetTolerance(const Standard_Real Tol3d, | |
1277 | const Standard_Real ) | |
1278 | { | |
1279 | TolRes(1) = myGuide->Resolution(Tol3d); | |
1280 | Resolution(1, Tol3d, TolRes(2), TolRes(3)); | |
1281 | ||
1282 | } | |
1283 | ||
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 | |
1293 | { | |
1294 | TolU = Tol/100; | |
1295 | TolV = Tol/100; | |
1296 | } | |
1297 | ||
1298 | //================================================================== | |
1299 | //Function:GetMaximalNorm | |
1300 | //Purpose : On suppose les triedres normes => return 1 | |
1301 | //================================================================== | |
1302 | Standard_Real GeomFill_LocationGuide::GetMaximalNorm() | |
1303 | { | |
1304 | return 1.; | |
1305 | } | |
1306 | ||
1307 | //================================================================== | |
1308 | //Function:GetAverageLaw | |
1309 | //Purpose : | |
1310 | //================================================================== | |
1311 | void GeomFill_LocationGuide::GetAverageLaw(gp_Mat& AM, | |
1312 | gp_Vec& AV) | |
1313 | { | |
1314 | Standard_Integer ii; | |
1315 | Standard_Real U, delta; | |
1316 | gp_Vec V, V1, V2, V3; | |
1317 | ||
1318 | myLaw->GetAverageLaw(V1, V2, V3); | |
1319 | AM.SetCols(V1.XYZ(), V2.XYZ(), V3.XYZ()); | |
1320 | ||
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() ); | |
1326 | AV += V; | |
1327 | } | |
1328 | AV = AV/(myNbPts+1); | |
1329 | } | |
1330 | ||
1331 | ||
1332 | //================================================================== | |
1333 | //Function : Section | |
1334 | //Purpose : | |
1335 | //================================================================== | |
1336 | Handle(Geom_Curve) GeomFill_LocationGuide::Section() const | |
1337 | { | |
1338 | return mySec->ConstantSection(); | |
1339 | } | |
1340 | ||
1341 | //================================================================== | |
1342 | //Function : Guide | |
1343 | //Purpose : | |
1344 | //================================================================== | |
1345 | Handle(Adaptor3d_HCurve) GeomFill_LocationGuide::Guide() const | |
1346 | { | |
1347 | return myGuide; | |
1348 | } | |
1349 | ||
1350 | //================================================================== | |
1351 | //Function : IsRotation | |
1352 | //Purpose : | |
1353 | //================================================================== | |
1354 | // Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& Error) const | |
1355 | Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& ) const | |
1356 | { | |
1357 | return Standard_False; | |
1358 | } | |
1359 | ||
1360 | //================================================================== | |
1361 | //Function : Rotation | |
1362 | //Purpose : | |
1363 | //================================================================== | |
1364 | // void GeomFill_LocationGuide::Rotation(gp_Pnt& Centre) const | |
1365 | void GeomFill_LocationGuide::Rotation(gp_Pnt& ) const | |
1366 | { | |
9775fa61 | 1367 | throw Standard_NotImplemented("GeomFill_LocationGuide::Rotation"); |
7fd59977 | 1368 | } |
1369 | ||
1370 | //================================================================== | |
1371 | //Function : IsTranslation | |
1372 | //Purpose : | |
1373 | //================================================================== | |
1374 | // Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& Error) const | |
1375 | Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& ) const | |
1376 | { | |
1377 | return Standard_False; | |
1378 | } | |
1379 | ||
1380 | //================================================================== | |
1381 | //Function : InitX | |
1382 | //Purpose : recherche par interpolation d'une valeur initiale | |
1383 | //================================================================== | |
18434846 | 1384 | void GeomFill_LocationGuide::InitX(const Standard_Real Param) |
7fd59977 | 1385 | { |
1386 | ||
1387 | Standard_Integer Ideb = 1, Ifin = myPoles2d->RowLength(), Idemi; | |
1388 | Standard_Real Valeur, t1, t2; | |
1389 | ||
1390 | ||
1391 | Valeur = myPoles2d->Value(1, Ideb).X(); | |
1392 | if (Param == Valeur) { | |
1393 | Ifin = Ideb+1; | |
1394 | } | |
1395 | ||
1396 | Valeur = myPoles2d->Value(1, Ifin).X(); | |
1397 | if (Param == Valeur) { | |
1398 | Ideb = Ifin-1; | |
1399 | } | |
1400 | ||
1401 | while ( Ideb+1 != Ifin) { | |
1402 | Idemi = (Ideb+Ifin)/2; | |
1403 | Valeur = myPoles2d->Value(1, Idemi).X(); | |
1404 | if (Valeur < Param) { | |
1405 | Ideb = Idemi; | |
1406 | } | |
1407 | else { | |
1408 | if ( Valeur > Param) { Ifin = Idemi;} | |
1409 | else { | |
1410 | Ideb = Idemi; | |
1411 | Ifin = Ideb+1; | |
1412 | } | |
1413 | } | |
1414 | } | |
1415 | ||
1416 | t1 = myPoles2d->Value(1,Ideb).X(); | |
1417 | t2 = myPoles2d->Value(1,Ifin).X(); | |
1418 | Standard_Real diff = t2-t1; | |
1419 | ||
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); | |
1425 | ||
1426 | if (diff > 1.e-7) { | |
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 | |
1432 | } | |
1433 | else { | |
1434 | X(1) = (W1+W2) /2; | |
1435 | X(2) = (P1.Coord(1) + P2.Coord(1)) /2; | |
1436 | X(3) = (P1.Coord(2) + P2.Coord(2)) /2; | |
1437 | } | |
1438 | ||
1439 | if (myGuide->IsPeriodic()) { | |
1440 | X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(), | |
1441 | myGuide->LastParameter()); | |
1442 | } | |
c6541a0c | 1443 | X(2) = ElCLib::InPeriod(X(2), 0, 2*M_PI); |
7fd59977 | 1444 | if (mySec->IsUPeriodic()) { |
1445 | X(3) = ElCLib::InPeriod(X(3), Uf, Ul); | |
1446 | } | |
1447 | } | |
1448 | ||
1449 | ||
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) | |
1456 | { | |
1457 | OrigParam1 = Param1; | |
1458 | OrigParam2 = Param2; | |
1459 | } | |
1460 | ||
f9032cf2 | 1461 | //================================================================== |
1462 | //Function : ComputeAutomaticLaw | |
1463 | //Purpose : | |
1464 | //================================================================== | |
1465 | GeomFill_PipeError GeomFill_LocationGuide::ComputeAutomaticLaw(Handle(TColgp_HArray1OfPnt2d)& ParAndRad) const | |
1466 | { | |
1467 | gp_Pnt P; | |
1468 | gp_Vec T,N,B; | |
1469 | Standard_Integer ii; | |
1470 | Standard_Real t; | |
1471 | ||
1472 | GeomFill_PipeError theStatus = GeomFill_PipeOk; | |
1473 | ||
1474 | Standard_Real f = myCurve->FirstParameter(); | |
1475 | Standard_Real l = myCurve->LastParameter(); | |
1476 | ||
1477 | ParAndRad = new TColgp_HArray1OfPnt2d(1, myNbPts); | |
1478 | for (ii = 1; ii <= myNbPts; ii++) | |
1479 | { | |
1480 | t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l; | |
1481 | t /= (myNbPts-1); | |
1482 | myCurve->D0(t, P); | |
1483 | Standard_Boolean Ok = myLaw->D0(t, T, N, B); | |
1484 | if (!Ok) | |
1485 | { | |
1486 | theStatus = myLaw->ErrorStatus(); | |
1487 | return theStatus; | |
1488 | } | |
1489 | gp_Pnt PointOnGuide = myLaw->CurrentPointOnGuide(); | |
1490 | Standard_Real CurWidth = P.Distance(PointOnGuide); | |
1491 | ||
1492 | gp_Pnt2d aParamWithRadius(t, CurWidth); | |
1493 | ParAndRad->SetValue(ii, aParamWithRadius); | |
1494 | } | |
1495 | ||
1496 | return theStatus; | |
1497 | } |