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