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