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: BaseQuaternion()24 BaseQuaternion() : x(0.0f), y(0.0f), z(0.0f), w(1.0f) {} 25 BaseQuaternion(T aX,T aY,T aZ,T aW)26 BaseQuaternion(T aX, T aY, T aZ, T aW) : x(aX), y(aY), z(aZ), w(aW) {} 27 BaseQuaternion(const BaseQuaternion & aOther)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 Set(T aX,T aY,T aZ,T aW)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) SetFromRotationMatrix(const Matrix4x4Typed<UnknownUnits,UnknownUnits,T> & m)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 Length()99 T Length() const { return sqrt(x * x + y * y + z * z + w * w); } 100 Conjugate()101 BaseQuaternion& Conjugate() { 102 x *= -1.f; 103 y *= -1.f; 104 z *= -1.f; 105 return *this; 106 } 107 Normalize()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 Invert()123 BaseQuaternion& Invert() { return Conjugate().Normalize(); } 124 Inverse()125 BaseQuaternion Inverse() const { 126 BaseQuaternion q = *this; 127 q.Invert(); 128 return q; 129 } 130 RotatePoint(const Point3DTyped<UnknownUnits,T> & aPoint)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 151