From 103109b3697bbea9fddfa3f0e221f3f583ad2f8a Mon Sep 17 00:00:00 2001 From: "Eric S. Raymond" Date: Sat, 20 Sep 2014 07:43:35 -0400 Subject: 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. --- SConstruct | 1 + libgpsd_core.c | 94 +------------------------------------------- matrix.c | 120 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++ matrix.h | 11 ++++++ 4 files changed, 134 insertions(+), 92 deletions(-) create mode 100644 matrix.c create mode 100644 matrix.h diff --git a/SConstruct b/SConstruct index 4480c833..a2e0a232 100644 --- a/SConstruct +++ b/SConstruct @@ -846,6 +846,7 @@ libgpsd_sources = [ "geoid.c", "isgps.c", "libgpsd_core.c", + "matrix.c", "net_dgpsip.c", "net_gnss_dispatch.c", "net_ntrip.c", diff --git a/libgpsd_core.c b/libgpsd_core.c index 7b679174..c35d98f7 100644 --- a/libgpsd_core.c +++ b/libgpsd_core.c @@ -32,6 +32,7 @@ #endif /* S_SPLINT_S */ #include "gpsd.h" +#include "matrix.h" #if defined(NMEA2000_ENABLE) #include "driver_nmea2000.h" #endif /* defined(NMEA2000_ENABLE) */ @@ -653,97 +654,6 @@ driver. ******************************************************************************/ -/*@ -fixedformalarray -mustdefine @*/ -static bool invert(double mat[4][4], /*@out@*/ double inverse[4][4]) -{ - // 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 @*/ static gps_mask_t fill_dop(const struct gpsd_errout_t *errout, const struct gps_data_t * gpsdata, @@ -814,7 +724,7 @@ static gps_mask_t fill_dop(const struct gpsd_errout_t *errout, } #endif /* __UNUSED__ */ - if (invert(prod, inv)) { + if (matrix_invert(prod, inv)) { #ifdef __UNUSED__ /* * Note: this will print garbage unless all the subdeterminants 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 +#include + +#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 */ diff --git a/matrix.h b/matrix.h new file mode 100644 index 00000000..9050e6b6 --- /dev/null +++ b/matrix.h @@ -0,0 +1,11 @@ +/* + * matrix.h - matrix-algebra prototypes + * + * This file is Copyright (c)2010 by the GPSD project + * BSD terms apply: see the file COPYING in the distribution root for details. + */ + +extern bool matrix_invert(double mat[4][4], /*@out@*/ double inverse[4][4]); +extern void matrix_symmetrize(double mat[4][4], /*@out@*/ double inverse[4][4]); + +/* end */ -- cgit v1.2.1