src/gp/gp_Quaternion.lxx

   1 // Copyright (c) 1999-2014 OPEN CASCADE SAS
   2 //
   3 // This file is part of Open CASCADE Technology software library.
   4 //
   5 // This library is free software; you can redistribute it and/or modify it under
   6 // the terms of the GNU Lesser General Public License version 2.1 as published
   7 // by the Free Software Foundation, with special exception defined in the file
   8 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
   9 // distribution for complete text of the license and disclaimer of any warranty.
  10 //
  11 // Alternatively, this file may be used under the terms of Open CASCADE
  12 // commercial license or contractual agreement.
  13
  14 //=======================================================================
  15 //function : gp_Quaternion
  16 //purpose  :
  17 //=======================================================================
  18
  19 inline gp_Quaternion::gp_Quaternion()
  20 : x(0.0), y(0.0), z(0.0), w(1.0)
  21 {
  22 }
  23
  24 //=======================================================================
  25 //function : gp_Quaternion
  26 //purpose  :
  27 //=======================================================================
  28
  29 inline gp_Quaternion::gp_Quaternion (const Standard_Real theX, const Standard_Real theY,
  30                                      const Standard_Real theZ, const Standard_Real theW)
  31 : x(theX), y(theY), z(theZ), w(theW)
  32 {
  33 }
  34
  35 //=======================================================================
  36 //function : gp_Quaternion
  37 //purpose  :
  38 //=======================================================================
  39
  40 inline gp_Quaternion::gp_Quaternion (const gp_Vec& theVecFrom, const gp_Vec& theVecTo)
  41 {
  42   SetRotation (theVecFrom, theVecTo);
  43 }
  44
  45 //=======================================================================
  46 //function : gp_Quaternion
  47 //purpose  :
  48 //=======================================================================
  49
  50 inline gp_Quaternion::gp_Quaternion (const gp_Vec& theVecFrom, const gp_Vec& theVecTo, const gp_Vec& theHelpCrossVec)
  51 {
  52   SetRotation (theVecFrom, theVecTo, theHelpCrossVec);
  53 }
  54
  55 //=======================================================================
  56 //function : gp_Quaternion
  57 //purpose  :
  58 //=======================================================================
  59
  60 inline gp_Quaternion::gp_Quaternion (const gp_Vec& theAxis, const Standard_Real theAngle)
  61 {
  62   SetVectorAndAngle (theAxis, theAngle);
  63 }
  64
  65 //=======================================================================
  66 //function : gp_Quaternion
  67 //purpose  :
  68 //=======================================================================
  69
  70 inline gp_Quaternion::gp_Quaternion (const gp_Mat& theMat)
  71 {
  72   SetMatrix (theMat);
  73 }
  74
  75 //=======================================================================
  76 //function : Set
  77 //purpose  :
  78 //=======================================================================
  79
  80 inline void gp_Quaternion::Set (Standard_Real theX, Standard_Real theY,
  81                                 Standard_Real theZ, Standard_Real theW)
  82 {
  83   this->x = theX;
  84   this->y = theY;
  85   this->z = theZ;
  86   this->w = theW;
  87 }
  88
  89 //=======================================================================
  90 //function : Set
  91 //purpose  :
  92 //=======================================================================
  93
  94 inline void gp_Quaternion::Set (const gp_Quaternion& theQuaternion)
  95 {
  96   x = theQuaternion.x;
  97   y = theQuaternion.y;
  98   z = theQuaternion.z;
  99   w = theQuaternion.w;
 100 }
 101
 102 //=======================================================================
 103 //function : X
 104 //purpose  :
 105 //=======================================================================
 106
 107 inline Standard_Real gp_Quaternion::X() const
 108 {
 109     return x;
 110 }
 111
 112 //=======================================================================
 113 //function : Y
 114 //purpose  :
 115 //=======================================================================
 116
 117 inline Standard_Real gp_Quaternion::Y() const
 118 {
 119   return y;
 120 }
 121
 122 //=======================================================================
 123 //function : Z
 124 //purpose  :
 125 //=======================================================================
 126
 127 inline Standard_Real gp_Quaternion::Z() const
 128 {
 129   return z;
 130 }
 131
 132 //=======================================================================
 133 //function : W
 134 //purpose  :
 135 //=======================================================================
 136
 137 inline Standard_Real gp_Quaternion::W() const
 138 {
 139   return w;
 140 }
 141
 142 //=======================================================================
 143 //function : SetIdent
 144 //purpose  :
 145 //=======================================================================
 146
 147 inline void gp_Quaternion::SetIdent()
 148 {
 149   x = y = z = 0.0;
 150   w = 1.0;
 151 }
 152
 153 //=======================================================================
 154 //function : Reverse
 155 //purpose  :
 156 //=======================================================================
 157
 158 inline void gp_Quaternion::Reverse()
 159 {
 160   x = -x;
 161   y = -y;
 162   z = -z;
 163 }
 164
 165 //=======================================================================
 166 //function : Reversed
 167 //purpose  :
 168 //=======================================================================
 169
 170 inline gp_Quaternion gp_Quaternion::Reversed() const
 171 {
 172   return gp_Quaternion (-x, -y, -z, w);
 173 }
 174
 175 //=======================================================================
 176 //function : Scale
 177 //purpose  :
 178 //=======================================================================
 179
 180 inline void gp_Quaternion::Scale (const Standard_Real theScale)
 181 {
 182   x *= theScale;
 183   y *= theScale;
 184   z *= theScale;
 185   w *= theScale;
 186 }
 187
 188 //=======================================================================
 189 //function : Scaled
 190 //purpose  :
 191 //=======================================================================
 192
 193 inline gp_Quaternion gp_Quaternion::Scaled (const Standard_Real theScale) const
 194 {
 195   return gp_Quaternion (x * theScale, y * theScale, z * theScale, w * theScale);
 196 }
 197
 198 //=======================================================================
 199 //function : SquareNorm
 200 //purpose  :
 201 //=======================================================================
 202
 203 inline Standard_Real gp_Quaternion::SquareNorm() const
 204 {
 205   return x * x + y * y + z * z + w * w;
 206 }
 207
 208 //=======================================================================
 209 //function : Norm
 210 //purpose  :
 211 //=======================================================================
 212
 213 inline Standard_Real gp_Quaternion::Norm() const
 214 {
 215   return Sqrt (SquareNorm());
 216 }
 217
 218 //=======================================================================
 219 //function : Invert
 220 //purpose  :
 221 //=======================================================================
 222
 223 inline void gp_Quaternion::Invert()
 224 {
 225   Standard_Real in = 1.0 / SquareNorm();
 226   Set (-x * in, -y * in, -z * in, w * in);
 227 }
 228
 229 //=======================================================================
 230 //function : Inverted
 231 //purpose  :
 232 //=======================================================================
 233
 234 inline gp_Quaternion gp_Quaternion::Inverted() const
 235 {
 236   Standard_Real in = 1.0 / SquareNorm();
 237   return gp_Quaternion (-x * in, -y * in, -z * in, w * in);
 238 }
 239
 240 //=======================================================================
 241 //function : Normalized
 242 //purpose  :
 243 //=======================================================================
 244
 245 inline gp_Quaternion gp_Quaternion::Normalized() const
 246 {
 247   gp_Quaternion aNormilizedQ (*this);
 248   aNormilizedQ.Normalize();
 249   return aNormilizedQ;
 250 }
 251
 252 //=======================================================================
 253 //function : Negated
 254 //purpose  :
 255 //=======================================================================
 256
 257 inline gp_Quaternion gp_Quaternion::Negated () const
 258 {
 259   return gp_Quaternion (-x, -y, -z, -w);
 260 }
 261
 262 //=======================================================================
 263 //function : Added
 264 //purpose  :
 265 //=======================================================================
 266
 267 inline gp_Quaternion gp_Quaternion::Added (const gp_Quaternion& theQ) const
 268 {
 269   return gp_Quaternion (x + theQ.x, y + theQ.y, z + theQ.z, w + theQ.w);
 270 }
 271
 272 //=======================================================================
 273 //function : Subtracted
 274 //purpose  :
 275 //=======================================================================
 276
 277 inline gp_Quaternion gp_Quaternion::Subtracted (const gp_Quaternion& theQ) const
 278 {
 279   return gp_Quaternion (x - theQ.x, y - theQ.y, z - theQ.z, w - theQ.w);
 280 }
 281
 282 //=======================================================================
 283 //function : Multiplied
 284 //purpose  :
 285 //=======================================================================
 286
 287 inline gp_Quaternion gp_Quaternion::Multiplied (const gp_Quaternion& theQ) const
 288 {
 289   return gp_Quaternion (w * theQ.x + x * theQ.w + y * theQ.z - z * theQ.y,
 290                         w * theQ.y + y * theQ.w + z * theQ.x - x * theQ.z,
 291                         w * theQ.z + z * theQ.w + x * theQ.y - y * theQ.x,
 292                         w * theQ.w - x * theQ.x - y * theQ.y - z * theQ.z);
 293   // 16 multiplications    12 addidtions    0 variables
 294 }
 295
 296 //=======================================================================
 297 //function : Add
 298 //purpose  :
 299 //=======================================================================
 300
 301 inline void gp_Quaternion::Add (const gp_Quaternion& theQ)
 302 {
 303   x += theQ.x;
 304   y += theQ.y;
 305   z += theQ.z;
 306   w += theQ.w;
 307 }
 308
 309 //=======================================================================
 310 //function : Subtract
 311 //purpose  :
 312 //=======================================================================
 313
 314 inline void gp_Quaternion::Subtract (const gp_Quaternion& theQ)
 315 {
 316   x -= theQ.x;
 317   y -= theQ.y;
 318   z -= theQ.z;
 319   w -= theQ.w;
 320 }
 321
 322 //=======================================================================
 323 //function : Multiply
 324 //purpose  :
 325 //=======================================================================
 326
 327 inline void gp_Quaternion::Multiply (const gp_Quaternion& theQ)
 328 {
 329   (*this) = Multiplied (theQ);  // have no optimization here
 330 }
 331
 332 //=======================================================================
 333 //function : Dot
 334 //purpose  :
 335 //=======================================================================
 336
 337 inline Standard_Real gp_Quaternion::Dot (const gp_Quaternion& theQ) const
 338 {
 339   return x * theQ.x + y * theQ.y + z * theQ.z + w * theQ.w;
 340 }
 341