Commit | Line | Data |
---|---|---|
7fd59977 | 1 | // File: GeomFill_LocationGuide.cxx |
2 | // Created: Wed Jul 8 15:16:45 1998 | |
3 | // Author: Stephanie HUMEAU | |
4 | // <shu@sun17> | |
5 | ||
6 | ||
7 | #include <GeomFill_LocationGuide.ixx> | |
8 | #include <gp.hxx> | |
9 | #include <gp_Pnt.hxx> | |
10 | #include <gp_Vec.hxx> | |
11 | #include <gp_Dir.hxx> | |
12 | #include <gp_Trsf.hxx> | |
13 | #include <gp_GTrsf.hxx> | |
14 | #include <gp_XYZ.hxx> | |
15 | #include <gp_Ax1.hxx> | |
16 | #include <gp_Pnt2d.hxx> | |
17 | ||
18 | #include <math_Vector.hxx> | |
19 | #include <math_Gauss.hxx> | |
20 | #include <math_FunctionSetRoot.hxx> | |
21 | #include <Precision.hxx> | |
22 | ||
23 | #include <Geom_SurfaceOfRevolution.hxx> | |
24 | #include <Geom_BSplineCurve.hxx> | |
25 | #include <Geom_Curve.hxx> | |
26 | ||
27 | #include <Adaptor3d_SurfaceOfRevolution.hxx> | |
28 | #include <Adaptor3d_HSurface.hxx> | |
29 | ||
30 | #include <IntCurveSurface_IntersectionPoint.hxx> | |
31 | #include <IntCurveSurface_HInter.hxx> | |
32 | #include <Adaptor3d_Surface.hxx> | |
33 | #include <GeomAdaptor.hxx> | |
34 | #include <GeomAdaptor_HSurface.hxx> | |
35 | #include <GeomAdaptor_HCurve.hxx> | |
36 | ||
37 | ||
38 | #include <GeomFill_FunctionGuide.ixx> | |
39 | #include <GeomFill_UniformSection.hxx> | |
40 | #include <GeomFill_SectionPlacement.hxx> | |
41 | #include <Geom_TrimmedCurve.hxx> | |
42 | #include <GeomLib.hxx> | |
43 | #include <ElCLib.hxx> | |
44 | ||
45 | #include <TColStd_HArray1OfInteger.hxx> | |
46 | #include <TColStd_HArray1OfReal.hxx> | |
47 | #include <TColgp_HArray1OfPnt.hxx> | |
48 | ||
49 | #if DRAW | |
50 | static Standard_Integer Affich = 0; | |
51 | #include <Approx_Curve3d.hxx> | |
52 | #include <DrawTrSurf.hxx> | |
53 | #endif | |
54 | ||
55 | //======================================================================= | |
56 | //function : TraceRevol | |
57 | //purpose : Trace la surface de revolution (Debug) | |
58 | //======================================================================= | |
59 | #if DEB | |
60 | static void TraceRevol(const Standard_Real t, | |
61 | const Standard_Real s, | |
62 | const Handle(GeomFill_TrihedronWithGuide)& Law, | |
63 | const Handle(GeomFill_SectionLaw)& Section, | |
64 | const Handle(Adaptor3d_HCurve)& Curve, | |
65 | const gp_Mat& Trans) | |
66 | ||
67 | { | |
68 | gp_Vec T, N, B; | |
69 | gp_Pnt P; | |
70 | Standard_Boolean Ok; | |
71 | gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX()); | |
72 | ||
73 | Curve->D0(t, P); | |
74 | Ok = Law->D0(t, T, N, B); | |
75 | ||
76 | gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ()); | |
77 | M *= Trans; | |
78 | ||
79 | gp_Dir D = M.Column(3); | |
80 | gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion | |
81 | ||
82 | // calculer transfo entre triedre et Oxyz | |
83 | gp_Dir N2 = N; | |
84 | gp_Ax3 N3(P,D,N2); | |
85 | gp_Trsf Transfo; | |
86 | Transfo.SetTransformation(N3, Rep); | |
87 | ||
88 | // transformer la section | |
89 | Standard_Real f, l,e=1.e-7; | |
90 | Handle (Geom_Curve) S, C; | |
91 | ||
92 | if (Section->IsConstant(e)) { | |
93 | C = Section->ConstantSection(); | |
94 | } | |
95 | else { | |
96 | Standard_Integer NbPoles, NbKnots, Deg; | |
97 | Section->SectionShape(NbPoles, NbKnots, Deg); | |
98 | TColStd_Array1OfInteger Mult(1,NbKnots); | |
99 | Section->Mults( Mult); | |
100 | TColStd_Array1OfReal Knots(1,NbKnots); | |
101 | Section->Knots(Knots); | |
102 | TColgp_Array1OfPnt Poles(1, NbPoles); | |
103 | TColStd_Array1OfReal Weights(1, NbPoles); | |
104 | Section->D0(s, Poles, Weights); | |
105 | if (Section->IsRational()) | |
106 | C = new (Geom_BSplineCurve) | |
107 | (Poles, Weights, Knots, Mult , | |
108 | Deg, Section->IsUPeriodic()); | |
109 | else | |
110 | C = new (Geom_BSplineCurve) | |
111 | (Poles, Knots, Mult, | |
112 | Deg, Section->IsUPeriodic()); | |
113 | ||
114 | } | |
115 | ||
116 | f = C->FirstParameter(); | |
117 | l = C->LastParameter(); | |
118 | S = new (Geom_TrimmedCurve) (C, f, l); | |
119 | S->Transform(Transfo); | |
120 | ||
121 | // Surface de revolution | |
122 | Handle (Geom_Surface) Revol = new(Geom_SurfaceOfRevolution) (S, Ax); | |
123 | cout << "Surf Revol at parameter t = " << t << endl; | |
124 | ||
125 | #if DRAW | |
126 | Standard_CString aName = "TheRevol" ; | |
127 | DrawTrSurf::Set(aName,Revol); | |
128 | #endif | |
129 | } | |
130 | #endif | |
131 | ||
132 | //================================================================== | |
133 | //Function: InGoodPeriod | |
134 | //Purpose : Recadre un paramtere | |
135 | //================================================================== | |
136 | static void InGoodPeriod(const Standard_Real Prec, | |
137 | const Standard_Real Period, | |
138 | Standard_Real& Current) | |
139 | { | |
140 | Standard_Real Diff=Current-Prec; | |
141 | Standard_Integer nb = (Standard_Integer ) IntegerPart(Diff/Period); | |
142 | Current -= nb*Period; | |
143 | Diff = Current-Prec; | |
144 | if (Diff > Period/2) Current -= Period; | |
145 | else if (Diff < -Period/2) Current += Period; | |
146 | } | |
147 | ||
148 | //================================================================== | |
149 | //Function: GeomFill_LocationGuide | |
150 | //Purpose : constructor | |
151 | //================================================================== | |
152 | GeomFill_LocationGuide:: | |
153 | GeomFill_LocationGuide (const Handle(GeomFill_TrihedronWithGuide)& Triedre) | |
154 | : TolRes(1,3), Inf(1,3,0.), Sup(1,3,0.), | |
155 | X(1,3), R(1,3), myStatus(GeomFill_PipeOk) | |
156 | { | |
157 | TolRes.Init(1.e-6); | |
158 | myLaw = Triedre; // loi de triedre | |
159 | mySec.Nullify(); // loi de section | |
160 | myCurve.Nullify(); | |
161 | myFirstS = myLastS = -505e77; | |
162 | ||
163 | myNbPts = 21; // nb points pour les calculs | |
164 | myGuide = myLaw->Guide(); // courbe guide | |
165 | if (!myGuide->IsPeriodic()) { | |
166 | Standard_Real f, l, delta; | |
167 | f = myGuide->FirstParameter(); | |
168 | l = myGuide->LastParameter(); | |
169 | delta = (l-f)/100; | |
170 | f-=delta; | |
171 | l+=delta; | |
172 | myGuide = myGuide->Trim(f,l,delta*1.e-7); // courbe guide | |
173 | }// if | |
174 | ||
175 | myPoles2d = new (TColgp_HArray2OfPnt2d)(1, 2, 1, myNbPts); | |
176 | rotation = Standard_False; // contact ou non | |
177 | OrigParam1 = 0; // param pour ACR quand trajectoire | |
178 | OrigParam2 = 1; // et guide pas meme sens de parcourt | |
179 | Trans.SetIdentity(); | |
180 | WithTrans = Standard_False; | |
181 | ||
182 | #if DRAW | |
183 | if (Affich) { | |
184 | Approx_Curve3d approx(myGuide, 1.e-4, | |
185 | GeomAbs_C1, | |
186 | 15+myGuide->NbIntervals(GeomAbs_CN), | |
187 | 14); | |
188 | if (approx.HasResult()) { | |
189 | Standard_CString aName = "TheGuide" ; | |
190 | DrawTrSurf::Set(aName, approx.Curve()); | |
191 | } | |
192 | } | |
193 | #endif | |
194 | } | |
195 | ||
196 | //================================================================== | |
197 | //Function: SetRotation | |
198 | //Purpose : init et force la Rotation | |
199 | //================================================================== | |
200 | void GeomFill_LocationGuide::SetRotation(const Standard_Real PrecAngle, | |
201 | Standard_Real& LastAngle) | |
202 | { | |
203 | if (myCurve.IsNull()) | |
204 | Standard_ConstructionError::Raise( | |
205 | "GeomFill_LocationGuide::The path is not setted !!"); | |
206 | ||
207 | //repere fixe | |
208 | gp_Ax3 Rep(gp::Origin(), gp::DZ(), gp::DX()); | |
209 | // gp_Pnt P,P1,P2; | |
210 | gp_Pnt P; | |
211 | gp_Vec T,N,B; | |
212 | Standard_Integer ii, Deg; | |
213 | Standard_Boolean isconst, israt=Standard_False; | |
214 | Standard_Real t, v,w, OldAngle=0, Angle, DeltaG, DeltaU, Diff; | |
215 | Standard_Real CurAngle = PrecAngle, a1, a2; | |
216 | gp_Pnt2d p1,p2; | |
217 | Handle(Geom_SurfaceOfRevolution) Revol; // surface de revolution | |
218 | Handle(GeomAdaptor_HSurface) Pl; // = Revol | |
219 | Handle(Geom_TrimmedCurve) S; | |
220 | IntCurveSurface_IntersectionPoint PInt; // intersection guide/Revol | |
221 | IntCurveSurface_HInter Int; | |
222 | Handle(TColStd_HArray1OfInteger) Mult; | |
223 | Handle(TColStd_HArray1OfReal) Knots, Weights; | |
224 | Handle(TColgp_HArray1OfPnt) Poles; | |
225 | ||
226 | ||
227 | Standard_Real U=0, UPeriod=0; | |
228 | Standard_Real f = myCurve->FirstParameter(); | |
229 | Standard_Real l = myCurve->LastParameter(); | |
230 | Standard_Boolean Ok, uperiodic = mySec->IsUPeriodic(); | |
231 | ||
232 | DeltaG = (myGuide->LastParameter() - myGuide->FirstParameter())/5; | |
233 | Handle(Geom_Curve) mySection; | |
234 | Standard_Real Tol =1.e-9; | |
235 | ||
236 | Standard_Integer NbPoles, NbKnots; | |
237 | mySec->SectionShape(NbPoles, NbKnots, Deg); | |
238 | ||
239 | ||
240 | if (mySec->IsConstant(Tol)) { | |
241 | mySection = mySec->ConstantSection(); | |
242 | Uf = mySection->FirstParameter(); | |
243 | Ul = mySection->LastParameter(); | |
244 | ||
245 | isconst = Standard_True; | |
246 | } | |
247 | else { | |
248 | isconst = Standard_False; | |
249 | israt = mySec->IsRational(); | |
250 | Mult = new (TColStd_HArray1OfInteger) (1, NbKnots); | |
251 | mySec->Mults( Mult->ChangeArray1()); | |
252 | Knots = new (TColStd_HArray1OfReal) (1, NbKnots); | |
253 | mySec->Knots(Knots->ChangeArray1()); | |
254 | Poles = new (TColgp_HArray1OfPnt) (1, NbPoles); | |
255 | Weights = new (TColStd_HArray1OfReal) (1, NbPoles); | |
256 | Uf = Knots->Value(1); | |
257 | Ul = Knots->Value(NbKnots); | |
258 | } | |
259 | ||
260 | // Bornes de calculs | |
261 | Standard_Real Delta; | |
262 | // Standard_Integer bid1, bid2, NbK; | |
263 | Delta = myGuide->LastParameter() - myGuide->FirstParameter(); | |
264 | Inf(1) = myGuide->FirstParameter() - Delta/10; | |
265 | Sup(1) = myGuide->LastParameter() + Delta/10; | |
266 | ||
c6541a0c D |
267 | Inf(2) = -M_PI; |
268 | Sup(2) = 3*M_PI; | |
7fd59977 | 269 | |
270 | Delta = Ul - Uf; | |
271 | Inf(3) = Uf - Delta/10; | |
272 | Sup(3) = Ul + Delta/10; | |
273 | ||
274 | // JALONNEMENT | |
275 | DeltaU = (Ul-Uf)/(2+NbKnots); | |
276 | if (uperiodic) UPeriod = Ul-Uf; | |
277 | ||
278 | for (ii=1; ii<=myNbPts; ii++) { | |
279 | t = Standard_Real(myNbPts - ii)*f + Standard_Real(ii - 1)*l; | |
280 | t /= (myNbPts-1); | |
281 | myCurve->D0(t, P); | |
282 | Ok = myLaw->D0(t, T, N, B); | |
283 | if (!Ok) { | |
284 | myStatus = myLaw->ErrorStatus(); | |
285 | return; //Y a rien a faire. | |
286 | } | |
287 | gp_Dir D = T; | |
288 | if (WithTrans) { | |
289 | gp_Mat M(N.XYZ(), B.XYZ(), T.XYZ()); | |
290 | M *= Trans; | |
291 | D = M.Column(3); | |
292 | } | |
293 | gp_Ax1 Ax(P,D); // axe pour la surface de revoltuion | |
294 | ||
295 | // calculer transfo entre triedre et Oxyz | |
296 | gp_Dir N2 = N; | |
297 | gp_Ax3 N3(P,D,N2); | |
298 | gp_Trsf Transfo; | |
299 | Transfo.SetTransformation(N3, Rep); | |
300 | ||
301 | // transformer la section | |
302 | if (! isconst) { | |
303 | U = myFirstS + (t-myCurve->FirstParameter())*ratio; | |
304 | mySec->D0(U, Poles->ChangeArray1(), Weights->ChangeArray1()); | |
305 | if (israt) | |
306 | mySection = new (Geom_BSplineCurve) | |
307 | (Poles->Array1(), | |
308 | Weights->Array1(), | |
309 | Knots->Array1(), | |
310 | Mult->Array1(), | |
311 | Deg, mySec->IsUPeriodic()); | |
312 | else | |
313 | mySection = new (Geom_BSplineCurve) | |
314 | (Poles->Array1(), | |
315 | Knots->Array1(), | |
316 | Mult->Array1(), | |
317 | Deg, mySec->IsUPeriodic()); | |
318 | S = new (Geom_TrimmedCurve) (mySection, Uf, Ul); | |
319 | } | |
320 | else { | |
321 | S = new (Geom_TrimmedCurve) | |
322 | (Handle(Geom_Curve)::DownCast(mySection->Copy()), Uf, Ul); | |
323 | } | |
324 | S->Transform(Transfo); | |
325 | ||
326 | // Surface de revolution | |
327 | Revol = new(Geom_SurfaceOfRevolution) (S, Ax); | |
328 | ||
329 | Pl = new (GeomAdaptor_HSurface)(Revol); | |
330 | Int.Perform(myGuide, Pl); // intersection surf. revol / guide | |
331 | if (Int.NbPoints() == 0) { | |
332 | #if DEB | |
333 | cout <<"LocationGuide : Pas d'intersection"<<endl; | |
334 | TraceRevol(t, U, myLaw, mySec, myCurve, Trans); | |
335 | #endif | |
336 | Standard_Boolean SOS=Standard_False; | |
337 | if (ii>1) { | |
338 | // Intersection de secour entre surf revol et guide | |
339 | // equation | |
340 | X(1) = myPoles2d->Value(1,ii-1).Y(); | |
341 | X(2) = myPoles2d->Value(2,ii-1).X(); | |
342 | X(3) = myPoles2d->Value(2,ii-1).Y(); | |
343 | GeomFill_FunctionGuide E (mySec, myGuide, U); | |
344 | E.SetParam(U, P, T.XYZ(), N.XYZ()); | |
345 | // resolution => angle | |
346 | math_FunctionSetRoot Result(E, X, TolRes, | |
347 | Inf, Sup); | |
348 | ||
349 | if (Result.IsDone() && | |
350 | (Result.FunctionSetErrors().Norm() < TolRes(1)*TolRes(1)) ) { | |
351 | #if DEB | |
352 | cout << "Ratrappage Reussi !" << endl; | |
353 | #endif | |
354 | SOS = Standard_True; | |
355 | math_Vector RR(1,3); | |
356 | Result.Root(RR); | |
357 | PInt.SetValues(P, RR(2), RR(3), RR(1), IntCurveSurface_Out); | |
358 | } | |
359 | else { | |
360 | #if DEB | |
361 | cout << "Echec du Ratrappage !" << endl; | |
362 | #endif | |
363 | } | |
364 | } | |
365 | if (!SOS) { | |
366 | myStatus = GeomFill_ImpossibleContact; | |
367 | return; | |
368 | } | |
369 | } | |
370 | else { // on prend le point d'intersection | |
371 | // d'angle le plus proche de P | |
372 | PInt = Int.Point(1); | |
373 | a1 = PInt.U(); | |
c6541a0c | 374 | InGoodPeriod (CurAngle, 2*M_PI, a1); |
7fd59977 | 375 | Standard_Real Dmin = Abs(a1-CurAngle); |
376 | for (Standard_Integer jj=2;jj<=Int.NbPoints();jj++) { | |
377 | a2 = Int.Point(jj).U(); | |
c6541a0c | 378 | InGoodPeriod (CurAngle, 2*M_PI, a2); |
7fd59977 | 379 | if (Abs(a2-CurAngle) < Dmin) { |
380 | PInt = Int.Point(jj); | |
381 | Dmin = Abs(a2-CurAngle); | |
382 | }//if | |
383 | }//for | |
384 | }//else | |
385 | ||
386 | // Controle de w | |
387 | w = PInt.W(); | |
388 | if (ii>1) { | |
389 | Diff = w - myPoles2d->Value(1, ii-1).Y(); | |
390 | if (Abs(Diff) > DeltaG) { | |
391 | if (myGuide->IsPeriodic()) { | |
392 | InGoodPeriod (myPoles2d->Value(1, ii-1).Y(), | |
393 | myGuide->Period(), w); | |
394 | Diff = w - myPoles2d->Value(1, ii-1).Y(); | |
395 | } | |
396 | } | |
397 | ||
398 | #if DEB | |
399 | if (Abs(Diff) > DeltaG) { | |
400 | cout << "Location :: Diff on Guide : " << | |
401 | Diff << endl; | |
402 | } | |
403 | #endif | |
404 | } | |
405 | //Recadrage de l'angle. | |
406 | Angle = PInt.U(); | |
407 | if (ii > 1) { | |
408 | Diff = Angle - OldAngle; | |
c6541a0c D |
409 | if (Abs(Diff) > M_PI) { |
410 | InGoodPeriod (OldAngle, 2*M_PI, Angle); | |
7fd59977 | 411 | Diff = Angle - OldAngle; |
412 | } | |
413 | #if DEB | |
c6541a0c | 414 | if (Abs(Diff) > M_PI/4) { |
7fd59977 | 415 | cout << "Diff d'angle trop grand !!" << endl; |
416 | } | |
417 | #endif | |
418 | } | |
419 | ||
420 | ||
421 | //Recadrage du V | |
422 | v = PInt.V(); | |
423 | if (ii > 1) { | |
424 | if (uperiodic) { | |
425 | InGoodPeriod (myPoles2d->Value(2, ii-1).Y(), UPeriod, v); | |
426 | } | |
427 | Diff = v - myPoles2d->Value(2, ii-1).Y(); | |
428 | #if DEB | |
429 | if (Abs(Diff) > DeltaU) { | |
430 | cout << "Diff sur section trop grand !!" << endl; | |
431 | } | |
432 | #endif | |
433 | } | |
434 | ||
435 | p1.SetCoord(t, w); // on stocke les parametres | |
436 | p2.SetCoord(Angle , v); | |
437 | CurAngle = Angle; | |
438 | myPoles2d->SetValue(1, ii, p1); | |
439 | myPoles2d->SetValue(2, ii, p2); | |
440 | OldAngle = Angle; | |
441 | } | |
442 | ||
443 | LastAngle = CurAngle; | |
444 | rotation = Standard_True; //C'est pret ! | |
445 | } | |
446 | ||
447 | ||
448 | //================================================================== | |
449 | //Function: Set | |
450 | //Purpose : init loi de section et force la Rotation | |
451 | //================================================================== | |
452 | void GeomFill_LocationGuide::Set(const Handle(GeomFill_SectionLaw)& Section, | |
453 | const Standard_Boolean rotat, | |
454 | const Standard_Real SFirst, | |
455 | const Standard_Real SLast, | |
456 | const Standard_Real PrecAngle, | |
457 | Standard_Real& LastAngle) | |
458 | { | |
459 | myStatus = GeomFill_PipeOk; | |
460 | myFirstS = SFirst; | |
461 | myLastS = SLast; | |
462 | LastAngle = PrecAngle; | |
463 | if (myCurve.IsNull()) | |
464 | ratio = 0.; | |
465 | else | |
466 | ratio = (SLast-SFirst) / (myCurve->LastParameter() - | |
467 | myCurve->FirstParameter()); | |
468 | mySec = Section; | |
469 | ||
470 | if (rotat) SetRotation(PrecAngle, LastAngle); | |
471 | else rotation = Standard_False; | |
472 | } | |
473 | ||
474 | //================================================================== | |
475 | //Function: EraseRotation | |
476 | //Purpose : Supprime la Rotation | |
477 | //================================================================== | |
478 | void GeomFill_LocationGuide:: EraseRotation() | |
479 | { | |
480 | rotation = Standard_False; | |
481 | if (myStatus == GeomFill_ImpossibleContact) myStatus = GeomFill_PipeOk; | |
482 | } | |
483 | ||
484 | //================================================================== | |
485 | //Function: Copy | |
486 | //Purpose : | |
487 | //================================================================== | |
488 | Handle(GeomFill_LocationLaw) GeomFill_LocationGuide::Copy() const | |
489 | { | |
490 | Standard_Real la; | |
491 | Handle(GeomFill_TrihedronWithGuide) L; | |
492 | L = Handle(GeomFill_TrihedronWithGuide)::DownCast(myLaw->Copy()); | |
493 | Handle(GeomFill_LocationGuide) copy = new | |
494 | (GeomFill_LocationGuide) (L); | |
495 | copy->SetOrigine(OrigParam1, OrigParam2); | |
496 | copy->Set(mySec, rotation, myFirstS, myLastS, | |
497 | myPoles2d->Value(1,1).X(), la); | |
498 | copy->SetTrsf(Trans); | |
499 | ||
500 | return copy; | |
501 | } | |
502 | ||
503 | ||
504 | //================================================================== | |
505 | //Function: SetCurve | |
506 | //Purpose : Calcul des poles sur la surface d'arret (intersection | |
507 | // courbe guide / surface de revolution en myNbPts points) | |
508 | //================================================================== | |
509 | void GeomFill_LocationGuide::SetCurve(const Handle(Adaptor3d_HCurve)& C) | |
510 | { | |
511 | Standard_Real LastAngle; | |
512 | myCurve = C; | |
513 | myTrimmed = C; | |
514 | ||
515 | if (!myCurve.IsNull()){ | |
516 | myLaw->SetCurve(C); | |
517 | myLaw->Origine(OrigParam1, OrigParam2); | |
518 | myStatus = myLaw->ErrorStatus(); | |
519 | ||
520 | if (rotation) SetRotation(myPoles2d->Value(1,1).X(), LastAngle); | |
521 | } | |
522 | } | |
523 | ||
524 | //================================================================== | |
525 | //Function: GetCurve | |
526 | //Purpose : return the trajectoire | |
527 | //================================================================== | |
528 | const Handle(Adaptor3d_HCurve)& GeomFill_LocationGuide::GetCurve() const | |
529 | { | |
530 | return myCurve; | |
531 | } | |
532 | ||
533 | //================================================================== | |
534 | //Function: SetTrsf | |
535 | //Purpose : | |
536 | //================================================================== | |
537 | void GeomFill_LocationGuide::SetTrsf(const gp_Mat& Transfo) | |
538 | { | |
539 | Trans = Transfo; | |
540 | gp_Mat Aux; | |
541 | Aux.SetIdentity(); | |
542 | Aux -= Trans; | |
543 | WithTrans = Standard_False; // Au cas ou Trans = I | |
544 | for (Standard_Integer ii=1; ii<=3 && !WithTrans ; ii++) | |
545 | for (Standard_Integer jj=1; jj<=3 && !WithTrans; jj++) | |
546 | if (Abs(Aux.Value(ii, jj)) > 1.e-14) WithTrans = Standard_True; | |
547 | } | |
548 | ||
549 | //================================================================== | |
550 | //Function: D0 | |
551 | //Purpose : | |
552 | //================================================================== | |
553 | Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param, | |
554 | gp_Mat& M, | |
555 | gp_Vec& V) | |
556 | { | |
557 | Standard_Boolean Ok; | |
558 | gp_Vec T,N,B; | |
559 | gp_Pnt P; | |
560 | ||
561 | myCurve->D0(Param, P); | |
562 | V.SetXYZ(P.XYZ()); | |
563 | Ok = myLaw->D0(Param, T, N, B); | |
564 | if (!Ok) { | |
565 | myStatus = myLaw->ErrorStatus(); | |
566 | return Ok; | |
567 | } | |
568 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
569 | ||
570 | if (WithTrans) { | |
571 | M *= Trans; | |
572 | } | |
573 | ||
574 | if(rotation) { | |
575 | Standard_Real U = myFirstS + | |
576 | (Param-myCurve->FirstParameter())*ratio; | |
577 | // initialisations germe | |
578 | InitX(Param); | |
579 | ||
580 | Standard_Integer Iter = 100; | |
581 | gp_XYZ t,b,n; | |
582 | t = M.Column(3); | |
583 | b = M.Column(2); | |
584 | n = M.Column(1); | |
585 | ||
586 | // Intersection entre surf revol et guide | |
587 | // equation | |
588 | GeomFill_FunctionGuide E (mySec, myGuide, U); | |
589 | E.SetParam(Param, P, t, n); | |
590 | // resolution => angle | |
591 | math_FunctionSetRoot Result(E, X, TolRes, | |
592 | Inf, Sup, Iter); | |
593 | ||
594 | if (Result.IsDone()) { | |
595 | // solution | |
596 | Result.Root(R); | |
597 | ||
598 | // rotation | |
599 | gp_Mat Rot; | |
600 | Rot.SetRotation(t, R(2)); | |
601 | b *= Rot; | |
602 | n *= Rot; | |
603 | ||
604 | M.SetCols(n, b, t); | |
605 | } | |
606 | else { | |
607 | #if DEB | |
608 | cout << "LocationGuide::D0 : No Result !"<<endl; | |
609 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); | |
610 | #endif | |
611 | myStatus = GeomFill_ImpossibleContact; | |
612 | return Standard_False; | |
613 | } | |
614 | } | |
615 | ||
616 | return Standard_True; | |
617 | } | |
618 | ||
619 | //================================================================== | |
620 | //Function: D0 | |
621 | //Purpose : calcul de l'intersection (C0) surface revol / guide | |
622 | //================================================================== | |
623 | Standard_Boolean GeomFill_LocationGuide::D0(const Standard_Real Param, | |
624 | gp_Mat& M, | |
625 | gp_Vec& V, | |
626 | // TColgp_Array1OfPnt2d& Poles2d) | |
627 | TColgp_Array1OfPnt2d& ) | |
628 | { | |
629 | gp_Vec T, N, B; | |
630 | gp_Pnt P; | |
631 | Standard_Boolean Ok; | |
7fd59977 | 632 | |
633 | myCurve->D0(Param, P); | |
634 | V.SetXYZ(P.XYZ()); | |
635 | Ok = myLaw->D0(Param, T, N, B); | |
636 | if (!Ok) { | |
637 | myStatus = myLaw->ErrorStatus(); | |
638 | return Ok; | |
639 | } | |
640 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
641 | ||
642 | if (WithTrans) { | |
643 | M *= Trans; | |
644 | } | |
645 | ||
646 | if (rotation) { | |
7fd59977 | 647 | //initialisation du germe |
648 | InitX(Param); | |
649 | Standard_Integer Iter = 100; | |
650 | gp_XYZ b, n, t; | |
651 | t = M.Column(3); | |
652 | b = M.Column(2); | |
653 | n = M.Column(1); | |
654 | ||
655 | // equation d'intersection entre surf revol et guide => angle | |
656 | GeomFill_FunctionGuide E (mySec, myGuide, myFirstS + | |
657 | (Param-myCurve->FirstParameter())*ratio); | |
658 | E.SetParam(Param, P, t, n); | |
659 | ||
660 | // resolution | |
661 | math_FunctionSetRoot Result(E, X, TolRes, | |
662 | Inf, Sup, Iter); | |
663 | ||
664 | if (Result.IsDone()) { | |
665 | // solution | |
666 | Result.Root(R); | |
667 | ||
668 | // rotation | |
669 | gp_Mat Rot; | |
670 | Rot.SetRotation(t, R(2)); | |
671 | ||
672 | ||
673 | b *= Rot; | |
674 | n *= Rot; | |
675 | ||
676 | M.SetCols(n, b, t); | |
677 | } | |
678 | else { | |
679 | #if DEB | |
6e6cd5d9 | 680 | Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter()); |
7fd59977 | 681 | cout << "LocationGuide::D0 : No Result !"<<endl; |
682 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); | |
683 | #endif | |
684 | myStatus = GeomFill_ImpossibleContact; | |
685 | return Standard_False; | |
686 | } | |
687 | } | |
688 | ||
689 | return Standard_True; | |
690 | } | |
691 | ||
692 | ||
693 | //================================================================== | |
694 | //Function: D1 | |
695 | //Purpose : calcul de l'intersection (C1) surface revol / guide | |
696 | //================================================================== | |
697 | Standard_Boolean GeomFill_LocationGuide::D1(const Standard_Real Param, | |
698 | gp_Mat& M, | |
699 | gp_Vec& V, | |
700 | gp_Mat& DM, | |
701 | gp_Vec& DV, | |
702 | // TColgp_Array1OfPnt2d& Poles2d, | |
703 | TColgp_Array1OfPnt2d& , | |
704 | // TColgp_Array1OfVec2d& DPoles2d) | |
705 | TColgp_Array1OfVec2d& ) | |
706 | { | |
707 | // gp_Vec T, N, B, DT, DN, DB, T0, N0, B0; | |
708 | gp_Vec T, N, B, DT, DN, DB; | |
709 | // gp_Pnt P, P0; | |
710 | gp_Pnt P; | |
711 | Standard_Boolean Ok; | |
712 | ||
713 | myCurve->D1(Param, P, DV); | |
714 | V.SetXYZ(P.XYZ()); | |
715 | Ok = myLaw->D1(Param, T, DT, N, DN, B, DB); | |
716 | if (!Ok) { | |
717 | myStatus = myLaw->ErrorStatus(); | |
718 | return Ok; | |
719 | } | |
720 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
721 | DM.SetCols(DN.XYZ() , DB.XYZ(), DT.XYZ()); | |
722 | ||
723 | if (WithTrans) { | |
724 | M *= Trans; | |
725 | DM *= Trans; | |
726 | } | |
727 | ||
728 | if (rotation) { | |
729 | return Standard_False; | |
730 | ||
731 | #ifdef DEB | |
732 | Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter()); | |
733 | #else | |
734 | myCurve->FirstParameter() ; | |
735 | #endif | |
736 | ||
737 | // initialisation du germe | |
738 | InitX(Param); | |
739 | ||
740 | Standard_Integer Iter = 100; | |
741 | gp_XYZ t,b,n, dt, db, dn; | |
742 | t = M.Column(3); | |
743 | b = M.Column(2); | |
744 | n = M.Column(1); | |
745 | dt = M.Column(3); | |
746 | db = M.Column(2); | |
747 | dn = M.Column(1); | |
748 | ||
749 | // equation d'intersection surf revol / guide => angle | |
750 | GeomFill_FunctionGuide E (mySec, myGuide, myFirstS + | |
751 | (Param-myCurve->FirstParameter())*ratio); | |
752 | E.SetParam(Param, P, t, n); | |
753 | ||
754 | // resolution | |
755 | math_FunctionSetRoot Result(E, X, TolRes, | |
756 | Inf, Sup, Iter); | |
757 | ||
758 | if (Result.IsDone()) | |
759 | { | |
760 | // solution de la fonction | |
761 | Result.Root(R); | |
762 | ||
763 | // derivee de la fonction | |
764 | math_Vector DEDT(1,3); | |
765 | E.DerivT(R, DV.XYZ(), dt, DEDT); // dE/dt => DEDT | |
766 | ||
767 | math_Vector DSDT (1,3,0); | |
768 | math_Matrix DEDX (1,3,1,3,0); | |
769 | E.Derivatives(R, DEDX); // dE/dx au point R => DEDX | |
770 | ||
771 | // resolution du syst. : DEDX*DSDT = -DEDT | |
772 | math_Gauss Ga(DEDX); | |
773 | if (Ga.IsDone()) | |
774 | { | |
775 | Ga.Solve (DEDT.Opposite(), DSDT);// resolution du syst. | |
776 | }//if | |
777 | else { | |
778 | #if DEB | |
779 | cout << "DEDX = " << DEDX << endl; | |
780 | cout << "DEDT = " << DEDT << endl; | |
781 | #endif | |
782 | Standard_ConstructionError::Raise( | |
783 | "LocationGuide::D1 : No Result dans la derivee"); | |
784 | } | |
785 | ||
786 | // transformation = rotation | |
787 | gp_Mat Rot, DRot; | |
788 | Rot.SetRotation(t, R(2)); | |
789 | ||
790 | ||
791 | ||
792 | M.SetCols(n*Rot, b*Rot, t); | |
793 | ||
794 | // transfo entre triedre (en Q) et Oxyz | |
795 | gp_Ax3 Rep(gp::Origin(),gp::DZ(), gp::DX()); | |
796 | gp_Ax3 RepTriedre(gp::Origin(),t,n); | |
797 | gp_Trsf Transfo3; | |
798 | Transfo3.SetTransformation(Rep,RepTriedre); | |
799 | // on se place dans Oxyz | |
800 | Transfo3.Transforms(n); | |
801 | Transfo3.Transforms(b); | |
802 | Transfo3.Transforms(dn); | |
803 | Transfo3.Transforms(db); | |
804 | ||
805 | // matrices de rotation et derivees | |
806 | Standard_Real A = R(2); | |
807 | Standard_Real Aprim = DSDT(2); | |
808 | ||
809 | #ifdef DEB | |
810 | gp_Mat M2 (Cos(A), -Sin(A),0, // rotation autour de T | |
811 | Sin(A), Cos(A),0, | |
812 | 0,0,1); | |
813 | #endif | |
814 | ||
815 | gp_Mat M2prim (-Sin(A), -Cos(A), 0, // derivee rotation autour de T | |
816 | Cos(A), -Sin(A), 0, | |
817 | 0, 0, 0); | |
818 | M2prim.Multiply(Aprim); | |
819 | ||
820 | // transformations | |
821 | ||
822 | ||
823 | dn *= Rot; | |
824 | db *= Rot; | |
825 | ||
826 | n *= DRot; | |
827 | b *= DRot; | |
828 | ||
829 | dn += n; | |
830 | db += b; | |
831 | ||
832 | // on repasse dans repere triedre | |
833 | gp_Trsf InvTrsf; | |
834 | InvTrsf = Transfo3.Inverted(); | |
835 | InvTrsf.Transforms(dn); | |
836 | InvTrsf.Transforms(db); | |
837 | ||
838 | DM.SetCols(dn , db , dt); | |
839 | }//if_Result | |
840 | ||
841 | else { | |
842 | #if DEB | |
843 | cout << "LocationGuide::D1 : No Result !!"<<endl; | |
844 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); | |
845 | #endif | |
846 | myStatus = GeomFill_ImpossibleContact; | |
847 | return Standard_False; | |
848 | } | |
849 | ||
850 | }//if_rotation | |
851 | ||
852 | ||
853 | return Standard_True; | |
854 | ||
855 | } | |
856 | ||
857 | //================================================================== | |
858 | //Function: D2 | |
859 | //Purpose : calcul de l'intersection (C2) surface revol / guide | |
860 | //================================================================== | |
861 | Standard_Boolean GeomFill_LocationGuide::D2(const Standard_Real Param, | |
862 | gp_Mat& M, | |
863 | gp_Vec& V, | |
864 | gp_Mat& DM, | |
865 | gp_Vec& DV, | |
866 | gp_Mat& D2M, | |
867 | gp_Vec& D2V, | |
868 | // TColgp_Array1OfPnt2d& Poles2d, | |
869 | TColgp_Array1OfPnt2d& , | |
870 | // TColgp_Array1OfVec2d& DPoles2d, | |
871 | TColgp_Array1OfVec2d& , | |
872 | // TColgp_Array1OfVec2d& D2Poles2d) | |
873 | TColgp_Array1OfVec2d& ) | |
874 | { | |
875 | gp_Vec T, N, B, DT, DN, DB, D2T, D2N, D2B; | |
876 | // gp_Vec T0, N0, B0, T1, N1, B1; | |
877 | // gp_Pnt P, P0, P1; | |
878 | gp_Pnt P; | |
879 | Standard_Boolean Ok; | |
880 | ||
881 | myCurve->D2(Param, P, DV, D2V); | |
882 | V.SetXYZ(P.XYZ()); | |
883 | Ok = myLaw->D2(Param, T, DT, D2T, N, DN, D2N, B, DB, D2B); | |
884 | if (!Ok) { | |
885 | myStatus = myLaw->ErrorStatus(); | |
886 | return Ok; | |
887 | } | |
888 | ||
889 | if (WithTrans) { | |
890 | M *= Trans; | |
891 | DM *= Trans; | |
892 | D2M *= Trans; | |
893 | } | |
894 | ||
895 | if (rotation) | |
896 | { | |
897 | return Standard_False; | |
898 | /* | |
899 | Standard_Real U = myFirstS + | |
900 | (Param-myCurve->FirstParameter())*ratio; | |
901 | // rotation | |
902 | math_Vector X(1,3,0); | |
903 | InitX(Param,X); | |
904 | // tolerance sur X | |
905 | ||
906 | TolRes.Init(1.e-6); | |
907 | // tolerance sur E | |
908 | // Standard_Real ETol = 1.e-6; | |
909 | Standard_Integer Iter = 100; | |
910 | ||
911 | ||
912 | // resoudre equation d'intersection entre surf revol et guide => angle | |
913 | GeomFill_FunctionGuide E (mySec, myGuide, myFirstS + | |
914 | (Param-myCurve->FirstParameter())*ratio); | |
915 | E.SetParam(Param, P, T, N); | |
916 | ||
917 | // resolution | |
918 | math_FunctionSetRoot Result(E, X, TolRes, | |
919 | Inf, Sup, Iter); | |
920 | ||
921 | if (Result.IsDone()) | |
922 | { | |
923 | Result.Root(R); // solution | |
924 | ||
925 | //gp_Pnt2d p (R(2), R(3)); // point sur la surface (angle, v) | |
926 | //Poles2d.SetValue(1,p); | |
927 | ||
928 | // derivee de la fonction | |
929 | math_Vector DEDT(1,3,0); | |
930 | E.DerivT(Param, Param0, R, R0, DEDT); // dE/dt => DEDT | |
931 | math_Vector DSDT (1,3,0); | |
932 | math_Matrix DEDX (1,3,1,3,0); | |
933 | E.Derivatives(R, DEDX); // dE/dx au point R => DEDX | |
934 | ||
935 | // resolution du syst. lin. : DEDX*DSDT = -DEDT | |
936 | math_Gauss Ga(DEDX); | |
937 | if (Ga.IsDone()) | |
938 | { | |
939 | Ga.Solve (DEDT.Opposite(), DSDT); // resolution du syst. lin. | |
940 | //gp_Vec2d dp (DSDT(2), DSDT(3)); // surface | |
941 | //DPoles2d.SetValue(1, dp); | |
942 | }//if | |
943 | else cout <<"LocationGuide::D2 : No Result dans la derivee premiere"<<endl; | |
944 | ||
945 | // deuxieme derivee | |
946 | GeomFill_Tensor D2EDX2(3,3,3); | |
947 | E.Deriv2X(R, D2EDX2); // d2E/dx2 | |
948 | ||
949 | math_Vector D2EDT2(1,3,0); | |
950 | ||
951 | // if(Param1 < Param && Param < Param0) | |
952 | E.Deriv2T(Param1, Param, Param0, R1, R, R0, D2EDT2); // d2E/dt2 | |
953 | // else if (Param < Param0 && Param0 < Param1) | |
954 | // E.Deriv2T(Param, Param0, Param1, R, R0, R1, D2EDT2); // d2E/dt2 | |
955 | // else | |
956 | // E.Deriv2T(Param0, Param1, Param, R0, R1, R, D2EDT2); // d2E/dt2 | |
957 | ||
958 | math_Matrix D2EDTDX(1,3,1,3,0); | |
959 | E.DerivTX(Param, Param0, R, R0, D2EDTDX); // d2E/dtdx | |
960 | ||
961 | math_Vector D2SDT2(1,3,0); // d2s/dt2 | |
962 | math_Matrix M1(1,3,1,3,0); | |
963 | D2EDX2.Multiply(DSDT,M1); | |
964 | ||
965 | // resolution du syst. lin. | |
966 | math_Gauss Ga1 (DEDX); | |
967 | if (Ga1.IsDone()) | |
968 | { | |
969 | Ga1.Solve ( - M1*DSDT - 2*D2EDTDX*DSDT - D2EDT2 , D2SDT2); | |
970 | //gp_Vec2d d2p (D2SDT2(2), D2SDT2(3)); // surface | |
971 | //D2Poles2d.SetValue(1, d2p); | |
972 | }//if | |
973 | else { | |
974 | cout <<"LocationGuide::D2 : No Result dans la derivee seconde"<<endl; | |
975 | myStatus = GeomFill_ImpossibleContact; | |
976 | } | |
977 | ||
978 | //------------------------------------------ | |
979 | // rotation | |
980 | //------------------------------------------ | |
981 | ||
982 | gp_Trsf Tr; | |
983 | gp_Pnt Q (0, 0 ,0); | |
984 | gp_Ax1 Axe (Q, D); | |
985 | Tr.SetRotation(Axe, R(2)); | |
986 | ||
987 | gp_Vec b,b2; | |
988 | b = b2 = B; | |
989 | gp_Vec n,n2; | |
990 | n = n2 = N; | |
991 | ||
992 | B.Transform(Tr); | |
993 | N.Transform(Tr); | |
994 | ||
995 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
996 | ||
997 | //------------------------------------------ | |
998 | // derivees de la rotation | |
999 | // A VERIFIER !!!! | |
1000 | //----------------------------------------- | |
1001 | gp_Vec db,dn,db3,dn3; | |
1002 | db = db3 = DB; | |
1003 | dn = dn3 = DN; | |
1004 | ||
1005 | gp_Vec db1,dn1,db2,dn2; | |
1006 | ||
1007 | //transfo entre triedre et Oxyz | |
1008 | gp_Ax3 RepTriedre4(Q,D,B2); | |
1009 | gp_Trsf Transfo3; | |
1010 | Transfo3.SetTransformation(Rep,RepTriedre4); | |
1011 | ||
1012 | //on passe dans le repere du triedre | |
1013 | n.Transform(Transfo3); | |
1014 | b.Transform(Transfo3); | |
1015 | n2.Transform(Transfo3); | |
1016 | b2.Transform(Transfo3); | |
1017 | dn.Transform(Transfo3); | |
1018 | db.Transform(Transfo3); | |
1019 | dn3.Transform(Transfo3); | |
1020 | db3.Transform(Transfo3); | |
1021 | D2N.Transform(Transfo3); | |
1022 | D2B.Transform(Transfo3); | |
1023 | ||
1024 | //matrices de rotation et derivees | |
1025 | Standard_Real A = R(2); | |
1026 | Standard_Real Aprim = DSDT(2); | |
1027 | Standard_Real Asec = D2SDT2(2); | |
1028 | ||
1029 | gp_Mat M2 (Cos(A),-Sin(A),0, // rotation autour de T | |
1030 | Sin(A), Cos(A),0, | |
1031 | 0, 0, 1); | |
1032 | ||
1033 | gp_Mat M2prim (-Sin(A),-Cos(A),0, // derivee 1ere rotation autour de T | |
1034 | Cos(A), -Sin(A),0, | |
1035 | 0,0,0); | |
1036 | ||
1037 | gp_Mat M2sec (-Cos(A), Sin(A), 0, // derivee 2nde rotation autour de T | |
1038 | -Sin(A), -Cos(A), 0, | |
1039 | 0,0,0); | |
1040 | M2sec.Multiply(Aprim*Aprim); | |
1041 | gp_Mat M2p = M2prim.Multiplied(Asec); | |
1042 | M2sec.Add(M2p); | |
1043 | ||
1044 | M2prim.Multiply(Aprim); | |
1045 | ||
1046 | // transformation | |
1047 | gp_Trsf Rot; | |
1048 | Rot.SetValues(M2(1,1),M2(1,2),M2(1,3),0, | |
1049 | M2(2,1),M2(2,2),M2(2,3),0, | |
1050 | M2(3,1),M2(3,2),M2(3,3),0, | |
1051 | 1.e-8,1.e-8); | |
1052 | gp_Trsf DRot; | |
1053 | DRot.SetValues(M2prim(1,1),M2prim(1,2),M2prim(1,3),0, | |
1054 | M2prim(2,1),M2prim(2,2),M2prim(2,3),0, | |
1055 | M2prim(3,1),M2prim(3,2),M2prim(3,3),0, | |
1056 | 1.e-8,1.e-8); | |
1057 | ||
1058 | gp_Trsf D2Rot; | |
1059 | D2Rot.SetValues(M2sec(1,1),M2sec(1,2),M2sec(1,3),0, | |
1060 | M2sec(2,1),M2sec(2,2),M2sec(2,3),0, | |
1061 | M2sec(3,1),M2sec(3,2),M2sec(3,3),0, | |
1062 | 1.e-8,1.e-8); | |
1063 | ||
1064 | ||
1065 | //derivee premiere | |
1066 | dn.Transform(Rot); | |
1067 | db.Transform(Rot); | |
1068 | n.Transform(DRot); | |
1069 | b.Transform(DRot); | |
1070 | dn1 = dn + n; | |
1071 | db1 = db + b; | |
1072 | dn1.Transform(Transfo3.Inverted()); | |
1073 | db1.Transform(Transfo3.Inverted()); | |
1074 | ||
1075 | DM.SetCols(dn1.XYZ(), db1.XYZ(), DT.XYZ()); | |
1076 | ||
1077 | //derivee seconde | |
1078 | D2N.Transform(Rot); | |
1079 | D2B.Transform(Rot); | |
1080 | dn3.Transform(DRot); | |
1081 | db3.Transform(DRot); | |
1082 | n2.Transform(D2Rot); | |
1083 | b2.Transform(D2Rot); | |
1084 | dn2 = n2 + 2*dn3 + D2N; | |
1085 | db2 = b2 + 2*db3 + D2B; | |
1086 | dn2.Transform(Transfo3.Inverted()); | |
1087 | db2.Transform(Transfo3.Inverted()); | |
1088 | ||
1089 | D2M.SetCols(dn2.XYZ(), db2.XYZ(), D2T.XYZ()); | |
1090 | ||
1091 | }//if_result | |
1092 | else { | |
1093 | #if DEB | |
1094 | cout << "LocationGuide::D2 : No Result !!" <<endl; | |
1095 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); | |
1096 | #endif | |
1097 | return Standard_False; | |
1098 | }*/ | |
1099 | }//if_rotation | |
1100 | ||
1101 | else | |
1102 | { | |
1103 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); | |
1104 | DM.SetCols(DN.XYZ(), DB.XYZ(), DT.XYZ()); | |
1105 | D2M.SetCols(D2N.XYZ(), D2B.XYZ(), D2T.XYZ()); | |
1106 | } | |
1107 | ||
1108 | return Standard_True; | |
1109 | // return Standard_False; | |
1110 | } | |
1111 | ||
1112 | //================================================================== | |
1113 | //Function : HasFirstRestriction | |
1114 | //Purpose : | |
1115 | //================================================================== | |
1116 | Standard_Boolean GeomFill_LocationGuide::HasFirstRestriction() const | |
1117 | { | |
1118 | return Standard_False; | |
1119 | } | |
1120 | ||
1121 | //================================================================== | |
1122 | //Function : HasLastRestriction | |
1123 | //Purpose : | |
1124 | //================================================================== | |
1125 | Standard_Boolean GeomFill_LocationGuide::HasLastRestriction() const | |
1126 | { | |
1127 | return Standard_False; | |
1128 | } | |
1129 | ||
1130 | //================================================================== | |
1131 | //Function : TraceNumber | |
1132 | //Purpose : | |
1133 | //================================================================== | |
1134 | Standard_Integer GeomFill_LocationGuide::TraceNumber() const | |
1135 | { | |
1136 | return 0; | |
1137 | } | |
1138 | ||
1139 | //================================================================== | |
1140 | //Function : ErrorStatus | |
1141 | //Purpose : | |
1142 | //================================================================== | |
1143 | GeomFill_PipeError GeomFill_LocationGuide::ErrorStatus() const | |
1144 | { | |
1145 | return myStatus; | |
1146 | } | |
1147 | ||
1148 | //================================================================== | |
1149 | //Function:NbIntervals | |
1150 | //Purpose : | |
1151 | //================================================================== | |
1152 | Standard_Integer GeomFill_LocationGuide::NbIntervals | |
1153 | (const GeomAbs_Shape S) const | |
1154 | { | |
1155 | Standard_Integer Nb_Sec, Nb_Law; | |
1156 | Nb_Sec = myTrimmed->NbIntervals(S); | |
1157 | Nb_Law = myLaw->NbIntervals(S); | |
1158 | ||
1159 | if (Nb_Sec==1) { | |
1160 | return Nb_Law; | |
1161 | } | |
1162 | else if (Nb_Law==1) { | |
1163 | return Nb_Sec; | |
1164 | } | |
1165 | ||
1166 | TColStd_Array1OfReal IntC(1, Nb_Sec+1); | |
1167 | TColStd_Array1OfReal IntL(1, Nb_Law+1); | |
1168 | TColStd_SequenceOfReal Inter; | |
1169 | myTrimmed->Intervals(IntC, S); | |
1170 | myLaw->Intervals(IntL, S); | |
1171 | ||
1172 | GeomLib::FuseIntervals( IntC, IntL, Inter, Precision::PConfusion()*0.99); | |
1173 | return Inter.Length()-1; | |
1174 | ||
1175 | } | |
1176 | ||
1177 | //================================================================== | |
1178 | //Function:Intervals | |
1179 | //Purpose : | |
1180 | //================================================================== | |
1181 | void GeomFill_LocationGuide::Intervals(TColStd_Array1OfReal& T, | |
1182 | const GeomAbs_Shape S) const | |
1183 | { | |
1184 | Standard_Integer Nb_Sec, Nb_Law; | |
1185 | Nb_Sec = myTrimmed->NbIntervals(S); | |
1186 | Nb_Law = myLaw->NbIntervals(S); | |
1187 | ||
1188 | if (Nb_Sec==1) { | |
1189 | myLaw->Intervals(T, S); | |
1190 | return; | |
1191 | } | |
1192 | else if (Nb_Law==1) { | |
1193 | myTrimmed->Intervals(T, S); | |
1194 | return; | |
1195 | } | |
1196 | ||
1197 | TColStd_Array1OfReal IntC(1, Nb_Sec+1); | |
1198 | TColStd_Array1OfReal IntL(1, Nb_Law+1); | |
1199 | TColStd_SequenceOfReal Inter; | |
1200 | myTrimmed->Intervals(IntC, S); | |
1201 | myLaw->Intervals(IntL, S); | |
1202 | ||
1203 | GeomLib::FuseIntervals(IntC, IntL, Inter, Precision::PConfusion()*0.99); | |
1204 | for (Standard_Integer ii=1; ii<=Inter.Length(); ii++) | |
1205 | T(ii) = Inter(ii); | |
1206 | } | |
1207 | ||
1208 | //================================================================== | |
1209 | //Function:SetInterval | |
1210 | //Purpose : | |
1211 | //================================================================== | |
1212 | void GeomFill_LocationGuide::SetInterval(const Standard_Real First, | |
1213 | const Standard_Real Last) | |
1214 | { | |
1215 | myLaw->SetInterval(First, Last); | |
1216 | myTrimmed = myCurve->Trim(First, Last, 0); | |
1217 | } | |
1218 | //================================================================== | |
1219 | //Function: GetInterval | |
1220 | //Purpose : | |
1221 | //================================================================== | |
1222 | void GeomFill_LocationGuide::GetInterval(Standard_Real& First, | |
1223 | Standard_Real& Last) const | |
1224 | { | |
1225 | First = myTrimmed->FirstParameter(); | |
1226 | Last = myTrimmed->LastParameter(); | |
1227 | } | |
1228 | ||
1229 | //================================================================== | |
1230 | //Function: GetDomain | |
1231 | //Purpose : | |
1232 | //================================================================== | |
1233 | void GeomFill_LocationGuide::GetDomain(Standard_Real& First, | |
1234 | Standard_Real& Last) const | |
1235 | { | |
1236 | First = myCurve->FirstParameter(); | |
1237 | Last = myCurve->LastParameter(); | |
1238 | } | |
1239 | ||
1240 | //================================================================== | |
1241 | //function : SetTolerance | |
1242 | //purpose : | |
1243 | //================================================================== | |
1244 | void GeomFill_LocationGuide::SetTolerance(const Standard_Real Tol3d, | |
1245 | const Standard_Real ) | |
1246 | { | |
1247 | TolRes(1) = myGuide->Resolution(Tol3d); | |
1248 | Resolution(1, Tol3d, TolRes(2), TolRes(3)); | |
1249 | ||
1250 | } | |
1251 | ||
1252 | //================================================================== | |
1253 | //function : Resolution | |
1254 | //purpose : A definir | |
1255 | //================================================================== | |
1256 | //void GeomFill_LocationGuide::Resolution (const Standard_Integer Index, | |
1257 | void GeomFill_LocationGuide::Resolution (const Standard_Integer , | |
1258 | const Standard_Real Tol, | |
1259 | Standard_Real& TolU, | |
1260 | Standard_Real& TolV) const | |
1261 | { | |
1262 | TolU = Tol/100; | |
1263 | TolV = Tol/100; | |
1264 | } | |
1265 | ||
1266 | //================================================================== | |
1267 | //Function:GetMaximalNorm | |
1268 | //Purpose : On suppose les triedres normes => return 1 | |
1269 | //================================================================== | |
1270 | Standard_Real GeomFill_LocationGuide::GetMaximalNorm() | |
1271 | { | |
1272 | return 1.; | |
1273 | } | |
1274 | ||
1275 | //================================================================== | |
1276 | //Function:GetAverageLaw | |
1277 | //Purpose : | |
1278 | //================================================================== | |
1279 | void GeomFill_LocationGuide::GetAverageLaw(gp_Mat& AM, | |
1280 | gp_Vec& AV) | |
1281 | { | |
1282 | Standard_Integer ii; | |
1283 | Standard_Real U, delta; | |
1284 | gp_Vec V, V1, V2, V3; | |
1285 | ||
1286 | myLaw->GetAverageLaw(V1, V2, V3); | |
1287 | AM.SetCols(V1.XYZ(), V2.XYZ(), V3.XYZ()); | |
1288 | ||
1289 | AV.SetCoord(0., 0., 0.); | |
1290 | delta = (myTrimmed->LastParameter() - myTrimmed->FirstParameter())/10; | |
1291 | U = myTrimmed->FirstParameter(); | |
1292 | for (ii=0; ii<=myNbPts; ii++, U+=delta) { | |
1293 | V.SetXYZ( myTrimmed->Value(U).XYZ() ); | |
1294 | AV += V; | |
1295 | } | |
1296 | AV = AV/(myNbPts+1); | |
1297 | } | |
1298 | ||
1299 | ||
1300 | //================================================================== | |
1301 | //Function : Section | |
1302 | //Purpose : | |
1303 | //================================================================== | |
1304 | Handle(Geom_Curve) GeomFill_LocationGuide::Section() const | |
1305 | { | |
1306 | return mySec->ConstantSection(); | |
1307 | } | |
1308 | ||
1309 | //================================================================== | |
1310 | //Function : Guide | |
1311 | //Purpose : | |
1312 | //================================================================== | |
1313 | Handle(Adaptor3d_HCurve) GeomFill_LocationGuide::Guide() const | |
1314 | { | |
1315 | return myGuide; | |
1316 | } | |
1317 | ||
1318 | //================================================================== | |
1319 | //Function : IsRotation | |
1320 | //Purpose : | |
1321 | //================================================================== | |
1322 | // Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& Error) const | |
1323 | Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& ) const | |
1324 | { | |
1325 | return Standard_False; | |
1326 | } | |
1327 | ||
1328 | //================================================================== | |
1329 | //Function : Rotation | |
1330 | //Purpose : | |
1331 | //================================================================== | |
1332 | // void GeomFill_LocationGuide::Rotation(gp_Pnt& Centre) const | |
1333 | void GeomFill_LocationGuide::Rotation(gp_Pnt& ) const | |
1334 | { | |
1335 | Standard_NotImplemented::Raise("GeomFill_LocationGuide::Rotation"); | |
1336 | } | |
1337 | ||
1338 | //================================================================== | |
1339 | //Function : IsTranslation | |
1340 | //Purpose : | |
1341 | //================================================================== | |
1342 | // Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& Error) const | |
1343 | Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& ) const | |
1344 | { | |
1345 | return Standard_False; | |
1346 | } | |
1347 | ||
1348 | //================================================================== | |
1349 | //Function : InitX | |
1350 | //Purpose : recherche par interpolation d'une valeur initiale | |
1351 | //================================================================== | |
1352 | void GeomFill_LocationGuide::InitX(const Standard_Real Param) const | |
1353 | { | |
1354 | ||
1355 | Standard_Integer Ideb = 1, Ifin = myPoles2d->RowLength(), Idemi; | |
1356 | Standard_Real Valeur, t1, t2; | |
1357 | ||
1358 | ||
1359 | Valeur = myPoles2d->Value(1, Ideb).X(); | |
1360 | if (Param == Valeur) { | |
1361 | Ifin = Ideb+1; | |
1362 | } | |
1363 | ||
1364 | Valeur = myPoles2d->Value(1, Ifin).X(); | |
1365 | if (Param == Valeur) { | |
1366 | Ideb = Ifin-1; | |
1367 | } | |
1368 | ||
1369 | while ( Ideb+1 != Ifin) { | |
1370 | Idemi = (Ideb+Ifin)/2; | |
1371 | Valeur = myPoles2d->Value(1, Idemi).X(); | |
1372 | if (Valeur < Param) { | |
1373 | Ideb = Idemi; | |
1374 | } | |
1375 | else { | |
1376 | if ( Valeur > Param) { Ifin = Idemi;} | |
1377 | else { | |
1378 | Ideb = Idemi; | |
1379 | Ifin = Ideb+1; | |
1380 | } | |
1381 | } | |
1382 | } | |
1383 | ||
1384 | t1 = myPoles2d->Value(1,Ideb).X(); | |
1385 | t2 = myPoles2d->Value(1,Ifin).X(); | |
1386 | Standard_Real diff = t2-t1; | |
1387 | ||
1388 | Standard_Real W1, W2; | |
1389 | W1 = myPoles2d->Value(1,Ideb).Coord(2); | |
1390 | W2 = myPoles2d->Value(1,Ifin).Coord(2); | |
1391 | const gp_Pnt2d& P1 = myPoles2d->Value(2, Ideb); | |
1392 | const gp_Pnt2d& P2 = myPoles2d->Value(2, Ifin); | |
1393 | ||
1394 | if (diff > 1.e-7) { | |
1395 | Standard_Real b = (Param-t1) / diff, | |
1396 | a = (t2-Param) / diff; | |
1397 | X(1) = a * W1 + b * W2; | |
1398 | X(2) = a * P1.Coord(1) + b * P2.Coord(1); // angle | |
1399 | X(3) = a * P1.Coord(2) + b * P2.Coord(2); // param isov | |
1400 | } | |
1401 | else { | |
1402 | X(1) = (W1+W2) /2; | |
1403 | X(2) = (P1.Coord(1) + P2.Coord(1)) /2; | |
1404 | X(3) = (P1.Coord(2) + P2.Coord(2)) /2; | |
1405 | } | |
1406 | ||
1407 | if (myGuide->IsPeriodic()) { | |
1408 | X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(), | |
1409 | myGuide->LastParameter()); | |
1410 | } | |
c6541a0c | 1411 | X(2) = ElCLib::InPeriod(X(2), 0, 2*M_PI); |
7fd59977 | 1412 | if (mySec->IsUPeriodic()) { |
1413 | X(3) = ElCLib::InPeriod(X(3), Uf, Ul); | |
1414 | } | |
1415 | } | |
1416 | ||
1417 | ||
1418 | //================================================================== | |
1419 | //Function : SetOrigine | |
1420 | //Purpose : utilise pour ACR dans le cas ou la trajectoire est multi-edges | |
1421 | //================================================================== | |
1422 | void GeomFill_LocationGuide::SetOrigine(const Standard_Real Param1, | |
1423 | const Standard_Real Param2) | |
1424 | { | |
1425 | OrigParam1 = Param1; | |
1426 | OrigParam2 = Param2; | |
1427 | } | |
1428 |