IPP Software Navigation Tools IPP Links Communication Pan-STARRS Links

Ignore:
Timestamp:
Jan 26, 2023, 10:04:39 AM (3 years ago)
Author:
eugene
Message:

rework the splines to use (x,y,dy2) values instead of full polynomials

File:
1 edited

Legend:

Unmodified
Added
Removed

  • branches/eam_branches/psLib.20230123/src/math/psSpline.c

    r42319 r42324  
    2828    if (tmpSpline == NULL) return;
    2929
    30     if (tmpSpline->spline != NULL) {
    31         for (psS32 i=0;i<tmpSpline->n;i++) {
    32             psFree((tmpSpline->spline)[i]);
    33         }
    34         psFree(tmpSpline->spline);
    35     }
    36 
    37     if (tmpSpline->p_psDeriv2 != NULL) {
    38         psFree(tmpSpline->p_psDeriv2);
    39     }
    40 
    41     psFree(tmpSpline->knots);
     30    psFree(tmpSpline->xKnots);
     31    psFree(tmpSpline->yKnots);
     32    psFree(tmpSpline->d2yKnots);
    4233
    4334    return;
    4435}
    4536
     37/*
    4638static void PS_PRINT_SPLINE2(psSpline1D *mySpline)
    4739{
    4840    printf("-------------- PS_PRINT_SPLINE2() --------------\n");
    4941    printf("mySpline->n is %d\n", mySpline->n);
     42    if (!mySpline->xKnots) return;
     43    if (!mySpline->yKnots) return;
     44    if (!mySpline->d2yKnots) return;
    5045    for (psS32 i = 0 ; i < mySpline->n ; i++) {
    51         // print the knots
    52     }
    53 }
     46        printf("(x, y, d2y) : %f %f %f\n", mySpline->xKnots[i], mySpline->yKnots[i], mySpline->d2yKnots[i]);
     47    }
     48}
     49*/
    5450
    5551/*****************************************************************************
    … …  
    6561NOTE: vectors must be F32
    6662
     63EAM 2023.01.19 : the comment above is wrong: the code below implements the
     64splines with *only* the 1st derivatives at the end points set to 0.0.  the
     652nd derivatives are constrained by the equations for the splines.  it is not
     66possible to specify both the endpoint 1st and 2nd derivaties: there would be
     67too many constraints for the number of free parameters.
     68
     69It is not clear that choosing to set the end point 1st derivatives to 0.0 is the best
     70option.  setting the 2nd derivatives to zero allow for linear extrapolation.
     71
    6772 *****************************************************************************/
    6873
    6974static psF32 *calculateSecondDerivs(
    70     const psVector* x, ///< Ordinates
    71     const psVector* y, ///< Coordinates
    72     psF32 dyLower; // if not NAN, lower-bound 1st derivative is defined
    73     psF32 dyUpper; // if not NAN, lower-bound 1st derivative is defined
     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
    7478    )                 
    7579{
    7680    psTrace("psLib.math", 4, "---- %s() begin ----\n", __func__);
    77     if (psTraceGetLevel("psLib.math") >= 6) {
    78         p_psVectorPrint(1, (psVector *) x, "x");
    79         p_psVectorPrint(1, (psVector *) y, "y");
    80     }
    81     psS32 n = y->n;
    82     psF32 *u = (psF32 *) psAlloc(n * sizeof(psF32));
    83     psF32 *derivs2 = (psF32 *) psAlloc(n * sizeof(psF32));
    84     psF32 *X = (psF32 *) & (x->data.F32[0]);
    85     psF32 *Y = (psF32 *) & (y->data.F32[0]);
    86     //
    87     // The second derivatives at the endpoints, undefined in the SDR,
    88     // are set in psAssert.h: PS_LEFT_SPLINE_DERIV, PS_RIGHT_SPLINE_DERIV.
    89     //
    90     derivs2[0] = -0.5;
    91     u[0]= (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - PS_LEFT_SPLINE_DERIV);
    92 
    93     for (psS32 i=1;i<=(n-2);i++) {
    94         psF32 sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]);
    95         psF32 p = sig * derivs2[i-1] + 2.0;
    96         derivs2[i] = (sig - 1.0) / p;
    97         u[i] = ((Y[i+1] - Y[i])/(X[i+1]-X[i])) - ((Y[i]-Y[i-1])/(X[i]-X[i-1]));
    98         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    psF32 *d2y = (psF32 *) psAlloc(n * sizeof(psF32));
     85    psF32 *X = mySpline->xKnots;
     86    psF32 *Y = mySpline->yKnots;
     87
     88    if (isfinite(dyLower)) {
     89      d2y[0] = -0.5;
     90      u[0]   = (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - dyLower);
     91    } else {
     92      d2y[0] = 0.0;
     93      u[0]   = 0.0;
     94    }
     95
     96    for (psS32 i = 1; i < n - 1; i++) {
     97        psF32 dX = (X[i] - X[i-1]) / (X[i+1] - X[i-1]);
     98        psF32 dY = dX * d2y[i-1] + 2.0;
     99        d2y[i] = (dX - 1.0) / dY;
     100        u[i]   = ((Y[i+1] - Y[i])/(X[i+1] - X[i])) - ((Y[i] - Y[i-1])/(X[i] - X[i-1]));
     101        u[i]   = ((6.0 * u[i] / (X[i+1] - X[i-1])) - (dX * u[i-1])) / dY;
    99102
    100103        psTrace("psLib.math", 6, "X[%d] is %f\n", i, X[i]);
    … …  
    103106    }
    104107
    105     psF32 qn = 0.5;
    106     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]));
    107     derivs2[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * derivs2[n-2]) + 1.0);
    108 
    109     for (psS32 k=(n-2);k>=0;k--) {
    110         derivs2[k] = derivs2[k] * derivs2[k+1] + u[k];
     108    if (isfinite(dyUpper)) {
     109      psF32 qn = 0.5;
     110      u[n-1] = (3.0/(X[n-1]-X[n-2])) * (dyUpper - (Y[n-1]-Y[n-2])/(X[n-1]-X[n-2]));
     111      d2y[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * d2y[n-2]) + 1.0);
     112    } else {
     113      d2y[n-1] = 0;
     114    }
     115
     116    for (psS32 k = n-2; k >= 0; k--) {
     117        d2y[k] = d2y[k] * d2y[k+1] + u[k];
    111118        psTrace("psLib.math", 6, "derivs2[%d] is %f\n", k, derivs2[k]);
    112119    }
    113120    psFree(u);
    114121    psTrace("psLib.math", 4, "---- %s() end ----\n", __func__);
    115     return(derivs2);
     122
     123    return(d2y);
    116124}
    117125
    … …  
    124132{
    125133    PS_ASSERT_PTR(ptr, false);
    126     return ( psMemGetDeallocator(ptr) == (psFreeFunc)spline1DFree );
     134    return ( psMemGetDeallocator(ptr) == (psFreeFunc) psSpline1DFree );
    127135}
    128136
    … …  
    130138{
    131139    psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
     140
    132141    tmpSpline->n = 0;
    133     tmpSpline->spline = NULL;
    134     tmpSpline->knots = NULL;
    135     tmpSpline->p_psDeriv2 = NULL;
    136     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);
    137151
    138152    return(tmpSpline);
    … …  
    140154
    141155/*****************************************************************************
    142 psSpline1DFitVector(): given a set of x/y vectors, this routine generates the
     156psSpline1DFitVector(): given a set of x,y vectors, this routine generates the
    143157linear or cublic splines which satisfy those data points.
    144158
    … …  
    161175psSpline1D *psSpline1DFitVector(
    162176    const psVector* x,                  ///< Ordinates.
    163     const psVector* y)                  ///< Coordinates.
     177    const psVector* y,                  ///< Coordinates.
     178    psF32 dyLower,                      ///< 1st derivative at lower bound
     179    psF32 dyUpper)                      ///< 1st derivative at upper bound
    164180{
    165181    psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__);
     182    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    166183    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
     184    PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL);
     185    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    167186    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    168187    PS_ASSERT_LONG_LARGER_THAN_OR_EQUAL(y->n, (long)2, NULL);
    169     psS32 numSplines = (y->n)-1;
    170     psTrace("psLib.math", 5, "numSplines is %d\n", numSplines);
    171 
    172     //
    173     // Create the psSpline1D struct.
    174     //
     188
    175189    psSpline1D *spline = psSpline1DAlloc();
    176     spline->n = numSplines;
    177     spline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    178     for (psS32 i=0;i<numSplines;i++) {
    179         spline->spline[i] = psPolynomial1DAlloc(PS_POLYNOMIAL_ORD, 3);
    180     }
    181 
    182     //
    183     // The following code ensures that xPtr and yPtr points to a psF32 psVector.
    184     //
    185     // XXX: Use the vector copy and create routines here:
    186     //
    187 
    188     spline->knots = psVectorAlloc(y->n, PS_TYPE_F32);
    189     if (x != NULL) {
    190         PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    191         PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL);
    192         PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    193         if (x->type.type == PS_TYPE_F32) {
    194             for (psS32 i = 0 ; i < x->n ; i++) {
    195                 spline->knots->data.F32[i] = x->data.F32[i];
    196             }
    197         } else if (x->type.type == PS_TYPE_F64) {
    198             for (psS32 i = 0 ; i < x->n ; i++) {
    199                 spline->knots->data.F32[i] = (psF32) x->data.F64[i];
    200             }
    201         }
    202     } else {
    203         for (psS32 i = 0 ; i < y->n ; i++) {
    204             spline->knots->data.F32[i] = (psF32) i;
    205         }
    206     }
    207     psVector *xPtr = spline->knots;
    208 
    209     psVector *yPtr = NULL;
    210     // Convert y to F32 if necessary.
    211     if (PS_TYPE_F64 == y->type.type) {
    212         yPtr = psVectorCopy(NULL, y, PS_TYPE_F32);
    213     } else {
    214         yPtr = (psVector *) y;
    215     }
    216 
    217     //
     190    spline->n = y->n; // number of knots
     191
     192    spline->xKnots   = (psF32 *) psAlloc( spline->n * sizeof(psF32));
     193    spline->yKnots   = (psF32 *) psAlloc( spline->n * sizeof(psF32));
     194    spline->d2yKnots = (psF32 *) psAlloc( spline->n * sizeof(psF32));
     195
     196    // x & y can both be F32 or F64. should knots be F64?
     197    for (psS32 i = 0 ; i < spline->n ; i++) {
     198        spline->xKnots[i] = (x->type.type == PS_TYPE_F32) ? x->data.F32[i] : x->data.F64[i];
     199        spline->yKnots[i] = (y->type.type == PS_TYPE_F32) ? y->data.F32[i] : y->data.F64[i];
     200    }
     201
    218202    // Generate the second derivatives at each data point.
    219     //
    220     spline->p_psDeriv2 = calculateSecondDerivs(xPtr, yPtr);
    221 
    222     //
    223     // We generate the coefficients of the spline polynomials.  I can't
    224     // concisely explain how this code works.  See above function comments
    225     // and Numerical Recipes in C.
    226     //
    227     for (psS32 i=0 ; i < numSplines ; i++) {
    228         psF32 H = xPtr->data.F32[i+1] - xPtr->data.F32[i];
    229         if (fabs(H) <= FLT_EPSILON) {
    230             psError(PS_ERR_UNKNOWN, false, "x data points are not distinct (%d %d) (%f %f).\n",
    231                     i, i+1, xPtr->data.F32[i], xPtr->data.F32[i+1]);
    232         }
    233         psTrace("psLib.math", 6, "x data (%f - %f) (%f)\n", xPtr->data.F32[i], xPtr->data.F32[i+1], H);
    234         //
    235         // ******** Calculate 0-order term ********
    236         //
    237         // From (1)
    238         spline->spline[i]->coeff[0] = yPtr->data.F32[i] * xPtr->data.F32[i+1]/H;
    239         // From (2)
    240         spline->spline[i]->coeff[0]-= (yPtr->data.F32[i+1] * xPtr->data.F32[i])/H;
    241         // From (3)
    242         psF32 tmp = (xPtr->data.F32[i+1] * xPtr->data.F32[i+1] * xPtr->data.F32[i+1]) / (H * H * H);
    243         tmp-= xPtr->data.F32[i+1] / H;
    244         tmp*= spline->p_psDeriv2[i] * H * H / 6.0;
    245         spline->spline[i]->coeff[0]+= tmp;
    246         // From (4)
    247         tmp = -(xPtr->data.F32[i] * xPtr->data.F32[i] * xPtr->data.F32[i]) / (H * H * H);
    248         tmp+= xPtr->data.F32[i] / H;
    249         tmp*= spline->p_psDeriv2[i+1] * H * H / 6.0;
    250         spline->spline[i]->coeff[0]+= tmp;
    251 
    252         //
    253         // ******** Calculate 1-order term ********
    254         //
    255         // From (1)
    256         spline->spline[i]->coeff[1] = -(yPtr->data.F32[i]) / H;
    257         // From (2)
    258         spline->spline[i]->coeff[1]+= yPtr->data.F32[i+1] / H;
    259         // From (3)
    260         tmp = -3.0 * xPtr->data.F32[i+1] * xPtr->data.F32[i+1] / (H * H * H);
    261         tmp+= (1.0 / H);
    262         tmp*= spline->p_psDeriv2[i] * H * H / 6.0;
    263         spline->spline[i]->coeff[1]+= tmp;
    264         // From (4)
    265         tmp = 3.0 * xPtr->data.F32[i] * xPtr->data.F32[i] / (H * H * H);
    266         tmp-= 1.0 / H;
    267         tmp*= spline->p_psDeriv2[i+1] * H * H / 6.0;
    268         spline->spline[i]->coeff[1]+= tmp;
    269 
    270         //
    271         // ******** Calculate 2-order term ********
    272         //
    273         // From (3)
    274         spline->spline[i]->coeff[2] = spline->p_psDeriv2[i] * 3.0 * xPtr->data.F32[i+1] / (6.0 * H);
    275         // From (4)
    276         spline->spline[i]->coeff[2]-= spline->p_psDeriv2[i+1] * 3.0 * xPtr->data.F32[i] / (6.0 * H);
    277 
    278         //
    279         // ******** Calculate 3-order term ********
    280         //
    281         // From (3)
    282         spline->spline[i]->coeff[3] = -spline->p_psDeriv2[i] / (6.0 * H);
    283         // From (4)
    284         spline->spline[i]->coeff[3]+=  spline->p_psDeriv2[i+1] / (6.0 * H);
    285 
    286         psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[0] is %f\n", i, spline->spline[i]->coeff[0]);
    287         psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[1] is %f\n", i, spline->spline[i]->coeff[1]);
    288         psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[2] is %f\n", i, spline->spline[i]->coeff[2]);
    289         psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[3] is %f\n", i, spline->spline[i]->coeff[3]);
    290     }
    291 
    292     if (PS_TYPE_F64 == y->type.type) {
    293         psFree(yPtr);
    294     }
     203    spline->d2yKnots = calculateSecondDerivs(spline, dyLower, dyUpper);
     204
    295205    psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    296206    return(spline);
    297207}
    298208
    299 
    300 /*****************************************************************************
    301 psSpline1DEval(): this routine takes an existing spline of arbitrary order
    302 and an independent x value.  It determines which spline that x corresponds
    303 to by doing a bracket disection on the knots of the spline data structure
    304 (vectorBinDisectF32()).  Then it evaluates the spline at that x location
    305 by a call to the 1D polynomial functions.
    306 
    307 XXX: The spline eval functions require input and output to be F32.  however
    308      the spline fit functions require F32 and F64.
    309 
    310 XXX: This only works if spline->knots if psF32.  Must we add support for psU32 and
    311 psF64?
    312  *****************************************************************************/
    313 float psSpline1DEval_Old(
    314     const psSpline1D_Old *spline,
     209psPolynomial1D *psSpline1DToPoly (psSpline1D *spline, int n) {
     210    PS_ASSERT_INT_LESS_THAN(n, spline->n - 1, NULL);
     211
     212    // convert the cubic spline coeffs to a polynomial. See above function comments and
     213    // Numerical Recipes in C.
     214
     215    psF32 *xKnots   = spline->xKnots;
     216    psF32 *yKnots   = spline->yKnots;
     217    psF32 *d2yKnots = spline->d2yKnots;
     218
     219    psF32 H = xKnots[n+1] - xKnots[n];
     220    if (fabs(H) <= FLT_EPSILON) {
     221        psError(PS_ERR_UNKNOWN, false, "x data points are not distinct (%d %d) (%f %f).\n",
     222                n, n+1, xKnots[n], xKnots[n+1]);
     223    }
     224
     225    psPolynomial1D *myPoly = psPolynomial1DAlloc(PS_POLYNOMIAL_ORD, 3);
     226
     227    // ******** Calculate 0-order term ********
     228    // From (1)
     229    myPoly->coeff[0] = yKnots[n] * xKnots[n+1]/H;
     230    // From (2)
     231    myPoly->coeff[0]-= (yKnots[n+1] * xKnots[n])/H;
     232    // From (3)
     233    psF32 tmp = (xKnots[n+1] * xKnots[n+1] * xKnots[n+1]) / (H * H * H);
     234    tmp-= xKnots[n+1] / H;
     235    tmp*= d2yKnots[n] * H * H / 6.0;
     236    myPoly->coeff[0]+= tmp;
     237    // From (4)
     238    tmp = -(xKnots[n] * xKnots[n] * xKnots[n]) / (H * H * H);
     239    tmp+= xKnots[n] / H;
     240    tmp*= d2yKnots[n+1] * H * H / 6.0;
     241    myPoly->coeff[0]+= tmp;
     242
     243    //
     244    // ******** Calculate 1-order term ********
     245    //
     246    // From (1)
     247    myPoly->coeff[1] = -(yKnots[n]) / H;
     248    // From (2)
     249    myPoly->coeff[1]+= yKnots[n+1] / H;
     250    // From (3)
     251    tmp = -3.0 * xKnots[n+1] * xKnots[n+1] / (H * H * H);
     252    tmp+= (1.0 / H);
     253    tmp*= d2yKnots[n] * H * H / 6.0;
     254    myPoly->coeff[1]+= tmp;
     255    // From (4)
     256    tmp = 3.0 * xKnots[n] * xKnots[n] / (H * H * H);
     257    tmp-= 1.0 / H;
     258    tmp*= d2yKnots[n+1] * H * H / 6.0;
     259    myPoly->coeff[1]+= tmp;
     260
     261    //
     262    // ******** Calculate 2-order term ********
     263    //
     264    // From (3)
     265    myPoly->coeff[2] = d2yKnots[n] * 3.0 * xKnots[n+1] / (6.0 * H);
     266    // From (4)
     267    myPoly->coeff[2]-= d2yKnots[n+1] * 3.0 * xKnots[n] / (6.0 * H);
     268
     269    //
     270    // ******** Calculate 3-order term ********
     271    //
     272    // From (3)
     273    myPoly->coeff[3] = -d2yKnots[n] / (6.0 * H);
     274    // From (4)
     275    myPoly->coeff[3]+=  d2yKnots[n+1] / (6.0 * H);
     276
     277    psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[0] is %f\n", i, myPoly->coeff[0]);
     278    psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[1] is %f\n", i, myPoly->coeff[1]);
     279    psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[2] is %f\n", i, myPoly->coeff[2]);
     280    psTrace("psLib.math", 6, "(spline->spline[%u])->coeff[3] is %f\n", i, myPoly->coeff[3]);
     281
     282    return myPoly;
     283}
     284
     285// given an already-constructed spline, check/assert that the
     286// knot spacing is equal.  this allows some optimization
     287bool psSpline1DisEqualSpacing (psSpline1D *spline) {
     288
     289    PS_ASSERT_PTR_NON_NULL(spline, false);
     290    PS_ASSERT_PTR_NON_NULL(spline->xKnots, false);
     291    PS_ASSERT_PTR_NON_NULL(spline->yKnots, false);
     292    PS_ASSERT_PTR_NON_NULL(spline->d2yKnots, false);
     293   
     294    // if the spline has equally-spaced xKnots, the values of the
     295    // xKnots can be predicted from the first, last, and delta values
     296
     297    int n = spline->n;
     298    spline->xMax = spline->xKnots[n-1];
     299    spline->xMin = spline->xKnots[0];
     300    spline->xDel = (spline->xMax - spline->xMin) / (n - 1);
     301   
     302    // check that the xKnots actually follow this spacing:
     303
     304    for (int i = 1; i < n - 1; i++) {
     305        float xValue = spline->xMin + i*spline->xDel;
     306        fprintf (stderr, "%d %f - %f = %f\n", i, spline->xKnots[i], xValue, spline->xKnots[i] - xValue);
     307    }
     308
     309    spline->equalSpacing = true;
     310    return true;
     311}
     312
     313// XXX EAM : changing implementation to use yKnot, d2yKnot instead of polynomials
     314float psSpline1DEval(
     315    const psSpline1D *spline,
    315316    float x)
    316317{
    … …  
    318319    PS_ASSERT_PTR_NON_NULL(spline, NAN);
    319320    PS_ASSERT_INT_NONNEGATIVE(spline->n, NAN);
    320     PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NAN);
     321    PS_ASSERT_PTR_NON_NULL(spline->xKnots, NAN);
    321322
    322323    psS32 n = spline->n;
    323     if ((x < spline->knots->data.F32[0]) || (x > spline->knots->data.F32[spline->knots->n-1])) {
    324         // If x is outside the range of spline->knots, generate a warning
    325         // message, then return the left, or right, endpoint.
    326         psLogMsg(__func__, PS_LOG_WARN,
    327                  "psSpline1DEval(): x ordinate (%f) is outside the spline range (%f - %f) (%d).",
    328                  x, spline->knots->data.F32[0], spline->knots->data.F32[n-1], n);
    329 
    330         psS32 binNum = (x < spline->knots->data.F32[0]) ? 0 : n-1;
    331         psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    332         return(psPolynomial1DEval(spline->spline[binNum], x));
    333     }
    334 
    335     psScalar tmpScalar;
    336     tmpScalar.type.type = PS_TYPE_F32;
    337     tmpScalar.data.F32 = x;
    338     psVectorBinaryDisectResult result;
    339     psS32 binNum = psVectorBinaryDisect(&result, spline->knots, &tmpScalar);
    340     if (result != PS_BINARY_DISECT_PASS) {
    341         psError(PS_ERR_UNKNOWN, false, "Could not perform bin dissection on spline->knots.\n");
    342         return(NAN);
    343     }
    344 
    345     psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    346     return(psPolynomial1DEval(spline->spline[binNum], x));
    347 }
    348 
    349 // XXX EAM : changing implementation to use yKnot, d2yKnot instead of polynomials
    350 float psSpline1DEval_New(
     324
     325    // XXX this should be linear extrapolation at the high or low ends
     326    if (x < spline->xKnots[0])   return psSpline1DEval_Segment(spline,   0, x);
     327    if (x > spline->xKnots[n-1]) return psSpline1DEval_Segment(spline, n-2, x);
     328
     329    if (spline->equalSpacing) {
     330        int bin = (x - spline->xMin) / spline->xDel;
     331        bin = PS_MIN(PS_MAX(bin, 0), n - 2);
     332        return psSpline1DEval_Segment(spline, bin, x);
     333    }
     334
     335    /* find correct element in array (x must be sorted) */
     336    int lo = 0;
     337    int hi = n-1;
     338    while (hi - lo > 1) {
     339        int i = 0.5*(hi+lo);
     340        if (spline->xKnots[i] > x) {
     341            hi = i;
     342        } else {
     343            lo = i;
     344        }
     345    }
     346
     347    return psSpline1DEval_Segment(spline, lo, x);
     348}
     349
     350// evaluate the spline at the given coordinate using the specified segment
     351// (YMMV if you use the wrong segment!)
     352float psSpline1DEval_Segment(
    351353    const psSpline1D *spline,
     354    int n,
    352355    float x)
    353356{
    354357    psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__);
    355358    PS_ASSERT_PTR_NON_NULL(spline, NAN);
     359    PS_ASSERT_PTR_NON_NULL(spline->xKnots, NAN);
     360    PS_ASSERT_PTR_NON_NULL(spline->yKnots, NAN);
     361    PS_ASSERT_PTR_NON_NULL(spline->d2yKnots, NAN);
    356362    PS_ASSERT_INT_NONNEGATIVE(spline->n, NAN);
    357     PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NAN);
    358 
    359     psS32 n = spline->n;
    360     if ((x < spline->knots->data.F32[0]) || (x > spline->knots->data.F32[spline->knots->n-1])) {
    361         // If x is outside the range of spline->knots, generate a warning
    362         // message, then return the left, or right, endpoint.
    363         psLogMsg(__func__, PS_LOG_WARN,
    364                  "psSpline1DEval(): x ordinate (%f) is outside the spline range (%f - %f) (%d).",
    365                  x, spline->knots->data.F32[0], spline->knots->data.F32[n-1], n);
    366 
    367         psS32 binNum = (x < spline->knots->data.F32[0]) ? 0 : n-1;
    368         psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    369         return(psPolynomial1DEval(spline->spline[binNum], x));
    370     }
    371 
    372     psScalar tmpScalar;
    373     tmpScalar.type.type = PS_TYPE_F32;
    374     tmpScalar.data.F32 = x;
    375     psVectorBinaryDisectResult result;
    376     psS32 binNum = psVectorBinaryDisect(&result, spline->knots, &tmpScalar);
    377     if (result != PS_BINARY_DISECT_PASS) {
    378         psError(PS_ERR_UNKNOWN, false, "Could not perform bin dissection on spline->knots.\n");
    379         return(NAN);
    380     }
    381 
    382     psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    383     return(psPolynomial1DEval(spline->spline[binNum], x));
    384 }
    385 
     363    PS_ASSERT_INT_LESS_THAN(n, spline->n - 1, NAN);
     364
     365    psF32 dX = spline->xKnots[n+1] - spline->xKnots[n];
     366    psF32 A  = (spline->xKnots[n+1] - x) / dX;
     367    psF32 B  = (x - spline->xKnots[n]) / dX;
     368
     369    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;
     370    return value;
     371}
    386372
    387373/*****************************************************************************
     374 returns a vector of the same type as the input (x)
    388375 *****************************************************************************/
    389376psVector *psSpline1DEvalVector(
    … …  
    392379{
    393380    psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__);
    394     PS_ASSERT_PTR_NON_NULL(spline, NULL);
    395     PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL);
     381    PS_ASSERT_PTR_NON_NULL(spline,           NULL);
     382    PS_ASSERT_PTR_NON_NULL(spline->xKnots,   NULL);
     383    PS_ASSERT_PTR_NON_NULL(spline->yKnots,   NULL);
     384    PS_ASSERT_PTR_NON_NULL(spline->d2yKnots, NULL);
     385    PS_ASSERT_INT_NONNEGATIVE(spline->n,     NULL);
     386
    396387    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    397388    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    398     if (psTraceGetLevel("psLib.math") >= 6) {
    399         PS_VECTOR_PRINT_F32(x);
    400         PS_PRINT_SPLINE2((psSpline1D *) spline);
    401     }
    402 
    403     psVector *tmpVector = psVectorAlloc(x->n, PS_TYPE_F32);
     389
     390    psVector *tmpVector = psVectorAlloc(x->n, x->type.type);
    404391    if (x->type.type == PS_TYPE_F32) {
    405392        for (psS32 i=0;i<x->n;i++) {
    406393            tmpVector->data.F32[i] = psSpline1DEval(spline, x->data.F32[i]);
    407394        }
    408     } else if (x->type.type == PS_TYPE_F64) {
     395    } else {
    409396        for (psS32 i=0;i<x->n;i++) {
    410             tmpVector->data.F32[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]);
     397            tmpVector->data.F64[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]);
    411398        }
    412399    }
    413 
     400   
    414401    psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    415402    return(tmpVector);
Note: See TracChangeset for help on using the changeset viewer.