math_3d.h

Go to the documentation of this file.

/* This file is part of the 'atomes' software
 
'atomes' is free software: you can redistribute it and/or modify it under the terms
of the GNU Affero General Public License as published by the Free Software Foundation,
either version 3 of the License, or (at your option) any later version.
 
'atomes' is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU General Public License for more details.
 
You should have received a copy of the GNU Affero General Public License along with 'atomes'.
If not, see <https://www.gnu.org/licenses/>
 
Copyright (C) 2022-2024 by CNRS and University of Strasbourg */
 
/*
* This header file: 'math_3d.h'
*
* Contains:
 
 - Functions for OpenGL math
 
*/
 
/* The following was adapted from the file Math 3D v1.0
By Stephan Soller <stephan.soller@helionweb.de> and Tobias Malmsheimer
The original file was modified and completed to match 'atomes' needs. */
 
/*
Math 3D is a compact C99 library meant to be used with OpenGL. It provides basic
3D vector and 4x4 matrix operations as well as functions to create transformation
and projection matrices. The OpenGL binary layout is used so you can just upload
vectors and matrices into shaders and work with them without any conversions.
 
It's an stb style single header file library. Define MATH_3D_IMPLEMENTATION
before you include this file in *one* C file to create the implementation.
 
 
QUICK NOTES
 
- If not explicitly stated by a parameter name all angles are in radians.
- The matrices use column-major indices. This is the same as in OpenGL and GLSL.
  The matrix documentation below for details.
- Matrices are passed by value. This is probably a bit inefficient but
  simplifies code quite a bit. Most operations will be inlined by the compiler
  anyway so the difference shouldn't matter that much. A matrix fits into 4 of
  the 16 SSE2 registers anyway. If profiling shows significant slowdowns the
  matrix type might change but ease of use is more important than every last
  percent of performance.
- When combining matrices with multiplication the effects apply right to left.
  This is the convention used in mathematics and OpenGL. Source:
  https://en.wikipedia.org/wiki/Transformation_matrix#Composing_and_inverting_transformations
  Direct3D does it differently.
- The `m4_mul_pos()` and `m4_mul_dir()` functions do a correct perspective
  divide (division by w) when necessary. This is a bit slower but ensures that
  the functions will properly work with projection matrices. If profiling shows
  this is a bottleneck special functions without perspective division can be
  added. But the normal multiplications should avoid any surprises.
- The library consistently uses a right-handed coordinate system. The old
  `glOrtho()` broke that rule and `m4_ortho()` has be slightly modified so you
  can always think of right-handed cubes that are projected into OpenGLs
  normalized device coordinates.
- Special care has been taken to document all complex operations and important
  sources. Most code is covered by test cases that have been manually calculated
  and checked on the whiteboard. Since indices and math code is prone to be
  confusing we used pair programming to avoid mistakes.
 
 
FURTHER IDEAS
 
These are ideas for future work on the library. They're implemented as soon as
there is a proper use case and we can find good names for them.
 
- bool v3_is_null(vec3_t v, float epsilon)
  To check if the length of a vector is smaller than `epsilon`.
- vec3_t v3_length_default(vec3_t v, float default_length, float epsilon)
  Returns `default_length` if the length of `v` is smaller than `epsilon`.
  Otherwise same as `v3_length()`.
- vec3_t v3_norm_default(vec3_t v, vec3_t default_vector, float epsilon)
  Returns `default_vector` if the length of `v` is smaller than `epsilon`.
  Otherwise the same as `v3_norm()`.
- mat4_t m4_invert(mat4_t matrix)
  Matrix inversion that works with arbitrary matrices. `m4_invert_affine()` can
  already invert translation, rotation, scaling, mirroring, reflection and
  shearing matrices. So a general inversion might only be useful to invert
  projection matrices for picking. But with orthographic and perspective
  projection it's probably simpler to calculate the ray into the scene directly
  based on the screen coordinates.
 
 
VERSION HISTORY
 
v1.0  2016-02-15  Initial release
 
**/
 
#ifndef MATH_3D_H_
#define MATH_3D_H_
 
#include <math.h>
#include <stdio.h>
 
 
// Define PI directly because we would need to define the _BSD_SOURCE or
// _XOPEN_SOURCE feature test macros to get it from math.h. That would be a
// rather harsh dependency. So we define it directly if necessary.
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
 
 
//
// 3D vectors
//
// Use the `vec3()` function to create vectors. All other vector functions start
// with the `v3_` prefix.
//
// The binary layout is the same as in GLSL and everything else (just 3 floats).
// So you can just upload the vectors into shaders as they are.
//
 
typedef struct { float x, y, z; } vec3_t;
static inline vec3_t vec3(float x, float y, float z)        { return (vec3_t){ x, y, z }; }
static inline vec3_t v3_add   (vec3_t a, vec3_t b)          { return (vec3_t){ a.x + b.x, a.y + b.y, a.z + b.z }; }
static inline vec3_t v3_adds  (vec3_t a, float s)           { return (vec3_t){ a.x + s,   a.y + s,   a.z + s   }; }
static inline vec3_t v3_sub   (vec3_t a, vec3_t b)          { return (vec3_t){ a.x - b.x, a.y - b.y, a.z - b.z }; }
static inline vec3_t v3_subs  (vec3_t a, float s)           { return (vec3_t){ a.x - s,   a.y - s,   a.z - s   }; }
static inline vec3_t v3_mul   (vec3_t a, vec3_t b)          { return (vec3_t){ a.x * b.x, a.y * b.y, a.z * b.z }; }
static inline vec3_t v3_muls  (vec3_t a, float s)           { return (vec3_t){ a.x * s,   a.y * s,   a.z * s   }; }
static inline vec3_t v3_div   (vec3_t a, vec3_t b)          { return (vec3_t){ a.x / b.x, a.y / b.y, a.z / b.z }; }
static inline vec3_t v3_divs  (vec3_t a, float s)           { return (vec3_t){ a.x / s,   a.y / s,   a.z / s   }; }
static inline float  v3_length(vec3_t v)                    { return sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);          }
static inline vec3_t v3_norm  (vec3_t v);
static inline float  v3_dot   (vec3_t a, vec3_t b)          { return a.x*b.x + a.y*b.y + a.z*b.z; }
static inline vec3_t v3_proj  (vec3_t v, vec3_t onto);
static inline vec3_t v3_cross (vec3_t a, vec3_t b);
static inline float  v3_angle_between(vec3_t a, vec3_t b);
 
 
/* Sébastien Le Roux - quaternions - 6/02/2017 */
typedef struct { float w, x, y, z; } vec4_t;
static inline vec4_t vec4(float w, float x, float y, float z) { return (vec4_t){ w, x, y, z }; }
static inline vec4_t v4_add   (vec4_t a, vec4_t b)          { return (vec4_t){ a.w + b.w, a.x + b.x, a.y + b.y, a.z + b.z }; }
static inline vec4_t v4_adds  (vec4_t a, float s)           { return (vec4_t){ a.w + s, a.x + s,   a.y + s,   a.z + s     }; }
static inline vec4_t v4_sub   (vec4_t a, vec4_t b)          { return (vec4_t){ a.w - b.w, a.x - b.x, a.y - b.y, a.z - b.z }; }
static inline vec4_t v4_subs  (vec4_t a, float s)           { return (vec4_t){ a.w - s, a.x - s,   a.y - s,   a.z - s     }; }
static inline vec4_t v4_mul   (vec4_t a, vec4_t b)          { return (vec4_t){ a.w * b.w, a.x * b.x, a.y * b.y, a.z * b.z }; }
static inline vec4_t v4_muls  (vec4_t a, float s)           { return (vec4_t){ a.w * s, a.x * s,   a.y * s,   a.z * s     }; }
static inline vec4_t v4_div   (vec4_t a, vec4_t b)          { return (vec4_t){ a.w / b.w, a.x / b.x, a.y / b.y, a.z / b.z }; }
static inline vec4_t v4_divs  (vec4_t a, float s)           { return (vec4_t){ a.w / s, a.x / s,   a.y / s,   a.z / s     }; }
static inline float  v4_length(vec4_t v)                    { return sqrtf(v.w*v.w + v.x*v.x + v.y*v.y + v.z*v.z); }
static inline vec4_t v4_norm  (vec4_t v);
static inline vec4_t v4_mul   (vec4_t a, vec4_t b);
 
//
// 4x4 matrices
//
// Use the `mat4()` function to create a matrix. You can write the matrix
// members in the same way as you would write them on paper or on a whiteboard:
//
// mat4_t m = mat4(
//     1,  0,  0,  7,
//     0,  1,  0,  5,
//     0,  0,  1,  3,
//     0,  0,  0,  1
// )
//
// This creates a matrix that translates points by vec3(7, 5, 3). All other
// matrix functions start with the `m4_` prefix. Among them functions to create
// identity, translation, rotation, scaling and projection matrices.
//
// The matrix is stored in column-major order, just as OpenGL expects. Members
// can be accessed by indices or member names. When you write a matrix on paper
// or on the whiteboard the indices and named members correspond to these
// positions:
//
// | m[0][0]  m[1][0]  m[2][0]  m[3][0] |
// | m[0][1]  m[1][1]  m[2][1]  m[3][1] |
// | m[0][2]  m[1][2]  m[2][2]  m[3][2] |
// | m[0][3]  m[1][3]  m[2][3]  m[3][3] |
//
// | m00  m10  m20  m30 |
// | m01  m11  m21  m31 |
// | m02  m12  m22  m32 |
// | m03  m13  m23  m33 |
//
// The first index or number in a name denotes the column, the second the row.
// So m[i][j] denotes the member in the ith column and the jth row. This is the
// same as in GLSL (source: GLSL v1.3 specification, 5.6 Matrix Components).
//
 
typedef union {
    // The first index is the column index, the second the row index. The memory
    // layout of nested arrays in C matches the memory layout expected by OpenGL.
    float m[4][4];
    // OpenGL expects the first 4 floats to be the first column of the matrix.
    // So we need to define the named members column by column for the names to
    // match the memory locations of the array elements.
    struct {
        float m00, m01, m02, m03;
        float m10, m11, m12, m13;
        float m20, m21, m22, m23;
        float m30, m31, m32, m33;
    };
} mat4_t;
 
static inline mat4_t mat4(
    float m00, float m10, float m20, float m30,
    float m01, float m11, float m21, float m31,
    float m02, float m12, float m22, float m32,
    float m03, float m13, float m23, float m33
);
 
static inline mat4_t m4_identity     ();
static inline mat4_t m4_translation  (vec3_t offset);
static inline mat4_t m4_scaling      (vec3_t scale);
static inline mat4_t m4_rotation_x   (float angle_in_rad);
static inline mat4_t m4_rotation_y   (float angle_in_rad);
static inline mat4_t m4_rotation_z   (float angle_in_rad);
static inline mat4_t m4_rotation     (float angle_in_rad, vec3_t axis);
 
//
// Sébastien Le Roux - Quaternions -  6/02/2017
//
static inline vec4_t axis_to_quat (vec3_t axis, float angle_in_rad);
static inline mat4_t m4_quat_rotation (vec4_t q);
 
static inline mat4_t m4_ortho        (float left, float right, float bottom, float top, float back, float front);
static inline mat4_t m4_perspective  (float vertical_field_of_view_in_deg, float aspect_ratio, float near_view_distance, float far_view_distance);
static inline mat4_t m4_frustum      (float left, float right, float bottom, float top, float front, float back);
static inline mat4_t m4_look_at      (vec3_t from, vec3_t to, vec3_t up);
 
static inline mat4_t m4_transpose    (mat4_t matrix);
static inline mat4_t m4_mul          (mat4_t a, mat4_t b);
static inline mat4_t m43_mul(mat4_t a, mat4_t b);
static inline mat4_t m4_invert_affine(mat4_t matrix);
static inline mat4_t m4_translate    (mat4_t matrix, vec3_t translation);
static inline vec3_t m4_mul_pos      (mat4_t matrix, vec3_t position);
static inline vec3_t m4_mul_coord    (mat4_t matrix, vec3_t position);
static inline vec3_t m4_mul_abc      (mat4_t matrix, vec3_t position);
static inline vec3_t m4_mul_dir      (mat4_t matrix, vec3_t direction);
 
static inline void   m4_print        (mat4_t matrix);
static inline void   m4_printp       (mat4_t matrix, int width, int precision);
static inline void   m4_fprint       (FILE* stream, mat4_t matrix);
static inline void   m4_fprintp      (FILE* stream, mat4_t matrix, int width, int precision);
 
 
 
//
// 3D vector functions header implementation
//
 
static inline vec3_t v3_norm(vec3_t v) {
    float len = v3_length(v);
    if (len > 0.0)
        return (vec3_t){ v.x / len, v.y / len, v.z / len };
    else
        return (vec3_t){ 0, 0, 0};
}
 
static inline vec3_t v3_proj(vec3_t v, vec3_t onto) {
    return v3_muls(onto, v3_dot(v, onto) / v3_dot(onto, onto));
}
 
static inline vec3_t v3_cross(vec3_t a, vec3_t b) {
    return (vec3_t){
        a.y * b.z - a.z * b.y,
        a.z * b.x - a.x * b.z,
        a.x * b.y - a.y * b.x
    };
}
 
static inline float v3_angle_between(vec3_t a, vec3_t b) {
    return acosf( v3_dot(a, b) / (v3_length(a) * v3_length(b)) );
}
 
//
// Sébastien Le Roux - Quaternions -  6/02/2017
//
 
static inline vec4_t v4_norm(vec4_t v) {
    float len = v4_length(v);
    if (len > 0.0)
        return (vec4_t){ v.w / len, v.x / len, v.y / len , v.z / len};
    else
        return (vec4_t){ 0, 0, 0, 0};
}
 
static inline vec4_t q4_mul(vec4_t a, vec4_t b) {
    return (vec4_t){
        a.z * b.w + a.w * b.z + a.x * b.y - a.y * b.x,
        a.z * b.x + a.x * b.z + a.y * b.w - a.w * b.y,
        a.z * b.y + a.y * b.z + a.w * b.x - a.x * b.w,
        a.z * b.z - (a.w * b.w + a.x * b.x + a.y * b.y)};
}
 
 
//
// Matrix functions header implementation
//
 
static inline mat4_t mat4(
    float m00, float m10, float m20, float m30,
    float m01, float m11, float m21, float m31,
    float m02, float m12, float m22, float m32,
    float m03, float m13, float m23, float m33
) {
    return (mat4_t){
        .m[0][0] = m00, .m[1][0] = m10, .m[2][0] = m20, .m[3][0] = m30,
        .m[0][1] = m01, .m[1][1] = m11, .m[2][1] = m21, .m[3][1] = m31,
        .m[0][2] = m02, .m[1][2] = m12, .m[2][2] = m22, .m[3][2] = m32,
        .m[0][3] = m03, .m[1][3] = m13, .m[2][3] = m23, .m[3][3] = m33
    };
}
 
static inline mat4_t m4_identity() {
    return mat4(
         1,  0,  0,  0,
         0,  1,  0,  0,
         0,  0,  1,  0,
         0,  0,  0,  1
    );
}
 
static inline mat4_t m4_translation(vec3_t offset) {
    return mat4(
         1,  0,  0,  offset.x,
         0,  1,  0,  offset.y,
         0,  0,  1,  offset.z,
         0,  0,  0,  1
    );
}
 
static inline mat4_t m4_scaling(vec3_t scale) {
    float x = scale.x, y = scale.y, z = scale.z;
    return mat4(
         x,  0,  0,  0,
         0,  y,  0,  0,
         0,  0,  z,  0,
         0,  0,  0,  1
    );
}
 
static inline mat4_t m4_rotation_x(float angle_in_rad) {
    float s = sinf(angle_in_rad), c = cosf(angle_in_rad);
    return mat4(
        1,  0,  0,  0,
        0,  c, -s,  0,
        0,  s,  c,  0,
        0,  0,  0,  1
    );
}
 
static inline mat4_t m4_rotation_y(float angle_in_rad) {
    float s = sinf(angle_in_rad), c = cosf(angle_in_rad);
    return mat4(
         c,  0,  s,  0,
         0,  1,  0,  0,
        -s,  0,  c,  0,
         0,  0,  0,  1
    );
}
 
static inline mat4_t m4_rotation_z(float angle_in_rad) {
    float s = sinf(angle_in_rad), c = cosf(angle_in_rad);
    return mat4(
         c, -s,  0,  0,
         s,  c,  0,  0,
         0,  0,  1,  0,
         0,  0,  0,  1
    );
}
 
static inline mat4_t m4_transpose(mat4_t matrix) {
    return mat4(
        matrix.m00, matrix.m01, matrix.m02, matrix.m03,
        matrix.m10, matrix.m11, matrix.m12, matrix.m13,
        matrix.m20, matrix.m21, matrix.m22, matrix.m23,
        matrix.m30, matrix.m31, matrix.m32, matrix.m33
    );
}
 
static inline mat4_t m4_mul(mat4_t a, mat4_t b) {
    mat4_t result;
        int i, j, k;
    for(i = 0; i < 4; i++) {
        for(j = 0; j < 4; j++) {
            float sum = 0;
            for(k = 0; k < 4; k++) {
                sum += a.m[k][j] * b.m[i][k];
            }
            result.m[i][j] = sum;
        }
    }
 
    return result;
}
 
static inline mat4_t m43_mul(mat4_t a, mat4_t b) {
    mat4_t result;
        int i, j, k;
    for(i = 0; i < 4; i++) {
        for(j = 0; j < 3; j++) {
            float sum = 0;
            for(k = 0; k < 4; k++) {
                sum += a.m[k][j] * b.m[i][k];
            }
            result.m[i][j] = sum;
            //if (i < 3 && result.m[i][j] != 0.0) result.m[i][j] /= fabs(sum);
        }
    }
    result.m03 = result.m13 = result.m23 = result.m33 = 0.0;
    return result;
}
 
#endif // MATH_3D_HEADER
 
 
#ifndef MATH_3D_IMPLEMENTATION
 
#define MATH_3D_IMPLEMENTATION
 
static inline mat4_t m4_rotation (float angle_in_rad, vec3_t axis) {
    vec3_t normalized_axis = v3_norm(axis);
    float x = normalized_axis.x, y = normalized_axis.y, z = normalized_axis.z;
    float c = cosf(angle_in_rad), s = sinf(angle_in_rad);
 
    return mat4(
        c + x*x*(1-c),            x*y*(1-c) - z*s,      x*z*(1-c) + y*s,  0,
            y*x*(1-c) + z*s,  c + y*y*(1-c),            y*z*(1-c) - x*s,  0,
            z*x*(1-c) - y*s,      z*y*(1-c) + x*s,  c + z*z*(1-c),        0,
            0,                        0,                    0,            1
    );
}
 
/*
   Sébastien Le Roux 08/07/2022
   Rotation 3D, on x, y and z, providing angles in degrees [ 0 - 90 ]
*/
static inline mat4_t m4_rotation_anti_xyz (float alpha, float beta, float gamma) {
    mat4_t rot = m4_identity ();
    vec4_t qr;
    double pi = 3.141592653589793238462643383279502884197;
    vec3_t ax[3];
    ax[0] = vec3(1.0,0.0,0.0);
    ax[1] = vec3(0.0,1.0,0.0);
    ax[2] = vec3(0.0,0.0,1.0);
    qr = axis_to_quat (ax[2], - gamma*pi/90.0);
    rot = m4_mul (rot, m4_quat_rotation (qr));
    qr = axis_to_quat (ax[1], - beta*pi/90.0);
    rot = m4_mul (rot, m4_quat_rotation (qr));
    qr = axis_to_quat (ax[0], - alpha*pi/90.0);
    rot = m4_mul (rot, m4_quat_rotation (qr));
    return rot;
}
 
static inline mat4_t m4_rotation_xyz (float alpha, float beta, float gamma) {
    mat4_t rot = m4_identity ();
    vec4_t qr;
    double pi = 3.141592653589793238462643383279502884197;
    vec3_t ax[3];
    ax[0] = vec3(1.0,0.0,0.0);
    ax[1] = vec3(0.0,1.0,0.0);
    ax[2] = vec3(0.0,0.0,1.0);
    qr = axis_to_quat (ax[0], alpha*pi/90.0);
    rot = m4_mul (rot, m4_quat_rotation (qr));
    qr = axis_to_quat (ax[1], beta*pi/90.0);
    rot = m4_mul (rot, m4_quat_rotation (qr));
    qr = axis_to_quat (ax[2], gamma*pi/90.0);
    rot = m4_mul (rot, m4_quat_rotation (qr));
    return rot;
}
/*
   Sébastien Le Roux 06/02/2017
   Quaternion rotation
*/
static inline vec4_t axis_to_quat (vec3_t axis, float angle_in_rad)
{
  vec4_t qr;
  float half_angle = angle_in_rad/2.0;
  float n = sinf(half_angle)/v3_length(axis);
  qr.w = axis.x * n;
  qr.x = axis.y * n;
  qr.y = axis.z * n;
  qr.z = cosf(half_angle);
  return v4_norm (qr);
}
 
static inline vec4_t m4_mul_v4 (mat4_t mat, vec4_t vec) {
  int i;
  float out[4];
  float in[4] = {vec.w, vec.x, vec.y, vec.z};
  for (i=0; i<4; i++)
  {
    out[i] = in[0] * mat.m[0][i] + in[1] * mat.m[1][i] + in[2] * mat.m[2][i] + in[3] * mat.m[3][i];
  }
  return vec4(out[0], out[1], out[2], out[3]);
}
 
static inline mat4_t m4_quat_rotation (vec4_t q) {
//  I consider that the quaternion is already normalized
//  vec4_t n_quat = v4_norm(q);
    return mat4(
        1.0 - 2.0 * (q.x * q.x + q.y * q.y),        2.0 * (q.w * q.x + q.y * q.z),       2.0 * (q.y * q.w - q.x * q.z),  0,
        2.0 * (q.w * q.x - q.y * q.z),        1.0 - 2.0 * (q.y * q.y + q.w * q.w),       2.0 * (q.x * q.y + q.w * q.z),  0,
        2.0 * (q.y * q.w + q.x * q.z),              2.0 * (q.x * q.y - q.w * q.z), 1.0 - 2.0 * (q.x * q.x + q.w * q.w),  0,
                                    0,                        0,                    0,            1
    );
}
 
static inline vec3_t v3_project (vec3_t worldpos, vec4_t viewport, mat4_t projection) {
 
  mat4_t mod = m4_mul(projection, m4_translation(worldpos));
  vec4_t out = m4_mul_v4 (mod, vec4(0.0, 0.0, 0.0, 1.0));
  if (out.z != 0.0)
  {
    out.z = 1.0 / out.z;
    out.w = out.z * out.w + 1.0;
    out.x = out.z * out.x + 1.0;
    out.y = out.z * out.y + 1.0;
    return vec3(out.w*viewport.y + viewport.w, out.x*viewport.z + viewport.x, out.y);
  }
  else
  {
    return vec3 (0.0, 0.0, -1.0);
  }
}
 
static inline vec3_t v3_un_project (vec3_t winpos, vec4_t viewport, mat4_t projection) {
 
  vec3_t in = vec3 (2.0*(winpos.x-viewport.w)/viewport.y - 1.0,
                    2.0*((viewport.z-winpos.y)-viewport.x)/viewport.z - 1.0,
                    2.0*winpos.z-1.0);
  return m4_mul_pos (m4_invert_affine(projection), in);
}
 
 
static inline mat4_t m4_ortho(float left, float right, float bottom, float top, float back, float front) {
    float l = left, r = right, b = bottom, t = top, n = front, f = back;
    float tx = -(r + l) / (r - l);
    float ty = -(t + b) / (t - b);
    float tz = -(f + n) / (f - n);
    return mat4(
         2 / (r - l),  0,            0,            tx,
         0,            2 / (t - b),  0,            ty,
         0,            0,            2 / (f - n),  tz,
         0,            0,            0,            1
    );
}
 
static inline mat4_t m4_perspective(float vertical_field_of_view_in_deg, float aspect_ratio, float near_view_distance, float far_view_distance) {
    float fovy_in_rad = vertical_field_of_view_in_deg / 180 * M_PI;
    float f = 1.0f / tanf(fovy_in_rad / 2.0f);
    float ar = aspect_ratio;
    float nd = near_view_distance, fd = far_view_distance;
 
    return mat4(
         f / ar,           0,                0,                0,
         0,                f,                0,                0,
         0,                0,               (fd+nd)/(nd-fd),  (2*fd*nd)/(nd-fd),
         0,                0,               -1,                0
    );
}
 
static inline mat4_t m4_frustum (float left, float right, float bottom, float top, float front, float back) {
    float n = 2.0f * front;
    float l = right - left;
    float p = top - bottom;
    float a = (right + left) / l;
    float b = (top + bottom) / p;
    float c = -(back + front) / (back - front);
    float d = - back * n / (back - front);
 
    return mat4(
         n / l,            0,               a,                0,
         0,                n / p,           b,                0,
         0,                0,               c,                d,
         0,                0,              -1,                0
    );
}
 
 
static inline mat4_t m4_look_at(vec3_t from, vec3_t to, vec3_t up) {
    vec3_t z = v3_muls(v3_norm(v3_sub(to, from)), -1);
    vec3_t x = v3_norm(v3_cross(up, z));
    vec3_t y = v3_cross(z, x);
 
    return mat4(
        x.x, x.y, x.z, -v3_dot(from, x),
        y.x, y.y, y.z, -v3_dot(from, y),
        z.x, z.y, z.z, -v3_dot(from, z),
        0,   0,   0,    1
    );
}
 
 
static inline mat4_t m4_invert_affine(mat4_t matrix) {
    // Create shorthands to access matrix members
    float m00 = matrix.m00,  m10 = matrix.m10,  m20 = matrix.m20,  m30 = matrix.m30;
    float m01 = matrix.m01,  m11 = matrix.m11,  m21 = matrix.m21,  m31 = matrix.m31;
    float m02 = matrix.m02,  m12 = matrix.m12,  m22 = matrix.m22,  m32 = matrix.m32;
 
    // Invert 3x3 part of the 4x4 matrix that contains the rotation, etc.
    // That part is called R from here on.
 
        // Calculate cofactor matrix of R
        float c00 =   m11*m22 - m12*m21,   c10 = -(m01*m22 - m02*m21),  c20 =   m01*m12 - m02*m11;
        float c01 = -(m10*m22 - m12*m20),  c11 =   m00*m22 - m02*m20,   c21 = -(m00*m12 - m02*m10);
        float c02 =   m10*m21 - m11*m20,   c12 = -(m00*m21 - m01*m20),  c22 =   m00*m11 - m01*m10;
 
        // Caclculate the determinant by using the already calculated determinants
        // in the cofactor matrix.
        // Second sign is already minus from the cofactor matrix.
        float det = m00*c00 + m10*c10 + m20 * c20;
        if (fabsf(det) == 0.0000)
            return m4_identity();
 
        // Calcuate inverse of R by dividing the transposed cofactor matrix by the
        // determinant.
        float i00 = c00 / det,  i10 = c01 / det,  i20 = c02 / det;
        float i01 = c10 / det,  i11 = c11 / det,  i21 = c12 / det;
        float i02 = c20 / det,  i12 = c21 / det,  i22 = c22 / det;
 
    // Combine the inverted R with the inverted translation
    return mat4(
        i00, i10, i20,  -(i00*m30 + i10*m31 + i20*m32),
        i01, i11, i21,  -(i01*m30 + i11*m31 + i21*m32),
        i02, i12, i22,  -(i02*m30 + i12*m31 + i22*m32),
        0,   0,   0,      1
    );
}
 
 
static inline mat4_t m4_translate (mat4_t matrix, vec3_t translation) {
    return mat4(
        matrix.m00, matrix.m10, matrix.m20, matrix.m30 + translation.x,
        matrix.m01, matrix.m11, matrix.m21, matrix.m31 + translation.y,
        matrix.m02, matrix.m12, matrix.m22, matrix.m32 + translation.z,
        matrix.m03, matrix.m13, matrix.m23, matrix.m33
    );
}
 
static inline vec3_t m4_mul_pos(mat4_t matrix, vec3_t position) {
    vec3_t result = vec3(
        matrix.m00 * position.x + matrix.m10 * position.y + matrix.m20 * position.z + matrix.m30,
        matrix.m01 * position.x + matrix.m11 * position.y + matrix.m21 * position.z + matrix.m31,
        matrix.m02 * position.x + matrix.m12 * position.y + matrix.m22 * position.z + matrix.m32
    );
 
    float w = matrix.m03 * position.x + matrix.m13 * position.y + matrix.m23 * position.z + matrix.m33;
    if (w != 0.0) return vec3(result.x / w, result.y / w, result.z / w);
    return result;
}
 
static inline vec3_t m4_mul_coord(mat4_t matrix, vec3_t position) {
    vec3_t result = vec3(
        matrix.m00 * position.x + matrix.m10 * position.y + matrix.m20 * position.z + matrix.m30,
        matrix.m01 * position.x + matrix.m11 * position.y + matrix.m21 * position.z + matrix.m31,
        matrix.m02 * position.x + matrix.m12 * position.y + matrix.m22 * position.z + matrix.m32
    );
    return result;
}
static inline vec3_t m4_mul_abc(mat4_t matrix, vec3_t direction) {
    vec3_t result = vec3(
        matrix.m00 * direction.x + matrix.m10 * direction.y + matrix.m20 * direction.z,
        matrix.m01 * direction.x + matrix.m11 * direction.y + matrix.m21 * direction.z,
        matrix.m02 * direction.x + matrix.m12 * direction.y + matrix.m22 * direction.z
    );
    return result;
}
 
static inline vec3_t m4_mul_dir(mat4_t matrix, vec3_t direction) {
    vec3_t result = vec3(
        matrix.m00 * direction.x + matrix.m10 * direction.y + matrix.m20 * direction.z,
        matrix.m01 * direction.x + matrix.m11 * direction.y + matrix.m21 * direction.z,
        matrix.m02 * direction.x + matrix.m12 * direction.y + matrix.m22 * direction.z
    );
 
    float w = matrix.m03 * direction.x + matrix.m13 * direction.y + matrix.m23 * direction.z;
    if (w != 0.0) return vec3(result.x / w, result.y / w, result.z / w);
    return result;
}
 
static inline void m4_print(mat4_t matrix) {
    m4_fprintp(stdout, matrix, 6, 4);
}
 
static inline void m4_printp(mat4_t matrix, int width, int precision) {
    m4_fprintp(stdout, matrix, width, precision);
}
 
static inline void m4_fprint(FILE* stream, mat4_t matrix) {
    m4_fprintp(stream, matrix, 6, 2);
}
 
static inline void m4_fprintp(FILE* stream, mat4_t matrix, int width, int precision) {
    mat4_t m = matrix;
    int w = width, p = precision;
    int r;
    for(r = 0; r < 4; r++) {
        fprintf(stream, "| %*.*f %*.*f %*.*f %*.*f |\n",
            w, p, m.m[0][r], w, p, m.m[1][r], w, p, m.m[2][r], w, p, m.m[3][r]
        );
    }
}
 
#endif // MATH_3D_IMPLEMENTATION