aboutsummaryrefslogtreecommitdiff
path: root/include
diff options
context:
space:
mode:
author3gg <3gg@shellblade.net>2023-02-04 11:07:21 -0800
committer3gg <3gg@shellblade.net>2023-02-04 11:07:21 -0800
commitfc175dbb72f80764431c1fe82ab8996b114831da (patch)
tree306c54ac2f43e00b56161199de6caf3cb244fe13 /include
parent9f39e29b40ce952773ff97c468384475d1fdcc53 (diff)
Add slerp and qslerp.
Diffstat (limited to 'include')
-rw-r--r--include/math/defs.h17
-rw-r--r--include/math/quat.h87
-rw-r--r--include/math/spatial3.h2
-rw-r--r--include/math/vec3.h44
4 files changed, 136 insertions, 14 deletions
diff --git a/include/math/defs.h b/include/math/defs.h
index f67ee97..f572d8f 100644
--- a/include/math/defs.h
+++ b/include/math/defs.h
@@ -14,7 +14,7 @@ typedef double R;
14#define R_MIN DBL_MIN 14#define R_MIN DBL_MIN
15#define R_MAX DBL_MAX 15#define R_MAX DBL_MAX
16#else // floats 16#else // floats
17typedef float R; 17typedef float R;
18#define R_MIN FLT_MIN 18#define R_MIN FLT_MIN
19#define R_MAX FLT_MAX 19#define R_MAX FLT_MAX
20#endif 20#endif
@@ -29,13 +29,15 @@ typedef float R;
29#define TO_DEG (180.0 / PI) 29#define TO_DEG (180.0 / PI)
30 30
31#ifdef MATH_USE_DOUBLE 31#ifdef MATH_USE_DOUBLE
32static inline double min(R a, R b) { return fmin(a, b); } 32static inline R min(R a, R b) { return fmin(a, b); }
33static inline double max(R a, R b) { return fmax(a, b); } 33static inline R max(R a, R b) { return fmax(a, b); }
34static inline double rlog2(R a) { return log2(a); } 34static inline R rlog2(R a) { return log2(a); }
35static inline R rmod(R a, R m) { return fmodf(a, m); }
35#else // floats 36#else // floats
36static inline float min(R a, R b) { return fminf(a, b); } 37static inline R min(R a, R b) { return fminf(a, b); }
37static inline float max(R a, R b) { return fmaxf(a, b); } 38static inline R max(R a, R b) { return fmaxf(a, b); }
38static inline float rlog2(R a) { return log2f(a); } 39static inline R rlog2(R a) { return log2f(a); }
40static inline R rmod(R a, R m) { return fmod(a, m); }
39#endif 41#endif
40 42
41static inline R rabs(R x) { return x >= 0.0 ? x : -x; } 43static inline R rabs(R x) { return x >= 0.0 ? x : -x; }
@@ -51,3 +53,4 @@ static inline R sign(R x) {
51 } 53 }
52} 54}
53static inline R lerp(R a, R b, R t) { return a + (b - a) * t; } 55static inline R lerp(R a, R b, R t) { return a + (b - a) * t; }
56static inline R R_eq(R a, R b, R eps) { return rabs(a - b) <= eps; }
diff --git a/include/math/quat.h b/include/math/quat.h
index b284567..a154044 100644
--- a/include/math/quat.h
+++ b/include/math/quat.h
@@ -4,23 +4,38 @@
4#include "mat4.h" 4#include "mat4.h"
5#include "vec3.h" 5#include "vec3.h"
6 6
7#include <assert.h>
8
7/// A quaternion. 9/// A quaternion.
8typedef struct quat { 10typedef struct quat {
9 R w, x, y, z; 11 R x, y, z, w;
10} quat; 12} quat;
11 13
14/// Return the unit/identity quaternion.
12static inline quat qunit() { 15static inline quat qunit() {
13 return (quat){.x = 0.0, .y = 0.0, .z = 0.0, .w = 0.0}; 16 return (quat){.x = 0.0, .y = 0.0, .z = 0.0, .w = 1.0};
14} 17}
15 18
19/// Construct a quaternion.
16static inline quat qmake(R x, R y, R z, R w) { 20static inline quat qmake(R x, R y, R z, R w) {
17 return (quat){.x = x, .y = y, .z = z, .w = w}; 21 return (quat){.x = x, .y = y, .z = z, .w = w};
18} 22}
19 23
24/// Construct a quaternion from an array.
20static inline quat quat_from_array(const R xyzw[4]) { 25static inline quat quat_from_array(const R xyzw[4]) {
21 return (quat){.x = xyzw[0], .y = xyzw[1], .z = xyzw[2], .w = xyzw[3]}; 26 return (quat){.x = xyzw[0], .y = xyzw[1], .z = xyzw[2], .w = xyzw[3]};
22} 27}
23 28
29/// Construct a quaternion from a 3D point.
30static inline quat quat_from_vec3(vec3 p) {
31 return (quat){.x = p.x, .y = p.y, .z = p.z, .w = 0.0};
32}
33
34/// Get a 3D point back from a quaternion.
35static inline vec3 vec3_from_quat(quat q) {
36 return (vec3){.x = q.x, .y = q.y, .z = q.z};
37}
38
24/// Construct a rotation quaternion. 39/// Construct a rotation quaternion.
25static inline quat qmake_rot(R angle, R x, R y, R z) { 40static inline quat qmake_rot(R angle, R x, R y, R z) {
26 const R a = angle * 0.5; 41 const R a = angle * 0.5;
@@ -34,6 +49,11 @@ static inline quat qmake_rot(R angle, R x, R y, R z) {
34 return (quat){x / mag, y / mag, z / mag, w}; 49 return (quat){x / mag, y / mag, z / mag, w};
35} 50}
36 51
52/// Add two quaternions.
53static inline quat qadd(quat a, quat b) {
54 return (quat){.x = a.x + b.x, .y = a.y + b.y, .z = a.z + b.z, .w = a.w + b.w};
55}
56
37/// Multiply two quaternions. 57/// Multiply two quaternions.
38static inline quat qmul(quat q1, quat q2) { 58static inline quat qmul(quat q1, quat q2) {
39 const R x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y; 59 const R x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
@@ -61,6 +81,37 @@ static inline vec3 qrot(quat q, vec3 v) {
61 return vec3_make(u.x, u.y, u.z); 81 return vec3_make(u.x, u.y, u.z);
62} 82}
63 83
84/// Negate the quaternion.
85static inline quat qneg(quat q) {
86 return (quat){.x = -q.x, .y = -q.y, .z = -q.z, .w = -q.w};
87}
88
89/// Return the dot product of two quaternions.
90static inline R qdot(quat a, quat b) {
91 return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
92}
93
94/// Return the quaternion's magnitude.
95static inline R qnorm(quat q) {
96 return sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
97}
98
99/// Return the quaternion's squared magnitude.
100static inline R qnorm2(quat q) {
101 return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
102}
103
104/// Normalize the quaternion.
105static inline quat qnormalize(quat q) {
106 const R n = qnorm(q);
107 return (quat){.x = q.x / n, .y = q.y / n, .z = q.z / n, .w = q.w / n};
108}
109
110/// Scale the quaternion.
111static inline quat qscale(quat q, R s) {
112 return (quat){.x = s * q.x, .y = s * q.y, .z = s * q.z, .w = s * q.w};
113}
114
64/// Get a 4x4 rotation matrix from a quaternion. 115/// Get a 4x4 rotation matrix from a quaternion.
65static inline mat4 mat4_from_quat(quat q) { 116static inline mat4 mat4_from_quat(quat q) {
66 const R xx = q.x * q.x; 117 const R xx = q.x * q.x;
@@ -77,3 +128,35 @@ static inline mat4 mat4_from_quat(quat q) {
77 1 - 2 * xx - 2 * zz, 2 * yz - 2 * wx, 0, 2 * xz - 2 * wy, 2 * yz + 2 * wx, 128 1 - 2 * xx - 2 * zz, 2 * yz - 2 * wx, 0, 2 * xz - 2 * wy, 2 * yz + 2 * wx,
78 1 - 2 * xx - 2 * yy, 0, 0, 0, 0, 1); 129 1 - 2 * xx - 2 * yy, 0, 0, 0, 0, 1);
79} 130}
131
132/// Interpolate two unit quaternions using spherical linear interpolation.
133///
134/// Note: You might want to normalize the result.
135static inline quat qslerp(quat a, quat b, R t) {
136 assert(0.0 <= t);
137 assert(t <= 1.0);
138 const R eps = 1e-5;
139 (void)eps;
140 assert(R_eq(qnorm2(a), 1.0, eps));
141 assert(R_eq(qnorm2(b), 1.0, eps));
142 R dot = qdot(a, b);
143 // Make the rotation path follow the "short way", i.e., ensure that:
144 // -90 <= angle <= 90
145 if (dot < 0.0) {
146 dot = -dot;
147 b = qneg(b);
148 }
149 // For numerical stability, perform linear interpolation when the two
150 // quaternions are close to each other.
151 R ta, tb;
152 if (1.0 - dot > 1e-6) {
153 const R theta = acos(dot);
154 const R sin_theta = sqrt(1 - dot * dot);
155 ta = sin(theta * (1.0 - t)) / sin_theta;
156 tb = sin(theta * t) / sin_theta;
157 } else { // Linear interpolation.
158 ta = 1.0 - t;
159 tb = t;
160 }
161 return qadd(qscale(a, ta), qscale(b, tb));
162}
diff --git a/include/math/spatial3.h b/include/math/spatial3.h
index 8de38bf..9065972 100644
--- a/include/math/spatial3.h
+++ b/include/math/spatial3.h
@@ -118,7 +118,7 @@ static inline void spatial3_set_transform(Spatial3* spatial, mat4 transform) {
118static inline void spatial3_set_forward(Spatial3* spatial, vec3 forward) { 118static inline void spatial3_set_forward(Spatial3* spatial, vec3 forward) {
119 spatial->f = vec3_normalize(forward); 119 spatial->f = vec3_normalize(forward);
120 // Use aux vector to define right vector orthogonal to forward. 120 // Use aux vector to define right vector orthogonal to forward.
121 if (vec3_eq(vec3_abs(spatial->f), up3())) { 121 if (vec3_eq(vec3_abs(spatial->f), up3(), 1e-9)) {
122 spatial->r = vec3_normalize(vec3_cross(spatial->f, forward3())); 122 spatial->r = vec3_normalize(vec3_cross(spatial->f, forward3()));
123 } else { 123 } else {
124 spatial->r = vec3_normalize(vec3_cross(spatial->f, up3())); 124 spatial->r = vec3_normalize(vec3_cross(spatial->f, up3()));
diff --git a/include/math/vec3.h b/include/math/vec3.h
index 641c02f..d8e1248 100644
--- a/include/math/vec3.h
+++ b/include/math/vec3.h
@@ -51,14 +51,14 @@ static inline vec3 vec3_div(vec3 a, vec3 b) {
51 return (vec3){a.x / b.x, a.y / b.y, a.z / b.z}; 51 return (vec3){a.x / b.x, a.y / b.y, a.z / b.z};
52} 52}
53 53
54/// Scale a vector by a scalar value. 54/// Scale the vector.
55static inline vec3 vec3_scale(vec3 v, R s) { 55static inline vec3 vec3_scale(vec3 v, R s) {
56 return (vec3){v.x * s, v.y * s, v.z * s}; 56 return (vec3){v.x * s, v.y * s, v.z * s};
57} 57}
58 58
59/// Compare two vectors for equality. 59/// Compare two vectors for equality.
60static inline bool vec3_eq(vec3 a, vec3 b) { 60static inline bool vec3_eq(vec3 a, vec3 b, R eps) {
61 return a.x == b.x && a.y == b.y && a.z == b.z; 61 return R_eq(a.x, b.x, eps) && R_eq(a.y, b.y, eps) && R_eq(a.z, b.z, eps);
62} 62}
63 63
64/// Return the absolute value of the vector. 64/// Return the absolute value of the vector.
@@ -67,7 +67,9 @@ static inline vec3 vec3_abs(vec3 v) {
67} 67}
68 68
69/// Compare two vectors for inequality. 69/// Compare two vectors for inequality.
70static inline bool vec3_ne(vec3 a, vec3 b) { return !(vec3_eq(a, b)); } 70static inline bool vec3_ne(vec3 a, vec3 b, R eps) {
71 return !(vec3_eq(a, b, eps));
72}
71 73
72/// Return the vector's squared magnitude. 74/// Return the vector's squared magnitude.
73static inline R vec3_norm2(vec3 v) { return v.x * v.x + v.y * v.y + v.z * v.z; } 75static inline R vec3_norm2(vec3 v) { return v.x * v.x + v.y * v.y + v.z * v.z; }
@@ -123,6 +125,40 @@ static inline vec3 vec3_pow(vec3 v, R p) {
123 return (vec3){pow(v.x, p), pow(v.y, p), pow(v.z, p)}; 125 return (vec3){pow(v.x, p), pow(v.y, p), pow(v.z, p)};
124} 126}
125 127
128/// Linearly interpolate two vectors.
129static inline vec3 vec3_lerp(vec3 a, vec3 b, R t) {
130 return (vec3){
131 .x = a.x + t * (b.x - a.x),
132 .y = a.y + t * (b.y - a.y),
133 .z = a.z + t * (b.z - a.z)};
134}
135
136/// Interpolate two unit vectors using spherical linear interpolation.
137///
138/// Note: You might want to normalize the result.
139static inline vec3 vec3_slerp(vec3 a, vec3 b, R t) {
140 assert(0.0 <= t);
141 assert(t <= 1.0);
142 const R eps = 1e-5;
143 (void)eps;
144 assert(R_eq(vec3_norm2(a), 1.0, eps));
145 assert(R_eq(vec3_norm2(b), 1.0, eps));
146 R dot = vec3_dot(a, b);
147 // For numerical stability, perform linear interpolation when the two vectors
148 // are close to each other.
149 R ta, tb;
150 if (1.0 - dot > 1e-6) {
151 const R theta = acos(dot);
152 const R sin_theta = sqrt(1.0 - dot * dot);
153 ta = sin((1.0 - t) * theta) / sin_theta;
154 tb = sin(t * theta) / sin_theta;
155 } else { // Linear interpolation.
156 ta = 1.0 - t;
157 tb = t;
158 }
159 return vec3_add(vec3_scale(a, ta), vec3_scale(b, tb));
160}
161
126/// The (1, 0, 0) vector. 162/// The (1, 0, 0) vector.
127static inline vec3 right3() { return (vec3){1.0, 0.0, 0.0}; } 163static inline vec3 right3() { return (vec3){1.0, 0.0, 0.0}; }
128 164