Gavan Fantom
62732758a8
This brings in an implementation of some general matrix manipulation routines, not all of which are used at teh moment. Output the DCM on the UART at 50Hz for the host to display it usefully.
148 lines
2.8 KiB
C
148 lines
2.8 KiB
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];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/* 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 */
|
|
}
|