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