7fd59977 |
1 | // File: IntAna_Curve.cxx |
2 | // Created: Tue Jun 30 10:40:07 1992 |
3 | // Author: Laurent BUCHARD |
4 | // <lbr@topsn3> |
5 | |
6 | #ifndef DEB |
7 | #define No_Standard_RangeError |
8 | #define No_Standard_OutOfRange |
9 | #endif |
10 | |
11 | //---------------------------------------------------------------------- |
12 | //-- Differents constructeurs sont proposes qui correspondent aux |
13 | //-- polynomes en Z : |
14 | //-- A(Sin(Theta),Cos(Theta)) Z**2 |
15 | //-- + B(Sin(Theta),Cos(Theta)) Z |
16 | //-- + C(Sin(Theta),Cos(Theta)) |
17 | //-- |
18 | //-- Une Courbe est definie sur un domaine |
19 | //-- |
20 | //-- Value retourne le point de parametre U(Theta),V(Theta) |
21 | //-- ou V est la solution du polynome A V**2 + B V + C |
22 | //-- (Selon les cas, on prend V+ ou V-) |
23 | //-- |
24 | //-- D1u calcule le vecteur tangent a la courbe |
25 | //-- et retourne le booleen Standard_False si ce calcul ne peut |
26 | //-- pas etre mene a bien. |
27 | //---------------------------------------------------------------------- |
28 | |
29 | #include <Precision.hxx> |
30 | |
31 | #include <IntAna_Curve.ixx> |
32 | |
33 | #include <Standard_DomainError.hxx> |
34 | #include <math_DirectPolynomialRoots.hxx> |
35 | #include <ElSLib.hxx> |
36 | #include <gp_XYZ.hxx> |
37 | |
38 | //======================================================================= |
39 | //function : IntAna_Curve |
40 | //purpose : |
41 | //======================================================================= |
42 | IntAna_Curve::IntAna_Curve() |
43 | { |
44 | typequadric=GeomAbs_OtherSurface; |
45 | firstbounded=Standard_False; |
46 | lastbounded=Standard_False; |
47 | } |
48 | //======================================================================= |
49 | //function : SetConeQuadValues |
50 | //purpose : Description de l intersection Cone Quadrique |
51 | //======================================================================= |
52 | void IntAna_Curve::SetConeQuadValues(const gp_Cone& Cone, |
53 | const Standard_Real Qxx, |
54 | const Standard_Real Qyy, |
55 | const Standard_Real Qzz, |
56 | const Standard_Real Qxy, |
57 | const Standard_Real Qxz, |
58 | const Standard_Real Qyz, |
59 | const Standard_Real Qx, |
60 | const Standard_Real Qy, |
61 | const Standard_Real Qz, |
62 | const Standard_Real Q1, |
63 | const Standard_Real TOL, |
64 | const Standard_Real DomInf, |
65 | const Standard_Real DomSup, |
66 | const Standard_Boolean twocurves, |
67 | const Standard_Boolean takezpositive) |
68 | { |
69 | |
70 | Ax3 = Cone.Position(); |
71 | RCyl = Cone.RefRadius(); |
72 | |
73 | Angle = Cone.SemiAngle(); |
74 | Standard_Real UnSurTgAngle = 1.0/(Tan(Cone.SemiAngle())); |
75 | |
76 | typequadric= GeomAbs_Cone; |
77 | |
78 | TwoCurves = twocurves; //-- deux Z pour un meme parametre |
79 | TakeZPositive = takezpositive; //-- Prendre sur la courbe le Z Positif |
80 | //-- ( -B + Sqrt()) et non (-B - Sqrt()) |
81 | |
82 | |
83 | Z0Cte = Q1; //-- Attention On a Z?Cos Cos(t) |
84 | Z0Sin = 0.0; //-- et Non 2 Z?Cos Cos(t) !!! |
85 | Z0Cos = 0.0; //-- Ce pour tous les Parametres |
86 | Z0CosCos = 0.0; //-- ie pas de Coefficient 2 |
87 | Z0SinSin = 0.0; //-- devant les termes CS C S |
88 | Z0CosSin = 0.0; |
89 | |
90 | Z1Cte = 2.0*(UnSurTgAngle)*Qz; |
91 | Z1Sin = Qy+Qy; |
92 | Z1Cos = Qx+Qx; |
93 | Z1CosCos = 0.0; |
94 | Z1SinSin = 0.0; |
95 | Z1CosSin = 0.0; |
96 | |
97 | Z2Cte = Qzz * UnSurTgAngle*UnSurTgAngle; |
98 | Z2Sin = (UnSurTgAngle+UnSurTgAngle)*Qyz; |
99 | Z2Cos = (UnSurTgAngle+UnSurTgAngle)*Qxz; |
100 | Z2CosCos = Qxx; |
101 | Z2SinSin = Qyy; |
102 | Z2CosSin = Qxy+Qxy; |
103 | |
104 | Tolerance = TOL; |
105 | DomainInf = DomInf; |
106 | DomainSup = DomSup; |
107 | |
108 | RestrictedInf = RestrictedSup = Standard_True; //-- Le Domaine est Borne |
109 | firstbounded = lastbounded = Standard_False; |
110 | } |
111 | |
112 | //======================================================================= |
113 | //function : SetCylinderQuadValues |
114 | //purpose : Description de l intersection Cylindre Quadrique |
115 | //======================================================================= |
116 | void IntAna_Curve::SetCylinderQuadValues(const gp_Cylinder& Cyl, |
117 | const Standard_Real Qxx, |
118 | const Standard_Real Qyy, |
119 | const Standard_Real Qzz, |
120 | const Standard_Real Qxy, |
121 | const Standard_Real Qxz, |
122 | const Standard_Real Qyz, |
123 | const Standard_Real Qx, |
124 | const Standard_Real Qy, |
125 | const Standard_Real Qz, |
126 | const Standard_Real Q1, |
127 | const Standard_Real TOL, |
128 | const Standard_Real DomInf, |
129 | const Standard_Real DomSup, |
130 | const Standard_Boolean twocurves, |
131 | const Standard_Boolean takezpositive) |
132 | { |
133 | |
134 | Ax3 = Cyl.Position(); |
135 | RCyl = Cyl.Radius(); |
136 | typequadric= GeomAbs_Cylinder; |
137 | |
138 | TwoCurves = twocurves; //-- deux Z pour un meme parametre |
139 | TakeZPositive = takezpositive; //-- Prendre sur la courbe le Z Positif |
140 | Standard_Real RCylmul2 = RCyl+RCyl; //-- ( -B + Sqrt()) |
141 | |
142 | Z0Cte = Q1; |
143 | Z0Sin = RCylmul2*Qy; |
144 | Z0Cos = RCylmul2*Qx; |
145 | Z0CosCos = Qxx*RCyl*RCyl; |
146 | Z0SinSin = Qyy*RCyl*RCyl; |
147 | Z0CosSin = RCylmul2*RCyl*Qxy; |
148 | |
149 | Z1Cte = Qz+Qz; |
150 | Z1Sin = RCylmul2*Qyz; |
151 | Z1Cos = RCylmul2*Qxz; |
152 | Z1CosCos = 0.0; |
153 | Z1SinSin = 0.0; |
154 | Z1CosSin = 0.0; |
155 | |
156 | Z2Cte = Qzz; |
157 | Z2Sin = 0.0; |
158 | Z2Cos = 0.0; |
159 | Z2CosCos = 0.0; |
160 | Z2SinSin = 0.0; |
161 | Z2CosSin = 0.0; |
162 | |
163 | Tolerance = TOL; |
164 | DomainInf = DomInf; |
165 | DomainSup = DomSup; |
166 | |
167 | RestrictedInf = RestrictedSup = Standard_True; |
168 | firstbounded = lastbounded = Standard_False; |
169 | } |
170 | |
171 | //======================================================================= |
172 | //function : IsOpen |
173 | //purpose : |
174 | //======================================================================= |
175 | Standard_Boolean IntAna_Curve::IsOpen() const |
176 | { |
177 | return(RestrictedInf && RestrictedSup); |
178 | } |
179 | |
180 | //======================================================================= |
181 | //function : Domain |
182 | //purpose : |
183 | //======================================================================= |
184 | void IntAna_Curve::Domain(Standard_Real& DInf, |
185 | Standard_Real& DSup) const |
186 | { |
187 | if(RestrictedInf && RestrictedSup) { |
188 | DInf=DomainInf; |
189 | DSup=DomainSup; |
190 | if(TwoCurves) { |
191 | DSup+=DSup-DInf; |
192 | } |
193 | } |
194 | else { |
195 | Standard_DomainError::Raise("IntAna_Curve::Domain"); |
196 | } |
197 | } |
198 | //======================================================================= |
199 | //function : IsConstant |
200 | //purpose : |
201 | //======================================================================= |
202 | Standard_Boolean IntAna_Curve::IsConstant() const |
203 | { |
204 | //-- ??? Pas facile de decider a la seule vue des Param. |
205 | return(Standard_False); |
206 | } |
207 | |
208 | //======================================================================= |
209 | //function : IsFirstOpen |
210 | //purpose : |
211 | //======================================================================= |
212 | Standard_Boolean IntAna_Curve::IsFirstOpen() const |
213 | { |
214 | return(firstbounded); |
215 | } |
216 | |
217 | //======================================================================= |
218 | //function : IsLastOpen |
219 | //purpose : |
220 | //======================================================================= |
221 | Standard_Boolean IntAna_Curve::IsLastOpen() const |
222 | { |
223 | return(lastbounded); |
224 | } |
225 | //======================================================================= |
226 | //function : SetIsFirstOpen |
227 | //purpose : |
228 | //======================================================================= |
229 | void IntAna_Curve::SetIsFirstOpen(const Standard_Boolean Flag) |
230 | { |
231 | firstbounded = Flag; |
232 | } |
233 | |
234 | //======================================================================= |
235 | //function : SetIsLastOpen |
236 | //purpose : |
237 | //======================================================================= |
238 | void IntAna_Curve::SetIsLastOpen(const Standard_Boolean Flag) |
239 | { |
240 | lastbounded = Flag; |
241 | } |
242 | |
243 | //======================================================================= |
244 | //function : InternalUVValue |
245 | //purpose : |
246 | //======================================================================= |
247 | void IntAna_Curve::InternalUVValue(const Standard_Real theta, |
248 | Standard_Real& Param1, |
249 | Standard_Real& Param2, |
250 | Standard_Real& A, |
251 | Standard_Real& B, |
252 | Standard_Real& C, |
253 | Standard_Real& cost, |
254 | Standard_Real& sint, |
255 | Standard_Real& SigneSqrtDis) const |
256 | { |
257 | Standard_Real Theta=theta; |
258 | Standard_Boolean SecondSolution=Standard_False; |
259 | |
260 | if((Theta<DomainInf) || |
261 | ((Theta>DomainSup) && (!TwoCurves)) || |
262 | (Theta>(DomainSup+DomainSup-DomainInf+0.00000000000001))) { |
263 | Standard_DomainError::Raise("IntAna_Curve::Domain"); |
264 | } |
265 | |
266 | if(Theta>DomainSup) { |
267 | Theta=DomainSup+DomainSup-Theta; |
268 | SecondSolution=Standard_True; |
269 | } |
270 | |
271 | Param1=Theta; |
272 | |
273 | if(!TwoCurves) { |
274 | SecondSolution=TakeZPositive; |
275 | } |
276 | // |
277 | cost = Cos(Theta); |
278 | sint = Sin(Theta); |
279 | Standard_Real costsint = cost*sint; |
280 | |
281 | A=Z2Cte+sint*(Z2Sin+sint*Z2SinSin)+cost*(Z2Cos+cost*Z2CosCos) |
282 | +Z2CosSin*costsint; |
283 | |
284 | B=Z1Cte+sint*(Z1Sin+sint*Z1SinSin)+cost*(Z1Cos+cost*Z1CosCos) |
285 | +Z1CosSin*costsint; |
286 | |
287 | C=Z0Cte+sint*(Z0Sin+sint*Z0SinSin)+cost*(Z0Cos+cost*Z0CosCos) |
288 | +Z0CosSin*costsint; |
289 | |
290 | |
291 | Standard_Real Discriminant = B*B-4.0*A*C; |
292 | |
293 | if(Abs(A)<=0.000000001) { |
294 | //-- cout<<" IntAna_Curve:: Internal UV Value : A="<<A<<" -> Abs(A)="<<Abs(A)<<endl; |
295 | if(Abs(B)<=0.0000000001) { |
296 | //-- cout<<" Probleme : Pas de solutions "<<endl; |
297 | Param2=0.0; |
298 | } |
299 | else { |
300 | //modified by NIZNHY-PKV Fri Dec 2 16:02:46 2005f |
301 | Param2 = -C/B; |
302 | /* |
303 | if(!SecondSolution) { |
304 | //-- Cas Param2 = (-B+Sqrt(Discriminant))/(A+A); |
305 | //-- = (-B+Sqrt(B**2 - Eps)) / 2A |
306 | //-- = -C / B |
307 | Param2 = -C/B; |
308 | } |
309 | else { |
310 | //-- Cas Param2 = (-B-Sqrt(Discriminant))/(A+A); |
311 | //-- = (-B-Sqrt(B**2 - Eps)) / 2A |
312 | if(A) { |
313 | Param2 = -B/A; |
314 | } |
315 | else { |
316 | Param2 = -B*10000000.0; |
317 | } |
318 | } |
319 | */ |
320 | //modified by NIZNHY-PKV Fri Dec 2 16:02:54 2005t |
321 | } |
322 | } |
323 | else { |
324 | if(Discriminant<=0.0000000001 || |
325 | Abs(Discriminant/(4*A))<=0.0000000001) Discriminant=0.0; |
326 | SigneSqrtDis = (SecondSolution)? Sqrt(Discriminant) |
327 | : -Sqrt(Discriminant); |
328 | Param2=(-B+SigneSqrtDis)/(A+A); |
329 | } |
330 | } |
331 | |
332 | //======================================================================= |
333 | //function : Value |
334 | //purpose : |
335 | //======================================================================= |
336 | gp_Pnt IntAna_Curve::Value(const Standard_Real theta) |
337 | { |
338 | Standard_Real A, B, C, U, V, sint, cost, SigneSqrtDis; |
339 | // |
340 | A=0.0; B=0.0; C=0.0; |
341 | U=0.0; V=0.0; |
342 | sint=0.0; cost=0.0; |
343 | SigneSqrtDis=0.0; |
344 | InternalUVValue(theta,U,V,A,B,C,cost,sint,SigneSqrtDis); |
345 | //-- checked the parameter U and Raises Domain Error if Error |
346 | return(InternalValue(U,V)); |
347 | } |
348 | //======================================================================= |
349 | //function : D1u |
350 | //purpose : |
351 | //======================================================================= |
352 | Standard_Boolean IntAna_Curve::D1u(const Standard_Real theta, |
353 | gp_Pnt& Pt, |
354 | gp_Vec& Vec) |
355 | { |
356 | //-- Pour detecter le cas ou le calcul est impossible |
357 | Standard_Real A, B, C, U, V, sint, cost, SigneSqrtDis; |
358 | A=0.0; B=0.0; C=0.0; |
359 | U=0.0; V=0.0; |
360 | sint=0.0; cost=0.0; |
361 | // |
362 | InternalUVValue(theta,U,V,A,B,C,cost,sint,SigneSqrtDis); |
363 | // |
364 | Pt = Value(theta); |
365 | if(Abs(SigneSqrtDis)<0.0000000001 || Abs(A)<0.0000001) return(Standard_False); |
366 | |
367 | |
368 | //-- Approximation de la derivee (mieux que le calcul mathematique!) |
369 | Standard_Real dtheta = (DomainSup-DomainInf)*0.000001; |
370 | Standard_Real theta2 = theta+dtheta; |
371 | if((theta2<DomainInf) || ((theta2>DomainSup) && (!TwoCurves)) |
372 | || (theta2>(DomainSup+DomainSup-DomainInf+0.00000000000001))) { |
373 | dtheta = -dtheta; |
374 | theta2 = theta+dtheta; |
375 | } |
376 | gp_Pnt P2 = Value(theta2); |
377 | dtheta = 1.0/dtheta; |
378 | Vec.SetCoord((P2.X()-Pt.X())*dtheta, |
379 | (P2.Y()-Pt.Y())*dtheta, |
380 | (P2.Z()-Pt.Z())*dtheta); |
381 | |
382 | return(Standard_True); |
383 | } |
384 | //======================================================================= |
385 | //function : FindParameter |
386 | //purpose : Para est en sortie le parametre sur la courbe |
387 | //======================================================================= |
388 | Standard_Boolean IntAna_Curve::FindParameter (const gp_Pnt& P, |
389 | Standard_Real& Para) const |
390 | { |
391 | Standard_Real theta,z, aTolPrecision=0.0001; |
c6541a0c |
392 | Standard_Real PIpPI = M_PI + M_PI; |
7fd59977 |
393 | // |
394 | switch (typequadric) { |
395 | |
396 | case GeomAbs_Cylinder: |
397 | { |
398 | ElSLib::CylinderParameters(Ax3,RCyl,P,theta,z); |
399 | } |
400 | break; |
401 | |
402 | case GeomAbs_Cone : |
403 | { |
404 | ElSLib::ConeParameters(Ax3,RCyl,Angle,P,theta,z); |
405 | } |
406 | break; |
407 | |
408 | default: |
409 | break; |
410 | } |
411 | // |
412 | Standard_Real epsAng = 1.e-8; |
413 | Standard_Real tmin = DomainInf; |
414 | Standard_Real tmax = DomainSup; |
415 | Standard_Real U,V,A,B,C,sint,cost,SigneSqrtDis; |
416 | Standard_Real z1,z2; |
417 | |
418 | A=0.0; B=0.0; C=0.0; |
419 | U=0.0; V=0.0; |
420 | sint=0.0; cost=0.0; |
421 | SigneSqrtDis=0.0; |
422 | //U=V=A=B=C=sint=cost=SigneSqrtDis=0.0; |
423 | // |
424 | if (!firstbounded && tmin > theta && (tmin-theta) <= epsAng) { |
425 | theta = tmin; |
426 | } |
427 | else if (!lastbounded && theta > tmax && (theta-tmax) <= epsAng) { |
428 | theta = tmax; |
429 | } |
430 | // |
431 | if (theta < tmin ) { |
432 | theta = theta + PIpPI; |
433 | } |
434 | else if (theta > tmax) { |
435 | theta = theta - PIpPI; |
436 | } |
437 | if (theta < tmin || theta > tmax) { |
438 | if(theta>tmax) { |
439 | InternalUVValue(tmax,U,V,A,B,C,cost,sint,SigneSqrtDis); |
440 | gp_Pnt PMax(InternalValue(U,V)); |
441 | if(PMax.Distance(P) < aTolPrecision) { |
442 | Para = tmax; |
443 | return(Standard_True); |
444 | } |
445 | } |
446 | if(theta<tmin) { |
447 | InternalUVValue(tmin,U,V,A,B,C,cost,sint,SigneSqrtDis); |
448 | gp_Pnt PMin(InternalValue(U,V)); |
449 | if(PMin.Distance(P) < aTolPrecision) { |
450 | Para = tmin; |
451 | return(Standard_True); |
452 | } |
453 | } |
454 | //-- lbr le 14 Fev 96 : On teste malgre tout si le point n est pas le |
455 | //-- point de debut ou de fin |
456 | //-- cout<<"False 1 "<<endl; |
457 | // theta = tmin; le 25 Nov 96 |
458 | } |
459 | |
460 | if (TwoCurves) { |
461 | if(theta > tmax) |
462 | theta = tmax; |
463 | if(theta < tmin) |
464 | theta = tmin; |
465 | InternalUVValue(theta,U,z1,A,B,C,cost,sint,SigneSqrtDis); |
466 | A = B = C = sint = cost = SigneSqrtDis = 0.0; |
467 | InternalUVValue(tmax+tmax - theta,U,z2,A,B,C,cost,sint,SigneSqrtDis); |
468 | |
469 | if (Abs(z-z1) <= Abs(z-z2)) { |
470 | Para = theta; |
471 | } |
472 | else { |
473 | Para = tmax+tmax - theta; |
474 | } |
475 | } |
476 | else { |
477 | Para = theta; |
478 | } |
479 | |
480 | if((Para<DomainInf) || ((Para>DomainSup) && (!TwoCurves)) |
481 | || (Para>(DomainSup+DomainSup-DomainInf+0.00000000000001))) { |
482 | return(Standard_False); |
483 | } |
484 | |
485 | InternalUVValue(Para,U,V,A,B,C,cost,sint,SigneSqrtDis); |
486 | gp_Pnt PPara = InternalValue(U,V); |
487 | Standard_Real Dist = PPara.Distance(P); |
488 | if(Dist > aTolPrecision) { |
489 | //-- Il y a eu un probleme |
490 | //-- On teste si le point est un point double |
491 | InternalUVValue(tmin,U,V,A,B,C,cost,sint,SigneSqrtDis); |
492 | PPara = InternalValue(U,V); |
493 | Dist = PPara.Distance(P); |
494 | if(Dist <= aTolPrecision) { |
495 | Para = tmin; |
496 | return(Standard_True); |
497 | } |
498 | |
499 | InternalUVValue(tmax,U,V,A,B,C,cost,sint,SigneSqrtDis); |
500 | PPara = InternalValue(U,V); |
501 | Dist = PPara.Distance(P); |
502 | if(Dist <= aTolPrecision) { |
503 | Para = tmax; |
504 | return(Standard_True); |
505 | } |
506 | if (TwoCurves) { |
507 | Standard_Real Theta = DomainSup+DomainSup-DomainInf; |
508 | InternalUVValue(Theta,U,V,A,B,C,cost,sint,SigneSqrtDis); |
509 | PPara = InternalValue(U,V); |
510 | Dist = PPara.Distance(P); |
511 | if(Dist <= aTolPrecision) { |
512 | Para = Theta; |
513 | return(Standard_True); |
514 | } |
515 | } |
516 | return(Standard_False); |
517 | } |
518 | return(Standard_True); |
519 | } |
520 | //======================================================================= |
521 | //function : InternalValue |
522 | //purpose : |
523 | //======================================================================= |
524 | gp_Pnt IntAna_Curve::InternalValue(const Standard_Real U, |
525 | const Standard_Real _V) const |
526 | { |
527 | //-- cout<<" ["<<U<<","<<V<<"]"; |
528 | Standard_Real V = _V; |
529 | if(V > 100000.0 ) { V= 100000.0; } |
530 | if(V < -100000.0 ) { V=-100000.0; } |
531 | |
532 | switch(typequadric) { |
533 | case GeomAbs_Cone: |
534 | { |
535 | //------------------------------------------------ |
536 | //-- Parametrage : X = V * Cos(U) --- |
537 | //-- Y = V * Sin(U) --- |
538 | //-- Z = (V-RCyl) / Tan(SemiAngle)-- |
539 | //------------------------------------------------ |
540 | //-- Angle Vaut Cone.SemiAngle() |
541 | return(ElSLib::ConeValue(U,(V-RCyl)/Sin(Angle),Ax3,RCyl,Angle)); |
542 | } |
543 | break; |
544 | |
545 | case GeomAbs_Cylinder: |
546 | return(ElSLib::CylinderValue(U,V,Ax3,RCyl)); |
547 | case GeomAbs_Sphere: |
548 | return(ElSLib::SphereValue(U,V,Ax3,RCyl)); |
549 | default: |
550 | return(gp_Pnt(0.0,0.0,0.0)); |
551 | } |
552 | return(gp_Pnt(0.0,0.0,0.0)); |
553 | } |
554 | |
555 | // |
556 | //======================================================================= |
557 | //function : SetDomain |
558 | //purpose : |
559 | //======================================================================= |
560 | void IntAna_Curve::SetDomain(const Standard_Real DInf, |
561 | const Standard_Real DSup) |
562 | { |
563 | if(DInf>=DSup) { |
564 | Standard_DomainError::Raise("IntAna_Curve::Domain"); |
565 | } |
566 | // |
567 | DomainInf=DInf; |
568 | DomainSup=DSup; |
569 | } |