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.

4 files changed, 134 insertions, 92 deletions

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