Add a 4x4 matrix type and some functions for it.

include/QF/mathlib.h

@@ -278,6 +278,48 @@ extern const vec_t *const quat_origin;
 //For printf etc
 #define QuatExpand(q) (q)[0], (q)[1], (q)[2], (q)[3]
 
+#define Mat4Copy(a, b) \
+	do { \
+		QuatCopy ((a) + 0, (b) + 0); \
+		QuatCopy ((a) + 4, (b) + 4); \
+		QuatCopy ((a) + 8, (b) + 8); \
+		QuatCopy ((a) + 12, (b) + 12); \
+	} while (0)
+#define Mat4Add(a, b, c) \
+	do { \
+		QuatAdd ((a) + 0, (b) + 0, (c) + 0); \
+		QuatAdd ((a) + 4, (b) + 4, (c) + 4); \
+		QuatAdd ((a) + 8, (b) + 8, (c) + 8); \
+		QuatAdd ((a) + 12, (b) + 12, (c) + 12); \
+	} while (0)
+#define Mat4Subtract(a, b, c) \
+	do { \
+		QuatSubtract ((a) + 0, (b) + 0, (c) + 0); \
+		QuatSubtract ((a) + 4, (b) + 4, (c) + 4); \
+		QuatSubtract ((a) + 8, (b) + 8, (c) + 8); \
+		QuatSubtract ((a) + 12, (b) + 12, (c) + 12); \
+	} while (0)
+#define Mat4Scale(a, b, c) \
+	do { \
+		QuatScale ((a) + 0, (b), (c) + 0); \
+		QuatScale ((a) + 4, (b), (c) + 4); \
+		QuatScale ((a) + 8, (b), (c) + 8); \
+		QuatScale ((a) + 12, (b), (c) + 12); \
+	} while (0)
+#define Mat4CompMult(a, b, c) \
+	do { \
+		QuatCompMult ((a) + 0, (b) + 0, (c) + 0); \
+		QuatCompMult ((a) + 4, (b) + 4, (c) + 4); \
+		QuatCompMult ((a) + 8, (b) + 8, (c) + 8); \
+		QuatCompMult ((a) + 12, (b) + 12, (c) + 12); \
+	} while (0)
+#define Mat4Zero(a) \
+	memset (a, 0, sizeof (16 * sizeof a[0]))
+#define Mat4Identity(a) \
+	do { \
+		Mat4Zero (a); \
+		a[15] = a[10] = a[5] = a[0] = 1; \
+	} while (0)
 
 #define qfrandom(MAX) ((float) MAX * (rand() * (1.0 / (RAND_MAX + 1.0))))
 
@@ -376,6 +418,9 @@ void QuatInverse (const quat_t in, quat_t out);
 
 void QuatToMatrix (const quat_t q, vec_t *m, int homogenous, int vertical);
 
+void Mat4Transpose (const mat4_t a, mat4_t b);
+void Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c);
+
 #define BOX_ON_PLANE_SIDE(emins, emaxs, p)				\
 	(((p)->type < 3)?									\
 	(													\

include/QF/qtypes.h

@@ -86,6 +86,8 @@ typedef	int		fixed4_t;
 typedef int		fixed8_t;
 typedef	int		fixed16_t;
 
+typedef float   mat4_t[4 * 4];
+
 #define SIDE_FRONT  0
 #define SIDE_BACK   1
 #define SIDE_ON     2

libs/util/mathlib.c

@@ -695,3 +695,35 @@ Invert24To16 (fixed16_t val)
 		(((double) 0x10000 * (double) 0x1000000 / (double) val) + 0.5);
 }
 #endif
+
+void
+Mat4Transpose (const mat4_t a, mat4_t b)
+{
+	vec_t       t;
+	int         i, j;
+
+	for (i = 0; i < 3; i++) {
+		for (j = i + 1; j < 4; j++) {
+			t = a[i * 4 + j];				// in case b == a
+			b[i * 4 + j] = a[j * 4 + i];
+			b[j * 4 + i] = t;
+		}
+	}
+}
+
+void
+Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c)
+{
+	mat4_t      ta, tb;					// in case c == b or c == a
+	int         i, j, k;
+
+	Mat4Transpose (a, ta);				// transpose so we can use dot
+	Mat4Copy (b, tb);
+
+	k = 0;
+	for (i = 0; i < 4; i++) {
+		for (j = 0; j < 4; j++) {
+			c[k++] = QDotProduct (ta + 4 * j, tb + 4 * i);
+		}
+	}
+}