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