Changeset 42336 for trunk/psLib/src/math/psSpline.c
- Timestamp:
- Jan 30, 2023, 9:51:20 AM (3 years ago)
- Location:
- trunk/psLib
- Files:
-
- 2 edited
-
. (modified) (1 prop)
-
src/math/psSpline.c (modified) (9 diffs)
Legend:
- Unmodified
- Added
- Removed
-
trunk/psLib
-
Property svn:mergeinfo
set to (toggle deleted branches)
/branches/IPP-308_move_backups_folder/psLib merged eligible /branches/czw_branch/20160809/psLib merged eligible /branches/czw_branch/20170908/psLib merged eligible /branches/eam_branches/ipp-20191011/psLib merged eligible /branches/eam_branches/ipp-20211108/psLib merged eligible /branches/eam_branches/ipp-dev-20210817/psLib merged eligible /branches/eam_branches/psLib.20230123 merged eligible /tags/ipp-ops-20220303-rc/psLib merged eligible /tags/ipp-ops-20220705/psLib merged eligible /tags/ipp-ps1-20210510/psLib merged eligible /tags/ipp-ps1-20210708/psLib merged eligible /branches/ipp-259_genericise_backups/psLib 40910-40966
-
Property svn:mergeinfo
set to (toggle deleted branches)
-
trunk/psLib/src/math/psSpline.c
r23486 r42336 1 1 /** @file psSpline.c 2 * 3 * @brief Contains basic spline allocation, deallocation, fitting, 4 * and evaluation routines. 5 * 6 * This file contains the routines that allocate, free, and evaluate splines. 7 * 8 * @version $Revision: 1.158 $ $Name: not supported by cvs2svn $ 9 * @date $Date: 2007-03-14 02:36:28 $ 10 * 11 * Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii 2 re-written by EAM 2023.01.23 12 3 */ 13 4 … … 16 7 #endif 17 8 18 /*****************************************************************************/19 /* INCLUDE FILES */20 /*****************************************************************************/21 9 #include <stdio.h> 22 10 #include <stdbool.h> … … 36 24 #include "psMathUtils.h" 37 25 38 /*****************************************************************************/ 39 /* DEFINE STATEMENTS */ 40 /*****************************************************************************/ 41 42 /*****************************************************************************/ 43 /* TYPE DEFINITIONS */ 44 /*****************************************************************************/ 45 46 /*****************************************************************************/ 47 /* GLOBAL VARIABLES */ 48 /*****************************************************************************/ 49 50 /*****************************************************************************/ 51 /* FILE STATIC VARIABLES */ 52 /*****************************************************************************/ 53 54 /*****************************************************************************/ 55 /* FUNCTION IMPLEMENTATION - LOCAL */ 56 /*****************************************************************************/ 57 58 static void spline1DFree(psSpline1D *tmpSpline) 59 { 60 if (tmpSpline == NULL) { 61 return; 62 } 63 64 if (tmpSpline->spline != NULL) { 65 for (psS32 i=0;i<tmpSpline->n;i++) { 66 psFree((tmpSpline->spline)[i]); 67 } 68 psFree(tmpSpline->spline); 69 } 70 71 if (tmpSpline->p_psDeriv2 != NULL) { 72 psFree(tmpSpline->p_psDeriv2); 73 } 74 75 psFree(tmpSpline->knots); 26 static void psSpline1DFree(psSpline1D *tmpSpline) 27 { 28 if (tmpSpline == NULL) return; 29 30 psFree(tmpSpline->xKnots); 31 psFree(tmpSpline->yKnots); 32 psFree(tmpSpline->d2yKnots); 76 33 77 34 return; 78 35 } 79 36 80 81 static void PS_POLY1D_PRINT( 82 psPolynomial1D *poly) 83 { 84 printf("-------------- PS_POLY1D_PRINT() --------------\n"); 85 printf("poly->nX is %d\n", poly->nX); 86 for (psS32 i = 0 ; i < (1 + poly->nX) ; i++) { 87 printf("poly->coeff[%d] is %f\n", i, poly->coeff[i]); 88 } 89 } 90 37 /* 91 38 static void PS_PRINT_SPLINE2(psSpline1D *mySpline) 92 39 { 93 40 printf("-------------- PS_PRINT_SPLINE2() --------------\n"); 94 41 printf("mySpline->n is %d\n", mySpline->n); 42 if (!mySpline->xKnots) return; 43 if (!mySpline->yKnots) return; 44 if (!mySpline->d2yKnots) return; 95 45 for (psS32 i = 0 ; i < mySpline->n ; i++) { 96 PS_POLY1D_PRINT(mySpline->spline[i]);97 } 98 PS_VECTOR_PRINT_F32(mySpline->knots); 99 } 46 printf("(x, y, d2y) : %f %f %f\n", mySpline->xKnots[i], mySpline->yKnots[i], mySpline->d2yKnots[i]); 47 } 48 } 49 */ 100 50 101 51 /***************************************************************************** 102 CalculateSecondDerivs(): Given a set of x/y vectors corresponding to a 103 tabulated function at n points, this routine calculates the second 104 derivatives of the interpolating cubic splines at those n points. 105 106 The first and second derivatives at the endpoints were previously undefined in 107 the SDR. From bugzilla #???, they are required to be 0.0, implementing natural 108 splines. 109 110 Endpoints are defined by 111 PS_LEFT_SPLINE_DERIV 112 PS_RIGHT_SPLINE_DERIV 113 114 This routine assumes that vectors x and y are of the appropriate types/sizes 115 (F32). 116 117 XXX: use recycled vectors for internal data. 118 XXX: do an F64 version? 52 CalculateSecondDerivs(): Given a set of x,y vectors corresponding to the spline knots, this 53 routine calculates the second derivatives of the interpolating cubic splines at those n points. 54 55 The boundary conditions may be: 56 * first derivative specified OR 57 * second derivatives = 0 58 59 One or the other condition may be true for each of the upper and lower bounds 60 61 NOTE: vectors must be F32 62 63 EAM 2023.01.19 : the comment above is wrong: the code below implements the 64 splines with *only* the 1st derivatives at the end points set to 0.0. the 65 2nd derivatives are constrained by the equations for the splines. it is not 66 possible to specify both the endpoint 1st and 2nd derivaties: there would be 67 too many constraints for the number of free parameters. 68 69 It is not clear that choosing to set the end point 1st derivatives to 0.0 is the best 70 option. setting the 2nd derivatives to zero allow for linear extrapolation. 71 119 72 *****************************************************************************/ 120 #define PS_LEFT_SPLINE_DERIV 0.0 121 #define PS_RIGHT_SPLINE_DERIV 0.0 122 static psF32 *calculateSecondDerivs( 123 const psVector* x, ///< Ordinates 124 const psVector* y) ///< Coordinates 73 74 void calculateSecondDerivs( 75 const psSpline1D *mySpline, 76 psF32 dyLower, // if not NAN, lower-bound 1st derivative is defined 77 psF32 dyUpper // if not NAN, lower-bound 1st derivative is defined 78 ) 125 79 { 126 80 psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__); 127 if (psTraceGetLevel("psLib.math") >= 6) { 128 p_psVectorPrint(1, (psVector *) x, "x");129 p_psVectorPrint(1, (psVector *) y, "y");130 } 131 ps S32 n = y->n;132 psF32 * u = (psF32 *) psAlloc(n * sizeof(psF32));133 psF32 *d erivs2 = (psF32 *) psAlloc(n * sizeof(psF32));134 psF32 *X = (psF32 *) & (x->data.F32[0]); 135 psF32 *Y = (psF32 *) & (y->data.F32[0]);136 //137 // The second derivatives at the endpoints, undefined in the SDR,138 // are set in psAssert.h: PS_LEFT_SPLINE_DERIV, PS_RIGHT_SPLINE_DERIV.139 //140 derivs2[0] = -0.5;141 u[0]= (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - PS_LEFT_SPLINE_DERIV);142 143 for (psS32 i =1;i<=(n-2);i++) {144 psF32 sig= (X[i] - X[i-1]) / (X[i+1] - X[i-1]);145 psF32 p = sig * derivs2[i-1] + 2.0;146 d erivs2[i] = (sig - 1.0) / p;147 u[i] = ((Y[i+1] - Y[i])/(X[i+1]-X[i])) - ((Y[i]-Y[i-1])/(X[i]-X[i-1]));148 u[i] = ((6.0 * u[i] / (X[i+1] - X[i-1])) - (sig * u[i-1])) / p;81 82 psS32 n = mySpline->n; // n is the number of knots 83 psF32 *u = (psF32 *) psAlloc(n * sizeof(psF32)); 84 85 psF32 *X = mySpline->xKnots; 86 psF32 *Y = mySpline->yKnots; 87 psF32 *d2y = mySpline->d2yKnots; 88 89 if (isfinite(dyLower)) { 90 d2y[0] = -0.5; 91 u[0] = (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - dyLower); 92 } else { 93 d2y[0] = 0.0; 94 u[0] = 0.0; 95 } 96 97 for (psS32 i = 1; i < n - 1; i++) { 98 psF32 dX = (X[i] - X[i-1]) / (X[i+1] - X[i-1]); 99 psF32 dY = dX * d2y[i-1] + 2.0; 100 d2y[i] = (dX - 1.0) / dY; 101 u[i] = ((Y[i+1] - Y[i])/(X[i+1] - X[i])) - ((Y[i] - Y[i-1])/(X[i] - X[i-1])); 102 u[i] = ((6.0 * u[i] / (X[i+1] - X[i-1])) - (dX * u[i-1])) / dY; 149 103 150 104 psTrace("psLib.math", 6, "X[%d] is %f\n", i, X[i]); … … 153 107 } 154 108 155 psF32 qn = 0.5; 156 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])); 157 derivs2[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * derivs2[n-2]) + 1.0); 158 159 for (psS32 k=(n-2);k>=0;k--) { 160 derivs2[k] = derivs2[k] * derivs2[k+1] + u[k]; 161 psTrace("psLib.math", 6, "derivs2[%d] is %f\n", k, derivs2[k]); 109 if (isfinite(dyUpper)) { 110 psF32 qn = 0.5; 111 u[n-1] = (3.0/(X[n-1]-X[n-2])) * (dyUpper - (Y[n-1]-Y[n-2])/(X[n-1]-X[n-2])); 112 d2y[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * d2y[n-2]) + 1.0); 113 } else { 114 d2y[n-1] = 0; 115 } 116 117 for (psS32 k = n-2; k >= 0; k--) { 118 d2y[k] = d2y[k] * d2y[k+1] + u[k]; 119 psTrace("psLib.math", 6, "derivs2[%d] is %f\n", k, d2y[k]); 162 120 } 163 121 psFree(u); 164 122 psTrace("psLib.math", 4, "---- %s() end ----\n", __func__); 165 return (derivs2);123 return; 166 124 } 167 125 … … 174 132 { 175 133 PS_ASSERT_PTR(ptr, false); 176 return ( psMemGetDeallocator(ptr) == (psFreeFunc) spline1DFree );134 return ( psMemGetDeallocator(ptr) == (psFreeFunc) psSpline1DFree ); 177 135 } 178 136 … … 180 138 { 181 139 psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D)); 140 182 141 tmpSpline->n = 0; 183 tmpSpline->spline = NULL; 184 tmpSpline->knots = NULL; 185 tmpSpline->p_psDeriv2 = NULL; 186 psMemSetDeallocator(tmpSpline, (psFreeFunc) spline1DFree); 142 tmpSpline->xMin = NAN; 143 tmpSpline->xMax = NAN; 144 tmpSpline->xDel = NAN; 145 tmpSpline->equalSpacing = false; 146 147 tmpSpline->xKnots = NULL; 148 tmpSpline->yKnots = NULL; 149 tmpSpline->d2yKnots = NULL; 150 psMemSetDeallocator(tmpSpline, (psFreeFunc) psSpline1DFree); 187 151 188 152 return(tmpSpline); 189 153 } 190 154 155 /** Create an empty 1D spline **/ 156 psSpline1D *psSpline1DCreate( 157 int nKnots) ///< number of knots 158 { 159 psSpline1D *spline = psSpline1DAlloc(); 160 spline->n = nKnots; // number of knots 161 162 spline->xKnots = (psF32 *) psAlloc( spline->n * sizeof(psF32)); 163 spline->yKnots = (psF32 *) psAlloc( spline->n * sizeof(psF32)); 164 spline->d2yKnots = (psF32 *) psAlloc( spline->n * sizeof(psF32)); 165 166 // x & y can both be F32 or F64. should knots be F64? 167 for (psS32 i = 0 ; i < spline->n ; i++) { 168 spline->xKnots[i] = NAN; 169 spline->yKnots[i] = NAN; 170 spline->d2yKnots[i] = NAN; 171 } 172 return(spline); 173 } 174 191 175 /***************************************************************************** 192 ps VectorFitSpline1D(): given a set of x/y vectors, this routine generates the176 psSpline1DFitVector(): given a set of x,y vectors, this routine generates the 193 177 linear or cublic splines which satisfy those data points. 194 178 … … 209 193 XXX: What types must be supported? 210 194 *****************************************************************************/ 211 psSpline1D *ps VectorFitSpline1D(195 psSpline1D *psSpline1DFitVector( 212 196 const psVector* x, ///< Ordinates. 213 const psVector* y) ///< Coordinates. 197 const psVector* y, ///< Coordinates. 198 psF32 dyLower, ///< 1st derivative at lower bound 199 psF32 dyUpper) ///< 1st derivative at upper bound 214 200 { 215 201 psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); 202 PS_ASSERT_VECTOR_NON_NULL(x, NULL); 216 203 PS_ASSERT_VECTOR_NON_NULL(y, NULL); 204 PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL); 205 PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); 217 206 PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL); 218 207 PS_ASSERT_LONG_LARGER_THAN_OR_EQUAL(y->n, (long)2, NULL); 219 psS32 numSplines = (y->n)-1; 220 psTrace("psLib.math", 5, "numSplines is %d\n", numSplines); 221 222 // 223 // Create the psSpline1D struct. 224 // 225 psSpline1D *spline = psSpline1DAlloc(); 226 spline->n = numSplines; 227 spline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *)); 228 for (psS32 i=0;i<numSplines;i++) { 229 spline->spline[i] = psPolynomial1DAlloc(PS_POLYNOMIAL_ORD, 3); 230 } 231 232 // 233 // The following code ensures that xPtr and yPtr points to a psF32 psVector. 234 // 235 // XXX: Use the vector copy and create routines here: 236 // 237 238 spline->knots = psVectorAlloc(y->n, PS_TYPE_F32); 239 if (x != NULL) { 240 PS_ASSERT_VECTOR_NON_NULL(x, NULL); 241 PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL); 242 PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); 243 if (x->type.type == PS_TYPE_F32) { 244 for (psS32 i = 0 ; i < x->n ; i++) { 245 spline->knots->data.F32[i] = x->data.F32[i]; 246 } 247 } else if (x->type.type == PS_TYPE_F64) { 248 for (psS32 i = 0 ; i < x->n ; i++) { 249 spline->knots->data.F32[i] = (psF32) x->data.F64[i]; 250 } 251 } 252 } else { 253 for (psS32 i = 0 ; i < y->n ; i++) { 254 spline->knots->data.F32[i] = (psF32) i; 255 } 256 } 257 psVector *xPtr = spline->knots; 258 259 psVector *yPtr = NULL; 260 // Convert y to F32 if necessary. 261 if (PS_TYPE_F64 == y->type.type) { 262 yPtr = psVectorCopy(NULL, y, PS_TYPE_F32); 263 } else { 264 yPtr = (psVector *) y; 265 } 266 267 // 208 209 psSpline1D *spline = psSpline1DCreate(y->n); 210 211 // x & y can both be F32 or F64. should knots be F64? 212 for (psS32 i = 0 ; i < spline->n ; i++) { 213 spline->xKnots[i] = (x->type.type == PS_TYPE_F32) ? x->data.F32[i] : x->data.F64[i]; 214 spline->yKnots[i] = (y->type.type == PS_TYPE_F32) ? y->data.F32[i] : y->data.F64[i]; 215 } 216 268 217 // Generate the second derivatives at each data point. 269 // 270 spline->p_psDeriv2 = calculateSecondDerivs(xPtr, yPtr); 271 272 // 273 // We generate the coefficients of the spline polynomials. I can't 274 // concisely explain how this code works. See above function comments 275 // and Numerical Recipes in C. 276 // 277 for (psS32 i=0 ; i < numSplines ; i++) { 278 psF32 H = xPtr->data.F32[i+1] - xPtr->data.F32[i]; 279 if (fabs(H) <= FLT_EPSILON) { 280 psError(PS_ERR_UNKNOWN, false, "x data points are not distinct (%d %d) (%f %f).\n", 281 i, i+1, xPtr->data.F32[i], xPtr->data.F32[i+1]); 282 } 283 psTrace("psLib.math", 6, "x data (%f - %f) (%f)\n", xPtr->data.F32[i], xPtr->data.F32[i+1], H); 284 // 285 // ******** Calculate 0-order term ******** 286 // 287 // From (1) 288 spline->spline[i]->coeff[0] = yPtr->data.F32[i] * xPtr->data.F32[i+1]/H; 289 // From (2) 290 spline->spline[i]->coeff[0]-= (yPtr->data.F32[i+1] * xPtr->data.F32[i])/H; 291 // From (3) 292 psF32 tmp = (xPtr->data.F32[i+1] * xPtr->data.F32[i+1] * xPtr->data.F32[i+1]) / (H * H * H); 293 tmp-= xPtr->data.F32[i+1] / H; 294 tmp*= spline->p_psDeriv2[i] * H * H / 6.0; 295 spline->spline[i]->coeff[0]+= tmp; 296 // From (4) 297 tmp = -(xPtr->data.F32[i] * xPtr->data.F32[i] * xPtr->data.F32[i]) / (H * H * H); 298 tmp+= xPtr->data.F32[i] / H; 299 tmp*= spline->p_psDeriv2[i+1] * H * H / 6.0; 300 spline->spline[i]->coeff[0]+= tmp; 301 302 // 303 // ******** Calculate 1-order term ******** 304 // 305 // From (1) 306 spline->spline[i]->coeff[1] = -(yPtr->data.F32[i]) / H; 307 // From (2) 308 spline->spline[i]->coeff[1]+= yPtr->data.F32[i+1] / H; 309 // From (3) 310 tmp = -3.0 * xPtr->data.F32[i+1] * xPtr->data.F32[i+1] / (H * H * H); 311 tmp+= (1.0 / H); 312 tmp*= spline->p_psDeriv2[i] * H * H / 6.0; 313 spline->spline[i]->coeff[1]+= tmp; 314 // From (4) 315 tmp = 3.0 * xPtr->data.F32[i] * xPtr->data.F32[i] / (H * H * H); 316 tmp-= 1.0 / H; 317 tmp*= spline->p_psDeriv2[i+1] * H * H / 6.0; 318 spline->spline[i]->coeff[1]+= tmp; 319 320 // 321 // ******** Calculate 2-order term ******** 322 // 323 // From (3) 324 spline->spline[i]->coeff[2] = spline->p_psDeriv2[i] * 3.0 * xPtr->data.F32[i+1] / (6.0 * H); 325 // From (4) 326 spline->spline[i]->coeff[2]-= spline->p_psDeriv2[i+1] * 3.0 * xPtr->data.F32[i] / (6.0 * H); 327 328 // 329 // ******** Calculate 3-order term ******** 330 // 331 // From (3) 332 spline->spline[i]->coeff[3] = -spline->p_psDeriv2[i] / (6.0 * H); 333 // From (4) 334 spline->spline[i]->coeff[3]+= spline->p_psDeriv2[i+1] / (6.0 * H); 335 336 psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[0] is %f\n", i, spline->spline[i]->coeff[0]); 337 psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[1] is %f\n", i, spline->spline[i]->coeff[1]); 338 psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[2] is %f\n", i, spline->spline[i]->coeff[2]); 339 psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[3] is %f\n", i, spline->spline[i]->coeff[3]); 340 } 341 342 if (PS_TYPE_F64 == y->type.type) { 343 psFree(yPtr); 344 } 218 calculateSecondDerivs(spline, dyLower, dyUpper); 219 345 220 psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); 346 221 return(spline); 347 222 } 348 223 349 350 /***************************************************************************** 351 psSpline1DEval(): this routine takes an existing spline of arbitrary order 352 and an independent x value. It determines which spline that x corresponds 353 to by doing a bracket disection on the knots of the spline data structure 354 (vectorBinDisectF32()). Then it evaluates the spline at that x location 355 by a call to the 1D polynomial functions. 356 357 XXX: The spline eval functions require input and output to be F32. however 358 the spline fit functions require F32 and F64. 359 360 XXX: This only works if spline->knots if psF32. Must we add support for psU32 and 361 psF64? 362 *****************************************************************************/ 224 psPolynomial1D *psSpline1DToPoly (psSpline1D *spline, int n) { 225 PS_ASSERT_INT_LESS_THAN(n, spline->n - 1, NULL); 226 227 // convert the cubic spline coeffs to a polynomial. See above function comments and 228 // Numerical Recipes in C. 229 230 psF32 *xKnots = spline->xKnots; 231 psF32 *yKnots = spline->yKnots; 232 psF32 *d2yKnots = spline->d2yKnots; 233 234 psF32 H = xKnots[n+1] - xKnots[n]; 235 if (fabs(H) <= FLT_EPSILON) { 236 psError(PS_ERR_UNKNOWN, false, "x data points are not distinct (%d %d) (%f %f).\n", 237 n, n+1, xKnots[n], xKnots[n+1]); 238 } 239 240 psPolynomial1D *myPoly = psPolynomial1DAlloc(PS_POLYNOMIAL_ORD, 3); 241 242 // ******** Calculate 0-order term ******** 243 // From (1) 244 myPoly->coeff[0] = yKnots[n] * xKnots[n+1]/H; 245 // From (2) 246 myPoly->coeff[0]-= (yKnots[n+1] * xKnots[n])/H; 247 // From (3) 248 psF32 tmp = (xKnots[n+1] * xKnots[n+1] * xKnots[n+1]) / (H * H * H); 249 tmp-= xKnots[n+1] / H; 250 tmp*= d2yKnots[n] * H * H / 6.0; 251 myPoly->coeff[0]+= tmp; 252 // From (4) 253 tmp = -(xKnots[n] * xKnots[n] * xKnots[n]) / (H * H * H); 254 tmp+= xKnots[n] / H; 255 tmp*= d2yKnots[n+1] * H * H / 6.0; 256 myPoly->coeff[0]+= tmp; 257 258 // 259 // ******** Calculate 1-order term ******** 260 // 261 // From (1) 262 myPoly->coeff[1] = -(yKnots[n]) / H; 263 // From (2) 264 myPoly->coeff[1]+= yKnots[n+1] / H; 265 // From (3) 266 tmp = -3.0 * xKnots[n+1] * xKnots[n+1] / (H * H * H); 267 tmp+= (1.0 / H); 268 tmp*= d2yKnots[n] * H * H / 6.0; 269 myPoly->coeff[1]+= tmp; 270 // From (4) 271 tmp = 3.0 * xKnots[n] * xKnots[n] / (H * H * H); 272 tmp-= 1.0 / H; 273 tmp*= d2yKnots[n+1] * H * H / 6.0; 274 myPoly->coeff[1]+= tmp; 275 276 // 277 // ******** Calculate 2-order term ******** 278 // 279 // From (3) 280 myPoly->coeff[2] = d2yKnots[n] * 3.0 * xKnots[n+1] / (6.0 * H); 281 // From (4) 282 myPoly->coeff[2]-= d2yKnots[n+1] * 3.0 * xKnots[n] / (6.0 * H); 283 284 // 285 // ******** Calculate 3-order term ******** 286 // 287 // From (3) 288 myPoly->coeff[3] = -d2yKnots[n] / (6.0 * H); 289 // From (4) 290 myPoly->coeff[3]+= d2yKnots[n+1] / (6.0 * H); 291 292 psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[0] is %f\n", n, myPoly->coeff[0]); 293 psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[1] is %f\n", n, myPoly->coeff[1]); 294 psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[2] is %f\n", n, myPoly->coeff[2]); 295 psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[3] is %f\n", n, myPoly->coeff[3]); 296 297 return myPoly; 298 } 299 300 // given an already-constructed spline, check/assert that the 301 // knot spacing is equal. this allows some optimization 302 bool psSpline1DisEqualSpacing (psSpline1D *spline) { 303 304 PS_ASSERT_PTR_NON_NULL(spline, false); 305 PS_ASSERT_PTR_NON_NULL(spline->xKnots, false); 306 PS_ASSERT_PTR_NON_NULL(spline->yKnots, false); 307 PS_ASSERT_PTR_NON_NULL(spline->d2yKnots, false); 308 309 // if the spline has equally-spaced xKnots, the values of the 310 // xKnots can be predicted from the first, last, and delta values 311 312 int n = spline->n; 313 spline->xMax = spline->xKnots[n-1]; 314 spline->xMin = spline->xKnots[0]; 315 spline->xDel = (spline->xMax - spline->xMin) / (n - 1); 316 317 // check that the xKnots actually follow this spacing: 318 319 for (int i = 1; i < n - 1; i++) { 320 float xValue = spline->xMin + i*spline->xDel; 321 fprintf (stderr, "%d %f - %f = %f\n", i, spline->xKnots[i], xValue, spline->xKnots[i] - xValue); 322 } 323 324 spline->equalSpacing = true; 325 return true; 326 } 327 328 // XXX EAM : changing implementation to use yKnot, d2yKnot instead of polynomials 363 329 float psSpline1DEval( 364 330 const psSpline1D *spline, … … 368 334 PS_ASSERT_PTR_NON_NULL(spline, NAN); 369 335 PS_ASSERT_INT_NONNEGATIVE(spline->n, NAN); 370 PS_ASSERT_ VECTOR_TYPE(spline->knots, PS_TYPE_F32, NAN);336 PS_ASSERT_PTR_NON_NULL(spline->xKnots, NAN); 371 337 372 338 psS32 n = spline->n; 373 if ((x < spline->knots->data.F32[0]) || (x > spline->knots->data.F32[spline->knots->n-1])) { 374 // If x is outside the range of spline->knots, generate a warning 375 // message, then return the left, or right, endpoint. 376 psLogMsg(__func__, PS_LOG_WARN, 377 "psSpline1DEval(): x ordinate (%f) is outside the spline range (%f - %f) (%d).", 378 x, spline->knots->data.F32[0], spline->knots->data.F32[n-1], n); 379 380 psS32 binNum = (x < spline->knots->data.F32[0]) ? 0 : n-1; 381 psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); 382 return(psPolynomial1DEval(spline->spline[binNum], x)); 383 } 384 385 psScalar tmpScalar; 386 tmpScalar.type.type = PS_TYPE_F32; 387 tmpScalar.data.F32 = x; 388 psVectorBinaryDisectResult result; 389 psS32 binNum = psVectorBinaryDisect(&result, spline->knots, &tmpScalar); 390 if (result != PS_BINARY_DISECT_PASS) { 391 psError(PS_ERR_UNKNOWN, false, "Could not perform bin dissection on spline->knots.\n"); 392 return(NAN); 393 } 394 395 psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); 396 return(psPolynomial1DEval(spline->spline[binNum], x)); 397 } 398 339 340 // XXX this should be linear extrapolation at the high or low ends 341 if (x < spline->xKnots[0]) return psSpline1DEval_Segment(spline, 0, x); 342 if (x > spline->xKnots[n-1]) return psSpline1DEval_Segment(spline, n-2, x); 343 344 if (spline->equalSpacing) { 345 int bin = (x - spline->xMin) / spline->xDel; 346 bin = PS_MIN(PS_MAX(bin, 0), n - 2); 347 return psSpline1DEval_Segment(spline, bin, x); 348 } 349 350 /* find correct element in array (x must be sorted) */ 351 int lo = 0; 352 int hi = n-1; 353 while (hi - lo > 1) { 354 int i = 0.5*(hi+lo); 355 if (spline->xKnots[i] > x) { 356 hi = i; 357 } else { 358 lo = i; 359 } 360 } 361 362 return psSpline1DEval_Segment(spline, lo, x); 363 } 364 365 // evaluate the spline at the given coordinate using the specified segment 366 // (YMMV if you use the wrong segment!) 367 float psSpline1DEval_Segment( 368 const psSpline1D *spline, 369 int n, 370 float x) 371 { 372 psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); 373 PS_ASSERT_PTR_NON_NULL(spline, NAN); 374 PS_ASSERT_PTR_NON_NULL(spline->xKnots, NAN); 375 PS_ASSERT_PTR_NON_NULL(spline->yKnots, NAN); 376 PS_ASSERT_PTR_NON_NULL(spline->d2yKnots, NAN); 377 PS_ASSERT_INT_NONNEGATIVE(spline->n, NAN); 378 PS_ASSERT_INT_LESS_THAN(n, spline->n - 1, NAN); 379 380 psF32 dX = spline->xKnots[n+1] - spline->xKnots[n]; 381 psF32 A = (spline->xKnots[n+1] - x) / dX; 382 psF32 B = (x - spline->xKnots[n]) / dX; 383 384 psF32 value = A*spline->yKnots[n] + B*spline->yKnots[n+1] + ((A*A*A - A)*spline->d2yKnots[n] + (B*B*B - B)*spline->d2yKnots[n+1])*(dX*dX) / 6.0; 385 return value; 386 } 399 387 400 388 /***************************************************************************** 389 returns a vector of the same type as the input (x) 401 390 *****************************************************************************/ 402 391 psVector *psSpline1DEvalVector( … … 405 394 { 406 395 psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__); 407 PS_ASSERT_PTR_NON_NULL(spline, NULL); 408 PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL); 396 PS_ASSERT_PTR_NON_NULL(spline, NULL); 397 PS_ASSERT_PTR_NON_NULL(spline->xKnots, NULL); 398 PS_ASSERT_PTR_NON_NULL(spline->yKnots, NULL); 399 PS_ASSERT_PTR_NON_NULL(spline->d2yKnots, NULL); 400 PS_ASSERT_INT_NONNEGATIVE(spline->n, NULL); 401 409 402 PS_ASSERT_VECTOR_NON_NULL(x, NULL); 410 403 PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL); 411 if (psTraceGetLevel("psLib.math") >= 6) { 412 PS_VECTOR_PRINT_F32(x); 413 PS_PRINT_SPLINE2((psSpline1D *) spline); 414 } 415 416 psVector *tmpVector = psVectorAlloc(x->n, PS_TYPE_F32); 404 405 psVector *tmpVector = psVectorAlloc(x->n, x->type.type); 417 406 if (x->type.type == PS_TYPE_F32) { 418 407 for (psS32 i=0;i<x->n;i++) { 419 408 tmpVector->data.F32[i] = psSpline1DEval(spline, x->data.F32[i]); 420 409 } 421 } else if (x->type.type == PS_TYPE_F64){410 } else { 422 411 for (psS32 i=0;i<x->n;i++) { 423 tmpVector->data.F 32[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]);412 tmpVector->data.F64[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]); 424 413 } 425 414 } 426 415 427 416 psTrace("psLib.math", 3, "---- %s() end ----\n", __func__); 428 417 return(tmpVector);
Note:
See TracChangeset
for help on using the changeset viewer.
