1 /* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*-
2  * This Source Code Form is subject to the terms of the Mozilla Public
3  * License, v. 2.0. If a copy of the MPL was not distributed with this
4  * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
5 
6 #ifndef GFX_QUATERNION_H
7 #define GFX_QUATERNION_H
8 
9 #include "mozilla/gfx/BasePoint4D.h"
10 #include "mozilla/gfx/Matrix.h"
11 #include "nsAlgorithm.h"
12 #include <algorithm>
13 
14 struct gfxQuaternion : public mozilla::gfx::BasePoint4D<gfxFloat, gfxQuaternion> {
15     typedef mozilla::gfx::BasePoint4D<gfxFloat, gfxQuaternion> Super;
16 
gfxQuaterniongfxQuaternion17     gfxQuaternion() : Super() {}
gfxQuaterniongfxQuaternion18     gfxQuaternion(gfxFloat aX, gfxFloat aY, gfxFloat aZ, gfxFloat aW) : Super(aX, aY, aZ, aW) {}
19 
gfxQuaterniongfxQuaternion20     explicit gfxQuaternion(const mozilla::gfx::Matrix4x4& aMatrix) {
21         w = 0.5 * sqrt(std::max(1 + aMatrix[0][0] + aMatrix[1][1] + aMatrix[2][2], 0.0f));
22         x = 0.5 * sqrt(std::max(1 + aMatrix[0][0] - aMatrix[1][1] - aMatrix[2][2], 0.0f));
23         y = 0.5 * sqrt(std::max(1 - aMatrix[0][0] + aMatrix[1][1] - aMatrix[2][2], 0.0f));
24         z = 0.5 * sqrt(std::max(1 - aMatrix[0][0] - aMatrix[1][1] + aMatrix[2][2], 0.0f));
25 
26         if(aMatrix[2][1] > aMatrix[1][2])
27             x = -x;
28         if(aMatrix[0][2] > aMatrix[2][0])
29             y = -y;
30         if(aMatrix[1][0] > aMatrix[0][1])
31             z = -z;
32     }
33 
34     // Convert from |direction axis, angle| pair to gfxQuaternion.
35     //
36     // Reference:
37     // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
38     //
39     // if the direction axis is (x, y, z) = xi + yj + zk,
40     // and the angle is |theta|, this formula can be done using
41     // an extension of Euler's formula:
42     //   q = cos(theta/2) + (xi + yj + zk)(sin(theta/2))
43     //     = cos(theta/2) +
44     //       x*sin(theta/2)i + y*sin(theta/2)j + z*sin(theta/2)k
45     // Note: aDirection should be an unit vector and
46     //       the unit of aAngle should be Radian.
gfxQuaterniongfxQuaternion47     gfxQuaternion(const mozilla::gfx::Point3D &aDirection, gfxFloat aAngle) {
48         MOZ_ASSERT(mozilla::gfx::FuzzyEqual(aDirection.Length(), 1.0f),
49                    "aDirection should be an unit vector");
50         x = aDirection.x * sin(aAngle/2.0);
51         y = aDirection.y * sin(aAngle/2.0);
52         z = aDirection.z * sin(aAngle/2.0);
53         w = cos(aAngle/2.0);
54     }
55 
SlerpgfxQuaternion56     gfxQuaternion Slerp(const gfxQuaternion &aOther, gfxFloat aCoeff) {
57         gfxFloat dot = mozilla::clamped(DotProduct(aOther), -1.0, 1.0);
58         if (dot == 1.0) {
59             return *this;
60         }
61 
62         gfxFloat theta = acos(dot);
63         gfxFloat rsintheta = 1/sqrt(1 - dot*dot);
64         gfxFloat rightWeight = sin(aCoeff*theta)*rsintheta;
65 
66         gfxQuaternion left = *this;
67         gfxQuaternion right = aOther;
68 
69         left *= cos(aCoeff*theta) - dot*rightWeight;
70         right *= rightWeight;
71 
72         return left + right;
73     }
74 
ToMatrixgfxQuaternion75     mozilla::gfx::Matrix4x4 ToMatrix() {
76       mozilla::gfx::Matrix4x4 temp;
77 
78         temp[0][0] = 1 - 2 * (y * y + z * z);
79         temp[0][1] = 2 * (x * y + w * z);
80         temp[0][2] = 2 * (x * z - w * y);
81         temp[1][0] = 2 * (x * y - w * z);
82         temp[1][1] = 1 - 2 * (x * x + z * z);
83         temp[1][2] = 2 * (y * z + w * x);
84         temp[2][0] = 2 * (x * z + w * y);
85         temp[2][1] = 2 * (y * z - w * x);
86         temp[2][2] = 1 - 2 * (x * x + y * y);
87 
88         return temp;
89     }
90 
91 };
92 
93 #endif /* GFX_QUATERNION_H */
94