From 6174eda0fc17901d88b8ff238f02c1d92b78e219 Mon Sep 17 00:00:00 2001
From: havoc <havoc@d7cf8633-e32d-0410-b094-e92efae38249>
Date: Sun, 7 Jul 2002 23:42:36 +0000
Subject: [PATCH] created a couple matrix libraries (4x4 and 3x4), hopefully
 these will be useful

git-svn-id: svn://svn.icculus.org/twilight/trunk/darkplaces@2009 d7cf8633-e32d-0410-b094-e92efae38249
---
 makefile    |   2 +-
 matrix3x4.c | 219 ++++++++++++++++++++++++++++++++++++
 matrix3x4.h |  35 ++++++
 matrix4x4.c | 312 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 matrix4x4.h |  39 +++++++
 5 files changed, 606 insertions(+), 1 deletion(-)
 create mode 100644 matrix3x4.c
 create mode 100644 matrix3x4.h
 create mode 100644 matrix4x4.c
 create mode 100644 matrix4x4.h

diff --git a/makefile b/makefile
index 55b1a77f..b93a0f47 100644
--- a/makefile
+++ b/makefile
@@ -15,7 +15,7 @@ SOUNDLIB=
 #if you want no CD audio
 CD=cd_null.o
 
-OBJECTS= builddate.o $(CD) chase.o cl_demo.o cl_input.o cl_main.o cl_parse.o cl_tent.o cmd.o common.o console.o crc.o cvar.o fractalnoise.o gl_draw.o r_sky.o gl_rmain.o gl_rsurf.o host.o host_cmd.o image.o keys.o mathlib.o menu.o model_alias.o model_brush.o model_shared.o model_sprite.o net_bsd.o net_udp.o net_dgrm.o net_loop.o net_main.o pr_cmds.o pr_edict.o pr_exec.o r_light.o r_explosion.o sbar.o snd_dma.o snd_mem.o snd_mix.o $(SND) sv_main.o sv_move.o sv_phys.o sv_user.o sv_light.o sys_linux.o transform.o view.o wad.o world.o zone.o vid_shared.o palette.o r_crosshairs.o gl_textures.o gl_models.o r_sprites.o r_modules.o r_explosion.o r_lerpanim.o protocol.o quakeio.o r_clip.o ui.o portals.o sys_shared.o cl_light.o gl_backend.o cl_particles.o cl_screen.o cgamevm.o cgame.o filematch.o collision.o cl_collision.o
+OBJECTS= builddate.o $(CD) chase.o cl_demo.o cl_input.o cl_main.o cl_parse.o cl_tent.o cmd.o common.o console.o crc.o cvar.o fractalnoise.o gl_draw.o r_sky.o gl_rmain.o gl_rsurf.o host.o host_cmd.o image.o keys.o mathlib.o menu.o model_alias.o model_brush.o model_shared.o model_sprite.o net_bsd.o net_udp.o net_dgrm.o net_loop.o net_main.o pr_cmds.o pr_edict.o pr_exec.o r_light.o r_explosion.o sbar.o snd_dma.o snd_mem.o snd_mix.o $(SND) sv_main.o sv_move.o sv_phys.o sv_user.o sv_light.o sys_linux.o transform.o view.o wad.o world.o zone.o vid_shared.o palette.o r_crosshairs.o gl_textures.o gl_models.o r_sprites.o r_modules.o r_explosion.o r_lerpanim.o protocol.o quakeio.o r_clip.o ui.o portals.o sys_shared.o cl_light.o gl_backend.o cl_particles.o cl_screen.o cgamevm.o cgame.o filematch.o collision.o cl_collision.o matrix4x4.o matrix3x4.o
 
 #K6/athlon optimizations
 CPUOPTIMIZATIONS=-march=k6
diff --git a/matrix3x4.c b/matrix3x4.c
new file mode 100644
index 00000000..a716b120
--- /dev/null
+++ b/matrix3x4.c
@@ -0,0 +1,219 @@
+
+#include "matrix3x4.h"
+#include <math.h>
+
+void Matrix3x4_Copy (matrix3x4_t *out, matrix3x4_t *in)
+{
+	*out = *in;
+}
+
+void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2)
+{
+	out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0];
+	out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1];
+	out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2];
+	out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3];
+	out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0];
+	out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1];
+	out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2];
+	out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3];
+	out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0];
+	out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1];
+	out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2];
+	out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3];
+}
+
+void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1)
+{
+	out->m[0][0] = in1->m[0][0];
+	out->m[0][1] = in1->m[1][0];
+	out->m[0][2] = in1->m[2][0];
+	out->m[0][3] = 0.0f;
+	out->m[1][0] = in1->m[0][1];
+	out->m[1][1] = in1->m[1][1];
+	out->m[1][2] = in1->m[2][1];
+	out->m[1][3] = 0.0f;
+	out->m[2][0] = in1->m[0][2];
+	out->m[2][1] = in1->m[1][2];
+	out->m[2][2] = in1->m[2][2];
+	out->m[2][3] = 0.0f;
+}
+
+void Matrix3x4_CreateIdentity (matrix3x4_t *out)
+{
+	out->m[0][0]=1.0f;
+	out->m[0][1]=0.0f;
+	out->m[0][2]=0.0f;
+	out->m[0][3]=0.0f;
+	out->m[1][0]=0.0f;
+	out->m[1][1]=1.0f;
+	out->m[1][2]=0.0f;
+	out->m[1][3]=0.0f;
+	out->m[2][0]=0.0f;
+	out->m[2][1]=0.0f;
+	out->m[2][2]=1.0f;
+	out->m[2][3]=0.0f;
+}
+
+void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z)
+{
+	out->m[0][0]=1.0f;
+	out->m[0][1]=0.0f;
+	out->m[0][2]=0.0f;
+	out->m[0][3]=x;
+	out->m[1][0]=0.0f;
+	out->m[1][1]=1.0f;
+	out->m[1][2]=0.0f;
+	out->m[1][3]=y;
+	out->m[2][0]=0.0f;
+	out->m[2][1]=0.0f;
+	out->m[2][2]=1.0f;
+	out->m[2][3]=z;
+}
+
+void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z)
+{
+	float len, c, s;
+
+	len = x*x+y*y+z*z;
+	if (len != 0.0f)
+		len = 1.0f / sqrt(len);
+	x *= len;
+	y *= len;
+	z *= len;
+
+	c = cos(angle);
+	s = sin(angle);
+
+	out->m[0][0]=x * x + c * (1 - x * x);
+	out->m[0][1]=x * y * (1 - c) + z * s;
+	out->m[0][2]=z * x * (1 - c) - y * s;
+	out->m[0][3]=0.0f;
+	out->m[1][0]=x * y * (1 - c) - z * s;
+	out->m[1][1]=y * y + c * (1 - y * y);
+	out->m[1][2]=y * z * (1 - c) + x * s;
+	out->m[1][3]=0.0f;
+	out->m[2][0]=z * x * (1 - c) + y * s;
+	out->m[2][1]=y * z * (1 - c) - x * s;
+	out->m[2][2]=z * z + c * (1 - z * z);
+	out->m[2][3]=0.0f;
+}
+
+void Matrix3x4_CreateScale (matrix3x4_t *out, float x)
+{
+	out->m[0][0]=x;
+	out->m[0][1]=0.0f;
+	out->m[0][2]=0.0f;
+	out->m[0][3]=0.0f;
+	out->m[1][0]=0.0f;
+	out->m[1][1]=x;
+	out->m[1][2]=0.0f;
+	out->m[1][3]=0.0f;
+	out->m[2][0]=0.0f;
+	out->m[2][1]=0.0f;
+	out->m[2][2]=x;
+	out->m[2][3]=0.0f;
+}
+
+void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z)
+{
+	out->m[0][0]=x;
+	out->m[0][1]=0.0f;
+	out->m[0][2]=0.0f;
+	out->m[0][3]=0.0f;
+	out->m[1][0]=0.0f;
+	out->m[1][1]=y;
+	out->m[1][2]=0.0f;
+	out->m[1][3]=0.0f;
+	out->m[2][0]=0.0f;
+	out->m[2][1]=0.0f;
+	out->m[2][2]=z;
+	out->m[2][3]=0.0f;
+}
+
+void Matrix3x4_ToVectors(const matrix3x4_t *in, vec3_t vx, vec3_t vy, vec3_t vz, vec3_t t)
+{
+	vx[0] = in->m[0][0];
+	vx[1] = in->m[1][0];
+	vx[2] = in->m[2][0];
+	vy[0] = in->m[0][1];
+	vy[1] = in->m[1][1];
+	vy[2] = in->m[2][1];
+	vz[0] = in->m[0][2];
+	vz[1] = in->m[1][2];
+	vz[2] = in->m[2][2];
+	t[0] = in->m[0][3];
+	t[1] = in->m[1][3];
+	t[2] = in->m[2][3];
+}
+
+void Matrix3x4_FromVectors(matrix3x4_t *out, const vec3_t vx, const vec3_t vy, const vec3_t vz, const vec3_t t)
+{
+	out->m[0][0] = vx[0];
+	out->m[0][1] = vy[0];
+	out->m[0][2] = vz[0];
+	out->m[0][3] = t[0];
+	out->m[1][0] = vx[1];
+	out->m[1][1] = vy[1];
+	out->m[1][2] = vz[1];
+	out->m[1][3] = t[1];
+	out->m[2][0] = vx[2];
+	out->m[2][1] = vy[2];
+	out->m[2][2] = vz[2];
+	out->m[2][3] = t[2];
+}
+
+void Matrix3x4_Transform (const matrix3x4_t *in, const vec3_t v, vec3_t out)
+{
+	out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
+	out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
+	out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
+}
+
+void Matrix3x4_Untransform (const matrix3x4_t *in, const vec3_t v, vec3_t out)
+{
+	vec3_t t;
+	t[0] = v[0] - in->m[0][3];
+	t[1] = v[1] - in->m[1][3];
+	t[2] = v[2] - in->m[2][3];
+	out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
+	out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
+	out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
+}
+
+// FIXME: optimize
+void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z)
+{
+	matrix3x4_t base, temp;
+	Matrix3x4_Copy(out, &base);
+	Matrix3x4_CreateTranslate(&temp, x, y, z);
+	Matrix3x4_Concat(out, &base, &temp);
+}
+
+// FIXME: optimize
+void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z)
+{
+	matrix3x4_t base, temp;
+	Matrix3x4_Copy(out, &base);
+	Matrix3x4_CreateRotate(&temp, angle, x, y, z);
+	Matrix3x4_Concat(out, &base, &temp);
+}
+
+// FIXME: optimize
+void Matrix3x4_ConcatScale (matrix3x4_t *out, float x)
+{
+	matrix3x4_t base, temp;
+	Matrix3x4_Copy(out, &base);
+	Matrix3x4_CreateScale(&temp, x);
+	Matrix3x4_Concat(out, &base, &temp);
+}
+
+// FIXME: optimize
+void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z)
+{
+	matrix3x4_t base, temp;
+	Matrix3x4_Copy(out, &base);
+	Matrix3x4_CreateScale3(&temp, x, y, z);
+	Matrix3x4_Concat(out, &base, &temp);
+}
+
diff --git a/matrix3x4.h b/matrix3x4.h
new file mode 100644
index 00000000..4c16302d
--- /dev/null
+++ b/matrix3x4.h
@@ -0,0 +1,35 @@
+
+#ifndef MATRIX3X4_H
+#define MATRIX3X4_H
+
+// to get the vec3_t type
+#include "mathlib.h"
+
+typedef struct matrix3x4_s
+{
+	float m[3][4];
+}
+matrix3x4_t;
+
+void Matrix3x4_Copy (matrix3x4_t *out, matrix3x4_t *in);
+void Matrix3x4_Concat (matrix3x4_t *out, const matrix3x4_t *in1, const matrix3x4_t *in2);
+void Matrix3x4_Transpose3x3 (matrix3x4_t *out, const matrix3x4_t *in1);
+
+void Matrix3x4_CreateIdentity (matrix3x4_t *out);
+void Matrix3x4_CreateTranslate (matrix3x4_t *out, float x, float y, float z);
+void Matrix3x4_CreateRotate (matrix3x4_t *out, float angle, float x, float y, float z);
+void Matrix3x4_CreateScale (matrix3x4_t *out, float x);
+void Matrix3x4_CreateScale3 (matrix3x4_t *out, float x, float y, float z);
+
+void Matrix3x4_ToVectors(const matrix3x4_t *in, vec3_t vx, vec3_t vy, vec3_t vz, vec3_t t);
+void Matrix3x4_FromVectors(matrix3x4_t *out, const vec3_t vx, const vec3_t vy, const vec3_t vz, const vec3_t t);
+
+void Matrix3x4_Transform (const matrix3x4_t *in, const vec3_t v, vec3_t out);
+void Matrix3x4_Untransform (const matrix3x4_t *in, const vec3_t v, vec3_t out);
+
+void Matrix3x4_ConcatTranslate (matrix3x4_t *out, float x, float y, float z);
+void Matrix3x4_ConcatRotate (matrix3x4_t *out, float angle, float x, float y, float z);
+void Matrix3x4_ConcatScale (matrix3x4_t *out, float x);
+void Matrix3x4_ConcatScale3 (matrix3x4_t *out, float x, float y, float z);
+
+#endif
diff --git a/matrix4x4.c b/matrix4x4.c
new file mode 100644
index 00000000..2fe644bf
--- /dev/null
+++ b/matrix4x4.c
@@ -0,0 +1,312 @@
+
+#include "matrix4x4.h"
+#include <math.h>
+
+void Matrix4x4_Copy (matrix4x4_t *out, matrix4x4_t *in)
+{
+	*out = *in;
+}
+
+void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2)
+{
+	out->m[0][0] = in1->m[0][0] * in2->m[0][0] + in1->m[0][1] * in2->m[1][0] + in1->m[0][2] * in2->m[2][0] + in1->m[0][3] * in2->m[3][0];
+	out->m[0][1] = in1->m[0][0] * in2->m[0][1] + in1->m[0][1] * in2->m[1][1] + in1->m[0][2] * in2->m[2][1] + in1->m[0][3] * in2->m[3][1];
+	out->m[0][2] = in1->m[0][0] * in2->m[0][2] + in1->m[0][1] * in2->m[1][2] + in1->m[0][2] * in2->m[2][2] + in1->m[0][3] * in2->m[3][2];
+	out->m[0][3] = in1->m[0][0] * in2->m[0][3] + in1->m[0][1] * in2->m[1][3] + in1->m[0][2] * in2->m[2][3] + in1->m[0][3] * in2->m[3][3];
+	out->m[1][0] = in1->m[1][0] * in2->m[0][0] + in1->m[1][1] * in2->m[1][0] + in1->m[1][2] * in2->m[2][0] + in1->m[1][3] * in2->m[3][0];
+	out->m[1][1] = in1->m[1][0] * in2->m[0][1] + in1->m[1][1] * in2->m[1][1] + in1->m[1][2] * in2->m[2][1] + in1->m[1][3] * in2->m[3][1];
+	out->m[1][2] = in1->m[1][0] * in2->m[0][2] + in1->m[1][1] * in2->m[1][2] + in1->m[1][2] * in2->m[2][2] + in1->m[1][3] * in2->m[3][2];
+	out->m[1][3] = in1->m[1][0] * in2->m[0][3] + in1->m[1][1] * in2->m[1][3] + in1->m[1][2] * in2->m[2][3] + in1->m[1][3] * in2->m[3][3];
+	out->m[2][0] = in1->m[2][0] * in2->m[0][0] + in1->m[2][1] * in2->m[1][0] + in1->m[2][2] * in2->m[2][0] + in1->m[2][3] * in2->m[3][0];
+	out->m[2][1] = in1->m[2][0] * in2->m[0][1] + in1->m[2][1] * in2->m[1][1] + in1->m[2][2] * in2->m[2][1] + in1->m[2][3] * in2->m[3][1];
+	out->m[2][2] = in1->m[2][0] * in2->m[0][2] + in1->m[2][1] * in2->m[1][2] + in1->m[2][2] * in2->m[2][2] + in1->m[2][3] * in2->m[3][2];
+	out->m[2][3] = in1->m[2][0] * in2->m[0][3] + in1->m[2][1] * in2->m[1][3] + in1->m[2][2] * in2->m[2][3] + in1->m[2][3] * in2->m[3][3];
+	out->m[3][0] = in1->m[3][0] * in2->m[0][0] + in1->m[3][1] * in2->m[1][0] + in1->m[3][2] * in2->m[2][0] + in1->m[3][3] * in2->m[3][0];
+	out->m[3][1] = in1->m[3][0] * in2->m[0][1] + in1->m[3][1] * in2->m[1][1] + in1->m[3][2] * in2->m[2][1] + in1->m[3][3] * in2->m[3][1];
+	out->m[3][2] = in1->m[3][0] * in2->m[0][2] + in1->m[3][1] * in2->m[1][2] + in1->m[3][2] * in2->m[2][2] + in1->m[3][3] * in2->m[3][2];
+	out->m[3][3] = in1->m[3][0] * in2->m[0][3] + in1->m[3][1] * in2->m[1][3] + in1->m[3][2] * in2->m[2][3] + in1->m[3][3] * in2->m[3][3];
+}
+
+void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1)
+{
+	out->m[0][0] = in1->m[0][0];
+	out->m[0][1] = in1->m[1][0];
+	out->m[0][2] = in1->m[2][0];
+	out->m[0][3] = in1->m[3][0];
+	out->m[1][0] = in1->m[0][1];
+	out->m[1][1] = in1->m[1][1];
+	out->m[1][2] = in1->m[2][1];
+	out->m[1][3] = in1->m[3][1];
+	out->m[2][0] = in1->m[0][2];
+	out->m[2][1] = in1->m[1][2];
+	out->m[2][2] = in1->m[2][2];
+	out->m[2][3] = in1->m[3][2];
+	out->m[3][0] = in1->m[0][3];
+	out->m[3][1] = in1->m[1][3];
+	out->m[3][2] = in1->m[2][3];
+	out->m[3][3] = in1->m[3][3];
+}
+
+void Matrix4x4_CreateIdentity (matrix4x4_t *out)
+{
+	out->m[0][0]=1.0f;
+	out->m[0][1]=0.0f;
+	out->m[0][2]=0.0f;
+	out->m[0][3]=0.0f;
+	out->m[1][0]=0.0f;
+	out->m[1][1]=1.0f;
+	out->m[1][2]=0.0f;
+	out->m[1][3]=0.0f;
+	out->m[2][0]=0.0f;
+	out->m[2][1]=0.0f;
+	out->m[2][2]=1.0f;
+	out->m[2][3]=0.0f;
+	out->m[3][0]=0.0f;
+	out->m[3][1]=0.0f;
+	out->m[3][2]=0.0f;
+	out->m[3][3]=1.0f;
+}
+
+void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z)
+{
+	out->m[0][0]=1.0f;
+	out->m[0][1]=0.0f;
+	out->m[0][2]=0.0f;
+	out->m[0][3]=x;
+	out->m[1][0]=0.0f;
+	out->m[1][1]=1.0f;
+	out->m[1][2]=0.0f;
+	out->m[1][3]=y;
+	out->m[2][0]=0.0f;
+	out->m[2][1]=0.0f;
+	out->m[2][2]=1.0f;
+	out->m[2][3]=z;
+	out->m[3][0]=0.0f;
+	out->m[3][1]=0.0f;
+	out->m[3][2]=0.0f;
+	out->m[3][3]=1.0f;
+}
+
+void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z)
+{
+	float len, c, s;
+
+	len = x*x+y*y+z*z;
+	if (len != 0.0f)
+		len = 1.0f / sqrt(len);
+	x *= len;
+	y *= len;
+	z *= len;
+
+	c = cos(angle);
+	s = sin(angle);
+
+	out->m[0][0]=x * x + c * (1 - x * x);
+	out->m[0][1]=x * y * (1 - c) + z * s;
+	out->m[0][2]=z * x * (1 - c) - y * s;
+	out->m[0][3]=0.0f;
+	out->m[1][0]=x * y * (1 - c) - z * s;
+	out->m[1][1]=y * y + c * (1 - y * y);
+	out->m[1][2]=y * z * (1 - c) + x * s;
+	out->m[1][3]=0.0f;
+	out->m[2][0]=z * x * (1 - c) + y * s;
+	out->m[2][1]=y * z * (1 - c) - x * s;
+	out->m[2][2]=z * z + c * (1 - z * z);
+	out->m[2][3]=0.0f;
+	out->m[3][0]=0.0f;
+	out->m[3][1]=0.0f;
+	out->m[3][2]=0.0f;
+	out->m[3][3]=1.0f;
+}
+
+void Matrix4x4_CreateScale (matrix4x4_t *out, float x)
+{
+	out->m[0][0]=x;
+	out->m[0][1]=0.0f;
+	out->m[0][2]=0.0f;
+	out->m[0][3]=0.0f;
+	out->m[1][0]=0.0f;
+	out->m[1][1]=x;
+	out->m[1][2]=0.0f;
+	out->m[1][3]=0.0f;
+	out->m[2][0]=0.0f;
+	out->m[2][1]=0.0f;
+	out->m[2][2]=x;
+	out->m[2][3]=0.0f;
+	out->m[3][0]=0.0f;
+	out->m[3][1]=0.0f;
+	out->m[3][2]=0.0f;
+	out->m[3][3]=1.0f;
+}
+
+void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z)
+{
+	out->m[0][0]=x;
+	out->m[0][1]=0.0f;
+	out->m[0][2]=0.0f;
+	out->m[0][3]=0.0f;
+	out->m[1][0]=0.0f;
+	out->m[1][1]=y;
+	out->m[1][2]=0.0f;
+	out->m[1][3]=0.0f;
+	out->m[2][0]=0.0f;
+	out->m[2][1]=0.0f;
+	out->m[2][2]=z;
+	out->m[2][3]=0.0f;
+	out->m[3][0]=0.0f;
+	out->m[3][1]=0.0f;
+	out->m[3][2]=0.0f;
+	out->m[3][3]=1.0f;
+}
+
+void Matrix4x4_ToVectors(const matrix4x4_t *in, vec3_t vx, vec3_t vy, vec3_t vz, vec3_t t)
+{
+	vx[0] = in->m[0][0];
+	vx[1] = in->m[1][0];
+	vx[2] = in->m[2][0];
+	vy[0] = in->m[0][1];
+	vy[1] = in->m[1][1];
+	vy[2] = in->m[2][1];
+	vz[0] = in->m[0][2];
+	vz[1] = in->m[1][2];
+	vz[2] = in->m[2][2];
+	t[0] = in->m[0][3];
+	t[1] = in->m[1][3];
+	t[2] = in->m[2][3];
+}
+
+void Matrix4x4_ToVectors4(const matrix4x4_t *in, vec4_t vx, vec4_t vy, vec4_t vz, vec4_t t)
+{
+	vx[0] = in->m[0][0];
+	vx[1] = in->m[1][0];
+	vx[2] = in->m[2][0];
+	vx[3] = in->m[3][0];
+	vy[0] = in->m[0][1];
+	vy[1] = in->m[1][1];
+	vy[2] = in->m[2][1];
+	vy[3] = in->m[3][1];
+	vz[0] = in->m[0][2];
+	vz[1] = in->m[1][2];
+	vz[2] = in->m[2][2];
+	vz[3] = in->m[3][2];
+	t[0] = in->m[0][3];
+	t[1] = in->m[1][3];
+	t[2] = in->m[2][3];
+	t[3] = in->m[3][3];
+}
+
+void Matrix4x4_FromVectors(matrix4x4_t *out, const vec3_t vx, const vec3_t vy, const vec3_t vz, const vec3_t t)
+{
+	out->m[0][0] = vx[0];
+	out->m[0][1] = vy[0];
+	out->m[0][2] = vz[0];
+	out->m[0][3] = t[0];
+	out->m[1][0] = vx[1];
+	out->m[1][1] = vy[1];
+	out->m[1][2] = vz[1];
+	out->m[1][3] = t[1];
+	out->m[2][0] = vx[2];
+	out->m[2][1] = vy[2];
+	out->m[2][2] = vz[2];
+	out->m[2][3] = t[2];
+	out->m[3][0] = 0.0f;
+	out->m[3][1] = 0.0f;
+	out->m[3][2] = 0.0f;
+	out->m[3][3] = 1.0f;
+}
+
+void Matrix4x4_FromVectors4(matrix4x4_t *out, const vec4_t vx, const vec4_t vy, const vec4_t vz, const vec4_t t)
+{
+	out->m[0][0] = vx[0];
+	out->m[0][1] = vy[0];
+	out->m[0][2] = vz[0];
+	out->m[0][3] = t[0];
+	out->m[1][0] = vx[1];
+	out->m[1][1] = vy[1];
+	out->m[1][2] = vz[1];
+	out->m[1][3] = t[1];
+	out->m[2][0] = vx[2];
+	out->m[2][1] = vy[2];
+	out->m[2][2] = vz[2];
+	out->m[2][3] = t[2];
+	out->m[3][0] = vx[3];
+	out->m[3][1] = vy[3];
+	out->m[3][2] = vz[3];
+	out->m[3][3] = t[3];
+}
+
+void Matrix4x4_Transform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
+{
+	out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + in->m[0][3];
+	out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + in->m[1][3];
+	out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + in->m[2][3];
+}
+
+void Matrix4x4_Transform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
+{
+	out[0] = v[0] * in->m[0][0] + v[1] * in->m[0][1] + v[2] * in->m[0][2] + v[3] * in->m[0][3];
+	out[1] = v[0] * in->m[1][0] + v[1] * in->m[1][1] + v[2] * in->m[1][2] + v[3] * in->m[1][3];
+	out[2] = v[0] * in->m[2][0] + v[1] * in->m[2][1] + v[2] * in->m[2][2] + v[3] * in->m[2][3];
+	out[3] = v[0] * in->m[3][0] + v[1] * in->m[3][1] + v[2] * in->m[3][2] + v[3] * in->m[3][3];
+}
+
+void Matrix4x4_Untransform (const matrix4x4_t *in, const vec3_t v, vec3_t out)
+{
+	vec3_t t;
+	t[0] = v[0] - in->m[0][3];
+	t[1] = v[1] - in->m[1][3];
+	t[2] = v[2] - in->m[2][3];
+	out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0];
+	out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1];
+	out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2];
+}
+
+void Matrix4x4_Untransform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out)
+{
+	vec3_t t;
+	t[0] = v[0] - in->m[0][3];
+	t[1] = v[1] - in->m[1][3];
+	t[2] = v[2] - in->m[2][3];
+	t[3] = v[3] - in->m[3][3];
+	out[0] = t[0] * in->m[0][0] + t[1] * in->m[1][0] + t[2] * in->m[2][0] + t[3] * in->m[3][0];
+	out[1] = t[0] * in->m[0][1] + t[1] * in->m[1][1] + t[2] * in->m[2][1] + t[3] * in->m[3][1];
+	out[2] = t[0] * in->m[0][2] + t[1] * in->m[1][2] + t[2] * in->m[2][2] + t[3] * in->m[3][2];
+	out[3] = t[0] * in->m[0][3] + t[1] * in->m[1][3] + t[2] * in->m[2][3] + t[3] * in->m[3][3];
+}
+
+// FIXME: optimize
+void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z)
+{
+	matrix4x4_t base, temp;
+	Matrix4x4_Copy(out, &base);
+	Matrix4x4_CreateTranslate(&temp, x, y, z);
+	Matrix4x4_Concat(out, &base, &temp);
+}
+
+// FIXME: optimize
+void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z)
+{
+	matrix4x4_t base, temp;
+	Matrix4x4_Copy(out, &base);
+	Matrix4x4_CreateRotate(&temp, angle, x, y, z);
+	Matrix4x4_Concat(out, &base, &temp);
+}
+
+// FIXME: optimize
+void Matrix4x4_ConcatScale (matrix4x4_t *out, float x)
+{
+	matrix4x4_t base, temp;
+	Matrix4x4_Copy(out, &base);
+	Matrix4x4_CreateScale(&temp, x);
+	Matrix4x4_Concat(out, &base, &temp);
+}
+
+// FIXME: optimize
+void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z)
+{
+	matrix4x4_t base, temp;
+	Matrix4x4_Copy(out, &base);
+	Matrix4x4_CreateScale3(&temp, x, y, z);
+	Matrix4x4_Concat(out, &base, &temp);
+}
+
diff --git a/matrix4x4.h b/matrix4x4.h
new file mode 100644
index 00000000..332ee254
--- /dev/null
+++ b/matrix4x4.h
@@ -0,0 +1,39 @@
+
+#ifndef MATRIX4X4_H
+#define MATRIX4X4_H
+
+// to get the vec3_t and vec4_t types
+#include "mathlib.h"
+
+typedef struct matrix4x4_s
+{
+	float m[4][4];
+}
+matrix4x4_t;
+
+void Matrix4x4_Copy (matrix4x4_t *out, matrix4x4_t *in);
+void Matrix4x4_Concat (matrix4x4_t *out, const matrix4x4_t *in1, const matrix4x4_t *in2);
+void Matrix4x4_Transpose (matrix4x4_t *out, const matrix4x4_t *in1);
+
+void Matrix4x4_CreateIdentity (matrix4x4_t *out);
+void Matrix4x4_CreateTranslate (matrix4x4_t *out, float x, float y, float z);
+void Matrix4x4_CreateRotate (matrix4x4_t *out, float angle, float x, float y, float z);
+void Matrix4x4_CreateScale (matrix4x4_t *out, float x);
+void Matrix4x4_CreateScale3 (matrix4x4_t *out, float x, float y, float z);
+
+void Matrix4x4_ToVectors(const matrix4x4_t *in, vec3_t vx, vec3_t vy, vec3_t vz, vec3_t t);
+void Matrix4x4_ToVectors4(const matrix4x4_t *in, vec4_t vx, vec4_t vy, vec4_t vz, vec4_t t);
+void Matrix4x4_FromVectors(matrix4x4_t *out, const vec3_t vx, const vec3_t vy, const vec3_t vz, const vec3_t t);
+void Matrix4x4_FromVectors4(matrix4x4_t *out, const vec4_t vx, const vec4_t vy, const vec4_t vz, const vec4_t t);
+
+void Matrix4x4_Transform (const matrix4x4_t *in, const vec3_t v, vec3_t out);
+void Matrix4x4_Transform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out);
+void Matrix4x4_Untransform (const matrix4x4_t *in, const vec3_t v, vec3_t out);
+void Matrix4x4_Untransform4 (const matrix4x4_t *in, const vec4_t v, vec4_t out);
+
+void Matrix4x4_ConcatTranslate (matrix4x4_t *out, float x, float y, float z);
+void Matrix4x4_ConcatRotate (matrix4x4_t *out, float angle, float x, float y, float z);
+void Matrix4x4_ConcatScale (matrix4x4_t *out, float x);
+void Matrix4x4_ConcatScale3 (matrix4x4_t *out, float x, float y, float z);
+
+#endif
-- 
2.39.2