The unified diff between revisions [65df00aa..] and [d0420ebd..] is displayed below. It can also be downloaded as a raw diff.

This diff has been restricted to the following files: 'matrix.c'

#
# old_revision [65df00aa2705ce33fd74f4dd706d2879fe99b2b0]
# new_revision [d0420ebd87c820e33a32b29727989516e15980a8]
#
# add_file "matrix.c"
#  content [7aac09561911cbe10901d55f177c6c7e5177729c]
#
============================================================
--- /dev/null	
+++ matrix.c	7aac09561911cbe10901d55f177c6c7e5177729c
@@ -0,0 +1,147 @@
+/* matrix.c */
+
+#ifdef MATRIX_DEBUG
+#include <stdio.h>
+#endif
+
+/* dest[r][c] = m1[r][n] * m2[n][c] */
+void matrix_multiply(float *dest, float *m1, float *m2, int r, int c, int n)
+{
+	int i, j, k;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = 0;
+			for (k = 0; k < n; k++) {
+				dest[i*c+j] += m1[i*n+k] * m2[k*c+j];
+			}
+		}
+	}
+}
+
+/* dest[r][c] = m1[r][n] * m2[c][n] */
+void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n)
+{
+	int i, j, k;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = 0;
+			for (k = 0; k < n; k++) {
+				dest[i*c+j] += m1[i*n+k] * m2[j*n+k];
+			}
+		}
+	}
+}
+
+/* dest[r][c] = m1[r][c] + m2[r][c] */
+void matrix_add(float *dest, float *m1, float *m2, int r, int c)
+{
+	int i, j;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = m1[i*c+j] + m2[i*c+j];
+		}
+	}
+}
+
+#ifdef MATRIX_DEBUG
+void dump_matrix(float *m, int r, int c)
+{
+	int i, j;
+
+	for (i = 0; i < r; i++) {
+		printf("(");
+		for (j = 0; j < c; j++) {
+			printf("%f", *(m++));
+			if (j < c-1)
+				printf(" ");
+		}
+		printf(")\n");
+	}
+	printf("\n");
+}
+#endif
+
+/* Invert src into dst.
+ * NOTE: destroys the src matrix
+ */
+void matrix_invert(float *dst, float *src, int size)
+{
+	int i, j, k;
+
+	/* Initialise answer with identity matrix */
+	for (i = 0; i < size*size; i++)
+		dst[i] = 0.0;
+	for (i = 0; i < size; i++)
+		dst[i*(size+1)] = 1.0;
+
+	/* Manipulate the matrix into row-echelon form */
+	for (i = 0; i < size; i++) {
+		float max;
+		int maxi;
+
+		/* find a pivot */
+		max = src[i*size+i];
+		maxi = i;
+		for (j = i+1; j < size; j++) {
+			if (src[j*size+i] > max) {
+				max = src[j*size+i];
+				maxi = j;
+			}
+		}
+		if (maxi != i) {
+			/* swap rows */
+			for (j = 0; j < size; j++) {
+				float tmp;
+
+				tmp = src[i*size+j];
+				src[i*size+j] = src[maxi*size+j];
+				src[maxi*size+j] = tmp;
+				tmp = dst[i*size+j];
+				dst[i*size+j] = dst[maxi*size+j];
+				dst[maxi*size+j] = tmp;
+			}
+		}
+		for (j = size-1; j > i; j--) {
+			float factor;
+
+			factor = src[j*size+i] / max;
+			for (k = 0; k < size; k++) {
+				src[j*size+k] = src[j*size+k] -
+						    src[i*size+k] * factor;
+				dst[j*size+k] = dst[j*size+k] -
+						    dst[i*size+k] * factor;
+			}
+		}
+	}
+
+	/* Back-substitute to get src into diagonal form */
+	for (i = size-1; i > 0; i--) {
+		for (j = 0; j < i; j++) {
+			float factor;
+
+			factor = src[j*size+i] / src[i*size+i];
+			for (k = 0; k < size; k++) {
+				src[j*size+k] = src[j*size+k] -
+						    src[i*size+k] * factor;
+				dst[j*size+k] = dst[j*size+k] -
+						    dst[i*size+k] * factor;
+			}
+		}
+	}
+
+	/* Divide each row by its diagonal element */
+	for (i = 0; i < size; i++) {
+		float factor;
+
+		factor = src[i*size+i];
+		for (j = 0; j < size; j++) {
+			src[i*size+j] = src[i*size+j] / factor;
+			dst[i*size+j] = dst[i*size+j] / factor;
+		}
+	}
+
+	/* src should now be the identiy matrix while dst holds the answer */
+}