quat.h

Go to the documentation of this file.

#ifndef BJ_QUAT_H
#define BJ_QUAT_H
 
#include <banjo/api.h>
#include <banjo/math.h>
#include <banjo/vec.h>
#include <banjo/mat.h>

static BJ_INLINE struct bj_vec4 bj_quat_identity(
    void
) {
    return (struct bj_vec4){ BJ_FZERO, BJ_FZERO, BJ_FZERO, BJ_F(1.0) };
}

static BJ_INLINE bj_real bj_quat_dot(
    struct bj_vec4 a,
    struct bj_vec4 b
) {
    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}

static BJ_INLINE bj_real bj_quat_norm2(
    struct bj_vec4 q
) {
    return bj_quat_dot(q, q);
}

static BJ_INLINE bj_real bj_quat_norm(
    struct bj_vec4 q
) {
    return bj_sqrt(bj_quat_norm2(q));
}

static BJ_INLINE struct bj_vec4 bj_quat_normalize(
    struct bj_vec4 q
) {
    const bj_real n2 = bj_quat_norm2(q);
    if (n2 <= BJ_EPSILON) {
        return bj_quat_identity();
    }
    const bj_real inv = BJ_F(1.0) / bj_sqrt(n2);
    return (struct bj_vec4){ q.x * inv, q.y * inv, q.z * inv, q.w * inv };
}

static BJ_INLINE struct bj_vec4 bj_quat_conjugate(
    struct bj_vec4 q
) {
    return (struct bj_vec4){ -q.x, -q.y, -q.z, q.w };
}

static BJ_INLINE struct bj_vec4 bj_quat_inverse(
    struct bj_vec4 q
) {
    const bj_real n2 = bj_quat_norm2(q);
    if (n2 <= BJ_EPSILON) {
        return bj_quat_identity();
    }
    const bj_real inv = BJ_F(1.0) / n2;
    return (struct bj_vec4){ -q.x * inv, -q.y * inv, -q.z * inv, q.w * inv };
}

static BJ_INLINE struct bj_vec4 bj_quat_mul(
    struct bj_vec4 p,
    struct bj_vec4 q
) {
    return (struct bj_vec4){
        p.w*q.x + p.x*q.w + p.y*q.z - p.z*q.y,
        p.w*q.y - p.x*q.z + p.y*q.w + p.z*q.x,
        p.w*q.z + p.x*q.y - p.y*q.x + p.z*q.w,
        p.w*q.w - p.x*q.x - p.y*q.y - p.z*q.z
    };
}

static BJ_INLINE struct bj_vec4 bj_quat_slerp(
    struct bj_vec4 a,
    struct bj_vec4 b,
    bj_real t
) {
    bj_real cos_omega = bj_quat_dot(a, b);
    struct bj_vec4 bb = b;
    if (cos_omega < BJ_FZERO) {
        cos_omega = -cos_omega;
        bb.x = -b.x;
        bb.y = -b.y;
        bb.z = -b.z;
        bb.w = -b.w;
    }
 
    /* Clamp for numerical safety */
    if (cos_omega > BJ_F(1.0)) cos_omega = BJ_F(1.0);
    if (cos_omega < -BJ_F(1.0)) cos_omega = -BJ_F(1.0);
 
    if (cos_omega > BJ_F(1.0) - BJ_EPSILON) {
        return bj_quat_normalize((struct bj_vec4){
            a.x + t*(bb.x - a.x),
            a.y + t*(bb.y - a.y),
            a.z + t*(bb.z - a.z),
            a.w + t*(bb.w - a.w)
        });
    }
 
    const bj_real omega = bj_acos(cos_omega);
    const bj_real sin_omega = bj_sin(omega);
    if (sin_omega <= BJ_EPSILON) {
        /* Fallback to nlerp */
        return bj_quat_normalize((struct bj_vec4){
            a.x + t*(bb.x - a.x),
            a.y + t*(bb.y - a.y),
            a.z + t*(bb.z - a.z),
            a.w + t*(bb.w - a.w)
        });
    }
 
    const bj_real wa = bj_sin((BJ_F(1.0) - t) * omega) / sin_omega;
    const bj_real wb = bj_sin(t * omega) / sin_omega;
    return (struct bj_vec4){
        wa * a.x + wb * bb.x,
        wa * a.y + wb * bb.y,
        wa * a.z + wb * bb.z,
        wa * a.w + wb * bb.w
    };
}

static BJ_INLINE struct bj_vec4 bj_quat_from_axis_angle(
    struct bj_vec3 axis,
    bj_real angle_rad
) {
    const bj_real alen2 = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z;
    if (alen2 <= BJ_EPSILON) {
        return bj_quat_identity();
    }
    const bj_real invlen = BJ_F(1.0) / bj_sqrt(alen2);
    const struct bj_vec3 n = (struct bj_vec3){
        axis.x * invlen, axis.y * invlen, axis.z * invlen
    };
    const bj_real h = angle_rad * BJ_F(0.5);
    const bj_real s = bj_sin(h);
    const bj_real c = bj_cos(h);
    return (struct bj_vec4){ n.x * s, n.y * s, n.z * s, c };
}

static BJ_INLINE struct bj_vec3 bj_quat_rotate_vec3(
    struct bj_vec4 q,
    struct bj_vec3 v
) {
    struct bj_vec3 u = (struct bj_vec3){ q.x, q.y, q.z };
    struct bj_vec3 t = bj_vec3_cross(u, v);
    t.x += t.x; t.y += t.y; t.z += t.z; /* 2*(u×v) */
    struct bj_vec3 r = (struct bj_vec3){
        v.x + q.w * t.x + (u.y * t.z - u.z * t.y),
        v.y + q.w * t.y + (u.z * t.x - u.x * t.z),
        v.z + q.w * t.z + (u.x * t.y - u.y * t.x)
    };
    return r;
}

static BJ_INLINE struct bj_vec4 bj_quat_rotate_vec4(
    struct bj_vec4 q,
    struct bj_vec4 v
) {
    struct bj_vec3 r3 = bj_quat_rotate_vec3(q, (struct bj_vec3){ v.x, v.y, v.z });
    return (struct bj_vec4){ .x = r3.x, .y = r3.y, .z = r3.z, .w = v.w };
}

static BJ_INLINE void bj_quat_to_mat4(
    struct bj_mat4x4* BJ_RESTRICT M,
    struct bj_vec4              q
) {
    q = bj_quat_normalize(q);
    bj_real xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z;
    bj_real xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z;
    bj_real wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z;
 
    bj_mat4_set_identity(M);
 
    bj_real* m = M->m;
    /* Column 0 */
    m[BJ_M4(0,0)] = BJ_F(1.0) - BJ_F(2.0) * (yy + zz);
    m[BJ_M4(0,1)] = BJ_F(2.0) * (xy + wz);
    m[BJ_M4(0,2)] = BJ_F(2.0) * (xz - wy);
    m[BJ_M4(0,3)] = BJ_FZERO;
    /* Column 1 */
    m[BJ_M4(1,0)] = BJ_F(2.0) * (xy - wz);
    m[BJ_M4(1,1)] = BJ_F(1.0) - BJ_F(2.0) * (xx + zz);
    m[BJ_M4(1,2)] = BJ_F(2.0) * (yz + wx);
    m[BJ_M4(1,3)] = BJ_FZERO;
    /* Column 2 */
    m[BJ_M4(2,0)] = BJ_F(2.0) * (xz + wy);
    m[BJ_M4(2,1)] = BJ_F(2.0) * (yz - wx);
    m[BJ_M4(2,2)] = BJ_F(1.0) - BJ_F(2.0) * (xx + yy);
    m[BJ_M4(2,3)] = BJ_FZERO;
    /* Column 3 */
    m[BJ_M4(3,0)] = BJ_FZERO;
    m[BJ_M4(3,1)] = BJ_FZERO;
    m[BJ_M4(3,2)] = BJ_FZERO;
    m[BJ_M4(3,3)] = BJ_F(1.0);
}

static BJ_INLINE struct bj_vec4 bj_quat_from_mat4(
    const struct bj_mat4x4* BJ_RESTRICT M
) {
    const bj_real* m = M->m;
    bj_real m00 = m[BJ_M4(0,0)], m01 = m[BJ_M4(1,0)], m02 = m[BJ_M4(2,0)];
    bj_real m10 = m[BJ_M4(0,1)], m11 = m[BJ_M4(1,1)], m12 = m[BJ_M4(2,1)];
    bj_real m20 = m[BJ_M4(0,2)], m21 = m[BJ_M4(1,2)], m22 = m[BJ_M4(2,2)];
 
    const bj_real trace = m00 + m11 + m22;
    if (trace > BJ_FZERO) {
        const bj_real s = bj_sqrt(trace + BJ_F(1.0)) * BJ_F(2.0);
        const bj_real w = BJ_F(0.25) * s;
        const bj_real x = (m21 - m12) / s;
        const bj_real y = (m02 - m20) / s;
        const bj_real z = (m10 - m01) / s;
        return bj_quat_normalize((struct bj_vec4){ x, y, z, w });
    }
 
    if (m00 > m11 && m00 > m22) {
        const bj_real s = bj_sqrt(BJ_F(1.0) + m00 - m11 - m22) * BJ_F(2.0);
        const bj_real w = (m21 - m12) / s;
        const bj_real x = BJ_F(0.25) * s;
        const bj_real y = (m01 + m10) / s;
        const bj_real z = (m02 + m20) / s;
        return bj_quat_normalize((struct bj_vec4){ x, y, z, w });
    } else if (m11 > m22) {
        const bj_real s = bj_sqrt(BJ_F(1.0) - m00 + m11 - m22) * BJ_F(2.0);
        const bj_real w = (m02 - m20) / s;
        const bj_real x = (m01 + m10) / s;
        const bj_real y = BJ_F(0.25) * s;
        const bj_real z = (m12 + m21) / s;
        return bj_quat_normalize((struct bj_vec4){ x, y, z, w });
    } else {
        const bj_real s = bj_sqrt(BJ_F(1.0) - m00 - m11 + m22) * BJ_F(2.0);
        const bj_real w = (m10 - m01) / s;
        const bj_real x = (m02 + m20) / s;
        const bj_real y = (m12 + m21) / s;
        const bj_real z = BJ_F(0.25) * s;
        return bj_quat_normalize((struct bj_vec4){ x, y, z, w });
    }
}
 
#endif /* BJ_QUAT_H */