Index: trunk/psLib/src/math/Makefile.am =================================================================== --- trunk/psLib/src/math/Makefile.am (revision 16696) +++ trunk/psLib/src/math/Makefile.am (revision 16793) @@ -19,4 +19,5 @@ psPolynomialMetadata.c \ psPolynomialUtils.c \ + psPolynomialMD.c \ psRandom.c \ psRegion.c \ @@ -47,4 +48,5 @@ psPolynomialMetadata.h \ psPolynomialUtils.h \ + psPolynomialMD.h \ psRandom.h \ psRegion.h \ Index: trunk/psLib/src/math/psPolynomialMD.c =================================================================== --- trunk/psLib/src/math/psPolynomialMD.c (revision 16793) +++ trunk/psLib/src/math/psPolynomialMD.c (revision 16793) @@ -0,0 +1,439 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "psPolynomialMD.h" + + +#define DEBUG + +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Private (file-static) functions +////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +// Destructor +static void polynomialMDFree(psPolynomialMD *poly // Polynomial + ) +{ + psFree(poly->orders); + psFree(poly->coeff); + return; +} + +// Generate a least-squares matrix and vector +// Note: only generates the diagonal and lower triangle of the matrix +static void polynomialMDLeastSquares(const psPolynomialMD *poly, // Polynomial + psImage *matrix, // Least-squares matrix to fill out + psVector *vector, // Least-squares vector to fill out + const psVector *coords, // Coordinates + float value, // Value + float error, // Error + psVector *buffer // Buffer for evaluations + ) +{ + int numTerms = vector->n; // Number of terms + assert(matrix->numCols == numTerms && matrix->numRows == numTerms); + assert(buffer && buffer->n == numTerms && buffer->type.type == PS_TYPE_F64); + +#ifdef DEBUG + psImageInit(matrix, NAN); + psVectorInit(vector, NAN); +#endif + + buffer->data.F64[0] = 1.0; + for (int i = 0, index = 1; i < poly->dim; i++) { + int order = poly->orders->data.U8[i]; // Order of polynomial + float coord = coords->data.F32[i]; // Coordinate of interest + double value = coord; // Value of polynomial stages + buffer->data.F64[index++] = value; + for (int j = 2; j <= order; j++, index++) { + value *= coord; + buffer->data.F64[index] = value; + } + } + + double invSigma2 = (error == 0.0) ? 1.0 : 1.0 / PS_SQR(error); // 1/sigma^2 + for (int i = 0; i < numTerms; i++) { + for (int j = 0; j < i; j++) { + matrix->data.F64[i][j] = buffer->data.F64[i] * buffer->data.F64[j] * invSigma2; + } + matrix->data.F64[i][i] = PS_SQR(buffer->data.F64[i]) * invSigma2; + vector->data.F64[i] = value * buffer->data.F64[i] * invSigma2; + } + + return; +} + +// Accumulate the lower triangle of the matrix +static void polynomialMDAccumulate(psImage *targetMatrix, // Final least-squares matrix + psVector *targetVector, // Final least-squares vector + const psImage *sourceMatrix, // Input least-squares matrix + const psVector *sourceVector // Input least-squares vector + ) +{ + int numTerms = targetVector->n; // Number of terms in polynomial + + for (int j = 0; j < numTerms; j++) { + for (int k = 0; k < j; k++) { + targetMatrix->data.F64[j][k] += sourceMatrix->data.F64[j][k]; + } + targetMatrix->data.F64[j][j] += sourceMatrix->data.F64[j][j]; + targetVector->data.F64[j] += sourceVector->data.F64[j]; + } + + return; +} + +// Fill in the upper triangle of the matrix +static void polynomialMDFill(psImage *matrix // Final least-squares matrix + ) +{ + int numTerms = matrix->numCols; // Number of terms in polynomial + + for (int j = 0; j < numTerms; j++) { + for (int k = j + 1; k < numTerms; k++) { + matrix->data.F64[j][k] = matrix->data.F64[k][j]; + } + } + + return; +} + +// Accumulate and solve the least-squares equation +static bool polynomialMDClipFit(psPolynomialMD *poly, // Polynomial + psImage *matrix, // Least-squares matrix + psVector *vector, // Least-squares vector + psImage **lu, // LU-decomposed matrix + psVector **perm, // Permutations vector + const psArray *matrices, // Individual least-squares matrices + const psArray *vectors, // Individual least-squares vectors + const psVector *mask // Mask for values + ) +{ + int numValues = vectors->n; // Number of values + + // Accumulate the least-squares matrix and vector + psImageInit(matrix, 0.0); + psVectorInit(vector, 0.0); + for (int i = 0; i < numValues; i++) { + if (mask->data.U8[i]) { + continue; + } + + psImage *newMatrix = matrices->data[i]; // Matrix to accumulate + psVector *newVector = vectors->data[i]; // Vector to accumulate + + polynomialMDAccumulate(matrix, vector, newMatrix, newVector); + } + polynomialMDFill(matrix); + + // Solve least-squares equation + *lu = psMatrixLUD(*lu, perm, matrix); + if (!*lu) { + psError(PS_ERR_UNKNOWN, false, "Unable to LU-Decompose least-squares matrix."); + return false; + } + poly->coeff = psMatrixLUSolve(poly->coeff, *lu, vector, *perm); + if (!poly->coeff) { + psError(PS_ERR_UNKNOWN, false, "Unable to solve least-squares equation."); + return false; + } + return true; +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Public functions +////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +psPolynomialMD *psPolynomialMDAlloc(const psVector *orders) +{ + psPolynomialMD *poly = psAlloc(sizeof(psPolynomialMD)); + psMemSetDeallocator(poly, (psFreeFunc)polynomialMDFree); + + int dim = orders->n; // Number of dimensions + + #define CHECK_ORDER_CASE_SIGNED(TYPE) \ + case PS_TYPE_##TYPE: { \ + for (int i = 0; i < dim; i++) { \ + if (orders->data.TYPE[i] <= 0) { \ + psAbort("Order %d is non-positive.", i); \ + } \ + } \ + break; \ + } + + switch (orders->type.type) { + CHECK_ORDER_CASE_SIGNED(S8); + CHECK_ORDER_CASE_SIGNED(S16); + CHECK_ORDER_CASE_SIGNED(S32); + CHECK_ORDER_CASE_SIGNED(S64); + case PS_TYPE_F32: + case PS_TYPE_F64: + psAbort("Floating-point order vector is not supported."); + case PS_TYPE_U8: + case PS_TYPE_U16: + case PS_TYPE_U32: + case PS_TYPE_U64: + // These can pass unchecked + break; + default: + psAbort("Unknown type: %x", orders->type.type); + } + + poly->dim = dim; + poly->orders = psVectorCopy(NULL, orders, PS_TYPE_U8); + + int numTerms = 1; // Number of terms in polynomial + for (int i = 0; i < dim; i++) { + numTerms += poly->orders->data.U8[i]; + } + poly->coeff = psVectorAlloc(numTerms, PS_TYPE_F64); + + return poly; +} + +double psPolynomialMDEval(const psPolynomialMD *poly, ///< Polynomial + const psVector *coords ///< Coordinates + ) +{ + PS_ASSERT_POLYNOMIALMD_NON_NULL(poly, NAN); + PS_ASSERT_VECTOR_NON_NULL(coords, NAN); + PS_ASSERT_VECTOR_SIZE(coords, (long)poly->dim, NAN); + PS_ASSERT_VECTOR_TYPE(coords, PS_TYPE_F32, NAN); + + psVector *orders = poly->orders; // Orders for each polynomial + psF64 *coeff = poly->coeff->data.F64; // Coefficients + + double sum = coeff[0]; // Sum of all polynomials + for (int i = 0, index = 0; i < poly->dim; i++) { + int order = orders->data.U8[i]; // Order of polynomial + float coord = coords->data.F32[i]; // Coordinate of interest + float value = 1.0 + coeff[index++]; // Value of the polynomial term + for (int j = 2; j <= order; j++, index++) { + value *= coord; + sum += coeff[index] * value; + } + } + + return sum; +} + +bool psPolynomialMDFit(psPolynomialMD *poly, const psVector *values, const psVector *errors, + const psVector *mask, psMaskType maskVal, const psArray *coordsArray) +{ + PS_ASSERT_POLYNOMIALMD_NON_NULL(poly, false); + PS_ASSERT_VECTOR_NON_NULL(values, false); + PS_ASSERT_VECTOR_TYPE(values, PS_TYPE_F32, false); + if (errors) { + PS_ASSERT_VECTOR_NON_NULL(errors, false); + PS_ASSERT_VECTOR_TYPE(errors, PS_TYPE_F32, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(values, errors, false); + } + if (maskVal == 0) { + mask = NULL; + } + if (mask) { + PS_ASSERT_VECTOR_NON_NULL(mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_MASK, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(values, mask, false); + } + PS_ASSERT_ARRAY_NON_NULL(coordsArray, false); + PS_ASSERT_ARRAY_SIZE(coordsArray, values->n, false); + + int numValues = values->n; // Number of values + int numTerms = poly->coeff->n; // Number of terms + psImage *matrix = psImageAlloc(numTerms, numTerms, PS_TYPE_F64); // Least-squares matrix + psImageInit(matrix, 0.0); + psVector *vector = psVectorAlloc(numTerms, PS_TYPE_F64); // Least-squares vector + psVectorInit(vector, 0.0); + + psImage *newMatrix = psImageAlloc(numTerms, numTerms, PS_TYPE_F64); // Least-squares matrix for each term + psVector *newVector = psVectorAlloc(numTerms, PS_TYPE_F64); // Least-squares vector for each term + psVector *buffer = psVectorAlloc(numTerms, PS_TYPE_F64); // Buffer for evaluations of polynomial stages + + for (int i = 0; i < numValues; i++) { + psVector *coords = coordsArray->data[i]; + PS_ASSERT_VECTOR_NON_NULL(coords, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(coords, poly->orders, false); + PS_ASSERT_VECTOR_TYPE(coords, PS_TYPE_F32, false); + + if (mask && (mask->data.PS_TYPE_MASK_DATA[i] & maskVal)) { + continue; + } + + polynomialMDLeastSquares(poly, newMatrix, newVector, coords, values->data.F32[i], + errors ? errors->data.F32[i] : 0.0, buffer); + polynomialMDAccumulate(matrix, vector, newMatrix, newVector); + } + polynomialMDFill(matrix); + + psFree(newMatrix); + psFree(newVector); + psFree(buffer); + + psVector *perm = NULL; // Permutation vector + psImage *lu = psMatrixLUD(NULL, &perm, matrix); // LU-decomposed matrix + psFree(matrix); + if (!lu) { + psError(PS_ERR_UNKNOWN, false, "Unable to LU-Decompose least-squares matrix."); + psFree(vector); + return false; + } + poly->coeff = psMatrixLUSolve(poly->coeff, lu, vector, perm); + psFree(vector); + psFree(lu); + psFree(perm); + if (!poly->coeff) { + psError(PS_ERR_UNKNOWN, false, "Unable to solve least-squares equation."); + return false; + } + + return true; +} + +bool psPolynomialMDClipFit(psPolynomialMD *poly, const psVector *values, const psVector *errors, + const psVector *mask, psMaskType maskVal, const psArray *coordsArray, + int numIter, float rej) +{ + PS_ASSERT_POLYNOMIALMD_NON_NULL(poly, false); + PS_ASSERT_VECTOR_NON_NULL(values, false); + PS_ASSERT_VECTOR_TYPE(values, PS_TYPE_F32, false); + if (errors) { + PS_ASSERT_VECTOR_NON_NULL(errors, false); + PS_ASSERT_VECTOR_TYPE(errors, PS_TYPE_F32, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(values, errors, false); + } + if (maskVal == 0) { + mask = NULL; + } + if (mask) { + PS_ASSERT_VECTOR_NON_NULL(mask, false); + PS_ASSERT_VECTOR_TYPE(mask, PS_TYPE_MASK, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(values, mask, false); + } + PS_ASSERT_ARRAY_NON_NULL(coordsArray, false); + PS_ASSERT_ARRAY_SIZE(coordsArray, (long)values->n, false); + PS_ASSERT_INT_POSITIVE(numIter, false); + PS_ASSERT_FLOAT_LARGER_THAN(rej, 0.0, false); + + int numValues = values->n; // Number of values + int numTerms = poly->coeff->n; // Number of terms + + psArray *matrices = psArrayAlloc(numValues); // Least-squares matrix for each input value + psArray *vectors = psArrayAlloc(numValues); // Least-squares vector for each input value + + // Generate least-squares matrices and vectors + psVector *buffer = psVectorAlloc(numTerms, PS_TYPE_F64); // Buffer for evaluations of polynomial stages + for (int i = 0; i < numValues; i++) { + psVector *coords = coordsArray->data[i]; + PS_ASSERT_VECTOR_NON_NULL(coords, false); + PS_ASSERT_VECTORS_SIZE_EQUAL(coords, poly->orders, false); + PS_ASSERT_VECTOR_TYPE(coords, PS_TYPE_F32, false); + + if (mask && (mask->data.PS_TYPE_MASK_DATA[i] & maskVal)) { + continue; + } + + matrices->data[i] = psImageAlloc(numTerms, numTerms, PS_TYPE_F64); + vectors->data[i] = psVectorAlloc(numTerms, PS_TYPE_F64); + + polynomialMDLeastSquares(poly, matrices->data[i], vectors->data[i], coords, values->data.F32[i], + errors ? errors->data.F32[i] : 0.0, buffer); + } + psFree(buffer); + + // Iterate over the solution + int numGood = numValues; // Number of good values + psImage *matrix = psImageAlloc(numTerms, numTerms, PS_TYPE_F64); // Least-squares matrix + psVector *vector = psVectorAlloc(numTerms, PS_TYPE_F64); // Least-squares vector + psVector *ownMask = psVectorAlloc(numValues, PS_TYPE_U8); // Our own mask for input values + if (mask) { + for (int i = 0; i < numValues; i++) { + if (mask->data.PS_TYPE_MASK_DATA[i] & maskVal) { + ownMask->data.U8[i] = 0xff; + numGood--; + } + } + } + psImage *lu = NULL; // LU-decomposed matrix + psVector *perm = NULL; // Permutation vector + psVector *deviations = psVectorAlloc(numValues, PS_TYPE_F32); // Deviations from fit + int numClipped = INT_MAX; // Number of values clipped int an interation + for (int iter = 0; iter < numIter && numClipped > 0 && numGood > numTerms; iter++) { + numClipped = 0; + + if (!polynomialMDClipFit(poly, matrix, vector, &lu, &perm, matrices, vectors, ownMask)) { + psFree(matrix); + psFree(vector); + psFree(lu); + psFree(perm); + psFree(matrices); + psFree(vectors); + psFree(ownMask); + psFree(deviations); + return false; + } + + // Evaluate distribution + double rms = 0.0; // Root mean square deviation + for (int i = 0; i < numValues; i++) { + if (ownMask->data.U8[i]) { + continue; + } + float diff = values->data.F32[i] - psPolynomialMDEval(poly, coordsArray->data[i]); + deviations->data.F32[i] = diff; + rms += PS_SQR(diff); + } + rms = sqrt(rms) / numGood; + + // Reject + float limit = rej * rms; // Rejection limit + for (int i = 0; i < numValues; i++) { + if (ownMask->data.U8[i]) { + continue; + } + if (deviations->data.F32[i] > limit) { + ownMask->data.U8[i] = 0xff; + numClipped++; + numGood--; + } + } + } + + if (numClipped > 0) { + // Need to do a final re-evaluation of the fit + if (!polynomialMDClipFit(poly, matrix, vector, &lu, &perm, matrices, vectors, ownMask)) { + psFree(matrix); + psFree(vector); + psFree(lu); + psFree(perm); + psFree(matrices); + psFree(vectors); + psFree(ownMask); + psFree(deviations); + return false; + } + } + + psFree(matrix); + psFree(vector); + psFree(lu); + psFree(perm); + psFree(matrices); + psFree(vectors); + psFree(ownMask); + psFree(deviations); + + return true; +} Index: trunk/psLib/src/math/psPolynomialMD.h =================================================================== --- trunk/psLib/src/math/psPolynomialMD.h (revision 16793) +++ trunk/psLib/src/math/psPolynomialMD.h (revision 16793) @@ -0,0 +1,52 @@ +#ifndef PS_POLYNOMIAL_MD_H +#define PS_POLYNOMIAL_MD_H + +#include +#include +#include + +/// Multi-dimensional polynomial +typedef struct { + int dim; ///< Dimensions + psVector *orders; ///< Polynomial orders for each dimension + psVector *coeff; ///< Coefficients +} psPolynomialMD; + +/// Assertion for valid polynomial +#define PS_ASSERT_POLYNOMIALMD_NON_NULL(POLY, RETURN) \ + if (!(POLY) || (POLY)->dim <= 0 || !(POLY)->orders || (POLY)->orders->n != (POLY)->dim || \ + !(POLY)->coeff) { \ + psError(PS_ERR_UNEXPECTED_NULL, true, "Invalid polynomial."); \ + return RETURN; \ + } + +/// Constructor +psPolynomialMD *psPolynomialMDAlloc(const psVector *orders ///< Orders for each dimension + ) PS_ATTR_MALLOC; + +/// Evaluate a polynomial +double psPolynomialMDEval(const psPolynomialMD *poly, ///< Polynomial + const psVector *coords ///< Coordinates + ); + +/// Fit a polynomial +bool psPolynomialMDFit(psPolynomialMD *poly, ///< Polynomial to fit + const psVector *values, ///< Values + const psVector *errors, ///< Errors + const psVector *mask, ///< Mask + psMaskType maskVal, ///< Value to mask + const psArray *coordsArray ///< Array of coordinates + ); + +/// Fit a polynomial, with clipping +bool psPolynomialMDClipFit(psPolynomialMD *poly, ///< Polynomial to fit + const psVector *values, ///< Values + const psVector *errors, ///< Errors + const psVector *mask, ///< Mask + psMaskType maskVal, ///< Value to mask + const psArray *coordsArray, ///< Array of coordinates + int numIter, ///< Number of rejection iterations + float rej ///< Rejection limit, standard deviations + ); + +#endif Index: trunk/psLib/src/pslib_strict.h =================================================================== --- trunk/psLib/src/pslib_strict.h (revision 16696) +++ trunk/psLib/src/pslib_strict.h (revision 16793) @@ -9,6 +9,6 @@ * @author Eric Van Alst, MHPCC * -* @version $Revision: 1.34 $ $Name: not supported by cvs2svn $ -* @date $Date: 2008-01-23 03:08:03 $ +* @version $Revision: 1.35 $ $Name: not supported by cvs2svn $ +* @date $Date: 2008-03-04 22:02:09 $ * * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii @@ -78,4 +78,5 @@ #include "psPolynomialMetadata.h" #include "psPolynomialUtils.h" +#include "psPolynomialMD.h" #include "psSort.h" #include "psSpline.h" Index: trunk/psLib/test/math/.cvsignore =================================================================== --- trunk/psLib/test/math/.cvsignore (revision 16696) +++ trunk/psLib/test/math/.cvsignore (revision 16793) @@ -52,2 +52,3 @@ tap_psRandom tap_psSpline1D +tap_psPolynomialMD Index: trunk/psLib/test/math/Makefile.am =================================================================== --- trunk/psLib/test/math/Makefile.am (revision 16696) +++ trunk/psLib/test/math/Makefile.am (revision 16793) @@ -49,5 +49,6 @@ tap_psRandom \ tap_psMinimizePowell \ - tap_psSpline1D + tap_psSpline1D \ + tap_psPolynomialMD if BUILD_TESTS Index: trunk/psLib/test/math/tap_psPolynomialMD.c =================================================================== --- trunk/psLib/test/math/tap_psPolynomialMD.c (revision 16793) +++ trunk/psLib/test/math/tap_psPolynomialMD.c (revision 16793) @@ -0,0 +1,97 @@ +#include +#include + +#include "tap.h" +#include "pstap.h" + +#define XORDER 3 +#define YORDER 2 +static float constant = 1.0; +static float xCoeff[XORDER] = { 5.0, -3.0, 2.0 }; +static float yCoeff[YORDER] = { -6.0, 4.0 }; +#define XNUM 10 +#define YNUM 10 +#define NUM ((XNUM)*(YNUM)) +#define TOL 1.0e-8 + +int main(int argc, char *argv[]) +{ + plan_tests(2 * (1 + (1 + XORDER + YORDER)) + 1); + + { + psMemId id = psMemGetId(); + + psArray *ordinates = psArrayAlloc(NUM); + psVector *values = psVectorAlloc(NUM, PS_TYPE_F32); + + for (int i = 0, index = 0; i < XNUM; i++) { + int x = i - XNUM/2; + for (int j = 0; j < YNUM; j++, index++) { + int y = j - YNUM/2; + + float value = constant; + for (int k = 0; k < XORDER; k++) { + value += xCoeff[k] * powf(x, k+1); + } + for (int k = 0; k < YORDER; k++) { + value += yCoeff[k] * powf(y, k+1); + } + + values->data.F32[index] = value; + psVector *ord = psVectorAlloc(2, PS_TYPE_F32); + ord->data.F32[0] = x; + ord->data.F32[1] = y; + ordinates->data[index] = ord; + } + } + + psVector *orders = psVectorAlloc(2, PS_TYPE_S32); + orders->data.S32[0] = XORDER; + orders->data.S32[1] = YORDER; + + { + psPolynomialMD *poly = psPolynomialMDAlloc(orders); + bool polyOK = psPolynomialMDFit(poly, values, NULL, NULL, 0, ordinates); + ok(polyOK, "Fit polynomial"); + skip_start(!polyOK, 1+XORDER+YORDER, "Skipping coefficient checks since fit failed."); + int index = 0; + is_double_tol(poly->coeff->data.F64[index], constant, TOL, "Coefficient %d", index); + index++; + for (int i = 0; i < XORDER; i++, index++) { + is_double_tol(poly->coeff->data.F64[index], xCoeff[i], TOL, "Coefficient %d", index); + } + for (int i = 0; i < YORDER; i++, index++) { + is_double_tol(poly->coeff->data.F64[index], yCoeff[i], TOL, "Coefficient %d", index); + } + skip_end(); + psFree(poly); + } + + { + psPolynomialMD *poly = psPolynomialMDAlloc(orders); + values->data.F32[NUM/2] *= 10; + bool polyOK = psPolynomialMDClipFit(poly, values, NULL, NULL, 0, ordinates, 1, 3.0); + ok(polyOK, "Clip-fit polynomial"); + skip_start(!polyOK, 1+XORDER+YORDER, "Skipping coefficient checks since clip-fit failed."); + int index = 0; + is_double_tol(poly->coeff->data.F64[index], constant, TOL, "Coefficient %d", index); + index++; + for (int i = 0; i < XORDER; i++, index++) { + is_double_tol(poly->coeff->data.F64[index], xCoeff[i], TOL, "Coefficient %d", index); + } + for (int i = 0; i < YORDER; i++, index++) { + is_double_tol(poly->coeff->data.F64[index], yCoeff[i], TOL, "Coefficient %d", index); + } + skip_end(); + psFree(poly); + } + + psFree(orders); + psFree(values); + psFree(ordinates); + + ok(!psMemCheckLeaks (id, NULL, NULL, false), "no memory leaks"); + } + + exit(EXIT_SUCCESS); +}