Index: trunk/psLib/src/math/psSpline.c =================================================================== --- trunk/psLib/src/math/psSpline.c (revision 4971) +++ trunk/psLib/src/math/psSpline.c (revision 4991) @@ -7,6 +7,6 @@ * splines. * -* @version $Revision: 1.122 $ $Name: not supported by cvs2svn $ -* @date $Date: 2005-09-08 00:14:32 $ +* @version $Revision: 1.123 $ $Name: not supported by cvs2svn $ +* @date $Date: 2005-09-11 22:18:40 $ * * @@ -207,4 +207,502 @@ } + +/***************************************************************************** +CalculateSecondDerivs(): Given a set of x/y vectors corresponding to a +tabulated function at n points, this routine calculates the second +derivatives of the interpolating cubic splines at those n points. + +The first and second derivatives at the endpoints, undefined in the SDR, are +here defined to be 0.0. They can be modified via ypo and yp1. + +This routine assumes that vectors x and y are of the appropriate types/sizes +(F32). + +XXX: This algorithm is derived from the Numerical Recipes. +XXX: use recycled vectors for internal data. +XXX: do an F64 version? + *****************************************************************************/ +static psF32 *calculateSecondDerivs(const psVector* x, ///< Ordinates (or NULL to just use the indices) + const psVector* y) ///< Coordinates +{ + psTrace(".psLib.dataManip.calculateSecondDerivs", 4, + "---- calculateSecondDerivs() begin ----\n"); + + psS32 i; + psS32 k; + psF32 sig; + psF32 p; + psS32 n = y->n; + psF32 *u = (psF32 *) psAlloc(n * sizeof(psF32)); + psF32 *derivs2 = (psF32 *) psAlloc(n * sizeof(psF32)); + 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; + 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; + 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--) { + derivs2[k] = derivs2[k] * derivs2[k+1] + u[k]; + + psTrace(".psLib.dataManip.calculateSecondDerivs", 6, + "derivs2[%d] is %f\n", k, derivs2[k]); + } + + psFree(u); + psTrace(".psLib.dataManip.calculateSecondDerivs", 4, + "---- calculateSecondDerivs() end ----\n"); + 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 *mySpline, ///< 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 (mySpline != NULL) { + PS_ASSERT_VECTOR_TYPE(mySpline->knots, PS_TYPE_F32, NULL); + } + psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, + "---- psVectorFitSpline1D() begin ----\n"); + psS32 numSplines = (y->n)-1; + psF32 tmp; + psF32 H; + psS32 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 mySpline is not NULL? + Should we error if mySPline is not NULL? + */ + if (mySpline == NULL) { + mySpline = psSpline1DAllocGeneric(x32, 3); + } + PS_ASSERT_PTR_NON_NULL(mySpline, NULL); + PS_ASSERT_INT_NONNEGATIVE(mySpline->n, NULL); + + if (y32->n != (1 + mySpline->n)) { + psError(PS_ERR_BAD_PARAMETER_SIZE, true, + "data size / spline size mismatch (%d %d)\n", + y32->n, mySpline->n); + return(NULL); + } + + // If these are linear splines, which means their polynomials will have + // two coefficients, then we do the simple calculation. + if (2 == (mySpline->spline[0])->n) { + 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 %d 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 (4 != (mySpline->spline[0])->n) { + psError(PS_ERR_BAD_PARAMETER_SIZE, true, + "Don't know how to generate %d-order splines.", + (mySpline->spline[0])->n-1); + 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[%d] 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[%d])->coeff[0] is %f\n", i, (mySpline->spline[i])->coeff[0]); + psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, + "(mySpline->spline[%d])->coeff[1] is %f\n", i, (mySpline->spline[i])->coeff[1]); + psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, + "(mySpline->spline[%d])->coeff[2] is %f\n", i, (mySpline->spline[i])->coeff[2]); + psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, + "(mySpline->spline[%d])->coeff[3] is %f\n", i, (mySpline->spline[i])->coeff[3]); + + } + + psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, + "---- psVectorFitSpline1D() end ----\n"); + return(mySpline); +} + + + + + + + + + +/***************************************************************************** +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 + 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 (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; + } 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 (psS32 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 (psS32 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); + + psS32 numSplines = nKnots - 1; + psF32 tmp; + psF32 H; + psS32 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 %d 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[%d] 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[%d])->coeff[0] is %f\n", i, (mySpline->spline[i])->coeff[0]); + psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, + "(mySpline->spline[%d])->coeff[1] is %f\n", i, (mySpline->spline[i])->coeff[1]); + psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, + "(mySpline->spline[%d])->coeff[2] is %f\n", i, (mySpline->spline[i])->coeff[2]); + psTrace(".psLib.dataManip.psVectorFitSpline1D", 6, + "(mySpline->spline[%d])->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 */ @@ -283,5 +781,5 @@ } } else { - printf("XXX: Generate an error here.\n"); + psError(PS_ERR_UNKNOWN, true, "psVector in has an incorrect type.\n"); return(NULL); }