Index: trunk/psLib/src/math/psMatrix.c
===================================================================
--- trunk/psLib/src/math/psMatrix.c	(revision 24122)
+++ trunk/psLib/src/math/psMatrix.c	(revision 26001)
@@ -94,102 +94,109 @@
 LHS_NAME.data  = RHS_NAME;
 
-
-/*****************************************************************************/
-/* FILE STATIC FUNCTIONS                                                     */
-/*****************************************************************************/
-
-static void  psVectorToGslVector(gsl_vector *outGslVector, const psVector *inVector);
-static void gslVectorToPsVector(psVector *outVector, gsl_vector *inGslVector);
-static void  psImageToGslMatrix(gsl_matrix *outGslMatrix, const psImage *inImage);
-static void gslMatrixToPsImage(psImage *outImage, gsl_matrix *inGslMatrix);
+////////////////////////////////////////////////////////////////////////////////
+// Conversion functions
+////////////////////////////////////////////////////////////////////////////////
+
+// gsl_vector holds *doubles*, so we can directly copy F64, but need to convert F32
 
 /** Static function to copy psF32 or psF64 vector data to a GSL vector */
-static void  psVectorToGslVector(gsl_vector *outGslVector,
-                                 const psVector *inVector)
-{
-    psU32 i = 0;
-    psU32 n = 0;
-
-
-    n = inVector->n;
-    for(i=0; i<n; i++) {
-        if(inVector->type.type == PS_TYPE_F32) {
-            outGslVector->data[i] = (psF64)inVector->data.F32[i];
+static void vectorPStoGSL(gsl_vector *out, const psVector *in)
+{
+    psAssert(out->size == in->n, "Sizes don't match!");
+
+    long n = in->n;                     // Size of input
+    switch (in->type.type) {
+      case PS_TYPE_F32:
+        for (long i = 0; i < n; i++) {
+            out->data[i] = in->data.F32[i];
+        }
+        break;
+      case PS_TYPE_F64:
+        memcpy(out->data, in->data.F64, n * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+        break;
+      default:
+        psAbort("Unsupported vector type: %x\n", in->type.type);
+    }
+    return;
+}
+
+/** Static function to copy GSL vector data to a psF32 or psF64 vector */
+static void vectorGSLtoPS(psVector *out, const gsl_vector *in)
+{
+    psAssert(in->size == out->n, "Sizes don't match!");
+
+    long n = out->n;                    // Size of output
+    switch (out->type.type) {
+      case PS_TYPE_F32:
+        for (long i = 0; i < n; i++) {
+            out->data.F32[i] = in->data[i];
+        }
+        break;
+      case PS_TYPE_F64:
+        memcpy(out->data.F64, in->data, n * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+        break;
+      default:
+        psAbort("Unsupported vector type: %x\n", out->type.type);
+    }
+    return;
+}
+
+
+/** Static function to copy psF32 or psF64 image data to a GSL matrix */
+static void matrixPStoGSL(gsl_matrix *out, const psImage *in)
+{
+    psAssert(out->size1 == in->numRows && out->size2 == in->numCols, "Sizes don't match!");
+
+    int numCols = in->numCols, numRows = in->numRows; // Size of matrix
+    switch (in->type.type) {
+      case PS_TYPE_F32:
+        for (int y = 0, i = 0; y < numRows; y++) {
+            for (int x = 0; x < numCols; x++, i++) {
+                out->data[i] = in->data.F32[y][x];
+            }
+        }
+        break;
+      case PS_TYPE_F64:
+        if (in->parent|| out->tda != out->size1) {
+            for (int y = 0, i = 0; y < numRows; y++, i += numCols) {
+                memcpy(&out->data[i], in->data.F64[y], numCols * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+            }
         } else {
-            outGslVector->data[i] = inVector->data.F64[i];
-        }
-    }
-}
-
-/** Static function to copy GSL vector data to a psF32 or psF64 vector */
-static void gslVectorToPsVector(psVector *outVector,
-                                gsl_vector *inGslVector)
-{
-    psU32 i = 0;
-    psU32 n = 0;
-
-
-    n = outVector->n;
-    for(i=0; i<n; i++) {
-        if(outVector->type.type == PS_TYPE_F32) {
-            outVector->data.F32[i] = (psF32)inGslVector->data[i];
+            memcpy(out->data, in->p_rawDataBuffer, numCols * numRows * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+        }
+        break;
+      default:
+        psAbort("Unsupported vector type: %x\n", in->type.type);
+    }
+    return;
+}
+
+/** Static function to copy GSL matrix data to a psF32 or psF64 image */
+static void matrixGSLtoPS(psImage *out, const gsl_matrix *in)
+{
+    psAssert(in->size1 == out->numRows && in->size2 == out->numCols, "Sizes don't match!");
+
+    int numCols = out->numCols, numRows = out->numRows; // Size of matrix
+    switch (out->type.type) {
+      case PS_TYPE_F32:
+        for (int y = 0, i = 0; y < numRows; y++) {
+            for (int x = 0; x < numCols; x++, i++) {
+                out->data.F32[y][x] = in->data[i];
+            }
+        }
+        break;
+      case PS_TYPE_F64:
+        if (out->parent || in->tda != in->size1) {
+            for (int y = 0, i = 0; y < numRows; y++, i += numCols) {
+                memcpy(out->data.F64[y], &in->data[i], numCols * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+            }
         } else {
-            outVector->data.F64[i] = inGslVector->data[i];
-        }
-    }
-}
-
-/** Static function to copy psF32 or psF64 image data to a GSL matrix */
-static void  psImageToGslMatrix(gsl_matrix *outGslMatrix,
-                                const psImage *inImage)
-{
-    psU32 i = 0;
-    psU32 j = 0;
-    psU32 numRows = 0;
-    psU32 numCols = 0;
-
-
-    numRows = inImage->numRows;
-    numCols = inImage->numCols;
-    if(inImage->type.type == PS_TYPE_F32) {
-        for(i=0; i<numRows; i++) {
-            for(j=0; j<numCols; j++) {
-                outGslMatrix->data[i*numCols+j] = inImage->data.F32[i][j];
-            }
-        }
-    } else {
-        for(i=0; i<numRows; i++) {
-            for(j=0; j<numCols; j++) {
-                outGslMatrix->data[i*numCols+j] = inImage->data.F64[i][j];
-            }
-        }
-    }
-}
-
-/** Static function to copy GSL matrix data to a psF32 or psF64 image */
-static void gslMatrixToPsImage(psImage *outImage,
-                               gsl_matrix *inGslMatrix)
-{
-    psU32 i = 0;
-    psU32 j = 0;
-    psU32 numRows = 0;
-    psU32 numCols = 0;
-
-
-    numRows = outImage->numRows;
-    numCols = outImage->numCols;
-    if(outImage->type.type == PS_TYPE_F32) {
-        for(i=0; i<numRows; i++) {
-            for(j=0; j<numCols; j++) {
-                outImage->data.F32[i][j] = inGslMatrix->data[i*numCols+j];
-            }
-        }
-    } else {
-        for(i=0; i<numRows; i++) {
-            for(j=0; j<numCols; j++) {
-                outImage->data.F64[i][j] = inGslMatrix->data[i*numCols+j];
-            }
-        }
-    }
+            memcpy(out->p_rawDataBuffer, in->data, numCols * numRows * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+        }
+        break;
+      default:
+        psAbort("Unsupported vector type: %x\n", out->type.type);
+    }
+    return;
 }
 
@@ -245,5 +252,5 @@
 
     // Copy psImage data into GSL matrix data
-    psImageToGslMatrix(lu, in);
+    matrixPStoGSL(lu, in);
 
     // Calculate LU decomposition
@@ -251,5 +258,5 @@
 
     // Copy GSL matrix data to psImage data
-    gslMatrixToPsImage(out, lu);
+    matrixGSLtoPS(out, lu);
 
     // Free GSL data
@@ -293,7 +300,7 @@
     // Initialize GSL data
     lu = gsl_matrix_alloc(numRows, numCols);
-    psImageToGslMatrix(lu, LU);
+    matrixPStoGSL(lu, LU);
     b = gsl_vector_alloc(RHS->n);
-    psVectorToGslVector(b, RHS);
+    vectorPStoGSL(b, RHS);
     x = gsl_vector_alloc(RHS->n);
 
@@ -306,5 +313,5 @@
 
     // Copy GSL vector data to psVector data
-    gslVectorToPsVector(out, x);
+    vectorGSLtoPS(out, x);
 
     // Free GSL data
@@ -341,5 +348,5 @@
     // Initialize GSL data
     lu = gsl_matrix_alloc(numRows, numCols);
-    psImageToGslMatrix(lu, LU);
+    matrixPStoGSL(lu, LU);
 
     permGSL.size = perm->n;
@@ -352,5 +359,5 @@
 
     // Copy GSL vector data to psVector data
-    gslMatrixToPsImage(out, inverse);
+    matrixGSLtoPS(out, inverse);
 
     // Free GSL data
@@ -379,37 +386,37 @@
     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])) {
-		      // 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;
+          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])) {
+                      // 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;
       }
       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])) {
-		      // 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;
+          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])) {
+                      // 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;
       }
-	// MATRIX_CHECK_NONFINITE_CASE(F32, a);
-	// MATRIX_CHECK_NONFINITE_CASE(F64, a);
+        // MATRIX_CHECK_NONFINITE_CASE(F32, a);
+        // MATRIX_CHECK_NONFINITE_CASE(F64, a);
       default:
-	psAbort("Should never get here.");
+        psAbort("Should never get here.");
     }
 
@@ -471,31 +478,31 @@
     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;
+          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;
+          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.");
-    }
-  
+        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
@@ -511,149 +518,149 @@
 
     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;
-	psF32 growth = 1.0;
-
-	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 (!isfinite(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;
-	    // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
-	    // Here we are going to raise an error if the dynamic range is too large.
-
-	    /* 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;
-
-	    // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
-	    growth *= tmpval;
-	    psTrace ("psLib.math", 4, "growth : %e\n", growth);
-	    if (fabs(growth) > MAX_RANGE) goto escape;
-
-	    /* 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]]);
-	    }
-	}
+        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;
+        psF32 growth = 1.0;
+
+        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 (!isfinite(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;
+            // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
+            // Here we are going to raise an error if the dynamic range is too large.
+
+            /* 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;
+
+            // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
+            growth *= tmpval;
+            psTrace ("psLib.math", 4, "growth : %e\n", growth);
+            if (fabs(growth) > MAX_RANGE) goto escape;
+
+            /* 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;
-	psF64 growth = 1.0;
-
-	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 (!isfinite(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;
-	    // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
-	    // Here we are going to raise an error if the dynamic range is too large.
-
-	    /* 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;
-
-	    // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
-	    growth *= tmpval;
-	    psTrace ("psLib.math", 4, "growth : %e\n", growth);
-	    if (fabs(growth) > MAX_RANGE) goto escape;
-
-	    /* 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]]);
-	    }
-	}
+        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;
+        psF64 growth = 1.0;
+
+        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 (!isfinite(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;
+            // Kahan replaces the 0.0 pivot with epsilon*(largest element in column) + underFlow.
+            // Here we are going to raise an error if the dynamic range is too large.
+
+            /* 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;
+
+            // check for ill-conditioned matrix: measure the pivot growth and trigger on over/under flow
+            growth *= tmpval;
+            psTrace ("psLib.math", 4, "growth : %e\n", growth);
+            if (fabs(growth) > MAX_RANGE) goto escape;
+
+            /* 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]]);
+            }
+        }
     }
 
@@ -701,5 +708,5 @@
     lu = gsl_matrix_alloc(numRows, numCols);
     inv = gsl_matrix_alloc(numRows, numCols);
-    psImageToGslMatrix(lu, in);
+    matrixPStoGSL(lu, in);
 
     // Invert data and calculate determinant
@@ -708,5 +715,5 @@
     if (determinant) {
       // XXX this is getting the wrong value: is it the wrong calculation?
-      // it disagrees with the results of 
+      // it disagrees with the results of
       // det = (psF32)gsl_linalg_LU_det(lu, signum);
       // used in psMatrixDeterminatn
@@ -716,5 +723,5 @@
 
     // Copy GSL matrix data to psImage data
-    gslMatrixToPsImage(out, inv);
+    matrixGSLtoPS(out, inv);
 
     // Free GSL structs
@@ -749,9 +756,9 @@
     perm = gsl_permutation_alloc(numRows);
     lu = gsl_matrix_alloc(numRows, numCols);
-    psImageToGslMatrix(lu, in);
+    matrixPStoGSL(lu, in);
 
     // Calculate determinant
     gsl_linalg_LU_decomp(lu, perm, &signum);
-    det = (psF32)gsl_linalg_LU_det(lu, signum);
+    det = (psF32)gsl_linalg_LU_lndet(lu);
 
     // Free GSL structs
@@ -877,5 +884,5 @@
 
     inGSL = gsl_matrix_alloc(numRows, numCols);
-    psImageToGslMatrix(inGSL, in);
+    matrixPStoGSL(inGSL, in);
     outGSL = gsl_matrix_alloc(numRows, numCols);
 
@@ -892,5 +899,5 @@
 
     // Copy GSL matrix data to psImage data
-    gslMatrixToPsImage(out, outGSL);
+    matrixGSLtoPS(out, outGSL);
 
     // Free GSL structs
@@ -1026,4 +1033,66 @@
 }
 
+psVector *psMatrixSolveSVD(psVector *out, const psImage *matrix, const psVector *vector)
+{
+    #define psMatrixSolveSVD_EXIT {psFree(out); return NULL; }
+    PS_ASSERT_GENERAL_IMAGE_NON_NULL(matrix, psMatrixSolveSVD_EXIT);
+    PS_CHECK_DIMEN_AND_TYPE(matrix, PS_DIMEN_IMAGE, psMatrixSolveSVD_EXIT);
+    PS_ASSERT_GENERAL_VECTOR_NON_NULL(vector, psMatrixSolveSVD_EXIT);
+    PS_CHECK_DIMEN_AND_TYPE(vector, PS_DIMEN_VECTOR, psMatrixSolveSVD_EXIT);
+
+    int numCols = matrix->numCols, numRows = matrix->numRows; // Size of matrix
+
+    // Decompose matrix: A = U S V^T
+    gsl_matrix *A = gsl_matrix_alloc(numRows, numCols); // Input matrix in GSL-speak; becomes matrix U
+    gsl_matrix *V = gsl_matrix_alloc(numCols, numCols); // Untransposed matrix V
+    gsl_vector *S = gsl_vector_alloc(numCols);          // Singular values
+    gsl_vector *work = gsl_vector_alloc(numCols);       // Work space for GSL
+
+    matrixPStoGSL(A, matrix);
+
+    int gslStatus = 0;                  // Status of GSL
+    if ((gslStatus = gsl_linalg_SV_decomp(A, V, S, work))) {
+        const char *err = gsl_strerror(gslStatus);
+        psError(PS_ERR_UNKNOWN, true, "Unable to decompose matrix: %s", err);
+        gsl_matrix_free(A);
+        gsl_matrix_free(V);
+        gsl_vector_free(S);
+        gsl_vector_free(work);
+        return NULL;
+    }
+    gsl_vector_free(work);
+
+    // Solve system (or minimise least-squares if overconstrained): Ax = b
+    gsl_vector *b = gsl_vector_alloc(numCols); // Vector b
+    gsl_vector *x = gsl_vector_alloc(numCols); // Solution
+
+    vectorPStoGSL(b, vector);
+
+    if ((gslStatus = gsl_linalg_SV_solve(A, V, S, b, x))) {
+        const char *err = gsl_strerror(gslStatus);
+        psError(PS_ERR_UNKNOWN, true, "Unable to solve matrix equation: %s", err);
+        gsl_matrix_free(A);
+        gsl_matrix_free(V);
+        gsl_vector_free(S);
+        gsl_vector_free(b);
+        gsl_vector_free(x);
+        return NULL;
+    }
+
+    gsl_matrix_free(A);
+    gsl_matrix_free(V);
+    gsl_vector_free(S);
+    gsl_vector_free(b);
+
+    out = psVectorRecycle(out, numCols, PS_TYPE_F64);
+
+    vectorGSLtoPS(out, x);
+    gsl_vector_free(x);
+
+    return out;
+}
+
+
+
 // This code supplied by Andy Becker (becker@astro.washington.edu)
 psImage *psMatrixSVD(psImage* evec, psVector* eval, const psImage* in)
@@ -1050,5 +1119,5 @@
 
     // Copy psImage data into GSL matrix data
-    psImageToGslMatrix(A, in);
+    matrixPStoGSL(A, in);
 
     // Calculate SVD decomposition
@@ -1056,6 +1125,6 @@
 
     // Copy GSL matrix data to psImage data
-    gslMatrixToPsImage(evec, V);
-    gslVectorToPsVector(eval, S);
+    matrixGSLtoPS(evec, V);
+    vectorGSLtoPS(eval, S);
 
     // Take the square root of eval