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

diff --git a/makefile b/makefile
index 55b1a77f250f36e3b610f42480ed59e641c36953..b93a0f478eb67a59f1115ae9128fe1060aa203db 100644 (file)
--- 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 (file)
index 0000000..a716b12
--- /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 (file)
index 0000000..4c16302
--- /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 (file)
index 0000000..2fe644b
--- /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 (file)
index 0000000..332ee25
--- /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