mat.h

Go to the documentation of this file.

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

struct bj_mat3x3 { bj_real m[9];  };
#define BJ_M3(c,r)    ((c)*3 + (r))

struct bj_mat3x2 { bj_real m[6];  };
#define BJ_M32(c,r)   ((c)*2 + (r))

struct bj_mat4x4 { bj_real m[16]; };
#define BJ_M4(c,r)    ((c)*4 + (r))

struct bj_mat4x3 { bj_real m[12]; };
#define BJ_M43(c,r)   ((c)*3 + (r))

static BJ_INLINE void bj_mat3_set_identity(struct bj_mat3x3* BJ_RESTRICT M) {
    bj_real* m = M->m;
    m[BJ_M3(0, 0)] = BJ_F(1.0); m[BJ_M3(0, 1)] = BJ_FZERO;  m[BJ_M3(0, 2)] = BJ_FZERO;
    m[BJ_M3(1, 0)] = BJ_FZERO;  m[BJ_M3(1, 1)] = BJ_F(1.0); m[BJ_M3(1, 2)] = BJ_FZERO;
    m[BJ_M3(2, 0)] = BJ_FZERO;  m[BJ_M3(2, 1)] = BJ_FZERO;  m[BJ_M3(2, 2)] = BJ_F(1.0);
}

static BJ_INLINE void bj_mat3_copy(
    struct bj_mat3x3* BJ_RESTRICT       dst,
    const struct bj_mat3x3* BJ_RESTRICT src
) {
    for (int i = 0 ; i < 9 ; ++i) {
        dst->m[i] = src->m[i];
    }
}

static BJ_INLINE struct bj_vec3 bj_mat3_row(const struct bj_mat3x3* BJ_RESTRICT M, int r) {
    const bj_real* m = M->m;
    return (struct bj_vec3) { 
        m[BJ_M3(0, r)], m[BJ_M3(1, r)], m[BJ_M3(2, r)] 
    };
}

static BJ_INLINE struct bj_vec3 bj_mat3_col(const struct bj_mat3x3* BJ_RESTRICT M, int c) {
    const bj_real* m = M->m;
    return (struct bj_vec3) { m[BJ_M3(c, 0)], m[BJ_M3(c, 1)], m[BJ_M3(c, 2)] };
}

static BJ_INLINE void bj_mat3_transpose(struct bj_mat3x3* BJ_RESTRICT out, const struct bj_mat3x3* BJ_RESTRICT A) {
    const bj_real* a = A->m;
    bj_real r[9];
    r[BJ_M3(0,0)] = a[BJ_M3(0,0)];
    r[BJ_M3(0,1)] = a[BJ_M3(1,0)];
    r[BJ_M3(0,2)] = a[BJ_M3(2,0)];
    r[BJ_M3(1,0)] = a[BJ_M3(0,1)];
    r[BJ_M3(1,1)] = a[BJ_M3(1,1)];
    r[BJ_M3(1,2)] = a[BJ_M3(2,1)];
    r[BJ_M3(2,0)] = a[BJ_M3(0,2)];
    r[BJ_M3(2,1)] = a[BJ_M3(1,2)];
    r[BJ_M3(2,2)] = a[BJ_M3(2,2)];
    for (int i = 0 ; i<9 ; ++i) {
        out->m[i] = r[i];
    }
}

static BJ_INLINE void bj_mat3_add(
    struct bj_mat3x3* BJ_RESTRICT       out,
    const struct bj_mat3x3* BJ_RESTRICT A,
    const struct bj_mat3x3* BJ_RESTRICT B
) {
    for (int i = 0 ; i < 9 ; ++i) {
        out->m[i] = A->m[i] + B->m[i];
    }
}

static BJ_INLINE void bj_mat3_sub(
    struct bj_mat3x3* BJ_RESTRICT       out,
    const struct bj_mat3x3* BJ_RESTRICT A,
    const struct bj_mat3x3* BJ_RESTRICT B
) {
    for (int i = 0 ; i < 9 ; ++i) {
        out->m[i] = A->m[i] - B->m[i];
    }
}

static BJ_INLINE void bj_mat3_mul_scalar(
    struct bj_mat3x3* BJ_RESTRICT       out,
    const struct bj_mat3x3* BJ_RESTRICT A,
    bj_real                    k
) {
    for (int i = 0 ; i < 9 ; ++i) {
        out->m[i] = A->m[i] * k;
    }
}

static BJ_INLINE void bj_mat3_mul(
    struct bj_mat3x3* BJ_RESTRICT out,
    const struct bj_mat3x3* BJ_RESTRICT A,
    const struct bj_mat3x3* BJ_RESTRICT B
) {
    const bj_real* a = A->m;
    const bj_real* b = B->m;
    bj_real* o       = out->m;
 
    for (int c = 0; c < 3; ++c) {
        const bj_real b0 = b[BJ_M3(c, 0)];
        const bj_real b1 = b[BJ_M3(c, 1)];
        const bj_real b2 = b[BJ_M3(c, 2)];
        o[BJ_M3(c,0)] = a[BJ_M3(0,0)] * b0 + a[BJ_M3(1,0)] * b1 + a[BJ_M3(2,0)] * b2;
        o[BJ_M3(c,1)] = a[BJ_M3(0,1)] * b0 + a[BJ_M3(1,1)] * b1 + a[BJ_M3(2,1)] * b2;
        o[BJ_M3(c,2)] = a[BJ_M3(0,2)] * b0 + a[BJ_M3(1,2)] * b1 + a[BJ_M3(2,2)] * b2;
    }
}

static BJ_INLINE struct bj_vec3 bj_mat3_transform_vec3(
    const struct bj_mat3x3* BJ_RESTRICT M,
    struct bj_vec3                    v
) {
    const bj_real* m = M->m;
    return (struct bj_vec3) {
        m[BJ_M3(0,0)] * v.x + m[BJ_M3(1,0)] * v.y + m[BJ_M3(2,0)] * v.z,
        m[BJ_M3(0,1)] * v.x + m[BJ_M3(1,1)] * v.y + m[BJ_M3(2,1)] * v.z,
        m[BJ_M3(0,2)] * v.x + m[BJ_M3(1,2)] * v.y + m[BJ_M3(2,2)] * v.z
    };
}

static BJ_INLINE struct bj_vec2 bj_mat3_transform_point(
    const struct bj_mat3x3* BJ_RESTRICT M,
    struct bj_vec2                    p
) {
    const bj_real* m = M->m;
    const bj_real x  = p.x;
    const bj_real y  = p.y;
    const bj_real w  = m[BJ_M3(0,2)] * x + m[BJ_M3(1,2)] * y + m[BJ_M3(2,2)];
    const bj_real rx = m[BJ_M3(0,0)] * x + m[BJ_M3(1,0)] * y + m[BJ_M3(2,0)];
    const bj_real ry = m[BJ_M3(0,1)] * x + m[BJ_M3(1,1)] * y + m[BJ_M3(2,1)];
    if (w !=  BJ_FZERO) {
        return (struct bj_vec2) { rx / w, ry / w };
    }
    return (struct bj_vec2) { rx, ry };
}

static BJ_INLINE void bj_mat3_set_translation(
    struct bj_mat3x3* BJ_RESTRICT M,
    bj_real              tx,
    bj_real              ty
) {
    bj_mat3_set_identity(M);
    M->m[BJ_M3(2,0)] = tx;
    M->m[BJ_M3(2,1)] = ty;
}

static BJ_INLINE void bj_mat3_translate(
    struct bj_mat3x3* BJ_RESTRICT M,
    bj_real              tx,
    bj_real              ty
) {
    const struct bj_vec3 t = { tx, ty, BJ_FZERO };
    for (int r = 0 ; r<3 ; ++r) {
        const struct bj_vec3 row = bj_mat3_row(M, r);
        M->m[BJ_M3(2,r)] +=  row.x * t.x + row.y * t.y + row.z * t.z;
    }
}

static BJ_INLINE void bj_mat3_set_scaling_xy(
    struct bj_mat3x3* BJ_RESTRICT M,
    bj_real              sx,
    bj_real              sy
) {
    bj_mat3_set_identity(M);
    M->m[BJ_M3(0,0)] = sx;
    M->m[BJ_M3(1,1)] = sy;
}

static BJ_INLINE void bj_mat3_set_shear_xy(
    struct bj_mat3x3* BJ_RESTRICT M,
    bj_real              shx,
    bj_real              shy
) {
    bj_mat3_set_identity(M);
    M->m[BJ_M3(1,0)] = shy;
    M->m[BJ_M3(0,1)] = shx;
}

static BJ_INLINE void bj_mat3_set_rotation_z(
    struct bj_mat3x3* BJ_RESTRICT M,
    bj_real              angle
) {
    const bj_real s = bj_sin(angle);
    const bj_real c = bj_cos(angle);
    bj_real* m = M->m;
    m[BJ_M3(0,0)] =  c;
    m[BJ_M3(0,1)] =  s;
    m[BJ_M3(0,2)] = BJ_FZERO;
    m[BJ_M3(1,0)] = -s;
    m[BJ_M3(1,1)] =  c;
    m[BJ_M3(1,2)] = BJ_FZERO;
    m[BJ_M3(2,0)] = BJ_FZERO;
    m[BJ_M3(2,1)] = BJ_FZERO;
    m[BJ_M3(2,2)] = BJ_F(1.0);
}

static BJ_INLINE bj_real bj_mat3_determinant(
    const struct bj_mat3x3* BJ_RESTRICT A
) {
    const bj_real* m = A->m;
    return
      m[BJ_M3(0,0)] * (m[BJ_M3(1,1)] * m[BJ_M3(2,2)]-m[BJ_M3(2,1)] * m[BJ_M3(1,2)])
    - m[BJ_M3(1,0)] * (m[BJ_M3(0,1)] * m[BJ_M3(2,2)]-m[BJ_M3(2,1)] * m[BJ_M3(0,2)])
    + m[BJ_M3(2,0)] * (m[BJ_M3(0,1)] * m[BJ_M3(1,2)]-m[BJ_M3(1,1)] * m[BJ_M3(0,2)]);
}

static BJ_INLINE bj_bool bj_mat3_invert(
    struct bj_mat3x3* BJ_RESTRICT       out,
    const struct bj_mat3x3* BJ_RESTRICT A
) {
    const bj_real* m = A->m;
    const bj_real c00 =  (m[BJ_M3(1,1)] * m[BJ_M3(2,2)] - m[BJ_M3(2,1)] * m[BJ_M3(1,2)]);
    const bj_real c01 = -(m[BJ_M3(0,1)] * m[BJ_M3(2,2)] - m[BJ_M3(2,1)] * m[BJ_M3(0,2)]);
    const bj_real c02 =  (m[BJ_M3(0,1)] * m[BJ_M3(1,2)] - m[BJ_M3(1,1)] * m[BJ_M3(0,2)]);
    const bj_real c10 = -(m[BJ_M3(1,0)] * m[BJ_M3(2,2)] - m[BJ_M3(2,0)] * m[BJ_M3(1,2)]);
    const bj_real c11 =  (m[BJ_M3(0,0)] * m[BJ_M3(2,2)] - m[BJ_M3(2,0)] * m[BJ_M3(0,2)]);
    const bj_real c12 = -(m[BJ_M3(0,0)] * m[BJ_M3(1,2)] - m[BJ_M3(1,0)] * m[BJ_M3(0,2)]);
    const bj_real c20 =  (m[BJ_M3(1,0)] * m[BJ_M3(2,1)] - m[BJ_M3(2,0)] * m[BJ_M3(1,1)]);
    const bj_real c21 = -(m[BJ_M3(0,0)] * m[BJ_M3(2,1)] - m[BJ_M3(2,0)] * m[BJ_M3(0,1)]);
    const bj_real c22 =  (m[BJ_M3(0,0)] * m[BJ_M3(1,1)] - m[BJ_M3(1,0)] * m[BJ_M3(0,1)]);
    const bj_real det = m[BJ_M3(0,0)] * c00 + m[BJ_M3(1,0)] * c01 + m[BJ_M3(2,0)] * c02;
 
    if (bj_real_is_zero(det)) {
        return BJ_FALSE;
    }
    const bj_real id = BJ_F(1.0) / det;
    out->m[BJ_M3(0,0)] = c00 * id; out->m[BJ_M3(1,0)] = c01 * id; out->m[BJ_M3(2,0)] = c02 * id;
    out->m[BJ_M3(0,1)] = c10 * id; out->m[BJ_M3(1,1)] = c11 * id; out->m[BJ_M3(2,1)] = c12 * id;
    out->m[BJ_M3(0,2)] = c20 * id; out->m[BJ_M3(1,2)] = c21 * id; out->m[BJ_M3(2,2)] = c22 * id;
    return BJ_TRUE;
}

static BJ_INLINE void bj_mat3_invert_unsafe(
    struct bj_mat3x3* BJ_RESTRICT       out,
    const struct bj_mat3x3* BJ_RESTRICT A
) {
    const bj_real* m = A->m;
    const bj_real c00 =  (m[BJ_M3(1,1)] * m[BJ_M3(2,2)] - m[BJ_M3(2,1)] * m[BJ_M3(1,2)]);
    const bj_real c01 = -(m[BJ_M3(0,1)] * m[BJ_M3(2,2)] - m[BJ_M3(2,1)] * m[BJ_M3(0,2)]);
    const bj_real c02 =  (m[BJ_M3(0,1)] * m[BJ_M3(1,2)] - m[BJ_M3(1,1)] * m[BJ_M3(0,2)]);
    const bj_real c10 = -(m[BJ_M3(1,0)] * m[BJ_M3(2,2)] - m[BJ_M3(2,0)] * m[BJ_M3(1,2)]);
    const bj_real c11 =  (m[BJ_M3(0,0)] * m[BJ_M3(2,2)] - m[BJ_M3(2,0)] * m[BJ_M3(0,2)]);
    const bj_real c12 = -(m[BJ_M3(0,0)] * m[BJ_M3(1,2)] - m[BJ_M3(1,0)] * m[BJ_M3(0,2)]);
    const bj_real c20 =  (m[BJ_M3(1,0)] * m[BJ_M3(2,1)] - m[BJ_M3(2,0)] * m[BJ_M3(1,1)]);
    const bj_real c21 = -(m[BJ_M3(0,0)] * m[BJ_M3(2,1)] - m[BJ_M3(2,0)] * m[BJ_M3(0,1)]);
    const bj_real c22 =  (m[BJ_M3(0,0)] * m[BJ_M3(1,1)] - m[BJ_M3(1,0)] * m[BJ_M3(0,1)]);
    const bj_real id  = BJ_F(1.0) / (m[BJ_M3(0,0)] * c00 + m[BJ_M3(1,0)] * c01 + m[BJ_M3(2,0)] * c02);
 
    out->m[BJ_M3(0,0)] = c00 * id; out->m[BJ_M3(1,0)] = c01 * id; out->m[BJ_M3(2,0)] = c02 * id;
    out->m[BJ_M3(0,1)] = c10 * id; out->m[BJ_M3(1,1)] = c11 * id; out->m[BJ_M3(2,1)] = c12 * id;
    out->m[BJ_M3(0,2)] = c20 * id; out->m[BJ_M3(1,2)] = c21 * id; out->m[BJ_M3(2,2)] = c22 * id;
}
 

static BJ_INLINE void bj_mat3_set_ortho(
    struct bj_mat3x3* BJ_RESTRICT M,
    bj_real l, bj_real r,
    bj_real b, bj_real t
) {
    bj_real* m = M->m;
    m[BJ_M3(0,0)] = BJ_F(2.0)/(r-l);
    m[BJ_M3(0,1)] = BJ_FZERO;
    m[BJ_M3(0,2)] = BJ_FZERO;
 
    m[BJ_M3(1,0)] = BJ_FZERO;
    m[BJ_M3(1,1)] = BJ_F(-2.0)/(t-b);
    m[BJ_M3(1,2)] = BJ_FZERO;
 
    m[BJ_M3(2,0)] = -(r+l)/(r-l);
    m[BJ_M3(2,1)] =  (t+b)/(t-b);
    m[BJ_M3(2,2)] = BJ_F(1.0);
}

static BJ_INLINE void bj_mat3_set_viewport(
    struct bj_mat3x3* BJ_RESTRICT M,
    bj_real              x,
    bj_real              y,
    bj_real              w,
    bj_real              h
) {
    bj_real* m = M->m;
    m[BJ_M3(0,0)] = BJ_F(0.5) * w;
    m[BJ_M3(0,1)] = BJ_FZERO;
    m[BJ_M3(0,2)] = BJ_FZERO;
    m[BJ_M3(1,0)] = BJ_FZERO;
    m[BJ_M3(1,1)] = BJ_F(0.5) * h;
    m[BJ_M3(1,2)] = BJ_FZERO;
    m[BJ_M3(2,0)] = x + BJ_F(0.5) * w;
    m[BJ_M3(2,1)] = y + BJ_F(0.5) * h;
    m[BJ_M3(2,2)] = BJ_F(1.0);
}

static BJ_INLINE void bj_mat3x2_set_identity(
    struct bj_mat3x2* BJ_RESTRICT M
) {
    bj_real* m = M->m;
    m[BJ_M32(0,0)] = BJ_F(1.0);
    m[BJ_M32(0,1)] = BJ_FZERO;
    m[BJ_M32(1,0)] = BJ_FZERO;
    m[BJ_M32(1,1)] = BJ_F(1.0);
    m[BJ_M32(2,0)] = BJ_FZERO;
    m[BJ_M32(2,1)] = BJ_FZERO;
}

static BJ_INLINE void bj_mat3x2_set_translation(
    struct bj_mat3x2* BJ_RESTRICT M,
    bj_real                tx,
    bj_real                ty
) {
    bj_mat3x2_set_identity(M);
    M->m[BJ_M32(2,0)] = tx;
    M->m[BJ_M32(2,1)] = ty;
}

static BJ_INLINE void bj_mat3x2_set_scaling_xy(
    struct bj_mat3x2* BJ_RESTRICT M,
    bj_real                sx,
    bj_real                sy
) {
    bj_mat3x2_set_identity(M);
    M->m[BJ_M32(0,0)] = sx;
    M->m[BJ_M32(1,1)] = sy;
}

static BJ_INLINE void bj_mat3x2_set_rotation_z(
    struct bj_mat3x2* BJ_RESTRICT M,
    bj_real                angle
) {
    const bj_real c = bj_cos(angle);
    const bj_real s = bj_sin(angle);
    bj_real* m = M->m;
    m[BJ_M32(0,0)] =  c;
    m[BJ_M32(0,1)] =  s;
    m[BJ_M32(1,0)] = -s;
    m[BJ_M32(1,1)] =  c;
    m[BJ_M32(2,0)] = BJ_FZERO;
    m[BJ_M32(2,1)] = BJ_FZERO;
}

static BJ_INLINE void bj_mat3x2_mul(
    struct bj_mat3x2* BJ_RESTRICT       out,
    const struct bj_mat3x2* BJ_RESTRICT A,
    const struct bj_mat3x2* BJ_RESTRICT B
) {
    const bj_real a00 = A->m[BJ_M32(0,0)];
    const bj_real a10 = A->m[BJ_M32(0,1)];
    const bj_real a01 = A->m[BJ_M32(1,0)];
    const bj_real a11 = A->m[BJ_M32(1,1)];
    const bj_real a02 = A->m[BJ_M32(2,0)];
    const bj_real a12 = A->m[BJ_M32(2,1)];
 
    const bj_real b00 = B->m[BJ_M32(0,0)];
    const bj_real b10 = B->m[BJ_M32(0,1)];
    const bj_real b01 = B->m[BJ_M32(1,0)];
    const bj_real b11 = B->m[BJ_M32(1,1)];
    const bj_real b02 = B->m[BJ_M32(2,0)];
    const bj_real b12 = B->m[BJ_M32(2,1)];
 
    out->m[BJ_M32(0,0)] = a00*b00 + a01*b10;
    out->m[BJ_M32(0,1)] = a10*b00 + a11*b10;
    out->m[BJ_M32(1,0)] = a00*b01 + a01*b11;
    out->m[BJ_M32(1,1)] = a10*b01 + a11*b11;
    out->m[BJ_M32(2,0)] = a00*b02 + a01*b12 + a02;
    out->m[BJ_M32(2,1)] = a10*b02 + a11*b12 + a12;
}
 

static BJ_INLINE struct bj_vec2 bj_mat3x2_transform_point(
    const struct bj_mat3x2* BJ_RESTRICT M,
    struct bj_vec2                      p
) {
    const bj_real* m = M->m;
    const bj_real x = p.x;
    const bj_real y = p.y;
    return (struct bj_vec2) {
        m[BJ_M32(0,0)] * x + m[BJ_M32(1,0)] * y + m[BJ_M32(2,0)],
        m[BJ_M32(0,1)] * x + m[BJ_M32(1,1)] * y + m[BJ_M32(2,1)]
    };
}

static BJ_INLINE struct bj_vec2 bj_mat3x2_transform_dir(
    const struct bj_mat3x2* BJ_RESTRICT M,
    struct bj_vec2                      v
) {
    const bj_real* m = M->m;
    const bj_real x  = v.x;
    const bj_real y  = v.y;
    return (struct bj_vec2) {
        m[BJ_M32(0,0)] * x + m[BJ_M32(1,0)] * y,
        m[BJ_M32(0,1)] * x + m[BJ_M32(1,1)] * y
    };
}

static BJ_INLINE void bj_mat3_from_mat3x2(
    struct bj_mat3x3* BJ_RESTRICT         M,
    const struct bj_mat3x2* BJ_RESTRICT A
) {
    bj_real* o = M->m;
    const bj_real* a = A->m;
    o[BJ_M3(0,0)] = a[BJ_M32(0,0)];
    o[BJ_M3(0,1)] = a[BJ_M32(0,1)];
    o[BJ_M3(0,2)] = BJ_FZERO;
    o[BJ_M3(1,0)] = a[BJ_M32(1,0)];
    o[BJ_M3(1,1)] = a[BJ_M32(1,1)];
    o[BJ_M3(1,2)] = BJ_FZERO;
    o[BJ_M3(2,0)] = a[BJ_M32(2,0)];
    o[BJ_M3(2,1)] = a[BJ_M32(2,1)];
    o[BJ_M3(2,2)] = BJ_F(1.0);
}

static BJ_INLINE void bj_mat3x2_from_mat3(
    struct bj_mat3x2* BJ_RESTRICT     M,
    const struct bj_mat3x3* BJ_RESTRICT A
) {
    const bj_real* a = A->m;
    bj_real* o = M->m;
    o[BJ_M32(0,0)] = a[BJ_M3(0,0)];
    o[BJ_M32(0,1)] = a[BJ_M3(0,1)];
    o[BJ_M32(1,0)] = a[BJ_M3(1,0)];
    o[BJ_M32(1,1)] = a[BJ_M3(1,1)];
    o[BJ_M32(2,0)] = a[BJ_M3(2,0)];
    o[BJ_M32(2,1)] = a[BJ_M3(2,1)];
}

static BJ_INLINE void bj_mat4_set_identity(
    struct bj_mat4x4* BJ_RESTRICT M
) {
    bj_real* m = M->m;
    m[BJ_M4(0,0)] = BJ_F(1.0);
    m[BJ_M4(0,1)] = BJ_FZERO;
    m[BJ_M4(0,2)] = BJ_FZERO;
    m[BJ_M4(0,3)] = BJ_FZERO;
    m[BJ_M4(1,0)] = BJ_FZERO;
    m[BJ_M4(1,1)] = BJ_F(1.0);
    m[BJ_M4(1,2)] = BJ_FZERO;
    m[BJ_M4(1,3)] = BJ_FZERO;
    m[BJ_M4(2,0)] = BJ_FZERO;
    m[BJ_M4(2,1)] = BJ_FZERO;
    m[BJ_M4(2,2)] = BJ_F(1.0);
    m[BJ_M4(2,3)] = BJ_FZERO;
    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 void bj_mat4_copy(
    struct bj_mat4x4* BJ_RESTRICT       dst,
    const struct bj_mat4x4* BJ_RESTRICT src
) {
    for(int i = 0 ; i < 16 ; ++i) {
        dst->m[i] = src->m[i];
    }
}

static BJ_INLINE struct bj_vec4 bj_mat4_row(
    const struct bj_mat4x4* BJ_RESTRICT M,
    int                        r
) {
    const bj_real* m = M->m;
    return (struct bj_vec4) { 
        m[BJ_M4(0,r)],
        m[BJ_M4(1,r)],
        m[BJ_M4(2,r)],
        m[BJ_M4(3,r)] 
    };
}

static BJ_INLINE struct bj_vec4 bj_mat4_col(
    const struct bj_mat4x4* BJ_RESTRICT M,
    int                        c
) {
    const bj_real* m = M->m;
    return (struct bj_vec4) { 
        m[BJ_M4(c,0)],
        m[BJ_M4(c,1)],
        m[BJ_M4(c,2)],
        m[BJ_M4(c,3)] 
    };
}

static BJ_INLINE void bj_mat4_transpose(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT A
) {
    const bj_real* a = A->m;
    bj_real r[16];
    for(int c = 0 ; c < 4; ++c) {
        for(int r0 = 0 ; r0 < 4; ++r0) {
            r[BJ_M4(r0,c)] = a[BJ_M4(c,r0)];
        }
    }
    for(int i = 0 ; i < 16 ; ++i) {
        out->m[i] = r[i];
    }
}

static BJ_INLINE void bj_mat4_add(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT A,
    const struct bj_mat4x4* BJ_RESTRICT B
) {
 
    for(int i = 0 ; i < 16 ; ++i) {
        out->m[i] = A->m[i] + B->m[i];
    }
}

static BJ_INLINE void bj_mat4_sub(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT A,
    const struct bj_mat4x4* BJ_RESTRICT B
) {
 
    for(int i = 0 ; i < 16 ; ++i) {
        out->m[i] = A->m[i] - B->m[i];
    }
}

static BJ_INLINE void bj_mat4_mul_scalar(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT A,
    bj_real                    k
) {
 
    for(int i = 0;i<16;++i) {
        out->m[i] = A->m[i] * k;
    }
}

static BJ_INLINE void bj_mat4_scale_axes(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT A,
    bj_real                    sx,
    bj_real                    sy,
    bj_real                    sz
) {
 
    const bj_real* a = A->m;
    bj_real* m = out->m;
    m[BJ_M4(0,0)] = a[BJ_M4(0,0)] * sx;
    m[BJ_M4(0,1)] = a[BJ_M4(0,1)] * sx;
    m[BJ_M4(0,2)] = a[BJ_M4(0,2)] * sx;
    m[BJ_M4(0,3)] = a[BJ_M4(0,3)] * sx;
    m[BJ_M4(1,0)] = a[BJ_M4(1,0)] * sy;
    m[BJ_M4(1,1)] = a[BJ_M4(1,1)] * sy;
    m[BJ_M4(1,2)] = a[BJ_M4(1,2)] * sy;
    m[BJ_M4(1,3)] = a[BJ_M4(1,3)] * sy;
    m[BJ_M4(2,0)] = a[BJ_M4(2,0)] * sz;
    m[BJ_M4(2,1)] = a[BJ_M4(2,1)] * sz;
    m[BJ_M4(2,2)] = a[BJ_M4(2,2)] * sz;
    m[BJ_M4(2,3)] = a[BJ_M4(2,3)] * sz;
    m[BJ_M4(3,0)] = a[BJ_M4(3,0)];
    m[BJ_M4(3,1)] = a[BJ_M4(3,1)];
    m[BJ_M4(3,2)] = a[BJ_M4(3,2)];
    m[BJ_M4(3,3)] = a[BJ_M4(3,3)];
}

static BJ_INLINE void bj_mat4_mul(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT A,
    const struct bj_mat4x4* BJ_RESTRICT B
)
{
    const bj_real* a = A->m;
    const bj_real* b = B->m;
    bj_real* o = out->m;
 
    for (int c = 0; c < 4; ++c) {
        const bj_real b0 = b[BJ_M4(c,0)];
        const bj_real b1 = b[BJ_M4(c,1)];
        const bj_real b2 = b[BJ_M4(c,2)];
        const bj_real b3 = b[BJ_M4(c,3)];
 
        o[BJ_M4(c,0)] = a[BJ_M4(0,0)] * b0 
                      + a[BJ_M4(1,0)] * b1 
                      + a[BJ_M4(2,0)] * b2 
                      + a[BJ_M4(3,0)] * b3;
        o[BJ_M4(c,1)] = a[BJ_M4(0,1)] * b0 
                      + a[BJ_M4(1,1)] * b1 
                      + a[BJ_M4(2,1)] * b2 
                      + a[BJ_M4(3,1)] * b3;
        o[BJ_M4(c,2)] = a[BJ_M4(0,2)] * b0 
                      + a[BJ_M4(1,2)] * b1 
                      + a[BJ_M4(2,2)] * b2 
                      + a[BJ_M4(3,2)] * b3;
        o[BJ_M4(c,3)] = a[BJ_M4(0,3)] * b0 
                      + a[BJ_M4(1,3)] * b1 
                      + a[BJ_M4(2,3)] * b2 
                      + a[BJ_M4(3,3)] * b3;
    }
}

static BJ_INLINE struct bj_vec4 bj_mat4_transform_vec4(
    const struct bj_mat4x4* BJ_RESTRICT M,
    struct bj_vec4                    v
) {
    const bj_real* m = M->m;
    return (struct bj_vec4) {
        m[BJ_M4(0,0)] * v.x + m[BJ_M4(1,0)] * v.y + 
        m[BJ_M4(2,0)] * v.z + m[BJ_M4(3,0)] * v.w,
        m[BJ_M4(0,1)] * v.x + m[BJ_M4(1,1)] * v.y + 
        m[BJ_M4(2,1)] * v.z + m[BJ_M4(3,1)] * v.w,
        m[BJ_M4(0,2)] * v.x + m[BJ_M4(1,2)] * v.y + 
        m[BJ_M4(2,2)] * v.z + m[BJ_M4(3,2)] * v.w,
        m[BJ_M4(0,3)] * v.x + m[BJ_M4(1,3)] * v.y + 
        m[BJ_M4(2,3)] * v.z + m[BJ_M4(3,3)] * v.w
    };
}

static BJ_INLINE void bj_mat4_set_translation(
    struct bj_mat4x4* BJ_RESTRICT M,
    bj_real              x,
    bj_real              y,
    bj_real              z
) {
    bj_mat4_set_identity(M);
    M->m[BJ_M4(3,0)] = x;
    M->m[BJ_M4(3,1)] = y;
    M->m[BJ_M4(3,2)] = z;
}

static BJ_INLINE void bj_mat4_translate(
    struct bj_mat4x4* BJ_RESTRICT M,
    bj_real x,
    bj_real y,
    bj_real z
) {
    const struct bj_vec4 t = { x, y, z, BJ_FZERO };
    for (int r = 0 ; r<4 ; ++r) {
        const struct bj_vec4 row = bj_mat4_row(M, r);
        M->m[BJ_M4(3,r)] +=  bj_vec4_dot(row, t);
    }
}

static BJ_INLINE void bj_mat4_set_outer_product(
    struct bj_mat4x4* BJ_RESTRICT out,
    struct bj_vec3              a,
    struct bj_vec3              b
) {
    bj_real* m = out->m;
    m[BJ_M4(0,0)] = a.x*b.x;
    m[BJ_M4(1,0)] = a.y*b.x;
    m[BJ_M4(2,0)] = a.z*b.x;
    m[BJ_M4(3,0)] = BJ_FZERO;
    m[BJ_M4(0,1)] = a.x*b.y;
    m[BJ_M4(1,1)] = a.y*b.y;
    m[BJ_M4(2,1)] = a.z*b.y;
    m[BJ_M4(3,1)] = BJ_FZERO;
    m[BJ_M4(0,2)] = a.x*b.z;
    m[BJ_M4(1,2)] = a.y*b.z;
    m[BJ_M4(2,2)] = a.z*b.z;
    m[BJ_M4(3,2)] = BJ_FZERO;
    m[BJ_M4(0,3)] = BJ_FZERO;
    m[BJ_M4(1,3)] = BJ_FZERO;
    m[BJ_M4(2,3)] = BJ_FZERO;
    m[BJ_M4(3,3)] = BJ_FZERO;
}

static BJ_INLINE void bj_mat4_rotate_axis_angle(
    struct bj_mat4x4* restrict       out,
    const struct bj_mat4x4* restrict M,
    struct bj_vec3                 axis,
    bj_real                 angle
) {
    const bj_real s = bj_sin(angle);
    const bj_real c = bj_cos(angle);
    const bj_real len = bj_vec3_len(axis);
    if (len <= BJ_F(1e-4)) { 
        bj_mat4_copy(out, M);
        return; 
    }
 
    const bj_real inv = BJ_F(1.0) / len;
    const bj_real ux = axis.x * inv, uy = axis.y * inv, uz = axis.z * inv;
    const bj_real t = BJ_F(1.0) - c;
 
    /* 3x3 rotation block */
    const bj_real r00 = c + t*ux*ux;
    const bj_real r01 = t*ux*uy + s*uz;
    const bj_real r02 = t*ux*uz - s*uy;
 
    const bj_real r10 = t*uy*ux - s*uz;
    const bj_real r11 = c + t*uy*uy;
    const bj_real r12 = t*uy*uz + s*ux;
 
    const bj_real r20 = t*uz*ux + s*uy;
    const bj_real r21 = t*uz*uy - s*ux;
    const bj_real r22 = c + t*uz*uz;
 
    const bj_real* m = M->m;
    bj_real* o = out->m;
 
    /* Columns of M (column-major) */
    const bj_real* m0 = &m[BJ_M4(0,0)];
    const bj_real* m1 = &m[BJ_M4(1,0)];
    const bj_real* m2 = &m[BJ_M4(2,0)];
    const bj_real* m3 = &m[BJ_M4(3,0)];
 
    bj_real* o0 = &o[BJ_M4(0,0)];
    bj_real* o1 = &o[BJ_M4(1,0)];
    bj_real* o2 = &o[BJ_M4(2,0)];
    bj_real* o3 = &o[BJ_M4(3,0)];
 
    for (int r = 0; r < 4; ++r) {
        const bj_real M0 = m0[r], M1 = m1[r], M2 = m2[r];
        o0[r] = M0*r00 + M1*r10 + M2*r20;
        o1[r] = M0*r01 + M1*r11 + M2*r21;
        o2[r] = M0*r02 + M1*r12 + M2*r22;
        o3[r] = m3[r];
    }
}

static BJ_INLINE void bj_mat4_rotate_x(
    struct bj_mat4x4* restrict       out,
    const struct bj_mat4x4* restrict M,
    bj_real                 a
) {
 
    const bj_real s = bj_sin(a), c = bj_cos(a);
    const bj_real* m = M->m; bj_real* o = out->m;
 
    const bj_real* m0 = &m[BJ_M4(0,0)];
    const bj_real* m1 = &m[BJ_M4(1,0)];
    const bj_real* m2 = &m[BJ_M4(2,0)];
    const bj_real* m3 = &m[BJ_M4(3,0)];
 
    bj_real* o0 = &o[BJ_M4(0,0)];
    bj_real* o1 = &o[BJ_M4(1,0)];
    bj_real* o2 = &o[BJ_M4(2,0)];
    bj_real* o3 = &o[BJ_M4(3,0)];
 
    for (int r = 0; r < 4; ++r) {
        const bj_real M1 = m1[r], M2 = m2[r];
        o0[r] = m0[r];
        o1[r] =  c*M1 - s*M2;
        o2[r] =  s*M1 + c*M2;
        o3[r] = m3[r];
    }
}

static BJ_INLINE void bj_mat4_rotate_y(
    struct bj_mat4x4* restrict       out,
    const struct bj_mat4x4* restrict M,
    bj_real                 a
) {
    const bj_real s = bj_sin(a), c = bj_cos(a);
    const bj_real* m = M->m; bj_real* o = out->m;
 
    const bj_real* m0 = &m[BJ_M4(0,0)];
    const bj_real* m1 = &m[BJ_M4(1,0)];
    const bj_real* m2 = &m[BJ_M4(2,0)];
    const bj_real* m3 = &m[BJ_M4(3,0)];
 
    bj_real* o0 = &o[BJ_M4(0,0)];
    bj_real* o1 = &o[BJ_M4(1,0)];
    bj_real* o2 = &o[BJ_M4(2,0)];
    bj_real* o3 = &o[BJ_M4(3,0)];
 
    for (int r = 0; r < 4; ++r) {
        const bj_real M0 = m0[r], M2 = m2[r];
        o0[r] =  c*M0 + s*M2;
        o1[r] =  m1[r];
        o2[r] = -s*M0 + c*M2;
        o3[r] =  m3[r];
    }
}

static BJ_INLINE void bj_mat4_rotate_z(
    struct bj_mat4x4* restrict       out,
    const struct bj_mat4x4* restrict M,
    bj_real                 a
) {
    const bj_real s = bj_sin(a), c = bj_cos(a);
    const bj_real* m = M->m; bj_real* o = out->m;
 
    const bj_real* m0 = &m[BJ_M4(0,0)];
    const bj_real* m1 = &m[BJ_M4(1,0)];
    const bj_real* m2 = &m[BJ_M4(2,0)];
    const bj_real* m3 = &m[BJ_M4(3,0)];
 
    bj_real* o0 = &o[BJ_M4(0,0)];
    bj_real* o1 = &o[BJ_M4(1,0)];
    bj_real* o2 = &o[BJ_M4(2,0)];
    bj_real* o3 = &o[BJ_M4(3,0)];
 
    for (int r = 0; r < 4; ++r) {
        const bj_real M0 = m0[r], M1 = m1[r];
        o0[r] =  c*M0 - s*M1;
        o1[r] =  s*M0 + c*M1;
        o2[r] =  m2[r];
        o3[r] =  m3[r];
    }
}

static BJ_INLINE void bj_mat4_rotate_arcball(
    struct bj_mat4x4* BJ_RESTRICT       R,
    const struct bj_mat4x4* BJ_RESTRICT M,
    struct bj_vec2                    a,
    struct bj_vec2                    b,
    bj_real                    s
) {
    bj_real z_a = BJ_FZERO;
    bj_real z_b = BJ_FZERO;
    const bj_real la = bj_vec2_len(a);
    const bj_real lb = bj_vec2_len(b);
    if (la < BJ_F(1.0)) {
        z_a = bj_sqrt(BJ_F(1.0) - bj_vec2_dot(a,a));
    } else {
        a = bj_vec2_normalize(a);
    }
    if (lb < BJ_F(1.0)) {
        z_b = bj_sqrt(BJ_F(1.0) - bj_vec2_dot(b,b));
    } else {
        b = bj_vec2_normalize(b);
    }
    const struct bj_vec3 A = { a.x, a.y, z_a };
    const struct bj_vec3 B = { b.x, b.y, z_b };
    const struct bj_vec3 C = bj_vec3_cross(A,B);
    const bj_real ang = bj_acos(bj_vec3_dot(A,B)) * s;
    bj_mat4_rotate_axis_angle(R, M, C, ang);
}

static BJ_INLINE bj_bool bj_mat4_invert(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT M
) {
    const bj_real* m = M->m;
    bj_real s[6], c[6];
 
    s[0] = m[BJ_M4(0,0)] * m[BJ_M4(1,1)] - m[BJ_M4(1,0)] * m[BJ_M4(0,1)];
    s[1] = m[BJ_M4(0,0)] * m[BJ_M4(1,2)] - m[BJ_M4(1,0)] * m[BJ_M4(0,2)];
    s[2] = m[BJ_M4(0,0)] * m[BJ_M4(1,3)] - m[BJ_M4(1,0)] * m[BJ_M4(0,3)];
    s[3] = m[BJ_M4(0,1)] * m[BJ_M4(1,2)] - m[BJ_M4(1,1)] * m[BJ_M4(0,2)];
    s[4] = m[BJ_M4(0,1)] * m[BJ_M4(1,3)] - m[BJ_M4(1,1)] * m[BJ_M4(0,3)];
    s[5] = m[BJ_M4(0,2)] * m[BJ_M4(1,3)] - m[BJ_M4(1,2)] * m[BJ_M4(0,3)];
 
    c[0] = m[BJ_M4(2,0)] * m[BJ_M4(3,1)] - m[BJ_M4(3,0)] * m[BJ_M4(2,1)];
    c[1] = m[BJ_M4(2,0)] * m[BJ_M4(3,2)] - m[BJ_M4(3,0)] * m[BJ_M4(2,2)];
    c[2] = m[BJ_M4(2,0)] * m[BJ_M4(3,3)] - m[BJ_M4(3,0)] * m[BJ_M4(2,3)];
    c[3] = m[BJ_M4(2,1)] * m[BJ_M4(3,2)] - m[BJ_M4(3,1)] * m[BJ_M4(2,2)];
    c[4] = m[BJ_M4(2,1)] * m[BJ_M4(3,3)] - m[BJ_M4(3,1)] * m[BJ_M4(2,3)];
    c[5] = m[BJ_M4(2,2)] * m[BJ_M4(3,3)] - m[BJ_M4(3,2)] * m[BJ_M4(2,3)];
 
    const bj_real det = s[0]*c[5] - s[1]*c[4] + s[2]*c[3]
                      + s[3]*c[2] - s[4]*c[1] + s[5]*c[0];
 
    if (bj_real_is_zero(det)) {
        return BJ_FALSE;
    }
    const bj_real id = BJ_F(1.0) / det;
    bj_real* o = out->m;
 
    o[BJ_M4(0,0)] = ( m[BJ_M4(1,1)] * c[5] - m[BJ_M4(1,2)] * c[4] + m[BJ_M4(1,3)] * c[3]) * id;
    o[BJ_M4(0,1)] = (-m[BJ_M4(0,1)] * c[5] + m[BJ_M4(0,2)] * c[4] - m[BJ_M4(0,3)] * c[3]) * id;
    o[BJ_M4(0,2)] = ( m[BJ_M4(3,1)] * s[5] - m[BJ_M4(3,2)] * s[4] + m[BJ_M4(3,3)] * s[3]) * id;
    o[BJ_M4(0,3)] = (-m[BJ_M4(2,1)] * s[5] + m[BJ_M4(2,2)] * s[4] - m[BJ_M4(2,3)] * s[3]) * id;
 
    o[BJ_M4(1,0)] = (-m[BJ_M4(1,0)] * c[5] + m[BJ_M4(1,2)] * c[2] - m[BJ_M4(1,3)] * c[1]) * id;
    o[BJ_M4(1,1)] = ( m[BJ_M4(0,0)] * c[5] - m[BJ_M4(0,2)] * c[2] + m[BJ_M4(0,3)] * c[1]) * id;
    o[BJ_M4(1,2)] = (-m[BJ_M4(3,0)] * s[5] + m[BJ_M4(3,2)] * s[2] - m[BJ_M4(3,3)] * s[1]) * id;
    o[BJ_M4(1,3)] = ( m[BJ_M4(2,0)] * s[5] - m[BJ_M4(2,2)] * s[2] + m[BJ_M4(2,3)] * s[1]) * id;
 
    o[BJ_M4(2,0)] = ( m[BJ_M4(1,0)] * c[4] - m[BJ_M4(1,1)] * c[2] + m[BJ_M4(1,3)] * c[0]) * id;
    o[BJ_M4(2,1)] = (-m[BJ_M4(0,0)] * c[4] + m[BJ_M4(0,1)] * c[2] - m[BJ_M4(0,3)] * c[0]) * id;
    o[BJ_M4(2,2)] = ( m[BJ_M4(3,0)] * s[4] - m[BJ_M4(3,1)] * s[2] + m[BJ_M4(3,3)] * s[0]) * id;
    o[BJ_M4(2,3)] = (-m[BJ_M4(2,0)] * s[4] + m[BJ_M4(2,1)] * s[2] - m[BJ_M4(2,3)] * s[0]) * id;
 
    o[BJ_M4(3,0)] = (-m[BJ_M4(1,0)] * c[3] + m[BJ_M4(1,1)] * c[1] - m[BJ_M4(1,2)] * c[0]) * id;
    o[BJ_M4(3,1)] = ( m[BJ_M4(0,0)] * c[3] - m[BJ_M4(0,1)] * c[1] + m[BJ_M4(0,2)] * c[0]) * id;
    o[BJ_M4(3,2)] = (-m[BJ_M4(3,0)] * s[3] + m[BJ_M4(3,1)] * s[1] - m[BJ_M4(3,2)] * s[0]) * id;
    o[BJ_M4(3,3)] = ( m[BJ_M4(2,0)] * s[3] - m[BJ_M4(2,1)] * s[1] + m[BJ_M4(2,2)] * s[0]) * id;
 
    return BJ_TRUE;
}

static BJ_INLINE void bj_mat4_invert_unsafe(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT M
) {
    const bj_real* m = M->m;
    bj_real s[6], c[6];
 
    s[0] = m[BJ_M4(0,0)] * m[BJ_M4(1,1)] - m[BJ_M4(1,0)] * m[BJ_M4(0,1)];
    s[1] = m[BJ_M4(0,0)] * m[BJ_M4(1,2)] - m[BJ_M4(1,0)] * m[BJ_M4(0,2)];
    s[2] = m[BJ_M4(0,0)] * m[BJ_M4(1,3)] - m[BJ_M4(1,0)] * m[BJ_M4(0,3)];
    s[3] = m[BJ_M4(0,1)] * m[BJ_M4(1,2)] - m[BJ_M4(1,1)] * m[BJ_M4(0,2)];
    s[4] = m[BJ_M4(0,1)] * m[BJ_M4(1,3)] - m[BJ_M4(1,1)] * m[BJ_M4(0,3)];
    s[5] = m[BJ_M4(0,2)] * m[BJ_M4(1,3)] - m[BJ_M4(1,2)] * m[BJ_M4(0,3)];
 
    c[0] = m[BJ_M4(2,0)] * m[BJ_M4(3,1)] - m[BJ_M4(3,0)] * m[BJ_M4(2,1)];
    c[1] = m[BJ_M4(2,0)] * m[BJ_M4(3,2)] - m[BJ_M4(3,0)] * m[BJ_M4(2,2)];
    c[2] = m[BJ_M4(2,0)] * m[BJ_M4(3,3)] - m[BJ_M4(3,0)] * m[BJ_M4(2,3)];
    c[3] = m[BJ_M4(2,1)] * m[BJ_M4(3,2)] - m[BJ_M4(3,1)] * m[BJ_M4(2,2)];
    c[4] = m[BJ_M4(2,1)] * m[BJ_M4(3,3)] - m[BJ_M4(3,1)] * m[BJ_M4(2,3)];
    c[5] = m[BJ_M4(2,2)] * m[BJ_M4(3,3)] - m[BJ_M4(3,2)] * m[BJ_M4(2,3)];
 
    const bj_real id = BJ_F(1.0) /
        (s[0]*c[5] - s[1]*c[4] + s[2]*c[3] + s[3]*c[2] - s[4]*c[1] + s[5]*c[0]);
 
    bj_real* o = out->m;
 
    o[BJ_M4(0,0)] = ( m[BJ_M4(1,1)] * c[5] - m[BJ_M4(1,2)] * c[4] + m[BJ_M4(1,3)] * c[3]) * id;
    o[BJ_M4(0,1)] = (-m[BJ_M4(0,1)] * c[5] + m[BJ_M4(0,2)] * c[4] - m[BJ_M4(0,3)] * c[3]) * id;
    o[BJ_M4(0,2)] = ( m[BJ_M4(3,1)] * s[5] - m[BJ_M4(3,2)] * s[4] + m[BJ_M4(3,3)] * s[3]) * id;
    o[BJ_M4(0,3)] = (-m[BJ_M4(2,1)] * s[5] + m[BJ_M4(2,2)] * s[4] - m[BJ_M4(2,3)] * s[3]) * id;
 
    o[BJ_M4(1,0)] = (-m[BJ_M4(1,0)] * c[5] + m[BJ_M4(1,2)] * c[2] - m[BJ_M4(1,3)] * c[1]) * id;
    o[BJ_M4(1,1)] = ( m[BJ_M4(0,0)] * c[5] - m[BJ_M4(0,2)] * c[2] + m[BJ_M4(0,3)] * c[1]) * id;
    o[BJ_M4(1,2)] = (-m[BJ_M4(3,0)] * s[5] + m[BJ_M4(3,2)] * s[2] - m[BJ_M4(3,3)] * s[1]) * id;
    o[BJ_M4(1,3)] = ( m[BJ_M4(2,0)] * s[5] - m[BJ_M4(2,2)] * s[2] + m[BJ_M4(2,3)] * s[1]) * id;
 
    o[BJ_M4(2,0)] = ( m[BJ_M4(1,0)] * c[4] - m[BJ_M4(1,1)] * c[2] + m[BJ_M4(1,3)] * c[0]) * id;
    o[BJ_M4(2,1)] = (-m[BJ_M4(0,0)] * c[4] + m[BJ_M4(0,1)] * c[2] - m[BJ_M4(0,3)] * c[0]) * id;
    o[BJ_M4(2,2)] = ( m[BJ_M4(3,0)] * s[4] - m[BJ_M4(3,1)] * s[2] + m[BJ_M4(3,3)] * s[0]) * id;
    o[BJ_M4(2,3)] = (-m[BJ_M4(2,0)] * s[4] + m[BJ_M4(2,1)] * s[2] - m[BJ_M4(2,3)] * s[0]) * id;
 
    o[BJ_M4(3,0)] = (-m[BJ_M4(1,0)] * c[3] + m[BJ_M4(1,1)] * c[1] - m[BJ_M4(1,2)] * c[0]) * id;
    o[BJ_M4(3,1)] = ( m[BJ_M4(0,0)] * c[3] - m[BJ_M4(0,1)] * c[1] + m[BJ_M4(0,2)] * c[0]) * id;
    o[BJ_M4(3,2)] = (-m[BJ_M4(3,0)] * s[3] + m[BJ_M4(3,1)] * s[1] - m[BJ_M4(3,2)] * s[0]) * id;
    o[BJ_M4(3,3)] = ( m[BJ_M4(2,0)] * s[3] - m[BJ_M4(2,1)] * s[1] + m[BJ_M4(2,2)] * s[0]) * id;
}

static BJ_INLINE void bj_mat4_orthonormalize(
    struct bj_mat4x4* BJ_RESTRICT       out,
    const struct bj_mat4x4* BJ_RESTRICT A
) {
    bj_mat4_copy(out, A);
    struct bj_vec3 z = { 
        out->m[BJ_M4(2,0)],
        out->m[BJ_M4(2,1)],
        out->m[BJ_M4(2,2)] 
    };
    z = bj_vec3_normalize(z);
 
    struct bj_vec3 y = { 
        out->m[BJ_M4(1,0)],
        out->m[BJ_M4(1,1)],
        out->m[BJ_M4(1,2)] 
    };
    y = bj_vec3_sub(y, bj_vec3_scale(z, bj_vec3_dot(y,z)));
    y = bj_vec3_normalize(y);
 
    struct bj_vec3 x = {
        out->m[BJ_M4(0,0)],
        out->m[BJ_M4(0,1)],
        out->m[BJ_M4(0,2)] 
    };
    x = bj_vec3_sub(x, bj_vec3_scale(z, bj_vec3_dot(x,z)));
    x = bj_vec3_sub(x, bj_vec3_scale(y, bj_vec3_dot(x,y)));
    x = bj_vec3_normalize(x);
 
    out->m[BJ_M4(0,0)] = x.x;
    out->m[BJ_M4(0,1)] = x.y;
    out->m[BJ_M4(0,2)] = x.z;
    out->m[BJ_M4(1,0)] = y.x;
    out->m[BJ_M4(1,1)] = y.y;
    out->m[BJ_M4(1,2)] = y.z;
    out->m[BJ_M4(2,0)] = z.x;
    out->m[BJ_M4(2,1)] = z.y;
    out->m[BJ_M4(2,2)] = z.z;
}

static BJ_INLINE void bj_mat4_set_frustum(
    struct bj_mat4x4* BJ_RESTRICT M,
    bj_real              l,
    bj_real              r,
    bj_real              b,
    bj_real              t,
    bj_real              n,
    bj_real              f
) {
 
    bj_real* m = M->m;
    m[BJ_M4(0,0)] = BJ_F(2.0)*n/(r-l);
    m[BJ_M4(0,1)] = m[BJ_M4(0,2)] = m[BJ_M4(0,3)] = BJ_FZERO;
 
    m[BJ_M4(1,0)] = BJ_FZERO;
    m[BJ_M4(1,1)] = BJ_F(-2.0)*n/(t-b);
    m[BJ_M4(1,2)] = m[BJ_M4(1,3)] = BJ_FZERO;
 
    m[BJ_M4(2,0)] = (r+l)/(r-l);
    m[BJ_M4(2,1)] = (t+b)/(t-b);
    m[BJ_M4(2,2)] =  f/(f-n);
    m[BJ_M4(2,3)] =  BJ_F(1.0);
 
    m[BJ_M4(3,0)] = BJ_FZERO; m[BJ_M4(3,1)] = BJ_FZERO;
    m[BJ_M4(3,2)] = -(f*n)/(f-n);
    m[BJ_M4(3,3)] = BJ_FZERO;
}

static BJ_INLINE void bj_mat4_set_ortho(
    struct bj_mat4x4* BJ_RESTRICT M,
    bj_real              l,
    bj_real              r,
    bj_real              b,
    bj_real              t,
    bj_real              n,
    bj_real              f
) {
 
    bj_real* m = M->m;
    m[BJ_M4(0,0)] = BJ_F(2.0)/(r-l);
    m[BJ_M4(0,1)] = m[BJ_M4(0,2)] = m[BJ_M4(0,3)] = BJ_FZERO;
 
    m[BJ_M4(1,0)] = m[BJ_M4(1,2)] = m[BJ_M4(1,3)] = BJ_FZERO;
    m[BJ_M4(1,1)] = BJ_F(-2.0)/(t-b);
 
    m[BJ_M4(2,0)] = m[BJ_M4(2,1)] = m[BJ_M4(2,3)] = BJ_FZERO;
    m[BJ_M4(2,2)] = BJ_F(1.0)/(f-n);
 
    m[BJ_M4(3,0)] = -(r+l)/(r-l);
    m[BJ_M4(3,1)] =  (t+b)/(t-b);
    m[BJ_M4(3,2)] = -n/(f-n);
    m[BJ_M4(3,3)] = BJ_F(1.0);
}
 

static BJ_INLINE void bj_mat4_set_perspective(
    struct bj_mat4x4* BJ_RESTRICT M,
    bj_real              y_fov,
    bj_real              aspect,
    bj_real              n,
    bj_real              f
) {
 
    const bj_real a = BJ_F(1.0)/bj_tan(y_fov/BJ_F(2.0));
    bj_real* m = M->m;
    m[BJ_M4(0,0)] = a/aspect;
    m[BJ_M4(0,1)] = m[BJ_M4(0,2)] = m[BJ_M4(0,3)] = BJ_FZERO;
 
    m[BJ_M4(1,0)] = BJ_FZERO;
    m[BJ_M4(1,1)] = -a;
    m[BJ_M4(1,2)] = m[BJ_M4(1,3)] = BJ_FZERO;
 
    m[BJ_M4(2,0)] = BJ_FZERO; m[BJ_M4(2,1)] = BJ_FZERO;
    m[BJ_M4(2,2)] = f/(f-n);
    m[BJ_M4(2,3)] = BJ_F(1.0);
 
    m[BJ_M4(3,0)] = BJ_FZERO; m[BJ_M4(3,1)] = BJ_FZERO;
    m[BJ_M4(3,2)] = -(f*n)/(f-n);
    m[BJ_M4(3,3)] = BJ_FZERO;
}

static BJ_INLINE void bj_mat4_set_viewport(
    struct bj_mat4x4* BJ_RESTRICT M,
    bj_real              x,
    bj_real              y,
    bj_real              w,
    bj_real              h
) {
    const bj_real zmin = BJ_FZERO;
    const bj_real zmax = BJ_F(1.0);
    bj_real* m = M->m;
    m[BJ_M4(0,0)] = BJ_F(0.5) * w;
    m[BJ_M4(0,1)] = BJ_FZERO;
    m[BJ_M4(0,2)] = BJ_FZERO;
    m[BJ_M4(0,3)] = BJ_FZERO;
    m[BJ_M4(1,0)] = BJ_FZERO;
    m[BJ_M4(1,1)] = BJ_F(0.5) * h;
    m[BJ_M4(1,2)] = BJ_FZERO;
    m[BJ_M4(1,3)] = BJ_FZERO;
    m[BJ_M4(2,0)] = BJ_FZERO;
    m[BJ_M4(2,1)] = BJ_FZERO;
    m[BJ_M4(2,2)] = (zmax - zmin);
    m[BJ_M4(2,3)] = BJ_FZERO;
    m[BJ_M4(3,0)] = x + BJ_F(0.5) * w;
    m[BJ_M4(3,1)] = y + BJ_F(0.5) * h;
    m[BJ_M4(3,2)] = zmin;
    m[BJ_M4(3,3)] = BJ_F(1.0);
}

static BJ_INLINE void bj_mat4_set_lookat(
    struct bj_mat4x4* BJ_RESTRICT M,
    struct bj_vec3              eye,
    struct bj_vec3              center,
    struct bj_vec3              up
) {
    struct bj_vec3 f = bj_vec3_normalize(bj_vec3_sub(center, eye));
    struct bj_vec3 s = bj_vec3_normalize(bj_vec3_cross(up, f));
    struct bj_vec3 t = bj_vec3_cross(f, s);
 
    bj_mat4_set_identity(M);
    M->m[BJ_M4(0,0)] = s.x;
    M->m[BJ_M4(0,1)] = t.x;
    M->m[BJ_M4(0,2)] = f.x;
    M->m[BJ_M4(1,0)] = s.y;
    M->m[BJ_M4(1,1)] = t.y;
    M->m[BJ_M4(1,2)] = f.y;
    M->m[BJ_M4(2,0)] = s.z;
    M->m[BJ_M4(2,1)] = t.z;
    M->m[BJ_M4(2,2)] = f.z;
    bj_mat4_translate(M, -eye.x, -eye.y, -eye.z);
}

static BJ_INLINE void bj_mat4x3_set_identity(
    struct bj_mat4x3* BJ_RESTRICT M
) {
    bj_real* m = M->m;
    m[BJ_M43(0,0)] = BJ_F(1.0);
    m[BJ_M43(0,1)] = BJ_FZERO;
    m[BJ_M43(0,2)] = BJ_FZERO;
    m[BJ_M43(1,0)] = BJ_FZERO;
    m[BJ_M43(1,1)] = BJ_F(1.0);
    m[BJ_M43(1,2)] = BJ_FZERO;
    m[BJ_M43(2,0)] = BJ_FZERO;
    m[BJ_M43(2,1)] = BJ_FZERO;
    m[BJ_M43(2,2)] = BJ_F(1.0);
    m[BJ_M43(3,0)] = BJ_FZERO;
    m[BJ_M43(3,1)] = BJ_FZERO;
    m[BJ_M43(3,2)] = BJ_FZERO;
}

static BJ_INLINE void bj_mat4x3_set_translation(
    struct bj_mat4x3* BJ_RESTRICT M,
    bj_real                tx,
    bj_real                ty,
    bj_real                tz
) {
    bj_mat4x3_set_identity(M);
    M->m[BJ_M43(3,0)] = tx;
    M->m[BJ_M43(3,1)] = ty;
    M->m[BJ_M43(3,2)] = tz;
}

static BJ_INLINE void bj_mat4x3_set_scaling_xyz(
    struct bj_mat4x3* BJ_RESTRICT M,
    bj_real                sx,
    bj_real                sy,
    bj_real                sz
) {
    bj_mat4x3_set_identity(M);
    M->m[BJ_M43(0,0)] = sx;
    M->m[BJ_M43(1,1)] = sy;
    M->m[BJ_M43(2,2)] = sz;
}

static BJ_INLINE void bj_mat4x3_set_rotation_x(
    struct bj_mat4x3* BJ_RESTRICT M,
    bj_real                a
) {
    const bj_real c = bj_cos(a), s = bj_sin(a);
    bj_mat4x3_set_identity(M);
    M->m[BJ_M43(1,1)] = c; M->m[BJ_M43(1,2)] = s;
    M->m[BJ_M43(2,1)]  = -s; M->m[BJ_M43(2,2)] = c;
}

static BJ_INLINE void bj_mat4x3_set_rotation_y(
    struct bj_mat4x3* BJ_RESTRICT M,
    bj_real                a
) {
    const bj_real c = bj_cos(a), s = bj_sin(a);
    bj_mat4x3_set_identity(M);
    M->m[BJ_M43(0,0)] = c; M->m[BJ_M43(0,2)] = -s;
    M->m[BJ_M43(2,0)] = s; M->m[BJ_M43(2,2)] =  c;
}

static BJ_INLINE void bj_mat4x3_set_rotation_z(
    struct bj_mat4x3* BJ_RESTRICT M,
    bj_real                a
) {
    const bj_real c = bj_cos(a), s = bj_sin(a);
    bj_mat4x3_set_identity(M);
    M->m[BJ_M43(0,0)] = c; M->m[BJ_M43(0,1)] = s;
    M->m[BJ_M43(1,0)]  = -s; M->m[BJ_M43(1,1)] = c;
}

static BJ_INLINE void bj_mat4x3_mul(
    struct bj_mat4x3* BJ_RESTRICT       out,
    const struct bj_mat4x3* BJ_RESTRICT A,
    const struct bj_mat4x3* BJ_RESTRICT B
){
 
    const bj_real a00 = A->m[BJ_M43(0,0)];
    const bj_real a10 = A->m[BJ_M43(0,1)];
    const bj_real a20 = A->m[BJ_M43(0,2)];
    const bj_real a30 = A->m[BJ_M43(3,0)];
    const bj_real a01 = A->m[BJ_M43(1,0)];
    const bj_real a11 = A->m[BJ_M43(1,1)];
    const bj_real a21 = A->m[BJ_M43(1,2)];
    const bj_real a31 = A->m[BJ_M43(3,1)];
    const bj_real a02 = A->m[BJ_M43(2,0)];
    const bj_real a12 = A->m[BJ_M43(2,1)];
    const bj_real a22 = A->m[BJ_M43(2,2)];
    const bj_real a32 = A->m[BJ_M43(3,2)];
 
    const bj_real b00 = B->m[BJ_M43(0,0)];
    const bj_real b01 = B->m[BJ_M43(1,0)];
    const bj_real bt0 = B->m[BJ_M43(3,0)];
    const bj_real b10 = B->m[BJ_M43(0,1)];
    const bj_real b11 = B->m[BJ_M43(1,1)];
    const bj_real bt1 = B->m[BJ_M43(3,1)];
    const bj_real b20 = B->m[BJ_M43(0,2)];
    const bj_real b21 = B->m[BJ_M43(1,2)];
    const bj_real b22 = B->m[BJ_M43(2,2)];
    const bj_real b02 = B->m[BJ_M43(2,0)];
    const bj_real b12 = B->m[BJ_M43(2,1)];
    const bj_real bt2 = B->m[BJ_M43(3,2)];
 
    out->m[BJ_M43(0,0)] = a00 * b00 + a01 * b10 + a02 * b20;
    out->m[BJ_M43(0,1)] = a10 * b00 + a11 * b10 + a12 * b20;
    out->m[BJ_M43(0,2)] = a20 * b00 + a21 * b10 + a22 * b20;
 
    out->m[BJ_M43(1,0)] = a00 * b01 + a01 * b11 + a02 * b21;
    out->m[BJ_M43(1,1)] = a10 * b01 + a11 * b11 + a12 * b21;
    out->m[BJ_M43(1,2)] = a20 * b01 + a21 * b11 + a22 * b21;
 
    out->m[BJ_M43(2,0)] = a00 * b02 + a01 * b12 + a02 * b22;
    out->m[BJ_M43(2,1)] = a10 * b02 + a11 * b12 + a12 * b22;
    out->m[BJ_M43(2,2)] = a20 * b02 + a21 * b12 + a22 * b22;
 
    out->m[BJ_M43(3,0)] = a00 * bt0 + a01 * bt1 + a02 * bt2 + a30;
    out->m[BJ_M43(3,1)] = a10 * bt0 + a11 * bt1 + a12 * bt2 + a31;
    out->m[BJ_M43(3,2)] = a20 * bt0 + a21 * bt1 + a22 * bt2 + a32;
}

static BJ_INLINE struct bj_vec3 bj_mat4x3_transform_point(
    const struct bj_mat4x3* BJ_RESTRICT M,
    struct bj_vec3                      p
){
    const bj_real* m = M->m; const bj_real x = p.x,y = p.y,z = p.z;
    return (struct bj_vec3){
        m[BJ_M43(0,0)]*x + m[BJ_M43(1,0)]*y + m[BJ_M43(2,0)]*z + m[BJ_M43(3,0)],
        m[BJ_M43(0,1)]*x + m[BJ_M43(1,1)]*y + m[BJ_M43(2,1)]*z + m[BJ_M43(3,1)],
        m[BJ_M43(0,2)]*x + m[BJ_M43(1,2)]*y + m[BJ_M43(2,2)]*z + m[BJ_M43(3,2)]
    };
}

static BJ_INLINE struct bj_vec3 bj_mat4x3_transform_dir(
    const struct bj_mat4x3* BJ_RESTRICT M,
    struct bj_vec3                      v
){
    const bj_real* m = M->m; const bj_real x = v.x,y = v.y,z = v.z;
    return (struct bj_vec3){
        m[BJ_M43(0,0)]*x + m[BJ_M43(1,0)]*y + m[BJ_M43(2,0)]*z,
        m[BJ_M43(0,1)]*x + m[BJ_M43(1,1)]*y + m[BJ_M43(2,1)]*z,
        m[BJ_M43(0,2)]*x + m[BJ_M43(1,2)]*y + m[BJ_M43(2,2)]*z
    };
}

static BJ_INLINE void bj_mat4_from_mat4x3(
    struct bj_mat4x4* BJ_RESTRICT         M,
    const struct bj_mat4x3* BJ_RESTRICT A
){
    const bj_real* a = A->m; bj_real* o = M->m;
    o[BJ_M4(0,0)] = a[BJ_M43(0,0)];
    o[BJ_M4(0,1)] = a[BJ_M43(0,1)];
    o[BJ_M4(0,2)] = a[BJ_M43(0,2)];
    o[BJ_M4(0,3)] = BJ_FZERO;
    o[BJ_M4(1,0)] = a[BJ_M43(1,0)];
    o[BJ_M4(1,1)] = a[BJ_M43(1,1)];
    o[BJ_M4(1,2)] = a[BJ_M43(1,2)];
    o[BJ_M4(1,3)] = BJ_FZERO;
    o[BJ_M4(2,0)] = a[BJ_M43(2,0)];
    o[BJ_M4(2,1)] = a[BJ_M43(2,1)];
    o[BJ_M4(2,2)] = a[BJ_M43(2,2)];
    o[BJ_M4(2,3)] = BJ_FZERO;
    o[BJ_M4(3,0)] = a[BJ_M43(3,0)];
    o[BJ_M4(3,1)] = a[BJ_M43(3,1)];
    o[BJ_M4(3,2)] = a[BJ_M43(3,2)];
    o[BJ_M4(3,3)] = BJ_F(1.0);
}

static BJ_INLINE void bj_mat4x3_from_mat4(
    struct bj_mat4x3* BJ_RESTRICT     M,
    const struct bj_mat4x4* BJ_RESTRICT A
){
    const bj_real* a = A->m; bj_real* o = M->m;
    o[BJ_M43(0,0)] = a[BJ_M4(0,0)];
    o[BJ_M43(0,1)] = a[BJ_M4(0,1)];
    o[BJ_M43(0,2)] = a[BJ_M4(0,2)];
    o[BJ_M43(1,0)] = a[BJ_M4(1,0)];
    o[BJ_M43(1,1)] = a[BJ_M4(1,1)];
    o[BJ_M43(1,2)] = a[BJ_M4(1,2)];
    o[BJ_M43(2,0)] = a[BJ_M4(2,0)];
    o[BJ_M43(2,1)] = a[BJ_M4(2,1)];
    o[BJ_M43(2,2)] = a[BJ_M4(2,2)];
    o[BJ_M43(3,0)] = a[BJ_M4(3,0)];
    o[BJ_M43(3,1)] = a[BJ_M4(3,1)];
    o[BJ_M43(3,2)] = a[BJ_M4(3,2)];
}
 
#endif