Linear algebra improvements

1 files changed, 187 insertions, 0 deletions

diff --git a/inc/shared/matrix.h b/inc/shared/matrix.h
new file mode 100644
index 0000000..b106d8c
--- /dev/null
+++ b/inc/shared/matrix.h
@@ -0,0 +1,187 @@
+#ifndef _Q_MATRIX_H
+#define _Q_MATRIX_H
+
+/* Linear algebra - vectors and matrices: */
+
+typedef float vec_t;
+typedef vec_t vec2_t[2];
+typedef vec_t vec3_t[3];
+typedef vec_t vec4_t[4];
+typedef vec_t vec5_t[5];
+
+typedef struct {
+    union {
+	vec_t   i[3][3];
+	vec_t	array[9];
+    };
+} mat3_t;
+
+typedef struct {
+    union {
+	vec_t   i[4][4];
+	vec_t	flat[16];
+    };
+} mat4_t;
+
+#define DotProduct(x,y)         ((x)[0]*(y)[0]+(x)[1]*(y)[1]+(x)[2]*(y)[2])
+
+#define CrossProduct(v1,v2,cross) \
+        ((cross)[0]=(v1)[1]*(v2)[2]-(v1)[2]*(v2)[1], \
+         (cross)[1]=(v1)[2]*(v2)[0]-(v1)[0]*(v2)[2], \
+         (cross)[2]=(v1)[0]*(v2)[1]-(v1)[1]*(v2)[0])
+
+#define VectorSubtract(a,b,c) \
+        ((c)[0]=(a)[0]-(b)[0], \
+         (c)[1]=(a)[1]-(b)[1], \
+         (c)[2]=(a)[2]-(b)[2])
+
+#define VectorAdd(a,b,c) \
+        ((c)[0]=(a)[0]+(b)[0], \
+         (c)[1]=(a)[1]+(b)[1], \
+         (c)[2]=(a)[2]+(b)[2])
+
+#define VectorAdd3(a,b,c,d) \
+        ((d)[0]=(a)[0]+(b)[0]+(c)[0], \
+         (d)[1]=(a)[1]+(b)[1]+(c)[1], \
+         (d)[2]=(a)[2]+(b)[2]+(c)[2])
+
+#define VectorCopy(a,b)     ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2])
+#define VectorClear(a)      ((a)[0]=(a)[1]=(a)[2]=0)
+#define VectorNegate(a,b)   ((b)[0]=-(a)[0],(b)[1]=-(a)[1],(b)[2]=-(a)[2])
+#define VectorInverse(a)    ((a)[0]=-(a)[0],(a)[1]=-(a)[1],(a)[2]=-(a)[2])
+#define VectorSet(v, x, y, z)   ((v)[0]=(x),(v)[1]=(y),(v)[2]=(z))
+#define VectorAvg(a,b,c) \
+        ((c)[0]=((a)[0]+(b)[0])*0.5f, \
+         (c)[1]=((a)[1]+(b)[1])*0.5f, \
+         (c)[2]=((a)[2]+(b)[2])*0.5f)
+#define VectorMA(a,b,c,d) \
+        ((d)[0]=(a)[0]+(b)*(c)[0], \
+         (d)[1]=(a)[1]+(b)*(c)[1], \
+         (d)[2]=(a)[2]+(b)*(c)[2])
+#define VectorVectorMA(a,b,c,d) \
+        ((d)[0]=(a)[0]+(b)[0]*(c)[0], \
+         (d)[1]=(a)[1]+(b)[1]*(c)[1], \
+         (d)[2]=(a)[2]+(b)[2]*(c)[2])
+#define VectorEmpty(v) ((v)[0]==0&&(v)[1]==0&&(v)[2]==0)
+#define VectorCompare(v1,v2)    ((v1)[0]==(v2)[0]&&(v1)[1]==(v2)[1]&&(v1)[2]==(v2)[2])
+#define VectorLength(v)     (sqrt(DotProduct((v),(v))))
+#define VectorLengthSquared(v)      (DotProduct((v),(v)))
+#define VectorScale(in,scale,out) \
+        ((out)[0]=(in)[0]*(scale), \
+         (out)[1]=(in)[1]*(scale), \
+         (out)[2]=(in)[2]*(scale))
+#define VectorVectorScale(in,scale,out) \
+        ((out)[0]=(in)[0]*(scale)[0], \
+         (out)[1]=(in)[1]*(scale)[1], \
+         (out)[2]=(in)[2]*(scale)[2])
+#define DistanceSquared(v1,v2) \
+        (((v1)[0]-(v2)[0])*((v1)[0]-(v2)[0])+ \
+        ((v1)[1]-(v2)[1])*((v1)[1]-(v2)[1])+ \
+        ((v1)[2]-(v2)[2])*((v1)[2]-(v2)[2]))
+#define Distance(v1,v2) (sqrt(DistanceSquared(v1,v2)))
+#define LerpAngles(a,b,c,d) \
+        ((d)[0]=LerpAngle((a)[0],(b)[0],c), \
+         (d)[1]=LerpAngle((a)[1],(b)[1],c), \
+         (d)[2]=LerpAngle((a)[2],(b)[2],c))
+#define LerpVector(a,b,c,d) \
+    ((d)[0]=(a)[0]+(c)*((b)[0]-(a)[0]), \
+     (d)[1]=(a)[1]+(c)*((b)[1]-(a)[1]), \
+     (d)[2]=(a)[2]+(c)*((b)[2]-(a)[2]))
+#define LerpVector2(a,b,c,d,e) \
+    ((e)[0]=(a)[0]*(c)+(b)[0]*(d), \
+     (e)[1]=(a)[1]*(c)+(b)[1]*(d), \
+     (e)[2]=(a)[2]*(c)+(b)[2]*(d))
+#define PlaneDiff(v,p)   (DotProduct(v,(p)->normal)-(p)->dist)
+
+#define Vector4Subtract(a,b,c)  ((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2],(c)[3]=(a)[3]-(b)[3])
+#define Vector4Add(a,b,c)       ((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2],(c)[3]=(a)[3]+(b)[3])
+#define Vector4Copy(a,b)        ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
+#define Vector4Clear(a)         ((a)[0]=(a)[1]=(a)[2]=(a)[3]=0)
+#define Vector4Negate(a,b)      ((b)[0]=-(a)[0],(b)[1]=-(a)[1],(b)[2]=-(a)[2],(b)[3]=-(a)[3])
+#define Vector4Set(v, a, b, c, d)   ((v)[0]=(a),(v)[1]=(b),(v)[2]=(c),(v)[3]=(d))
+
+void AngleVectors(vec3_t angles, vec3_t forward, vec3_t right, vec3_t up);
+vec_t VectorNormalize(vec3_t v);        // returns vector length
+vec_t VectorNormalize2(vec3_t v, vec3_t out);
+mat4_t mul_mat4(const mat4_t *restrict a, const mat4_t *restrict b);
+
+static inline void AnglesToAxis(vec3_t angles, vec3_t axis[3])
+{
+    AngleVectors(angles, axis[0], axis[1], axis[2]);
+    VectorInverse(axis[1]);
+}
+
+static inline void TransposeAxis(vec3_t axis[3])
+{
+    vec_t temp;
+
+    temp = axis[0][1];
+    axis[0][1] = axis[1][0];
+    axis[1][0] = temp;
+
+    temp = axis[0][2];
+    axis[0][2] = axis[2][0];
+    axis[2][0] = temp;
+
+    temp = axis[1][2];
+    axis[1][2] = axis[2][1];
+    axis[2][1] = temp;
+}
+
+static inline void RotatePoint(vec3_t point, vec3_t axis[3])
+{
+    vec3_t temp;
+
+    VectorCopy(point, temp);
+    point[0] = DotProduct(temp, axis[0]);
+    point[1] = DotProduct(temp, axis[1]);
+    point[2] = DotProduct(temp, axis[2]);
+}
+
+static inline mat3_t vec3_to_mat3(vec3_t v0, vec3_t v1, vec3_t v2)
+{
+    return (mat3_t) {
+	.i = {
+	    [0][0] = v0[0],
+	    [0][1] = v0[1],
+	    [0][2] = v0[2],
+
+	    [1][0] = v1[0],
+	    [1][1] = v1[1],
+	    [1][2] = v1[2],
+
+	    [2][0] = v2[0],
+	    [2][1] = v2[1],
+	    [2][2] = v2[2],
+	},
+    };
+}
+
+static inline mat4_t AffineMatrix(mat3_t m, vec3_t v)
+{
+    mat4_t a;
+
+    for (unsigned i = 0; i < 3; i++) {
+	for (unsigned j = 0; j < 3; j++)
+	    a.i[i][j] = m.i[i][j];
+
+	a.i[3][i] = v[i];
+    }
+
+    a.i[0][3] = 0;
+    a.i[1][3] = 0;
+    a.i[2][3] = 0;
+    a.i[3][3] = 1;
+
+    return a;
+}
+
+static const mat3_t mat3_identity = {
+    .i = {
+	[0][0] = 1,
+	[1][1] = 1,
+	[2][2] = 1,
+    }
+};
+
+#endif /* _Q_MATRIX_H */