266 lines
6.9 KiB
C
266 lines
6.9 KiB
C
#pragma once
|
|
|
|
#include "float.h"
|
|
#include "tuples.h"
|
|
|
|
struct mat4x4 {
|
|
float e[4][4];
|
|
};
|
|
|
|
struct mat3x3 {
|
|
float e[3][3];
|
|
};
|
|
|
|
struct mat2x2 {
|
|
float e[2][2];
|
|
};
|
|
|
|
inline static struct mat4x4 mat4x4(float a1, float a2, float a3, float a4,
|
|
float b1, float b2, float b3, float b4,
|
|
float c1, float c2, float c3, float c4,
|
|
float d1, float d2, float d3, float d4)
|
|
{
|
|
return (struct mat4x4){{
|
|
{ a1, a2, a3, a4 },
|
|
{ b1, b2, b3, b4 },
|
|
{ c1, c2, c3, c4 },
|
|
{ d1, d2, d3, d4 }
|
|
}};
|
|
}
|
|
|
|
inline static struct mat3x3 mat3x3(float a1, float a2, float a3,
|
|
float b1, float b2, float b3,
|
|
float c1, float c2, float c3)
|
|
{
|
|
return (struct mat3x3){{
|
|
{ a1, a2, a3 },
|
|
{ b1, b2, b3 },
|
|
{ c1, c2, c3 },
|
|
}};
|
|
}
|
|
|
|
inline static struct mat2x2 mat2x2(float a1, float a2,
|
|
float b1, float b2)
|
|
{
|
|
return (struct mat2x2){{
|
|
{ a1, a2 },
|
|
{ b1, b2 },
|
|
}};
|
|
}
|
|
|
|
inline static bool mat4x4_equal(struct mat4x4 a,
|
|
struct mat4x4 b)
|
|
{
|
|
return
|
|
float_equal(a.e[0][0], b.e[0][0]) &&
|
|
float_equal(a.e[0][1], b.e[0][1]) &&
|
|
float_equal(a.e[0][2], b.e[0][2]) &&
|
|
float_equal(a.e[0][3], b.e[0][3]) &&
|
|
float_equal(a.e[1][0], b.e[1][0]) &&
|
|
float_equal(a.e[1][1], b.e[1][1]) &&
|
|
float_equal(a.e[1][2], b.e[1][2]) &&
|
|
float_equal(a.e[1][3], b.e[1][3]) &&
|
|
float_equal(a.e[2][0], b.e[2][0]) &&
|
|
float_equal(a.e[2][1], b.e[2][1]) &&
|
|
float_equal(a.e[2][2], b.e[2][2]) &&
|
|
float_equal(a.e[2][3], b.e[2][3]) &&
|
|
float_equal(a.e[3][0], b.e[3][0]) &&
|
|
float_equal(a.e[3][1], b.e[3][1]) &&
|
|
float_equal(a.e[3][2], b.e[3][2]) &&
|
|
float_equal(a.e[3][3], b.e[3][3]);
|
|
}
|
|
|
|
inline static bool mat3x3_equal(struct mat3x3 a,
|
|
struct mat3x3 b)
|
|
{
|
|
return
|
|
float_equal(a.e[0][0], b.e[0][0]) &&
|
|
float_equal(a.e[0][1], b.e[0][1]) &&
|
|
float_equal(a.e[0][2], b.e[0][2]) &&
|
|
float_equal(a.e[1][0], b.e[1][0]) &&
|
|
float_equal(a.e[1][1], b.e[1][1]) &&
|
|
float_equal(a.e[1][2], b.e[1][2]) &&
|
|
float_equal(a.e[2][0], b.e[2][0]) &&
|
|
float_equal(a.e[2][1], b.e[2][1]) &&
|
|
float_equal(a.e[2][2], b.e[2][2]);
|
|
}
|
|
|
|
inline static bool mat2x2_equal(struct mat2x2 a,
|
|
struct mat2x2 b)
|
|
{
|
|
return
|
|
float_equal(a.e[0][0], b.e[0][0]) &&
|
|
float_equal(a.e[0][1], b.e[0][1]) &&
|
|
float_equal(a.e[1][0], b.e[1][0]) &&
|
|
float_equal(a.e[1][1], b.e[1][1]);
|
|
}
|
|
|
|
inline static struct mat4x4 mat4x4_mul_m(struct mat4x4 a,
|
|
struct mat4x4 b)
|
|
{
|
|
#define dot(row, col) \
|
|
a.e[row][0] * b.e[0][col] + \
|
|
a.e[row][1] * b.e[1][col] + \
|
|
a.e[row][2] * b.e[2][col] + \
|
|
a.e[row][3] * b.e[3][col]
|
|
|
|
return mat4x4(dot(0, 0), dot(0, 1), dot(0, 2), dot(0, 3),
|
|
dot(1, 0), dot(1, 1), dot(1, 2), dot(1, 3),
|
|
dot(2, 0), dot(2, 1), dot(2, 2), dot(2, 3),
|
|
dot(3, 0), dot(3, 1), dot(3, 2), dot(3, 3)
|
|
);
|
|
#undef dot
|
|
}
|
|
|
|
inline static struct tuple mat4x4_mul_t(struct mat4x4 a,
|
|
struct tuple b)
|
|
{
|
|
#define dot(row) \
|
|
a.e[row][0] * b.e[0] + \
|
|
a.e[row][1] * b.e[1] + \
|
|
a.e[row][2] * b.e[2] + \
|
|
a.e[row][3] * b.e[3]
|
|
|
|
return tuple(dot(0), dot(1), dot(2), dot(3));
|
|
#undef dot
|
|
}
|
|
|
|
inline static struct mat4x4 mat4x4_identity()
|
|
{
|
|
return mat4x4(1.0f, 0.0f, 0.0f, 0.0f,
|
|
0.0f, 1.0f, 0.0f, 0.0f,
|
|
0.0f, 0.0f, 1.0f, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|
|
}
|
|
|
|
inline static struct mat4x4 mat4x4_transpose(struct mat4x4 a)
|
|
{
|
|
return mat4x4(a.e[0][0], a.e[1][0], a.e[2][0], a.e[3][0],
|
|
a.e[0][1], a.e[1][1], a.e[2][1], a.e[3][1],
|
|
a.e[0][2], a.e[1][2], a.e[2][2], a.e[3][2],
|
|
a.e[0][3], a.e[1][3], a.e[2][3], a.e[3][3]);
|
|
}
|
|
|
|
inline static float mat2x2_determinant(struct mat2x2 a)
|
|
{
|
|
return a.e[0][0] * a.e[1][1] - a.e[0][1] * a.e[1][0];
|
|
}
|
|
|
|
inline static struct mat2x2 mat3x3_submatrix(struct mat3x3 a, int r, int c)
|
|
{
|
|
struct mat2x2 b;
|
|
int row2 = 0;
|
|
for (int row3 = 0; row3 < 3; row3++) {
|
|
if (row3 == r) continue;
|
|
int col2 = 0;
|
|
for (int col3 = 0; col3 < 3; col3++) {
|
|
if (col3 == c) continue;
|
|
b.e[row2][col2] = a.e[row3][col3];
|
|
col2++;
|
|
}
|
|
row2++;
|
|
}
|
|
return b;
|
|
}
|
|
|
|
inline static struct mat3x3 mat4x4_submatrix(struct mat4x4 a, int r, int c)
|
|
{
|
|
struct mat3x3 b;
|
|
int row3 = 0;
|
|
for (int row4 = 0; row4 < 4; row4++) {
|
|
if (row4 == r) continue;
|
|
int col3 = 0;
|
|
for (int col4 = 0; col4 < 4; col4++) {
|
|
if (col4 == c) continue;
|
|
b.e[row3][col3] = a.e[row4][col4];
|
|
col3++;
|
|
}
|
|
row3++;
|
|
}
|
|
return b;
|
|
}
|
|
|
|
inline static float mat3x3_minor(struct mat3x3 a, int r, int c)
|
|
{
|
|
struct mat2x2 s = mat3x3_submatrix(a, r, c);
|
|
float ret = mat2x2_determinant(s);
|
|
return ret;
|
|
}
|
|
|
|
inline static float mat3x3_cofactor(struct mat3x3 a, int r, int c)
|
|
{
|
|
float minor = mat3x3_minor(a, r, c);
|
|
if ((r + c) & 1)
|
|
return -minor;
|
|
else
|
|
return minor;
|
|
}
|
|
|
|
inline static float mat3x3_determinant(struct mat3x3 a)
|
|
{
|
|
float f0 = mat3x3_cofactor(a, 0, 0);
|
|
float f1 = mat3x3_cofactor(a, 0, 1);
|
|
float f2 = mat3x3_cofactor(a, 0, 2);
|
|
return
|
|
a.e[0][0] * f0 +
|
|
a.e[0][1] * f1 +
|
|
a.e[0][2] * f2;
|
|
}
|
|
|
|
inline static float mat4x4_minor(struct mat4x4 a, int r, int c)
|
|
{
|
|
struct mat3x3 s = mat4x4_submatrix(a, r, c);
|
|
float ret = mat3x3_determinant(s);
|
|
return ret;
|
|
}
|
|
|
|
inline static float mat4x4_cofactor(struct mat4x4 a, int r, int c)
|
|
{
|
|
float minor = mat4x4_minor(a, r, c);
|
|
if ((r + c) & 1)
|
|
return -minor;
|
|
else
|
|
return minor;
|
|
}
|
|
|
|
inline static float mat4x4_determinant(struct mat4x4 a)
|
|
{
|
|
float f0 = mat4x4_cofactor(a, 0, 0);
|
|
float f1 = mat4x4_cofactor(a, 0, 1);
|
|
float f2 = mat4x4_cofactor(a, 0, 2);
|
|
float f3 = mat4x4_cofactor(a, 0, 3);
|
|
return
|
|
a.e[0][0] * f0 +
|
|
a.e[0][1] * f1 +
|
|
a.e[0][2] * f2 +
|
|
a.e[0][3] * f3;
|
|
}
|
|
|
|
inline static bool mat4x4_is_invertible(struct mat4x4 a)
|
|
{
|
|
return !float_equal(mat4x4_determinant(a), 0.f);
|
|
}
|
|
|
|
inline static struct mat4x4 mat4x4_inverse(struct mat4x4 a)
|
|
{
|
|
struct mat4x4 m;
|
|
float det = mat4x4_determinant(a);
|
|
m.e[0][0] = mat4x4_cofactor(a, 0, 0) / det;
|
|
m.e[1][0] = mat4x4_cofactor(a, 0, 1) / det;
|
|
m.e[2][0] = mat4x4_cofactor(a, 0, 2) / det;
|
|
m.e[3][0] = mat4x4_cofactor(a, 0, 3) / det;
|
|
m.e[0][1] = mat4x4_cofactor(a, 1, 0) / det;
|
|
m.e[1][1] = mat4x4_cofactor(a, 1, 1) / det;
|
|
m.e[2][1] = mat4x4_cofactor(a, 1, 2) / det;
|
|
m.e[3][1] = mat4x4_cofactor(a, 1, 3) / det;
|
|
m.e[0][2] = mat4x4_cofactor(a, 2, 0) / det;
|
|
m.e[1][2] = mat4x4_cofactor(a, 2, 1) / det;
|
|
m.e[2][2] = mat4x4_cofactor(a, 2, 2) / det;
|
|
m.e[3][2] = mat4x4_cofactor(a, 2, 3) / det;
|
|
m.e[0][3] = mat4x4_cofactor(a, 3, 0) / det;
|
|
m.e[1][3] = mat4x4_cofactor(a, 3, 1) / det;
|
|
m.e[2][3] = mat4x4_cofactor(a, 3, 2) / det;
|
|
m.e[3][3] = mat4x4_cofactor(a, 3, 3) / det;
|
|
return m;
|
|
}
|