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     // see
52     // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
53     const T trace = m._11 + m._22 + m._33;
54     if (trace > 0.0) {
55       const T s = 0.5f / sqrt(trace + 1.0f);
56       w = 0.25f / s;
57       x = (m._32 - m._23) * s;
58       y = (m._13 - m._31) * s;
59       z = (m._21 - m._12) * s;
60     } else if (m._11 > m._22 && m._11 > m._33) {
61       const T s = 2.0f * sqrt(1.0f + m._11 - m._22 - m._33);
62       w = (m._32 - m._23) / s;
63       x = 0.25f * s;
64       y = (m._12 + m._21) / s;
65       z = (m._13 + m._31) / s;
66     } else if (m._22 > m._33) {
67       const T s = 2.0 * sqrt(1.0f + m._22 - m._11 - m._33);
68       w = (m._13 - m._31) / s;
69       x = (m._12 + m._21) / s;
70       y = 0.25f * s;
71       z = (m._23 + m._32) / s;
72     } else {
73       const T s = 2.0 * sqrt(1.0f + m._33 - m._11 - m._22);
74       w = (m._21 - m._12) / s;
75       x = (m._13 + m._31) / s;
76       y = (m._23 + m._32) / s;
77       z = 0.25f * s;
78     }
79   }
80 
81   // result = this * aQuat
82   BaseQuaternion operator*(const BaseQuaternion& aQuat) const {
83     BaseQuaternion o;
84     const T bx = aQuat.x, by = aQuat.y, bz = aQuat.z, bw = aQuat.w;
85 
86     o.x = x * bw + w * bx + y * bz - z * by;
87     o.y = y * bw + w * by + z * bx - x * bz;
88     o.z = z * bw + w * bz + x * by - y * bx;
89     o.w = w * bw - x * bx - y * by - z * bz;
90     return o;
91   }
92 
93   BaseQuaternion& operator*=(const BaseQuaternion& aQuat) {
94     *this = *this * aQuat;
95     return *this;
96   }
97 
Length()98   T Length() const { return sqrt(x * x + y * y + z * z + w * w); }
99 
Conjugate()100   BaseQuaternion& Conjugate() {
101     x *= -1.f;
102     y *= -1.f;
103     z *= -1.f;
104     return *this;
105   }
106 
Normalize()107   BaseQuaternion& Normalize() {
108     T l = Length();
109     if (l) {
110       l = 1.0f / l;
111       x *= l;
112       y *= l;
113       z *= l;
114       w *= l;
115     } else {
116       x = y = z = 0.f;
117       w = 1.f;
118     }
119     return *this;
120   }
121 
Invert()122   BaseQuaternion& Invert() { return Conjugate().Normalize(); }
123 
RotatePoint(const Point3DTyped<UnknownUnits,T> & aPoint)124   Point3DTyped<UnknownUnits, T> RotatePoint(
125       const Point3DTyped<UnknownUnits, T>& aPoint) const {
126     T uvx = T(2.0) * (y * aPoint.z - z * aPoint.y);
127     T uvy = T(2.0) * (z * aPoint.x - x * aPoint.z);
128     T uvz = T(2.0) * (x * aPoint.y - y * aPoint.x);
129 
130     return Point3DTyped<UnknownUnits, T>(
131         aPoint.x + w * uvx + y * uvz - z * uvy,
132         aPoint.y + w * uvy + z * uvx - x * uvz,
133         aPoint.z + w * uvz + x * uvy - y * uvx);
134   }
135 };
136 
137 typedef BaseQuaternion<Float> Quaternion;
138 typedef BaseQuaternion<Double> QuaternionDouble;
139 
140 }  // namespace gfx
141 }  // namespace mozilla
142 
143 #endif
144