diff options
author | Eric S. Raymond <esr@thyrsus.com> | 2014-09-20 07:43:35 -0400 |
---|---|---|
committer | Eric S. Raymond <esr@thyrsus.com> | 2014-09-20 07:43:35 -0400 |
commit | 103109b3697bbea9fddfa3f0e221f3f583ad2f8a (patch) | |
tree | 4ac5e270c84328343084b439e8cd16c229eb64fd /matrix.c | |
parent | 2b0a007ab49e5e3a9f535bc5a591a73ec16959d2 (diff) | |
download | gpsd-103109b3697bbea9fddfa3f0e221f3f583ad2f8a.tar.gz |
Begin factoring out matrix algebra so it can be unit-tested.
For some odd reason trying to faxtor out the symmetrize operation
induces a core dump. To be investigated.
Diffstat (limited to 'matrix.c')
-rw-r--r-- | matrix.c | 120 |
1 files changed, 120 insertions, 0 deletions
diff --git a/matrix.c b/matrix.c new file mode 100644 index 00000000..53fee223 --- /dev/null +++ b/matrix.c @@ -0,0 +1,120 @@ +/* matrix.c - matrix-algebra code + * + * This file is Copyright (c)2014 by the GPSD project + * BSD terms apply: see the file COPYING in the distribution root for details. + */ +#include <math.h> +#include <stdbool.h> + +#include "matrix.h" + +/*@ -fixedformalarray -mustdefine @*/ +bool matrix_invert(double mat[4][4], /*@out@*/ double inverse[4][4]) +/* selected elements from 4x4 matrox inversion */ +{ + // Find all NECESSARY 2x2 subdeterminants + double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; + double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]; + //double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0]; + double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; + //double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1]; + //double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2]; + double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; + //double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0]; + double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; + //double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1]; + double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; + //double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2]; + double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; + double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; + double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; + double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; + double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; + double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; + + // Find all NECESSARY 3x3 subdeterminants + double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 + + mat[0][2] * Det2_12_01; + //double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03 + // + mat[0][3]*Det2_12_01; + //double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03 + // + mat[0][3]*Det2_12_02; + //double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13 + // + mat[0][3]*Det2_12_12; + //double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02 + // + mat[0][2]*Det2_13_01; + double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 + + mat[0][3] * Det2_13_01; + //double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03 + // + mat[0][3]*Det2_13_02; + //double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13 + // + mat[0][3]*Det2_13_12; + //double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02 + // + mat[0][2]*Det2_23_01; + //double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03 + // + mat[0][3]*Det2_23_01; + double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 + + mat[0][3] * Det2_23_02; + //double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13 + // + mat[0][3]*Det2_23_12; + double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 + + mat[1][2] * Det2_23_01; + double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 + + mat[1][3] * Det2_23_01; + double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 + + mat[1][3] * Det2_23_02; + double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 + + mat[1][3] * Det2_23_12; + + // Find the 4x4 determinant + static double det; + det = mat[0][0] * Det3_123_123 + - mat[0][1] * Det3_123_023 + + mat[0][2] * Det3_123_013 - mat[0][3] * Det3_123_012; + + // Very small determinants probably reflect floating-point fuzz near zero + if (fabs(det) < 0.0001) + return false; + + inverse[0][0] = Det3_123_123 / det; + //inverse[0][1] = -Det3_023_123 / det; + //inverse[0][2] = Det3_013_123 / det; + //inverse[0][3] = -Det3_012_123 / det; + + //inverse[1][0] = -Det3_123_023 / det; + inverse[1][1] = Det3_023_023 / det; + //inverse[1][2] = -Det3_013_023 / det; + //inverse[1][3] = Det3_012_023 / det; + + //inverse[2][0] = Det3_123_013 / det; + //inverse[2][1] = -Det3_023_013 / det; + inverse[2][2] = Det3_013_013 / det; + //inverse[2][3] = -Det3_012_013 / det; + + //inverse[3][0] = -Det3_123_012 / det; + //inverse[3][1] = Det3_023_012 / det; + //inverse[3][2] = -Det3_013_012 / det; + inverse[3][3] = Det3_012_012 / det; + + return true; +} +/*@ +fixedformalarray +mustdefine @*/ + +#ifdef __UNUSED_ +void matrix_symmetrize(double mat[4][4], /*@out@*/ double prod[4][4]) +/* symmetrize a matrix, multiply it by its transpose */ +{ + int i, j, k, n; + + for (i = 0; i < 4; ++i) { //< rows + for (j = 0; j < 4; ++j) { //< cols + prod[i][j] = 0.0; + for (k = 0; k < n; ++k) { + prod[i][j] += mat[k][i] * mat[k][j]; + } + } + } +} +#endif /* UNUSED */ + +/* end */ |