b311480e |
1 | // Copyright (c) 1995-1999 Matra Datavision |
973c2be1 |
2 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
3 | // |
973c2be1 |
4 | // This file is part of Open CASCADE Technology software library. |
b311480e |
5 | // |
d5f74e42 |
6 | // This library is free software; you can redistribute it and/or modify it under |
7 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
8 | // by the Free Software Foundation, with special exception defined in the file |
9 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
10 | // distribution for complete text of the license and disclaimer of any warranty. |
b311480e |
11 | // |
973c2be1 |
12 | // Alternatively, this file may be used under the terms of Open CASCADE |
13 | // commercial license or contractual agreement. |
b311480e |
14 | |
7fd59977 |
15 | //-- IntAna_IntLinTorus.cxx |
16 | //-- lbr : la methode avec les coefficients est catastrophique. |
17 | //-- Mise en place d'une vraie solution. |
18 | |
42cf5bc1 |
19 | #include <ElCLib.hxx> |
20 | #include <ElSLib.hxx> |
7fd59977 |
21 | #include <gp_Dir.hxx> |
42cf5bc1 |
22 | #include <gp_Lin.hxx> |
7fd59977 |
23 | #include <gp_Pnt.hxx> |
42cf5bc1 |
24 | #include <gp_Torus.hxx> |
7fd59977 |
25 | #include <gp_Trsf.hxx> |
42cf5bc1 |
26 | #include <IntAna_IntLinTorus.hxx> |
27 | #include <math_DirectPolynomialRoots.hxx> |
28 | #include <Standard_OutOfRange.hxx> |
29 | #include <StdFail_NotDone.hxx> |
30 | #include <TColStd_Array1OfReal.hxx> |
7fd59977 |
31 | |
d533dafb |
32 | IntAna_IntLinTorus::IntAna_IntLinTorus () |
33 | : done(Standard_False), |
34 | nbpt(0) |
35 | { |
36 | memset (theFi, 0, sizeof (theFi)); |
37 | memset (theParam, 0, sizeof (theParam)); |
38 | memset (theTheta, 0, sizeof (theTheta)); |
39 | } |
7fd59977 |
40 | |
41 | IntAna_IntLinTorus::IntAna_IntLinTorus (const gp_Lin& L, const gp_Torus& T) { |
42 | Perform(L,T); |
43 | } |
44 | |
45 | |
46 | void IntAna_IntLinTorus::Perform (const gp_Lin& L, const gp_Torus& T) { |
47 | gp_Pnt PL=L.Location(); |
48 | gp_Dir DL=L.Direction(); |
49 | |
50 | // Reparametrize the line: |
51 | // set its location as nearest to the location of torus |
52 | gp_Pnt TorLoc = T.Location(); |
53 | Standard_Real ParamOfNewPL = gp_Vec(PL, TorLoc).Dot(gp_Vec(DL)); |
54 | gp_Pnt NewPL( PL.XYZ() + ParamOfNewPL * DL.XYZ() ); |
55 | |
56 | //-------------------------------------------------------------- |
57 | //-- Coefficients de la ligne dans le repere du cone |
58 | //-- |
59 | gp_Trsf trsf; |
60 | trsf.SetTransformation(T.Position()); |
61 | NewPL.Transform(trsf); |
62 | DL.Transform(trsf); |
63 | |
64 | Standard_Real a,b,c,x1,y1,z1,x0,y0,z0; |
65 | Standard_Real a0,a1,a2,a3,a4; |
66 | Standard_Real R,r,R2,r2; |
67 | |
68 | x1 = DL.X(); y1 = DL.Y(); z1 = DL.Z(); |
69 | x0 = NewPL.X(); y0 = NewPL.Y(); z0 = NewPL.Z(); |
70 | R = T.MajorRadius(); R2 = R*R; |
71 | r = T.MinorRadius(); r2 = r*r; |
72 | |
73 | a = x1*x1+y1*y1+z1*z1; |
74 | b = 2.0*(x1*x0+y1*y0+z1*z0); |
75 | c = x0*x0+y0*y0+z0*z0 - (R2+r2); |
76 | |
77 | a4 = a*a; |
78 | a3 = 2.0*a*b; |
79 | a2 = 2.0*a*c+4.0*R2*z1*z1+b*b; |
80 | a1 = 2.0*b*c+8.0*R2*z1*z0; |
81 | a0 = c*c+4.0*R2*(z0*z0-r2); |
82 | |
83 | Standard_Real u,v; |
84 | math_DirectPolynomialRoots mdpr(a4,a3,a2,a1,a0); |
85 | if(mdpr.IsDone()) { |
86 | Standard_Integer nbsolvalid = 0; |
87 | Standard_Integer n = mdpr.NbSolutions(); |
751d0553 |
88 | Standard_Integer aNbBadSol = 0; |
7fd59977 |
89 | for(Standard_Integer i = 1; i<=n ; i++) { |
90 | Standard_Real t = mdpr.Value(i); |
91 | t += ParamOfNewPL; |
92 | gp_Pnt PSolL(ElCLib::Value(t,L)); |
93 | ElSLib::Parameters(T,PSolL,u,v); |
94 | gp_Pnt PSolT(ElSLib::Value(u,v,T)); |
95 | a0 = PSolT.SquareDistance(PSolL); |
96 | |
97 | if(a0>0.0000000001) { |
751d0553 |
98 | aNbBadSol++; |
7fd59977 |
99 | #if 0 |
04232180 |
100 | std::cout<<" ------- Erreur : P Ligne < > P Tore "<<std::endl; |
101 | std::cout<<"Ligne : X:"<<PSolL.X()<<" Y:"<<PSolL.Y()<<" Z:"<<PSolL.Z()<<" l:"<<t<<std::endl; |
102 | std::cout<<"Tore : X:"<<PSolT.X()<<" Y:"<<PSolT.Y()<<" Z:"<<PSolT.Z()<<" u:"<<u<<" v:"<<v<<std::endl; |
7fd59977 |
103 | #endif |
104 | } |
105 | else { |
106 | theParam[nbsolvalid] = t; |
107 | theFi[nbsolvalid] = u; |
108 | theTheta[nbsolvalid] = v; |
109 | thePoint[nbsolvalid] = PSolL; |
110 | nbsolvalid++; |
111 | } |
112 | } |
751d0553 |
113 | if (n > 0 && nbsolvalid == 0 && aNbBadSol == n) |
114 | { |
115 | nbpt = 0; |
116 | done = Standard_False; |
117 | } |
118 | else |
119 | { |
120 | nbpt = nbsolvalid; |
121 | done = Standard_True; |
122 | } |
7fd59977 |
123 | } |
124 | else { |
125 | nbpt = 0; |
126 | done = Standard_False; |
127 | } |
128 | } |
129 | |
130 | |
131 | #if 0 |
132 | |
133 | static void MULT_A3_B1(Standard_Real& c4, |
134 | Standard_Real& c3, |
135 | Standard_Real& c2, |
136 | Standard_Real& c1, |
137 | Standard_Real& c0, |
138 | const Standard_Real a3, |
139 | const Standard_Real a2, |
140 | const Standard_Real a1, |
141 | const Standard_Real a0, |
142 | const Standard_Real b1, |
143 | const Standard_Real b0) { |
144 | c4 = a3 * b1; |
145 | c3 = a3 * b0 + a2 * b1; |
146 | c2 = a2 * b0 + a1 * b1; |
147 | c1 = a1 * b0 + a0 * b1; |
148 | c0 = a0 * b0; |
149 | } |
150 | |
151 | static void MULT_A2_B2(Standard_Real& c4, |
152 | Standard_Real& c3, |
153 | Standard_Real& c2, |
154 | Standard_Real& c1, |
155 | Standard_Real& c0, |
156 | const Standard_Real a2, |
157 | const Standard_Real a1, |
158 | const Standard_Real a0, |
159 | const Standard_Real b2, |
160 | const Standard_Real b1, |
161 | const Standard_Real b0) { |
162 | c4 = a2 * b2; |
163 | c3 = a2 * b1 + a1 * b2; |
164 | c2 = a2 * b0 + a1 * b1 + a0 * b2; |
165 | c1 = a1 * b0 + a0 * b1; |
166 | c0 = a0 * b0; |
167 | } |
168 | |
169 | static void MULT_A2_B1(Standard_Real& c3, |
170 | Standard_Real& c2, |
171 | Standard_Real& c1, |
172 | Standard_Real& c0, |
173 | const Standard_Real a2, |
174 | const Standard_Real a1, |
175 | const Standard_Real a0, |
176 | const Standard_Real b1, |
177 | const Standard_Real b0) { |
178 | c3 = a2 * b1; |
179 | c2 = a2 * b0 + a1 * b1; |
180 | c1 = a1 * b0 + a0 * b1; |
181 | c0 = a0 * b0; |
182 | } |
183 | |
184 | void IntAna_IntLinTorus::Perform (const gp_Lin& L, const gp_Torus& T) { |
185 | TColStd_Array1OfReal C(1,31); |
186 | T.Coefficients(C); |
187 | const gp_Pnt& PL=L.Location(); |
188 | const gp_Dir& DL=L.Direction(); |
189 | |
190 | //---------------------------------------------------------------- |
191 | //-- X = ax1 l + ax0 |
192 | //-- X2 = ax2 l2 + 2 ax1 ax0 l + bx2 |
193 | //-- X3 = ax3 l3 + 3 ax2 ax0 l2 + 3 ax1 bx2 l + bx3 |
194 | //-- X4 = ax4 l4 + 4 ax3 ax0 l3 + 6 ax2 bx2 l2 + 4 ax1 bx3 l + bx4 |
195 | |
196 | Standard_Real ax1,ax2,ax3,ax4,ax0,bx2,bx3,bx4; |
197 | Standard_Real ay1,ay2,ay3,ay4,ay0,by2,by3,by4; |
198 | Standard_Real az1,az2,az3,az4,az0,bz2,bz3,bz4; |
199 | Standard_Real c0,c1,c2,c3,c4; |
200 | ax1=DL.X(); ax0=PL.X(); ay1=DL.Y(); ay0=PL.Y(); az1=DL.Z(); az0=PL.Z(); |
201 | ax2=ax1*ax1; ax3=ax2*ax1; ax4=ax3*ax1; bx2=ax0*ax0; bx3=bx2*ax0; bx4=bx3*ax0; |
202 | ay2=ay1*ay1; ay3=ay2*ay1; ay4=ay3*ay1; by2=ay0*ay0; by3=by2*ay0; by4=by3*ay0; |
203 | az2=az1*az1; az3=az2*az1; az4=az3*az1; bz2=az0*az0; bz3=bz2*az0; bz4=bz3*az0; |
204 | |
205 | //--------------------------------------------------------------------------- Terme X**4 |
206 | Standard_Real c=C(1); |
207 | Standard_Real a4 = c *ax4; |
208 | Standard_Real a3 = c *4.0*ax3*ax0; |
209 | Standard_Real a2 = c *6.0*ax2*bx2; |
210 | Standard_Real a1 = c *4.0*ax1*bx3; |
211 | Standard_Real a0 = c *bx4; |
212 | //--------------------------------------------------------------------------- Terme Y**4 |
213 | c = C(2); |
214 | a4+= c*ay4; |
215 | a3+= c*4.0*ay3*ay0; |
216 | a2+= c*6.0*ay2*by2; |
217 | a1+= c*4.0*ay1*by3; |
218 | a0+= c*by4; |
219 | //--------------------------------------------------------------------------- Terme Z**4 |
220 | c = C(3); |
221 | a4+= c*az4 ; |
222 | a3+= c*4.0*az3*az0; |
223 | a2+= c*6.0*az2*bz2; |
224 | a1+= c*4.0*az1*bz3; |
225 | a0+= c*bz4; |
226 | //--------------------------------------------------------------------------- Terme X**3 Y |
227 | c = C(4); |
228 | MULT_A3_B1(c4,c3,c2,c1,c0, ax3, 3.0*ax2*ax0, 3.0*ax1*bx2, bx3, ay1,ay0); |
229 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
230 | //--------------------------------------------------------------------------- Terme X**3 Z |
231 | c = C(5); |
232 | MULT_A3_B1(c4,c3,c2,c1,c0, ax3, 3.0*ax2*ax0, 3.0*ax1*bx2, bx3, az1,az0); |
233 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
234 | //--------------------------------------------------------------------------- Terme Y**3 X |
235 | c = C(6); |
236 | MULT_A3_B1(c4,c3,c2,c1,c0, ay3, 3.0*ay2*ay0, 3.0*ay1*by2, by3, ax1,ax0); |
237 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
238 | //--------------------------------------------------------------------------- Terme Y**3 Z |
239 | c = C(7); |
240 | MULT_A3_B1(c4,c3,c2,c1,c0, ay3, 3.0*ay2*ay0, 3.0*ay1*by2, by3, az1,az0); |
241 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
242 | //--------------------------------------------------------------------------- Terme Z**3 X |
243 | c = C(8); |
244 | MULT_A3_B1(c4,c3,c2,c1,c0, az3, 3.0*az2*az0, 3.0*az1*bz2, bz3, ax1,ax0); |
245 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
246 | //--------------------------------------------------------------------------- Terme Z**3 Y |
247 | c = C(9); |
248 | MULT_A3_B1(c4,c3,c2,c1,c0, az3, 3.0*az2*az0, 3.0*az1*bz2, bz3, ay1,ay0); |
249 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
250 | |
251 | |
252 | //--------------------------------------------------------------------------- Terme X**2 Y**2 |
253 | c = C(10); |
254 | MULT_A2_B2(c4,c3,c2,c1,c0, ax2, 2.0*ax1*ax0, bx2, ay2,2.0*ay1*ay0, by2); |
255 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
256 | //--------------------------------------------------------------------------- Terme X**2 Z**2 |
257 | c = C(11); |
258 | MULT_A2_B2(c4,c3,c2,c1,c0, ax2, 2.0*ax1*ax0, bx2, az2,2.0*az1*az0, bz2); |
259 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
260 | //--------------------------------------------------------------------------- Terme Y**2 Z**2 |
261 | c = C(12); |
262 | MULT_A2_B2(c4,c3,c2,c1,c0, ay2, 2.0*ay1*ay0, by2, az2,2.0*az1*az0, bz2); |
263 | a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0; |
264 | |
265 | |
266 | //--------------------------------------------------------------------------- Terme X**3 |
267 | c = C(13); |
268 | a3+= c*( ax3 ); |
269 | a2+= c*( 3.0*ax2*ax0 ); |
270 | a1+= c*( 3.0*ax1*bx2 ); |
271 | a0+= c*( bx3 ); |
272 | //--------------------------------------------------------------------------- Terme Y**3 |
273 | c = C(14); |
274 | a3+= c*( ay3 ); |
275 | a2+= c*( 3.0*ay2*ay0 ); |
276 | a1+= c*( 3.0*ay1*by2 ); |
277 | a0+= c*( by3 ); |
278 | //--------------------------------------------------------------------------- Terme Y**3 |
279 | c = C(15); |
280 | a3+= c*( az3 ); |
281 | a2+= c*( 3.0*az2*az0 ); |
282 | a1+= c*( 3.0*az1*bz2 ); |
283 | a0+= c*( bz3 ); |
284 | |
285 | |
286 | //--------------------------------------------------------------------------- Terme X**2 Y |
287 | c = C(16); |
288 | MULT_A2_B1(c3,c2,c1,c0, ax2, 2.0*ax1*ax0, bx2, ay1,ay0); |
289 | a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0; |
290 | //--------------------------------------------------------------------------- Terme X**2 Z |
291 | c = C(17); |
292 | MULT_A2_B1(c3,c2,c1,c0, ax2, 2.0*ax1*ax0, bx2, az1,az0); |
293 | a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0; |
294 | //--------------------------------------------------------------------------- Terme Y**2 X |
295 | c = C(18); |
296 | MULT_A2_B1(c3,c2,c1,c0, ay2, 2.0*ay1*ay0, by2, ax1,ax0); |
297 | a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0; |
298 | //--------------------------------------------------------------------------- Terme Y**2 Z |
299 | c = C(19); |
300 | MULT_A2_B1(c3,c2,c1,c0, ay2, 2.0*ay1*ay0, by2, az1,az0); |
301 | a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0; |
302 | //--------------------------------------------------------------------------- Terme Z**2 X |
303 | c = C(20); |
304 | MULT_A2_B1(c3,c2,c1,c0, az2, 2.0*az1*az0, bz2, ax1,ax0); |
305 | a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0; |
306 | //--------------------------------------------------------------------------- Terme Z**2 Y |
307 | c = C(21); |
308 | MULT_A2_B1(c3,c2,c1,c0, az2, 2.0*az1*az0, bz2, ay1,ay0); |
309 | a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0; |
310 | |
311 | |
312 | //--------------------------------------------------------------------------- Terme X**2 |
313 | c = C(22); |
314 | a2+= c*ax2; |
315 | a1+= c*2.0*ax1*ax0; |
316 | a0+= c*bx2; |
317 | //--------------------------------------------------------------------------- Terme Y**2 |
318 | c = C(23); |
319 | a2+= c*ay2; |
320 | a1+= c*2.0*ay1*ay0; |
321 | a0+= c*by2; |
322 | //--------------------------------------------------------------------------- Terme Z**2 |
323 | c = C(24); |
324 | a2+= c*az2; |
325 | a1+= c*2.0*az1*az0; |
326 | a0+= c*bz2; |
327 | |
328 | |
329 | //--------------------------------------------------------------------------- Terme X Y |
330 | c = C(25); |
331 | a2+= c*(ax1*ay1); |
332 | a1+= c*(ax1*ay0 + ax0*ay1); |
333 | a0+= c*(ax0*ay0); |
334 | //--------------------------------------------------------------------------- Terme X Z |
335 | c = C(26); |
336 | a2+= c*(ax1*az1); |
337 | a1+= c*(ax1*az0 + ax0*az1); |
338 | a0+= c*(ax0*az0); |
339 | //--------------------------------------------------------------------------- Terme Y Z |
340 | c = C(27); |
341 | a2+= c*(ay1*az1); |
342 | a1+= c*(ay1*az0 + ay0*az1); |
343 | a0+= c*(ay0*az0); |
344 | |
345 | //--------------------------------------------------------------------------- Terme X |
346 | c = C(28); |
347 | a1+= c*ax1; |
348 | a0+= c*ax0; |
349 | //--------------------------------------------------------------------------- Terme Y |
350 | c = C(29); |
351 | a1+= c*ay1; |
352 | a0+= c*ay0; |
353 | //--------------------------------------------------------------------------- Terme Z |
354 | c = C(30); |
355 | a1+= c*az1; |
356 | a0+= c*az0; |
357 | |
358 | //--------------------------------------------------------------------------- Terme Constant |
359 | c = C(31); |
360 | a0+=c; |
361 | |
362 | |
363 | |
04232180 |
364 | std::cout<<"\n ---------- Coefficients Line - Torus : "<<std::endl; |
365 | std::cout<<" a0 : "<<a0<<std::endl; |
366 | std::cout<<" a1 : "<<a1<<std::endl; |
367 | std::cout<<" a2 : "<<a2<<std::endl; |
368 | std::cout<<" a3 : "<<a3<<std::endl; |
369 | std::cout<<" a4 : "<<a4<<std::endl; |
7fd59977 |
370 | |
371 | Standard_Real u,v; |
372 | math_DirectPolynomialRoots mdpr(a4,a3,a2,a1,a0); |
373 | if(mdpr.IsDone()) { |
374 | Standard_Integer nbsolvalid = 0; |
375 | Standard_Integer n = mdpr.NbSolutions(); |
376 | for(Standard_Integer i = 1; i<=n ; i++) { |
377 | Standard_Real t = mdpr.Value(i); |
378 | gp_Pnt PSolL(ax0+ax1*t, ay0+ay1*t, az0+az1*t); |
379 | ElSLib::Parameters(T,PSolL,u,v); |
380 | gp_Pnt PSolT(ElSLib::Value(u,v,T)); |
381 | |
382 | a0 = PSolT.SquareDistance(PSolL); |
383 | if(a0>0.0000000001) { |
04232180 |
384 | std::cout<<" ------- Erreur : P Ligne < > P Tore "; |
385 | std::cout<<"Ligne : X:"<<PSolL.X()<<" Y:"<<PSolL.Y()<<" Z:"<<PSolL.Z()<<" l:"<<t<<std::endl; |
386 | std::cout<<"Tore : X:"<<PSolL.X()<<" Y:"<<PSolL.Y()<<" Z:"<<PSolL.Z()<<" u:"<<u<<" v:"<<v<<std::endl; |
7fd59977 |
387 | } |
388 | else { |
389 | theParam[nbsolvalid] = t; |
390 | theFi[nbsolvalid] = v; |
391 | theTheta[nbsolvalid] = u; |
392 | thePoint[nbsolvalid] = PSolL; |
393 | nbsolvalid++; |
394 | } |
395 | } |
396 | nbpt = nbsolvalid; |
397 | done = Standard_True; |
398 | } |
399 | else { |
400 | nbpt = 0; |
401 | done = Standard_False; |
402 | } |
403 | } |
404 | #endif |
405 | |
406 | |