gfx/2d/Quaternion.h

   1 /* -*- Mode: C++; tab-width: 8; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
   2 /* vim: set ts=8 sts=2 et sw=2 tw=80: */
   3 /* This Source Code Form is subject to the terms of the Mozilla Public
   4  * License, v. 2.0. If a copy of the MPL was not distributed with this
   5  * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
   6
   7 #ifndef MOZILLA_GFX_QUATERNION_H_
   8 #define MOZILLA_GFX_QUATERNION_H_
   9
  10 #include "Types.h"
  11 #include <math.h>
  12 #include <ostream>
  13 #include "mozilla/Attributes.h"
  14 #include "mozilla/DebugOnly.h"
  15 #include "mozilla/gfx/MatrixFwd.h"
  16 #include "mozilla/gfx/Point.h"
  17
  18 namespace mozilla {
  19 namespace gfx {
  20
  21 template <class T>
  22 class BaseQuaternion {
  23  public:
  24   BaseQuaternion() : x(0.0f), y(0.0f), z(0.0f), w(1.0f) {}
  25
  26   BaseQuaternion(T aX, T aY, T aZ, T aW) : x(aX), y(aY), z(aZ), w(aW) {}
  27
  28   BaseQuaternion(const BaseQuaternion& aOther) {
  29     x = aOther.x;
  30     y = aOther.y;
  31     z = aOther.z;
  32     w = aOther.w;
  33   }
  34
  35   T x, y, z, w;
  36
  37   template <class U>
  38   friend std::ostream& operator<<(std::ostream& aStream,
  39                                   const BaseQuaternion<U>& aQuat);
  40
  41   void Set(T aX, T aY, T aZ, T aW) {
  42     x = aX;
  43     y = aY;
  44     z = aZ;
  45     w = aW;
  46   }
  47
  48   // Assumes upper 3x3 of aMatrix is a pure rotation matrix (no scaling)
  49   void SetFromRotationMatrix(
  50       const Matrix4x4Typed<UnknownUnits, UnknownUnits, T>& m) {
  51     const T trace = m._11 + m._22 + m._33 + 1.0f;
  52
  53     if (trace > 1e-4) {
  54       const T s = 0.5f / sqrt(trace);
  55       w = 0.25f / s;
  56       x = (m._23 - m._32) * s;
  57       y = (m._31 - m._13) * s;
  58       z = (m._12 - m._21) * s;
  59     } else if (m._11 > m._22 && m._11 > m._33) {
  60       const T s = 2.0f * sqrt(1.0f + m._11 - m._22 - m._33);
  61       w = (m._23 - m._32) / s;
  62       x = 0.25f * s;
  63       y = (m._21 + m._12) / s;
  64       z = (m._31 + m._13) / s;
  65     } else if (m._22 > m._33) {
  66       const T s = 2.0 * sqrt(1.0f + m._22 - m._11 - m._33);
  67       w = (m._31 - m._13) / s;
  68       x = (m._21 + m._12) / s;
  69       y = 0.25f * s;
  70       z = (m._32 + m._23) / s;
  71     } else {
  72       const T s = 2.0 * sqrt(1.0f + m._33 - m._11 - m._22);
  73       w = (m._12 - m._21) / s;
  74       x = (m._31 + m._13) / s;
  75       y = (m._32 + m._23) / s;
  76       z = 0.25f * s;
  77     }
  78
  79     Normalize();
  80   }
  81
  82   // result = this * aQuat
  83   BaseQuaternion operator*(const BaseQuaternion& aQuat) const {
  84     BaseQuaternion o;
  85     const T bx = aQuat.x, by = aQuat.y, bz = aQuat.z, bw = aQuat.w;
  86
  87     o.x = x * bw + w * bx + y * bz - z * by;
  88     o.y = y * bw + w * by + z * bx - x * bz;
  89     o.z = z * bw + w * bz + x * by - y * bx;
  90     o.w = w * bw - x * bx - y * by - z * bz;
  91     return o;
  92   }
  93
  94   BaseQuaternion& operator*=(const BaseQuaternion& aQuat) {
  95     *this = *this * aQuat;
  96     return *this;
  97   }
  98
  99   T Length() const { return sqrt(x * x + y * y + z * z + w * w); }
 100
 101   BaseQuaternion& Conjugate() {
 102     x *= -1.f;
 103     y *= -1.f;
 104     z *= -1.f;
 105     return *this;
 106   }
 107
 108   BaseQuaternion& Normalize() {
 109     T l = Length();
 110     if (l) {
 111       l = 1.0f / l;
 112       x *= l;
 113       y *= l;
 114       z *= l;
 115       w *= l;
 116     } else {
 117       x = y = z = 0.f;
 118       w = 1.f;
 119     }
 120     return *this;
 121   }
 122
 123   BaseQuaternion& Invert() { return Conjugate().Normalize(); }
 124
 125   BaseQuaternion Inverse() const {
 126     BaseQuaternion q = *this;
 127     q.Invert();
 128     return q;
 129   }
 130
 131   Point3DTyped<UnknownUnits, T> RotatePoint(
 132       const Point3DTyped<UnknownUnits, T>& aPoint) const {
 133     T uvx = T(2.0) * (y * aPoint.z - z * aPoint.y);
 134     T uvy = T(2.0) * (z * aPoint.x - x * aPoint.z);
 135     T uvz = T(2.0) * (x * aPoint.y - y * aPoint.x);
 136
 137     return Point3DTyped<UnknownUnits, T>(
 138         aPoint.x + w * uvx + y * uvz - z * uvy,
 139         aPoint.y + w * uvy + z * uvx - x * uvz,
 140         aPoint.z + w * uvz + x * uvy - y * uvx);
 141   }
 142 };
 143
 144 typedef BaseQuaternion<Float> Quaternion;
 145 typedef BaseQuaternion<Double> QuaternionDouble;
 146
 147 }  // namespace gfx
 148 }  // namespace mozilla
 149
 150 #endif