Add quat.

1 files changed, 79 insertions, 0 deletions

diff --git a/include/math/quat.h b/include/math/quat.h
new file mode 100644
index 0000000..4503abd
--- /dev/null
+++ b/include/math/quat.h
@@ -0,0 +1,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);
+}