diff options
author | rodri <rgl@antares-labs.eu> | 2023-01-29 22:27:33 +0000 |
---|---|---|
committer | rodri <rgl@antares-labs.eu> | 2023-01-29 22:27:33 +0000 |
commit | e70ecd9d84578dad435a39b40592f9a7cd1908d3 (patch) | |
tree | 0abadddf4f131a1aa30fed5a3a434805868fa1e6 /quat.c | |
download | qball-e70ecd9d84578dad435a39b40592f9a7cd1908d3.tar.gz qball-e70ecd9d84578dad435a39b40592f9a7cd1908d3.tar.bz2 qball-e70ecd9d84578dad435a39b40592f9a7cd1908d3.zip |
initial public release.
the original qball doesn't seem to work at all. burnzez's qb adaptation has proven to be very close, with some minor modifications, but still doesn't get it.
Diffstat (limited to 'quat.c')
-rw-r--r-- | quat.c | 93 |
1 files changed, 93 insertions, 0 deletions
@@ -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); +} + |