Quaternion.h (3804B)
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/gfx/MatrixFwd.h" 14 #include "mozilla/gfx/Point.h" 15 16 namespace mozilla { 17 namespace gfx { 18 19 template <class T> 20 class BaseQuaternion { 21 public: 22 BaseQuaternion() : x(0.0f), y(0.0f), z(0.0f), w(1.0f) {} 23 24 BaseQuaternion(T aX, T aY, T aZ, T aW) : x(aX), y(aY), z(aZ), w(aW) {} 25 26 BaseQuaternion(const BaseQuaternion& aOther) { 27 x = aOther.x; 28 y = aOther.y; 29 z = aOther.z; 30 w = aOther.w; 31 } 32 33 T x, y, z, w; 34 35 template <class U> 36 friend std::ostream& operator<<(std::ostream& aStream, 37 const BaseQuaternion<U>& aQuat); 38 39 void Set(T aX, T aY, T aZ, T aW) { 40 x = aX; 41 y = aY; 42 z = aZ; 43 w = aW; 44 } 45 46 // Assumes upper 3x3 of aMatrix is a pure rotation matrix (no scaling) 47 void SetFromRotationMatrix( 48 const Matrix4x4Typed<UnknownUnits, UnknownUnits, T>& m) { 49 const T trace = m._11 + m._22 + m._33 + 1.0f; 50 51 if (trace > 1e-4) { 52 const T s = 0.5f / sqrt(trace); 53 w = 0.25f / s; 54 x = (m._23 - m._32) * s; 55 y = (m._31 - m._13) * s; 56 z = (m._12 - m._21) * s; 57 } else if (m._11 > m._22 && m._11 > m._33) { 58 const T s = 2.0f * sqrt(1.0f + m._11 - m._22 - m._33); 59 w = (m._23 - m._32) / s; 60 x = 0.25f * s; 61 y = (m._21 + m._12) / s; 62 z = (m._31 + m._13) / s; 63 } else if (m._22 > m._33) { 64 const T s = 2.0 * sqrt(1.0f + m._22 - m._11 - m._33); 65 w = (m._31 - m._13) / s; 66 x = (m._21 + m._12) / s; 67 y = 0.25f * s; 68 z = (m._32 + m._23) / s; 69 } else { 70 const T s = 2.0 * sqrt(1.0f + m._33 - m._11 - m._22); 71 w = (m._12 - m._21) / s; 72 x = (m._31 + m._13) / s; 73 y = (m._32 + m._23) / s; 74 z = 0.25f * s; 75 } 76 77 Normalize(); 78 } 79 80 // result = this * aQuat 81 BaseQuaternion operator*(const BaseQuaternion& aQuat) const { 82 BaseQuaternion o; 83 const T bx = aQuat.x, by = aQuat.y, bz = aQuat.z, bw = aQuat.w; 84 85 o.x = x * bw + w * bx + y * bz - z * by; 86 o.y = y * bw + w * by + z * bx - x * bz; 87 o.z = z * bw + w * bz + x * by - y * bx; 88 o.w = w * bw - x * bx - y * by - z * bz; 89 return o; 90 } 91 92 BaseQuaternion& operator*=(const BaseQuaternion& aQuat) { 93 *this = *this * aQuat; 94 return *this; 95 } 96 97 T Length() const { return sqrt(x * x + y * y + z * z + w * w); } 98 99 BaseQuaternion& Conjugate() { 100 x *= -1.f; 101 y *= -1.f; 102 z *= -1.f; 103 return *this; 104 } 105 106 BaseQuaternion& Normalize() { 107 T l = Length(); 108 if (l) { 109 l = 1.0f / l; 110 x *= l; 111 y *= l; 112 z *= l; 113 w *= l; 114 } else { 115 x = y = z = 0.f; 116 w = 1.f; 117 } 118 return *this; 119 } 120 121 BaseQuaternion& Invert() { return Conjugate().Normalize(); } 122 123 BaseQuaternion Inverse() const { 124 BaseQuaternion q = *this; 125 q.Invert(); 126 return q; 127 } 128 129 Point3DTyped<UnknownUnits, T> RotatePoint( 130 const Point3DTyped<UnknownUnits, T>& aPoint) const { 131 T uvx = T(2.0) * (y * aPoint.z - z * aPoint.y); 132 T uvy = T(2.0) * (z * aPoint.x - x * aPoint.z); 133 T uvz = T(2.0) * (x * aPoint.y - y * aPoint.x); 134 135 return Point3DTyped<UnknownUnits, T>( 136 aPoint.x + w * uvx + y * uvz - z * uvy, 137 aPoint.y + w * uvy + z * uvx - x * uvz, 138 aPoint.z + w * uvz + x * uvy - y * uvx); 139 } 140 }; 141 142 typedef BaseQuaternion<Float> Quaternion; 143 typedef BaseQuaternion<Double> QuaternionDouble; 144 145 } // namespace gfx 146 } // namespace mozilla 147 148 #endif