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