src/gp/gp_Quaternion.cxx

   1 // Created on: 2010-05-11
   2 // Created by: Kirill GAVRILOV
   3 // Copyright (c) 2010-2014 OPEN CASCADE SAS
   4 //
   5 // This file is part of Open CASCADE Technology software library.
   6 //
   7 // This library is free software; you can redistribute it and/or modify it under
   8 // the terms of the GNU Lesser General Public License version 2.1 as published
   9 // by the Free Software Foundation, with special exception defined in the file
  10 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
  11 // distribution for complete text of the license and disclaimer of any warranty.
  12 //
  13 // Alternatively, this file may be used under the terms of Open CASCADE
  14 // commercial license or contractual agreement.
  15
  16 // Note: implementation is based on free samples from
  17 // http://www.gamedev.ru/code/articles/?id=4215&page=3
  18 // and maths found in Wikipedia and elsewhere
  19
  20 #include <gp_Quaternion.hxx>
  21 #include <gp_Vec.hxx>
  22 #include <gp_Mat.hxx>
  23
  24 //=======================================================================
  25 //function : IsEqual
  26 //purpose  :
  27 //=======================================================================
  28
  29 Standard_Boolean gp_Quaternion::IsEqual (const gp_Quaternion& theOther) const
  30 {
  31   if (this == &theOther)
  32     return Standard_True;
  33   return Abs (x - theOther.x) <= gp::Resolution() &&
  34          Abs (y - theOther.y) <= gp::Resolution() &&
  35          Abs (z - theOther.z) <= gp::Resolution() &&
  36          Abs (w - theOther.w) <= gp::Resolution();
  37 }
  38
  39 //=======================================================================
  40 //function : SetRotation
  41 //purpose  :
  42 //=======================================================================
  43
  44 void gp_Quaternion::SetRotation (const gp_Vec& theVecFrom, const gp_Vec& theVecTo)
  45 {
  46   gp_Vec aVecCross (theVecFrom.Crossed (theVecTo));
  47   Set (aVecCross.X(), aVecCross.Y(), aVecCross.Z(), theVecFrom.Dot (theVecTo));
  48   Normalize();    // if "from" or "to" not unit, normalize quat
  49   w += 1.0;       // reducing angle to halfangle
  50   if (w <= gp::Resolution())  // angle close to PI
  51   {
  52     if ((theVecFrom.Z() * theVecFrom.Z()) > (theVecFrom.X() * theVecFrom.X()))
  53       Set (           0.0,  theVecFrom.Z(), -theVecFrom.Y(), w); // theVecFrom * gp_Vec(1,0,0)
  54     else
  55       Set (theVecFrom.Y(), -theVecFrom.X(),             0.0, w); // theVecFrom * gp_Vec(0,0,1)
  56   }
  57   Normalize();
  58 }
  59
  60 //=======================================================================
  61 //function : SetRotation
  62 //purpose  :
  63 //=======================================================================
  64
  65 void gp_Quaternion::SetRotation (const gp_Vec& theVecFrom, const gp_Vec& theVecTo, const gp_Vec& theHelpCrossVec)
  66 {
  67   gp_Vec aVecCross (theVecFrom.Crossed (theVecTo));
  68   Set (aVecCross.X(), aVecCross.Y(), aVecCross.Z(), theVecFrom.Dot (theVecTo));
  69   Normalize();    // if "from" or "to" not unit, normalize quat
  70   w += 1.0;       // reducing angle to halfangle
  71   if (w <= gp::Resolution())  // angle close to PI
  72   {
  73     gp_Vec theAxis = theVecFrom.Crossed (theHelpCrossVec);
  74     Set (theAxis.X(), theAxis.Y(), theAxis.Z(), w);
  75   }
  76   Normalize();
  77 }
  78
  79 //=======================================================================
  80 //function : SetVectorAndAngle
  81 //purpose  :
  82 //=======================================================================
  83
  84 void gp_Quaternion::SetVectorAndAngle (const gp_Vec& theAxis, const Standard_Real theAngle)
  85 {
  86   gp_Vec anAxis = theAxis.Normalized();
  87   Standard_Real anAngleHalf = 0.5 * theAngle;
  88   Standard_Real sin_a = Sin (anAngleHalf);
  89   Set (anAxis.X() * sin_a, anAxis.Y() * sin_a, anAxis.Z() * sin_a, Cos (anAngleHalf));
  90 }
  91
  92 //=======================================================================
  93 //function : GetVectorAndAngle
  94 //purpose  :
  95 //=======================================================================
  96
  97 void gp_Quaternion::GetVectorAndAngle (gp_Vec& theAxis, Standard_Real& theAngle) const
  98 {
  99   Standard_Real vl = Sqrt (x * x + y * y + z * z);
 100   if (vl > gp::Resolution())
 101   {
 102     Standard_Real ivl = 1.0 / vl;
 103     theAxis.SetCoord (x * ivl, y * ivl, z * ivl);
 104     if (w < 0.0)
 105     {
 106       theAngle = 2.0 * ATan2 (-vl, -w); // [-PI,  0]
 107     }
 108     else
 109     {
 110       theAngle = 2.0 * ATan2 ( vl,  w); // [  0, PI]
 111     }
 112   }
 113   else
 114   {
 115     theAxis.SetCoord (0.0, 0.0, 1.0);
 116     theAngle = 0.0;
 117   }
 118 }
 119
 120 //=======================================================================
 121 //function : SetMatrix
 122 //purpose  :
 123 //=======================================================================
 124
 125 void gp_Quaternion::SetMatrix (const gp_Mat& theMat)
 126 {
 127   Standard_Real tr = theMat (1, 1) + theMat (2, 2) + theMat(3, 3); // trace of martix
 128   if (tr > 0.0)
 129   { // if trace positive than "w" is biggest component
 130     Set (theMat (3, 2) - theMat (2, 3),
 131          theMat (1, 3) - theMat (3, 1),
 132          theMat (2, 1) - theMat (1, 2),
 133          tr + 1.0);
 134     Scale (0.5 / Sqrt (w)); // "w" contain the "norm * 4"
 135   }
 136   else if ((theMat (1, 1) > theMat (2, 2)) && (theMat (1, 1) > theMat (3, 3)))
 137   { // Some of vector components is bigger
 138     Set (1.0 + theMat (1, 1) - theMat (2, 2) - theMat (3, 3),
 139          theMat (1, 2) + theMat (2, 1),
 140          theMat (1, 3) + theMat (3, 1),
 141          theMat (3, 2) - theMat (2, 3));
 142     Scale (0.5 / Sqrt (x));
 143   }
 144   else if (theMat (2, 2) > theMat (3, 3))
 145   {
 146     Set (theMat (1, 2) + theMat (2, 1),
 147          1.0 + theMat (2, 2) - theMat (1, 1) - theMat (3, 3),
 148          theMat (2, 3) + theMat (3, 2),
 149          theMat (1, 3) - theMat (3, 1));
 150     Scale (0.5 / Sqrt (y));
 151   }
 152   else
 153   {
 154     Set (theMat (1, 3) + theMat (3, 1),
 155          theMat (2, 3) + theMat (3, 2),
 156          1.0 + theMat (3, 3) - theMat (1, 1) - theMat (2, 2),
 157          theMat (2, 1) - theMat (1, 2));
 158     Scale (0.5 / Sqrt (z));
 159   }
 160 }
 161
 162 //=======================================================================
 163 //function : GetMatrix
 164 //purpose  :
 165 //=======================================================================
 166
 167 gp_Mat gp_Quaternion::GetMatrix () const
 168 {
 169   Standard_Real wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
 170   Standard_Real s  = 2.0 / SquareNorm();
 171   x2 = x * s;    y2 = y * s;    z2 = z * s;
 172   xx = x * x2;   xy = x * y2;   xz = x * z2;
 173   yy = y * y2;   yz = y * z2;   zz = z * z2;
 174   wx = w * x2;   wy = w * y2;   wz = w * z2;
 175
 176   gp_Mat aMat;
 177
 178   aMat (1, 1) = 1.0 - (yy + zz);
 179   aMat (1, 2) = xy - wz;
 180   aMat (1, 3) = xz + wy;
 181
 182   aMat (2, 1) = xy + wz;
 183   aMat (2, 2) = 1.0 - (xx + zz);
 184   aMat (2, 3) = yz - wx;
 185
 186   aMat (3, 1) = xz - wy;
 187   aMat (3, 2) = yz + wx;
 188   aMat (3, 3) = 1.0 - (xx + yy);
 189   // 1 division    16 multiplications    15 addidtions    12 variables
 190
 191   return aMat;
 192 }
 193
 194 namespace { // anonymous namespace
 195
 196 //=======================================================================
 197 //function : translateEulerSequence
 198 //purpose  :
 199 // Code supporting conversion between quaternion and generalized
 200 // Euler angles (sequence of three rotations) is based on
 201 // algorithm by Ken Shoemake, published in Graphics Gems IV, p. 222-22
 202 // http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
 203 //=======================================================================
 204
 205 struct gp_EulerSequence_Parameters
 206 {
 207   Standard_Integer i;           // first rotation axis
 208   Standard_Integer j;           // next axis of rotation
 209   Standard_Integer k;           // third axis
 210   Standard_Boolean isOdd;       // true if order of two first rotation axes is odd permutation, e.g. XZ
 211   Standard_Boolean isTwoAxes;   // true if third rotation is about the same axis as first
 212   Standard_Boolean isExtrinsic; // true if rotations are made around fixed axes
 213
 214   gp_EulerSequence_Parameters (Standard_Integer theAx1,
 215                                Standard_Boolean theisOdd,
 216                                Standard_Boolean theisTwoAxes,
 217                                Standard_Boolean theisExtrinsic)
 218     : i(theAx1),
 219       j(1 + (theAx1 + (theisOdd ? 1 : 0)) % 3),
 220       k(1 + (theAx1 + (theisOdd ? 0 : 1)) % 3),
 221       isOdd(theisOdd),
 222       isTwoAxes(theisTwoAxes),
 223       isExtrinsic(theisExtrinsic)
 224   {}
 225 };
 226
 227 gp_EulerSequence_Parameters translateEulerSequence (const gp_EulerSequence theSeq)
 228 {
 229   typedef gp_EulerSequence_Parameters Params;
 230   const Standard_Boolean F = Standard_False;
 231   const Standard_Boolean T = Standard_True;
 232
 233   switch (theSeq)
 234   {
 235   case gp_Extrinsic_XYZ: return Params (1, F, F, T);
 236   case gp_Extrinsic_XZY: return Params (1, T, F, T);
 237   case gp_Extrinsic_YZX: return Params (2, F, F, T);
 238   case gp_Extrinsic_YXZ: return Params (2, T, F, T);
 239   case gp_Extrinsic_ZXY: return Params (3, F, F, T);
 240   case gp_Extrinsic_ZYX: return Params (3, T, F, T);
 241
 242   // Conversion of intrinsic angles is made by the same code as for extrinsic,
 243   // using equivalence rule: intrinsic rotation is equivalent to extrinsic
 244   // rotation by the same angles but with inverted order of elemental rotations.
 245   // Swapping of angles (Alpha <-> Gamma) is done inside conversion procedure;
 246   // sequence of axes is inverted by setting appropriate parameters here.
 247   // Note that proper Euler angles (last block below) are symmetric for sequence of axes.
 248   case gp_Intrinsic_XYZ: return Params (3, T, F, F);
 249   case gp_Intrinsic_XZY: return Params (2, F, F, F);
 250   case gp_Intrinsic_YZX: return Params (1, T, F, F);
 251   case gp_Intrinsic_YXZ: return Params (3, F, F, F);
 252   case gp_Intrinsic_ZXY: return Params (2, T, F, F);
 253   case gp_Intrinsic_ZYX: return Params (1, F, F, F);
 254
 255   case gp_Extrinsic_XYX: return Params (1, F, T, T);
 256   case gp_Extrinsic_XZX: return Params (1, T, T, T);
 257   case gp_Extrinsic_YZY: return Params (2, F, T, T);
 258   case gp_Extrinsic_YXY: return Params (2, T, T, T);
 259   case gp_Extrinsic_ZXZ: return Params (3, F, T, T);
 260   case gp_Extrinsic_ZYZ: return Params (3, T, T, T);
 261
 262   case gp_Intrinsic_XYX: return Params (1, F, T, F);
 263   case gp_Intrinsic_XZX: return Params (1, T, T, F);
 264   case gp_Intrinsic_YZY: return Params (2, F, T, F);
 265   case gp_Intrinsic_YXY: return Params (2, T, T, F);
 266   case gp_Intrinsic_ZXZ: return Params (3, F, T, F);
 267   case gp_Intrinsic_ZYZ: return Params (3, T, T, F);
 268
 269   default:
 270   case gp_EulerAngles : return Params (3, F, T, F); // = Intrinsic_ZXZ
 271   case gp_YawPitchRoll: return Params (1, F, F, F); // = Intrinsic_ZYX
 272   };
 273 }
 274
 275 } // anonymous namespace
 276
 277 //=======================================================================
 278 //function : SetEulerAngles
 279 //purpose  :
 280 //=======================================================================
 281
 282 void gp_Quaternion::SetEulerAngles (const gp_EulerSequence theOrder,
 283                                     const Standard_Real theAlpha,
 284                                     const Standard_Real theBeta,
 285                                     const Standard_Real theGamma)
 286 {
 287   gp_EulerSequence_Parameters o = translateEulerSequence (theOrder);
 288
 289   Standard_Real a = theAlpha, b = theBeta, c = theGamma;
 290   if ( ! o.isExtrinsic )
 291   {
 292     a = theGamma;
 293     c = theAlpha;
 294   }
 295   if ( o.isOdd )
 296     b = -b;
 297
 298   Standard_Real ti = 0.5 * a;
 299   Standard_Real tj = 0.5 * b;
 300   Standard_Real th = 0.5 * c;
 301   Standard_Real ci = Cos (ti);
 302   Standard_Real cj = Cos (tj);
 303   Standard_Real ch = Cos (th);
 304   Standard_Real si = Sin (ti);
 305   Standard_Real sj = Sin (tj);
 306   Standard_Real sh = Sin (th);
 307   Standard_Real cc = ci * ch;
 308   Standard_Real cs = ci * sh;
 309   Standard_Real sc = si * ch;
 310   Standard_Real ss = si * sh;
 311
 312   Standard_Real values[4]; // w, x, y, z
 313   if ( o.isTwoAxes )
 314   {
 315     values[o.i] = cj * (cs + sc);
 316     values[o.j] = sj * (cc + ss);
 317     values[o.k] = sj * (cs - sc);
 318     values[0]   = cj * (cc - ss);
 319   }
 320   else
 321   {
 322     values[o.i] = cj * sc - sj * cs;
 323     values[o.j] = cj * ss + sj * cc;
 324     values[o.k] = cj * cs - sj * sc;
 325     values[0]   = cj * cc + sj * ss;
 326   }
 327   if ( o.isOdd )
 328     values[o.j] = -values[o.j];
 329
 330   x = values[1];
 331   y = values[2];
 332   z = values[3];
 333   w = values[0];
 334 }
 335
 336 //=======================================================================
 337 //function : GetEulerAngles
 338 //purpose  :
 339 //=======================================================================
 340
 341 void gp_Quaternion::GetEulerAngles (const gp_EulerSequence theOrder,
 342                                     Standard_Real& theAlpha,
 343                                     Standard_Real& theBeta,
 344                                     Standard_Real& theGamma) const
 345 {
 346   gp_Mat M = GetMatrix();
 347
 348   gp_EulerSequence_Parameters o = translateEulerSequence (theOrder);
 349   if ( o.isTwoAxes )
 350   {
 351     double sy = sqrt (M(o.i, o.j) * M(o.i, o.j) + M(o.i, o.k) * M(o.i, o.k));
 352     if (sy > 16 * DBL_EPSILON)
 353     {
 354       theAlpha = ATan2 (M(o.i, o.j),  M(o.i, o.k));
 355       theGamma = ATan2 (M(o.j, o.i), -M(o.k, o.i));
 356     }
 357     else
 358     {
 359       theAlpha = ATan2 (-M(o.j, o.k), M(o.j, o.j));
 360       theGamma = 0.;
 361     }
 362     theBeta = ATan2 (sy, M(o.i, o.i));
 363   }
 364   else
 365   {
 366     double cy = sqrt (M(o.i, o.i) * M(o.i, o.i) + M(o.j, o.i) * M(o.j, o.i));
 367     if (cy > 16 * DBL_EPSILON)
 368     {
 369       theAlpha = ATan2 (M(o.k, o.j), M(o.k, o.k));
 370       theGamma = ATan2 (M(o.j, o.i), M(o.i, o.i));
 371     }
 372     else
 373     {
 374       theAlpha = ATan2 (-M(o.j, o.k), M(o.j, o.j));
 375       theGamma = 0.;
 376     }
 377     theBeta = ATan2 (-M(o.k, o.i), cy);
 378   }
 379   if ( o.isOdd )
 380   {
 381     theAlpha = -theAlpha;
 382     theBeta  = -theBeta;
 383     theGamma = -theGamma;
 384   }
 385   if ( ! o.isExtrinsic )
 386   {
 387     Standard_Real aFirst = theAlpha;
 388     theAlpha = theGamma;
 389     theGamma = aFirst;
 390   }
 391 }
 392
 393 //=======================================================================
 394 //function : StabilizeLength
 395 //purpose  :
 396 //=======================================================================
 397
 398 void gp_Quaternion::StabilizeLength()
 399 {
 400   Standard_Real cs = Abs (x) + Abs (y) + Abs (z) + Abs (w);
 401   if (cs > 0.0)
 402   {
 403     x /= cs; y /= cs; z /= cs; w /= cs;
 404   }
 405   else
 406   {
 407     SetIdent();
 408   }
 409 }
 410
 411 //=======================================================================
 412 //function : Normalize
 413 //purpose  :
 414 //=======================================================================
 415
 416 void gp_Quaternion::Normalize()
 417 {
 418   Standard_Real aMagn = Norm();
 419   if (aMagn < gp::Resolution())
 420   {
 421     StabilizeLength();
 422     aMagn = Norm();
 423   }
 424   Scale (1.0 / aMagn);
 425 }
 426
 427 //=======================================================================
 428 //function : Normalize
 429 //purpose  :
 430 //=======================================================================
 431
 432 Standard_Real gp_Quaternion::GetRotationAngle() const
 433 {
 434   if (w < 0.0)
 435   {
 436     return 2.0 * ATan2 (-Sqrt (x * x + y * y + z * z), -w);
 437   }
 438   else
 439   {
 440     return 2.0 * ATan2 ( Sqrt (x * x + y * y + z * z),  w);
 441   }
 442 }
 443
 444 //=======================================================================
 445 //function : Multiply
 446 //purpose  :
 447 //=======================================================================
 448
 449 gp_Vec gp_Quaternion::Multiply (const gp_Vec& theVec) const
 450 {
 451   gp_Quaternion theQ (theVec.X() * w + theVec.Z() * y - theVec.Y() * z,
 452                       theVec.Y() * w + theVec.X() * z - theVec.Z() * x,
 453                       theVec.Z() * w + theVec.Y() * x - theVec.X() * y,
 454                       theVec.X() * x + theVec.Y() * y + theVec.Z() * z);
 455   return gp_Vec (w * theQ.x + x * theQ.w + y * theQ.z - z * theQ.y,
 456                  w * theQ.y + y * theQ.w + z * theQ.x - x * theQ.z,
 457                  w * theQ.z + z * theQ.w + x * theQ.y - y * theQ.x) * (1.0 / SquareNorm());
 458 }