quadrotor/matrix.c

/* 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];
			}
		}
	}
}

#if 0
/* 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];
			}
		}
	}
}
#endif

/* 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

#if 0
/* 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 */
}
#endif