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.
147 lines
2.8 KiB
147 lines
2.8 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]; |
|
} |
|
} |
|
} |
|
} |
|
|
|
/* 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 */ |
|
}
|
|
|