luboslenco
1030 lines
26 KiB
C
1030 lines
26 KiB
C
|
|
#include "iron_math.h"
|
|
#include "iron_alloc.h"
|
|
#include <limits.h>
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
void iron_color_components(uint32_t color, float *red, float *green, float *blue, float *alpha) {
|
|
*alpha = ((color & 0xff000000) >> 24) / 255.0f;
|
|
*red = ((color & 0x00ff0000) >> 16) / 255.0f;
|
|
*green = ((color & 0x0000ff00) >> 8) / 255.0f;
|
|
*blue = (color & 0x000000ff) / 255.0f;
|
|
}
|
|
|
|
// xoshiro256** 1.0
|
|
static inline uint64_t rotl(const uint64_t x, int k) {
|
|
return (x << k) | (x >> (64 - k));
|
|
}
|
|
|
|
static uint64_t s[4] = {1, 2, 3, 4};
|
|
|
|
uint64_t next(void) {
|
|
const uint64_t result = rotl(s[1] * 5, 7) * 9;
|
|
|
|
const uint64_t t = s[1] << 17;
|
|
|
|
s[2] ^= s[0];
|
|
s[3] ^= s[1];
|
|
s[1] ^= s[2];
|
|
s[0] ^= s[3];
|
|
|
|
s[2] ^= t;
|
|
|
|
s[3] = rotl(s[3], 45);
|
|
|
|
return result;
|
|
}
|
|
|
|
void iron_random_init(int64_t seed) {
|
|
s[0] = (uint64_t)seed;
|
|
s[1] = 2;
|
|
s[2] = 3;
|
|
s[3] = 4;
|
|
s[1] = next();
|
|
s[2] = next();
|
|
s[3] = next();
|
|
}
|
|
|
|
int64_t iron_random_get(void) {
|
|
return (int64_t)next();
|
|
}
|
|
|
|
int64_t iron_random_get_max(int64_t max) {
|
|
int64_t value = iron_random_get();
|
|
return (value < -LLONG_MAX ? LLONG_MAX : llabs(value)) % (max + 1);
|
|
}
|
|
|
|
int64_t iron_random_get_in(int64_t min, int64_t max) {
|
|
int64_t value = iron_random_get();
|
|
return (value < -LLONG_MAX ? LLONG_MAX : llabs(value)) % (max + 1 - min) + min;
|
|
}
|
|
|
|
// ██╗ ██╗███████╗ ██████╗██████╗
|
|
// ██║ ██║██╔════╝██╔════╝╚════██╗
|
|
// ██║ ██║█████╗ ██║ █████╔╝
|
|
// ╚██╗ ██╔╝██╔══╝ ██║ ██╔═══╝
|
|
// ╚████╔╝ ███████╗╚██████╗███████╗
|
|
// ╚═══╝ ╚══════╝ ╚═════╝╚══════╝
|
|
|
|
float vec2_len(vec2_t v) {
|
|
return sqrtf(v.x * v.x + v.y * v.y);
|
|
}
|
|
|
|
vec2_t vec2_set_len(vec2_t v, float length) {
|
|
float current_length = vec2_len(v);
|
|
if (current_length == 0) {
|
|
return v;
|
|
}
|
|
float mul = length / current_length;
|
|
v.x *= mul;
|
|
v.y *= mul;
|
|
return v;
|
|
}
|
|
|
|
vec2_t vec2_mult(vec2_t v, float f) {
|
|
v.x *= f;
|
|
v.y *= f;
|
|
return v;
|
|
}
|
|
|
|
vec2_t vec2_add(vec2_t a, vec2_t b) {
|
|
a.x += b.x;
|
|
a.y += b.y;
|
|
return a;
|
|
}
|
|
|
|
vec2_t vec2_sub(vec2_t a, vec2_t b) {
|
|
a.x -= b.x;
|
|
a.y -= b.y;
|
|
return a;
|
|
}
|
|
|
|
float vec2_cross(vec2_t a, vec2_t b) {
|
|
return a.x * b.y - a.y * b.x;
|
|
}
|
|
|
|
vec2_t vec2_norm(vec2_t v) {
|
|
float n = vec2_len(v);
|
|
if (n > 0.0) {
|
|
float inv_n = 1.0f / n;
|
|
v.x *= inv_n;
|
|
v.y *= inv_n;
|
|
}
|
|
return v;
|
|
}
|
|
|
|
float vec2_dot(vec2_t a, vec2_t b) {
|
|
return a.x * b.x + a.y * b.y;
|
|
}
|
|
|
|
vec2_t vec2_nan() {
|
|
vec2_t v;
|
|
v.x = NAN;
|
|
return v;
|
|
}
|
|
|
|
bool vec2_isnan(vec2_t v) {
|
|
return isnan(v.x);
|
|
}
|
|
|
|
// ██╗ ██╗███████╗ ██████╗██╗ ██╗
|
|
// ██║ ██║██╔════╝██╔════╝██║ ██║
|
|
// ██║ ██║█████╗ ██║ ███████║
|
|
// ╚██╗ ██╔╝██╔══╝ ██║ ╚════██║
|
|
// ╚████╔╝ ███████╗╚██████╗ ██║
|
|
// ╚═══╝ ╚══════╝ ╚═════╝ ╚═╝
|
|
|
|
f32_array_t *vec3_to_f32_array(vec4_t v) {
|
|
f32_array_t *res = f32_array_create(3);
|
|
res->buffer[0] = v.x;
|
|
res->buffer[1] = v.y;
|
|
res->buffer[2] = v.z;
|
|
return res;
|
|
}
|
|
|
|
vec4_t vec4_cross(vec4_t a, vec4_t b) {
|
|
vec4_t v;
|
|
v.x = a.y * b.z - a.z * b.y;
|
|
v.y = a.z * b.x - a.x * b.z;
|
|
v.z = a.x * b.y - a.y * b.x;
|
|
return v;
|
|
}
|
|
|
|
vec4_t vec4_add(vec4_t a, vec4_t b) {
|
|
a.x += b.x;
|
|
a.y += b.y;
|
|
a.z += b.z;
|
|
a.w += b.w;
|
|
return a;
|
|
}
|
|
|
|
vec4_t vec4_fadd(vec4_t a, float x, float y, float z, float w) {
|
|
a.x += x;
|
|
a.y += y;
|
|
a.z += z;
|
|
a.w += w;
|
|
return a;
|
|
}
|
|
|
|
vec4_t vec4_norm(vec4_t a) {
|
|
float n = vec4_len(a);
|
|
if (n > 0.0) {
|
|
float inv_n = 1.0f / n;
|
|
a.x *= inv_n;
|
|
a.y *= inv_n;
|
|
a.z *= inv_n;
|
|
}
|
|
return a;
|
|
}
|
|
|
|
vec4_t vec4_mult(vec4_t a, float f) {
|
|
a.x *= f;
|
|
a.y *= f;
|
|
a.z *= f;
|
|
a.w *= f;
|
|
return a;
|
|
}
|
|
|
|
float vec4_dot(vec4_t a, vec4_t b) {
|
|
return a.x * b.x + a.y * b.y + a.z * b.z;
|
|
}
|
|
|
|
vec4_t vec4_apply_proj(vec4_t a, mat4_t m) {
|
|
vec4_t v;
|
|
float d = 1.0 / (m.m[3] * a.x + m.m[7] * a.y + m.m[11] * a.z + m.m[15]); // Perspective divide
|
|
v.x = (m.m[0] * a.x + m.m[4] * a.y + m.m[8] * a.z + m.m[12]) * d;
|
|
v.y = (m.m[1] * a.x + m.m[5] * a.y + m.m[9] * a.z + m.m[13]) * d;
|
|
v.z = (m.m[2] * a.x + m.m[6] * a.y + m.m[10] * a.z + m.m[14]) * d;
|
|
return v;
|
|
}
|
|
|
|
vec4_t vec4_apply_mat4(vec4_t a, mat4_t m) {
|
|
vec4_t v;
|
|
v.x = m.m[0] * a.x + m.m[4] * a.y + m.m[8] * a.z + m.m[12] * a.w;
|
|
v.y = m.m[1] * a.x + m.m[5] * a.y + m.m[9] * a.z + m.m[13] * a.w;
|
|
v.z = m.m[2] * a.x + m.m[6] * a.y + m.m[10] * a.z + m.m[14] * a.w;
|
|
v.w = m.m[3] * a.x + m.m[7] * a.y + m.m[11] * a.z + m.m[15] * a.w;
|
|
return v;
|
|
}
|
|
|
|
vec4_t vec4_apply_axis_angle(vec4_t a, vec4_t axis, float angle) {
|
|
quat_t q = quat_from_axis_angle(axis, angle);
|
|
return vec4_apply_quat(a, q);
|
|
}
|
|
|
|
vec4_t vec4_apply_quat(vec4_t a, quat_t q) {
|
|
vec4_t v;
|
|
float ix = q.w * a.x + q.y * a.z - q.z * a.y;
|
|
float iy = q.w * a.y + q.z * a.x - q.x * a.z;
|
|
float iz = q.w * a.z + q.x * a.y - q.y * a.x;
|
|
float iw = -q.x * a.x - q.y * a.y - q.z * a.z;
|
|
v.x = ix * q.w + iw * -q.x + iy * -q.z - iz * -q.y;
|
|
v.y = iy * q.w + iw * -q.y + iz * -q.x - ix * -q.z;
|
|
v.z = iz * q.w + iw * -q.z + ix * -q.y - iy * -q.x;
|
|
return v;
|
|
}
|
|
|
|
bool vec4_equals(vec4_t a, vec4_t b) {
|
|
return a.x == b.x && a.y == b.y && a.z == b.z;
|
|
}
|
|
|
|
bool vec4_almost_equals(vec4_t a, vec4_t b, float prec) {
|
|
return fabs(a.x - b.x) < prec && fabs(a.y - b.y) < prec && fabs(a.z - b.z) < prec;
|
|
}
|
|
|
|
float vec4_len(vec4_t a) {
|
|
return sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
|
|
}
|
|
|
|
vec4_t vec4_sub(vec4_t a, vec4_t b) {
|
|
a.x -= b.x;
|
|
a.y -= b.y;
|
|
a.z -= b.z;
|
|
return a;
|
|
}
|
|
|
|
float vec4_dist(vec4_t v1, vec4_t v2) {
|
|
return vec4_fdist(v1.x, v1.y, v1.z, v2.x, v2.y, v2.z);
|
|
}
|
|
|
|
float vec4_fdist(float v1x, float v1y, float v1z, float v2x, float v2y, float v2z) {
|
|
float vx = v1x - v2x;
|
|
float vy = v1y - v2y;
|
|
float vz = v1z - v2z;
|
|
return sqrtf(vx * vx + vy * vy + vz * vz);
|
|
}
|
|
|
|
vec4_t vec4_reflect(vec4_t a, vec4_t n) {
|
|
float d = 2 * vec4_dot(a, n);
|
|
a.x -= d * n.x;
|
|
a.y -= d * n.y;
|
|
a.z -= d * n.z;
|
|
return a;
|
|
}
|
|
|
|
vec4_t vec4_clamp(vec4_t a, float min, float max) {
|
|
float l = vec4_len(a);
|
|
if (l < min) {
|
|
return vec4_mult(vec4_norm(a), min);
|
|
}
|
|
else if (l > max) {
|
|
return vec4_mult(vec4_norm(a), max);
|
|
}
|
|
return a;
|
|
}
|
|
|
|
vec4_t vec4_x_axis() {
|
|
return (vec4_t){1.0, 0.0, 0.0, 1.0};
|
|
}
|
|
|
|
vec4_t vec4_y_axis() {
|
|
return (vec4_t){0.0, 1.0, 0.0, 1.0};
|
|
}
|
|
|
|
vec4_t vec4_z_axis() {
|
|
return (vec4_t){0.0, 0.0, 1.0, 1.0};
|
|
}
|
|
|
|
vec4_t vec4_nan() {
|
|
vec4_t v;
|
|
v.x = NAN;
|
|
return v;
|
|
}
|
|
|
|
bool vec4_isnan(vec4_t v) {
|
|
return isnan(v.x);
|
|
}
|
|
|
|
// ██████╗ ██╗ ██╗ █████╗ ████████╗
|
|
// ██╔═══██╗██║ ██║██╔══██╗╚══██╔══╝
|
|
// ██║ ██║██║ ██║███████║ ██║
|
|
// ██║▄▄ ██║██║ ██║██╔══██║ ██║
|
|
// ╚██████╔╝╚██████╔╝██║ ██║ ██║
|
|
// ╚══▀▀═╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝
|
|
|
|
quat_t quat_from_axis_angle(vec4_t axis, float angle) {
|
|
float s = sinf(angle * 0.5);
|
|
quat_t q;
|
|
q.x = axis.x * s;
|
|
q.y = axis.y * s;
|
|
q.z = axis.z * s;
|
|
q.w = cosf(angle * 0.5);
|
|
return quat_norm(q);
|
|
}
|
|
|
|
quat_t quat_from_mat(mat4_t m) {
|
|
m = mat4_to_rot(m);
|
|
return quat_from_rot_mat(m);
|
|
}
|
|
|
|
quat_t quat_from_rot_mat(mat4_t m) {
|
|
// Assumes the upper 3x3 is a pure rotation matrix
|
|
float m11 = m.m[0];
|
|
float m12 = m.m[4];
|
|
float m13 = m.m[8];
|
|
float m21 = m.m[1];
|
|
float m22 = m.m[5];
|
|
float m23 = m.m[9];
|
|
float m31 = m.m[2];
|
|
float m32 = m.m[6];
|
|
float m33 = m.m[10];
|
|
float tr = m11 + m22 + m33;
|
|
float s = 0.0;
|
|
quat_t q;
|
|
|
|
if (tr > 0) {
|
|
s = 0.5 / sqrtf(tr + 1.0);
|
|
q.w = 0.25 / s;
|
|
q.x = (m32 - m23) * s;
|
|
q.y = (m13 - m31) * s;
|
|
q.z = (m21 - m12) * s;
|
|
}
|
|
else if (m11 > m22 && m11 > m33) {
|
|
s = 2.0 * sqrtf(1.0 + m11 - m22 - m33);
|
|
q.w = (m32 - m23) / s;
|
|
q.x = 0.25 * s;
|
|
q.y = (m12 + m21) / s;
|
|
q.z = (m13 + m31) / s;
|
|
}
|
|
else if (m22 > m33) {
|
|
s = 2.0 * sqrtf(1.0 + m22 - m11 - m33);
|
|
q.w = (m13 - m31) / s;
|
|
q.x = (m12 + m21) / s;
|
|
q.y = 0.25 * s;
|
|
q.z = (m23 + m32) / s;
|
|
}
|
|
else {
|
|
s = 2.0 * sqrtf(1.0 + m33 - m11 - m22);
|
|
q.w = (m21 - m12) / s;
|
|
q.x = (m13 + m31) / s;
|
|
q.y = (m23 + m32) / s;
|
|
q.z = 0.25 * s;
|
|
}
|
|
return quat_norm(q);
|
|
}
|
|
|
|
quat_t quat_mult(quat_t a, quat_t b) {
|
|
quat_t q;
|
|
q.x = a.x * b.w + a.w * b.x + a.y * b.z - a.z * b.y;
|
|
q.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
|
|
q.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
|
|
q.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
|
|
return q;
|
|
}
|
|
|
|
quat_t quat_norm(quat_t q) {
|
|
float l = sqrtf(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
|
|
if (l == 0.0) {
|
|
q.x = 0;
|
|
q.y = 0;
|
|
q.z = 0;
|
|
q.w = 0;
|
|
}
|
|
else {
|
|
l = 1.0 / l;
|
|
q.x *= l;
|
|
q.y *= l;
|
|
q.z *= l;
|
|
q.w *= l;
|
|
}
|
|
return q;
|
|
}
|
|
|
|
vec4_t quat_get_euler(quat_t q) {
|
|
float a = -2 * (q.x * q.z - q.w * q.y);
|
|
float b = q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z;
|
|
float c = 2 * (q.x * q.y + q.w * q.z);
|
|
float d = -2 * (q.y * q.z - q.w * q.x);
|
|
float e = q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z;
|
|
return (vec4_t){atan2f(d, e), atan2f(a, b), asinf(c), 1.0};
|
|
}
|
|
|
|
quat_t quat_from_euler(float x, float y, float z) {
|
|
float f = x / 2;
|
|
float c1 = cosf(f);
|
|
float s1 = sinf(f);
|
|
f = y / 2;
|
|
float c2 = cosf(f);
|
|
float s2 = sinf(f);
|
|
f = z / 2;
|
|
float c3 = cosf(f);
|
|
float s3 = sinf(f);
|
|
// YZX
|
|
quat_t q;
|
|
q.x = s1 * c2 * c3 + c1 * s2 * s3;
|
|
q.y = c1 * s2 * c3 + s1 * c2 * s3;
|
|
q.z = c1 * c2 * s3 - s1 * s2 * c3;
|
|
q.w = c1 * c2 * c3 - s1 * s2 * s3;
|
|
return q;
|
|
}
|
|
|
|
float quat_dot(quat_t a, quat_t b) {
|
|
return (a.x * b.x) + (a.y * b.y) + (a.z * b.z) + (a.w * b.w);
|
|
}
|
|
|
|
quat_t quat_from_to(vec4_t v0, vec4_t v1) {
|
|
// Rotation formed by direction vectors
|
|
// v0 and v1 should be normalized first
|
|
float dot = vec4_dot(v0, v1);
|
|
if (dot < -0.999999) {
|
|
vec4_t a = vec4_cross(vec4_x_axis(), v0);
|
|
if (vec4_len(a) < 0.000001) {
|
|
a = vec4_cross(vec4_y_axis(), v0);
|
|
}
|
|
a = vec4_norm(a);
|
|
return quat_from_axis_angle(a, IRON_PI);
|
|
}
|
|
else if (dot > 0.999999) {
|
|
return (quat_t){0, 0, 0, 1};
|
|
}
|
|
else {
|
|
vec4_t a = vec4_cross(v0, v1);
|
|
quat_t q = (quat_t){a.x, a.y, a.z, 1.0 + dot};
|
|
return quat_norm(q);
|
|
}
|
|
}
|
|
|
|
quat_t quat_inv(quat_t q) {
|
|
float l = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
|
|
l = -1.0 / l;
|
|
q.x = q.x * l;
|
|
q.y = q.y * l;
|
|
q.z = q.z * l;
|
|
q.w = -q.w * l;
|
|
return q;
|
|
}
|
|
|
|
quat_t quat_slerp(quat_t a, quat_t b, float t) {
|
|
float dot = quat_dot(a, b);
|
|
if (dot < 0.0f) {
|
|
b.x = -b.x;
|
|
b.y = -b.y;
|
|
b.z = -b.z;
|
|
b.w = -b.w;
|
|
dot = -dot;
|
|
}
|
|
if (dot > 0.9995f) {
|
|
quat_t r = {a.x + t * (b.x - a.x), a.y + t * (b.y - a.y), a.z + t * (b.z - a.z), a.w + t * (b.w - a.w)};
|
|
return quat_norm(r);
|
|
}
|
|
float theta_0 = acosf(dot);
|
|
float theta = theta_0 * t;
|
|
float sin_t = sinf(theta);
|
|
float sin_t0 = sinf(theta_0);
|
|
float s0 = cosf(theta) - dot * sin_t / sin_t0;
|
|
float s1 = sin_t / sin_t0;
|
|
return (quat_t){s0 * a.x + s1 * b.x, s0 * a.y + s1 * b.y, s0 * a.z + s1 * b.z, s0 * a.w + s1 * b.w};
|
|
}
|
|
|
|
// ███╗ ███╗ █████╗ ████████╗██████╗
|
|
// ████╗ ████║██╔══██╗╚══██╔══╝╚════██╗
|
|
// ██╔████╔██║███████║ ██║ █████╔╝
|
|
// ██║╚██╔╝██║██╔══██║ ██║ ╚═══██╗
|
|
// ██║ ╚═╝ ██║██║ ██║ ██║ ██████╔╝
|
|
// ╚═╝ ╚═╝╚═╝ ╚═╝ ╚═╝ ╚═════╝
|
|
|
|
mat3_t mat3_identity() {
|
|
return (mat3_t){1, 0, 0, 0, 1, 0, 0, 0, 1};
|
|
}
|
|
|
|
mat3_t mat3_translation(float x, float y) {
|
|
return (mat3_t){1, 0, 0, 0, 1, 0, x, y, 1};
|
|
}
|
|
|
|
mat3_t mat3_rotation(float alpha) {
|
|
return (mat3_t){cosf(alpha), sinf(alpha), 0, -sinf(alpha), cosf(alpha), 0, 0, 0, 1};
|
|
}
|
|
|
|
mat3_t mat3_scale(mat3_t m, vec4_t v) {
|
|
m.m[0] *= v.x;
|
|
m.m[1] *= v.x;
|
|
m.m[2] *= v.x;
|
|
m.m[3] *= v.y;
|
|
m.m[4] *= v.y;
|
|
m.m[5] *= v.y;
|
|
m.m[6] *= v.z;
|
|
m.m[7] *= v.z;
|
|
m.m[8] *= v.z;
|
|
return m;
|
|
}
|
|
|
|
mat3_t mat3_set_from4(mat4_t m4) {
|
|
mat3_t m;
|
|
m.m[0] = m4.m[0];
|
|
m.m[1] = m4.m[1];
|
|
m.m[2] = m4.m[2];
|
|
m.m[3] = m4.m[4];
|
|
m.m[4] = m4.m[5];
|
|
m.m[5] = m4.m[6];
|
|
m.m[6] = m4.m[8];
|
|
m.m[7] = m4.m[9];
|
|
m.m[8] = m4.m[10];
|
|
return m;
|
|
}
|
|
|
|
mat3_t mat3_multmat(mat3_t a, mat3_t b) {
|
|
return (mat3_t){a.m[0] * b.m[0] + a.m[3] * b.m[1] + a.m[6] * b.m[2], a.m[1] * b.m[0] + a.m[4] * b.m[1] + a.m[7] * b.m[2],
|
|
a.m[2] * b.m[0] + a.m[5] * b.m[1] + a.m[8] * b.m[2], a.m[0] * b.m[3] + a.m[3] * b.m[4] + a.m[6] * b.m[5],
|
|
a.m[1] * b.m[3] + a.m[4] * b.m[4] + a.m[7] * b.m[5], a.m[2] * b.m[3] + a.m[5] * b.m[4] + a.m[8] * b.m[5],
|
|
a.m[0] * b.m[6] + a.m[3] * b.m[7] + a.m[6] * b.m[8], a.m[1] * b.m[6] + a.m[4] * b.m[7] + a.m[7] * b.m[8],
|
|
a.m[2] * b.m[6] + a.m[5] * b.m[7] + a.m[8] * b.m[8]};
|
|
}
|
|
|
|
mat3_t mat3_transpose(mat3_t m) {
|
|
float f = m.m[1];
|
|
m.m[1] = m.m[3];
|
|
m.m[3] = f;
|
|
|
|
f = m.m[2];
|
|
m.m[2] = m.m[6];
|
|
m.m[6] = f;
|
|
|
|
f = m.m[5];
|
|
m.m[5] = m.m[7];
|
|
m.m[7] = f;
|
|
|
|
return m;
|
|
}
|
|
|
|
mat3_t mat3_nan() {
|
|
mat3_t m;
|
|
m.m[0] = NAN;
|
|
return m;
|
|
}
|
|
|
|
bool mat3_isnan(mat3_t m) {
|
|
return isnan(m.m[0]);
|
|
}
|
|
|
|
// ███╗ ███╗ █████╗ ████████╗██╗ ██╗
|
|
// ████╗ ████║██╔══██╗╚══██╔══╝██║ ██║
|
|
// ██╔████╔██║███████║ ██║ ███████║
|
|
// ██║╚██╔╝██║██╔══██║ ██║ ╚════██║
|
|
// ██║ ╚═╝ ██║██║ ██║ ██║ ██║
|
|
// ╚═╝ ╚═╝╚═╝ ╚═╝ ╚═╝ ╚═╝
|
|
|
|
mat4_t mat4_identity() {
|
|
return (mat4_t){1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0};
|
|
}
|
|
|
|
mat4_t mat4_from_f32_array(f32_array_t *a, int offset) {
|
|
mat4_t m;
|
|
memcpy(m.m, a->buffer + offset, 16 * sizeof(float));
|
|
return m;
|
|
}
|
|
|
|
f32_array_t *mat4_to_f32_array(mat4_t m) {
|
|
f32_array_t *a = f32_array_create(16);
|
|
memcpy(a->buffer, m.m, 16 * sizeof(float));
|
|
return a;
|
|
}
|
|
|
|
mat4_t mat4_persp(float fov_y, float aspect, float zn, float zf) {
|
|
float uh = 1.0 / tanf(fov_y / 2);
|
|
float uw = uh / aspect;
|
|
return (mat4_t){uw, 0, 0, 0, 0, uh, 0, 0, 0, 0, (zf + zn) / (zn - zf), -1, 0, 0, 2 * zf * zn / (zn - zf), 0};
|
|
}
|
|
|
|
mat4_t mat4_ortho(float left, float right, float bottom, float top, float znear, float zfar) {
|
|
float rl = right - left;
|
|
float tb = top - bottom;
|
|
float fn = zfar - znear;
|
|
float tx = -(right + left) / (rl);
|
|
float ty = -(top + bottom) / (tb);
|
|
float tz = -(zfar + znear) / (fn);
|
|
return (mat4_t){2 / rl, 0, 0, 0, 0, 2 / tb, 0, 0, 0, 0, -2 / fn, 0, tx, ty, tz, 1};
|
|
}
|
|
|
|
mat4_t mat4_rot_z(float alpha) {
|
|
float ca = cosf(alpha);
|
|
float sa = sinf(alpha);
|
|
return (mat4_t){ca, sa, 0, 0, -sa, ca, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
|
|
}
|
|
|
|
mat4_t mat4_compose(vec4_t loc, quat_t rot, vec4_t scl) {
|
|
mat4_t m = mat4_from_quat(rot);
|
|
m = mat4_scale(m, scl);
|
|
m = mat4_set_loc(m, loc);
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_tween(mat4_t a, mat4_t b, float t) {
|
|
mat4_decomposed_t *da = mat4_decompose(a);
|
|
mat4_decomposed_t *db = mat4_decompose(b);
|
|
vec4_t loc = {da->loc.x + t * (db->loc.x - da->loc.x), da->loc.y + t * (db->loc.y - da->loc.y), da->loc.z + t * (db->loc.z - da->loc.z), 1.0f};
|
|
vec4_t scl = {da->scl.x + t * (db->scl.x - da->scl.x), da->scl.y + t * (db->scl.y - da->scl.y), da->scl.z + t * (db->scl.z - da->scl.z), 1.0f};
|
|
quat_t rot = quat_slerp(da->rot, db->rot, t);
|
|
return mat4_compose(loc, rot, scl);
|
|
}
|
|
|
|
mat4_decomposed_t *mat4_decompose(mat4_t m) {
|
|
vec4_t loc;
|
|
quat_t rot;
|
|
vec4_t scl;
|
|
loc.x = m.m[12];
|
|
loc.y = m.m[13];
|
|
loc.z = m.m[14];
|
|
scl.x = vec4_len((vec4_t){m.m[0], m.m[1], m.m[2], 1.0});
|
|
scl.y = vec4_len((vec4_t){m.m[4], m.m[5], m.m[6], 1.0});
|
|
scl.z = vec4_len((vec4_t){m.m[8], m.m[9], m.m[10], 1.0});
|
|
if (mat4_determinant(m) < 0.0) {
|
|
scl.x = -scl.x;
|
|
}
|
|
float invs = 1.0 / scl.x; // Scale the rotation part
|
|
m.m[0] *= invs;
|
|
m.m[1] *= invs;
|
|
m.m[2] *= invs;
|
|
invs = 1.0 / scl.y;
|
|
m.m[4] *= invs;
|
|
m.m[5] *= invs;
|
|
m.m[6] *= invs;
|
|
invs = 1.0 / scl.z;
|
|
m.m[8] *= invs;
|
|
m.m[9] *= invs;
|
|
m.m[10] *= invs;
|
|
rot = quat_from_rot_mat(m);
|
|
|
|
// Frame temporary..
|
|
mat4_decomposed_t *dec = (mat4_decomposed_t *)string_tmp_alloc(sizeof(mat4_decomposed_t));
|
|
dec->loc = loc;
|
|
dec->rot = rot;
|
|
dec->scl = scl;
|
|
return dec;
|
|
}
|
|
|
|
mat4_t mat4_set_loc(mat4_t m, vec4_t v) {
|
|
m.m[12] = v.x;
|
|
m.m[13] = v.y;
|
|
m.m[14] = v.z;
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_from_quat(quat_t q) {
|
|
float x2 = q.x + q.x;
|
|
float y2 = q.y + q.y;
|
|
float z2 = q.z + q.z;
|
|
float xx = q.x * x2;
|
|
float xy = q.x * y2;
|
|
float xz = q.x * z2;
|
|
float yy = q.y * y2;
|
|
float yz = q.y * z2;
|
|
float zz = q.z * z2;
|
|
float wx = q.w * x2;
|
|
float wy = q.w * y2;
|
|
float wz = q.w * z2;
|
|
|
|
mat4_t m;
|
|
m.m[0] = 1.0 - (yy + zz);
|
|
m.m[4] = xy - wz;
|
|
m.m[8] = xz + wy;
|
|
|
|
m.m[1] = xy + wz;
|
|
m.m[5] = 1.0 - (xx + zz);
|
|
m.m[9] = yz - wx;
|
|
|
|
m.m[2] = xz - wy;
|
|
m.m[6] = yz + wx;
|
|
m.m[10] = 1.0 - (xx + yy);
|
|
|
|
m.m[3] = 0.0;
|
|
m.m[7] = 0.0;
|
|
m.m[11] = 0.0;
|
|
m.m[12] = 0.0;
|
|
m.m[13] = 0.0;
|
|
m.m[14] = 0.0;
|
|
m.m[15] = 1.0;
|
|
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_translate(mat4_t m, float x, float y, float z) {
|
|
m.m[0] += x * m.m[3];
|
|
m.m[1] += y * m.m[3];
|
|
m.m[2] += z * m.m[3];
|
|
m.m[4] += x * m.m[7];
|
|
m.m[5] += y * m.m[7];
|
|
m.m[6] += z * m.m[7];
|
|
m.m[8] += x * m.m[11];
|
|
m.m[9] += y * m.m[11];
|
|
m.m[10] += z * m.m[11];
|
|
m.m[12] += x * m.m[15];
|
|
m.m[13] += y * m.m[15];
|
|
m.m[14] += z * m.m[15];
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_scale(mat4_t m, vec4_t v) {
|
|
m.m[0] *= v.x;
|
|
m.m[1] *= v.x;
|
|
m.m[2] *= v.x;
|
|
m.m[3] *= v.x;
|
|
m.m[4] *= v.y;
|
|
m.m[5] *= v.y;
|
|
m.m[6] *= v.y;
|
|
m.m[7] *= v.y;
|
|
m.m[8] *= v.z;
|
|
m.m[9] *= v.z;
|
|
m.m[10] *= v.z;
|
|
m.m[11] *= v.z;
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_mult_mat3x4(mat4_t a, mat4_t b) {
|
|
float a00 = a.m[0];
|
|
float a01 = a.m[1];
|
|
float a02 = a.m[2];
|
|
float a03 = a.m[3];
|
|
float a10 = a.m[4];
|
|
float a11 = a.m[5];
|
|
float a12 = a.m[6];
|
|
float a13 = a.m[7];
|
|
float a20 = a.m[8];
|
|
float a21 = a.m[9];
|
|
float a22 = a.m[10];
|
|
float a23 = a.m[11];
|
|
float a30 = a.m[12];
|
|
float a31 = a.m[13];
|
|
float a32 = a.m[14];
|
|
float a33 = a.m[15];
|
|
float b0 = b.m[0];
|
|
float b1 = b.m[4];
|
|
float b2 = b.m[8];
|
|
float b3 = b.m[12];
|
|
|
|
mat4_t m;
|
|
m.m[0] = a00 * b0 + a01 * b1 + a02 * b2 + a03 * b3;
|
|
m.m[4] = a10 * b0 + a11 * b1 + a12 * b2 + a13 * b3;
|
|
m.m[8] = a20 * b0 + a21 * b1 + a22 * b2 + a23 * b3;
|
|
m.m[12] = a30 * b0 + a31 * b1 + a32 * b2 + a33 * b3;
|
|
|
|
b0 = b.m[1];
|
|
b1 = b.m[5];
|
|
b2 = b.m[9];
|
|
b3 = b.m[13];
|
|
m.m[1] = a00 * b0 + a01 * b1 + a02 * b2 + a03 * b3;
|
|
m.m[5] = a10 * b0 + a11 * b1 + a12 * b2 + a13 * b3;
|
|
m.m[9] = a20 * b0 + a21 * b1 + a22 * b2 + a23 * b3;
|
|
m.m[13] = a30 * b0 + a31 * b1 + a32 * b2 + a33 * b3;
|
|
|
|
b0 = b.m[2];
|
|
b1 = b.m[6];
|
|
b2 = b.m[10];
|
|
b3 = b.m[14];
|
|
m.m[2] = a00 * b0 + a01 * b1 + a02 * b2 + a03 * b3;
|
|
m.m[6] = a10 * b0 + a11 * b1 + a12 * b2 + a13 * b3;
|
|
m.m[10] = a20 * b0 + a21 * b1 + a22 * b2 + a23 * b3;
|
|
m.m[14] = a30 * b0 + a31 * b1 + a32 * b2 + a33 * b3;
|
|
|
|
m.m[3] = 0;
|
|
m.m[7] = 0;
|
|
m.m[11] = 0;
|
|
m.m[15] = 1;
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_mult_mat(mat4_t a, mat4_t b) {
|
|
float a00 = a.m[0];
|
|
float a01 = a.m[1];
|
|
float a02 = a.m[2];
|
|
float a03 = a.m[3];
|
|
float a10 = a.m[4];
|
|
float a11 = a.m[5];
|
|
float a12 = a.m[6];
|
|
float a13 = a.m[7];
|
|
float a20 = a.m[8];
|
|
float a21 = a.m[9];
|
|
float a22 = a.m[10];
|
|
float a23 = a.m[11];
|
|
float a30 = a.m[12];
|
|
float a31 = a.m[13];
|
|
float a32 = a.m[14];
|
|
float a33 = a.m[15];
|
|
|
|
float b0 = b.m[0];
|
|
float b1 = b.m[4];
|
|
float b2 = b.m[8];
|
|
float b3 = b.m[12];
|
|
a.m[0] = a00 * b0 + a01 * b1 + a02 * b2 + a03 * b3;
|
|
a.m[4] = a10 * b0 + a11 * b1 + a12 * b2 + a13 * b3;
|
|
a.m[8] = a20 * b0 + a21 * b1 + a22 * b2 + a23 * b3;
|
|
a.m[12] = a30 * b0 + a31 * b1 + a32 * b2 + a33 * b3;
|
|
|
|
b0 = b.m[1];
|
|
b1 = b.m[5];
|
|
b2 = b.m[9];
|
|
b3 = b.m[13];
|
|
a.m[1] = a00 * b0 + a01 * b1 + a02 * b2 + a03 * b3;
|
|
a.m[5] = a10 * b0 + a11 * b1 + a12 * b2 + a13 * b3;
|
|
a.m[9] = a20 * b0 + a21 * b1 + a22 * b2 + a23 * b3;
|
|
a.m[13] = a30 * b0 + a31 * b1 + a32 * b2 + a33 * b3;
|
|
|
|
b0 = b.m[2];
|
|
b1 = b.m[6];
|
|
b2 = b.m[10];
|
|
b3 = b.m[14];
|
|
a.m[2] = a00 * b0 + a01 * b1 + a02 * b2 + a03 * b3;
|
|
a.m[6] = a10 * b0 + a11 * b1 + a12 * b2 + a13 * b3;
|
|
a.m[10] = a20 * b0 + a21 * b1 + a22 * b2 + a23 * b3;
|
|
a.m[14] = a30 * b0 + a31 * b1 + a32 * b2 + a33 * b3;
|
|
|
|
b0 = b.m[3];
|
|
b1 = b.m[7];
|
|
b2 = b.m[11];
|
|
b3 = b.m[15];
|
|
a.m[3] = a00 * b0 + a01 * b1 + a02 * b2 + a03 * b3;
|
|
a.m[7] = a10 * b0 + a11 * b1 + a12 * b2 + a13 * b3;
|
|
a.m[11] = a20 * b0 + a21 * b1 + a22 * b2 + a23 * b3;
|
|
a.m[15] = a30 * b0 + a31 * b1 + a32 * b2 + a33 * b3;
|
|
|
|
return a;
|
|
}
|
|
|
|
mat4_t mat4_inv(mat4_t a) {
|
|
float a00 = a.m[0];
|
|
float a01 = a.m[1];
|
|
float a02 = a.m[2];
|
|
float a03 = a.m[3];
|
|
float a10 = a.m[4];
|
|
float a11 = a.m[5];
|
|
float a12 = a.m[6];
|
|
float a13 = a.m[7];
|
|
float a20 = a.m[8];
|
|
float a21 = a.m[9];
|
|
float a22 = a.m[10];
|
|
float a23 = a.m[11];
|
|
float a30 = a.m[12];
|
|
float a31 = a.m[13];
|
|
float a32 = a.m[14];
|
|
float a33 = a.m[15];
|
|
float b00 = a00 * a11 - a01 * a10;
|
|
float b01 = a00 * a12 - a02 * a10;
|
|
float b02 = a00 * a13 - a03 * a10;
|
|
float b03 = a01 * a12 - a02 * a11;
|
|
float b04 = a01 * a13 - a03 * a11;
|
|
float b05 = a02 * a13 - a03 * a12;
|
|
float b06 = a20 * a31 - a21 * a30;
|
|
float b07 = a20 * a32 - a22 * a30;
|
|
float b08 = a20 * a33 - a23 * a30;
|
|
float b09 = a21 * a32 - a22 * a31;
|
|
float b10 = a21 * a33 - a23 * a31;
|
|
float b11 = a22 * a33 - a23 * a32;
|
|
|
|
float det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
|
|
if (det == 0.0) {
|
|
return mat4_identity();
|
|
}
|
|
det = 1.0 / det;
|
|
|
|
mat4_t m;
|
|
m.m[0] = (a11 * b11 - a12 * b10 + a13 * b09) * det;
|
|
m.m[1] = (a02 * b10 - a01 * b11 - a03 * b09) * det;
|
|
m.m[2] = (a31 * b05 - a32 * b04 + a33 * b03) * det;
|
|
m.m[3] = (a22 * b04 - a21 * b05 - a23 * b03) * det;
|
|
m.m[4] = (a12 * b08 - a10 * b11 - a13 * b07) * det;
|
|
m.m[5] = (a00 * b11 - a02 * b08 + a03 * b07) * det;
|
|
m.m[6] = (a32 * b02 - a30 * b05 - a33 * b01) * det;
|
|
m.m[7] = (a20 * b05 - a22 * b02 + a23 * b01) * det;
|
|
m.m[8] = (a10 * b10 - a11 * b08 + a13 * b06) * det;
|
|
m.m[9] = (a01 * b08 - a00 * b10 - a03 * b06) * det;
|
|
m.m[10] = (a30 * b04 - a31 * b02 + a33 * b00) * det;
|
|
m.m[11] = (a21 * b02 - a20 * b04 - a23 * b00) * det;
|
|
m.m[12] = (a11 * b07 - a10 * b09 - a12 * b06) * det;
|
|
m.m[13] = (a00 * b09 - a01 * b07 + a02 * b06) * det;
|
|
m.m[14] = (a31 * b01 - a30 * b03 - a32 * b00) * det;
|
|
m.m[15] = (a20 * b03 - a21 * b01 + a22 * b00) * det;
|
|
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_transpose(mat4_t m) {
|
|
float f = m.m[1];
|
|
m.m[1] = m.m[4];
|
|
m.m[4] = f;
|
|
|
|
f = m.m[2];
|
|
m.m[2] = m.m[8];
|
|
m.m[8] = f;
|
|
|
|
f = m.m[3];
|
|
m.m[3] = m.m[12];
|
|
m.m[12] = f;
|
|
|
|
f = m.m[6];
|
|
m.m[6] = m.m[9];
|
|
m.m[9] = f;
|
|
|
|
f = m.m[7];
|
|
m.m[7] = m.m[13];
|
|
m.m[13] = f;
|
|
|
|
f = m.m[11];
|
|
m.m[11] = m.m[14];
|
|
m.m[14] = f;
|
|
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_transpose3(mat4_t m) {
|
|
float f = m.m[1];
|
|
m.m[1] = m.m[4];
|
|
m.m[4] = f;
|
|
|
|
f = m.m[2];
|
|
m.m[2] = m.m[8];
|
|
m.m[8] = f;
|
|
|
|
f = m.m[6];
|
|
m.m[6] = m.m[9];
|
|
m.m[9] = f;
|
|
|
|
return m;
|
|
}
|
|
|
|
vec4_t mat4_get_loc(mat4_t m) {
|
|
return (vec4_t){m.m[12], m.m[13], m.m[14], m.m[15]};
|
|
}
|
|
|
|
vec4_t mat4_get_scale(mat4_t m) {
|
|
return (vec4_t){sqrtf(m.m[0] * m.m[0] + m.m[4] * m.m[4] + m.m[8] * m.m[8]), sqrtf(m.m[1] * m.m[1] + m.m[5] * m.m[5] + m.m[9] * m.m[9]),
|
|
sqrtf(m.m[2] * m.m[2] + m.m[6] * m.m[6] + m.m[10] * m.m[10]), 1.0};
|
|
}
|
|
|
|
mat4_t mat4_mult(mat4_t m, float s) {
|
|
m.m[0] *= s;
|
|
m.m[1] *= s;
|
|
m.m[2] *= s;
|
|
m.m[3] *= s;
|
|
m.m[4] *= s;
|
|
m.m[5] *= s;
|
|
m.m[6] *= s;
|
|
m.m[7] *= s;
|
|
m.m[8] *= s;
|
|
m.m[9] *= s;
|
|
m.m[10] *= s;
|
|
m.m[11] *= s;
|
|
m.m[12] *= s;
|
|
m.m[13] *= s;
|
|
m.m[14] *= s;
|
|
m.m[15] *= s;
|
|
return m;
|
|
}
|
|
|
|
mat4_t mat4_to_rot(mat4_t m) {
|
|
float scale = 1.0 / vec4_len((vec4_t){m.m[0], m.m[1], m.m[2], 1.0});
|
|
m.m[0] = m.m[0] * scale;
|
|
m.m[1] = m.m[1] * scale;
|
|
m.m[2] = m.m[2] * scale;
|
|
scale = 1.0 / vec4_len((vec4_t){m.m[4], m.m[5], m.m[6], 1.0});
|
|
m.m[4] = m.m[4] * scale;
|
|
m.m[5] = m.m[5] * scale;
|
|
m.m[6] = m.m[6] * scale;
|
|
scale = 1.0 / vec4_len((vec4_t){m.m[8], m.m[9], m.m[10], 1.0});
|
|
m.m[8] = m.m[8] * scale;
|
|
m.m[9] = m.m[9] * scale;
|
|
m.m[10] = m.m[10] * scale;
|
|
m.m[3] = 0.0;
|
|
m.m[7] = 0.0;
|
|
m.m[11] = 0.0;
|
|
m.m[12] = 0.0;
|
|
m.m[13] = 0.0;
|
|
m.m[14] = 0.0;
|
|
m.m[15] = 1.0;
|
|
return m;
|
|
}
|
|
|
|
vec4_t mat4_right(mat4_t m) {
|
|
return vec4_norm((vec4_t){m.m[0], m.m[1], m.m[2], 1.0});
|
|
}
|
|
|
|
vec4_t mat4_look(mat4_t m) {
|
|
return vec4_norm((vec4_t){m.m[4], m.m[5], m.m[6], 1.0});
|
|
}
|
|
|
|
vec4_t mat4_up(mat4_t m) {
|
|
return vec4_norm((vec4_t){m.m[8], m.m[9], m.m[10], 1.0});
|
|
}
|
|
|
|
float mat4_cofactor(float m0, float m1, float m2, float m3, float m4, float m5, float m6, float m7, float m8) {
|
|
return m0 * (m4 * m8 - m5 * m7) - m1 * (m3 * m8 - m5 * m6) + m2 * (m3 * m7 - m4 * m6);
|
|
}
|
|
|
|
float mat4_determinant(mat4_t m) {
|
|
float c00 = mat4_cofactor(m.m[5], m.m[9], m.m[13], m.m[6], m.m[10], m.m[14], m.m[7], m.m[11], m.m[15]);
|
|
float c01 = mat4_cofactor(m.m[4], m.m[8], m.m[12], m.m[6], m.m[10], m.m[14], m.m[7], m.m[11], m.m[15]);
|
|
float c02 = mat4_cofactor(m.m[4], m.m[8], m.m[12], m.m[5], m.m[9], m.m[13], m.m[7], m.m[11], m.m[15]);
|
|
float c03 = mat4_cofactor(m.m[4], m.m[8], m.m[12], m.m[5], m.m[9], m.m[13], m.m[6], m.m[10], m.m[14]);
|
|
return m.m[0] * c00 - m.m[1] * c01 + m.m[2] * c02 - m.m[3] * c03;
|
|
}
|
|
|
|
mat4_t mat4_nan() {
|
|
mat4_t m;
|
|
m.m[0] = NAN;
|
|
return m;
|
|
}
|
|
|
|
bool mat4_isnan(mat4_t m) {
|
|
return isnan(m.m[0]);
|
|
}
|