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 | |
267 | Inf(2) = -PI; |
268 | Sup(2) = 3*PI; |
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(); |
374 | InGoodPeriod (CurAngle, 2*PI, a1); |
375 | Standard_Real Dmin = Abs(a1-CurAngle); |
376 | for (Standard_Integer jj=2;jj<=Int.NbPoints();jj++) { |
377 | a2 = Int.Point(jj).U(); |
378 | InGoodPeriod (CurAngle, 2*PI, a2); |
379 | if (Abs(a2-CurAngle) < Dmin) { |
380 | PInt = Int.Point(jj); |
381 | Dmin = Abs(a2-CurAngle); |
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; |
409 | if (Abs(Diff) > PI) { |
410 | InGoodPeriod (OldAngle, 2*PI, Angle); |
411 | Diff = Angle - OldAngle; |
412 | } |
413 | #if DEB |
414 | if (Abs(Diff) > PI/4) { |
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; |
632 | #ifdef DEB |
633 | Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter()); |
634 | #else |
635 | myCurve->FirstParameter() ; |
636 | #endif |
637 | |
638 | |
639 | myCurve->D0(Param, P); |
640 | V.SetXYZ(P.XYZ()); |
641 | Ok = myLaw->D0(Param, T, N, B); |
642 | if (!Ok) { |
643 | myStatus = myLaw->ErrorStatus(); |
644 | return Ok; |
645 | } |
646 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); |
647 | |
648 | if (WithTrans) { |
649 | M *= Trans; |
650 | } |
651 | |
652 | if (rotation) { |
653 | #ifdef DEB |
654 | Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter()); |
655 | #else |
656 | myCurve->FirstParameter() ; |
657 | #endif |
658 | |
659 | //initialisation du germe |
660 | InitX(Param); |
661 | Standard_Integer Iter = 100; |
662 | gp_XYZ b, n, t; |
663 | t = M.Column(3); |
664 | b = M.Column(2); |
665 | n = M.Column(1); |
666 | |
667 | // equation d'intersection entre surf revol et guide => angle |
668 | GeomFill_FunctionGuide E (mySec, myGuide, myFirstS + |
669 | (Param-myCurve->FirstParameter())*ratio); |
670 | E.SetParam(Param, P, t, n); |
671 | |
672 | // resolution |
673 | math_FunctionSetRoot Result(E, X, TolRes, |
674 | Inf, Sup, Iter); |
675 | |
676 | if (Result.IsDone()) { |
677 | // solution |
678 | Result.Root(R); |
679 | |
680 | // rotation |
681 | gp_Mat Rot; |
682 | Rot.SetRotation(t, R(2)); |
683 | |
684 | |
685 | b *= Rot; |
686 | n *= Rot; |
687 | |
688 | M.SetCols(n, b, t); |
689 | } |
690 | else { |
691 | #if DEB |
692 | cout << "LocationGuide::D0 : No Result !"<<endl; |
693 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); |
694 | #endif |
695 | myStatus = GeomFill_ImpossibleContact; |
696 | return Standard_False; |
697 | } |
698 | } |
699 | |
700 | return Standard_True; |
701 | } |
702 | |
703 | |
704 | //================================================================== |
705 | //Function: D1 |
706 | //Purpose : calcul de l'intersection (C1) surface revol / guide |
707 | //================================================================== |
708 | Standard_Boolean GeomFill_LocationGuide::D1(const Standard_Real Param, |
709 | gp_Mat& M, |
710 | gp_Vec& V, |
711 | gp_Mat& DM, |
712 | gp_Vec& DV, |
713 | // TColgp_Array1OfPnt2d& Poles2d, |
714 | TColgp_Array1OfPnt2d& , |
715 | // TColgp_Array1OfVec2d& DPoles2d) |
716 | TColgp_Array1OfVec2d& ) |
717 | { |
718 | // gp_Vec T, N, B, DT, DN, DB, T0, N0, B0; |
719 | gp_Vec T, N, B, DT, DN, DB; |
720 | // gp_Pnt P, P0; |
721 | gp_Pnt P; |
722 | Standard_Boolean Ok; |
723 | |
724 | myCurve->D1(Param, P, DV); |
725 | V.SetXYZ(P.XYZ()); |
726 | Ok = myLaw->D1(Param, T, DT, N, DN, B, DB); |
727 | if (!Ok) { |
728 | myStatus = myLaw->ErrorStatus(); |
729 | return Ok; |
730 | } |
731 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); |
732 | DM.SetCols(DN.XYZ() , DB.XYZ(), DT.XYZ()); |
733 | |
734 | if (WithTrans) { |
735 | M *= Trans; |
736 | DM *= Trans; |
737 | } |
738 | |
739 | if (rotation) { |
740 | return Standard_False; |
741 | |
742 | #ifdef DEB |
743 | Standard_Real U = myFirstS + ratio*(Param-myCurve->FirstParameter()); |
744 | #else |
745 | myCurve->FirstParameter() ; |
746 | #endif |
747 | |
748 | // initialisation du germe |
749 | InitX(Param); |
750 | |
751 | Standard_Integer Iter = 100; |
752 | gp_XYZ t,b,n, dt, db, dn; |
753 | t = M.Column(3); |
754 | b = M.Column(2); |
755 | n = M.Column(1); |
756 | dt = M.Column(3); |
757 | db = M.Column(2); |
758 | dn = M.Column(1); |
759 | |
760 | // equation d'intersection surf revol / guide => angle |
761 | GeomFill_FunctionGuide E (mySec, myGuide, myFirstS + |
762 | (Param-myCurve->FirstParameter())*ratio); |
763 | E.SetParam(Param, P, t, n); |
764 | |
765 | // resolution |
766 | math_FunctionSetRoot Result(E, X, TolRes, |
767 | Inf, Sup, Iter); |
768 | |
769 | if (Result.IsDone()) |
770 | { |
771 | // solution de la fonction |
772 | Result.Root(R); |
773 | |
774 | // derivee de la fonction |
775 | math_Vector DEDT(1,3); |
776 | E.DerivT(R, DV.XYZ(), dt, DEDT); // dE/dt => DEDT |
777 | |
778 | math_Vector DSDT (1,3,0); |
779 | math_Matrix DEDX (1,3,1,3,0); |
780 | E.Derivatives(R, DEDX); // dE/dx au point R => DEDX |
781 | |
782 | // resolution du syst. : DEDX*DSDT = -DEDT |
783 | math_Gauss Ga(DEDX); |
784 | if (Ga.IsDone()) |
785 | { |
786 | Ga.Solve (DEDT.Opposite(), DSDT);// resolution du syst. |
787 | }//if |
788 | else { |
789 | #if DEB |
790 | cout << "DEDX = " << DEDX << endl; |
791 | cout << "DEDT = " << DEDT << endl; |
792 | #endif |
793 | Standard_ConstructionError::Raise( |
794 | "LocationGuide::D1 : No Result dans la derivee"); |
795 | } |
796 | |
797 | // transformation = rotation |
798 | gp_Mat Rot, DRot; |
799 | Rot.SetRotation(t, R(2)); |
800 | |
801 | |
802 | |
803 | M.SetCols(n*Rot, b*Rot, t); |
804 | |
805 | // transfo entre triedre (en Q) et Oxyz |
806 | gp_Ax3 Rep(gp::Origin(),gp::DZ(), gp::DX()); |
807 | gp_Ax3 RepTriedre(gp::Origin(),t,n); |
808 | gp_Trsf Transfo3; |
809 | Transfo3.SetTransformation(Rep,RepTriedre); |
810 | // on se place dans Oxyz |
811 | Transfo3.Transforms(n); |
812 | Transfo3.Transforms(b); |
813 | Transfo3.Transforms(dn); |
814 | Transfo3.Transforms(db); |
815 | |
816 | // matrices de rotation et derivees |
817 | Standard_Real A = R(2); |
818 | Standard_Real Aprim = DSDT(2); |
819 | |
820 | #ifdef DEB |
821 | gp_Mat M2 (Cos(A), -Sin(A),0, // rotation autour de T |
822 | Sin(A), Cos(A),0, |
823 | 0,0,1); |
824 | #endif |
825 | |
826 | gp_Mat M2prim (-Sin(A), -Cos(A), 0, // derivee rotation autour de T |
827 | Cos(A), -Sin(A), 0, |
828 | 0, 0, 0); |
829 | M2prim.Multiply(Aprim); |
830 | |
831 | // transformations |
832 | |
833 | |
834 | dn *= Rot; |
835 | db *= Rot; |
836 | |
837 | n *= DRot; |
838 | b *= DRot; |
839 | |
840 | dn += n; |
841 | db += b; |
842 | |
843 | // on repasse dans repere triedre |
844 | gp_Trsf InvTrsf; |
845 | InvTrsf = Transfo3.Inverted(); |
846 | InvTrsf.Transforms(dn); |
847 | InvTrsf.Transforms(db); |
848 | |
849 | DM.SetCols(dn , db , dt); |
850 | }//if_Result |
851 | |
852 | else { |
853 | #if DEB |
854 | cout << "LocationGuide::D1 : No Result !!"<<endl; |
855 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); |
856 | #endif |
857 | myStatus = GeomFill_ImpossibleContact; |
858 | return Standard_False; |
859 | } |
860 | |
861 | }//if_rotation |
862 | |
863 | |
864 | return Standard_True; |
865 | |
866 | } |
867 | |
868 | //================================================================== |
869 | //Function: D2 |
870 | //Purpose : calcul de l'intersection (C2) surface revol / guide |
871 | //================================================================== |
872 | Standard_Boolean GeomFill_LocationGuide::D2(const Standard_Real Param, |
873 | gp_Mat& M, |
874 | gp_Vec& V, |
875 | gp_Mat& DM, |
876 | gp_Vec& DV, |
877 | gp_Mat& D2M, |
878 | gp_Vec& D2V, |
879 | // TColgp_Array1OfPnt2d& Poles2d, |
880 | TColgp_Array1OfPnt2d& , |
881 | // TColgp_Array1OfVec2d& DPoles2d, |
882 | TColgp_Array1OfVec2d& , |
883 | // TColgp_Array1OfVec2d& D2Poles2d) |
884 | TColgp_Array1OfVec2d& ) |
885 | { |
886 | gp_Vec T, N, B, DT, DN, DB, D2T, D2N, D2B; |
887 | // gp_Vec T0, N0, B0, T1, N1, B1; |
888 | // gp_Pnt P, P0, P1; |
889 | gp_Pnt P; |
890 | Standard_Boolean Ok; |
891 | |
892 | myCurve->D2(Param, P, DV, D2V); |
893 | V.SetXYZ(P.XYZ()); |
894 | Ok = myLaw->D2(Param, T, DT, D2T, N, DN, D2N, B, DB, D2B); |
895 | if (!Ok) { |
896 | myStatus = myLaw->ErrorStatus(); |
897 | return Ok; |
898 | } |
899 | |
900 | if (WithTrans) { |
901 | M *= Trans; |
902 | DM *= Trans; |
903 | D2M *= Trans; |
904 | } |
905 | |
906 | if (rotation) |
907 | { |
908 | return Standard_False; |
909 | /* |
910 | Standard_Real U = myFirstS + |
911 | (Param-myCurve->FirstParameter())*ratio; |
912 | // rotation |
913 | math_Vector X(1,3,0); |
914 | InitX(Param,X); |
915 | // tolerance sur X |
916 | |
917 | TolRes.Init(1.e-6); |
918 | // tolerance sur E |
919 | // Standard_Real ETol = 1.e-6; |
920 | Standard_Integer Iter = 100; |
921 | |
922 | |
923 | // resoudre equation d'intersection entre surf revol et guide => angle |
924 | GeomFill_FunctionGuide E (mySec, myGuide, myFirstS + |
925 | (Param-myCurve->FirstParameter())*ratio); |
926 | E.SetParam(Param, P, T, N); |
927 | |
928 | // resolution |
929 | math_FunctionSetRoot Result(E, X, TolRes, |
930 | Inf, Sup, Iter); |
931 | |
932 | if (Result.IsDone()) |
933 | { |
934 | Result.Root(R); // solution |
935 | |
936 | //gp_Pnt2d p (R(2), R(3)); // point sur la surface (angle, v) |
937 | //Poles2d.SetValue(1,p); |
938 | |
939 | // derivee de la fonction |
940 | math_Vector DEDT(1,3,0); |
941 | E.DerivT(Param, Param0, R, R0, DEDT); // dE/dt => DEDT |
942 | math_Vector DSDT (1,3,0); |
943 | math_Matrix DEDX (1,3,1,3,0); |
944 | E.Derivatives(R, DEDX); // dE/dx au point R => DEDX |
945 | |
946 | // resolution du syst. lin. : DEDX*DSDT = -DEDT |
947 | math_Gauss Ga(DEDX); |
948 | if (Ga.IsDone()) |
949 | { |
950 | Ga.Solve (DEDT.Opposite(), DSDT); // resolution du syst. lin. |
951 | //gp_Vec2d dp (DSDT(2), DSDT(3)); // surface |
952 | //DPoles2d.SetValue(1, dp); |
953 | }//if |
954 | else cout <<"LocationGuide::D2 : No Result dans la derivee premiere"<<endl; |
955 | |
956 | // deuxieme derivee |
957 | GeomFill_Tensor D2EDX2(3,3,3); |
958 | E.Deriv2X(R, D2EDX2); // d2E/dx2 |
959 | |
960 | math_Vector D2EDT2(1,3,0); |
961 | |
962 | // if(Param1 < Param && Param < Param0) |
963 | E.Deriv2T(Param1, Param, Param0, R1, R, R0, D2EDT2); // d2E/dt2 |
964 | // else if (Param < Param0 && Param0 < Param1) |
965 | // E.Deriv2T(Param, Param0, Param1, R, R0, R1, D2EDT2); // d2E/dt2 |
966 | // else |
967 | // E.Deriv2T(Param0, Param1, Param, R0, R1, R, D2EDT2); // d2E/dt2 |
968 | |
969 | math_Matrix D2EDTDX(1,3,1,3,0); |
970 | E.DerivTX(Param, Param0, R, R0, D2EDTDX); // d2E/dtdx |
971 | |
972 | math_Vector D2SDT2(1,3,0); // d2s/dt2 |
973 | math_Matrix M1(1,3,1,3,0); |
974 | D2EDX2.Multiply(DSDT,M1); |
975 | |
976 | // resolution du syst. lin. |
977 | math_Gauss Ga1 (DEDX); |
978 | if (Ga1.IsDone()) |
979 | { |
980 | Ga1.Solve ( - M1*DSDT - 2*D2EDTDX*DSDT - D2EDT2 , D2SDT2); |
981 | //gp_Vec2d d2p (D2SDT2(2), D2SDT2(3)); // surface |
982 | //D2Poles2d.SetValue(1, d2p); |
983 | }//if |
984 | else { |
985 | cout <<"LocationGuide::D2 : No Result dans la derivee seconde"<<endl; |
986 | myStatus = GeomFill_ImpossibleContact; |
987 | } |
988 | |
989 | //------------------------------------------ |
990 | // rotation |
991 | //------------------------------------------ |
992 | |
993 | gp_Trsf Tr; |
994 | gp_Pnt Q (0, 0 ,0); |
995 | gp_Ax1 Axe (Q, D); |
996 | Tr.SetRotation(Axe, R(2)); |
997 | |
998 | gp_Vec b,b2; |
999 | b = b2 = B; |
1000 | gp_Vec n,n2; |
1001 | n = n2 = N; |
1002 | |
1003 | B.Transform(Tr); |
1004 | N.Transform(Tr); |
1005 | |
1006 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); |
1007 | |
1008 | //------------------------------------------ |
1009 | // derivees de la rotation |
1010 | // A VERIFIER !!!! |
1011 | //----------------------------------------- |
1012 | gp_Vec db,dn,db3,dn3; |
1013 | db = db3 = DB; |
1014 | dn = dn3 = DN; |
1015 | |
1016 | gp_Vec db1,dn1,db2,dn2; |
1017 | |
1018 | //transfo entre triedre et Oxyz |
1019 | gp_Ax3 RepTriedre4(Q,D,B2); |
1020 | gp_Trsf Transfo3; |
1021 | Transfo3.SetTransformation(Rep,RepTriedre4); |
1022 | |
1023 | //on passe dans le repere du triedre |
1024 | n.Transform(Transfo3); |
1025 | b.Transform(Transfo3); |
1026 | n2.Transform(Transfo3); |
1027 | b2.Transform(Transfo3); |
1028 | dn.Transform(Transfo3); |
1029 | db.Transform(Transfo3); |
1030 | dn3.Transform(Transfo3); |
1031 | db3.Transform(Transfo3); |
1032 | D2N.Transform(Transfo3); |
1033 | D2B.Transform(Transfo3); |
1034 | |
1035 | //matrices de rotation et derivees |
1036 | Standard_Real A = R(2); |
1037 | Standard_Real Aprim = DSDT(2); |
1038 | Standard_Real Asec = D2SDT2(2); |
1039 | |
1040 | gp_Mat M2 (Cos(A),-Sin(A),0, // rotation autour de T |
1041 | Sin(A), Cos(A),0, |
1042 | 0, 0, 1); |
1043 | |
1044 | gp_Mat M2prim (-Sin(A),-Cos(A),0, // derivee 1ere rotation autour de T |
1045 | Cos(A), -Sin(A),0, |
1046 | 0,0,0); |
1047 | |
1048 | gp_Mat M2sec (-Cos(A), Sin(A), 0, // derivee 2nde rotation autour de T |
1049 | -Sin(A), -Cos(A), 0, |
1050 | 0,0,0); |
1051 | M2sec.Multiply(Aprim*Aprim); |
1052 | gp_Mat M2p = M2prim.Multiplied(Asec); |
1053 | M2sec.Add(M2p); |
1054 | |
1055 | M2prim.Multiply(Aprim); |
1056 | |
1057 | // transformation |
1058 | gp_Trsf Rot; |
1059 | Rot.SetValues(M2(1,1),M2(1,2),M2(1,3),0, |
1060 | M2(2,1),M2(2,2),M2(2,3),0, |
1061 | M2(3,1),M2(3,2),M2(3,3),0, |
1062 | 1.e-8,1.e-8); |
1063 | gp_Trsf DRot; |
1064 | DRot.SetValues(M2prim(1,1),M2prim(1,2),M2prim(1,3),0, |
1065 | M2prim(2,1),M2prim(2,2),M2prim(2,3),0, |
1066 | M2prim(3,1),M2prim(3,2),M2prim(3,3),0, |
1067 | 1.e-8,1.e-8); |
1068 | |
1069 | gp_Trsf D2Rot; |
1070 | D2Rot.SetValues(M2sec(1,1),M2sec(1,2),M2sec(1,3),0, |
1071 | M2sec(2,1),M2sec(2,2),M2sec(2,3),0, |
1072 | M2sec(3,1),M2sec(3,2),M2sec(3,3),0, |
1073 | 1.e-8,1.e-8); |
1074 | |
1075 | |
1076 | //derivee premiere |
1077 | dn.Transform(Rot); |
1078 | db.Transform(Rot); |
1079 | n.Transform(DRot); |
1080 | b.Transform(DRot); |
1081 | dn1 = dn + n; |
1082 | db1 = db + b; |
1083 | dn1.Transform(Transfo3.Inverted()); |
1084 | db1.Transform(Transfo3.Inverted()); |
1085 | |
1086 | DM.SetCols(dn1.XYZ(), db1.XYZ(), DT.XYZ()); |
1087 | |
1088 | //derivee seconde |
1089 | D2N.Transform(Rot); |
1090 | D2B.Transform(Rot); |
1091 | dn3.Transform(DRot); |
1092 | db3.Transform(DRot); |
1093 | n2.Transform(D2Rot); |
1094 | b2.Transform(D2Rot); |
1095 | dn2 = n2 + 2*dn3 + D2N; |
1096 | db2 = b2 + 2*db3 + D2B; |
1097 | dn2.Transform(Transfo3.Inverted()); |
1098 | db2.Transform(Transfo3.Inverted()); |
1099 | |
1100 | D2M.SetCols(dn2.XYZ(), db2.XYZ(), D2T.XYZ()); |
1101 | |
1102 | }//if_result |
1103 | else { |
1104 | #if DEB |
1105 | cout << "LocationGuide::D2 : No Result !!" <<endl; |
1106 | TraceRevol(Param, U, myLaw, mySec, myCurve, Trans); |
1107 | #endif |
1108 | return Standard_False; |
1109 | }*/ |
1110 | }//if_rotation |
1111 | |
1112 | else |
1113 | { |
1114 | M.SetCols(N.XYZ(), B.XYZ(), T.XYZ()); |
1115 | DM.SetCols(DN.XYZ(), DB.XYZ(), DT.XYZ()); |
1116 | D2M.SetCols(D2N.XYZ(), D2B.XYZ(), D2T.XYZ()); |
1117 | } |
1118 | |
1119 | return Standard_True; |
1120 | // return Standard_False; |
1121 | } |
1122 | |
1123 | //================================================================== |
1124 | //Function : HasFirstRestriction |
1125 | //Purpose : |
1126 | //================================================================== |
1127 | Standard_Boolean GeomFill_LocationGuide::HasFirstRestriction() const |
1128 | { |
1129 | return Standard_False; |
1130 | } |
1131 | |
1132 | //================================================================== |
1133 | //Function : HasLastRestriction |
1134 | //Purpose : |
1135 | //================================================================== |
1136 | Standard_Boolean GeomFill_LocationGuide::HasLastRestriction() const |
1137 | { |
1138 | return Standard_False; |
1139 | } |
1140 | |
1141 | //================================================================== |
1142 | //Function : TraceNumber |
1143 | //Purpose : |
1144 | //================================================================== |
1145 | Standard_Integer GeomFill_LocationGuide::TraceNumber() const |
1146 | { |
1147 | return 0; |
1148 | } |
1149 | |
1150 | //================================================================== |
1151 | //Function : ErrorStatus |
1152 | //Purpose : |
1153 | //================================================================== |
1154 | GeomFill_PipeError GeomFill_LocationGuide::ErrorStatus() const |
1155 | { |
1156 | return myStatus; |
1157 | } |
1158 | |
1159 | //================================================================== |
1160 | //Function:NbIntervals |
1161 | //Purpose : |
1162 | //================================================================== |
1163 | Standard_Integer GeomFill_LocationGuide::NbIntervals |
1164 | (const GeomAbs_Shape S) const |
1165 | { |
1166 | Standard_Integer Nb_Sec, Nb_Law; |
1167 | Nb_Sec = myTrimmed->NbIntervals(S); |
1168 | Nb_Law = myLaw->NbIntervals(S); |
1169 | |
1170 | if (Nb_Sec==1) { |
1171 | return Nb_Law; |
1172 | } |
1173 | else if (Nb_Law==1) { |
1174 | return Nb_Sec; |
1175 | } |
1176 | |
1177 | TColStd_Array1OfReal IntC(1, Nb_Sec+1); |
1178 | TColStd_Array1OfReal IntL(1, Nb_Law+1); |
1179 | TColStd_SequenceOfReal Inter; |
1180 | myTrimmed->Intervals(IntC, S); |
1181 | myLaw->Intervals(IntL, S); |
1182 | |
1183 | GeomLib::FuseIntervals( IntC, IntL, Inter, Precision::PConfusion()*0.99); |
1184 | return Inter.Length()-1; |
1185 | |
1186 | } |
1187 | |
1188 | //================================================================== |
1189 | //Function:Intervals |
1190 | //Purpose : |
1191 | //================================================================== |
1192 | void GeomFill_LocationGuide::Intervals(TColStd_Array1OfReal& T, |
1193 | const GeomAbs_Shape S) const |
1194 | { |
1195 | Standard_Integer Nb_Sec, Nb_Law; |
1196 | Nb_Sec = myTrimmed->NbIntervals(S); |
1197 | Nb_Law = myLaw->NbIntervals(S); |
1198 | |
1199 | if (Nb_Sec==1) { |
1200 | myLaw->Intervals(T, S); |
1201 | return; |
1202 | } |
1203 | else if (Nb_Law==1) { |
1204 | myTrimmed->Intervals(T, S); |
1205 | return; |
1206 | } |
1207 | |
1208 | TColStd_Array1OfReal IntC(1, Nb_Sec+1); |
1209 | TColStd_Array1OfReal IntL(1, Nb_Law+1); |
1210 | TColStd_SequenceOfReal Inter; |
1211 | myTrimmed->Intervals(IntC, S); |
1212 | myLaw->Intervals(IntL, S); |
1213 | |
1214 | GeomLib::FuseIntervals(IntC, IntL, Inter, Precision::PConfusion()*0.99); |
1215 | for (Standard_Integer ii=1; ii<=Inter.Length(); ii++) |
1216 | T(ii) = Inter(ii); |
1217 | } |
1218 | |
1219 | //================================================================== |
1220 | //Function:SetInterval |
1221 | //Purpose : |
1222 | //================================================================== |
1223 | void GeomFill_LocationGuide::SetInterval(const Standard_Real First, |
1224 | const Standard_Real Last) |
1225 | { |
1226 | myLaw->SetInterval(First, Last); |
1227 | myTrimmed = myCurve->Trim(First, Last, 0); |
1228 | } |
1229 | //================================================================== |
1230 | //Function: GetInterval |
1231 | //Purpose : |
1232 | //================================================================== |
1233 | void GeomFill_LocationGuide::GetInterval(Standard_Real& First, |
1234 | Standard_Real& Last) const |
1235 | { |
1236 | First = myTrimmed->FirstParameter(); |
1237 | Last = myTrimmed->LastParameter(); |
1238 | } |
1239 | |
1240 | //================================================================== |
1241 | //Function: GetDomain |
1242 | //Purpose : |
1243 | //================================================================== |
1244 | void GeomFill_LocationGuide::GetDomain(Standard_Real& First, |
1245 | Standard_Real& Last) const |
1246 | { |
1247 | First = myCurve->FirstParameter(); |
1248 | Last = myCurve->LastParameter(); |
1249 | } |
1250 | |
1251 | //================================================================== |
1252 | //function : SetTolerance |
1253 | //purpose : |
1254 | //================================================================== |
1255 | void GeomFill_LocationGuide::SetTolerance(const Standard_Real Tol3d, |
1256 | const Standard_Real ) |
1257 | { |
1258 | TolRes(1) = myGuide->Resolution(Tol3d); |
1259 | Resolution(1, Tol3d, TolRes(2), TolRes(3)); |
1260 | |
1261 | } |
1262 | |
1263 | //================================================================== |
1264 | //function : Resolution |
1265 | //purpose : A definir |
1266 | //================================================================== |
1267 | //void GeomFill_LocationGuide::Resolution (const Standard_Integer Index, |
1268 | void GeomFill_LocationGuide::Resolution (const Standard_Integer , |
1269 | const Standard_Real Tol, |
1270 | Standard_Real& TolU, |
1271 | Standard_Real& TolV) const |
1272 | { |
1273 | TolU = Tol/100; |
1274 | TolV = Tol/100; |
1275 | } |
1276 | |
1277 | //================================================================== |
1278 | //Function:GetMaximalNorm |
1279 | //Purpose : On suppose les triedres normes => return 1 |
1280 | //================================================================== |
1281 | Standard_Real GeomFill_LocationGuide::GetMaximalNorm() |
1282 | { |
1283 | return 1.; |
1284 | } |
1285 | |
1286 | //================================================================== |
1287 | //Function:GetAverageLaw |
1288 | //Purpose : |
1289 | //================================================================== |
1290 | void GeomFill_LocationGuide::GetAverageLaw(gp_Mat& AM, |
1291 | gp_Vec& AV) |
1292 | { |
1293 | Standard_Integer ii; |
1294 | Standard_Real U, delta; |
1295 | gp_Vec V, V1, V2, V3; |
1296 | |
1297 | myLaw->GetAverageLaw(V1, V2, V3); |
1298 | AM.SetCols(V1.XYZ(), V2.XYZ(), V3.XYZ()); |
1299 | |
1300 | AV.SetCoord(0., 0., 0.); |
1301 | delta = (myTrimmed->LastParameter() - myTrimmed->FirstParameter())/10; |
1302 | U = myTrimmed->FirstParameter(); |
1303 | for (ii=0; ii<=myNbPts; ii++, U+=delta) { |
1304 | V.SetXYZ( myTrimmed->Value(U).XYZ() ); |
1305 | AV += V; |
1306 | } |
1307 | AV = AV/(myNbPts+1); |
1308 | } |
1309 | |
1310 | |
1311 | //================================================================== |
1312 | //Function : Section |
1313 | //Purpose : |
1314 | //================================================================== |
1315 | Handle(Geom_Curve) GeomFill_LocationGuide::Section() const |
1316 | { |
1317 | return mySec->ConstantSection(); |
1318 | } |
1319 | |
1320 | //================================================================== |
1321 | //Function : Guide |
1322 | //Purpose : |
1323 | //================================================================== |
1324 | Handle(Adaptor3d_HCurve) GeomFill_LocationGuide::Guide() const |
1325 | { |
1326 | return myGuide; |
1327 | } |
1328 | |
1329 | //================================================================== |
1330 | //Function : IsRotation |
1331 | //Purpose : |
1332 | //================================================================== |
1333 | // Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& Error) const |
1334 | Standard_Boolean GeomFill_LocationGuide::IsRotation(Standard_Real& ) const |
1335 | { |
1336 | return Standard_False; |
1337 | } |
1338 | |
1339 | //================================================================== |
1340 | //Function : Rotation |
1341 | //Purpose : |
1342 | //================================================================== |
1343 | // void GeomFill_LocationGuide::Rotation(gp_Pnt& Centre) const |
1344 | void GeomFill_LocationGuide::Rotation(gp_Pnt& ) const |
1345 | { |
1346 | Standard_NotImplemented::Raise("GeomFill_LocationGuide::Rotation"); |
1347 | } |
1348 | |
1349 | //================================================================== |
1350 | //Function : IsTranslation |
1351 | //Purpose : |
1352 | //================================================================== |
1353 | // Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& Error) const |
1354 | Standard_Boolean GeomFill_LocationGuide::IsTranslation(Standard_Real& ) const |
1355 | { |
1356 | return Standard_False; |
1357 | } |
1358 | |
1359 | //================================================================== |
1360 | //Function : InitX |
1361 | //Purpose : recherche par interpolation d'une valeur initiale |
1362 | //================================================================== |
1363 | void GeomFill_LocationGuide::InitX(const Standard_Real Param) const |
1364 | { |
1365 | |
1366 | Standard_Integer Ideb = 1, Ifin = myPoles2d->RowLength(), Idemi; |
1367 | Standard_Real Valeur, t1, t2; |
1368 | |
1369 | |
1370 | Valeur = myPoles2d->Value(1, Ideb).X(); |
1371 | if (Param == Valeur) { |
1372 | Ifin = Ideb+1; |
1373 | } |
1374 | |
1375 | Valeur = myPoles2d->Value(1, Ifin).X(); |
1376 | if (Param == Valeur) { |
1377 | Ideb = Ifin-1; |
1378 | } |
1379 | |
1380 | while ( Ideb+1 != Ifin) { |
1381 | Idemi = (Ideb+Ifin)/2; |
1382 | Valeur = myPoles2d->Value(1, Idemi).X(); |
1383 | if (Valeur < Param) { |
1384 | Ideb = Idemi; |
1385 | } |
1386 | else { |
1387 | if ( Valeur > Param) { Ifin = Idemi;} |
1388 | else { |
1389 | Ideb = Idemi; |
1390 | Ifin = Ideb+1; |
1391 | } |
1392 | } |
1393 | } |
1394 | |
1395 | t1 = myPoles2d->Value(1,Ideb).X(); |
1396 | t2 = myPoles2d->Value(1,Ifin).X(); |
1397 | Standard_Real diff = t2-t1; |
1398 | |
1399 | Standard_Real W1, W2; |
1400 | W1 = myPoles2d->Value(1,Ideb).Coord(2); |
1401 | W2 = myPoles2d->Value(1,Ifin).Coord(2); |
1402 | const gp_Pnt2d& P1 = myPoles2d->Value(2, Ideb); |
1403 | const gp_Pnt2d& P2 = myPoles2d->Value(2, Ifin); |
1404 | |
1405 | if (diff > 1.e-7) { |
1406 | Standard_Real b = (Param-t1) / diff, |
1407 | a = (t2-Param) / diff; |
1408 | X(1) = a * W1 + b * W2; |
1409 | X(2) = a * P1.Coord(1) + b * P2.Coord(1); // angle |
1410 | X(3) = a * P1.Coord(2) + b * P2.Coord(2); // param isov |
1411 | } |
1412 | else { |
1413 | X(1) = (W1+W2) /2; |
1414 | X(2) = (P1.Coord(1) + P2.Coord(1)) /2; |
1415 | X(3) = (P1.Coord(2) + P2.Coord(2)) /2; |
1416 | } |
1417 | |
1418 | if (myGuide->IsPeriodic()) { |
1419 | X(1) = ElCLib::InPeriod(X(1), myGuide->FirstParameter(), |
1420 | myGuide->LastParameter()); |
1421 | } |
1422 | X(2) = ElCLib::InPeriod(X(2), 0, 2*PI); |
1423 | if (mySec->IsUPeriodic()) { |
1424 | X(3) = ElCLib::InPeriod(X(3), Uf, Ul); |
1425 | } |
1426 | } |
1427 | |
1428 | |
1429 | //================================================================== |
1430 | //Function : SetOrigine |
1431 | //Purpose : utilise pour ACR dans le cas ou la trajectoire est multi-edges |
1432 | //================================================================== |
1433 | void GeomFill_LocationGuide::SetOrigine(const Standard_Real Param1, |
1434 | const Standard_Real Param2) |
1435 | { |
1436 | OrigParam1 = Param1; |
1437 | OrigParam2 = Param2; |
1438 | } |
1439 | |