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
|
#pragma once
#include "defs.h"
#include "mat4.h"
#include "vec3.h"
/// A quaternion.
typedef struct quat {
R w, x, y, z;
} quat;
static inline quat qunit() {
return (quat){.x = 0.0, .y = 0.0, .z = 0.0, .w = 0.0};
}
static inline quat qmake(R x, R y, R z, R w) {
return (quat){.x = x, .y = y, .z = z, .w = w};
}
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 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};
}
/// 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);
}
/// 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);
}
|
