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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
|
#pragma once
#include "defs.h"
#include "mat4.h"
#include "vec3.h"
#include <assert.h>
/// A quaternion.
typedef struct quat {
R x, y, z, w;
} quat;
/// Return the unit/identity quaternion.
static inline quat qunit() {
return (quat){.x = 0.0, .y = 0.0, .z = 0.0, .w = 1.0};
}
/// Construct a quaternion.
static inline quat qmake(R x, R y, R z, R w) {
return (quat){.x = x, .y = y, .z = z, .w = w};
}
/// Construct a quaternion from an array.
static inline quat quat_from_array(const R xyzw[4]) {
return (quat){.x = xyzw[0], .y = xyzw[1], .z = xyzw[2], .w = xyzw[3]};
}
/// Construct a quaternion from a 3D point.
static inline quat quat_from_vec3(vec3 p) {
return (quat){.x = p.x, .y = p.y, .z = p.z, .w = 0.0};
}
/// Get a 3D point back from a quaternion.
static inline vec3 vec3_from_quat(quat q) {
return (vec3){.x = q.x, .y = q.y, .z = q.z};
}
/// Construct a rotation quaternion.
static inline quat qmake_rot(R angle, R x, R y, R z) {
const R a = angle * 0.5;
const R sa = sin(a);
const R w = cos(a);
R mag = sqrt(x * x + y * y + z * z);
mag = mag == 0.0 ? 1.0 : mag;
x = x * sa;
y = y * sa;
z = z * sa;
return (quat){x / mag, y / mag, z / mag, w};
}
/// Add two quaternions.
static inline quat qadd(quat a, quat b) {
return (quat){.x = a.x + b.x, .y = a.y + b.y, .z = a.z + b.z, .w = a.w + b.w};
}
/// Multiply two quaternions.
static inline quat qmul(quat q1, quat q2) {
const R x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
const R y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
const R z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
const R w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
return (quat){x, y, z, w};
}
/// Invert the quaternion.
static inline quat qinv(quat q) {
R magsq = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
magsq = magsq == 0.0f ? 1.0f : magsq;
return (quat){-q.x / magsq, -q.y / magsq, -q.z / magsq, q.w / magsq};
}
/// Return the quaternion's conjugate.
static inline quat qconj(quat q) { return (quat){-q.x, -q.y, -q.z, q.w}; }
/// Rotate the given vector by the given unit quaternion.
static inline vec3 qrot(quat q, vec3 v) {
const quat p = qconj(q);
const quat qv = (quat){v.x, v.y, v.z, 0};
const quat u = qmul(qmul(q, qv), p);
return vec3_make(u.x, u.y, u.z);
}
/// Negate the quaternion.
static inline quat qneg(quat q) {
return (quat){.x = -q.x, .y = -q.y, .z = -q.z, .w = -q.w};
}
/// Return the dot product of two quaternions.
static inline R qdot(quat a, quat b) {
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}
/// Return the quaternion's magnitude.
static inline R qnorm(quat q) {
return sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
}
/// Return the quaternion's squared magnitude.
static inline R qnorm2(quat q) {
return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
}
/// Normalize the quaternion.
static inline quat qnormalize(quat q) {
const R n = qnorm(q);
return (quat){.x = q.x / n, .y = q.y / n, .z = q.z / n, .w = q.w / n};
}
/// Scale the quaternion.
static inline quat qscale(quat q, R s) {
return (quat){.x = s * q.x, .y = s * q.y, .z = s * q.z, .w = s * q.w};
}
/// Get a 4x4 rotation matrix from a quaternion.
static inline mat4 mat4_from_quat(quat q) {
const R xx = q.x * q.x;
const R yy = q.y * q.y;
const R zz = q.z * q.z;
const R xy = q.x * q.y;
const R xz = q.x * q.z;
const R yz = q.y * q.z;
const R wx = q.w * q.x;
const R wy = q.w * q.y;
const R wz = q.w * q.z;
return mat4_make(
1 - 2 * yy - 2 * zz, 2 * xy - 2 * wz, 2 * xz + 2 * wy, 0, 2 * xy + 2 * wz,
1 - 2 * xx - 2 * zz, 2 * yz - 2 * wx, 0, 2 * xz - 2 * wy, 2 * yz + 2 * wx,
1 - 2 * xx - 2 * yy, 0, 0, 0, 0, 1);
}
/// Interpolate two unit quaternions using spherical linear interpolation.
///
/// Note: You might want to normalize the result.
static inline quat qslerp(quat a, quat b, R t) {
assert(0.0 <= t);
assert(t <= 1.0);
const R eps = 1e-5;
(void)eps;
assert(R_eq(qnorm2(a), 1.0, eps));
assert(R_eq(qnorm2(b), 1.0, eps));
R dot = qdot(a, b);
// Make the rotation path follow the "short way", i.e., ensure that:
// -90 <= angle <= 90
if (dot < 0.0) {
dot = -dot;
b = qneg(b);
}
// For numerical stability, perform linear interpolation when the two
// quaternions are close to each other.
R ta, tb;
if (1.0 - dot > 1e-6) {
const R theta = acos(dot);
const R sin_theta = sqrt(1 - dot * dot);
ta = sin(theta * (1.0 - t)) / sin_theta;
tb = sin(theta * t) / sin_theta;
} else { // Linear interpolation.
ta = 1.0 - t;
tb = t;
}
return qadd(qscale(a, ta), qscale(b, tb));
}
|
