Index: trunk/psLib/src/math/psMatrix.c =================================================================== --- trunk/psLib/src/math/psMatrix.c (revision 24084) +++ trunk/psLib/src/math/psMatrix.c (revision 24085) @@ -199,5 +199,5 @@ /*****************************************************************************/ -psImage* psMatrixLUD(psImage* out, +psImage* psMatrixLUDecomposition(psImage* out, psVector** perm, const psImage* in) @@ -210,16 +210,16 @@ - #define psMatrixLUD_EXIT {psFree(out); return NULL;} + #define psMatrixLUDecomposition_EXIT {psFree(out); return NULL;} // Error checks - PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, psMatrixLUD_EXIT); - PS_CHECK_POINTERS(in, out, psMatrixLUD_EXIT); - PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, psMatrixLUD_EXIT); - PS_ASSERT_GENERAL_PTR_NON_NULL(perm, psMatrixLUD_EXIT); + PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, psMatrixLUDecomposition_EXIT); + PS_CHECK_POINTERS(in, out, psMatrixLUDecomposition_EXIT); + PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, psMatrixLUDecomposition_EXIT); + PS_ASSERT_GENERAL_PTR_NON_NULL(perm, psMatrixLUDecomposition_EXIT); out = psImageRecycle(out, in->numCols, in->numRows, in->type.type); - PS_CHECK_SQUARE(in, psMatrixLUD_EXIT); // gsl_linalg_LU_decomp would fail on non-square input. - PS_CHECK_SQUARE(out, psMatrixLUD_EXIT); + PS_CHECK_SQUARE(in, psMatrixLUDecomposition_EXIT); // gsl_linalg_LU_decomp would fail on non-square input. + PS_CHECK_SQUARE(out, psMatrixLUDecomposition_EXIT); // Initialize data @@ -237,5 +237,5 @@ "Failed to allocate the permutation vector; " "could not determine the cooresponding data type."); - psMatrixLUD_EXIT; + psMatrixLUDecomposition_EXIT; } @@ -259,5 +259,5 @@ } -psVector* psMatrixLUSolve(psVector* out, +psVector* psMatrixLUSolution(psVector* out, const psImage* LU, const psVector* RHS, @@ -316,8 +316,54 @@ } -// 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, +psImage *psMatrixLUInvert(psImage *out, + const psImage* LU, + const psVector* perm) +{ + psS32 numRows = 0; + psS32 numCols = 0; + gsl_matrix *lu, *inverse; + gsl_permutation permGSL; + + #define LUSOLVE_CLEANUP {psFree(out); return NULL;} + + // Error checks + PS_ASSERT_GENERAL_IMAGE_NON_NULL(LU, LUSOLVE_CLEANUP); + PS_CHECK_DIMEN_AND_TYPE(LU, PS_DIMEN_IMAGE, LUSOLVE_CLEANUP); + PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(LU, LUSOLVE_CLEANUP); + PS_ASSERT_GENERAL_VECTOR_NON_NULL(perm, LUSOLVE_CLEANUP); + + out = psImageRecycle(out, LU->numCols, LU->numRows, LU->type.type); + + // Initialize data + numRows = LU->numRows; + numCols = LU->numCols; + + // Initialize GSL data + lu = gsl_matrix_alloc(numRows, numCols); + psImageToGslMatrix(lu, LU); + + permGSL.size = perm->n; + permGSL.data = (psPtr)(perm->data.U8); + + inverse = gsl_matrix_alloc(numRows, numCols); + + // Solve for {x} in equation: {b} = [A]{x} + gsl_linalg_LU_invert(lu, &permGSL, inverse); + + // Copy GSL vector data to psVector data + gslMatrixToPsImage(out, inverse); + + // Free GSL data + gsl_matrix_free(lu); + gsl_matrix_free(inverse); + + return out; +} + +// This is the LU Decomposition version of the matrix equation solver. It solves the equation +// Ax = B, where A is a square matrix (NxN) and B is a vector of length N. This solver only +// yields the solution for x, which is returned to the vector B. This now DOES calculate the +// inverse of A. A and B may be F32 or F64 XXX can they differ? +bool psMatrixLUSolve(psImage *a, psVector *b ) @@ -370,5 +416,5 @@ // Decompose the matrix and solve psVector *perm = NULL; // Permutation vector - psImage *lu = psMatrixLUD(NULL, &perm, a); // LU decomposed matrix + psImage *lu = psMatrixLUDecomposition(NULL, &perm, a); // LU decomposed matrix if (!lu) { psError(PS_ERR_UNKNOWN, false, "Unable to generate LU decomposed matrix"); @@ -376,5 +422,9 @@ return false; } - psVector *ans = psMatrixLUSolve(NULL, lu, b, perm); // Answer + psVector *ans = psMatrixLUSolution(NULL, lu, b, perm); // Answer + + // invert the matrix : check here for an ill-conditioned matrix? + psMatrixLUInvert (a, lu, perm); + psFree(lu); psFree(perm); @@ -390,11 +440,20 @@ } -# 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 +// This is the Gauss-Jordan elimination version of the matrix equation solver. It solves the +// equation Ax = B, where A is a square matrix (NxN) and B is a vector of length N. This +// solver calculates both the solution for x, which is returned to the vector B and the inverse +// of A (returned in A). A and B may be F32 or F64, but must match. + +// Gauss-Jordan elimination using full pivots based on William Kahan's BASIC example and 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 the Press implementation. + +// (based on version by William Kahan -- see Ohana/src/libohana/doc/kahan-gji.pdf) + +# define MAX_RANGE 1.0e7 +// MAX_RANGE is used to test for ill-conditioned input matrices. For an ill-conditioned +// matrix, one or more of the pivots trends towards zero, and growth goes to infinity. Rather +// than allow this to go to the numerical precision, I am raising an error if |growth| > MAX_RANGE bool psMatrixGJSolve(psImage *a, psVector *b @@ -439,8 +498,8 @@ } - // 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. + // 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; @@ -457,4 +516,5 @@ int *rowIndex = rowIndexV->data.S32; int *pivot = pivotV->data.S32; + psF32 growth = 1.0; for (int diag = 0; diag < nSquare; diag++) { @@ -493,5 +553,6 @@ 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 + // 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 */ @@ -500,7 +561,9 @@ 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); + + // 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 */ @@ -526,4 +589,5 @@ int *rowIndex = rowIndexV->data.S32; int *pivot = pivotV->data.S32; + psF64 growth = 1.0; for (int diag = 0; diag < nSquare; diag++) { @@ -562,5 +626,6 @@ 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 + // 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 */ @@ -569,7 +634,9 @@ 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); + + // 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 */ @@ -602,6 +669,4 @@ return false; } - -# endif psImage* psMatrixInvert(psImage* out,