1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
4 // This file is part of Open CASCADE Technology software library.
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
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.
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
15 //-- IntAna_IntLinTorus.cxx
16 //-- lbr : la methode avec les coefficients est catastrophique.
17 //-- Mise en place d'une vraie solution.
24 #include <gp_Torus.hxx>
25 #include <gp_Trsf.hxx>
26 #include <IntAna_IntLinTorus.hxx>
27 #include <math_DirectPolynomialRoots.hxx>
29 IntAna_IntLinTorus::IntAna_IntLinTorus ()
30 : done(Standard_False),
33 memset (theFi, 0, sizeof (theFi));
34 memset (theParam, 0, sizeof (theParam));
35 memset (theTheta, 0, sizeof (theTheta));
38 IntAna_IntLinTorus::IntAna_IntLinTorus (const gp_Lin& L, const gp_Torus& T) {
43 void IntAna_IntLinTorus::Perform (const gp_Lin& L, const gp_Torus& T) {
44 gp_Pnt PL=L.Location();
45 gp_Dir DL=L.Direction();
47 // Reparametrize the line:
48 // set its location as nearest to the location of torus
49 gp_Pnt TorLoc = T.Location();
50 Standard_Real ParamOfNewPL = gp_Vec(PL, TorLoc).Dot(gp_Vec(DL));
51 gp_Pnt NewPL( PL.XYZ() + ParamOfNewPL * DL.XYZ() );
53 //--------------------------------------------------------------
54 //-- Coefficients de la ligne dans le repere du cone
57 trsf.SetTransformation(T.Position());
58 NewPL.Transform(trsf);
61 Standard_Real a,b,c,x1,y1,z1,x0,y0,z0;
62 Standard_Real a0,a1,a2,a3,a4;
63 Standard_Real R,r,R2,r2;
65 x1 = DL.X(); y1 = DL.Y(); z1 = DL.Z();
66 x0 = NewPL.X(); y0 = NewPL.Y(); z0 = NewPL.Z();
67 R = T.MajorRadius(); R2 = R*R;
68 r = T.MinorRadius(); r2 = r*r;
70 a = x1*x1+y1*y1+z1*z1;
71 b = 2.0*(x1*x0+y1*y0+z1*z0);
72 c = x0*x0+y0*y0+z0*z0 - (R2+r2);
76 a2 = 2.0*a*c+4.0*R2*z1*z1+b*b;
77 a1 = 2.0*b*c+8.0*R2*z1*z0;
78 a0 = c*c+4.0*R2*(z0*z0-r2);
81 math_DirectPolynomialRoots mdpr(a4,a3,a2,a1,a0);
83 Standard_Integer nbsolvalid = 0;
84 Standard_Integer n = mdpr.NbSolutions();
85 Standard_Integer aNbBadSol = 0;
86 for(Standard_Integer i = 1; i<=n ; i++) {
87 Standard_Real t = mdpr.Value(i);
89 gp_Pnt PSolL(ElCLib::Value(t,L));
90 ElSLib::Parameters(T,PSolL,u,v);
91 gp_Pnt PSolT(ElSLib::Value(u,v,T));
92 a0 = PSolT.SquareDistance(PSolL);
97 std::cout<<" ------- Erreur : P Ligne < > P Tore "<<std::endl;
98 std::cout<<"Ligne : X:"<<PSolL.X()<<" Y:"<<PSolL.Y()<<" Z:"<<PSolL.Z()<<" l:"<<t<<std::endl;
99 std::cout<<"Tore : X:"<<PSolT.X()<<" Y:"<<PSolT.Y()<<" Z:"<<PSolT.Z()<<" u:"<<u<<" v:"<<v<<std::endl;
103 theParam[nbsolvalid] = t;
104 theFi[nbsolvalid] = u;
105 theTheta[nbsolvalid] = v;
106 thePoint[nbsolvalid] = PSolL;
110 if (n > 0 && nbsolvalid == 0 && aNbBadSol == n)
113 done = Standard_False;
118 done = Standard_True;
123 done = Standard_False;
130 static void MULT_A3_B1(Standard_Real& c4,
135 const Standard_Real a3,
136 const Standard_Real a2,
137 const Standard_Real a1,
138 const Standard_Real a0,
139 const Standard_Real b1,
140 const Standard_Real b0) {
142 c3 = a3 * b0 + a2 * b1;
143 c2 = a2 * b0 + a1 * b1;
144 c1 = a1 * b0 + a0 * b1;
148 static void MULT_A2_B2(Standard_Real& c4,
153 const Standard_Real a2,
154 const Standard_Real a1,
155 const Standard_Real a0,
156 const Standard_Real b2,
157 const Standard_Real b1,
158 const Standard_Real b0) {
160 c3 = a2 * b1 + a1 * b2;
161 c2 = a2 * b0 + a1 * b1 + a0 * b2;
162 c1 = a1 * b0 + a0 * b1;
166 static void MULT_A2_B1(Standard_Real& c3,
170 const Standard_Real a2,
171 const Standard_Real a1,
172 const Standard_Real a0,
173 const Standard_Real b1,
174 const Standard_Real b0) {
176 c2 = a2 * b0 + a1 * b1;
177 c1 = a1 * b0 + a0 * b1;
181 void IntAna_IntLinTorus::Perform (const gp_Lin& L, const gp_Torus& T) {
182 TColStd_Array1OfReal C(1,31);
184 const gp_Pnt& PL=L.Location();
185 const gp_Dir& DL=L.Direction();
187 //----------------------------------------------------------------
189 //-- X2 = ax2 l2 + 2 ax1 ax0 l + bx2
190 //-- X3 = ax3 l3 + 3 ax2 ax0 l2 + 3 ax1 bx2 l + bx3
191 //-- X4 = ax4 l4 + 4 ax3 ax0 l3 + 6 ax2 bx2 l2 + 4 ax1 bx3 l + bx4
193 Standard_Real ax1,ax2,ax3,ax4,ax0,bx2,bx3,bx4;
194 Standard_Real ay1,ay2,ay3,ay4,ay0,by2,by3,by4;
195 Standard_Real az1,az2,az3,az4,az0,bz2,bz3,bz4;
196 Standard_Real c0,c1,c2,c3,c4;
197 ax1=DL.X(); ax0=PL.X(); ay1=DL.Y(); ay0=PL.Y(); az1=DL.Z(); az0=PL.Z();
198 ax2=ax1*ax1; ax3=ax2*ax1; ax4=ax3*ax1; bx2=ax0*ax0; bx3=bx2*ax0; bx4=bx3*ax0;
199 ay2=ay1*ay1; ay3=ay2*ay1; ay4=ay3*ay1; by2=ay0*ay0; by3=by2*ay0; by4=by3*ay0;
200 az2=az1*az1; az3=az2*az1; az4=az3*az1; bz2=az0*az0; bz3=bz2*az0; bz4=bz3*az0;
202 //--------------------------------------------------------------------------- Terme X**4
203 Standard_Real c=C(1);
204 Standard_Real a4 = c *ax4;
205 Standard_Real a3 = c *4.0*ax3*ax0;
206 Standard_Real a2 = c *6.0*ax2*bx2;
207 Standard_Real a1 = c *4.0*ax1*bx3;
208 Standard_Real a0 = c *bx4;
209 //--------------------------------------------------------------------------- Terme Y**4
216 //--------------------------------------------------------------------------- Terme Z**4
223 //--------------------------------------------------------------------------- Terme X**3 Y
225 MULT_A3_B1(c4,c3,c2,c1,c0, ax3, 3.0*ax2*ax0, 3.0*ax1*bx2, bx3, ay1,ay0);
226 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
227 //--------------------------------------------------------------------------- Terme X**3 Z
229 MULT_A3_B1(c4,c3,c2,c1,c0, ax3, 3.0*ax2*ax0, 3.0*ax1*bx2, bx3, az1,az0);
230 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
231 //--------------------------------------------------------------------------- Terme Y**3 X
233 MULT_A3_B1(c4,c3,c2,c1,c0, ay3, 3.0*ay2*ay0, 3.0*ay1*by2, by3, ax1,ax0);
234 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
235 //--------------------------------------------------------------------------- Terme Y**3 Z
237 MULT_A3_B1(c4,c3,c2,c1,c0, ay3, 3.0*ay2*ay0, 3.0*ay1*by2, by3, az1,az0);
238 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
239 //--------------------------------------------------------------------------- Terme Z**3 X
241 MULT_A3_B1(c4,c3,c2,c1,c0, az3, 3.0*az2*az0, 3.0*az1*bz2, bz3, ax1,ax0);
242 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
243 //--------------------------------------------------------------------------- Terme Z**3 Y
245 MULT_A3_B1(c4,c3,c2,c1,c0, az3, 3.0*az2*az0, 3.0*az1*bz2, bz3, ay1,ay0);
246 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
249 //--------------------------------------------------------------------------- Terme X**2 Y**2
251 MULT_A2_B2(c4,c3,c2,c1,c0, ax2, 2.0*ax1*ax0, bx2, ay2,2.0*ay1*ay0, by2);
252 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
253 //--------------------------------------------------------------------------- Terme X**2 Z**2
255 MULT_A2_B2(c4,c3,c2,c1,c0, ax2, 2.0*ax1*ax0, bx2, az2,2.0*az1*az0, bz2);
256 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
257 //--------------------------------------------------------------------------- Terme Y**2 Z**2
259 MULT_A2_B2(c4,c3,c2,c1,c0, ay2, 2.0*ay1*ay0, by2, az2,2.0*az1*az0, bz2);
260 a4+= c*c4; a3+= c*c3; a2+= c*c2; a1+= c*c1; a0+= c*c0;
263 //--------------------------------------------------------------------------- Terme X**3
266 a2+= c*( 3.0*ax2*ax0 );
267 a1+= c*( 3.0*ax1*bx2 );
269 //--------------------------------------------------------------------------- Terme Y**3
272 a2+= c*( 3.0*ay2*ay0 );
273 a1+= c*( 3.0*ay1*by2 );
275 //--------------------------------------------------------------------------- Terme Y**3
278 a2+= c*( 3.0*az2*az0 );
279 a1+= c*( 3.0*az1*bz2 );
283 //--------------------------------------------------------------------------- Terme X**2 Y
285 MULT_A2_B1(c3,c2,c1,c0, ax2, 2.0*ax1*ax0, bx2, ay1,ay0);
286 a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0;
287 //--------------------------------------------------------------------------- Terme X**2 Z
289 MULT_A2_B1(c3,c2,c1,c0, ax2, 2.0*ax1*ax0, bx2, az1,az0);
290 a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0;
291 //--------------------------------------------------------------------------- Terme Y**2 X
293 MULT_A2_B1(c3,c2,c1,c0, ay2, 2.0*ay1*ay0, by2, ax1,ax0);
294 a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0;
295 //--------------------------------------------------------------------------- Terme Y**2 Z
297 MULT_A2_B1(c3,c2,c1,c0, ay2, 2.0*ay1*ay0, by2, az1,az0);
298 a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0;
299 //--------------------------------------------------------------------------- Terme Z**2 X
301 MULT_A2_B1(c3,c2,c1,c0, az2, 2.0*az1*az0, bz2, ax1,ax0);
302 a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0;
303 //--------------------------------------------------------------------------- Terme Z**2 Y
305 MULT_A2_B1(c3,c2,c1,c0, az2, 2.0*az1*az0, bz2, ay1,ay0);
306 a3+= c*c3; a2+= c* c2; a1+= c* c1; a0+= c*c0;
309 //--------------------------------------------------------------------------- Terme X**2
314 //--------------------------------------------------------------------------- Terme Y**2
319 //--------------------------------------------------------------------------- Terme Z**2
326 //--------------------------------------------------------------------------- Terme X Y
329 a1+= c*(ax1*ay0 + ax0*ay1);
331 //--------------------------------------------------------------------------- Terme X Z
334 a1+= c*(ax1*az0 + ax0*az1);
336 //--------------------------------------------------------------------------- Terme Y Z
339 a1+= c*(ay1*az0 + ay0*az1);
342 //--------------------------------------------------------------------------- Terme X
346 //--------------------------------------------------------------------------- Terme Y
350 //--------------------------------------------------------------------------- Terme Z
355 //--------------------------------------------------------------------------- Terme Constant
361 std::cout<<"\n ---------- Coefficients Line - Torus : "<<std::endl;
362 std::cout<<" a0 : "<<a0<<std::endl;
363 std::cout<<" a1 : "<<a1<<std::endl;
364 std::cout<<" a2 : "<<a2<<std::endl;
365 std::cout<<" a3 : "<<a3<<std::endl;
366 std::cout<<" a4 : "<<a4<<std::endl;
369 math_DirectPolynomialRoots mdpr(a4,a3,a2,a1,a0);
371 Standard_Integer nbsolvalid = 0;
372 Standard_Integer n = mdpr.NbSolutions();
373 for(Standard_Integer i = 1; i<=n ; i++) {
374 Standard_Real t = mdpr.Value(i);
375 gp_Pnt PSolL(ax0+ax1*t, ay0+ay1*t, az0+az1*t);
376 ElSLib::Parameters(T,PSolL,u,v);
377 gp_Pnt PSolT(ElSLib::Value(u,v,T));
379 a0 = PSolT.SquareDistance(PSolL);
380 if(a0>0.0000000001) {
381 std::cout<<" ------- Erreur : P Ligne < > P Tore ";
382 std::cout<<"Ligne : X:"<<PSolL.X()<<" Y:"<<PSolL.Y()<<" Z:"<<PSolL.Z()<<" l:"<<t<<std::endl;
383 std::cout<<"Tore : X:"<<PSolL.X()<<" Y:"<<PSolL.Y()<<" Z:"<<PSolL.Z()<<" u:"<<u<<" v:"<<v<<std::endl;
386 theParam[nbsolvalid] = t;
387 theFi[nbsolvalid] = v;
388 theTheta[nbsolvalid] = u;
389 thePoint[nbsolvalid] = PSolL;
394 done = Standard_True;
398 done = Standard_False;