Index: trunk/psLib/src/math/psSpline.c =================================================================== --- trunk/psLib/src/math/psSpline.c (revision 5102) +++ trunk/psLib/src/math/psSpline.c (revision 5155) @@ -1,14 +1,12 @@ /** @file psSpline.c * -* @brief Contains basic function allocation, deallocation, and evaluation -* routines. +* @brief Contains basic spline allocation, deallocation, fitting, +* and evaluation routines. * * This file will hold the functions for allocated, freeing, and evaluating * splines. * -* @version $Revision: 1.128 $ $Name: not supported by cvs2svn $ -* @date $Date: 2005-09-23 01:56:54 $ -* -* +* @version $Revision: 1.129 $ $Name: not supported by cvs2svn $ +* @date $Date: 2005-09-27 23:16:59 $ * * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii @@ -23,5 +21,4 @@ #include -#include "psRandom.h" #include "psMemory.h" #include "psVector.h" @@ -42,7 +39,4 @@ /* TYPE DEFINITIONS */ /*****************************************************************************/ -static void spline1DFree(psSpline1D *tmpSpline); -static unsigned int vectorBinDisectF32(psF32 *bins,unsigned int numBins,psF32 x); -static unsigned int vectorBinDisectS32(psS32 *bins,unsigned int numBins,psS32 x); /*****************************************************************************/ @@ -50,25 +44,14 @@ /*****************************************************************************/ -// None - /*****************************************************************************/ /* FILE STATIC VARIABLES */ /*****************************************************************************/ -// None - /*****************************************************************************/ /* FUNCTION IMPLEMENTATION - LOCAL */ /*****************************************************************************/ -bool psMemCheckSpline1D(psPtr ptr) -{ - return ( psMemGetDeallocator(ptr) == (psFreeFunc)spline1DFree ); -} - static void spline1DFree(psSpline1D *tmpSpline) { - unsigned int i; - if (tmpSpline == NULL) { return; @@ -76,5 +59,5 @@ if (tmpSpline->spline != NULL) { - for (i=0;in;i++) { + for (psS32 i=0;in;i++) { psFree((tmpSpline->spline)[i]); } @@ -85,124 +68,8 @@ psFree(tmpSpline->p_psDeriv2); } + psFree(tmpSpline->knots); return; -} - -/***************************************************************************** -fullInterpolate1DF32(): This routine will take as input n-element floating -point arrays domain and range, and the x value, assumed to lie with the -domain vector. It produces as output the (n-1)-order LaGrange interpolated -value of x. - -XXX: do we error check for non-distinct domain values? - *****************************************************************************/ -#define FUNC_MACRO_FULL_INTERPOLATE_1D(TYPE) \ -static psF32 fullInterpolate1D##TYPE(ps##TYPE *domain, \ - ps##TYPE *range, \ - unsigned int n, \ - ps##TYPE x) \ -{ \ - \ - unsigned int i; \ - unsigned int m; \ - static psVector *p = NULL; \ - p = psVectorRecycle(p, n, PS_TYPE_##TYPE); \ - p_psMemSetPersistent(p, true); \ - p_psMemSetPersistent(p->data.TYPE, true); \ - \ - psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 4, \ - "---- fullInterpolate1D##TYPE() begin (%u-order at x=%f) (%d data points)----\n", n-1, x, n); \ - \ - for (i=0;idata.TYPE[i] = range[i]; \ - psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 6, \ - "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \ - \ - } \ - \ - /* From NR, during each iteration of the m loop, we are computing the \ - p_{i ... i+m} terms. \ - */ \ - for (m=1;mdata.TYPE[i] = (((x-domain[i+m]) * p->data.TYPE[i]) + \ - ((domain[i]-x) * p->data.TYPE[i+1])) / \ - (domain[i] - domain[i+m]); \ - /*printf("((%f-%f * %f) + (%f-%f * %f)) / (%f - %f)\n", x, domain[i+m], p->data.TYPE[i], domain[i], x, p->data.TYPE[i+1], domain[i], domain[i+m]); \ - */ \ - psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 6, \ - "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \ - } \ - } \ - psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 4, \ - "---- fullInterpolate1D##TYPE() end ----\n"); \ - \ - return(p->data.TYPE[0]); \ -} \ - -/* -FUNC_MACRO_FULL_INTERPOLATE_1D(U8) -FUNC_MACRO_FULL_INTERPOLATE_1D(U16) -FUNC_MACRO_FULL_INTERPOLATE_1D(U32) -FUNC_MACRO_FULL_INTERPOLATE_1D(U64) -FUNC_MACRO_FULL_INTERPOLATE_1D(S8) -FUNC_MACRO_FULL_INTERPOLATE_1D(S16) -FUNC_MACRO_FULL_INTERPOLATE_1D(S32) -FUNC_MACRO_FULL_INTERPOLATE_1D(S64) -FUNC_MACRO_FULL_INTERPOLATE_1D(F64) -*/ -FUNC_MACRO_FULL_INTERPOLATE_1D(F32) - - -/***************************************************************************** -interpolate1DF32(): this is the base 1-D flat memory routine to perform -LaGrange interpolation. - *****************************************************************************/ -static psF32 interpolate1DF32(psF32 *domain, - psF32 *range, - unsigned int n, - unsigned int order, - psF32 x) -{ - PS_ASSERT_PTR_NON_NULL(domain, NAN) - PS_ASSERT_PTR_NON_NULL(range, NAN) - // XXX: Check valid values for n, order, and x? - - psS32 binNum; - unsigned int numIntPoints = order+1; - psS32 origin; - - psTrace(".psLib.dataManip.psSpline.interpolate1DF32", 4, - "---- interpolate1DF32() begin ----\n"); - - binNum = vectorBinDisectF32(domain, n, x); - - if (0 == numIntPoints%2) { - origin = binNum - ((numIntPoints/2) - 1); - } else { - origin = binNum - (numIntPoints/2); - if ((x-domain[binNum]) > (domain[binNum+1]-x)) { - // x is closer to binNum+1. - origin = 1 + (binNum - (numIntPoints/2)); - } - } - if (origin < 0) { - origin = 0; - } - if ((origin + numIntPoints) > n) { - origin = n - numIntPoints; - } - - psTrace(".psLib.dataManip.psSpline.interpolate1DF32", 4, - "---- interpolate1DF32() end ----\n"); - return(fullInterpolate1DF32(&domain[origin], &range[origin], order+1, x)); } @@ -222,14 +89,12 @@ XXX: do an F64 version? *****************************************************************************/ -static psF32 *calculateSecondDerivs(const psVector* x, ///< Ordinates (or NULL to just use the in! +static psF32 *calculateSecondDerivs(const psVector* x, ///< Ordinates const psVector* y) ///< Coordinates { - psTrace(".psLib.dataManip.calculateSecondDerivs", 4, - "---- calculateSecondDerivs() begin ----\n"); - - psS32 i; - psS32 k; - psF32 sig; - psF32 p; + psTrace(__func__, 4, "---- %s() begin ----\n", __func__); + if (psTraceGetLevel(__func__) >= 6) { + PS_VECTOR_PRINT_F32(x); + PS_VECTOR_PRINT_F32(y); + } psS32 n = y->n; psF32 *u = (psF32 *) psAlloc(n * sizeof(psF32)); @@ -237,600 +102,39 @@ psF32 *X = (psF32 *) & (x->data.F32[0]); psF32 *Y = (psF32 *) & (y->data.F32[0]); - psF32 qn; - + // // XXX: The second derivatives at the endpoints, undefined in the SDR, // are set in psConstants.h: PS_LEFT_SPLINE_DERIV, PS_RIGHT_SPLINE_DERIV. + // derivs2[0] = -0.5; u[0]= (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - PS_LEFT_SPLINE_DERIV); - for (i=1;i<=(n-2);i++) { - sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]); - p = sig * derivs2[i-1] + 2.0; + for (psS32 i=1;i<=(n-2);i++) { + psF32 sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]); + psF32 p = sig * derivs2[i-1] + 2.0; derivs2[i] = (sig - 1.0) / p; u[i] = ((Y[i+1] - Y[i])/(X[i+1]-X[i])) - ((Y[i]-Y[i-1])/(X[i]-X[i-1])); u[i] = ((6.0 * u[i] / (X[i+1] - X[i-1])) - (sig * u[i-1])) / p; - psTrace(".psLib.dataManip.calculateSecondDerivs", 6, - "X[%d] is %f\n", i, X[i]); - psTrace(".psLib.dataManip.calculateSecondDerivs", 6, - "Y[%d] is %f\n", i, Y[i]); - psTrace(".psLib.dataManip.calculateSecondDerivs", 6, - "u[%d] is %f\n", i, u[i]); - } - - qn = 0.5; + psTrace(__func__, 6, "X[%d] is %f\n", i, X[i]); + psTrace(__func__, 6, "Y[%d] is %f\n", i, Y[i]); + psTrace(__func__, 6, "u[%d] is %f\n", i, u[i]); + } + + psF32 qn = 0.5; u[n-1] = (3.0/(X[n-1]-X[n-2])) * (PS_RIGHT_SPLINE_DERIV - (Y[n-1]-Y[n-2])/(X[n-1]-X[n-2])); derivs2[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * derivs2[n-2]) + 1.0); - for (k=(n-2);k>=0;k--) { + for (psS32 k=(n-2);k>=0;k--) { derivs2[k] = derivs2[k] * derivs2[k+1] + u[k]; - - psTrace(".psLib.dataManip.calculateSecondDerivs", 6, - "derivs2[%d] is %f\n", k, derivs2[k]); + psTrace(__func__, 6, "derivs2[%d] is %f\n", k, derivs2[k]); } psFree(u); - psTrace(".psLib.dataManip.calculateSecondDerivs", 4, - "---- calculateSecondDerivs() end ----\n"); - + psTrace(__func__, 4, "---- %s() end ----\n", __func__); return(derivs2); } - -/*****************************************************************************/ -/* FUNCTION IMPLEMENTATION - PUBLIC */ -/*****************************************************************************/ - -/***************************************************************************** -psVectorFitSpline1D(): given a psSpline1D data structure and a set of x/y -vectors, this routine generates the linear or cublic splines which satisfy -those data points. - -The formula for calculating the spline polynomials is derived from Numerical -Recipes in C. The basic idea is that the polynomial is - (1) y = (A * y[0]) + - (2) (B * y[1]) + - (3) ((((A*A*A)-A) * mySpline->p_psDeriv2[0]) * H^2)/6.0 + - (4) ((((B*B*B)-B) * mySpline->p_psDeriv2[1]) * H^2)/6.0 -Where: - H = x[1]-x[0] - A = (x[1]-x)/H - B = (x-x[0])/H -The bulk of the code in this routine is the expansion of the above equation -into a polynomial in terms of x, and then saving the coefficients of the -powers of x in the spline polynomials. This gets pretty complicated. - -XXX: usage of yErr is not specified in IfA documentation. - -XXX: Is the x argument redundant? What do we do if the x argument is -supplied, but does not equal the knots specified in mySpline? - -XXX: can psSpline be NULL? - -XXX: reimplement this assuming that mySpline is NULL? - -XXX: What happens if X is NULL, then an index vector is generated for X, but -that index vector lies outside the range vectors in mySpline? - -XXX: Assumes mySpline->knots is psF32. Must add psU32 and psF64. - *****************************************************************************/ -psSpline1D *psVectorFitSpline1D(psSpline1D *spline, ///< The spline which will be generated. - const psVector* x, ///< Ordinates (or NULL to just use the indices) - const psVector* y, ///< Coordinates - const psVector* yErr) ///< Errors in coordinates, or NULL -{ - PS_ASSERT_VECTOR_NON_NULL(y, NULL); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL); - if (spline != NULL) { - PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL); - } - psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, - "---- psVectorFitSpline1D() begin ----\n"); - unsigned int numSplines = (y->n)-1; - psF32 tmp; - psF32 H; - unsigned int i; - psF32 slope; - psVector *x32 = NULL; - psVector *y32 = NULL; - psVector *yErr32 = NULL; - static psVector *x32Static = NULL; - static psVector *y32Static = NULL; - static psVector *yErr32Static = NULL; - - PS_VECTOR_CONVERT_F64_TO_F32_STATIC(y, y32, y32Static); - - // If yErr==NULL, set all errors equal. - if (yErr == NULL) { - PS_VECTOR_GEN_YERR_STATIC_F32(yErr32Static, y->n); - yErr32 = yErr32Static; - } else { - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(yErr, NULL); - PS_VECTOR_CONVERT_F64_TO_F32_STATIC(yErr, yErr32, yErr32Static); - } - - // If x==NULL, create an x32 vector with x values set to (0:n). - if (x == NULL) { - PS_VECTOR_GEN_X_INDEX_STATIC_F32(x32Static, y->n); - x32 = x32Static; - } else { - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); - PS_VECTOR_CONVERT_F64_TO_F32_STATIC(x, x32, x32Static); - } - PS_ASSERT_VECTORS_SIZE_EQUAL(x32, y32, NULL); - PS_ASSERT_VECTORS_SIZE_EQUAL(yErr32, y32, NULL); - - /* - XXX: - This can not be implemented until SDR states what order spline should be - created. - Should we error if spline is not NULL? - Should we error if mySPline is not NULL? - */ - if (spline == NULL) { - spline = psSpline1DAllocGeneric(x32, 3); - } - PS_ASSERT_PTR_NON_NULL(spline, NULL); - PS_ASSERT_INT_NONNEGATIVE(spline->n, NULL); - - if (y32->n != (1 + spline->n)) { - psError(PS_ERR_BAD_PARAMETER_SIZE, true, - "data size / spline size mismatch (%u %u)\n", - y32->n, spline->n); - return(NULL); - } - - // If these are linear splines, which means their polynomials will have - // two coefficients, then we do the simple calculation. - if (1 == (spline->spline[0])->COOL_1D_n) { - for (i=0;in;i++) { - slope = (y32->data.F32[i+1] - y32->data.F32[i]) / - (spline->knots->data.F32[i+1] - spline->knots->data.F32[i]); - (spline->spline[i])->coeff[0] = y32->data.F32[i] - - (slope * spline->knots->data.F32[i]); - - (spline->spline[i])->coeff[1] = slope; - psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4, - "---- spline %u coeffs are (%f, %f)\n", i, - (spline->spline[i])->coeff[0], - (spline->spline[i])->coeff[1]); - } - psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4, - "---- Exiting psVectorFitSpline1D()()\n"); - return((psSpline1D *) spline); - } - - // Check if these are cubic splines (n==4). If not, psError. - if (3 != (spline->spline[0])->COOL_1D_n) { - psError(PS_ERR_BAD_PARAMETER_SIZE, true, - "Don't know how to generate %u-order splines.", - (spline->spline[0])->COOL_1D_n); - return(NULL); - } - - // If we get here, then we know these are cubic splines. We first - // generate the second derivatives at each data point. - spline->p_psDeriv2 = calculateSecondDerivs(x32, y32); - for (i=0;in;i++) - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "Second deriv[%u] is %f\n", i, spline->p_psDeriv2[i]); - - // We generate the coefficients of the spline polynomials. I can't - // concisely explain how this code works. See above function comments - // and Numerical Recipes in C. - for (i=0;idata.F32[i+1] - x32->data.F32[i]; - psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, - "x data (%f - %f) (%f)\n", - x32->data.F32[i], - x32->data.F32[i+1], H); - // - // ******** Calculate 0-order term ******** - // - // From (1) - (spline->spline[i])->coeff[0] = (y32->data.F32[i] * x32->data.F32[i+1]/H); - // From (2) - ((spline->spline[i])->coeff[0])-= ((y32->data.F32[i+1] * x32->data.F32[i])/H); - // From (3) - tmp = (x32->data.F32[i+1] * x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H); - tmp-= (x32->data.F32[i+1] / H); - tmp*= (spline->p_psDeriv2)[i] * H * H / 6.0; - ((spline->spline[i])->coeff[0])+= tmp; - // From (4) - tmp = -(x32->data.F32[i] * x32->data.F32[i] * x32->data.F32[i]) / (H * H * H); - tmp+= (x32->data.F32[i] / H); - tmp*= (spline->p_psDeriv2)[i+1] * H * H / 6.0; - ((spline->spline[i])->coeff[0])+= tmp; - - // - // ******** Calculate 1-order term ******** - // - // From (1) - (spline->spline[i])->coeff[1] = -(y32->data.F32[i]) / H; - // From (2) - ((spline->spline[i])->coeff[1])+= (y32->data.F32[i+1] / H); - // From (3) - tmp = -3.0 * (x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H); - tmp+= (1.0 / H); - tmp*= ((spline->p_psDeriv2)[i]) * H * H / 6.0; - ((spline->spline[i])->coeff[1])+= tmp; - // From (4) - tmp = 3.0 * (x32->data.F32[i] * x32->data.F32[i]) / (H * H * H); - tmp-= (1.0 / H); - tmp*= ((spline->p_psDeriv2)[i+1]) * H * H / 6.0; - ((spline->spline[i])->coeff[1])+= tmp; - - // - // ******** Calculate 2-order term ******** - // - // From (3) - (spline->spline[i])->coeff[2] = ((spline->p_psDeriv2)[i]) * 3.0 * x32->data.F32[i+1] / (6.0 * H); - // From (4) - ((spline->spline[i])->coeff[2])-= (((spline->p_psDeriv2)[i+1]) * 3.0 * x32->data.F32[i] / (6.0 * H)); - - // - // ******** Calculate 3-order term ******** - // - // From (3) - (spline->spline[i])->coeff[3] = -((spline->p_psDeriv2)[i]) / (6.0 * H); - // From (4) - ((spline->spline[i])->coeff[3])+= ((spline->p_psDeriv2)[i+1]) / (6.0 * H); - - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "(spline->spline[%u])->coeff[0] is %f\n", i, (spline->spline[i])->coeff[0]); - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "(spline->spline[%u])->coeff[1] is %f\n", i, (spline->spline[i])->coeff[1]); - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "(spline->spline[%u])->coeff[2] is %f\n", i, (spline->spline[i])->coeff[2]); - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "(spline->spline[%u])->coeff[3] is %f\n", i, (spline->spline[i])->coeff[3]); - - } - - psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, - "---- psVectorFitSpline1D() end ----\n"); - return(spline); -} - - - - - - - - - -/***************************************************************************** -psVectorFitSpline1DNEW(): given a psSpline1D data structure and a set of x/y - -xF32 and yF32 are internal psVectors which are used to hold the psF32 versions -of the input data, if necessary. xPtr and yPtr are pointers to either xF32 or -the x argument. All computation is done on xPtr and yPtr. xF32 and yF32 will -simply be psFree() at the end. - -XXX: nKnots makes no sense. This number is always equal to the size of the x -an y vectors. - -XXX: How do we specify the spline order? For now, order=3. - *****************************************************************************/ -#define PS_XXX_SPLINE_ORDER 3 -psSpline1D *psVectorFitSpline1DNEW(const psVector* x, ///< Ordinates (or NULL to just use the indices) - const psVector* y, ///< Coordinates - unsigned int nKnots) -{ - psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, "---- psVectorFitSpline1DNEW() begin ----\n"); - PS_ASSERT_VECTOR_NON_NULL(y, NULL); - PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL); - // PS_ASSERT_INT_EQUAL(y->n, nKnots); - - // - // The following code ensures that xPtr points to a psF32 version of the - // ordinate data. - // - psVector *xF32 = NULL; - psVector *xPtr = NULL; - if (x != NULL) { - PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL); - if (PS_TYPE_F64 == x->type.type) { - xF32 = psVectorAlloc(y->n, PS_TYPE_F32); - for (unsigned int i = 0 ; i < x->n ; i++) { - xF32->data.F32[i] = (psF32) x->data.F64[i]; - } - xPtr = xF32; - } else if (PS_TYPE_F32 == x->type.type) { - xPtr = (psVector *) x; - } else { - psError(PS_ERR_UNKNOWN, true, "psVector x is wrong type.\n"); - return(NULL); - } - } else { - // If x==NULL, create an x32 vector with x values set to (0:n). - xF32 = psVectorAlloc(y->n, PS_TYPE_F32); - for (unsigned int i = 0 ; i < x->n ; i++) { - xF32->data.F32[i] = (psF32) i; - } - xPtr = xF32; - } - - // - // If y is of type psF64, then create a new vector yF32 and convert the - // y elements. Regardless of y's type, we create a yPtr which will be - // used in the remainder of this function. - // - psVector *yF32 = NULL; - psVector *yPtr = NULL; - if (PS_TYPE_F64 == y->type.type) { - yF32 = psVectorAlloc(y->n, PS_TYPE_F32); - for (unsigned int i = 0 ; i < y->n ; i++) { - yF32->data.F32[i] = (psF32) y->data.F64[i]; - } - yPtr = yF32; - } else { - yPtr = (psVector *) y; - } - - psSpline1D *mySpline = psSpline1DAllocGeneric(xPtr, PS_XXX_SPLINE_ORDER); - - unsigned int numSplines = nKnots - 1; - psF32 tmp; - psF32 H; - unsigned int i; - psF32 slope; - // XXX: get rid of x32 and y32 (this is from old code) - psVector *x32 = xPtr; - psVector *y32 = yPtr; - - // If these are linear splines, which means their polynomials will have - // two coefficients, then we do the simple calculation. - if (1 == PS_XXX_SPLINE_ORDER) { - for (i=0;in;i++) { - slope = (y32->data.F32[i+1] - y32->data.F32[i]) / - (mySpline->knots->data.F32[i+1] - mySpline->knots->data.F32[i]); - (mySpline->spline[i])->coeff[0] = y32->data.F32[i] - - (slope * mySpline->knots->data.F32[i]); - - (mySpline->spline[i])->coeff[1] = slope; - psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4, - "---- mySpline %u coeffs are (%f, %f)\n", i, - (mySpline->spline[i])->coeff[0], - (mySpline->spline[i])->coeff[1]); - } - psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4, - "---- Exiting psVectorFitSpline1D()()\n"); - return((psSpline1D *) mySpline); - } - - // - // Check if these are cubic splines (n==4). If not, psError. - // - if (3 != PS_XXX_SPLINE_ORDER) { - psError(PS_ERR_BAD_PARAMETER_SIZE, true, - "Don't know how to generate %d-order splines.", PS_XXX_SPLINE_ORDER); - return(NULL); - } - - // - // If we get here, then we know these are cubic splines. We first - // generate the second derivatives at each data point. - // - mySpline->p_psDeriv2 = calculateSecondDerivs(x32, y32); - for (i=0;in;i++) - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "Second deriv[%u] is %f\n", i, mySpline->p_psDeriv2[i]); - - // - // We generate the coefficients of the spline polynomials. I can't - // concisely explain how this code works. See above function comments - // and Numerical Recipes in C. - // - for (i=0;idata.F32[i+1] - x32->data.F32[i]; - psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, - "x data (%f - %f) (%f)\n", - x32->data.F32[i], - x32->data.F32[i+1], H); - // - // ******** Calculate 0-order term ******** - // - // From (1) - (mySpline->spline[i])->coeff[0] = (y32->data.F32[i] * x32->data.F32[i+1]/H); - // From (2) - ((mySpline->spline[i])->coeff[0])-= ((y32->data.F32[i+1] * x32->data.F32[i])/H); - // From (3) - tmp = (x32->data.F32[i+1] * x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H); - tmp-= (x32->data.F32[i+1] / H); - tmp*= (mySpline->p_psDeriv2)[i] * H * H / 6.0; - ((mySpline->spline[i])->coeff[0])+= tmp; - // From (4) - tmp = -(x32->data.F32[i] * x32->data.F32[i] * x32->data.F32[i]) / (H * H * H); - tmp+= (x32->data.F32[i] / H); - tmp*= (mySpline->p_psDeriv2)[i+1] * H * H / 6.0; - ((mySpline->spline[i])->coeff[0])+= tmp; - - // - // ******** Calculate 1-order term ******** - // - // From (1) - (mySpline->spline[i])->coeff[1] = -(y32->data.F32[i]) / H; - // From (2) - ((mySpline->spline[i])->coeff[1])+= (y32->data.F32[i+1] / H); - // From (3) - tmp = -3.0 * (x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H); - tmp+= (1.0 / H); - tmp*= ((mySpline->p_psDeriv2)[i]) * H * H / 6.0; - ((mySpline->spline[i])->coeff[1])+= tmp; - // From (4) - tmp = 3.0 * (x32->data.F32[i] * x32->data.F32[i]) / (H * H * H); - tmp-= (1.0 / H); - tmp*= ((mySpline->p_psDeriv2)[i+1]) * H * H / 6.0; - ((mySpline->spline[i])->coeff[1])+= tmp; - - // - // ******** Calculate 2-order term ******** - // - // From (3) - (mySpline->spline[i])->coeff[2] = ((mySpline->p_psDeriv2)[i]) * 3.0 * x32->data.F32[i+1] / (6.0 * H); - // From (4) - ((mySpline->spline[i])->coeff[2])-= (((mySpline->p_psDeriv2)[i+1]) * 3.0 * x32->data.F32[i] / (6.0 * H)); - - // - // ******** Calculate 3-order term ******** - // - // From (3) - (mySpline->spline[i])->coeff[3] = -((mySpline->p_psDeriv2)[i]) / (6.0 * H); - // From (4) - ((mySpline->spline[i])->coeff[3])+= ((mySpline->p_psDeriv2)[i+1]) / (6.0 * H); - - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "(mySpline->spline[%u])->coeff[0] is %f\n", i, (mySpline->spline[i])->coeff[0]); - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "(mySpline->spline[%u])->coeff[1] is %f\n", i, (mySpline->spline[i])->coeff[1]); - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "(mySpline->spline[%u])->coeff[2] is %f\n", i, (mySpline->spline[i])->coeff[2]); - psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, - "(mySpline->spline[%u])->coeff[3] is %f\n", i, (mySpline->spline[i])->coeff[3]); - - } - - psFree(xF32); - psFree(yF32); - - psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, - "---- psVectorFitSpline1D() end ----\n"); - - return(mySpline); -} - - - - - -/*****************************************************************************/ -/* FUNCTION IMPLEMENTATION - PUBLIC */ -/*****************************************************************************/ - -//typedef struct { -// psS32 n; -// psPolynomial1D **spline; -// psF32 *p_psDeriv2; -// psVector *knots; -//} psSpline1D; - -/***************************************************************************** - NOTE: "n" specifies the number of spline polynomials. Therefore, there - must exist n+1 points in "knots". - -XXX: Is this really needed anymore? - -XXX: Ensure that knots[i+1] != knots[i] - -XXX: What should be the default type for knots be? psF32 is assumed. - *****************************************************************************/ -psSpline1D *psSpline1DAlloc(unsigned int numSplines, - unsigned int order, - float min, - float max) -{ - PS_ASSERT_INT_NONNEGATIVE(numSplines, NULL); - PS_ASSERT_INT_NONNEGATIVE(order, NULL); - PS_ASSERT_FLOAT_NON_EQUAL(max, min, NULL); - - psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D)); - tmpSpline->n = numSplines; - - // - // XXX: We might have to allocate single or double polynomials depending on the type - // of the psVector bounds. For now, all knots and spline polynomials are 32-bit. - // - tmpSpline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *)); - for (unsigned int i=0;ispline)[i] = psPolynomial1DAlloc(order, PS_POLYNOMIAL_ORD); - } - - // This will be computed by psVectorFitSpline1D() - tmpSpline->p_psDeriv2 = NULL; - - // - // XXX:Ensure that the knots are distinct, and monotonic. - // - tmpSpline->knots = psVectorAlloc(numSplines+1, PS_TYPE_F32); - psF32 width = (max - min) / ((psF32) numSplines); - tmpSpline->knots->data.F32[0] = min; - for (unsigned int i=1;iknots->data.F32[i] = min + (width * (psF32) i); - } - tmpSpline->knots->data.F32[numSplines] = max; - - psMemSetDeallocator(tmpSpline, (psFreeFunc)spline1DFree); - return(tmpSpline); -} - -/***************************************************************************** -XXX: Is there a psLib function for this? - *****************************************************************************/ -psVector *PsVectorDup2(psVector *in) -{ - psVector *out = psVectorAlloc(in->n, in->type.type); - - if (in->type.type == PS_TYPE_F32) { - for (unsigned int i = 0 ; i < in->n ; i++) { - out->data.F32[i] = in->data.F32[i]; - } - } else if (in->type.type == PS_TYPE_F64) { - for (unsigned int i = 0 ; i < in->n ; i++) { - out->data.F64[i] = in->data.F64[i]; - } - } else { - psError(PS_ERR_UNKNOWN, true, "psVector in has an incorrect type.\n"); - return(NULL); - } - return(out); -} - -/***************************************************************************** -XXX: What should be the default type for knots, spline polys? psF32 is assumed. - *****************************************************************************/ -psSpline1D *psSpline1DAllocGeneric(const psVector *bounds, - unsigned int order) -{ - PS_ASSERT_VECTOR_NON_NULL(bounds, NULL); - PS_ASSERT_VECTOR_NON_EMPTY(bounds, NULL); - PS_ASSERT_VECTOR_TYPE(bounds, PS_TYPE_F32, NULL); - PS_ASSERT_INT_NONNEGATIVE(order, NULL); - - psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D)); - unsigned int numSplines = bounds->n - 1; - tmpSpline->n = numSplines; - - // - // XXX: We might have to allocate single or double polynomials depending on the type - // of the psVector bounds. For now, all knots and spline polynomials are 32-bit. - // - tmpSpline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *)); - for (unsigned int i=0;ispline)[i] = psPolynomial1DAlloc(order, PS_POLYNOMIAL_ORD); - } - - // This will be computed by psVectorFitSpline1D() - tmpSpline->p_psDeriv2 = NULL; - - // - // Ensure that all knots are distinct. - // XXX:Ensure that the knots are monotonic. - // - for (unsigned int i=0;in-1;i++) { - if (FLT_EPSILON >= fabs(bounds->data.F32[i+1]-bounds->data.F32[i])) { - psError(PS_ERR_UNKNOWN, true, "data points must be distinct ([%d] %f %f)\n", i, bounds->data.F32[i], bounds->data.F32[i+1]); - return(NULL); - } - } - tmpSpline->knots = PsVectorDup2((psVector *) bounds); - - psMemSetDeallocator(tmpSpline, (psFreeFunc)spline1DFree); - return(tmpSpline); -} - -/***************************************************************************** -vectorBinDisectF32(): This is a macro for a private function which takes as +/***************************************************************************** +vectorBinDisectTYPE(): This is a macro for a private function which takes as input a vector an array of data as well as a single value for that data. The input vector values are assumed to be non-decreasing (v[i-1] <= v[i] for all @@ -840,15 +144,12 @@ *****************************************************************************/ #define FUNC_MACRO_VECTOR_BIN_DISECT(TYPE) \ -static unsigned int vectorBinDisect##TYPE(ps##TYPE *bins, \ - unsigned int numBins, \ - ps##TYPE x) \ +static psS32 vectorBinDisect##TYPE(ps##TYPE *bins, \ + psS32 numBins, \ + ps##TYPE x) \ { \ - unsigned int min; \ - unsigned int max; \ - unsigned int mid; \ - \ - psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \ - "---- Calling vectorBinDisect##TYPE(%f)\n", x); \ - \ + psS32 min; \ + psS32 max; \ + psS32 mid; \ + psTrace(__func__, 4, "---- %s() begin ----\n", __func__); \ if (x < bins[0]) { \ psLogMsg(__func__, PS_LOG_WARN, \ @@ -857,5 +158,4 @@ return(-2); \ } \ - \ if (x > bins[numBins-1]) { \ psLogMsg(__func__, PS_LOG_WARN, \ @@ -868,13 +168,9 @@ max = numBins-2; \ mid = ((max+1)-min)/2; \ - \ while (min != max) { \ - psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \ - "(min, mid, max) is (%u, %u, %u): (x, bins) is (%f, %f)\n", \ - min, mid, max, x, bins[mid]); \ + psTrace(__func__, 6, "(min, mid, max) is (%u, %u, %u): (x, bins) is (%f, %f)\n", min, mid, max, x, bins[mid]); \ \ if (x == bins[mid]) { \ - psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \ - "---- Exiting vectorBinDisect##TYPE(): bin %u\n", mid); \ + psTrace(__func__, 4, "---- %s(%d) end ----\n", __func__, mid); \ return(mid); \ } else if (x < bins[mid]) { \ @@ -885,7 +181,5 @@ mid = ((max+1)+min)/2; \ } \ - \ - psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \ - "---- Exiting vectorBinDisect##TYPE(): bin %u\n", min); \ + psTrace(__func__, 4, "---- %s(%d) end ----\n", __func__, min); \ return(min); \ } \ @@ -904,9 +198,8 @@ /***************************************************************************** p_psVectorBinDisect(): A wrapper to the above p_psVectorBinDisect(). - -XXX: Assert that the psVector and psScalar have the same type. - *****************************************************************************/ -unsigned int p_psVectorBinDisect(psVector *bins, - psScalar *x) + *****************************************************************************/ +psS32 p_psVectorBinDisect( + psVector *bins, + psScalar *x) { PS_ASSERT_VECTOR_NON_NULL(bins, -4); @@ -960,4 +253,117 @@ /***************************************************************************** +fullInterpolate1DF32(): This routine will take as input n-element floating +point arrays domain and range, and the x value, assumed to lie with the +domain vector. It produces as output the (n-1)-order LaGrange interpolated +value of x. + +XXX: do we error check for non-distinct domain values? + +XXX: This is a somewhat general function. There's no reason to put it in this +file. + +XXX: Sort all these interpolation functions out. Why are there so many? + +XXX: The fullInterpolate1D macros are not used anywhere. + *****************************************************************************/ +#define FUNC_MACRO_FULL_INTERPOLATE_1D(TYPE) \ +static psF32 fullInterpolate1D##TYPE(ps##TYPE *domain, \ + ps##TYPE *range, \ + unsigned int n, \ + ps##TYPE x) \ +{ \ + \ + unsigned int i; \ + unsigned int m; \ + static psVector *p = NULL; \ + p = psVectorRecycle(p, n, PS_TYPE_##TYPE); \ + p_psMemSetPersistent(p, true); \ + p_psMemSetPersistent(p->data.TYPE, true); \ + \ + psTrace(__func__, 4, "---- %s() begin %u-order at x=%f) (%d data points) ----\n", __func__, n-1, x, n); \ + for (i=0;idata.TYPE[i] = range[i]; \ + psTrace(__func__, 6, "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \ + \ + } \ + \ + /* From NR, during each iteration of the m loop, we are computing the \ + p_{i ... i+m} terms. \ + */ \ + for (m=1;mdata.TYPE[i] = (((x-domain[i+m]) * p->data.TYPE[i]) + \ + ((domain[i]-x) * p->data.TYPE[i+1])) / \ + (domain[i] - domain[i+m]); \ + psTrace(__func__, 6, "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \ + } \ + } \ + psTrace(__func__, 4, "---- %s(....) end ----\n", __func__); \ + return(p->data.TYPE[0]); \ +} \ + +/* XXX: Do this correctly. +FUNC_MACRO_FULL_INTERPOLATE_1D(U8) +FUNC_MACRO_FULL_INTERPOLATE_1D(U16) +FUNC_MACRO_FULL_INTERPOLATE_1D(U32) +FUNC_MACRO_FULL_INTERPOLATE_1D(U64) +FUNC_MACRO_FULL_INTERPOLATE_1D(S8) +FUNC_MACRO_FULL_INTERPOLATE_1D(S16) +FUNC_MACRO_FULL_INTERPOLATE_1D(S32) +FUNC_MACRO_FULL_INTERPOLATE_1D(S64) +FUNC_MACRO_FULL_INTERPOLATE_1D(F64) +*/ +FUNC_MACRO_FULL_INTERPOLATE_1D(F32) + + +/***************************************************************************** +interpolate1DF32(): this is the base 1-D flat memory routine to perform +LaGrange interpolation. + *****************************************************************************/ +static psF32 interpolate1DF32(psF32 *domain, + psF32 *range, + unsigned int n, + unsigned int order, + psF32 x) +{ + psTrace(__func__, 4, "---- %s() begin ----\n", __func__); + PS_ASSERT_PTR_NON_NULL(domain, NAN) + PS_ASSERT_PTR_NON_NULL(range, NAN) + // XXX: Check valid values for n, order, and x? + + psS32 binNum; + unsigned int numIntPoints = order+1; + psS32 origin; + + binNum = vectorBinDisectF32(domain, n, x); + + if (0 == numIntPoints%2) { + origin = binNum - ((numIntPoints/2) - 1); + } else { + origin = binNum - (numIntPoints/2); + if ((x-domain[binNum]) > (domain[binNum+1]-x)) { + // x is closer to binNum+1. + origin = 1 + (binNum - (numIntPoints/2)); + } + } + if (origin < 0) { + origin = 0; + } + if ((origin + numIntPoints) > n) { + origin = n - numIntPoints; + } + + psTrace(__func__, 4, "---- %s(....) end ----\n", __func__); + return(fullInterpolate1DF32(&domain[origin], &range[origin], order+1, x)); +} + + +/***************************************************************************** p_psVectorInterpolate(): This routine will take as input psVectors domain and range, and the x value, assumed to lie with the domain vector. It produces @@ -967,13 +373,16 @@ XXX: This stuff does not currently work with a mask. -XXX: add another psScalar argument for the result. +XXX: add another psScalar argument for the result (so you don't have to + allocate it each time). XXX: The VectorCopy routines seg fault when I declare range32 as static. *****************************************************************************/ -psScalar *p_psVectorInterpolate(psVector *domain, - psVector *range, - unsigned int order, - psScalar *x) -{ +psScalar *p_psVectorInterpolate( + psVector *domain, + psVector *range, + psS32 order, + psScalar *x) +{ + psTrace(__func__, 4, "---- %s() begin ----\n", __func__); PS_ASSERT_VECTOR_NON_NULL(domain, NULL); PS_ASSERT_VECTOR_NON_NULL(range, NULL); @@ -986,7 +395,4 @@ psVector *range32 = NULL; psVector *domain32 = NULL; - psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4, - "---- p_psVectorInterpolate() begin ----\n"); - if (order > (domain->n - 1)) { psError(PS_ERR_BAD_PARAMETER_SIZE, true, @@ -997,6 +403,5 @@ if (x->type.type == PS_TYPE_F32) { - psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4, - "---- p_psVectorInterpolate() end ----\n"); + psTrace(__func__, 4, "---- %s(NULL) end ----\n", __func__); return(psScalarAlloc(interpolate1DF32(domain->data.F32, range->data.F32, @@ -1018,6 +423,5 @@ psFree(domain32); - psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4, - "---- p_psVectorInterpolate() end ----\n"); + psTrace(__func__, 4, "---- %s() end ----\n", __func__); // XXX: Convert data type to F64? return(tmpScalar); @@ -1031,10 +435,193 @@ } - psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4, - "return(NULL)\n"); - psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4, - "---- p_psVectorInterpolate() end ----\n"); - + psTrace(__func__, 4, "---- %s(NULL) end ----\n", __func__); return(NULL); +} + + +/*****************************************************************************/ +/* FUNCTION IMPLEMENTATION - PUBLIC */ +/*****************************************************************************/ + +bool psMemCheckSpline1D(psPtr ptr) +{ + return ( psMemGetDeallocator(ptr) == (psFreeFunc)spline1DFree ); +} + +psSpline1D *psSpline1DAlloc() +{ + psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D)); + tmpSpline->n = 0; + tmpSpline->spline = NULL; + tmpSpline->knots = NULL; + tmpSpline->p_psDeriv2 = NULL; + psMemSetDeallocator(tmpSpline, (psFreeFunc) spline1DFree); + + return(tmpSpline); +} + + +/***************************************************************************** +psVectorFitSpline1D(): given a set of x/y vectors, this routine generates the +linear or cublic splines which satisfy those data points. + +The formula for calculating the spline polynomials is derived from Numerical +Recipes in C. The basic idea is that the polynomial is + (1) y = (A * y[0]) + + (2) (B * y[1]) + + (3) ((((A*A*A)-A) * mySpline->p_psDeriv2[0]) * H^2)/6.0 + + (4) ((((B*B*B)-B) * mySpline->p_psDeriv2[1]) * H^2)/6.0 +Where: + H = x[1]-x[0] + A = (x[1]-x)/H + B = (x-x[0])/H +The bulk of the code in this routine is the expansion of the above equation +into a polynomial in terms of x, and then saving the coefficients of the +powers of x in the spline polynomials. This gets pretty complicated. + +XXX: What types must be supported? + +XXX: Should we allow x==NULL? + *****************************************************************************/ +psSpline1D *psVectorFitSpline1D( + const psVector* x, ///< Ordinates. + const psVector* y) ///< Coordinates. +{ + psTrace(__func__, 3, "---- %s() begin ----\n", __func__); + PS_ASSERT_VECTOR_NON_NULL(y, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL); + PS_ASSERT_VECTOR_NON_NULL(x, NULL); + PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); + PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL); + psS32 numSplines = (y->n)-1; + + psTrace(__func__, 5, "numSplines is %d\n", numSplines); + // + // The following code ensures that xPtr and yPtr points to a psF32 psVector. + // + psVector *xF32 = NULL; + psVector *xPtr = NULL; + if (PS_TYPE_F64 == x->type.type) { + xF32 = psVectorAlloc(y->n, PS_TYPE_F32); + for (psS32 i = 0 ; i < x->n ; i++) { + xF32->data.F32[i] = (psF32) x->data.F64[i]; + } + xPtr = xF32; + } else if (PS_TYPE_F32 == x->type.type) { + xPtr = (psVector *) x; + } + + psVector *yF32 = NULL; + psVector *yPtr = NULL; + if (PS_TYPE_F64 == y->type.type) { + yF32 = psVectorAlloc(y->n, PS_TYPE_F32); + for (psS32 i = 0 ; i < y->n ; i++) { + yF32->data.F32[i] = (psF32) y->data.F64[i]; + } + yPtr = yF32; + } else { + yPtr = (psVector *) y; + } + + // + // Create the psSpline1D struct. + // + psSpline1D *spline = psSpline1DAlloc(); + spline->n = numSplines; + spline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *)); + for (psS32 i=0;ispline)[i] = psPolynomial1DAlloc(3, PS_POLYNOMIAL_ORD); + } + // + // XXX: Ensure that the knots are distinct, and monotonic. + // XXX: Use a vector dup macro, or function here. + // + spline->knots = psVectorAlloc(x->n, PS_TYPE_F32); + for (psS32 i = 0 ; i < x->n ; i++) { + spline->knots->data.F32[i] = xPtr->data.F32[i]; + } + // + // Generate the second derivatives at each data point. + // + spline->p_psDeriv2 = calculateSecondDerivs(xPtr, yPtr); + + // + // We generate the coefficients of the spline polynomials. I can't + // concisely explain how this code works. See above function comments + // and Numerical Recipes in C. + // + for (psS32 i=0 ; i < numSplines ; i++) { + psF32 H = xPtr->data.F32[i+1] - xPtr->data.F32[i]; + if (fabs(H) <= FLT_EPSILON) { + printf("XXX: Generate error: x data points are not distinct (%d %d).\n", i, i+1); + } + psTrace(__func__, 6, "x data (%f - %f) (%f)\n", xPtr->data.F32[i], xPtr->data.F32[i+1], H); + + // + // ******** Calculate 0-order term ******** + // + // From (1) + (spline->spline[i])->coeff[0] = (yPtr->data.F32[i] * xPtr->data.F32[i+1]/H); + // From (2) + ((spline->spline[i])->coeff[0])-= ((yPtr->data.F32[i+1] * xPtr->data.F32[i])/H); + // From (3) + psF32 tmp = (xPtr->data.F32[i+1] * xPtr->data.F32[i+1] * xPtr->data.F32[i+1]) / (H * H * H); + tmp-= (xPtr->data.F32[i+1] / H); + tmp*= (spline->p_psDeriv2)[i] * H * H / 6.0; + ((spline->spline[i])->coeff[0])+= tmp; + // From (4) + tmp = -(xPtr->data.F32[i] * xPtr->data.F32[i] * xPtr->data.F32[i]) / (H * H * H); + tmp+= (xPtr->data.F32[i] / H); + tmp*= (spline->p_psDeriv2)[i+1] * H * H / 6.0; + ((spline->spline[i])->coeff[0])+= tmp; + + // + // ******** Calculate 1-order term ******** + // + // From (1) + (spline->spline[i])->coeff[1] = -(yPtr->data.F32[i]) / H; + // From (2) + ((spline->spline[i])->coeff[1])+= (yPtr->data.F32[i+1] / H); + // From (3) + tmp = -3.0 * (xPtr->data.F32[i+1] * xPtr->data.F32[i+1]) / (H * H * H); + tmp+= (1.0 / H); + tmp*= ((spline->p_psDeriv2)[i]) * H * H / 6.0; + ((spline->spline[i])->coeff[1])+= tmp; + // From (4) + tmp = 3.0 * (xPtr->data.F32[i] * xPtr->data.F32[i]) / (H * H * H); + tmp-= (1.0 / H); + tmp*= ((spline->p_psDeriv2)[i+1]) * H * H / 6.0; + ((spline->spline[i])->coeff[1])+= tmp; + + // + // ******** Calculate 2-order term ******** + // + // From (3) + (spline->spline[i])->coeff[2] = ((spline->p_psDeriv2)[i]) * 3.0 * xPtr->data.F32[i+1] / (6.0 * H); + // From (4) + ((spline->spline[i])->coeff[2])-= (((spline->p_psDeriv2)[i+1]) * 3.0 * xPtr->data.F32[i] / (6.0 * H)); + + // + // ******** Calculate 3-order term ******** + // + // From (3) + (spline->spline[i])->coeff[3] = -((spline->p_psDeriv2)[i]) / (6.0 * H); + // From (4) + ((spline->spline[i])->coeff[3])+= ((spline->p_psDeriv2)[i+1]) / (6.0 * H); + + psTrace(__func__, 6, "(spline->spline[%u])->coeff[0] is %f\n", i, (spline->spline[i])->coeff[0]); + psTrace(__func__, 6, "(spline->spline[%u])->coeff[1] is %f\n", i, (spline->spline[i])->coeff[1]); + psTrace(__func__, 6, "(spline->spline[%u])->coeff[2] is %f\n", i, (spline->spline[i])->coeff[2]); + psTrace(__func__, 6, "(spline->spline[%u])->coeff[3] is %f\n", i, (spline->spline[i])->coeff[3]); + } + + if (PS_TYPE_F64 == x->type.type) { + psFree(xF32); + } + if (PS_TYPE_F64 == y->type.type) { + psFree(yF32); + } + psTrace(__func__, 3, "---- %s() end ----\n", __func__); + return(spline); } @@ -1050,23 +637,23 @@ the spline fit functions require F32 and F64. -XXX: This only works if spline0>knots if psF32. Must add support for psU32 and -psF64. +XXX: This only works if spline0>knots if psF32. Must we add support for psU32 and +psF64? *****************************************************************************/ float psSpline1DEval( const psSpline1D *spline, - float x -) -{ + float x) +{ + psTrace(__func__, 3, "---- %s(%f) begin ----\n", __func__, x); PS_ASSERT_PTR_NON_NULL(spline, NAN); PS_ASSERT_INT_NONNEGATIVE(spline->n, NAN); PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NAN); - unsigned int binNum; - unsigned int n; - - n = spline->n; - //XXX binNum = vectorBinDisectF32(spline->domains, (spline->n)+1, x); - binNum = vectorBinDisectF32(spline->knots->data.F32, (spline->n)+1, x); + psS32 n = spline->n; + psS32 binNum = vectorBinDisectF32(spline->knots->data.F32, (spline->n)+1, x); if (binNum < 0) { + // + // If x is outside the range of spline->knots, generate a warning + // message, then return the left, or right, endpoint. + // psLogMsg(__func__, PS_LOG_WARN, "psSpline1DEval(): x ordinate (%f) is outside the spline range (%f - %f).", @@ -1075,55 +662,48 @@ if (x < spline->knots->data.F32[0]) { - return(psPolynomial1DEval(spline->spline[0], - x)); + psF32 rcF32 = psPolynomial1DEval(spline->spline[0], x); + psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32); + return(rcF32); } else if (x > spline->knots->data.F32[n-1]) { - return(psPolynomial1DEval(spline->spline[n-1], - x)); + psF32 rcF32 = psPolynomial1DEval(spline->spline[n-1], x); + psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32); + return(rcF32); } } - return(psPolynomial1DEval(spline->spline[binNum], - x)); -} - -// XXX: The spline eval functions require input and output to be F32. -// however the spline fit functions require F32 and F64. + psF32 rcF32 = psPolynomial1DEval(spline->spline[binNum], x); + psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32); + return(rcF32); +} + + +/***************************************************************************** + *****************************************************************************/ psVector *psSpline1DEvalVector( const psSpline1D *spline, - const psVector *x -) -{ + const psVector *x) +{ + psTrace(__func__, 3, "---- %s() begin ----\n", __func__); PS_ASSERT_PTR_NON_NULL(spline, NULL); + PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL); PS_ASSERT_VECTOR_NON_NULL(x, NULL); PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); - PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL); - - unsigned int i; - psVector *tmpVector; - - tmpVector = psVectorAlloc(x->n, PS_TYPE_F32); + if (psTraceGetLevel(__func__) >= 6) { + PS_VECTOR_PRINT_F32(x); + // XXX: print spline + } + + psVector *tmpVector = psVectorAlloc(x->n, PS_TYPE_F32); if (x->type.type == PS_TYPE_F32) { - for (i=0;in;i++) { - tmpVector->data.F32[i] = psSpline1DEval( - spline, - x->data.F32[i] - ); + for (psS32 i=0;in;i++) { + tmpVector->data.F32[i] = psSpline1DEval(spline, x->data.F32[i]); } } else if (x->type.type == PS_TYPE_F64) { - for (i=0;in;i++) { - tmpVector->data.F32[i] = psSpline1DEval( - spline, - (psF32) x->data.F64[i] - ); + for (psS32 i=0;in;i++) { + tmpVector->data.F32[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]); } - } else { - char* strType; - PS_TYPE_NAME(strType,x->type.type); - psError(PS_ERR_BAD_PARAMETER_TYPE, - PS_ERRORTEXT_psSpline_TYPE_NOT_SUPPORTED, - strType); - return(NULL); - } - + } + + psTrace(__func__, 3, "---- %s() end ----\n", __func__); return(tmpVector); }