1 files changed, 93 insertions, 0 deletions
diff --git a/quat.c b/quat.c
new file mode 100644
index 0000000..0a21968
--- /dev/null
+++ b/quat.c
@@ -0,0 +1,93 @@
+#include <u.h>
+#include <libc.h>
+#include <draw.h>
+#include "dat.h"
+#include "fns.h"
+
+Quaternion
+Quat(double r, double i, double j, double k)
+{
+	return (Quaternion){r, i, j, k};
+}
+
+Quaternion
+Quatvec(double s, Vector3 v)
+{
+	return (Quaternion){s, v.x, v.y, v.z};
+}
+
+Quaternion
+addq(Quaternion a, Quaternion b)
+{
+	return Quat(a.r+b.r, a.i+b.i, a.j+b.j, a.k+b.k);
+}
+
+Quaternion
+subq(Quaternion a, Quaternion b)
+{
+	return Quat(a.r-b.r, a.i-b.i, a.j-b.j, a.k-b.k);
+}
+
+Quaternion
+mulq(Quaternion q, Quaternion r)
+{
+	Vector3 qv, rv, tmp;
+
+	qv = Vec3(q.i, q.j, q.k);
+	rv = Vec3(r.i, r.j, r.k);
+	tmp = addvec3(addvec3(mulvec3(rv, q.r), mulvec3(qv, r.r)), crossvec(qv, rv));
+	return Quatvec(q.r*r.r - dotvec3(qv, rv), tmp);
+}
+
+Quaternion
+smulq(Quaternion q, double s)
+{
+	return Quat(q.r*s, q.i*s, q.j*s, q.k*s);
+}
+
+Quaternion
+sdivq(Quaternion q, double s)
+{
+	return Quat(q.r/s, q.i/s, q.j/s, q.k/s);
+}
+
+double
+dotq(Quaternion q, Quaternion r)
+{
+	return q.r*r.r + q.i*r.i + q.j*r.j + q.k*r.k;
+}
+
+Quaternion
+invq(Quaternion q)
+{
+	double len²;
+
+	len² = dotq(q, q);
+	if(len² == 0)
+		return Quat(0,0,0,0);
+	return Quat(q.r/len², -q.i/len², -q.j/len², -q.k/len²);
+}
+
+double
+qlen(Quaternion q)
+{
+	return sqrt(dotq(q, q));
+}
+
+Quaternion
+normq(Quaternion q)
+{
+	return sdivq(q, qlen(q));
+}
+
+Vector3
+qrotate(Vector3 p, Vector3 axis, double θ)
+{
+	Quaternion qaxis, qr;
+
+	θ /= 2;
+	qaxis = Quatvec(cos(θ), mulvec3(axis, sin(θ)));
+	qr = mulq(mulq(qaxis, Quatvec(0, p)), invq(qaxis)); /* qpq⁻¹ */
+	return Vec3(qr.i, qr.j, qr.k);
+}
+