Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

152 lines
2.9 KiB

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