1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
|
#include "quaternion.hpp"
#include <cassert>
#include <cmath>
namespace mbgl {
Quaternion Quaternion::identity = Quaternion(0.0, 0.0, 0.0, 1.0);
Quaternion Quaternion::conjugate() const {
return {-x, -y, -z, w};
}
Quaternion Quaternion::normalized() const {
const double len = length();
assert(len > 0.0);
return {x / len, y / len, z / len, w / len};
}
Quaternion Quaternion::fromAxisAngle(const vec3& axis, double angleRad) {
const double coss = std::cos(0.5 * angleRad);
const double sins = std::sin(0.5 * angleRad);
Quaternion q;
q.x = sins * axis[0];
q.y = sins * axis[1];
q.z = sins * axis[2];
q.w = coss;
return q;
}
Quaternion Quaternion::fromEulerAngles(double x, double y, double z) {
double cz = std::cos(z * 0.5);
double sz = std::sin(z * 0.5);
double cy = std::cos(y * 0.5);
double sy = std::sin(y * 0.5);
double cx = std::cos(x * 0.5);
double sx = std::sin(x * 0.5);
Quaternion q;
q.x = sx * cy * cz - cx * sy * sz;
q.y = cx * sy * cz + sx * cy * sz;
q.z = cx * cy * sz - sx * sy * cz;
q.w = cx * cy * cz + sx * sy * sz;
return q;
}
Quaternion Quaternion::multiply(const Quaternion& o) const {
Quaternion q;
q.w = w * o.w - x * o.x - y * o.y - z * o.z;
q.x = w * o.x + x * o.w + y * o.z - z * o.y;
q.y = w * o.y + y * o.w + z * o.x - x * o.z;
q.z = w * o.z + z * o.w + x * o.y - y * o.x;
return q;
}
double Quaternion::length() const {
return std::sqrt(x * x + y * y + z * z + w * w);
}
vec3 Quaternion::transform(const vec3& v) const {
const Quaternion src = {v[0], v[1], v[2], 0.0};
const Quaternion res = multiply(src).multiply(conjugate());
return {res.x, res.y, res.z};
}
mat4 Quaternion::toRotationMatrix() const {
mat4 mat;
const double tx = 2.0 * x;
const double ty = 2.0 * y;
const double tz = 2.0 * z;
const double twx = tx * w;
const double twy = ty * w;
const double twz = tz * w;
const double txx = tx * x;
const double txy = ty * x;
const double txz = tz * x;
const double tyy = ty * y;
const double tyz = tz * y;
const double tzz = tz * z;
mat[0] = 1.0 - (tyy + tzz);
mat[1] = txy + twz;
mat[2] = txz - twy;
mat[3] = 0.0;
mat[4] = txy - twz;
mat[5] = 1.0 - (txx + tzz);
mat[6] = tyz + twx;
mat[7] = 0.0;
mat[8] = txz + twy;
mat[9] = tyz - twx;
mat[10] = 1.0 - (txx + tyy);
mat[11] = 0.0;
mat[12] = 0.0;
mat[13] = 0.0;
mat[14] = 0.0;
mat[15] = 1.0;
return mat;
}
bool operator!=(const Quaternion& a, const Quaternion& b) {
return !(a == b);
}
bool operator==(const Quaternion& a, const Quaternion& b) {
return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;
}
} // namespace mbgl
|
