1 // Created on: 1991-09-09
2 // Created by: Michel Chauvat
3 // Copyright (c) 1991-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
5 //
6 // This file is part of Open CASCADE Technology software library.
7 //
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
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.
13 //
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
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
24
25 #include <ElSLib.hxx>
26 #include <gp.hxx>
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>
36 #include <gp_Trsf.hxx>
37 #include <gp_Vec.hxx>
38 #include <gp_XYZ.hxx>
39
40 static Standard_Real PIPI = M_PI + M_PI;
41
PlaneValue(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos)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
ConeValue(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle)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
CylinderValue(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius)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
SphereValue(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius)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
TorusValue(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius)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
PlaneDN(const Standard_Real,const Standard_Real,const gp_Ax3 & Pos,const Standard_Integer Nu,const Standard_Integer Nv)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
ConeDN(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle,const Standard_Integer Nu,const Standard_Integer Nv)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();
173 Standard_Real Um = U + Nu * M_PI_2; // M_PI * 0.5
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
CylinderDN(const Standard_Real U,const Standard_Real,const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Integer Nu,const Standard_Integer Nv)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
SphereDN(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Integer Nu,const Standard_Integer Nv)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
TorusDN(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius,const Standard_Integer Nu,const Standard_Integer Nv)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
PlaneD0(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,gp_Pnt & P)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
ConeD0(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle,gp_Pnt & P)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
CylinderD0(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,gp_Pnt & P)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
SphereD0(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,gp_Pnt & P)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
TorusD0(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius,gp_Pnt & P)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
PlaneD1(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv)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
ConeD1(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv)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
CylinderD1(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv)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
SphereD1(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv)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
TorusD1(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv)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
ConeD2(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv,gp_Vec & Vuu,gp_Vec & Vvv,gp_Vec & Vuv)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
CylinderD2(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv,gp_Vec & Vuu,gp_Vec & Vvv,gp_Vec & Vuv)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
SphereD2(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv,gp_Vec & Vuu,gp_Vec & Vvv,gp_Vec & Vuv)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
TorusD2(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv,gp_Vec & Vuu,gp_Vec & Vvv,gp_Vec & Vuv)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
ConeD3(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv,gp_Vec & Vuu,gp_Vec & Vvv,gp_Vec & Vuv,gp_Vec & Vuuu,gp_Vec & Vvvv,gp_Vec & Vuuv,gp_Vec & Vuvv)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
CylinderD3(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv,gp_Vec & Vuu,gp_Vec & Vvv,gp_Vec & Vuv,gp_Vec & Vuuu,gp_Vec & Vvvv,gp_Vec & Vuuv,gp_Vec & Vuvv)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
SphereD3(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real Radius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv,gp_Vec & Vuu,gp_Vec & Vvv,gp_Vec & Vuv,gp_Vec & Vuuu,gp_Vec & Vvvv,gp_Vec & Vuuv,gp_Vec & Vuvv)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
TorusD3(const Standard_Real U,const Standard_Real V,const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius,gp_Pnt & P,gp_Vec & Vu,gp_Vec & Vv,gp_Vec & Vuu,gp_Vec & Vvv,gp_Vec & Vuv,gp_Vec & Vuuu,gp_Vec & Vvvv,gp_Vec & Vuuv,gp_Vec & Vuvv)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
PlaneParameters(const gp_Ax3 & Pos,const gp_Pnt & P,Standard_Real & U,Standard_Real & V)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
CylinderParameters(const gp_Ax3 & Pos,const Standard_Real,const gp_Pnt & P,Standard_Real & U,Standard_Real & V)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
ConeParameters(const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle,const gp_Pnt & P,Standard_Real & U,Standard_Real & V)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) ) {
1385 // the point is at the wrong side of the apex
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
1394 // Evaluate V as follows :
1395 // P0 = Cone.Value(U,0)
1396 // P1 = Cone.Value(U,1)
1397 // V = P0 P1 . P0 Ploc
1398 // After simplification obtain:
1399 // V = Sin(Sang) * ( x cosU + y SinU - R) + z * Cos(Sang)
1400 // Method that permits to find V of the projected point if the point
1401 // is not actually on the cone.
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
SphereParameters(const gp_Ax3 & Pos,const Standard_Real,const gp_Pnt & P,Standard_Real & U,Standard_Real & V)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);
1424 if (l < gp::Resolution()) { // point on axis Z of the sphere
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
TorusParameters(const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius,const gp_Pnt & P,Standard_Real & U,Standard_Real & V)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
1457 // all that to process case of Major < Minor.
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;
1476 if (AD2 < AD1) U += M_PI;
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.);
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 }
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
PlaneUIso(const gp_Ax3 & Pos,const Standard_Real U)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
CylinderUIso(const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real U)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
ConeUIso(const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle,const Standard_Real U)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
SphereUIso(const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real U)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
TorusUIso(const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius,const Standard_Real U)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
PlaneVIso(const gp_Ax3 & Pos,const Standard_Real V)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
CylinderVIso(const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real V)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
ConeVIso(const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real SAngle,const Standard_Real V)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
SphereVIso(const gp_Ax3 & Pos,const Standard_Real Radius,const Standard_Real V)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);
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 }
1670 gp_Circ Circ(axes,radius);
1671 return Circ;
1672 }
1673
1674 //=======================================================================
1675 //function : TorusVIso
1676 //purpose :
1677 //=======================================================================
1678
TorusVIso(const gp_Ax3 & Pos,const Standard_Real MajorRadius,const Standard_Real MinorRadius,const Standard_Real V)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
1698