The unified diff between revisions [65df00aa..] and [9f05a1eb..] 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 [9f05a1eb606ea1c0421aa4a0b25b83b4fe4a20c8] # # 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 */ +}