src/ElSLib/ElSLib.cxx

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