singe/src/math3d.c

/*
 *
 *  Singe 3
 *  Copyright (C) 2006-2026 Scott Duensing <scott@kangaroopunch.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 3
 * of the License, or (at your option) any later version.
 *
 * This program 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 General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301, USA.
 *
 */

// Vector, quaternion and matrix arithmetic for the 3D scene.  Small on purpose:  only what a scene
// graph, a camera and glTF animation need.

#include <math.h>
#include <string.h>
#include "math3d.h"


// Translation * rotation * scale, the glTF node transform.
Mat4T mat4Compose(Vec3T translation, QuatT rotation, Vec3T scale) {
	Mat4T out;
	float xx = rotation.x * rotation.x;
	float yy = rotation.y * rotation.y;
	float zz = rotation.z * rotation.z;
	float xy = rotation.x * rotation.y;
	float xz = rotation.x * rotation.z;
	float yz = rotation.y * rotation.z;
	float wx = rotation.w * rotation.x;
	float wy = rotation.w * rotation.y;
	float wz = rotation.w * rotation.z;

	out.m[0]  = (1.0f - 2.0f * (yy + zz)) * scale.x;
	out.m[1]  = (2.0f * (xy + wz)) * scale.x;
	out.m[2]  = (2.0f * (xz - wy)) * scale.x;
	out.m[3]  = 0.0f;
	out.m[4]  = (2.0f * (xy - wz)) * scale.y;
	out.m[5]  = (1.0f - 2.0f * (xx + zz)) * scale.y;
	out.m[6]  = (2.0f * (yz + wx)) * scale.y;
	out.m[7]  = 0.0f;
	out.m[8]  = (2.0f * (xz + wy)) * scale.z;
	out.m[9]  = (2.0f * (yz - wx)) * scale.z;
	out.m[10] = (1.0f - 2.0f * (xx + yy)) * scale.z;
	out.m[11] = 0.0f;
	out.m[12] = translation.x;
	out.m[13] = translation.y;
	out.m[14] = translation.z;
	out.m[15] = 1.0f;
	return out;
}


// Splits a TRS matrix back into its parts (glTF nodes may carry a matrix instead of TRS).
void mat4Decompose(Mat4T a, Vec3T *translation, QuatT *rotation, Vec3T *scale) {
	Vec3T x = vec3(a.m[0], a.m[1], a.m[2]);
	Vec3T y = vec3(a.m[4], a.m[5], a.m[6]);
	Vec3T z = vec3(a.m[8], a.m[9], a.m[10]);
	Mat4T r = mat4Identity();

	*translation = vec3(a.m[12], a.m[13], a.m[14]);
	*scale       = vec3(vec3Length(x), vec3Length(y), vec3Length(z));
	// A negative determinant means one axis is mirrored; put the flip on X.
	if (vec3Dot(vec3Cross(x, y), z) < 0.0f) {
		scale->x = -scale->x;
	}
	x = vec3Scale(x, (scale->x != 0.0f) ? 1.0f / scale->x : 0.0f);
	y = vec3Scale(y, (scale->y != 0.0f) ? 1.0f / scale->y : 0.0f);
	z = vec3Scale(z, (scale->z != 0.0f) ? 1.0f / scale->z : 0.0f);
	r.m[0]  = x.x;
	r.m[1]  = x.y;
	r.m[2]  = x.z;
	r.m[4]  = y.x;
	r.m[5]  = y.y;
	r.m[6]  = y.z;
	r.m[8]  = z.x;
	r.m[9]  = z.y;
	r.m[10] = z.z;
	*rotation = quatFromMat4(r);
}


Mat4T mat4Identity(void) {
	Mat4T out;

	memset(&out, 0, sizeof(out));
	out.m[0]  = 1.0f;
	out.m[5]  = 1.0f;
	out.m[10] = 1.0f;
	out.m[15] = 1.0f;
	return out;
}


// General 4x4 inverse by cofactors.  Returns false for a singular matrix (out untouched):  one
// whose determinant is zero or too small to take a finite reciprocal of, so a node scaled down by
// any ordinary amount still inverts.
bool mat4Invert(Mat4T a, Mat4T *out) {
	float   inv[16];
	float   det;
	int32_t x;
	const float *m = a.m;

	inv[0]  =  m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10];
	inv[4]  = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10];
	inv[8]  =  m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9];
	inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9];
	inv[1]  = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10];
	inv[5]  =  m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10];
	inv[9]  = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9];
	inv[13] =  m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9];
	inv[2]  =  m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
	inv[6]  = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
	inv[10] =  m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
	inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
	inv[3]  = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
	inv[7]  =  m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
	inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
	inv[15] =  m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];
	det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
	if ((det == 0.0f) || !isfinite(1.0f / det)) {
		return false;
	}
	det = 1.0f / det;
	for (x = 0; x < 16; x++) {
		out->m[x] = inv[x] * det;
	}
	return true;
}


// A view matrix:  the camera at eye looking at target.
Mat4T mat4LookAt(Vec3T eye, Vec3T target, Vec3T up) {
	Mat4T out;
	Vec3T f = vec3Normalize(vec3Subtract(target, eye));
	Vec3T s = vec3Normalize(vec3Cross(f, up));
	Vec3T u = vec3Cross(s, f);

	out.m[0]  = s.x;
	out.m[1]  = u.x;
	out.m[2]  = -f.x;
	out.m[3]  = 0.0f;
	out.m[4]  = s.y;
	out.m[5]  = u.y;
	out.m[6]  = -f.y;
	out.m[7]  = 0.0f;
	out.m[8]  = s.z;
	out.m[9]  = u.z;
	out.m[10] = -f.z;
	out.m[11] = 0.0f;
	out.m[12] = -vec3Dot(s, eye);
	out.m[13] = -vec3Dot(u, eye);
	out.m[14] = vec3Dot(f, eye);
	out.m[15] = 1.0f;
	return out;
}


// a * b:  applies b first, then a.
Mat4T mat4Multiply(Mat4T a, Mat4T b) {
	Mat4T   out;
	int32_t column;
	int32_t row;
	int32_t k;
	float   sum;

	for (column = 0; column < 4; column++) {
		for (row = 0; row < 4; row++) {
			sum = 0.0f;
			for (k = 0; k < 4; k++) {
				sum += a.m[k * 4 + row] * b.m[column * 4 + k];
			}
			out.m[column * 4 + row] = sum;
		}
	}
	return out;
}


// The matrix that carries normals under a's rotation and scale:  the inverse transpose of its
// upper 3x3, taken as the cofactor matrix over the determinant (no 4x4 inverse needed).  Under a
// singular matrix (a zero scale) the normals keep their direction.
Mat4T mat4NormalMatrix(Mat4T a) {
	Mat4T        out = mat4Identity();
	const float *m   = a.m;
	float        c[9];
	float        det;
	int32_t      x;

	c[0] = m[5] * m[10] - m[6] * m[9];
	c[1] = m[6] * m[8] - m[4] * m[10];
	c[2] = m[4] * m[9] - m[5] * m[8];
	c[3] = m[2] * m[9] - m[1] * m[10];
	c[4] = m[0] * m[10] - m[2] * m[8];
	c[5] = m[1] * m[8] - m[0] * m[9];
	c[6] = m[1] * m[6] - m[2] * m[5];
	c[7] = m[2] * m[4] - m[0] * m[6];
	c[8] = m[0] * m[5] - m[1] * m[4];
	det  = m[0] * c[0] + m[1] * c[1] + m[2] * c[2];
	if ((det == 0.0f) || !isfinite(1.0f / det)) {
		return out;
	}
	det = 1.0f / det;
	for (x = 0; x < 9; x++) {
		out.m[(x / 3) * 4 + (x % 3)] = c[x] * det;
	}
	return out;
}


// Depth maps to 0..1 (what SDL_GPU expects on every backend).
Mat4T mat4Orthographic(float width, float height, float near, float far) {
	Mat4T out;

	memset(&out, 0, sizeof(out));
	out.m[0]  = 2.0f / width;
	out.m[5]  = 2.0f / height;
	out.m[10] = -1.0f / (far - near);
	out.m[14] = -near / (far - near);
	out.m[15] = 1.0f;
	return out;
}


// An off-centre parallel projection (a shadow map fitted to what it covers).
Mat4T mat4OrthographicBounds(float left, float right, float bottom, float top, float near, float far) {
	Mat4T out;

	memset(&out, 0, sizeof(out));
	out.m[0]  = 2.0f / (right - left);
	out.m[5]  = 2.0f / (top - bottom);
	out.m[10] = -1.0f / (far - near);
	out.m[12] = -(right + left) / (right - left);
	out.m[13] = -(top + bottom) / (top - bottom);
	out.m[14] = -near / (far - near);
	out.m[15] = 1.0f;
	return out;
}


Mat4T mat4Perspective(float fovDegrees, float aspect, float near, float far) {
	Mat4T out;
	float f = 1.0f / tanf(DEGREES_TO_RADIANS(fovDegrees) / 2.0f);

	memset(&out, 0, sizeof(out));
	out.m[0]  = f / aspect;
	out.m[5]  = f;
	out.m[10] = far / (near - far);
	out.m[11] = -1.0f;
	out.m[14] = (near * far) / (near - far);
	return out;
}


// A point through a matrix, divided by the clip w it comes out with (which is handed back for
// callers that need to know which side of the camera the point is on).
Vec3T mat4Project(Mat4T a, Vec3T p, float *w) {
	Vec3T out;

	*w    = a.m[3] * p.x + a.m[7] * p.y + a.m[11] * p.z + a.m[15];
	out.x = a.m[0] * p.x + a.m[4] * p.y + a.m[8] * p.z + a.m[12];
	out.y = a.m[1] * p.x + a.m[5] * p.y + a.m[9] * p.z + a.m[13];
	out.z = a.m[2] * p.x + a.m[6] * p.y + a.m[10] * p.z + a.m[14];
	if (fabsf(*w) > MATH_EPSILON) {
		out.x /= *w;
		out.y /= *w;
		out.z /= *w;
	}
	return out;
}


Vec3T mat4TransformPoint(Mat4T a, Vec3T p) {
	float w;

	return mat4Project(a, p, &w);
}


// Directions ignore translation.
Vec3T mat4TransformVector(Mat4T a, Vec3T v) {
	Vec3T out;

	out.x = a.m[0] * v.x + a.m[4] * v.y + a.m[8] * v.z;
	out.y = a.m[1] * v.x + a.m[5] * v.y + a.m[9] * v.z;
	out.z = a.m[2] * v.x + a.m[6] * v.y + a.m[10] * v.z;
	return out;
}


Mat4T mat4Transpose(Mat4T a) {
	Mat4T   out;
	int32_t column;
	int32_t row;

	for (column = 0; column < 4; column++) {
		for (row = 0; row < 4; row++) {
			out.m[column * 4 + row] = a.m[row * 4 + column];
		}
	}
	return out;
}


QuatT quatFromAxisAngle(Vec3T axis, float degrees) {
	QuatT out;
	float half = DEGREES_TO_RADIANS(degrees) / 2.0f;
	float s    = sinf(half);
	Vec3T n    = vec3Normalize(axis);

	out.x = n.x * s;
	out.y = n.y * s;
	out.z = n.z * s;
	out.w = cosf(half);
	return out;
}


// Intrinsic rotations applied in the order Y (yaw), X (pitch), Z (roll), matching what a script
// means by "turn, then tilt, then bank".
QuatT quatFromEuler(float xDegrees, float yDegrees, float zDegrees) {
	QuatT qx = quatFromAxisAngle(vec3(1.0f, 0.0f, 0.0f), xDegrees);
	QuatT qy = quatFromAxisAngle(vec3(0.0f, 1.0f, 0.0f), yDegrees);
	QuatT qz = quatFromAxisAngle(vec3(0.0f, 0.0f, 1.0f), zDegrees);

	return quatMultiply(quatMultiply(qy, qx), qz);
}


// The rotation part of a matrix, assuming no scale.
QuatT quatFromMat4(Mat4T a) {
	QuatT out;
	float trace = a.m[0] + a.m[5] + a.m[10];
	float s;

	if (trace > 0.0f) {
		s     = sqrtf(trace + 1.0f) * 2.0f;
		out.w = 0.25f * s;
		out.x = (a.m[6] - a.m[9]) / s;
		out.y = (a.m[8] - a.m[2]) / s;
		out.z = (a.m[1] - a.m[4]) / s;
	} else if ((a.m[0] > a.m[5]) && (a.m[0] > a.m[10])) {
		s     = sqrtf(1.0f + a.m[0] - a.m[5] - a.m[10]) * 2.0f;
		out.w = (a.m[6] - a.m[9]) / s;
		out.x = 0.25f * s;
		out.y = (a.m[4] + a.m[1]) / s;
		out.z = (a.m[8] + a.m[2]) / s;
	} else if (a.m[5] > a.m[10]) {
		s     = sqrtf(1.0f + a.m[5] - a.m[0] - a.m[10]) * 2.0f;
		out.w = (a.m[8] - a.m[2]) / s;
		out.x = (a.m[4] + a.m[1]) / s;
		out.y = 0.25f * s;
		out.z = (a.m[9] + a.m[6]) / s;
	} else {
		s     = sqrtf(1.0f + a.m[10] - a.m[0] - a.m[5]) * 2.0f;
		out.w = (a.m[1] - a.m[4]) / s;
		out.x = (a.m[8] + a.m[2]) / s;
		out.y = (a.m[9] + a.m[6]) / s;
		out.z = 0.25f * s;
	}
	return quatNormalize(out);
}


QuatT quatIdentity(void) {
	QuatT out = { 0.0f, 0.0f, 0.0f, 1.0f };

	return out;
}


// The inverse rotation:  normalised, then conjugated.
QuatT quatInverse(QuatT q) {
	QuatT out = quatNormalize(q);

	out.x = -out.x;
	out.y = -out.y;
	out.z = -out.z;
	return out;
}


// The rotation that points -Z along forward with +Y near up.
QuatT quatLookRotation(Vec3T forward, Vec3T up) {
	Mat4T m;
	Vec3T f = vec3Normalize(forward);
	Vec3T s;
	Vec3T u;

	if (vec3Length(vec3Cross(f, up)) < MATH_EPSILON) {
		// Looking straight along up:  pick any perpendicular.
		up = (fabsf(f.y) < 0.9f) ? vec3(0.0f, 1.0f, 0.0f) : vec3(0.0f, 0.0f, 1.0f);
	}
	s = vec3Normalize(vec3Cross(f, up));
	u = vec3Cross(s, f);
	m = mat4Identity();
	m.m[0]  = s.x;
	m.m[1]  = s.y;
	m.m[2]  = s.z;
	m.m[4]  = u.x;
	m.m[5]  = u.y;
	m.m[6]  = u.z;
	m.m[8]  = -f.x;
	m.m[9]  = -f.y;
	m.m[10] = -f.z;
	return quatFromMat4(m);
}


// a * b:  applies b first, then a.
QuatT quatMultiply(QuatT a, QuatT b) {
	QuatT out;

	out.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
	out.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
	out.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
	out.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
	return out;
}


QuatT quatNormalize(QuatT q) {
	float length = sqrtf(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);

	if (length < MATH_EPSILON) {
		return quatIdentity();
	}
	q.x /= length;
	q.y /= length;
	q.z /= length;
	q.w /= length;
	return q;
}


Vec3T quatRotate(QuatT q, Vec3T v) {
	Vec3T u = vec3(q.x, q.y, q.z);
	Vec3T t = vec3Scale(vec3Cross(u, v), 2.0f);

	return vec3Add(vec3Add(v, vec3Scale(t, q.w)), vec3Cross(u, t));
}


QuatT quatSlerp(QuatT a, QuatT b, float t) {
	QuatT out;
	float cosTheta = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
	float theta;
	float sinTheta;
	float wa;
	float wb;

	// Take the short way round.
	if (cosTheta < 0.0f) {
		b.x      = -b.x;
		b.y      = -b.y;
		b.z      = -b.z;
		b.w      = -b.w;
		cosTheta = -cosTheta;
	}
	if (cosTheta > 1.0f - MATH_EPSILON) {
		// Nearly parallel:  lerp is accurate and avoids the division.
		out.x = a.x + (b.x - a.x) * t;
		out.y = a.y + (b.y - a.y) * t;
		out.z = a.z + (b.z - a.z) * t;
		out.w = a.w + (b.w - a.w) * t;
		return quatNormalize(out);
	}
	theta    = acosf(cosTheta);
	sinTheta = sinf(theta);
	wa       = sinf((1.0f - t) * theta) / sinTheta;
	wb       = sinf(t * theta) / sinTheta;
	out.x    = a.x * wa + b.x * wb;
	out.y    = a.y * wa + b.y * wb;
	out.z    = a.z * wa + b.z * wb;
	out.w    = a.w * wa + b.w * wb;
	return out;
}


// The inverse of quatFromEuler (Y, then X, then Z).
void quatToEuler(QuatT q, float *xDegrees, float *yDegrees, float *zDegrees) {
	Mat4T m = mat4Compose(vec3(0.0f, 0.0f, 0.0f), q, vec3(1.0f, 1.0f, 1.0f));
	float sinX = -m.m[9];

	if (sinX > 1.0f) {
		sinX = 1.0f;
	}
	if (sinX < -1.0f) {
		sinX = -1.0f;
	}
	*xDegrees = RADIANS_TO_DEGREES(asinf(sinX));
	if (fabsf(sinX) < 1.0f - MATH_EPSILON) {
		*yDegrees = RADIANS_TO_DEGREES(atan2f(m.m[8], m.m[10]));
		*zDegrees = RADIANS_TO_DEGREES(atan2f(m.m[1], m.m[5]));
	} else {
		// Gimbal lock:  give all the twist to Y.
		*yDegrees = RADIANS_TO_DEGREES(atan2f(-m.m[2], m.m[0]));
		*zDegrees = 0.0f;
	}
}


Vec3T vec3(float x, float y, float z) {
	Vec3T out = { x, y, z };

	return out;
}


Vec3T vec3Add(Vec3T a, Vec3T b) {
	return vec3(a.x + b.x, a.y + b.y, a.z + b.z);
}


Vec3T vec3Cross(Vec3T a, Vec3T b) {
	return vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}


float vec3Dot(Vec3T a, Vec3T b) {
	return a.x * b.x + a.y * b.y + a.z * b.z;
}


float vec3Length(Vec3T a) {
	return sqrtf(vec3Dot(a, a));
}


Vec3T vec3Lerp(Vec3T a, Vec3T b, float t) {
	return vec3Add(a, vec3Scale(vec3Subtract(b, a), t));
}


Vec3T vec3Normalize(Vec3T a) {
	float length = vec3Length(a);

	if (length < MATH_EPSILON) {
		return vec3(0.0f, 0.0f, 0.0f);
	}
	return vec3Scale(a, 1.0f / length);
}


Vec3T vec3Scale(Vec3T a, float s) {
	return vec3(a.x * s, a.y * s, a.z * s);
}


Vec3T vec3Subtract(Vec3T a, Vec3T b) {
	return vec3(a.x - b.x, a.y - b.y, a.z - b.z);
}