matrix4_ptr.c

Go to the documentation of this file.

// SPDX-FileCopyrightText: 2025-2026 Igal Alkon
// SPDX-FileCopyrightText: 2026 ALKONTEK <git@alkontek.com>
// SPDX-License-Identifier: BSD-3-Clause
 
#include <string.h>
#include <vecmat.h>
#include "features/cpu.h"

void mat4_identity_ptr(matrix4 *res)
{
    memset(res->v, 0, sizeof(res->v));  // Zero out all 16 floats
    res->v[0] = 1.0f;
    res->v[5] = 1.0f;
    res->v[10] = 1.0f;
    res->v[15] = 1.0f;
}

VECMAT_SCALAR_API void mat4_mul_ptr_scalar(matrix4 *res, const matrix4 *a, const matrix4 *b)
{
    /* Column-major: C's column j is A times B's column j (matches mul_vec). */
    matrix4 tmp;
    for (int c = 0; c < 4; c++) {
        for (int r = 0; r < 4; r++) {
            vm_float_t s = 0.0f;
            for (int k = 0; k < 4; k++) {
                s += a->v[k * 4 + r] * b->v[c * 4 + k];
            }
            tmp.v[c * 4 + r] = s;
        }
    }
    *res = tmp;
}

VECMAT_SCALAR_API void mat4_transpose_ptr_scalar(matrix4 *res, const matrix4 *m)
{
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            res->v[i * 4 + j] = m->v[j * 4 + i];
        }
    }
}
 
void mat4_mul_ptr(matrix4 *res, const matrix4 *a, const matrix4 *b)
{
#ifdef VECMAT_RUNTIME_DISPATCH
    vm_cpu_init();
    mat4_mul_ptr_(res, a, b);
#else
    mat4_mul_ptr_scalar(res, a, b);
#endif
}
 
void mat4_transpose_ptr(matrix4 *res, const matrix4 *m)
{
#ifdef VECMAT_RUNTIME_DISPATCH
    vm_cpu_init();
    mat4_transpose_ptr_(res, m);
#else
    mat4_transpose_ptr_scalar(res, m);
#endif
}

void mat4_inverse_ptr(matrix4 *res, const matrix4 *m)
{
    vm_float_t inv[16];
 
    inv[0]  =  m->v[5] * m->v[10] * m->v[15] - m->v[5] * m->v[14] * m->v[11] - m->v[9] * m->v[6] * m->v[15] + m->v[9] * m->v[14] * m->v[7] + m->v[13] * m->v[6] * m->v[11] - m->v[13] * m->v[10] * m->v[7];
    inv[4]  = -m->v[4] * m->v[10] * m->v[15] + m->v[4] * m->v[14] * m->v[11] + m->v[8] * m->v[6] * m->v[15] - m->v[8] * m->v[14] * m->v[7] - m->v[12] * m->v[6] * m->v[11] + m->v[12] * m->v[10] * m->v[7];
    inv[8]  =  m->v[4] * m->v[9]  * m->v[15] - m->v[4] * m->v[13] * m->v[11] - m->v[8] * m->v[5] * m->v[15] + m->v[8] * m->v[13] * m->v[7] + m->v[12] * m->v[5] * m->v[11] - m->v[12] * m->v[9]  * m->v[7];
    inv[12] = -m->v[4] * m->v[9]  * m->v[14] + m->v[4] * m->v[13] * m->v[10] + m->v[8] * m->v[5] * m->v[14] - m->v[8] * m->v[13] * m->v[6] - m->v[12] * m->v[5] * m->v[10] + m->v[12] * m->v[9]  * m->v[6];
    inv[1]  = -m->v[1] * m->v[10] * m->v[15] + m->v[1] * m->v[14] * m->v[11] + m->v[9] * m->v[2] * m->v[15] - m->v[9] * m->v[14] * m->v[3] - m->v[13] * m->v[2] * m->v[11] + m->v[13] * m->v[10] * m->v[3];
    inv[5]  =  m->v[0] * m->v[10] * m->v[15] - m->v[0] * m->v[14] * m->v[11] - m->v[8] * m->v[2] * m->v[15] + m->v[8] * m->v[14] * m->v[3] + m->v[12] * m->v[2] * m->v[11] - m->v[12] * m->v[10] * m->v[3];
    inv[9]  = -m->v[0] * m->v[9]  * m->v[15] + m->v[0] * m->v[13] * m->v[11] + m->v[8] * m->v[1] * m->v[15] - m->v[8] * m->v[13] * m->v[3] - m->v[12] * m->v[1] * m->v[11] + m->v[12] * m->v[9]  * m->v[3];
    inv[13] =  m->v[0] * m->v[9]  * m->v[14] - m->v[0] * m->v[13] * m->v[10] - m->v[8] * m->v[1] * m->v[14] + m->v[8] * m->v[13] * m->v[2] + m->v[12] * m->v[1] * m->v[10] - m->v[12] * m->v[9]  * m->v[2];
    inv[2]  =  m->v[1] * m->v[6]  * m->v[15] - m->v[1] * m->v[14] * m->v[7]  - m->v[5] * m->v[2] * m->v[15] + m->v[5] * m->v[14] * m->v[3] + m->v[13] * m->v[2] * m->v[7]  - m->v[13] * m->v[6]  * m->v[3];
    inv[6]  = -m->v[0] * m->v[6]  * m->v[15] + m->v[0] * m->v[14] * m->v[7]  + m->v[4] * m->v[2] * m->v[15] - m->v[4] * m->v[14] * m->v[3] - m->v[12] * m->v[2] * m->v[7]  + m->v[12] * m->v[6]  * m->v[3];
    inv[10] =  m->v[0] * m->v[5]  * m->v[15] - m->v[0] * m->v[13] * m->v[7]  - m->v[4] * m->v[1] * m->v[15] + m->v[4] * m->v[13] * m->v[3] + m->v[12] * m->v[1] * m->v[7]  - m->v[12] * m->v[5]  * m->v[3];
    inv[14] = -m->v[0] * m->v[5]  * m->v[14] + m->v[0] * m->v[13] * m->v[6]  + m->v[4] * m->v[1] * m->v[14] - m->v[4] * m->v[13] * m->v[2] - m->v[12] * m->v[1] * m->v[6]  + m->v[12] * m->v[5]  * m->v[2];
    inv[3]  = -m->v[1] * m->v[6]  * m->v[11] + m->v[1] * m->v[10] * m->v[7]  + m->v[5] * m->v[2] * m->v[11] - m->v[5] * m->v[10] * m->v[3] - m->v[9]  * m->v[2] * m->v[7]  + m->v[9]  * m->v[6]  * m->v[3];
    inv[7]  =  m->v[0] * m->v[6]  * m->v[11] - m->v[0] * m->v[10] * m->v[7]  - m->v[4] * m->v[2] * m->v[11] + m->v[4] * m->v[10] * m->v[3] + m->v[8]  * m->v[2] * m->v[7]  - m->v[8]  * m->v[6]  * m->v[3];
    inv[11] = -m->v[0] * m->v[5]  * m->v[11] + m->v[0] * m->v[9]  * m->v[7]  + m->v[4] * m->v[1] * m->v[11] - m->v[4] * m->v[9]  * m->v[3] - m->v[8]  * m->v[1] * m->v[7]  + m->v[8]  * m->v[5]  * m->v[3];
    inv[15] =  m->v[0] * m->v[5]  * m->v[10] - m->v[0] * m->v[9]  * m->v[6]  - m->v[4] * m->v[1] * m->v[10] + m->v[4] * m->v[9]  * m->v[2] + m->v[8]  * m->v[1] * m->v[6]  - m->v[8]  * m->v[5]  * m->v[2];
 
    vm_float_t det = m->v[0] * inv[0] + m->v[1] * inv[4] + m->v[2] * inv[8] + m->v[3] * inv[12];
    if (det == 0.0f) {
        res->v[0] = 1.0f;
        res->v[5] = 1.0f;
        res->v[10] = 1.0f;
        res->v[15] = 1.0f;
        return;
    }
 
    det = 1.0f / det;
 
    for (int i = 0; i < 16; i++) {
        res->v[i] = inv[i] * det;
    }
}

void mat4_translate_ptr(matrix4 *res, const vector3 *v)
{
    res->v[0] = 1.0f;
    res->v[1] = 0.0f;
    res->v[2] = 0.0f;
    res->v[3] = 0.0f;
    res->v[4] = 0.0f;
    res->v[5] = 1.0f;
    res->v[6] = 0.0f;
    res->v[7] = 0.0f;
    res->v[8] = 0.0f;
    res->v[9] = 0.0f;
    res->v[10] = 1.0f;
    res->v[11] = 0.0f;
    res->v[12] = v->x;
    res->v[13] = v->y;
    res->v[14] = v->z;
    res->v[15] = 1.0f;
}

void mat4_scale_ptr(matrix4 *res, const vector3 *v)
{
    memset(res->v, 0, sizeof(res->v));
    res->v[0] = v->x;
    res->v[5] = v->y;
    res->v[10] = v->z;
    res->v[15] = 1.0f;
}

void mat4_rotation_ptr(matrix4 *res, const vector3 *axis, const vm_float_t angle)
{
    const vector3 normalized_axis = vec3_normalize(*axis);
    const vm_float_t rad = deg_to_rad(angle);
    const vm_float_t c = VECMAT_COS(rad);
    const vm_float_t s = VECMAT_SIN(rad);
    const vm_float_t omc = 1.0f - c;
    const vm_float_t x = normalized_axis.x, y = normalized_axis.y, z = normalized_axis.z;
 
    memset(res->v, 0, sizeof(res->v));
    res->v[0] = x * x * omc + c;
    res->v[1] = x * y * omc + z * s;
    res->v[2] = x * z * omc - y * s;
    res->v[4] = x * y * omc - z * s;
    res->v[5] = y * y * omc + c;
    res->v[6] = y * z * omc + x * s;
    res->v[8] = x * z * omc + y * s;
    res->v[9] = y * z * omc - x * s;
    res->v[10] = z * z * omc + c;
    res->v[15] = 1.0f;
}

void mat4_rotation_x_ptr(matrix4 *res, const vm_float_t degrees)
{
    const vm_float_t rad = deg_to_rad(degrees);
    const vm_float_t c = VECMAT_COS(rad);
    const vm_float_t s = VECMAT_SIN(rad);
    mat4_identity_ptr(res);
    res->m22 = c;
    res->m32 = s;
    res->m23 = -s;
    res->m33 = c;
}

void mat4_rotation_y_ptr(matrix4 *res, const vm_float_t degrees)
{
    const vm_float_t rad = deg_to_rad(degrees);
    const vm_float_t c = VECMAT_COS(rad);
    const vm_float_t s = VECMAT_SIN(rad);
    mat4_identity_ptr(res);
    res->m11 = c;
    res->m31 = -s;
    res->m13 = s;
    res->m33 = c;
}

void mat4_rotation_z_ptr(matrix4 *res, const vm_float_t degrees)
{
    const vm_float_t rad = deg_to_rad(degrees);
    const vm_float_t c = VECMAT_COS(rad);
    const vm_float_t s = VECMAT_SIN(rad);
    mat4_identity_ptr(res);
    res->m11 = c;
    res->m21 = s;
    res->m12 = -s;
    res->m22 = c;
}

void mat4_from_mat3_ptr(matrix4 *res, const matrix3 *m)
{
    mat4_identity_ptr(res);
    res->m11 = m->m11; res->m21 = m->m21; res->m31 = m->m31;
    res->m12 = m->m12; res->m22 = m->m22; res->m32 = m->m32;
    res->m13 = m->m13; res->m23 = m->m23; res->m33 = m->m33;
}

void mat4_trs_ptr(matrix4 *res, const vector3 *translation, const quaternion *rotation, const vector3 *scale)
{
    quat_to_mat4_ptr(res, rotation);
    res->m11 *= scale->x; res->m21 *= scale->x; res->m31 *= scale->x;
    res->m12 *= scale->y; res->m22 *= scale->y; res->m32 *= scale->y;
    res->m13 *= scale->z; res->m23 *= scale->z; res->m33 *= scale->z;
    res->m14 = translation->x;
    res->m24 = translation->y;
    res->m34 = translation->z;
}

void mat4_extract_translation_ptr(vector3 *res, const matrix4 *m)
{
    res->x = m->m14;
    res->y = m->m24;
    res->z = m->m34;
}

void mat4_extract_scale_ptr(vector3 *res, const matrix4 *m)
{
    res->x = VECMAT_SQRT(m->m11 * m->m11 + m->m21 * m->m21 + m->m31 * m->m31);
    res->y = VECMAT_SQRT(m->m12 * m->m12 + m->m22 * m->m22 + m->m32 * m->m32);
    res->z = VECMAT_SQRT(m->m13 * m->m13 + m->m23 * m->m23 + m->m33 * m->m33);
}

void mat4_extract_rotation_ptr(quaternion *res, const matrix4 *m)
{
    vector3 s;
    mat4_extract_scale_ptr(&s, m);
    matrix3 r;
    r.m11 = (s.x != 0.0f) ? m->m11 / s.x : 0.0f;
    r.m21 = (s.x != 0.0f) ? m->m21 / s.x : 0.0f;
    r.m31 = (s.x != 0.0f) ? m->m31 / s.x : 0.0f;
    r.m12 = (s.y != 0.0f) ? m->m12 / s.y : 0.0f;
    r.m22 = (s.y != 0.0f) ? m->m22 / s.y : 0.0f;
    r.m32 = (s.y != 0.0f) ? m->m32 / s.y : 0.0f;
    r.m13 = (s.z != 0.0f) ? m->m13 / s.z : 0.0f;
    r.m23 = (s.z != 0.0f) ? m->m23 / s.z : 0.0f;
    r.m33 = (s.z != 0.0f) ? m->m33 / s.z : 0.0f;
    quat_from_mat3_ptr(res, &r);
}

void mat4_perspective_ptr(matrix4 *res, const vm_float_t fov, const vm_float_t aspect,
                          const vm_float_t near, const vm_float_t far)
{
    const vm_float_t rad = deg_to_rad(fov / 2.0f);
    const vm_float_t tan_half_fov = VECMAT_TAN(rad);
 
    for (int i = 0; i < 16; i++) res->v[i] = 0.0f;
    res->v[0] = 1.0f / (aspect * tan_half_fov);
    res->v[5] = 1.0f / tan_half_fov;
    res->v[10] = -(far + near) / (far - near);
    res->v[11] = -1.0f;
    res->v[14] = -2.0f * far * near / (far - near);
}

void mat4_ortho_ptr(matrix4 *res, const vm_float_t left, const vm_float_t right, const vm_float_t bottom,
                    const vm_float_t top, const vm_float_t near, const vm_float_t far)
{
    res->v[0]  = 2.0f / (right - left);
    res->v[5]  = 2.0f / (top - bottom);
    res->v[10] = -2.0f / (far - near);
    res->v[12] = -(right + left) / (right - left);
    res->v[13] = -(top + bottom) / (top - bottom);
    res->v[14] = -(far + near) / (far - near);
    res->v[15] = 1.0f;
}

void mat4_look_at_ptr(matrix4 *res, const vector3 *position, const vector3 *target, const vector3 *up)
{
    const vector3 forward = vec3_normalize(vec3_sub(*target, *position));
    const vector3 right = vec3_normalize(vec3_cross(forward, *up));
    const vector3 true_up = vec3_cross(right, forward);
 
    *res = mat4_identity();
    res->v[0] = right.x;
    res->v[1] = true_up.x;
    res->v[2] = -forward.x;
    res->v[4] = right.y;
    res->v[5] = true_up.y;
    res->v[6] = -forward.y;
    res->v[8] = right.z;
    res->v[9] = true_up.z;
    res->v[10] = -forward.z;
    res->v[12] = -vec3_dot(right, *position);
    res->v[13] = -vec3_dot(true_up, *position);
    res->v[14] = vec3_dot(forward, *position);
}

void mat4_perspective_fov_ptr(matrix4 *res, const vm_float_t fov, const vm_float_t w, const vm_float_t h,
                              const vm_float_t n, const vm_float_t f)
{
    const vm_float_t rad = deg_to_rad(fov / 2.0f);
    const vm_float_t tan_half_fov = VECMAT_TAN(rad);
    const vm_float_t aspect = w / h;
 
    for (int i = 0; i < 16; i++) res->v[i] = 0.0f;
    res->v[0] = 1.0f / (aspect * tan_half_fov);
    res->v[5] = 1.0f / tan_half_fov;
    res->v[10] = -(f + n) / (f - n);
    res->v[11] = -1.0f;
    res->v[14] = -2.0f * f * n / (f - n);
}

void mat4_perspective_infinite_ptr(matrix4 *res, vm_float_t const fov_y, vm_float_t const aspect, vm_float_t const n)
{
    const vm_float_t rad = deg_to_rad(fov_y / 2.0f);
    const vm_float_t tan_half_fov = VECMAT_TAN(rad);
    for (int i = 0; i < 16; i++) res->v[i] = 0.0f;
    res->v[0] = 1.0f / (aspect * tan_half_fov);
    res->v[5] = 1.0f / tan_half_fov;
    res->v[10] = -1.0f;
    res->v[11] = -1.0f;
    res->v[14] = -2.0f * n;
}

VECMAT_SCALAR_API void mat4_mul_vec4_ptr_scalar(vector4 *res, const matrix4 *m, const vector4 *v)
{
    const vm_float_t x = v->x;
    const vm_float_t y = v->y;
    const vm_float_t z = v->z;
    const vm_float_t w = v->w;
    res->x = m->m11 * x + m->m12 * y + m->m13 * z + m->m14 * w;
    res->y = m->m21 * x + m->m22 * y + m->m23 * z + m->m24 * w;
    res->z = m->m31 * x + m->m32 * y + m->m33 * z + m->m34 * w;
    res->w = m->m41 * x + m->m42 * y + m->m43 * z + m->m44 * w;
}
 
void mat4_mul_vec4_ptr(vector4 *res, const matrix4 *m, const vector4 *v)
{
#ifdef VECMAT_RUNTIME_DISPATCH
    vm_cpu_init();
    mat4_mul_vec4_ptr_(res, m, v);
#else
    mat4_mul_vec4_ptr_scalar(res, m, v);
#endif
}

VECMAT_SCALAR_API void mat4_mul_vec3_ptr_scalar(vector3 *res, const matrix4 *m, const vector3 *v, const vm_float_t w)
{
    const vm_float_t x = v->x;
    const vm_float_t y = v->y;
    const vm_float_t z = v->z;
    res->x = m->m11 * x + m->m12 * y + m->m13 * z + m->m14 * w;
    res->y = m->m21 * x + m->m22 * y + m->m23 * z + m->m24 * w;
    res->z = m->m31 * x + m->m32 * y + m->m33 * z + m->m34 * w;
}
 
void mat4_mul_vec3_ptr(vector3 *res, const matrix4 *m, const vector3 *v, const vm_float_t w)
{
#ifdef VECMAT_RUNTIME_DISPATCH
    vm_cpu_init();
    mat4_mul_vec3_ptr_(res, m, v, w);
#else
    mat4_mul_vec3_ptr_scalar(res, m, v, w);
#endif
}