Index: trunk/psLib/src/math/psMatrix.c
===================================================================
--- trunk/psLib/src/math/psMatrix.c	(revision 19843)
+++ trunk/psLib/src/math/psMatrix.c	(revision 23983)
@@ -318,4 +318,5 @@
 // This used to be "a temporary gauss-jordan solver based on gene's version based on the Numerical Recipes
 // version".  However, it's been removed due to copyright, and replaced with LU Decomposition solving.
+# if (0) 
 bool psMatrixGJSolve(psImage *a,
                      psVector *b
@@ -328,25 +329,4 @@
     PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false);
     PS_ASSERT_VECTOR_SIZE(b, (long int)a->numCols, false);
-
-# if (0)
-    // XXX expand this out explicitly below
-    // Check for non-finite entries
-#define MATRIX_CHECK_NONFINITE_CASE(TYPE,MATRIX) \
-  case PS_TYPE_##TYPE: { \
-      ps##TYPE **values = MATRIX->data.TYPE; /* Dereference */ \
-      int numCols = (MATRIX)->numCols, numRows = (MATRIX)->numRows; /* Size of matrix */ \
-      for (int i = 0; i < numRows; i++) { \
-          for (int j = 0; j < numCols; j++) { \
-              if (!isfinite(values[i][j])) { \
-                  // psError(PS_ERR_BAD_PARAMETER_VALUE, 3,		
-		  // "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n", 
-		  // i, j, values[i][j]);				
-                  return false; \
-              } \
-          } \
-      } \
-      break; \
-  }
-# endif
 
     // Check for non-finite entries in matrix
@@ -410,4 +390,218 @@
 }
 
+# else 
+
+// Gauss-Jordan elimination using full pivots based on Press et al's description.  Substantially
+// reworked for psLib style: major modifications to conform to C indexing, use a boolean to track the
+// completed pivot rows and catch the singular matrix early on.  Also, much cleaner control loops
+// than their implementation.  XXX this really needs to check on round-off errors (see version by
+// William Kahan
+bool psMatrixGJSolve(psImage *a,
+                     psVector *b
+    )
+{
+    PS_ASSERT_IMAGE_NON_NULL(a, false);
+    PS_ASSERT_VECTOR_NON_NULL(b, false);
+    PS_ASSERT_IMAGE_TYPE_F32_OR_F64(a, false);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(b, false);
+    psAssert (a->type.type == b->type.type, "types must match for now");
+    PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false);
+    PS_ASSERT_VECTOR_SIZE(b, (long int)a->numCols, false);
+
+    // Check for non-finite entries in matrix
+    switch (a->type.type) {
+      case PS_TYPE_F32: {
+	  psF32 **values = a->data.F32; /* Dereference */
+	  int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
+	  for (int i = 0; i < numRows; i++) {
+	      for (int j = 0; j < numCols; j++) {
+		  if (!isfinite(values[i][j])) {
+		      return false;
+		  }
+	      }
+	  }
+	  break;
+      }
+      case PS_TYPE_F64: {
+	  psF64 **values = a->data.F64; /* Dereference */
+	  int numCols = a->numCols, numRows = a->numRows; /* Size of matrix */
+	  for (int i = 0; i < numRows; i++) {
+	      for (int j = 0; j < numCols; j++) {
+		  if (!isfinite(values[i][j])) {
+		      return false;
+		  }
+	      }
+	  }
+	  break;
+      }
+      default:
+	psAbort("Should never get here.");
+    }
+  
+    // Following the algorithm laid out by Press et al., we loop along the matrix diagonal,
+    // but we do not operate on the diagonal elements in order instead, we are looking for
+    // the current max element and operating on that diagonal element.  this is effectively
+    // column pivoting.  row pivoting is perfomed explicitly.  
+
+    int nSquare = a->numCols;
+
+    psVector *colIndexV = psVectorAlloc(nSquare, PS_TYPE_S32);
+    psVector *rowIndexV = psVectorAlloc(nSquare, PS_TYPE_S32);
+    psVector *pivotV    = psVectorAlloc(nSquare, PS_TYPE_S32);
+    psVectorInit (pivotV, 0.0);
+
+    if (a->type.type == PS_TYPE_F32) {
+	psF32 **A = a->data.F32;
+	psF32  *B = b->data.F32;
+	int *colIndex = colIndexV->data.S32;
+	int *rowIndex = rowIndexV->data.S32;
+	int *pivot    = pivotV->data.S32;
+
+	for (int diag = 0; diag < nSquare; diag++) {
+
+	    psF32 maxval = 0.0;
+	    int maxrow = 0;
+	    int maxcol = 0;
+
+	    // search for the next pivot
+	    for (int row = 0; row < nSquare; row++) {
+		if (!finite(A[row][diag])) goto escape;
+
+		// if we have already operated on this row (pivot[row] is true), skip it
+		if (pivot[row]) continue;
+
+		// if we have not yet operated on this row (pivot[row] is false), look for pivot for this row
+		for (int col = 0; col < nSquare; col++) {
+		    if (pivot[col]) continue;
+		    if (fabs (A[row][col]) < maxval) continue;
+		    maxval = fabs (A[row][col]);
+		    maxrow = row;
+		    maxcol = col;
+		}
+	    }
+
+	    // if pivot[maxcol] is set, we have already done this row: this implies a singular matrix
+	    if (pivot[maxcol]) goto escape;
+	    pivot[maxcol] = 1;
+
+	    // if the selected pivot is off the diagonal, do a row swap
+	    if (maxrow != maxcol) {
+		for (int col = 0; col < nSquare; col++) PS_SWAP (A[maxrow][col], A[maxcol][col]);
+		PS_SWAP (B[maxrow], B[maxcol]);
+	    }
+	    rowIndex[diag] = maxrow;
+	    colIndex[diag] = maxcol;
+	    if (A[maxcol][maxcol] == 0.0) goto escape;
+	    // XXX Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow
+
+	    /* rescale by pivot reciprocal */
+	    psF32 tmpval = 1.0 / A[maxcol][maxcol];
+	    A[maxcol][maxcol] = 1.0;
+	    for (int col = 0; col < nSquare; col++) A[maxcol][col] *= tmpval;
+	    B[maxcol] *= tmpval;
+	    // XXX measure the pivot growth and trigger on over/under flow
+	    // growth *= tmpval;
+	    // fprintf (stderr, "column: %d, growth: %e, epsilon: %e\n", maxcol, growth, epsilon);
+
+	    /* adjust the elements above the pivot */
+	    for (int row = 0; row < nSquare; row++) {
+		if (row == maxcol) continue;
+		tmpval = A[row][maxcol];
+		A[row][maxcol] = 0.0;
+		for (int col = 0; col < nSquare; col++) A[row][col] -= A[maxcol][col]*tmpval;
+		B[row] -= B[maxcol]*tmpval;
+	    }
+	}
+
+	// swap back the inverse matrix based on the row swaps above
+	for (int col = nSquare - 1; col >= 0; col--) {
+	    if (rowIndex[col] != colIndex[col]) {
+		for (int row = 0; row < nSquare; row++) PS_SWAP (A[row][rowIndex[col]], A[row][colIndex[col]]);
+	    }
+	}
+    } else {
+	psF64 **A = a->data.F64;
+	psF64  *B = b->data.F64;
+	int *colIndex = colIndexV->data.S32;
+	int *rowIndex = rowIndexV->data.S32;
+	int *pivot    = pivotV->data.S32;
+
+	for (int diag = 0; diag < nSquare; diag++) {
+
+	    psF64 maxval = 0.0;
+	    int maxrow = 0;
+	    int maxcol = 0;
+
+	    // search for the next pivot
+	    for (int row = 0; row < nSquare; row++) {
+		if (!finite(A[row][diag])) goto escape;
+
+		// if we have already operated on this row (pivot[row] is true), skip it
+		if (pivot[row]) continue;
+
+		// if we have not yet operated on this row (pivot[row] is false), look for pivot for this row
+		for (int col = 0; col < nSquare; col++) {
+		    if (pivot[col]) continue;
+		    if (fabs (A[row][col]) < maxval) continue;
+		    maxval = fabs (A[row][col]);
+		    maxrow = row;
+		    maxcol = col;
+		}
+	    }
+
+	    // if pivot[maxcol] is set, we have already done this row: this implies a singular matrix
+	    if (pivot[maxcol]) goto escape;
+	    pivot[maxcol] = 1;
+
+	    // if the selected pivot is off the diagonal, do a row swap
+	    if (maxrow != maxcol) {
+		for (int col = 0; col < nSquare; col++) PS_SWAP (A[maxrow][col], A[maxcol][col]);
+		PS_SWAP (B[maxrow], B[maxcol]);
+	    }
+	    rowIndex[diag] = maxrow;
+	    colIndex[diag] = maxcol;
+	    if (A[maxcol][maxcol] == 0.0) goto escape;
+	    // XXX Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow
+
+	    /* rescale by pivot reciprocal */
+	    psF64 tmpval = 1.0 / A[maxcol][maxcol];
+	    A[maxcol][maxcol] = 1.0;
+	    for (int col = 0; col < nSquare; col++) A[maxcol][col] *= tmpval;
+	    B[maxcol] *= tmpval;
+	    // XXX measure the pivot growth and trigger on over/under flow
+	    // growth *= tmpval;
+	    // fprintf (stderr, "column: %d, growth: %e, epsilon: %e\n", maxcol, growth, epsilon);
+
+	    /* adjust the elements above the pivot */
+	    for (int row = 0; row < nSquare; row++) {
+		if (row == maxcol) continue;
+		tmpval = A[row][maxcol];
+		A[row][maxcol] = 0.0;
+		for (int col = 0; col < nSquare; col++) A[row][col] -= A[maxcol][col]*tmpval;
+		B[row] -= B[maxcol]*tmpval;
+	    }
+	}
+
+	// swap back the inverse matrix based on the row swaps above
+	for (int col = nSquare - 1; col >= 0; col--) {
+	    if (rowIndex[col] != colIndex[col]) {
+		for (int row = 0; row < nSquare; row++) PS_SWAP (A[row][rowIndex[col]], A[row][colIndex[col]]);
+	    }
+	}
+    }
+
+    psFree (pivotV);
+    psFree (rowIndexV);
+    psFree (colIndexV);
+    return true;
+
+escape:
+    psFree (pivotV);
+    psFree (rowIndexV);
+    psFree (colIndexV);
+    return false;
+}
+
+# endif
 
 psImage* psMatrixInvert(psImage* out,