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