IPP Software Navigation Tools IPP Links Communication Pan-STARRS Links

Ignore:
Timestamp:
Jan 30, 2023, 9:51:20 AM (3 years ago)
Author:
eugene
Message:

merge from branches/eam_branches/psLib.20230123: rework of psSpline (cleaner names, do not convert to generic polynomials, allow defined 1st deriv or 0.0 2nd deriv for boundary conditions)

Location:
trunk/psLib
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/psLib

  • trunk/psLib/src/math/psSpline.c

    r23486 r42336  
    11/** @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
    123*/
    134
     
    167#endif
    178
    18 /*****************************************************************************/
    19 /*  INCLUDE FILES                                                            */
    20 /*****************************************************************************/
    219#include <stdio.h>
    2210#include <stdbool.h>
     
    3624#include "psMathUtils.h"
    3725
    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);
     26static void psSpline1DFree(psSpline1D *tmpSpline)
     27{
     28    if (tmpSpline == NULL) return;
     29
     30    psFree(tmpSpline->xKnots);
     31    psFree(tmpSpline->yKnots);
     32    psFree(tmpSpline->d2yKnots);
    7633
    7734    return;
    7835}
    7936
    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/*
    9138static void PS_PRINT_SPLINE2(psSpline1D *mySpline)
    9239{
    9340    printf("-------------- PS_PRINT_SPLINE2() --------------\n");
    9441    printf("mySpline->n is %d\n", mySpline->n);
     42    if (!mySpline->xKnots) return;
     43    if (!mySpline->yKnots) return;
     44    if (!mySpline->d2yKnots) return;
    9545    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*/
    10050
    10151/*****************************************************************************
    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?
     52CalculateSecondDerivs(): Given a set of x,y vectors corresponding to the spline knots, this
     53routine calculates the second derivatives of the interpolating cubic splines at those n points.
     54
     55The boundary conditions may be:
     56* first derivative specified OR
     57* second derivatives = 0
     58
     59One or the other condition may be true for each of the upper and lower bounds
     60
     61NOTE: vectors must be F32
     62
     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
    11972 *****************************************************************************/
    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
     74void 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    )                 
    12579{
    12680    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     psS32 n = y->n;
    132     psF32 *u = (psF32 *) psAlloc(n * sizeof(psF32));
    133     psF32 *derivs2 = (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         derivs2[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;
    149103
    150104        psTrace("psLib.math", 6, "X[%d] is %f\n", i, X[i]);
     
    153107    }
    154108
    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]);
    162120    }
    163121    psFree(u);
    164122    psTrace("psLib.math", 4, "---- %s() end ----\n", __func__);
    165     return(derivs2);
     123    return;
    166124}
    167125
     
    174132{
    175133    PS_ASSERT_PTR(ptr, false);
    176     return ( psMemGetDeallocator(ptr) == (psFreeFunc)spline1DFree );
     134    return ( psMemGetDeallocator(ptr) == (psFreeFunc) psSpline1DFree );
    177135}
    178136
     
    180138{
    181139    psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
     140
    182141    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);
    187151
    188152    return(tmpSpline);
    189153}
    190154
     155/** Create an empty 1D spline **/
     156psSpline1D *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
    191175/*****************************************************************************
    192 psVectorFitSpline1D(): given a set of x/y vectors, this routine generates the
     176psSpline1DFitVector(): given a set of x,y vectors, this routine generates the
    193177linear or cublic splines which satisfy those data points.
    194178
     
    209193XXX: What types must be supported?
    210194 *****************************************************************************/
    211 psSpline1D *psVectorFitSpline1D(
     195psSpline1D *psSpline1DFitVector(
    212196    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
    214200{
    215201    psTrace("psLib.math", 3, "---- %s() begin ----\n", __func__);
     202    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    216203    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);
    217206    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    218207    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
    268217    // 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
    345220    psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    346221    return(spline);
    347222}
    348223
    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  *****************************************************************************/
     224psPolynomial1D *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
     302bool 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
    363329float psSpline1DEval(
    364330    const psSpline1D *spline,
     
    368334    PS_ASSERT_PTR_NON_NULL(spline, NAN);
    369335    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);
    371337
    372338    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!)
     367float 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}
    399387
    400388/*****************************************************************************
     389 returns a vector of the same type as the input (x)
    401390 *****************************************************************************/
    402391psVector *psSpline1DEvalVector(
     
    405394{
    406395    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
    409402    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    410403    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);
    417406    if (x->type.type == PS_TYPE_F32) {
    418407        for (psS32 i=0;i<x->n;i++) {
    419408            tmpVector->data.F32[i] = psSpline1DEval(spline, x->data.F32[i]);
    420409        }
    421     } else if (x->type.type == PS_TYPE_F64) {
     410    } else {
    422411        for (psS32 i=0;i<x->n;i++) {
    423             tmpVector->data.F32[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]);
     412            tmpVector->data.F64[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]);
    424413        }
    425414    }
    426 
     415   
    427416    psTrace("psLib.math", 3, "---- %s() end ----\n", __func__);
    428417    return(tmpVector);
Note: See TracChangeset for help on using the changeset viewer.