core/src/Quaternion.cpp

#include <math.h>

#include "core/Generic.hpp"
#include "core/ToString.hpp"

typedef Quaternion Q;

Q* axisAngleQ(Q* q, const Vector3* axis, float angle) {
    q->v.xyz = *axis;
    normalize(&q->v.xyz);
    angle *= 0.5f;
    q->v.w = cosf(angle);
    mulSet(&q->v.xyz, sinf(angle));
    return q;
}

Q* lerpQ(Q* q, const Q* a, float f, const Q* b) {
    add(&q->v, mul(&a->v, 1.0f - f), mul(&b->v, f));
    float iLength = 1.0f / length(&q->v);
    mulSet(&q->v, iLength);
    return q;
}

Q* mulSetQ(Q* q, const Q* o) {
    float dot = dot(&q->v.xyz, &o->v.xyz);
    add(&q->v.xyz, mul(&o->v.xyz, q->v.w),
        add(mul(&q->v.xyz, o->v.w), cross(&q->v.xyz, &o->v.xyz)));
    q->v.w = q->v.w * o->v.w - dot;
    return q;
}

Q* mulQ(Q* q, const Q* a, const Q* b) {
    *q = *a;
    mulSetQ(q, b);
    return q;
}

Vector3* mulQV3(Vector3* r, const Q* q, const Vector3* v) {
    Vector3 qv;
    add(&qv, mul(v, q->v.w), cross(&q->v.xyz, v));
    add(r, mul(&q->v.xyz, dot(&q->v.xyz, v)),
        sub(mul(&qv, q->v.w), cross(&qv, &q->v.xyz)));
    return r;
}

size_t toStringQ(const Q* q, char* buffer, size_t n) {
    return toString(
        buffer, n, "(%.3f i + %.3f j + %.3f k + %.3f)", (double)q->v.x,
        (double)q->v.y, (double)q->v.z, (double)q->v.w);
}