1 files changed, 83 insertions, 0 deletions
diff --git a/src/shared/matrix.c b/src/shared/matrix.c
new file mode 100644
index 0000000..86dffbb
--- /dev/null
+++ b/src/shared/matrix.c
@@ -0,0 +1,83 @@
+
+#include <math.h>
+
+#include "shared/shared.h"
+
+void AngleVectors(vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
+{
+    float        angle;
+    float        sr, sp, sy, cr, cp, cy;
+
+    angle = DEG2RAD(angles[YAW]);
+    sy = sin(angle);
+    cy = cos(angle);
+    angle = DEG2RAD(angles[PITCH]);
+    sp = sin(angle);
+    cp = cos(angle);
+    angle = DEG2RAD(angles[ROLL]);
+    sr = sin(angle);
+    cr = cos(angle);
+
+    if (forward) {
+        forward[0] = cp * cy;
+        forward[1] = cp * sy;
+        forward[2] = -sp;
+    }
+    if (right) {
+        right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
+        right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
+        right[2] = -1 * sr * cp;
+    }
+    if (up) {
+        up[0] = (cr * sp * cy + -sr * -sy);
+        up[1] = (cr * sp * sy + -sr * cy);
+        up[2] = cr * cp;
+    }
+}
+
+vec_t VectorNormalize(vec3_t v)
+{
+    float    length, ilength;
+
+    length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
+    length = sqrt(length);         // FIXME
+
+    if (length) {
+        ilength = 1 / length;
+        v[0] *= ilength;
+        v[1] *= ilength;
+        v[2] *= ilength;
+    }
+
+    return length;
+
+}
+
+vec_t VectorNormalize2(vec3_t v, vec3_t out)
+{
+    float    length, ilength;
+
+    length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
+    length = sqrt(length);         // FIXME
+
+    if (length) {
+        ilength = 1 / length;
+        out[0] = v[0] * ilength;
+        out[1] = v[1] * ilength;
+        out[2] = v[2] * ilength;
+    }
+
+    return length;
+}
+
+mat4_t mul_mat4(const mat4_t *restrict a, const mat4_t *restrict b)
+{
+    mat4_t p = { 0 };
+
+    for (int i = 0; i < 4; i++)
+        for (int j = 0; j < 4; j++)
+	    for (int k = 0; k < 4; k++)
+		p.i[i][j] += a->i[k][j] * b->i[i][k];
+
+    return p;
+}