Commit | Line | Data |
---|---|---|
b311480e | 1 | // Created on: 1991-09-09 |
2 | // Created by: Michel Chauvat | |
3 | // Copyright (c) 1991-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 | |
17 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 | |
18 | ||
19 | ||
20 | #ifndef No_Exception | |
21 | #define No_Exception | |
22 | #endif | |
23 | ||
42cf5bc1 | 24 | |
25 | #include <ElSLib.hxx> | |
7fd59977 | 26 | #include <gp.hxx> |
42cf5bc1 | 27 | #include <gp_Ax3.hxx> |
28 | #include <gp_Circ.hxx> | |
29 | #include <gp_Cone.hxx> | |
30 | #include <gp_Cylinder.hxx> | |
31 | #include <gp_Lin.hxx> | |
32 | #include <gp_Pln.hxx> | |
33 | #include <gp_Pnt.hxx> | |
34 | #include <gp_Sphere.hxx> | |
35 | #include <gp_Torus.hxx> | |
7fd59977 | 36 | #include <gp_Trsf.hxx> |
42cf5bc1 | 37 | #include <gp_Vec.hxx> |
38 | #include <gp_XYZ.hxx> | |
7fd59977 | 39 | |
c6541a0c | 40 | static Standard_Real PIPI = M_PI + M_PI; |
7fd59977 | 41 | |
42 | gp_Pnt ElSLib::PlaneValue (const Standard_Real U, | |
43 | const Standard_Real V, | |
44 | const gp_Ax3& Pos) | |
45 | { | |
46 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
47 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
48 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
49 | return gp_Pnt(U * XDir.X() + V * YDir.X() + PLoc.X(), | |
50 | U * XDir.Y() + V * YDir.Y() + PLoc.Y(), | |
51 | U * XDir.Z() + V * YDir.Z() + PLoc.Z()); | |
52 | } | |
53 | ||
54 | gp_Pnt ElSLib::ConeValue (const Standard_Real U, | |
55 | const Standard_Real V, | |
56 | const gp_Ax3& Pos, | |
57 | const Standard_Real Radius, | |
58 | const Standard_Real SAngle) | |
59 | { | |
60 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
61 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
62 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
63 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
64 | Standard_Real R = Radius + V * sin(SAngle); | |
65 | Standard_Real A3 = V * cos(SAngle); | |
66 | Standard_Real A1 = R * cos(U); | |
67 | Standard_Real A2 = R * sin(U); | |
68 | return gp_Pnt(A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X() + PLoc.X(), | |
69 | A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y() + PLoc.Y(), | |
70 | A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z() + PLoc.Z()); | |
71 | } | |
72 | ||
73 | gp_Pnt ElSLib::CylinderValue (const Standard_Real U, | |
74 | const Standard_Real V, | |
75 | const gp_Ax3& Pos, | |
76 | const Standard_Real Radius) | |
77 | { | |
78 | // M(u,v) = C + Radius * ( Xdir * Cos(u) + Ydir * Sin(u)) + V * Zdir | |
79 | // where C is the location point of the Axis2placement | |
80 | // Xdir, Ydir ,Zdir are the directions of the local coordinates system | |
81 | ||
82 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
83 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
84 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
85 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
86 | Standard_Real A1 = Radius * cos(U); | |
87 | Standard_Real A2 = Radius * sin(U); | |
88 | return gp_Pnt(A1 * XDir.X() + A2 * YDir.X() + V * ZDir.X() + PLoc.X(), | |
89 | A1 * XDir.Y() + A2 * YDir.Y() + V * ZDir.Y() + PLoc.Y(), | |
90 | A1 * XDir.Z() + A2 * YDir.Z() + V * ZDir.Z() + PLoc.Z()); | |
91 | } | |
92 | ||
93 | gp_Pnt ElSLib::SphereValue (const Standard_Real U, | |
94 | const Standard_Real V, | |
95 | const gp_Ax3& Pos, | |
96 | const Standard_Real Radius) | |
97 | { | |
98 | //M(U,V) = Location + | |
99 | // R * CosV (CosU * XDirection + SinU * YDirection) + | |
100 | // R * SinV * Direction | |
101 | ||
102 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
103 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
104 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
105 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
106 | Standard_Real R = Radius * cos(V); | |
107 | Standard_Real A3 = Radius * sin(V); | |
108 | Standard_Real A1 = R * cos(U); | |
109 | Standard_Real A2 = R * sin(U); | |
110 | return gp_Pnt(A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X() + PLoc.X(), | |
111 | A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y() + PLoc.Y(), | |
112 | A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z() + PLoc.Z()); | |
113 | } | |
114 | ||
115 | gp_Pnt ElSLib::TorusValue (const Standard_Real U, | |
116 | const Standard_Real V, | |
117 | const gp_Ax3& Pos, | |
118 | const Standard_Real MajorRadius, | |
119 | const Standard_Real MinorRadius) | |
120 | { | |
121 | //M(U,V) = | |
122 | // Location + | |
123 | // (MajRadius+MinRadius*Cos(V)) * (Cos(U)*XDirection + Sin(U)*YDirection) + | |
124 | // MinorRadius * Sin(V) * Direction | |
125 | ||
126 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
127 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
128 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
129 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
130 | Standard_Real R = MajorRadius + MinorRadius * cos(V); | |
131 | Standard_Real A3 = MinorRadius * sin(V); | |
132 | Standard_Real A1 = R * cos(U); | |
133 | Standard_Real A2 = R * sin(U); | |
134 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
135 | Standard_Real eps = 10.*(MinorRadius + MajorRadius)*RealEpsilon(); | |
136 | ||
137 | if (Abs(A1) <= eps) | |
138 | A1 = 0.; | |
139 | ||
140 | if (Abs(A2) <= eps) | |
141 | A2 = 0.; | |
142 | ||
143 | if (Abs(A3) <= eps) | |
144 | A3 = 0.; | |
145 | ||
146 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
147 | return gp_Pnt(A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X() + PLoc.X(), | |
148 | A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y() + PLoc.Y(), | |
149 | A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z() + PLoc.Z()); | |
150 | } | |
151 | ||
152 | gp_Vec ElSLib::PlaneDN (const Standard_Real, | |
153 | const Standard_Real, | |
154 | const gp_Ax3& Pos, | |
155 | const Standard_Integer Nu, | |
156 | const Standard_Integer Nv) | |
157 | { | |
158 | if (Nu == 0 && Nv == 1) { return gp_Vec (Pos.YDirection()); } | |
159 | else if (Nu == 1 && Nv == 0) { return gp_Vec (Pos.XDirection()); } | |
160 | return gp_Vec (0., 0., 0.); | |
161 | } | |
162 | ||
163 | gp_Vec ElSLib::ConeDN (const Standard_Real U, | |
164 | const Standard_Real V, | |
165 | const gp_Ax3& Pos, | |
166 | const Standard_Real Radius, | |
167 | const Standard_Real SAngle, | |
168 | const Standard_Integer Nu, | |
169 | const Standard_Integer Nv) | |
170 | { | |
171 | gp_XYZ Xdir = Pos.XDirection().XYZ(); | |
172 | gp_XYZ Ydir = Pos.YDirection().XYZ(); | |
c6541a0c | 173 | Standard_Real Um = U + Nu * M_PI_2; // M_PI * 0.5 |
7fd59977 | 174 | Xdir.Multiply(cos(Um)); |
175 | Ydir.Multiply(sin(Um)); | |
176 | Xdir.Add(Ydir); | |
177 | if(Nv == 0) { | |
178 | Xdir.Multiply(Radius + V * sin(SAngle)); | |
179 | if(Nu == 0) Xdir.Add(Pos.Location().XYZ()); | |
180 | return gp_Vec(Xdir); | |
181 | } | |
182 | else if(Nv == 1) { | |
183 | Xdir.Multiply(sin(SAngle)); | |
184 | return gp_Vec(Xdir); | |
185 | } | |
186 | return gp_Vec(0.0,0.0,0.0); | |
187 | } | |
188 | ||
189 | gp_Vec ElSLib::CylinderDN (const Standard_Real U, | |
190 | const Standard_Real, | |
191 | const gp_Ax3& Pos, | |
192 | const Standard_Real Radius, | |
193 | const Standard_Integer Nu, | |
194 | const Standard_Integer Nv) | |
195 | { | |
196 | if (Nu + Nv < 1 || Nu < 0 || Nv < 0) { return gp_Vec(); } | |
197 | if (Nv == 0) { | |
198 | Standard_Real RCosU = Radius * cos(U); | |
199 | Standard_Real RSinU = Radius * sin(U); | |
200 | gp_XYZ Xdir = Pos.XDirection().XYZ(); | |
201 | gp_XYZ Ydir = Pos.YDirection().XYZ(); | |
202 | if ((Nu + 6) % 4 == 0) { | |
203 | Xdir.Multiply (-RCosU); | |
204 | Ydir.Multiply (-RSinU); | |
205 | } | |
206 | else if ((Nu + 5) % 4 == 0) { | |
207 | Xdir.Multiply ( RSinU); | |
208 | Ydir.Multiply (-RCosU); | |
209 | } | |
210 | else if ((Nu + 3) % 4 == 0) { | |
211 | Xdir.Multiply (-RSinU); | |
212 | Ydir.Multiply ( RCosU); | |
213 | } | |
214 | else if (Nu % 4 == 0) { | |
215 | Xdir.Multiply ( RCosU); | |
216 | Ydir.Multiply ( RSinU); | |
217 | } | |
218 | Xdir.Add (Ydir); | |
219 | return gp_Vec (Xdir); | |
220 | } | |
221 | else if (Nv == 1 && Nu == 0) { return gp_Vec (Pos.Direction()); } | |
222 | else { return gp_Vec (0.0, 0.0, 0.0); } | |
223 | } | |
224 | ||
225 | gp_Vec ElSLib::SphereDN (const Standard_Real U, | |
226 | const Standard_Real V, | |
227 | const gp_Ax3& Pos, | |
228 | const Standard_Real Radius, | |
229 | const Standard_Integer Nu, | |
230 | const Standard_Integer Nv) | |
231 | { | |
232 | if (Nu + Nv < 1 || Nu < 0 || Nv < 0) { return gp_Vec(); } | |
233 | Standard_Real CosU = cos(U); | |
234 | Standard_Real SinU = sin(U); | |
235 | Standard_Real RCosV = Radius * cos(V); | |
236 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
237 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
238 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
239 | Standard_Real A1,A2,A3,X,Y,Z; | |
240 | if (Nu == 0) { | |
241 | Standard_Real RSinV = Radius * sin(V); | |
242 | if (IsOdd (Nv)) { A1 = - RSinV * CosU; A2 = - RSinV * SinU; A3 = RCosV; } | |
243 | else { A1 = - RCosV * CosU; A2 = - RCosV * SinU; A3 = - RSinV; } | |
244 | X = A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X(); | |
245 | Y = A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y(); | |
246 | Z = A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z(); | |
247 | if (!( (Nv + 2) % 4 == 0 || (Nv + 3) % 4 == 0 )) | |
248 | { X = - X; Y = - Y; Z = - Z; } | |
249 | } | |
250 | else if (Nv == 0) { | |
251 | if (IsOdd (Nu)) { A1 = - RCosV * SinU; A2 = RCosV * CosU; } | |
252 | else { A1 = RCosV * CosU; A2 = RCosV * SinU; } | |
253 | X = A1 * XDir.X() + A2 * YDir.X(); | |
254 | Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
255 | Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
256 | if ( (Nu + 2) % 4 == 0 || (Nu + 1) % 4 == 0 ) | |
257 | { X = - X; Y = - Y; Z = - Z; } | |
258 | } | |
259 | else { | |
260 | Standard_Real RSinV = Radius * sin(V); | |
261 | if (IsOdd (Nu)) { A1 = - SinU; A2 = CosU; } | |
262 | else { A1 = - CosU; A2 = - SinU; } | |
263 | if (IsOdd (Nv)) A3 = - RSinV; | |
264 | else A3 = - RCosV; | |
265 | X = (A1 * XDir.X() + A2 * YDir.X()) * A3; | |
266 | Y = (A1 * XDir.Y() + A2 * YDir.Y()) * A3; | |
267 | Z = (A1 * XDir.Z() + A2 * YDir.Z()) * A3; | |
268 | if ((!((Nu + 2) % 4 == 0 || (Nu + 3) % 4 == 0) && | |
269 | ((Nv + 2) % 4 == 0 || (Nv + 3) % 4 == 0)) || | |
270 | (((Nu + 2) % 4 == 0 || (Nu + 3) % 4 == 0) && | |
271 | !((Nv + 2) % 4 == 0 || (Nv + 3) % 4 == 0))) | |
272 | { X = - X; Y = - Y; Z = - Z; } | |
273 | } | |
274 | return gp_Vec(X,Y,Z); | |
275 | } | |
276 | ||
277 | gp_Vec ElSLib::TorusDN (const Standard_Real U, | |
278 | const Standard_Real V, | |
279 | const gp_Ax3& Pos, | |
280 | const Standard_Real MajorRadius, | |
281 | const Standard_Real MinorRadius, | |
282 | const Standard_Integer Nu, | |
283 | const Standard_Integer Nv) | |
284 | { | |
285 | if (Nu + Nv < 1 || Nu < 0 || Nv < 0) { return gp_Vec(); } | |
286 | Standard_Real CosU = cos(U); | |
287 | Standard_Real SinU = sin(U); | |
288 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
289 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
290 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
291 | Standard_Real A1,A2,A3,X=0,Y=0,Z=0; | |
292 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
293 | Standard_Real eps = 10.*(MinorRadius + MajorRadius)*RealEpsilon(); | |
294 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 End | |
295 | if (Nv == 0) { | |
296 | Standard_Real R = MajorRadius + MinorRadius * cos(V); | |
297 | if (IsOdd (Nu)) { A1 = - R * SinU; A2 = R * CosU; } | |
298 | else { A1 = - R * CosU; A2 = - R * SinU; } | |
299 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
300 | if (Abs(A1) <= eps) | |
301 | A1 = 0.; | |
302 | ||
303 | if (Abs(A2) <= eps) | |
304 | A2 = 0.; | |
305 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
306 | X = A1 * XDir.X() + A2 * YDir.X(); | |
307 | Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
308 | Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
309 | if (!((Nu + 2) % 4 == 0 || (Nu + 3) % 4 == 0)) | |
310 | { X = - X; Y = - Y; Z = - Z; } | |
311 | } | |
312 | else if (Nu == 0) { | |
313 | Standard_Real RCosV = MinorRadius * cos(V); | |
314 | Standard_Real RSinV = MinorRadius * sin(V); | |
315 | if (IsOdd (Nv)) { A1 = - RSinV * CosU; A2 = - RSinV * SinU; A3 = RCosV; } | |
316 | else { A1 = - RCosV * CosU; A2 = - RCosV * SinU; A3 = - RSinV; } | |
317 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
318 | if (Abs(A1) <= eps) | |
319 | A1 = 0.; | |
320 | ||
321 | if (Abs(A2) <= eps) | |
322 | A2 = 0.; | |
323 | ||
324 | if (Abs(A3) <= eps) | |
325 | A3 = 0.; | |
326 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
327 | X = A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X(); | |
328 | Y = A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y(); | |
329 | Z = A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z(); | |
330 | if (!((Nv + 2) % 4 == 0 || (Nv + 3) % 4 == 0)) | |
331 | { X = - X; Y = - Y; Z = - Z; } | |
332 | } | |
333 | else { | |
334 | if (IsOdd (Nu) && | |
335 | IsOdd (Nv)) { | |
336 | Standard_Real RSinV = MinorRadius * sin(V); | |
337 | A1 = RSinV * SinU; A2 = - RSinV * CosU; | |
338 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
339 | if (Abs(A1) <= eps) | |
340 | A1 = 0.; | |
341 | ||
342 | if (Abs(A2) <= eps) | |
343 | A2 = 0.; | |
344 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
345 | X = A1 * XDir.X() + A2 * YDir.X(); | |
346 | Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
347 | Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
348 | } | |
349 | else if (IsEven (Nu) && IsEven (Nv)) { | |
350 | Standard_Real RCosV = MinorRadius * cos(V); | |
351 | A1 = RCosV * CosU; A2 = RCosV * SinU; | |
352 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
353 | if (Abs(A1) <= eps) | |
354 | A1 = 0.; | |
355 | ||
356 | if (Abs(A2) <= eps) | |
357 | A2 = 0.; | |
358 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
359 | X = A1 * XDir.X() + A2 * YDir.X(); | |
360 | Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
361 | Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
362 | } | |
363 | else if (IsEven (Nv) && IsOdd (Nu)) { | |
364 | Standard_Real RCosV = MinorRadius * cos(V); | |
365 | A1 = RCosV * SinU; A2 = - RCosV * CosU; | |
366 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
367 | if (Abs(A1) <= eps) | |
368 | A1 = 0.; | |
369 | ||
370 | if (Abs(A2) <= eps) | |
371 | A2 = 0.; | |
372 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
373 | X = A1 * XDir.X() + A2 * YDir.X(); | |
374 | Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
375 | Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
376 | if (((Nv + Nu + 3) % 4) == 0) | |
377 | { X = - X; Y = - Y; Z = - Z; } | |
378 | } | |
379 | else if (IsOdd (Nv) && IsEven (Nu)) { | |
380 | Standard_Real RSinV = MinorRadius * sin(V); | |
381 | A1 = RSinV * CosU; A2 = RSinV * SinU; | |
382 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
383 | if (Abs(A1) <= eps) | |
384 | A1 = 0.; | |
385 | ||
386 | if (Abs(A2) <= eps) | |
387 | A2 = 0.; | |
388 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
389 | X = A1 * XDir.X() + A2 * YDir.X(); | |
390 | Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
391 | Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
392 | if (((Nu + Nv + 3) % 4) == 0) | |
393 | { X = - X; Y = - Y; Z = - Z; } | |
394 | } | |
395 | } | |
396 | return gp_Vec (X,Y,Z); | |
397 | } | |
398 | ||
399 | void ElSLib::PlaneD0 (const Standard_Real U, | |
400 | const Standard_Real V, | |
401 | const gp_Ax3& Pos, | |
402 | gp_Pnt& P) | |
403 | { | |
404 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
405 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
406 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
407 | P.SetX(U * XDir.X() + V * YDir.X() + PLoc.X()); | |
408 | P.SetY(U * XDir.Y() + V * YDir.Y() + PLoc.Y()); | |
409 | P.SetZ(U * XDir.Z() + V * YDir.Z() + PLoc.Z()); | |
410 | } | |
411 | ||
412 | void ElSLib::ConeD0 (const Standard_Real U, | |
413 | const Standard_Real V, | |
414 | const gp_Ax3& Pos, | |
415 | const Standard_Real Radius, | |
416 | const Standard_Real SAngle, | |
417 | gp_Pnt& P) | |
418 | { | |
419 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
420 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
421 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
422 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
423 | Standard_Real R = Radius + V * sin(SAngle); | |
424 | Standard_Real A3 = V * cos(SAngle); | |
425 | Standard_Real A1 = R * cos(U); | |
426 | Standard_Real A2 = R * sin(U); | |
427 | P.SetX(A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X() + PLoc.X()); | |
428 | P.SetY(A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y() + PLoc.Y()); | |
429 | P.SetZ(A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z() + PLoc.Z()); | |
430 | } | |
431 | ||
432 | void ElSLib::CylinderD0 (const Standard_Real U, | |
433 | const Standard_Real V, | |
434 | const gp_Ax3& Pos, | |
435 | const Standard_Real Radius, | |
436 | gp_Pnt& P) | |
437 | { | |
438 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
439 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
440 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
441 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
442 | Standard_Real A1 = Radius * cos(U); | |
443 | Standard_Real A2 = Radius * sin(U); | |
444 | P.SetX(A1 * XDir.X() + A2 * YDir.X() + V * ZDir.X() + PLoc.X()); | |
445 | P.SetY(A1 * XDir.Y() + A2 * YDir.Y() + V * ZDir.Y() + PLoc.Y()); | |
446 | P.SetZ(A1 * XDir.Z() + A2 * YDir.Z() + V * ZDir.Z() + PLoc.Z()); | |
447 | } | |
448 | ||
449 | void ElSLib::SphereD0 (const Standard_Real U, | |
450 | const Standard_Real V, | |
451 | const gp_Ax3& Pos, | |
452 | const Standard_Real Radius, gp_Pnt& P) | |
453 | { | |
454 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
455 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
456 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
457 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
458 | Standard_Real R = Radius * cos(V); | |
459 | Standard_Real A3 = Radius * sin(V); | |
460 | Standard_Real A1 = R * cos(U); | |
461 | Standard_Real A2 = R * sin(U); | |
462 | P.SetX(A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X() + PLoc.X()); | |
463 | P.SetY(A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y() + PLoc.Y()); | |
464 | P.SetZ(A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z() + PLoc.Z()); | |
465 | } | |
466 | ||
467 | void ElSLib::TorusD0 (const Standard_Real U, | |
468 | const Standard_Real V, | |
469 | const gp_Ax3& Pos, | |
470 | const Standard_Real MajorRadius, | |
471 | const Standard_Real MinorRadius, | |
472 | gp_Pnt& P ) | |
473 | { | |
474 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
475 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
476 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
477 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
478 | Standard_Real R = MajorRadius + MinorRadius * cos(V); | |
479 | Standard_Real A3 = MinorRadius * sin(V); | |
480 | Standard_Real A1 = R * cos(U); | |
481 | Standard_Real A2 = R * sin(U); | |
482 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
483 | Standard_Real eps = 10.*(MinorRadius + MajorRadius)*RealEpsilon(); | |
484 | ||
485 | if (Abs(A1) <= eps) | |
486 | A1 = 0.; | |
487 | ||
488 | if (Abs(A2) <= eps) | |
489 | A2 = 0.; | |
490 | ||
491 | if (Abs(A3) <= eps) | |
492 | A3 = 0.; | |
493 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
494 | P.SetX(A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X() + PLoc.X()); | |
495 | P.SetY(A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y() + PLoc.Y()); | |
496 | P.SetZ(A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z() + PLoc.Z()); | |
497 | } | |
498 | ||
499 | void ElSLib::PlaneD1 (const Standard_Real U, | |
500 | const Standard_Real V, | |
501 | const gp_Ax3& Pos, | |
502 | gp_Pnt& P, | |
503 | gp_Vec& Vu, | |
504 | gp_Vec& Vv) | |
505 | { | |
506 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
507 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
508 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
509 | P.SetX(U * XDir.X() + V * YDir.X() + PLoc.X()); | |
510 | P.SetY(U * XDir.Y() + V * YDir.Y() + PLoc.Y()); | |
511 | P.SetZ(U * XDir.Z() + V * YDir.Z() + PLoc.Z()); | |
512 | Vu.SetX(XDir.X()); | |
513 | Vu.SetY(XDir.Y()); | |
514 | Vu.SetZ(XDir.Z()); | |
515 | Vv.SetX(YDir.X()); | |
516 | Vv.SetY(YDir.Y()); | |
517 | Vv.SetZ(YDir.Z()); | |
518 | } | |
519 | ||
520 | void ElSLib::ConeD1 (const Standard_Real U, | |
521 | const Standard_Real V, | |
522 | const gp_Ax3& Pos, | |
523 | const Standard_Real Radius, | |
524 | const Standard_Real SAngle, | |
525 | gp_Pnt& P, | |
526 | gp_Vec& Vu, | |
527 | gp_Vec& Vv) | |
528 | { | |
529 | // Z = V * Cos(SAngle) | |
530 | // M(U,V) = Location() + V * Cos(SAngle) * ZDirection() + | |
531 | // (Radius + V*Sin(SAng)) * (Cos(U) * XDirection() + Sin(U) * YDirection()) | |
532 | ||
533 | // D1U = | |
534 | //(Radius + V*Sin(SAng)) * (-Sin(U) * XDirection() + Cos(U) * YDirection()) | |
535 | ||
536 | // D1V = | |
537 | // Direction() *Cos(SAngle) + Sin(SAng) * (Cos(U) * XDirection() + | |
538 | // Sin(U) * YDirection()) | |
539 | ||
540 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
541 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
542 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
543 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
544 | Standard_Real CosU = cos(U); | |
545 | Standard_Real SinU = sin(U); | |
546 | Standard_Real CosA = cos(SAngle); | |
547 | Standard_Real SinA = sin(SAngle); | |
548 | Standard_Real R = Radius + V * SinA; | |
549 | Standard_Real A3 = V * CosA; | |
550 | Standard_Real A1 = R * CosU; | |
551 | Standard_Real A2 = R * SinU; | |
552 | Standard_Real R1 = SinA * CosU; | |
553 | Standard_Real R2 = SinA * SinU; | |
554 | P .SetX( A1 * XDir.X() + A2 * YDir.X() + A3 * ZDir.X() + PLoc.X()); | |
555 | P .SetY( A1 * XDir.Y() + A2 * YDir.Y() + A3 * ZDir.Y() + PLoc.Y()); | |
556 | P .SetZ( A1 * XDir.Z() + A2 * YDir.Z() + A3 * ZDir.Z() + PLoc.Z()); | |
557 | Vu.SetX(- A2 * XDir.X() + A1 * YDir.X()); | |
558 | Vu.SetY(- A2 * XDir.Y() + A1 * YDir.Y()); | |
559 | Vu.SetZ(- A2 * XDir.Z() + A1 * YDir.Z()); | |
560 | Vv.SetX( R1 * XDir.X() + R2 * YDir.X() + CosA * ZDir.X()); | |
561 | Vv.SetY( R1 * XDir.Y() + R2 * YDir.Y() + CosA * ZDir.Y()); | |
562 | Vv.SetZ( R1 * XDir.Z() + R2 * YDir.Z() + CosA * ZDir.Z()); | |
563 | } | |
564 | ||
565 | void ElSLib::CylinderD1 (const Standard_Real U, | |
566 | const Standard_Real V, | |
567 | const gp_Ax3& Pos, | |
568 | const Standard_Real Radius, | |
569 | gp_Pnt& P, | |
570 | gp_Vec& Vu, | |
571 | gp_Vec& Vv) | |
572 | { | |
573 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
574 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
575 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
576 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
577 | Standard_Real A1 = Radius * cos(U); | |
578 | Standard_Real A2 = Radius * sin(U); | |
579 | P .SetX( A1 * XDir.X() + A2 * YDir.X() + V * ZDir.X() + PLoc.X()); | |
580 | P .SetY( A1 * XDir.Y() + A2 * YDir.Y() + V * ZDir.Y() + PLoc.Y()); | |
581 | P .SetZ( A1 * XDir.Z() + A2 * YDir.Z() + V * ZDir.Z() + PLoc.Z()); | |
582 | Vu.SetX(- A2 * XDir.X() + A1 * YDir.X()); | |
583 | Vu.SetY(- A2 * XDir.Y() + A1 * YDir.Y()); | |
584 | Vu.SetZ(- A2 * XDir.Z() + A1 * YDir.Z()); | |
585 | Vv.SetX( ZDir.X()); | |
586 | Vv.SetY( ZDir.Y()); | |
587 | Vv.SetZ( ZDir.Z()); | |
588 | } | |
589 | ||
590 | void ElSLib::SphereD1 (const Standard_Real U, | |
591 | const Standard_Real V, | |
592 | const gp_Ax3& Pos, | |
593 | const Standard_Real Radius, | |
594 | gp_Pnt& P, | |
595 | gp_Vec& Vu, | |
596 | gp_Vec& Vv) | |
597 | { | |
598 | // Vxy = CosU * XDirection + SinU * YDirection | |
599 | // DVxy = -SinU * XDirection + CosU * YDirection | |
600 | ||
601 | // P(U,V) = Location + R * CosV * Vxy + R * SinV * Direction | |
602 | ||
603 | // Vu = R * CosV * DVxy | |
604 | ||
605 | // Vv = -R * SinV * Vxy + R * CosV * Direction | |
606 | ||
607 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
608 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
609 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
610 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
611 | Standard_Real CosU = cos(U); | |
612 | Standard_Real SinU = sin(U); | |
613 | Standard_Real R1 = Radius * cos(V); | |
614 | Standard_Real R2 = Radius * sin(V); | |
615 | Standard_Real A1 = R1 * CosU; | |
616 | Standard_Real A2 = R1 * SinU; | |
617 | Standard_Real A3 = R2 * CosU; | |
618 | Standard_Real A4 = R2 * SinU; | |
619 | P .SetX( A1 * XDir.X() + A2 * YDir.X() + R2 * ZDir.X() + PLoc.X()); | |
620 | P .SetY( A1 * XDir.Y() + A2 * YDir.Y() + R2 * ZDir.Y() + PLoc.Y()); | |
621 | P .SetZ( A1 * XDir.Z() + A2 * YDir.Z() + R2 * ZDir.Z() + PLoc.Z()); | |
622 | Vu.SetX(- A2 * XDir.X() + A1 * YDir.X()); | |
623 | Vu.SetY(- A2 * XDir.Y() + A1 * YDir.Y()); | |
624 | Vu.SetZ(- A2 * XDir.Z() + A1 * YDir.Z()); | |
625 | Vv.SetX(- A3 * XDir.X() - A4 * YDir.X() + R1 * ZDir.X()); | |
626 | Vv.SetY(- A3 * XDir.Y() - A4 * YDir.Y() + R1 * ZDir.Y()); | |
627 | Vv.SetZ(- A3 * XDir.Z() - A4 * YDir.Z() + R1 * ZDir.Z()); | |
628 | } | |
629 | ||
630 | void ElSLib::TorusD1 ( const Standard_Real U, | |
631 | const Standard_Real V, | |
632 | const gp_Ax3& Pos, | |
633 | const Standard_Real MajorRadius, | |
634 | const Standard_Real MinorRadius, | |
635 | gp_Pnt& P, | |
636 | gp_Vec& Vu, | |
637 | gp_Vec& Vv) | |
638 | { | |
639 | ||
640 | //P(U,V) = | |
641 | // Location + | |
642 | // (MajorRadius+MinorRadius*Cos(V)) * | |
643 | // (Cos(U)*XDirection + Sin(U)*YDirection) + | |
644 | // MinorRadius * Sin(V) * Direction | |
645 | ||
646 | //Vv = -MinorRadius * Sin(V) * (Cos(U)*XDirection + Sin(U)*YDirection) + | |
647 | // MinorRadius * Cos(V) * Direction | |
648 | ||
649 | //Vu = | |
650 | // (MajorRadius+MinorRadius*Cos(V)) * | |
651 | // (-Sin(U)*XDirection + Cos(U)*YDirection) | |
652 | ||
653 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
654 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
655 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
656 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
657 | Standard_Real CosU = cos(U); | |
658 | Standard_Real SinU = sin(U); | |
659 | Standard_Real R1 = MinorRadius * cos(V); | |
660 | Standard_Real R2 = MinorRadius * sin(V); | |
661 | Standard_Real R = MajorRadius + R1; | |
662 | Standard_Real A1 = R * CosU; | |
663 | Standard_Real A2 = R * SinU; | |
664 | Standard_Real A3 = R2 * CosU; | |
665 | Standard_Real A4 = R2 * SinU; | |
666 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
667 | Standard_Real eps = 10.*(MinorRadius + MajorRadius)*RealEpsilon(); | |
668 | ||
669 | if (Abs(A1) <= eps) | |
670 | A1 = 0.; | |
671 | ||
672 | if (Abs(A2) <= eps) | |
673 | A2 = 0.; | |
674 | ||
675 | if (Abs(A3) <= eps) | |
676 | A3 = 0.; | |
677 | ||
678 | if (Abs(A4) <= eps) | |
679 | A4 = 0.; | |
680 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
681 | P .SetX( A1 * XDir.X() + A2 * YDir.X() + R2 * ZDir.X() + PLoc.X()); | |
682 | P .SetY( A1 * XDir.Y() + A2 * YDir.Y() + R2 * ZDir.Y() + PLoc.Y()); | |
683 | P .SetZ( A1 * XDir.Z() + A2 * YDir.Z() + R2 * ZDir.Z() + PLoc.Z()); | |
684 | Vu.SetX(- A2 * XDir.X() + A1 * YDir.X()); | |
685 | Vu.SetY(- A2 * XDir.Y() + A1 * YDir.Y()); | |
686 | Vu.SetZ(- A2 * XDir.Z() + A1 * YDir.Z()); | |
687 | Vv.SetX(- A3 * XDir.X() - A4 * YDir.X() + R1 * ZDir.X()); | |
688 | Vv.SetY(- A3 * XDir.Y() - A4 * YDir.Y() + R1 * ZDir.Y()); | |
689 | Vv.SetZ(- A3 * XDir.Z() - A4 * YDir.Z() + R1 * ZDir.Z()); | |
690 | } | |
691 | ||
692 | void ElSLib::ConeD2 (const Standard_Real U, | |
693 | const Standard_Real V, | |
694 | const gp_Ax3& Pos, | |
695 | const Standard_Real Radius, | |
696 | const Standard_Real SAngle, | |
697 | gp_Pnt& P, | |
698 | gp_Vec& Vu, | |
699 | gp_Vec& Vv, | |
700 | gp_Vec& Vuu, | |
701 | gp_Vec& Vvv, | |
702 | gp_Vec& Vuv) | |
703 | { | |
704 | // Z = V * Cos(SAngle) | |
705 | // M(U,V) = Location() + V * Cos(SAngle) * Direction() + | |
706 | // (Radius + V*Sin(SAng)) * (Cos(U) * XDirection() + Sin(U) * YDirection()) | |
707 | ||
708 | // DU = | |
709 | //(Radius + V*Sin(SAng)) * (-Sin(U) * XDirection() + Cos(U) * YDirection()) | |
710 | ||
711 | // DV = | |
712 | // Direction() *Cos(SAngle) + Sin(SAng) * (Cos(U) * XDirection() + | |
713 | // Sin(U) * YDirection()) | |
714 | ||
715 | // D2U = | |
716 | //(Radius + V*Sin(SAng)) * (-Cos(U) * XDirection() - Sin(U) * YDirection()) | |
717 | ||
718 | // D2V = 0.0 | |
719 | ||
720 | // DUV = | |
721 | //Sin(SAng) * (-Sin(U) * XDirection() + Cos(U) * YDirection()) | |
722 | ||
723 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
724 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
725 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
726 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
727 | Standard_Real CosU = cos(U); | |
728 | Standard_Real SinU = sin(U); | |
729 | Standard_Real CosA = cos(SAngle); | |
730 | Standard_Real SinA = sin(SAngle); | |
731 | Standard_Real R = Radius + V * SinA; | |
732 | Standard_Real A3 = V * CosA; | |
733 | Standard_Real A1 = R * CosU; | |
734 | Standard_Real A2 = R * SinU; | |
735 | Standard_Real R1 = SinA * CosU; | |
736 | Standard_Real R2 = SinA * SinU; | |
737 | Standard_Real Som1X = A1 * XDir.X() + A2 * YDir.X(); | |
738 | Standard_Real Som1Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
739 | Standard_Real Som1Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
740 | P .SetX( Som1X + A3 * ZDir.X() + PLoc.X()); | |
741 | P .SetY( Som1Y + A3 * ZDir.Y() + PLoc.Y()); | |
742 | P .SetZ( Som1Z + A3 * ZDir.Z() + PLoc.Z()); | |
743 | Vu .SetX(- A2 * XDir.X() + A1 * YDir.X()); | |
744 | Vu .SetY(- A2 * XDir.Y() + A1 * YDir.Y()); | |
745 | Vu .SetZ(- A2 * XDir.Z() + A1 * YDir.Z()); | |
746 | Vv .SetX( R1 * XDir.X() + R2 * YDir.X() + CosA * ZDir.X()); | |
747 | Vv .SetY( R1 * XDir.Y() + R2 * YDir.Y() + CosA * ZDir.Y()); | |
748 | Vv .SetZ( R1 * XDir.Z() + R2 * YDir.Z() + CosA * ZDir.Z()); | |
749 | Vuu.SetX(- Som1X); | |
750 | Vuu.SetY(- Som1Y); | |
751 | Vuu.SetZ(- Som1Z); | |
752 | Vvv.SetX( 0.0); | |
753 | Vvv.SetY( 0.0); | |
754 | Vvv.SetZ( 0.0); | |
755 | Vuv.SetX(- R2 * XDir.X() + R1 * YDir.X()); | |
756 | Vuv.SetY(- R2 * XDir.Y() + R1 * YDir.Y()); | |
757 | Vuv.SetZ(- R2 * XDir.Z() + R1 * YDir.Z()); | |
758 | } | |
759 | ||
760 | void ElSLib::CylinderD2 (const Standard_Real U, | |
761 | const Standard_Real V, | |
762 | const gp_Ax3& Pos, | |
763 | const Standard_Real Radius, | |
764 | gp_Pnt& P, | |
765 | gp_Vec& Vu, | |
766 | gp_Vec& Vv, | |
767 | gp_Vec& Vuu, | |
768 | gp_Vec& Vvv, | |
769 | gp_Vec& Vuv) | |
770 | { | |
771 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
772 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
773 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
774 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
775 | Standard_Real A1 = Radius * cos(U); | |
776 | Standard_Real A2 = Radius * sin(U); | |
777 | Standard_Real Som1X = A1 * XDir.X() + A2 * YDir.X(); | |
778 | Standard_Real Som1Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
779 | Standard_Real Som1Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
780 | P .SetX( Som1X + V * ZDir.X() + PLoc.X()); | |
781 | P .SetY( Som1Y + V * ZDir.Y() + PLoc.Y()); | |
782 | P .SetZ( Som1Z + V * ZDir.Z() + PLoc.Z()); | |
783 | Vu .SetX(- A2 * XDir.X() + A1 * YDir.X()); | |
784 | Vu .SetY(- A2 * XDir.Y() + A1 * YDir.Y()); | |
785 | Vu .SetZ(- A2 * XDir.Z() + A1 * YDir.Z()); | |
786 | Vv .SetX( ZDir.X()); | |
787 | Vv .SetY( ZDir.Y()); | |
788 | Vv .SetZ( ZDir.Z()); | |
789 | Vuu.SetX(- Som1X); | |
790 | Vuu.SetY(- Som1Y); | |
791 | Vuu.SetZ(- Som1Z); | |
792 | Vvv.SetX( 0.0); | |
793 | Vvv.SetY( 0.0); | |
794 | Vvv.SetZ( 0.0); | |
795 | Vuv.SetX( 0.0); | |
796 | Vuv.SetY( 0.0); | |
797 | Vuv.SetZ( 0.0); | |
798 | } | |
799 | ||
800 | void ElSLib::SphereD2 (const Standard_Real U, | |
801 | const Standard_Real V, | |
802 | const gp_Ax3& Pos, | |
803 | const Standard_Real Radius, | |
804 | gp_Pnt& P, | |
805 | gp_Vec& Vu, | |
806 | gp_Vec& Vv, | |
807 | gp_Vec& Vuu, | |
808 | gp_Vec& Vvv, | |
809 | gp_Vec& Vuv) | |
810 | { | |
811 | // Vxy = CosU * XDirection + SinU * YDirection | |
812 | // DVxy = -SinU * XDirection + CosU * YDirection | |
813 | ||
814 | // P(U,V) = Location + R * CosV * Vxy + R * SinV * Direction | |
815 | ||
816 | // Vu = R * CosV * DVxy | |
817 | ||
818 | // Vuu = - R * CosV * Vxy | |
819 | ||
820 | // Vv = -R * SinV * Vxy + R * CosV * Direction | |
821 | ||
822 | // Vvv = -R * CosV * Vxy - R * SinV * Direction | |
823 | ||
824 | // Vuv = - R * SinV * DVxy | |
825 | ||
826 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
827 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
828 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
829 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
830 | Standard_Real CosU = cos(U); | |
831 | Standard_Real SinU = sin(U); | |
832 | Standard_Real R1 = Radius * cos(V); | |
833 | Standard_Real R2 = Radius * sin(V); | |
834 | Standard_Real A1 = R1 * CosU; | |
835 | Standard_Real A2 = R1 * SinU; | |
836 | Standard_Real A3 = R2 * CosU; | |
837 | Standard_Real A4 = R2 * SinU; | |
838 | Standard_Real Som1X = A1 * XDir.X() + A2 * YDir.X(); | |
839 | Standard_Real Som1Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
840 | Standard_Real Som1Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
841 | Standard_Real R2ZX = R2 * ZDir.X(); | |
842 | Standard_Real R2ZY = R2 * ZDir.Y(); | |
843 | Standard_Real R2ZZ = R2 * ZDir.Z(); | |
844 | P .SetX( Som1X + R2ZX + PLoc.X()); | |
845 | P .SetY( Som1Y + R2ZY + PLoc.Y()); | |
846 | P .SetZ( Som1Z + R2ZZ + PLoc.Z()); | |
847 | Vu .SetX(- A2 * XDir.X() + A1 * YDir.X()); | |
848 | Vu .SetY(- A2 * XDir.Y() + A1 * YDir.Y()); | |
849 | Vu .SetZ(- A2 * XDir.Z() + A1 * YDir.Z()); | |
850 | Vv .SetX(- A3 * XDir.X() - A4 * YDir.X() + R1 * ZDir.X()); | |
851 | Vv .SetY(- A3 * XDir.Y() - A4 * YDir.Y() + R1 * ZDir.Y()); | |
852 | Vv .SetZ(- A3 * XDir.Z() - A4 * YDir.Z() + R1 * ZDir.Z()); | |
853 | Vuu.SetX(- Som1X); | |
854 | Vuu.SetY(- Som1Y); | |
855 | Vuu.SetZ(- Som1Z); | |
856 | Vvv.SetX(- Som1X - R2ZX); | |
857 | Vvv.SetY(- Som1Y - R2ZY); | |
858 | Vvv.SetZ(- Som1Z - R2ZZ); | |
859 | Vuv.SetX( A4 * XDir.X() - A3 * YDir.X()); | |
860 | Vuv.SetY( A4 * XDir.Y() - A3 * YDir.Y()); | |
861 | Vuv.SetZ( A4 * XDir.Z() - A3 * YDir.Z()); | |
862 | } | |
863 | ||
864 | void ElSLib::TorusD2 (const Standard_Real U, | |
865 | const Standard_Real V, | |
866 | const gp_Ax3& Pos, | |
867 | const Standard_Real MajorRadius, | |
868 | const Standard_Real MinorRadius, | |
869 | gp_Pnt& P, | |
870 | gp_Vec& Vu, | |
871 | gp_Vec& Vv, | |
872 | gp_Vec& Vuu, | |
873 | gp_Vec& Vvv, | |
874 | gp_Vec& Vuv) | |
875 | { | |
876 | //P(U,V) = | |
877 | // Location + | |
878 | // (MajorRadius+MinorRadius*Cos(V)) * | |
879 | // (Cos(U)*XDirection + Sin(U)*YDirection) + | |
880 | // MinorRadius * Sin(V) * Direction | |
881 | ||
882 | //Vv = -MinorRadius * Sin(V) * (Cos(U)*XDirection + Sin(U)*YDirection) + | |
883 | // MinorRadius * Cos(V) * Direction | |
884 | ||
885 | //Vu = | |
886 | // (MajorRadius+MinorRadius*Cos(V)) * | |
887 | // (-Sin(U)*XDirection + Cos(U)*YDirection) | |
888 | ||
889 | ||
890 | //Vvv = -MinorRadius * Cos(V) * (Cos(U)*XDirection + Sin(U)*YDirection) | |
891 | // -MinorRadius * Sin(V) * Direction | |
892 | ||
893 | //Vuu = | |
894 | // -(MajorRadius+MinorRadius*Cos(V)) * | |
895 | // (Cos(U)*XDirection + Sin(U)*YDirection) | |
896 | ||
897 | //Vuv = MinorRadius * Sin(V) * (Sin(U)*XDirection - Cos(U)*YDirection) | |
898 | ||
899 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
900 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
901 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
902 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
903 | Standard_Real CosU = cos(U); | |
904 | Standard_Real SinU = sin(U); | |
905 | Standard_Real R1 = MinorRadius * cos(V); | |
906 | Standard_Real R2 = MinorRadius * sin(V); | |
907 | Standard_Real R = MajorRadius + R1; | |
908 | Standard_Real A1 = R * CosU; | |
909 | Standard_Real A2 = R * SinU; | |
910 | Standard_Real A3 = R2 * CosU; | |
911 | Standard_Real A4 = R2 * SinU; | |
912 | Standard_Real A5 = R1 * CosU; | |
913 | Standard_Real A6 = R1 * SinU; | |
914 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
915 | Standard_Real eps = 10.*(MinorRadius + MajorRadius)*RealEpsilon(); | |
916 | ||
917 | if (Abs(A1) <= eps) | |
918 | A1 = 0.; | |
919 | ||
920 | if (Abs(A2) <= eps) | |
921 | A2 = 0.; | |
922 | ||
923 | if (Abs(A3) <= eps) | |
924 | A3 = 0.; | |
925 | ||
926 | if (Abs(A4) <= eps) | |
927 | A4 = 0.; | |
928 | ||
929 | if (Abs(A5) <= eps) | |
930 | A5 = 0.; | |
931 | ||
932 | if (Abs(A6) <= eps) | |
933 | A6 = 0.; | |
934 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
935 | Standard_Real Som1X = A1 * XDir.X() + A2 * YDir.X(); | |
936 | Standard_Real Som1Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
937 | Standard_Real Som1Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
938 | Standard_Real R2ZX = R2 * ZDir.X(); | |
939 | Standard_Real R2ZY = R2 * ZDir.Y(); | |
940 | Standard_Real R2ZZ = R2 * ZDir.Z(); | |
941 | P .SetX( Som1X + R2ZX + PLoc.X()); | |
942 | P .SetY( Som1Y + R2ZY + PLoc.Y()); | |
943 | P .SetZ( Som1Z + R2ZZ + PLoc.Z()); | |
944 | Vu .SetX(- A2 * XDir.X() + A1 * YDir.X()); | |
945 | Vu .SetY(- A2 * XDir.Y() + A1 * YDir.Y()); | |
946 | Vu .SetZ(- A2 * XDir.Z() + A1 * YDir.Z()); | |
947 | Vv .SetX(- A3 * XDir.X() - A4 * YDir.X() + R1 * ZDir.X()); | |
948 | Vv .SetY(- A3 * XDir.Y() - A4 * YDir.Y() + R1 * ZDir.Y()); | |
949 | Vv .SetZ(- A3 * XDir.Z() - A4 * YDir.Z() + R1 * ZDir.Z()); | |
950 | Vuu.SetX(- Som1X); | |
951 | Vuu.SetY(- Som1Y); | |
952 | Vuu.SetZ(- Som1Z); | |
953 | Vvv.SetX(- A5 * XDir.X() - A6 * YDir.X() - R2ZX); | |
954 | Vvv.SetY(- A5 * XDir.Y() - A6 * YDir.Y() - R2ZY); | |
955 | Vvv.SetZ(- A5 * XDir.Z() - A6 * YDir.Z() - R2ZZ); | |
956 | Vuv.SetX( A4 * XDir.X() - A3 * YDir.X()); | |
957 | Vuv.SetY( A4 * XDir.Y() - A3 * YDir.Y()); | |
958 | Vuv.SetZ( A4 * XDir.Z() - A3 * YDir.Z()); | |
959 | } | |
960 | ||
961 | void ElSLib::ConeD3 (const Standard_Real U, | |
962 | const Standard_Real V, | |
963 | const gp_Ax3& Pos, | |
964 | const Standard_Real Radius, | |
965 | const Standard_Real SAngle, | |
966 | gp_Pnt& P, | |
967 | gp_Vec& Vu, gp_Vec& Vv, | |
968 | gp_Vec& Vuu, gp_Vec& Vvv, gp_Vec& Vuv, | |
969 | gp_Vec& Vuuu, gp_Vec& Vvvv, | |
970 | gp_Vec& Vuuv, gp_Vec& Vuvv) | |
971 | { | |
972 | // Z = V * Cos(SAngle) | |
973 | // M(U,V) = Location() + V * Cos(SAngle) * Direction() + | |
974 | // (Radius + V*Sin(SAng)) * (Cos(U) * XDirection() + Sin(U) * YDirection()) | |
975 | ||
976 | // DU = | |
977 | //(Radius + V*Sin(SAng)) * (-Sin(U) * XDirection() + Cos(U) * YDirection()) | |
978 | ||
979 | // DV = | |
980 | // Direction() *Cos(SAngle) + Sin(SAng) * (Cos(U) * XDirection() + | |
981 | // Sin(U) * YDirection()) | |
982 | ||
983 | // D2U = | |
984 | //(Radius + V*Sin(SAng)) * (-Cos(U) * XDirection() - Sin(U) * YDirection()) | |
985 | ||
986 | // D2V = 0.0 | |
987 | ||
988 | // DUV = | |
989 | //Sin(SAng) * (-Sin(U) * XDirection() + Cos(U) * YDirection()) | |
990 | ||
991 | // D3U = | |
992 | //(Radius + V*Sin(SAng)) * (Sin(U) * XDirection() - Cos(U) * YDirection()) | |
993 | ||
994 | // DUVV = 0.0 | |
995 | ||
996 | // D3V = 0.0 | |
997 | ||
998 | // DUUV = Sin(SAng) * (-Cos(U)*XDirection()-Sin(U) * YDirection()) + | |
999 | ||
1000 | ||
1001 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
1002 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
1003 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
1004 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
1005 | Standard_Real CosU = cos(U); | |
1006 | Standard_Real SinU = sin(U); | |
1007 | Standard_Real CosA = cos(SAngle); | |
1008 | Standard_Real SinA = sin(SAngle); | |
1009 | Standard_Real R = Radius + V * SinA; | |
1010 | Standard_Real A3 = V * CosA; | |
1011 | Standard_Real A1 = R * CosU; | |
1012 | Standard_Real A2 = R * SinU; | |
1013 | Standard_Real R1 = SinA * CosU; | |
1014 | Standard_Real R2 = SinA * SinU; | |
1015 | Standard_Real Som1X = A1 * XDir.X() + A2 * YDir.X(); | |
1016 | Standard_Real Som1Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
1017 | Standard_Real Som1Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
1018 | Standard_Real Som2X = R1 * XDir.X() + R2 * YDir.X(); | |
1019 | Standard_Real Som2Y = R1 * XDir.Y() + R2 * YDir.Y(); | |
1020 | Standard_Real Som2Z = R1 * XDir.Z() + R2 * YDir.Z(); | |
1021 | Standard_Real Dif1X = A2 * XDir.X() - A1 * YDir.X(); | |
1022 | Standard_Real Dif1Y = A2 * XDir.Y() - A1 * YDir.Y(); | |
1023 | Standard_Real Dif1Z = A2 * XDir.Z() - A1 * YDir.Z(); | |
1024 | P .SetX( Som1X + A3 * ZDir.X() + PLoc.X()); | |
1025 | P .SetY( Som1Y + A3 * ZDir.Y() + PLoc.Y()); | |
1026 | P .SetZ( Som1Z + A3 * ZDir.Z() + PLoc.Z()); | |
1027 | Vu .SetX(- Dif1X); | |
1028 | Vu .SetY(- Dif1Y); | |
1029 | Vu .SetZ(- Dif1Z); | |
1030 | Vv .SetX( Som2X + CosA * ZDir.X()); | |
1031 | Vv .SetY( Som2Y + CosA * ZDir.Y()); | |
1032 | Vv .SetZ( Som2Z + CosA * ZDir.Z()); | |
1033 | Vuu .SetX(- Som1X); | |
1034 | Vuu .SetY(- Som1Y); | |
1035 | Vuu .SetZ(- Som1Z); | |
1036 | Vvv .SetX( 0.0); | |
1037 | Vvv .SetY( 0.0); | |
1038 | Vvv .SetZ( 0.0); | |
1039 | Vuv .SetX(- R2 * XDir.X() + R1 * YDir.X()); | |
1040 | Vuv .SetY(- R2 * XDir.Y() + R1 * YDir.Y()); | |
1041 | Vuv .SetZ(- R2 * XDir.Z() + R1 * YDir.Z()); | |
1042 | Vuuu.SetX( Dif1X); | |
1043 | Vuuu.SetY( Dif1Y); | |
1044 | Vuuu.SetZ( Dif1Z); | |
1045 | Vvvv.SetX( 0.0); | |
1046 | Vvvv.SetY( 0.0); | |
1047 | Vvvv.SetZ( 0.0); | |
1048 | Vuvv.SetX( 0.0); | |
1049 | Vuvv.SetY( 0.0); | |
1050 | Vuvv.SetZ( 0.0); | |
1051 | Vuuv.SetX(- Som2X); | |
1052 | Vuuv.SetY(- Som2Y); | |
1053 | Vuuv.SetZ(- Som2Z); | |
1054 | } | |
1055 | ||
1056 | void ElSLib::CylinderD3 (const Standard_Real U, | |
1057 | const Standard_Real V, | |
1058 | const gp_Ax3& Pos, | |
1059 | const Standard_Real Radius, | |
1060 | gp_Pnt& P, | |
1061 | gp_Vec& Vu, gp_Vec& Vv, | |
1062 | gp_Vec& Vuu, gp_Vec& Vvv, gp_Vec& Vuv, | |
1063 | gp_Vec& Vuuu, gp_Vec& Vvvv, | |
1064 | gp_Vec& Vuuv, gp_Vec& Vuvv) | |
1065 | { | |
1066 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
1067 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
1068 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
1069 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
1070 | Standard_Real A1 = Radius * cos(U); | |
1071 | Standard_Real A2 = Radius * sin(U); | |
1072 | Standard_Real Som1X = A1 * XDir.X() + A2 * YDir.X(); | |
1073 | Standard_Real Som1Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
1074 | Standard_Real Som1Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
1075 | Standard_Real Dif1X = A2 * XDir.X() - A1 * YDir.X(); | |
1076 | Standard_Real Dif1Y = A2 * XDir.Y() - A1 * YDir.Y(); | |
1077 | Standard_Real Dif1Z = A2 * XDir.Z() - A1 * YDir.Z(); | |
1078 | P .SetX( Som1X + V * ZDir.X() + PLoc.X()); | |
1079 | P .SetY( Som1Y + V * ZDir.Y() + PLoc.Y()); | |
1080 | P .SetZ( Som1Z + V * ZDir.Z() + PLoc.Z()); | |
1081 | Vu .SetX(- Dif1X); | |
1082 | Vu .SetY(- Dif1Y); | |
1083 | Vu .SetZ(- Dif1Z); | |
1084 | Vv .SetX( ZDir.X()); | |
1085 | Vv .SetY( ZDir.Y()); | |
1086 | Vv .SetZ( ZDir.Z()); | |
1087 | Vuu .SetX(- Som1X); | |
1088 | Vuu .SetY(- Som1Y); | |
1089 | Vuu .SetZ(- Som1Z); | |
1090 | Vvv .SetX( 0.0); | |
1091 | Vvv .SetY( 0.0); | |
1092 | Vvv .SetZ( 0.0); | |
1093 | Vuv .SetX( 0.0); | |
1094 | Vuv .SetY( 0.0); | |
1095 | Vuv .SetZ( 0.0); | |
1096 | Vuuu.SetX( Dif1X); | |
1097 | Vuuu.SetY( Dif1Y); | |
1098 | Vuuu.SetZ( Dif1Z); | |
1099 | Vvvv.SetX( 0.0); | |
1100 | Vvvv.SetY( 0.0); | |
1101 | Vvvv.SetZ( 0.0); | |
1102 | Vuvv.SetX( 0.0); | |
1103 | Vuvv.SetY( 0.0); | |
1104 | Vuvv.SetZ( 0.0); | |
1105 | Vuuv.SetX( 0.0); | |
1106 | Vuuv.SetY( 0.0); | |
1107 | Vuuv.SetZ( 0.0); | |
1108 | } | |
1109 | ||
1110 | void ElSLib::SphereD3 (const Standard_Real U, | |
1111 | const Standard_Real V, | |
1112 | const gp_Ax3& Pos, | |
1113 | const Standard_Real Radius, | |
1114 | gp_Pnt& P, | |
1115 | gp_Vec& Vu, gp_Vec& Vv, | |
1116 | gp_Vec& Vuu, gp_Vec& Vvv, gp_Vec& Vuv, | |
1117 | gp_Vec& Vuuu, gp_Vec& Vvvv, | |
1118 | gp_Vec& Vuuv, gp_Vec& Vuvv) | |
1119 | { | |
1120 | ||
1121 | // Vxy = CosU * XDirection + SinU * YDirection | |
1122 | // DVxy = -SinU * XDirection + CosU * YDirection | |
1123 | ||
1124 | // P(U,V) = Location + R * CosV * Vxy + R * SinV * Direction | |
1125 | ||
1126 | // Vu = R * CosV * DVxy | |
1127 | ||
1128 | // Vuu = - R * CosV * Vxy | |
1129 | ||
1130 | // Vuuu = - Vu | |
1131 | ||
1132 | // Vv = -R * SinV * Vxy + R * CosV * Direction | |
1133 | ||
1134 | // Vvv = -R * CosV * Vxy - R * SinV * Direction | |
1135 | ||
1136 | // Vvvv = -Vv | |
1137 | ||
1138 | // Vuv = - R * SinV * DVxy | |
1139 | ||
1140 | // Vuuv = R * SinV * Vxy | |
1141 | ||
1142 | // Vuvv = - R * CosV * DVxy = Vuuu = -Vu | |
1143 | ||
1144 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
1145 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
1146 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
1147 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
1148 | Standard_Real CosU = cos(U); | |
1149 | Standard_Real SinU = sin(U); | |
1150 | Standard_Real R1 = Radius * cos(V); | |
1151 | Standard_Real R2 = Radius * sin(V); | |
1152 | Standard_Real A1 = R1 * CosU; | |
1153 | Standard_Real A2 = R1 * SinU; | |
1154 | Standard_Real A3 = R2 * CosU; | |
1155 | Standard_Real A4 = R2 * SinU; | |
1156 | Standard_Real Som1X = A1 * XDir.X() + A2 * YDir.X(); | |
1157 | Standard_Real Som1Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
1158 | Standard_Real Som1Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
1159 | Standard_Real Som3X = A3 * XDir.X() + A4 * YDir.X(); | |
1160 | Standard_Real Som3Y = A3 * XDir.Y() + A4 * YDir.Y(); | |
1161 | Standard_Real Som3Z = A3 * XDir.Z() + A4 * YDir.Z(); | |
1162 | Standard_Real Dif1X = A2 * XDir.X() - A1 * YDir.X(); | |
1163 | Standard_Real Dif1Y = A2 * XDir.Y() - A1 * YDir.Y(); | |
1164 | Standard_Real Dif1Z = A2 * XDir.Z() - A1 * YDir.Z(); | |
1165 | Standard_Real R1ZX = R1 * ZDir.X(); | |
1166 | Standard_Real R1ZY = R1 * ZDir.Y(); | |
1167 | Standard_Real R1ZZ = R1 * ZDir.Z(); | |
1168 | Standard_Real R2ZX = R2 * ZDir.X(); | |
1169 | Standard_Real R2ZY = R2 * ZDir.Y(); | |
1170 | Standard_Real R2ZZ = R2 * ZDir.Z(); | |
1171 | P .SetX( Som1X + R2ZX + PLoc.X()); | |
1172 | P .SetY( Som1Y + R2ZY + PLoc.Y()); | |
1173 | P .SetZ( Som1Z + R2ZZ + PLoc.Z()); | |
1174 | Vu .SetX(- Dif1X); | |
1175 | Vu .SetY(- Dif1Y); | |
1176 | Vu .SetZ(- Dif1Z); | |
1177 | Vv .SetX(- Som3X + R1ZX); | |
1178 | Vv .SetY(- Som3Y + R1ZY); | |
1179 | Vv .SetZ(- Som3Z + R1ZZ); | |
1180 | Vuu .SetX(- Som1X); | |
1181 | Vuu .SetY(- Som1Y); | |
1182 | Vuu .SetZ(- Som1Z); | |
1183 | Vvv .SetX(- Som1X - R2ZX); | |
1184 | Vvv .SetY(- Som1Y - R2ZY); | |
1185 | Vvv .SetZ(- Som1Z - R2ZZ); | |
1186 | Vuv .SetX( A4 * XDir.X() - A3 * YDir.X()); | |
1187 | Vuv .SetY( A4 * XDir.Y() - A3 * YDir.Y()); | |
1188 | Vuv .SetZ( A4 * XDir.Z() - A3 * YDir.Z()); | |
1189 | Vuuu.SetX( Dif1X); | |
1190 | Vuuu.SetY( Dif1Y); | |
1191 | Vuuu.SetZ( Dif1Z); | |
1192 | Vvvv.SetX( Som3X - R1ZX); | |
1193 | Vvvv.SetY( Som3Y - R1ZY); | |
1194 | Vvvv.SetZ( Som3Z - R1ZZ); | |
1195 | Vuvv.SetX( Dif1X); | |
1196 | Vuvv.SetY( Dif1Y); | |
1197 | Vuvv.SetZ( Dif1Z); | |
1198 | Vuuv.SetX( Som3X); | |
1199 | Vuuv.SetY( Som3Y); | |
1200 | Vuuv.SetZ( Som3Z); | |
1201 | } | |
1202 | ||
1203 | void ElSLib::TorusD3 (const Standard_Real U, | |
1204 | const Standard_Real V, | |
1205 | const gp_Ax3& Pos, | |
1206 | const Standard_Real MajorRadius, | |
1207 | const Standard_Real MinorRadius, | |
1208 | gp_Pnt& P, | |
1209 | gp_Vec& Vu, gp_Vec& Vv, | |
1210 | gp_Vec& Vuu, gp_Vec& Vvv, gp_Vec& Vuv, | |
1211 | gp_Vec& Vuuu, gp_Vec& Vvvv, | |
1212 | gp_Vec& Vuuv, gp_Vec& Vuvv) | |
1213 | { | |
1214 | ||
1215 | //P(U,V) = | |
1216 | // Location + | |
1217 | // (MajorRadius+MinorRadius*Cos(V)) * | |
1218 | // (Cos(U)*XDirection + Sin(U)*YDirection) + | |
1219 | // MinorRadius * Sin(V) * Direction | |
1220 | ||
1221 | //Vv = -MinorRadius * Sin(V) * (Cos(U)*XDirection + Sin(U)*YDirection) + | |
1222 | // MinorRadius * Cos(V) * Direction | |
1223 | ||
1224 | //Vvv = -MinorRadius * Cos(V) * (Cos(U)*XDirection + Sin(U)*YDirection) | |
1225 | // -MinorRadius * Sin(V) * Direction | |
1226 | ||
1227 | //Vvvv = - Vv | |
1228 | ||
1229 | //Vu = | |
1230 | // (MajorRadius+MinorRadius*Cos(V)) * | |
1231 | // (-Sin(U)*XDirection + Cos(U)*YDirection) | |
1232 | ||
1233 | //Vuu = | |
1234 | // -(MajorRadius+MinorRadius*Cos(V)) * | |
1235 | // (Cos(U)*XDirection + Sin(U)*YDirection) | |
1236 | ||
1237 | //Vuuu = -Vu | |
1238 | ||
1239 | //Vuv = MinorRadius * Sin(V) * (Sin(U)*XDirection - Cos(U)*YDirection) | |
1240 | ||
1241 | //Vuvv = MinorRadius * Cos(V) * (Sin(U)*XDirection - Cos(U)*YDirection) | |
1242 | ||
1243 | //Vuuv = MinorRadius * Sin(V) * (Cos(U)*XDirection + Sin(U)*YDirection) | |
1244 | ||
1245 | const gp_XYZ& XDir = Pos.XDirection().XYZ(); | |
1246 | const gp_XYZ& YDir = Pos.YDirection().XYZ(); | |
1247 | const gp_XYZ& ZDir = Pos.Direction ().XYZ(); | |
1248 | const gp_XYZ& PLoc = Pos.Location ().XYZ(); | |
1249 | Standard_Real CosU = cos(U); | |
1250 | Standard_Real SinU = sin(U); | |
1251 | Standard_Real R1 = MinorRadius * cos(V); | |
1252 | Standard_Real R2 = MinorRadius * sin(V); | |
1253 | Standard_Real R = MajorRadius + R1; | |
1254 | Standard_Real A1 = R * CosU; | |
1255 | Standard_Real A2 = R * SinU; | |
1256 | Standard_Real A3 = R2 * CosU; | |
1257 | Standard_Real A4 = R2 * SinU; | |
1258 | Standard_Real A5 = R1 * CosU; | |
1259 | Standard_Real A6 = R1 * SinU; | |
1260 | // Modified by skv - Tue Sep 9 15:10:34 2003 OCC620 Begin | |
1261 | Standard_Real eps = 10.*(MinorRadius + MajorRadius)*RealEpsilon(); | |
1262 | ||
1263 | if (Abs(A1) <= eps) | |
1264 | A1 = 0.; | |
1265 | ||
1266 | if (Abs(A2) <= eps) | |
1267 | A2 = 0.; | |
1268 | ||
1269 | if (Abs(A3) <= eps) | |
1270 | A3 = 0.; | |
1271 | ||
1272 | if (Abs(A4) <= eps) | |
1273 | A4 = 0.; | |
1274 | ||
1275 | if (Abs(A5) <= eps) | |
1276 | A5 = 0.; | |
1277 | ||
1278 | if (Abs(A6) <= eps) | |
1279 | A6 = 0.; | |
1280 | // Modified by skv - Tue Sep 9 15:10:35 2003 OCC620 End | |
1281 | Standard_Real Som1X = A1 * XDir.X() + A2 * YDir.X(); | |
1282 | Standard_Real Som1Y = A1 * XDir.Y() + A2 * YDir.Y(); | |
1283 | Standard_Real Som1Z = A1 * XDir.Z() + A2 * YDir.Z(); | |
1284 | Standard_Real Som3X = A3 * XDir.X() + A4 * YDir.X(); | |
1285 | Standard_Real Som3Y = A3 * XDir.Y() + A4 * YDir.Y(); | |
1286 | Standard_Real Som3Z = A3 * XDir.Z() + A4 * YDir.Z(); | |
1287 | Standard_Real Dif1X = A2 * XDir.X() - A1 * YDir.X(); | |
1288 | Standard_Real Dif1Y = A2 * XDir.Y() - A1 * YDir.Y(); | |
1289 | Standard_Real Dif1Z = A2 * XDir.Z() - A1 * YDir.Z(); | |
1290 | Standard_Real R1ZX = R1 * ZDir.X(); | |
1291 | Standard_Real R1ZY = R1 * ZDir.Y(); | |
1292 | Standard_Real R1ZZ = R1 * ZDir.Z(); | |
1293 | Standard_Real R2ZX = R2 * ZDir.X(); | |
1294 | Standard_Real R2ZY = R2 * ZDir.Y(); | |
1295 | Standard_Real R2ZZ = R2 * ZDir.Z(); | |
1296 | P .SetX( Som1X + R2ZX + PLoc.X()); | |
1297 | P .SetY( Som1Y + R2ZY + PLoc.Y()); | |
1298 | P .SetZ( Som1Z + R2ZZ + PLoc.Z()); | |
1299 | Vu .SetX(- Dif1X); | |
1300 | Vu .SetY(- Dif1Y); | |
1301 | Vu .SetZ(- Dif1Z); | |
1302 | Vv .SetX(- Som3X + R1ZX); | |
1303 | Vv .SetY(- Som3Y + R1ZY); | |
1304 | Vv .SetZ(- Som3Z + R1ZZ); | |
1305 | Vuu .SetX(- Som1X); | |
1306 | Vuu .SetY(- Som1Y); | |
1307 | Vuu .SetZ(- Som1Z); | |
1308 | Vvv .SetX(- A5 * XDir.X() - A6 * YDir.X() - R2ZX); | |
1309 | Vvv .SetY(- A5 * XDir.Y() - A6 * YDir.Y() - R2ZY); | |
1310 | Vvv .SetZ(- A5 * XDir.Z() - A6 * YDir.Z() - R2ZZ); | |
1311 | Vuv .SetX( A4 * XDir.X() - A3 * YDir.X()); | |
1312 | Vuv .SetY( A4 * XDir.Y() - A3 * YDir.Y()); | |
1313 | Vuv .SetZ( A4 * XDir.Z() - A3 * YDir.Z()); | |
1314 | Vuuu.SetX( Dif1X); | |
1315 | Vuuu.SetY( Dif1Y); | |
1316 | Vuuu.SetZ( Dif1Z); | |
1317 | Vvvv.SetX( Som3X - R1ZX); | |
1318 | Vvvv.SetY( Som3Y - R1ZY); | |
1319 | Vvvv.SetZ( Som3Z - R1ZZ); | |
1320 | Vuuv.SetX( Som3X); | |
1321 | Vuuv.SetY( Som3Y); | |
1322 | Vuuv.SetZ( Som3Z); | |
1323 | Vuvv.SetX( A6 * XDir.X() - A5 * YDir.X() ); | |
1324 | Vuvv.SetY( A6 * XDir.Y() - A5 * YDir.Y() ); | |
1325 | Vuvv.SetZ( A6 * XDir.Z() - A5 * YDir.Z() ); | |
1326 | } | |
1327 | ||
1328 | //======================================================================= | |
1329 | //function : PlaneParameters | |
1330 | //purpose : | |
1331 | //======================================================================= | |
1332 | ||
1333 | void ElSLib::PlaneParameters (const gp_Ax3& Pos, | |
1334 | const gp_Pnt& P, | |
1335 | Standard_Real& U, | |
1336 | Standard_Real& V) | |
1337 | { | |
1338 | gp_Trsf T; | |
1339 | T.SetTransformation (Pos); | |
1340 | gp_Pnt Ploc = P.Transformed (T); | |
1341 | U = Ploc.X(); | |
1342 | V = Ploc.Y(); | |
1343 | } | |
1344 | ||
1345 | //======================================================================= | |
1346 | //function : CylindreParameters | |
1347 | //purpose : | |
1348 | //======================================================================= | |
1349 | ||
1350 | void ElSLib::CylinderParameters (const gp_Ax3& Pos, | |
1351 | const Standard_Real, | |
1352 | const gp_Pnt& P, | |
1353 | Standard_Real& U, | |
1354 | Standard_Real& V) | |
1355 | { | |
1356 | gp_Trsf T; | |
1357 | T.SetTransformation (Pos); | |
1358 | gp_Pnt Ploc = P.Transformed (T); | |
1359 | U = atan2(Ploc.Y(),Ploc.X()); | |
1360 | if (U < -1.e-16) U += PIPI; | |
1361 | else if (U < 0) U = 0; | |
1362 | V = Ploc.Z(); | |
1363 | } | |
1364 | ||
1365 | //======================================================================= | |
1366 | //function : ConeParameters | |
1367 | //purpose : | |
1368 | //======================================================================= | |
1369 | ||
1370 | void ElSLib::ConeParameters(const gp_Ax3& Pos, | |
1371 | const Standard_Real Radius, | |
1372 | const Standard_Real SAngle, | |
1373 | const gp_Pnt& P, | |
1374 | Standard_Real& U, | |
1375 | Standard_Real& V) | |
1376 | { | |
1377 | gp_Trsf T; | |
1378 | T.SetTransformation (Pos); | |
1379 | gp_Pnt Ploc = P.Transformed (T); | |
1380 | ||
1381 | if(Ploc.X() ==0.0 && Ploc.Y()==0.0 ) { | |
1382 | U = 0.0; | |
1383 | } | |
1384 | else if ( -Radius > Ploc.Z()* Tan(SAngle) ) { | |
0d969553 | 1385 | // the point is at the wrong side of the apex |
7fd59977 | 1386 | U = atan2(-Ploc.Y(), -Ploc.X()); |
1387 | } | |
1388 | else { | |
1389 | U = atan2(Ploc.Y(),Ploc.X()); | |
1390 | } | |
1391 | if (U < -1.e-16) U += PIPI; | |
1392 | else if (U < 0) U = 0; | |
1393 | ||
0d969553 | 1394 | // Evaluate V as follows : |
7fd59977 | 1395 | // P0 = Cone.Value(U,0) |
1396 | // P1 = Cone.Value(U,1) | |
1397 | // V = P0 P1 . P0 Ploc | |
0d969553 | 1398 | // After simplification obtain: |
7fd59977 | 1399 | // V = Sin(Sang) * ( x cosU + y SinU - R) + z * Cos(Sang) |
0d969553 Y |
1400 | // Method that permits to find V of the projected point if the point |
1401 | // is not actually on the cone. | |
7fd59977 | 1402 | |
1403 | V = sin(SAngle) * ( Ploc.X() * cos(U) + Ploc.Y() * sin(U) - Radius) | |
1404 | + cos(SAngle) * Ploc.Z(); | |
1405 | } | |
1406 | ||
1407 | //======================================================================= | |
1408 | //function : SphereParameters | |
1409 | //purpose : | |
1410 | //======================================================================= | |
1411 | ||
1412 | void ElSLib::SphereParameters(const gp_Ax3& Pos, | |
1413 | const Standard_Real, | |
1414 | const gp_Pnt& P, | |
1415 | Standard_Real& U, | |
1416 | Standard_Real& V) | |
1417 | { | |
1418 | gp_Trsf T; | |
1419 | T.SetTransformation (Pos); | |
1420 | gp_Pnt Ploc = P.Transformed (T); | |
1421 | Standard_Real x, y, z; | |
1422 | Ploc.Coord (x, y, z); | |
1423 | Standard_Real l = sqrt (x * x + y * y); | |
0d969553 | 1424 | if (l < gp::Resolution()) { // point on axis Z of the sphere |
7fd59977 | 1425 | if (z > 0.) |
1426 | V = M_PI_2; // PI * 0.5 | |
1427 | else | |
1428 | V = - M_PI_2; // PI * 0.5 | |
1429 | U = 0.; | |
1430 | } | |
1431 | else { | |
1432 | V = atan(z/l); | |
1433 | U = atan2(y,x); | |
1434 | if (U < -1.e-16) U += PIPI; | |
1435 | else if (U < 0) U = 0; | |
1436 | } | |
1437 | } | |
1438 | ||
1439 | //======================================================================= | |
1440 | //function : TorusParameters | |
1441 | //purpose : | |
1442 | //======================================================================= | |
1443 | ||
1444 | void ElSLib::TorusParameters(const gp_Ax3& Pos, | |
1445 | const Standard_Real MajorRadius, | |
1446 | const Standard_Real MinorRadius, | |
1447 | const gp_Pnt& P, | |
1448 | Standard_Real& U, | |
1449 | Standard_Real& V) | |
1450 | { | |
1451 | gp_Trsf Tref; | |
1452 | Tref.SetTransformation (Pos); | |
1453 | gp_Pnt Ploc = P.Transformed (Tref); | |
1454 | Standard_Real x, y, z; | |
1455 | Ploc.Coord (x, y, z); | |
1456 | ||
0d969553 | 1457 | // all that to process case of Major < Minor. |
7fd59977 | 1458 | U = atan2(y,x); |
1459 | if (MajorRadius < MinorRadius){ | |
1460 | Standard_Real cosu = cos(U); | |
1461 | Standard_Real sinu = sin(U); | |
1462 | Standard_Real z2 = z * z; | |
1463 | Standard_Real MinR2 = MinorRadius * MinorRadius; | |
1464 | Standard_Real RCosU = MajorRadius * cosu; | |
1465 | Standard_Real RSinU = MajorRadius * sinu; | |
1466 | Standard_Real xm = x - RCosU; | |
1467 | Standard_Real ym = y - RSinU; | |
1468 | Standard_Real xp = x + RCosU; | |
1469 | Standard_Real yp = y + RSinU; | |
1470 | Standard_Real D1 = xm * xm + ym * ym + z2 - MinR2; | |
1471 | Standard_Real D2 = xp * xp + yp * yp + z2 - MinR2; | |
1472 | Standard_Real AD1 = D1; | |
1473 | if (AD1 < 0) AD1 = - AD1; | |
1474 | Standard_Real AD2 = D2; | |
1475 | if (AD2 < 0) AD2 = - AD2; | |
c6541a0c | 1476 | if (AD2 < AD1) U += M_PI; |
7fd59977 | 1477 | } |
1478 | if (U < -1.e-16) U += PIPI; | |
1479 | else if (U < 0) U = 0; | |
1480 | Standard_Real cosu = cos(U); | |
1481 | Standard_Real sinu = sin(U); | |
1482 | gp_Dir dx(cosu,sinu,0.); | |
e8e8b273 | 1483 | gp_XYZ dPV(x - MajorRadius * cosu, |
1484 | y - MajorRadius * sinu, | |
1485 | z); | |
1486 | Standard_Real aMag = dPV.Modulus(); | |
1487 | if (aMag <= gp::Resolution()) | |
1488 | { | |
1489 | V = 0.; | |
1490 | } | |
1491 | else | |
1492 | { | |
1493 | gp_Dir dP(dPV); | |
1494 | V = dx.AngleWithRef(dP, dx^gp::DZ()); | |
1495 | } | |
7fd59977 | 1496 | if (V < -1.e-16) V += PIPI; |
1497 | else if (V < 0) V = 0; | |
1498 | } | |
1499 | ||
1500 | //======================================================================= | |
1501 | //function : PlaneUIso | |
1502 | //purpose : | |
1503 | //======================================================================= | |
1504 | ||
1505 | gp_Lin ElSLib::PlaneUIso(const gp_Ax3& Pos, | |
1506 | const Standard_Real U) | |
1507 | { | |
1508 | gp_Lin L(Pos.Location(),Pos.YDirection()); | |
1509 | gp_Vec Ve(Pos.XDirection()); | |
1510 | Ve *= U; | |
1511 | L.Translate(Ve); | |
1512 | return L; | |
1513 | } | |
1514 | ||
1515 | //======================================================================= | |
1516 | //function : CylinderUIso | |
1517 | //purpose : | |
1518 | //======================================================================= | |
1519 | ||
1520 | gp_Lin ElSLib::CylinderUIso(const gp_Ax3& Pos, | |
1521 | const Standard_Real Radius, | |
1522 | const Standard_Real U) | |
1523 | { | |
1524 | gp_Pnt P; | |
1525 | gp_Vec DU,DV; | |
1526 | CylinderD1(U,0.,Pos,Radius,P,DU,DV); | |
1527 | gp_Lin L(P,DV); | |
1528 | return L; | |
1529 | } | |
1530 | ||
1531 | //======================================================================= | |
1532 | //function : ConeUIso | |
1533 | //purpose : | |
1534 | //======================================================================= | |
1535 | ||
1536 | gp_Lin ElSLib::ConeUIso(const gp_Ax3& Pos, | |
1537 | const Standard_Real Radius, | |
1538 | const Standard_Real SAngle, | |
1539 | const Standard_Real U) | |
1540 | { | |
1541 | gp_Pnt P; | |
1542 | gp_Vec DU,DV; | |
1543 | ConeD1(U,0,Pos,Radius,SAngle,P,DU,DV); | |
1544 | gp_Lin L(P,DV); | |
1545 | return L; | |
1546 | } | |
1547 | ||
1548 | //======================================================================= | |
1549 | //function : SphereUIso | |
1550 | //purpose : | |
1551 | //======================================================================= | |
1552 | ||
1553 | gp_Circ ElSLib::SphereUIso(const gp_Ax3& Pos, | |
1554 | const Standard_Real Radius, | |
1555 | const Standard_Real U) | |
1556 | { | |
1557 | gp_Vec dx = Pos.XDirection(); | |
1558 | gp_Vec dy = Pos.YDirection(); | |
1559 | gp_Dir dz = Pos.Direction (); | |
1560 | gp_Dir cx = cos(U) * dx + sin(U) * dy; | |
1561 | gp_Ax2 axes(Pos.Location(), | |
1562 | cx.Crossed(dz), | |
1563 | cx); | |
1564 | gp_Circ Circ(axes,Radius); | |
1565 | return Circ; | |
1566 | } | |
1567 | ||
1568 | //======================================================================= | |
1569 | //function : TorusUIso | |
1570 | //purpose : | |
1571 | //======================================================================= | |
1572 | ||
1573 | gp_Circ ElSLib::TorusUIso(const gp_Ax3& Pos, | |
1574 | const Standard_Real MajorRadius, | |
1575 | const Standard_Real MinorRadius, | |
1576 | const Standard_Real U) | |
1577 | { | |
1578 | gp_Vec dx = Pos.XDirection(); | |
1579 | gp_Vec dy = Pos.YDirection(); | |
1580 | gp_Dir dz = Pos.Direction (); | |
1581 | gp_Dir cx = cos(U) * dx + sin(U) * dy; | |
1582 | gp_Ax2 axes(Pos.Location(), | |
1583 | cx.Crossed(dz), | |
1584 | cx); | |
1585 | gp_Vec Ve = cx; | |
1586 | Ve *= MajorRadius; | |
1587 | axes.Translate(Ve); | |
1588 | gp_Circ Circ(axes,MinorRadius); | |
1589 | return Circ; | |
1590 | } | |
1591 | ||
1592 | //======================================================================= | |
1593 | //function : PlaneVIso | |
1594 | //purpose : | |
1595 | //======================================================================= | |
1596 | ||
1597 | gp_Lin ElSLib::PlaneVIso(const gp_Ax3& Pos, | |
1598 | const Standard_Real V) | |
1599 | { | |
1600 | gp_Lin L(Pos.Location(),Pos.XDirection()); | |
1601 | gp_Vec Ve(Pos.YDirection()); | |
1602 | Ve *= V; | |
1603 | L.Translate(Ve); | |
1604 | return L; | |
1605 | } | |
1606 | ||
1607 | //======================================================================= | |
1608 | //function : CylinderVIso | |
1609 | //purpose : | |
1610 | //======================================================================= | |
1611 | ||
1612 | gp_Circ ElSLib::CylinderVIso(const gp_Ax3& Pos, | |
1613 | const Standard_Real Radius, | |
1614 | const Standard_Real V) | |
1615 | { | |
1616 | gp_Ax2 axes = Pos.Ax2(); | |
1617 | gp_Vec Ve(Pos.Direction()); | |
1618 | Ve.Multiply(V); | |
1619 | axes.Translate(Ve); | |
1620 | gp_Circ C(axes,Radius); | |
1621 | return C; | |
1622 | } | |
1623 | ||
1624 | //======================================================================= | |
1625 | //function : ConeVIso | |
1626 | //purpose : | |
1627 | //======================================================================= | |
1628 | ||
1629 | gp_Circ ElSLib::ConeVIso(const gp_Ax3& Pos, | |
1630 | const Standard_Real Radius, | |
1631 | const Standard_Real SAngle, | |
1632 | const Standard_Real V) | |
1633 | { | |
1634 | gp_Ax3 axes(Pos); | |
1635 | gp_Vec Ve(Pos.Direction()); | |
1636 | Ve.Multiply(V * cos(SAngle)); | |
1637 | axes.Translate(Ve); | |
1638 | Standard_Real R = Radius + V * sin(SAngle); | |
1639 | if (R < 0) { | |
1640 | axes.XReverse(); | |
1641 | axes.YReverse(); | |
1642 | R = - R; | |
1643 | } | |
1644 | gp_Circ C(axes.Ax2(),R); | |
1645 | return C; | |
1646 | } | |
1647 | ||
1648 | //======================================================================= | |
1649 | //function : SphereVIso | |
1650 | //purpose : | |
1651 | //======================================================================= | |
1652 | ||
1653 | gp_Circ ElSLib::SphereVIso(const gp_Ax3& Pos, | |
1654 | const Standard_Real Radius, | |
1655 | const Standard_Real V) | |
1656 | { | |
1657 | gp_Ax2 axes = Pos.Ax2(); | |
1658 | gp_Vec Ve(Pos.Direction()); | |
1659 | Ve.Multiply(Radius * sin(V)); | |
1660 | axes.Translate(Ve); | |
1661 | Standard_Real radius = Radius * cos(V); | |
5554ac68 | 1662 | // #23170: if V is even slightly (e.g. by double epsilon) greater than PI/2, |
1663 | // radius will become negative and constructor of gp_Circ will raise exception. | |
1664 | // Lets try to create correct isoline even on analytical continuation for |V| > PI/2... | |
1665 | if (radius < 0.) | |
1666 | { | |
1667 | axes.SetDirection (-axes.Direction()); | |
1668 | radius = -radius; | |
1669 | } | |
7fd59977 | 1670 | gp_Circ Circ(axes,radius); |
1671 | return Circ; | |
1672 | } | |
1673 | ||
1674 | //======================================================================= | |
1675 | //function : TorusVIso | |
1676 | //purpose : | |
1677 | //======================================================================= | |
1678 | ||
1679 | gp_Circ ElSLib::TorusVIso(const gp_Ax3& Pos, | |
1680 | const Standard_Real MajorRadius, | |
1681 | const Standard_Real MinorRadius, | |
1682 | const Standard_Real V) | |
1683 | { | |
1684 | gp_Ax3 axes = Pos.Ax2(); | |
1685 | gp_Vec Ve(Pos.Direction()); | |
1686 | Ve.Multiply(MinorRadius * sin(V)); | |
1687 | axes.Translate(Ve); | |
1688 | Standard_Real R = MajorRadius + MinorRadius * cos(V); | |
1689 | if (R < 0) { | |
1690 | axes.XReverse(); | |
1691 | axes.YReverse(); | |
1692 | R = - R; | |
1693 | } | |
1694 | gp_Circ Circ(axes.Ax2(),R); | |
1695 | return Circ; | |
1696 | } | |
1697 |