IPP Software Navigation Tools IPP Links Communication Pan-STARRS Links

Ignore:
Timestamp:
Sep 27, 2005, 1:19:47 PM (21 years ago)
Author:
gusciora
Message:

Incorporates new prototypes for the spline functions.

File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r5102 r5155  
    11/** @file psSpline.c
    22*
    3 *  @brief Contains basic function allocation, deallocation, and evaluation
    4 *         routines.
     3*  @brief Contains basic spline allocation, deallocation, fitting,
     4*         and evaluation routines.
    55*
    66*  This file will hold the functions for allocated, freeing, and evaluating
    77*  splines.
    88*
    9 *  @version $Revision: 1.128 $ $Name: not supported by cvs2svn $
    10 *  @date $Date: 2005-09-23 01:56:54 $
    11 *
    12 *
     9*  @version $Revision: 1.129 $ $Name: not supported by cvs2svn $
     10*  @date $Date: 2005-09-27 23:16:59 $
    1311*
    1412*  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
    … …  
    2321#include <math.h>
    2422
    25 #include "psRandom.h"
    2623#include "psMemory.h"
    2724#include "psVector.h"
    … …  
    4239/* TYPE DEFINITIONS                                                          */
    4340/*****************************************************************************/
    44 static void spline1DFree(psSpline1D *tmpSpline);
    45 static unsigned int vectorBinDisectF32(psF32 *bins,unsigned int numBins,psF32 x);
    46 static unsigned int vectorBinDisectS32(psS32 *bins,unsigned int numBins,psS32 x);
    4741
    4842/*****************************************************************************/
    … …  
    5044/*****************************************************************************/
    5145
    52 // None
    53 
    5446/*****************************************************************************/
    5547/* FILE STATIC VARIABLES                                                     */
    5648/*****************************************************************************/
    5749
    58 // None
    59 
    6050/*****************************************************************************/
    6151/* FUNCTION IMPLEMENTATION - LOCAL                                           */
    6252/*****************************************************************************/
    6353
    64 bool psMemCheckSpline1D(psPtr ptr)
    65 {
    66     return ( psMemGetDeallocator(ptr) == (psFreeFunc)spline1DFree );
    67 }
    68 
    6954static void spline1DFree(psSpline1D *tmpSpline)
    7055{
    71     unsigned int i;
    72 
    7356    if (tmpSpline == NULL) {
    7457        return;
    … …  
    7659
    7760    if (tmpSpline->spline != NULL) {
    78         for (i=0;i<tmpSpline->n;i++) {
     61        for (psS32 i=0;i<tmpSpline->n;i++) {
    7962            psFree((tmpSpline->spline)[i]);
    8063        }
    … …  
    8568        psFree(tmpSpline->p_psDeriv2);
    8669    }
     70
    8771    psFree(tmpSpline->knots);
    8872
    8973    return;
    90 }
    91 
    92 /*****************************************************************************
    93 fullInterpolate1DF32(): This routine will take as input n-element floating
    94 point arrays domain and range, and the x value, assumed to lie with the
    95 domain vector.  It produces as output the (n-1)-order LaGrange interpolated
    96 value of x.
    97  
    98 XXX: do we error check for non-distinct domain values?
    99  *****************************************************************************/
    100 #define FUNC_MACRO_FULL_INTERPOLATE_1D(TYPE) \
    101 static psF32 fullInterpolate1D##TYPE(ps##TYPE *domain, \
    102                                      ps##TYPE *range, \
    103                                      unsigned int n, \
    104                                      ps##TYPE x) \
    105 { \
    106     \
    107     unsigned int i; \
    108     unsigned int m; \
    109     static psVector *p = NULL; \
    110     p = psVectorRecycle(p, n, PS_TYPE_##TYPE); \
    111     p_psMemSetPersistent(p, true); \
    112     p_psMemSetPersistent(p->data.TYPE, true); \
    113     \
    114     psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 4, \
    115             "---- fullInterpolate1D##TYPE() begin (%u-order at x=%f) (%d data points)----\n", n-1, x, n); \
    116     \
    117     for (i=0;i<n;i++) { \
    118         psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 6, \
    119                 "domain/range is (%f %f)\n", domain[i], range[i]); \
    120     } \
    121     \
    122     for (i=0;i<n;i++) { \
    123         p->data.TYPE[i] = range[i]; \
    124         psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 6, \
    125                 "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \
    126         \
    127     } \
    128     \
    129     /* From NR, during each iteration of the m loop, we are computing the \
    130        p_{i ... i+m} terms. \
    131     */ \
    132     for (m=1;m<n;m++) { \
    133         for (i=0;i<n-m;i++) { \
    134             /* From NR: we are computing P_{i ... i+m} \
    135              */ \
    136             p->data.TYPE[i] = (((x-domain[i+m]) * p->data.TYPE[i]) + \
    137                                ((domain[i]-x) * p->data.TYPE[i+1])) / \
    138                               (domain[i] - domain[i+m]); \
    139             /*printf("((%f-%f * %f) + (%f-%f * %f)) / (%f - %f)\n", x, domain[i+m], p->data.TYPE[i], domain[i], x, p->data.TYPE[i+1], domain[i], domain[i+m]); \
    140              */ \
    141             psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 6, \
    142                     "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \
    143         } \
    144     } \
    145     psTrace(".psLib.dataManip.psSpline.fullInterpolate1D##TYPE", 4, \
    146             "---- fullInterpolate1D##TYPE() end ----\n"); \
    147     \
    148     return(p->data.TYPE[0]); \
    149 } \
    150 
    151 /*
    152 FUNC_MACRO_FULL_INTERPOLATE_1D(U8)
    153 FUNC_MACRO_FULL_INTERPOLATE_1D(U16)
    154 FUNC_MACRO_FULL_INTERPOLATE_1D(U32)
    155 FUNC_MACRO_FULL_INTERPOLATE_1D(U64)
    156 FUNC_MACRO_FULL_INTERPOLATE_1D(S8)
    157 FUNC_MACRO_FULL_INTERPOLATE_1D(S16)
    158 FUNC_MACRO_FULL_INTERPOLATE_1D(S32)
    159 FUNC_MACRO_FULL_INTERPOLATE_1D(S64)
    160 FUNC_MACRO_FULL_INTERPOLATE_1D(F64)
    161 */
    162 FUNC_MACRO_FULL_INTERPOLATE_1D(F32)
    163 
    164 
    165 /*****************************************************************************
    166 interpolate1DF32(): this is the base 1-D flat memory routine to perform
    167 LaGrange interpolation.
    168  *****************************************************************************/
    169 static psF32 interpolate1DF32(psF32 *domain,
    170                               psF32 *range,
    171                               unsigned int n,
    172                               unsigned int order,
    173                               psF32 x)
    174 {
    175     PS_ASSERT_PTR_NON_NULL(domain, NAN)
    176     PS_ASSERT_PTR_NON_NULL(range, NAN)
    177     // XXX: Check valid values for n, order, and x?
    178 
    179     psS32 binNum;
    180     unsigned int numIntPoints = order+1;
    181     psS32 origin;
    182 
    183     psTrace(".psLib.dataManip.psSpline.interpolate1DF32", 4,
    184             "---- interpolate1DF32() begin ----\n");
    185 
    186     binNum = vectorBinDisectF32(domain, n, x);
    187 
    188     if (0 == numIntPoints%2) {
    189         origin = binNum - ((numIntPoints/2) - 1);
    190     } else {
    191         origin = binNum - (numIntPoints/2);
    192         if ((x-domain[binNum]) > (domain[binNum+1]-x)) {
    193             // x is closer to binNum+1.
    194             origin = 1 + (binNum - (numIntPoints/2));
    195         }
    196     }
    197     if (origin < 0) {
    198         origin = 0;
    199     }
    200     if ((origin + numIntPoints) > n) {
    201         origin = n - numIntPoints;
    202     }
    203 
    204     psTrace(".psLib.dataManip.psSpline.interpolate1DF32", 4,
    205             "---- interpolate1DF32() end ----\n");
    206     return(fullInterpolate1DF32(&domain[origin], &range[origin], order+1, x));
    20774}
    20875
    … …  
    22289XXX: do an F64 version?
    22390 *****************************************************************************/
    224 static psF32 *calculateSecondDerivs(const psVector* x,        ///< Ordinates (or NULL to just use the in!
     91static psF32 *calculateSecondDerivs(const psVector* x,        ///< Ordinates
    22592                                    const psVector* y)        ///< Coordinates
    22693{
    227     psTrace(".psLib.dataManip.calculateSecondDerivs", 4,
    228             "---- calculateSecondDerivs() begin ----\n");
    229 
    230     psS32 i;
    231     psS32 k;
    232     psF32 sig;
    233     psF32 p;
     94    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
     95    if (psTraceGetLevel(__func__) >= 6) {
     96        PS_VECTOR_PRINT_F32(x);
     97        PS_VECTOR_PRINT_F32(y);
     98    }
    23499    psS32 n = y->n;
    235100    psF32 *u = (psF32 *) psAlloc(n * sizeof(psF32));
    … …  
    237102    psF32 *X = (psF32 *) & (x->data.F32[0]);
    238103    psF32 *Y = (psF32 *) & (y->data.F32[0]);
    239     psF32 qn;
    240 
     104    //
    241105    // XXX: The second derivatives at the endpoints, undefined in the SDR,
    242106    // are set in psConstants.h: PS_LEFT_SPLINE_DERIV, PS_RIGHT_SPLINE_DERIV.
     107    //
    243108    derivs2[0] = -0.5;
    244109    u[0]= (3.0/(X[1]-X[0])) * ((Y[1]-Y[0])/(X[1]-X[0]) - PS_LEFT_SPLINE_DERIV);
    245110
    246     for (i=1;i<=(n-2);i++) {
    247         sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]);
    248         p = sig * derivs2[i-1] + 2.0;
     111    for (psS32 i=1;i<=(n-2);i++) {
     112        psF32 sig = (X[i] - X[i-1]) / (X[i+1] - X[i-1]);
     113        psF32 p = sig * derivs2[i-1] + 2.0;
    249114        derivs2[i] = (sig - 1.0) / p;
    250115        u[i] = ((Y[i+1] - Y[i])/(X[i+1]-X[i])) - ((Y[i]-Y[i-1])/(X[i]-X[i-1]));
    251116        u[i] = ((6.0 * u[i] / (X[i+1] - X[i-1])) - (sig * u[i-1])) / p;
    252117
    253         psTrace(".psLib.dataManip.calculateSecondDerivs", 6,
    254                 "X[%d] is %f\n", i, X[i]);
    255         psTrace(".psLib.dataManip.calculateSecondDerivs", 6,
    256                 "Y[%d] is %f\n", i, Y[i]);
    257         psTrace(".psLib.dataManip.calculateSecondDerivs", 6,
    258                 "u[%d] is %f\n", i, u[i]);
    259     }
    260 
    261     qn = 0.5;
     118        psTrace(__func__, 6, "X[%d] is %f\n", i, X[i]);
     119        psTrace(__func__, 6, "Y[%d] is %f\n", i, Y[i]);
     120        psTrace(__func__, 6, "u[%d] is %f\n", i, u[i]);
     121    }
     122
     123    psF32 qn = 0.5;
    262124    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]));
    263125    derivs2[n-1] = (u[n-1] - (qn * u[n-2])) / ((qn * derivs2[n-2]) + 1.0);
    264126
    265     for (k=(n-2);k>=0;k--) {
     127    for (psS32 k=(n-2);k>=0;k--) {
    266128        derivs2[k] = derivs2[k] * derivs2[k+1] + u[k];
    267 
    268         psTrace(".psLib.dataManip.calculateSecondDerivs", 6,
    269                 "derivs2[%d] is %f\n", k, derivs2[k]);
     129        psTrace(__func__, 6, "derivs2[%d] is %f\n", k, derivs2[k]);
    270130    }
    271131    psFree(u);
    272     psTrace(".psLib.dataManip.calculateSecondDerivs", 4,
    273             "---- calculateSecondDerivs() end ----\n");
    274 
     132    psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    275133    return(derivs2);
    276134}
    277135
    278136
    279 
    280 /*****************************************************************************/
    281 /* FUNCTION IMPLEMENTATION - PUBLIC                                          */
    282 /*****************************************************************************/
    283 
    284 /*****************************************************************************
    285 psVectorFitSpline1D(): given a psSpline1D data structure and a set of x/y
    286 vectors, this routine generates the linear or cublic splines which satisfy
    287 those data points.
    288  
    289 The formula for calculating the spline polynomials is derived from Numerical
    290 Recipes in C.  The basic idea is that the polynomial is
    291  (1)     y = (A * y[0]) +
    292  (2)         (B * y[1]) +
    293  (3)         ((((A*A*A)-A) * mySpline->p_psDeriv2[0]) * H^2)/6.0 +
    294  (4)         ((((B*B*B)-B) * mySpline->p_psDeriv2[1]) * H^2)/6.0
    295 Where:
    296  H = x[1]-x[0]
    297  A = (x[1]-x)/H
    298  B = (x-x[0])/H
    299 The bulk of the code in this routine is the expansion of the above equation
    300 into a polynomial in terms of x, and then saving the coefficients of the
    301 powers of x in the spline polynomials.  This gets pretty complicated.
    302  
    303 XXX: usage of yErr is not specified in IfA documentation.
    304  
    305 XXX: Is the x argument redundant?  What do we do if the x argument is
    306 supplied, but does not equal the knots specified in mySpline?
    307  
    308 XXX: can psSpline be NULL?
    309  
    310 XXX: reimplement this assuming that mySpline is NULL?
    311  
    312 XXX: What happens if X is NULL, then an index vector is generated for X, but
    313 that index vector lies outside the range vectors in mySpline?
    314  
    315 XXX: Assumes mySpline->knots is psF32.  Must add psU32 and psF64.
    316  *****************************************************************************/
    317 psSpline1D *psVectorFitSpline1D(psSpline1D *spline,     ///< The spline which will be generated.
    318                                 const psVector* x,        ///< Ordinates (or NULL to just use the indices)
    319                                 const psVector* y,        ///< Coordinates
    320                                 const psVector* yErr)     ///< Errors in coordinates, or NULL
    321 {
    322     PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    323     PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    324     if (spline != NULL) {
    325         PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL);
    326     }
    327     psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
    328             "---- psVectorFitSpline1D() begin ----\n");
    329     unsigned int numSplines = (y->n)-1;
    330     psF32 tmp;
    331     psF32 H;
    332     unsigned int i;
    333     psF32 slope;
    334     psVector *x32 = NULL;
    335     psVector *y32 = NULL;
    336     psVector *yErr32 = NULL;
    337     static psVector *x32Static = NULL;
    338     static psVector *y32Static = NULL;
    339     static psVector *yErr32Static = NULL;
    340 
    341     PS_VECTOR_CONVERT_F64_TO_F32_STATIC(y, y32, y32Static);
    342 
    343     // If yErr==NULL, set all errors equal.
    344     if (yErr == NULL) {
    345         PS_VECTOR_GEN_YERR_STATIC_F32(yErr32Static, y->n);
    346         yErr32 = yErr32Static;
    347     } else {
    348         PS_ASSERT_VECTOR_TYPE_F32_OR_F64(yErr, NULL);
    349         PS_VECTOR_CONVERT_F64_TO_F32_STATIC(yErr, yErr32, yErr32Static);
    350     }
    351 
    352     // If x==NULL, create an x32 vector with x values set to (0:n).
    353     if (x == NULL) {
    354         PS_VECTOR_GEN_X_INDEX_STATIC_F32(x32Static, y->n);
    355         x32 = x32Static;
    356     } else {
    357         PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    358         PS_VECTOR_CONVERT_F64_TO_F32_STATIC(x, x32, x32Static);
    359     }
    360     PS_ASSERT_VECTORS_SIZE_EQUAL(x32, y32, NULL);
    361     PS_ASSERT_VECTORS_SIZE_EQUAL(yErr32, y32, NULL);
    362 
    363     /*
    364         XXX:
    365         This can not be implemented until SDR states what order spline should be
    366         created.
    367         Should we error if spline is not NULL?
    368         Should we error if mySPline is not NULL?
    369     */
    370     if (spline == NULL) {
    371         spline = psSpline1DAllocGeneric(x32, 3);
    372     }
    373     PS_ASSERT_PTR_NON_NULL(spline, NULL);
    374     PS_ASSERT_INT_NONNEGATIVE(spline->n, NULL);
    375 
    376     if (y32->n != (1 + spline->n)) {
    377         psError(PS_ERR_BAD_PARAMETER_SIZE, true,
    378                 "data size / spline size mismatch (%u %u)\n",
    379                 y32->n, spline->n);
    380         return(NULL);
    381     }
    382 
    383     // If these are linear splines, which means their polynomials will have
    384     // two coefficients, then we do the simple calculation.
    385     if (1 == (spline->spline[0])->COOL_1D_n) {
    386         for (i=0;i<spline->n;i++) {
    387             slope = (y32->data.F32[i+1] - y32->data.F32[i]) /
    388                     (spline->knots->data.F32[i+1] - spline->knots->data.F32[i]);
    389             (spline->spline[i])->coeff[0] = y32->data.F32[i] -
    390                                             (slope * spline->knots->data.F32[i]);
    391 
    392             (spline->spline[i])->coeff[1] = slope;
    393             psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
    394                     "---- spline %u coeffs are (%f, %f)\n", i,
    395                     (spline->spline[i])->coeff[0],
    396                     (spline->spline[i])->coeff[1]);
    397         }
    398         psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
    399                 "---- Exiting psVectorFitSpline1D()()\n");
    400         return((psSpline1D *) spline);
    401     }
    402 
    403     // Check if these are cubic splines (n==4).  If not, psError.
    404     if (3 != (spline->spline[0])->COOL_1D_n) {
    405         psError(PS_ERR_BAD_PARAMETER_SIZE, true,
    406                 "Don't know how to generate %u-order splines.",
    407                 (spline->spline[0])->COOL_1D_n);
    408         return(NULL);
    409     }
    410 
    411     // If we get here, then we know these are cubic splines.  We first
    412     // generate the second derivatives at each data point.
    413     spline->p_psDeriv2 = calculateSecondDerivs(x32, y32);
    414     for (i=0;i<y32->n;i++)
    415         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    416                 "Second deriv[%u] is %f\n", i, spline->p_psDeriv2[i]);
    417 
    418     // We generate the coefficients of the spline polynomials.  I can't
    419     // concisely explain how this code works.  See above function comments
    420     // and Numerical Recipes in C.
    421     for (i=0;i<numSplines;i++) {
    422         H = x32->data.F32[i+1] - x32->data.F32[i];
    423         psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
    424                 "x data (%f - %f) (%f)\n",
    425                 x32->data.F32[i],
    426                 x32->data.F32[i+1], H);
    427         //
    428         // ******** Calculate 0-order term ********
    429         //
    430         // From (1)
    431         (spline->spline[i])->coeff[0] = (y32->data.F32[i] * x32->data.F32[i+1]/H);
    432         // From (2)
    433         ((spline->spline[i])->coeff[0])-= ((y32->data.F32[i+1] * x32->data.F32[i])/H);
    434         // From (3)
    435         tmp = (x32->data.F32[i+1] * x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
    436         tmp-= (x32->data.F32[i+1] / H);
    437         tmp*= (spline->p_psDeriv2)[i] * H * H / 6.0;
    438         ((spline->spline[i])->coeff[0])+= tmp;
    439         // From (4)
    440         tmp = -(x32->data.F32[i] * x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
    441         tmp+= (x32->data.F32[i] / H);
    442         tmp*= (spline->p_psDeriv2)[i+1] * H * H / 6.0;
    443         ((spline->spline[i])->coeff[0])+= tmp;
    444 
    445         //
    446         // ******** Calculate 1-order term ********
    447         //
    448         // From (1)
    449         (spline->spline[i])->coeff[1] = -(y32->data.F32[i]) / H;
    450         // From (2)
    451         ((spline->spline[i])->coeff[1])+= (y32->data.F32[i+1] / H);
    452         // From (3)
    453         tmp = -3.0 * (x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
    454         tmp+= (1.0 / H);
    455         tmp*= ((spline->p_psDeriv2)[i]) * H * H / 6.0;
    456         ((spline->spline[i])->coeff[1])+= tmp;
    457         // From (4)
    458         tmp = 3.0 * (x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
    459         tmp-= (1.0 / H);
    460         tmp*= ((spline->p_psDeriv2)[i+1]) * H * H / 6.0;
    461         ((spline->spline[i])->coeff[1])+= tmp;
    462 
    463         //
    464         // ******** Calculate 2-order term ********
    465         //
    466         // From (3)
    467         (spline->spline[i])->coeff[2] = ((spline->p_psDeriv2)[i]) * 3.0 * x32->data.F32[i+1] / (6.0 * H);
    468         // From (4)
    469         ((spline->spline[i])->coeff[2])-= (((spline->p_psDeriv2)[i+1]) * 3.0 * x32->data.F32[i] / (6.0 * H));
    470 
    471         //
    472         // ******** Calculate 3-order term ********
    473         //
    474         // From (3)
    475         (spline->spline[i])->coeff[3] = -((spline->p_psDeriv2)[i]) / (6.0 * H);
    476         // From (4)
    477         ((spline->spline[i])->coeff[3])+=  ((spline->p_psDeriv2)[i+1]) / (6.0 * H);
    478 
    479         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    480                 "(spline->spline[%u])->coeff[0] is %f\n", i, (spline->spline[i])->coeff[0]);
    481         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    482                 "(spline->spline[%u])->coeff[1] is %f\n", i, (spline->spline[i])->coeff[1]);
    483         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    484                 "(spline->spline[%u])->coeff[2] is %f\n", i, (spline->spline[i])->coeff[2]);
    485         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    486                 "(spline->spline[%u])->coeff[3] is %f\n", i, (spline->spline[i])->coeff[3]);
    487 
    488     }
    489 
    490     psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
    491             "---- psVectorFitSpline1D() end ----\n");
    492     return(spline);
    493 }
    494 
    495 
    496 
    497 
    498 
    499 
    500 
    501 
    502 
    503 /*****************************************************************************
    504 psVectorFitSpline1DNEW(): given a psSpline1D data structure and a set of x/y
    505  
    506 xF32 and yF32 are internal psVectors which are used to hold the psF32 versions
    507 of the input data, if necessary.  xPtr and yPtr are pointers to either xF32 or
    508 the x argument.  All computation is done on xPtr and yPtr.  xF32 and yF32 will
    509 simply be psFree() at the end.
    510  
    511 XXX: nKnots makes no sense.  This number is always equal to the size of the x
    512 an y vectors.
    513  
    514 XXX: How do we specify the spline order?  For now, order=3.
    515  *****************************************************************************/
    516 #define PS_XXX_SPLINE_ORDER 3
    517 psSpline1D *psVectorFitSpline1DNEW(const psVector* x,        ///< Ordinates (or NULL to just use the indices)
    518                                    const psVector* y,        ///< Coordinates
    519                                    unsigned int nKnots)
    520 {
    521     psTrace(".psLib.dataManip.psVectorFitSpline1D", 4, "---- psVectorFitSpline1DNEW() begin ----\n");
    522     PS_ASSERT_VECTOR_NON_NULL(y, NULL);
    523     PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
    524     //    PS_ASSERT_INT_EQUAL(y->n, nKnots);
    525 
    526     //
    527     // The following code ensures that xPtr points to a psF32 version of the
    528     // ordinate data.
    529     //
    530     psVector *xF32 = NULL;
    531     psVector *xPtr = NULL;
    532     if (x != NULL) {
    533         PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL);
    534         if (PS_TYPE_F64 == x->type.type) {
    535             xF32 = psVectorAlloc(y->n, PS_TYPE_F32);
    536             for (unsigned int i = 0 ; i < x->n ; i++) {
    537                 xF32->data.F32[i] = (psF32) x->data.F64[i];
    538             }
    539             xPtr = xF32;
    540         } else if (PS_TYPE_F32 == x->type.type) {
    541             xPtr = (psVector *) x;
    542         } else {
    543             psError(PS_ERR_UNKNOWN, true, "psVector x is wrong type.\n");
    544             return(NULL);
    545         }
    546     } else {
    547         // If x==NULL, create an x32 vector with x values set to (0:n).
    548         xF32 = psVectorAlloc(y->n, PS_TYPE_F32);
    549         for (unsigned int i = 0 ; i < x->n ; i++) {
    550             xF32->data.F32[i] = (psF32) i;
    551         }
    552         xPtr = xF32;
    553     }
    554 
    555     //
    556     // If y is of type psF64, then create a new vector yF32 and convert the
    557     // y elements.  Regardless of y's type, we create a yPtr which will be
    558     // used in the remainder of this function.
    559     //
    560     psVector *yF32 = NULL;
    561     psVector *yPtr = NULL;
    562     if (PS_TYPE_F64 == y->type.type) {
    563         yF32 = psVectorAlloc(y->n, PS_TYPE_F32);
    564         for (unsigned int i = 0 ; i < y->n ; i++) {
    565             yF32->data.F32[i] = (psF32) y->data.F64[i];
    566         }
    567         yPtr = yF32;
    568     } else {
    569         yPtr = (psVector *) y;
    570     }
    571 
    572     psSpline1D *mySpline = psSpline1DAllocGeneric(xPtr, PS_XXX_SPLINE_ORDER);
    573 
    574     unsigned int numSplines = nKnots - 1;
    575     psF32 tmp;
    576     psF32 H;
    577     unsigned int i;
    578     psF32 slope;
    579     // XXX: get rid of x32 and y32 (this is from old code)
    580     psVector *x32 = xPtr;
    581     psVector *y32 = yPtr;
    582 
    583     // If these are linear splines, which means their polynomials will have
    584     // two coefficients, then we do the simple calculation.
    585     if (1 == PS_XXX_SPLINE_ORDER) {
    586         for (i=0;i<mySpline->n;i++) {
    587             slope = (y32->data.F32[i+1] - y32->data.F32[i]) /
    588                     (mySpline->knots->data.F32[i+1] - mySpline->knots->data.F32[i]);
    589             (mySpline->spline[i])->coeff[0] = y32->data.F32[i] -
    590                                               (slope * mySpline->knots->data.F32[i]);
    591 
    592             (mySpline->spline[i])->coeff[1] = slope;
    593             psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
    594                     "---- mySpline %u coeffs are (%f, %f)\n", i,
    595                     (mySpline->spline[i])->coeff[0],
    596                     (mySpline->spline[i])->coeff[1]);
    597         }
    598         psTrace(".psLib.dataManip.psMinimize.psVectorFitSpline1D", 4,
    599                 "---- Exiting psVectorFitSpline1D()()\n");
    600         return((psSpline1D *) mySpline);
    601     }
    602 
    603     //
    604     // Check if these are cubic splines (n==4).  If not, psError.
    605     //
    606     if (3 != PS_XXX_SPLINE_ORDER) {
    607         psError(PS_ERR_BAD_PARAMETER_SIZE, true,
    608                 "Don't know how to generate %d-order splines.", PS_XXX_SPLINE_ORDER);
    609         return(NULL);
    610     }
    611 
    612     //
    613     // If we get here, then we know these are cubic splines.  We first
    614     // generate the second derivatives at each data point.
    615     //
    616     mySpline->p_psDeriv2 = calculateSecondDerivs(x32, y32);
    617     for (i=0;i<y32->n;i++)
    618         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    619                 "Second deriv[%u] is %f\n", i, mySpline->p_psDeriv2[i]);
    620 
    621     //
    622     // We generate the coefficients of the spline polynomials.  I can't
    623     // concisely explain how this code works.  See above function comments
    624     // and Numerical Recipes in C.
    625     //
    626     for (i=0;i<numSplines;i++) {
    627         H = x32->data.F32[i+1] - x32->data.F32[i];
    628         psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
    629                 "x data (%f - %f) (%f)\n",
    630                 x32->data.F32[i],
    631                 x32->data.F32[i+1], H);
    632         //
    633         // ******** Calculate 0-order term ********
    634         //
    635         // From (1)
    636         (mySpline->spline[i])->coeff[0] = (y32->data.F32[i] * x32->data.F32[i+1]/H);
    637         // From (2)
    638         ((mySpline->spline[i])->coeff[0])-= ((y32->data.F32[i+1] * x32->data.F32[i])/H);
    639         // From (3)
    640         tmp = (x32->data.F32[i+1] * x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
    641         tmp-= (x32->data.F32[i+1] / H);
    642         tmp*= (mySpline->p_psDeriv2)[i] * H * H / 6.0;
    643         ((mySpline->spline[i])->coeff[0])+= tmp;
    644         // From (4)
    645         tmp = -(x32->data.F32[i] * x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
    646         tmp+= (x32->data.F32[i] / H);
    647         tmp*= (mySpline->p_psDeriv2)[i+1] * H * H / 6.0;
    648         ((mySpline->spline[i])->coeff[0])+= tmp;
    649 
    650         //
    651         // ******** Calculate 1-order term ********
    652         //
    653         // From (1)
    654         (mySpline->spline[i])->coeff[1] = -(y32->data.F32[i]) / H;
    655         // From (2)
    656         ((mySpline->spline[i])->coeff[1])+= (y32->data.F32[i+1] / H);
    657         // From (3)
    658         tmp = -3.0 * (x32->data.F32[i+1] * x32->data.F32[i+1]) / (H * H * H);
    659         tmp+= (1.0 / H);
    660         tmp*= ((mySpline->p_psDeriv2)[i]) * H * H / 6.0;
    661         ((mySpline->spline[i])->coeff[1])+= tmp;
    662         // From (4)
    663         tmp = 3.0 * (x32->data.F32[i] * x32->data.F32[i]) / (H * H * H);
    664         tmp-= (1.0 / H);
    665         tmp*= ((mySpline->p_psDeriv2)[i+1]) * H * H / 6.0;
    666         ((mySpline->spline[i])->coeff[1])+= tmp;
    667 
    668         //
    669         // ******** Calculate 2-order term ********
    670         //
    671         // From (3)
    672         (mySpline->spline[i])->coeff[2] = ((mySpline->p_psDeriv2)[i]) * 3.0 * x32->data.F32[i+1] / (6.0 * H);
    673         // From (4)
    674         ((mySpline->spline[i])->coeff[2])-= (((mySpline->p_psDeriv2)[i+1]) * 3.0 * x32->data.F32[i] / (6.0 * H));
    675 
    676         //
    677         // ******** Calculate 3-order term ********
    678         //
    679         // From (3)
    680         (mySpline->spline[i])->coeff[3] = -((mySpline->p_psDeriv2)[i]) / (6.0 * H);
    681         // From (4)
    682         ((mySpline->spline[i])->coeff[3])+=  ((mySpline->p_psDeriv2)[i+1]) / (6.0 * H);
    683 
    684         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    685                 "(mySpline->spline[%u])->coeff[0] is %f\n", i, (mySpline->spline[i])->coeff[0]);
    686         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    687                 "(mySpline->spline[%u])->coeff[1] is %f\n", i, (mySpline->spline[i])->coeff[1]);
    688         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    689                 "(mySpline->spline[%u])->coeff[2] is %f\n", i, (mySpline->spline[i])->coeff[2]);
    690         psTrace(".psLib.dataManip.psVectorFitSpline1D", 6,
    691                 "(mySpline->spline[%u])->coeff[3] is %f\n", i, (mySpline->spline[i])->coeff[3]);
    692 
    693     }
    694 
    695     psFree(xF32);
    696     psFree(yF32);
    697 
    698     psTrace(".psLib.dataManip.psVectorFitSpline1D", 4,
    699             "---- psVectorFitSpline1D() end ----\n");
    700 
    701     return(mySpline);
    702 }
    703 
    704 
    705 
    706 
    707 
    708 /*****************************************************************************/
    709 /*  FUNCTION IMPLEMENTATION - PUBLIC                                         */
    710 /*****************************************************************************/
    711 
    712 //typedef struct {
    713 //    psS32 n;
    714 //    psPolynomial1D **spline;
    715 //    psF32 *p_psDeriv2;
    716 //    psVector *knots;
    717 //} psSpline1D;
    718 
    719 /*****************************************************************************
    720     NOTE: "n" specifies the number of spline polynomials.  Therefore, there
    721     must exist n+1 points in "knots".
    722  
    723 XXX: Is this really needed anymore?
    724  
    725 XXX: Ensure that knots[i+1] != knots[i]
    726  
    727 XXX: What should be the default type for knots be?  psF32 is assumed.
    728  *****************************************************************************/
    729 psSpline1D *psSpline1DAlloc(unsigned int numSplines,
    730                             unsigned int order,
    731                             float min,
    732                             float max)
    733 {
    734     PS_ASSERT_INT_NONNEGATIVE(numSplines, NULL);
    735     PS_ASSERT_INT_NONNEGATIVE(order, NULL);
    736     PS_ASSERT_FLOAT_NON_EQUAL(max, min, NULL);
    737 
    738     psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
    739     tmpSpline->n = numSplines;
    740 
    741     //
    742     // XXX: We might have to allocate single or double polynomials depending on the type
    743     // of the psVector bounds.  For now, all knots and spline polynomials are 32-bit.
    744     //
    745     tmpSpline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    746     for (unsigned int i=0;i<numSplines;i++) {
    747         (tmpSpline->spline)[i] = psPolynomial1DAlloc(order, PS_POLYNOMIAL_ORD);
    748     }
    749 
    750     // This will be computed by psVectorFitSpline1D()
    751     tmpSpline->p_psDeriv2 = NULL;
    752 
    753     //
    754     // XXX:Ensure that the knots are distinct, and monotonic.
    755     //
    756     tmpSpline->knots = psVectorAlloc(numSplines+1, PS_TYPE_F32);
    757     psF32 width = (max - min) / ((psF32) numSplines);
    758     tmpSpline->knots->data.F32[0] = min;
    759     for (unsigned int i=1;i<numSplines;i++) {
    760         tmpSpline->knots->data.F32[i] = min + (width * (psF32) i);
    761     }
    762     tmpSpline->knots->data.F32[numSplines] = max;
    763 
    764     psMemSetDeallocator(tmpSpline, (psFreeFunc)spline1DFree);
    765     return(tmpSpline);
    766 }
    767 
    768 /*****************************************************************************
    769 XXX: Is there a psLib function for this?
    770  *****************************************************************************/
    771 psVector *PsVectorDup2(psVector *in)
    772 {
    773     psVector *out = psVectorAlloc(in->n, in->type.type);
    774 
    775     if (in->type.type == PS_TYPE_F32) {
    776         for (unsigned int i = 0 ; i < in->n ; i++) {
    777             out->data.F32[i] = in->data.F32[i];
    778         }
    779     } else if (in->type.type == PS_TYPE_F64) {
    780         for (unsigned int i = 0 ; i < in->n ; i++) {
    781             out->data.F64[i] = in->data.F64[i];
    782         }
    783     } else {
    784         psError(PS_ERR_UNKNOWN, true, "psVector in has an incorrect type.\n");
    785         return(NULL);
    786     }
    787     return(out);
    788 }
    789 
    790 /*****************************************************************************
    791 XXX: What should be the default type for knots, spline polys?  psF32 is assumed.
    792  *****************************************************************************/
    793 psSpline1D *psSpline1DAllocGeneric(const psVector *bounds,
    794                                    unsigned int order)
    795 {
    796     PS_ASSERT_VECTOR_NON_NULL(bounds, NULL);
    797     PS_ASSERT_VECTOR_NON_EMPTY(bounds, NULL);
    798     PS_ASSERT_VECTOR_TYPE(bounds, PS_TYPE_F32, NULL);
    799     PS_ASSERT_INT_NONNEGATIVE(order, NULL);
    800 
    801     psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
    802     unsigned int numSplines = bounds->n - 1;
    803     tmpSpline->n = numSplines;
    804 
    805     //
    806     // XXX: We might have to allocate single or double polynomials depending on the type
    807     // of the psVector bounds.  For now, all knots and spline polynomials are 32-bit.
    808     //
    809     tmpSpline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
    810     for (unsigned int i=0;i<numSplines;i++) {
    811         (tmpSpline->spline)[i] = psPolynomial1DAlloc(order, PS_POLYNOMIAL_ORD);
    812     }
    813 
    814     // This will be computed by psVectorFitSpline1D()
    815     tmpSpline->p_psDeriv2 = NULL;
    816 
    817     //
    818     // Ensure that all knots are distinct.
    819     // XXX:Ensure that the knots are monotonic.
    820     //
    821     for (unsigned int i=0;i<bounds->n-1;i++) {
    822         if (FLT_EPSILON >= fabs(bounds->data.F32[i+1]-bounds->data.F32[i])) {
    823             psError(PS_ERR_UNKNOWN, true, "data points must be distinct ([%d] %f %f)\n", i, bounds->data.F32[i], bounds->data.F32[i+1]);
    824             return(NULL);
    825         }
    826     }
    827     tmpSpline->knots = PsVectorDup2((psVector *) bounds);
    828 
    829     psMemSetDeallocator(tmpSpline, (psFreeFunc)spline1DFree);
    830     return(tmpSpline);
    831 }
    832 
    833 /*****************************************************************************
    834 vectorBinDisectF32(): This is a macro for a private function which takes as
     137/*****************************************************************************
     138vectorBinDisectTYPE(): This is a macro for a private function which takes as
    835139input a vector an array of data as well as a single value for that data.  The
    836140input vector values are assumed to be non-decreasing (v[i-1] <= v[i] for all
    … …  
    840144 *****************************************************************************/
    841145#define FUNC_MACRO_VECTOR_BIN_DISECT(TYPE) \
    842 static unsigned int vectorBinDisect##TYPE(ps##TYPE *bins, \
    843         unsigned int numBins, \
    844         ps##TYPE x) \
     146static psS32 vectorBinDisect##TYPE(ps##TYPE *bins, \
     147                                   psS32 numBins, \
     148                                   ps##TYPE x) \
    845149{ \
    846     unsigned int min; \
    847     unsigned int max; \
    848     unsigned int mid; \
    849     \
    850     psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \
    851             "---- Calling vectorBinDisect##TYPE(%f)\n", x); \
    852     \
     150    psS32 min; \
     151    psS32 max; \
     152    psS32 mid; \
     153    psTrace(__func__, 4, "---- %s() begin ----\n", __func__); \
    853154    if (x < bins[0]) { \
    854155        psLogMsg(__func__, PS_LOG_WARN, \
    … …  
    857158        return(-2); \
    858159    } \
    859     \
    860160    if (x > bins[numBins-1]) { \
    861161        psLogMsg(__func__, PS_LOG_WARN, \
    … …  
    868168    max = numBins-2; \
    869169    mid = ((max+1)-min)/2; \
    870     \
    871170    while (min != max) { \
    872         psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \
    873                 "(min, mid, max) is (%u, %u, %u): (x, bins) is (%f, %f)\n", \
    874                 min, mid, max, x, bins[mid]); \
     171        psTrace(__func__, 6, "(min, mid, max) is (%u, %u, %u): (x, bins) is (%f, %f)\n", min, mid, max, x, bins[mid]); \
    875172        \
    876173        if (x == bins[mid]) { \
    877             psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \
    878                     "---- Exiting vectorBinDisect##TYPE(): bin %u\n", mid); \
     174            psTrace(__func__, 4, "---- %s(%d) end ----\n", __func__, mid); \
    879175            return(mid); \
    880176        } else if (x < bins[mid]) { \
    … …  
    885181        mid = ((max+1)+min)/2; \
    886182    } \
    887     \
    888     psTrace(".psLib.dataManip.psSpline.vectorBinDisect##TYPE", 4, \
    889             "---- Exiting vectorBinDisect##TYPE(): bin %u\n", min); \
     183    psTrace(__func__, 4, "---- %s(%d) end ----\n", __func__, min); \
    890184    return(min); \
    891185} \
    … …  
    904198/*****************************************************************************
    905199p_psVectorBinDisect(): A wrapper to the above p_psVectorBinDisect().
    906  
    907 XXX: Assert that the psVector and psScalar have the same type.
    908  *****************************************************************************/
    909 unsigned int p_psVectorBinDisect(psVector *bins,
    910                                  psScalar *x)
     200  *****************************************************************************/
     201psS32 p_psVectorBinDisect(
     202    psVector *bins,
     203    psScalar *x)
    911204{
    912205    PS_ASSERT_VECTOR_NON_NULL(bins, -4);
    … …  
    960253
    961254/*****************************************************************************
     255fullInterpolate1DF32(): This routine will take as input n-element floating
     256point arrays domain and range, and the x value, assumed to lie with the
     257domain vector.  It produces as output the (n-1)-order LaGrange interpolated
     258value of x.
     259 
     260XXX: do we error check for non-distinct domain values?
     261 
     262XXX: This is a somewhat general function.  There's no reason to put it in this
     263file.
     264 
     265XXX: Sort all these interpolation functions out.  Why are there so many?
     266 
     267XXX: The fullInterpolate1D macros are not used anywhere.
     268 *****************************************************************************/
     269#define FUNC_MACRO_FULL_INTERPOLATE_1D(TYPE) \
     270static psF32 fullInterpolate1D##TYPE(ps##TYPE *domain, \
     271                                     ps##TYPE *range, \
     272                                     unsigned int n, \
     273                                     ps##TYPE x) \
     274{ \
     275    \
     276    unsigned int i; \
     277    unsigned int m; \
     278    static psVector *p = NULL; \
     279    p = psVectorRecycle(p, n, PS_TYPE_##TYPE); \
     280    p_psMemSetPersistent(p, true); \
     281    p_psMemSetPersistent(p->data.TYPE, true); \
     282    \
     283    psTrace(__func__, 4, "---- %s() begin %u-order at x=%f) (%d data points) ----\n", __func__, n-1, x, n); \
     284    for (i=0;i<n;i++) { \
     285        psTrace(__func__, 6, "domain/range is (%f %f)\n", domain[i], range[i]); \
     286    } \
     287    \
     288    for (i=0;i<n;i++) { \
     289        p->data.TYPE[i] = range[i]; \
     290        psTrace(__func__, 6, "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \
     291        \
     292    } \
     293    \
     294    /* From NR, during each iteration of the m loop, we are computing the \
     295       p_{i ... i+m} terms. \
     296    */ \
     297    for (m=1;m<n;m++) { \
     298        for (i=0;i<n-m;i++) { \
     299            /* From NR: we are computing P_{i ... i+m} \
     300             */ \
     301            p->data.TYPE[i] = (((x-domain[i+m]) * p->data.TYPE[i]) + \
     302                               ((domain[i]-x) * p->data.TYPE[i+1])) / \
     303                              (domain[i] - domain[i+m]); \
     304            psTrace(__func__, 6, "p->data.TYPE[%u] is %f\n", i, p->data.TYPE[i]); \
     305        } \
     306    } \
     307    psTrace(__func__, 4, "---- %s(....) end ----\n", __func__); \
     308    return(p->data.TYPE[0]); \
     309} \
     310
     311/* XXX: Do this correctly.
     312FUNC_MACRO_FULL_INTERPOLATE_1D(U8)
     313FUNC_MACRO_FULL_INTERPOLATE_1D(U16)
     314FUNC_MACRO_FULL_INTERPOLATE_1D(U32)
     315FUNC_MACRO_FULL_INTERPOLATE_1D(U64)
     316FUNC_MACRO_FULL_INTERPOLATE_1D(S8)
     317FUNC_MACRO_FULL_INTERPOLATE_1D(S16)
     318FUNC_MACRO_FULL_INTERPOLATE_1D(S32)
     319FUNC_MACRO_FULL_INTERPOLATE_1D(S64)
     320FUNC_MACRO_FULL_INTERPOLATE_1D(F64)
     321*/
     322FUNC_MACRO_FULL_INTERPOLATE_1D(F32)
     323
     324
     325/*****************************************************************************
     326interpolate1DF32(): this is the base 1-D flat memory routine to perform
     327LaGrange interpolation.
     328 *****************************************************************************/
     329static psF32 interpolate1DF32(psF32 *domain,
     330                              psF32 *range,
     331                              unsigned int n,
     332                              unsigned int order,
     333                              psF32 x)
     334{
     335    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
     336    PS_ASSERT_PTR_NON_NULL(domain, NAN)
     337    PS_ASSERT_PTR_NON_NULL(range, NAN)
     338    // XXX: Check valid values for n, order, and x?
     339
     340    psS32 binNum;
     341    unsigned int numIntPoints = order+1;
     342    psS32 origin;
     343
     344    binNum = vectorBinDisectF32(domain, n, x);
     345
     346    if (0 == numIntPoints%2) {
     347        origin = binNum - ((numIntPoints/2) - 1);
     348    } else {
     349        origin = binNum - (numIntPoints/2);
     350        if ((x-domain[binNum]) > (domain[binNum+1]-x)) {
     351            // x is closer to binNum+1.
     352            origin = 1 + (binNum - (numIntPoints/2));
     353        }
     354    }
     355    if (origin < 0) {
     356        origin = 0;
     357    }
     358    if ((origin + numIntPoints) > n) {
     359        origin = n - numIntPoints;
     360    }
     361
     362    psTrace(__func__, 4, "---- %s(....) end ----\n", __func__);
     363    return(fullInterpolate1DF32(&domain[origin], &range[origin], order+1, x));
     364}
     365
     366
     367/*****************************************************************************
    962368p_psVectorInterpolate(): This routine will take as input psVectors domain and
    963369range, and the x value, assumed to lie with the domain vector.  It produces
    … …  
    967373XXX: This stuff does not currently work with a mask.
    968374 
    969 XXX: add another psScalar argument for the result.
     375XXX: add another psScalar argument for the result (so you don't have to
     376     allocate it each time).
    970377 
    971378XXX: The VectorCopy routines seg fault when I declare range32 as static.
    972379 *****************************************************************************/
    973 psScalar *p_psVectorInterpolate(psVector *domain,
    974                                 psVector *range,
    975                                 unsigned int order,
    976                                 psScalar *x)
    977 {
     380psScalar *p_psVectorInterpolate(
     381    psVector *domain,
     382    psVector *range,
     383    psS32 order,
     384    psScalar *x)
     385{
     386    psTrace(__func__, 4, "---- %s() begin ----\n", __func__);
    978387    PS_ASSERT_VECTOR_NON_NULL(domain, NULL);
    979388    PS_ASSERT_VECTOR_NON_NULL(range, NULL);
    … …  
    986395    psVector *range32 = NULL;
    987396    psVector *domain32 = NULL;
    988     psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
    989             "---- p_psVectorInterpolate() begin ----\n");
    990 
    991397    if (order > (domain->n - 1)) {
    992398        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
    … …  
    997403
    998404    if (x->type.type == PS_TYPE_F32) {
    999         psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
    1000                 "---- p_psVectorInterpolate() end ----\n");
     405        psTrace(__func__, 4, "---- %s(NULL) end ----\n", __func__);
    1001406        return(psScalarAlloc(interpolate1DF32(domain->data.F32,
    1002407                                              range->data.F32,
    … …  
    1018423        psFree(domain32);
    1019424
    1020         psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
    1021                 "---- p_psVectorInterpolate() end ----\n");
     425        psTrace(__func__, 4, "---- %s() end ----\n", __func__);
    1022426        // XXX: Convert data type to F64?
    1023427        return(tmpScalar);
    … …  
    1031435    }
    1032436
    1033     psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
    1034             "return(NULL)\n");
    1035     psTrace(".psLib.dataManip.psSpline.p_psVectorInterpolate", 4,
    1036             "---- p_psVectorInterpolate() end ----\n");
    1037 
     437    psTrace(__func__, 4, "---- %s(NULL) end ----\n", __func__);
    1038438    return(NULL);
     439}
     440
     441
     442/*****************************************************************************/
     443/* FUNCTION IMPLEMENTATION - PUBLIC                                          */
     444/*****************************************************************************/
     445
     446bool psMemCheckSpline1D(psPtr ptr)
     447{
     448    return ( psMemGetDeallocator(ptr) == (psFreeFunc)spline1DFree );
     449}
     450
     451psSpline1D *psSpline1DAlloc()
     452{
     453    psSpline1D *tmpSpline = (psSpline1D *) psAlloc(sizeof(psSpline1D));
     454    tmpSpline->n = 0;
     455    tmpSpline->spline = NULL;
     456    tmpSpline->knots = NULL;
     457    tmpSpline->p_psDeriv2 = NULL;
     458    psMemSetDeallocator(tmpSpline, (psFreeFunc) spline1DFree);
     459
     460    return(tmpSpline);
     461}
     462
     463
     464/*****************************************************************************
     465psVectorFitSpline1D(): given a set of x/y vectors, this routine generates the
     466linear or cublic splines which satisfy those data points.
     467 
     468The formula for calculating the spline polynomials is derived from Numerical
     469Recipes in C.  The basic idea is that the polynomial is
     470 (1)     y = (A * y[0]) +
     471 (2)         (B * y[1]) +
     472 (3)         ((((A*A*A)-A) * mySpline->p_psDeriv2[0]) * H^2)/6.0 +
     473 (4)         ((((B*B*B)-B) * mySpline->p_psDeriv2[1]) * H^2)/6.0
     474Where:
     475 H = x[1]-x[0]
     476 A = (x[1]-x)/H
     477 B = (x-x[0])/H
     478The bulk of the code in this routine is the expansion of the above equation
     479into a polynomial in terms of x, and then saving the coefficients of the
     480powers of x in the spline polynomials.  This gets pretty complicated.
     481 
     482XXX: What types must be supported?
     483 
     484XXX: Should we allow x==NULL?
     485 *****************************************************************************/
     486psSpline1D *psVectorFitSpline1D(
     487    const psVector* x,        ///< Ordinates.
     488    const psVector* y)         ///< Coordinates.
     489{
     490    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
     491    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
     492    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
     493    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
     494    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
     495    PS_ASSERT_VECTORS_SIZE_EQUAL(x, y, NULL);
     496    psS32 numSplines = (y->n)-1;
     497
     498    psTrace(__func__, 5, "numSplines is %d\n", numSplines);
     499    //
     500    // The following code ensures that xPtr and yPtr points to a psF32 psVector.
     501    //
     502    psVector *xF32 = NULL;
     503    psVector *xPtr = NULL;
     504    if (PS_TYPE_F64 == x->type.type) {
     505        xF32 = psVectorAlloc(y->n, PS_TYPE_F32);
     506        for (psS32 i = 0 ; i < x->n ; i++) {
     507            xF32->data.F32[i] = (psF32) x->data.F64[i];
     508        }
     509        xPtr = xF32;
     510    } else if (PS_TYPE_F32 == x->type.type) {
     511        xPtr = (psVector *) x;
     512    }
     513
     514    psVector *yF32 = NULL;
     515    psVector *yPtr = NULL;
     516    if (PS_TYPE_F64 == y->type.type) {
     517        yF32 = psVectorAlloc(y->n, PS_TYPE_F32);
     518        for (psS32 i = 0 ; i < y->n ; i++) {
     519            yF32->data.F32[i] = (psF32) y->data.F64[i];
     520        }
     521        yPtr = yF32;
     522    } else {
     523        yPtr = (psVector *) y;
     524    }
     525
     526    //
     527    // Create the psSpline1D struct.
     528    //
     529    psSpline1D *spline = psSpline1DAlloc();
     530    spline->n = numSplines;
     531    spline->spline = (psPolynomial1D **) psAlloc(numSplines * sizeof(psPolynomial1D *));
     532    for (psS32 i=0;i<numSplines;i++) {
     533        (spline->spline)[i] = psPolynomial1DAlloc(3, PS_POLYNOMIAL_ORD);
     534    }
     535    //
     536    // XXX: Ensure that the knots are distinct, and monotonic.
     537    // XXX: Use a vector dup macro, or function here.
     538    //
     539    spline->knots = psVectorAlloc(x->n, PS_TYPE_F32);
     540    for (psS32 i = 0 ; i < x->n ; i++) {
     541        spline->knots->data.F32[i] = xPtr->data.F32[i];
     542    }
     543    //
     544    // Generate the second derivatives at each data point.
     545    //
     546    spline->p_psDeriv2 = calculateSecondDerivs(xPtr, yPtr);
     547
     548    //
     549    // We generate the coefficients of the spline polynomials.  I can't
     550    // concisely explain how this code works.  See above function comments
     551    // and Numerical Recipes in C.
     552    //
     553    for (psS32 i=0 ; i < numSplines ; i++) {
     554        psF32 H = xPtr->data.F32[i+1] - xPtr->data.F32[i];
     555        if (fabs(H) <= FLT_EPSILON) {
     556            printf("XXX: Generate error: x data points are not distinct (%d %d).\n", i, i+1);
     557        }
     558        psTrace(__func__, 6, "x data (%f - %f) (%f)\n", xPtr->data.F32[i], xPtr->data.F32[i+1], H);
     559
     560        //
     561        // ******** Calculate 0-order term ********
     562        //
     563        // From (1)
     564        (spline->spline[i])->coeff[0] = (yPtr->data.F32[i] * xPtr->data.F32[i+1]/H);
     565        // From (2)
     566        ((spline->spline[i])->coeff[0])-= ((yPtr->data.F32[i+1] * xPtr->data.F32[i])/H);
     567        // From (3)
     568        psF32 tmp = (xPtr->data.F32[i+1] * xPtr->data.F32[i+1] * xPtr->data.F32[i+1]) / (H * H * H);
     569        tmp-= (xPtr->data.F32[i+1] / H);
     570        tmp*= (spline->p_psDeriv2)[i] * H * H / 6.0;
     571        ((spline->spline[i])->coeff[0])+= tmp;
     572        // From (4)
     573        tmp = -(xPtr->data.F32[i] * xPtr->data.F32[i] * xPtr->data.F32[i]) / (H * H * H);
     574        tmp+= (xPtr->data.F32[i] / H);
     575        tmp*= (spline->p_psDeriv2)[i+1] * H * H / 6.0;
     576        ((spline->spline[i])->coeff[0])+= tmp;
     577
     578        //
     579        // ******** Calculate 1-order term ********
     580        //
     581        // From (1)
     582        (spline->spline[i])->coeff[1] = -(yPtr->data.F32[i]) / H;
     583        // From (2)
     584        ((spline->spline[i])->coeff[1])+= (yPtr->data.F32[i+1] / H);
     585        // From (3)
     586        tmp = -3.0 * (xPtr->data.F32[i+1] * xPtr->data.F32[i+1]) / (H * H * H);
     587        tmp+= (1.0 / H);
     588        tmp*= ((spline->p_psDeriv2)[i]) * H * H / 6.0;
     589        ((spline->spline[i])->coeff[1])+= tmp;
     590        // From (4)
     591        tmp = 3.0 * (xPtr->data.F32[i] * xPtr->data.F32[i]) / (H * H * H);
     592        tmp-= (1.0 / H);
     593        tmp*= ((spline->p_psDeriv2)[i+1]) * H * H / 6.0;
     594        ((spline->spline[i])->coeff[1])+= tmp;
     595
     596        //
     597        // ******** Calculate 2-order term ********
     598        //
     599        // From (3)
     600        (spline->spline[i])->coeff[2] = ((spline->p_psDeriv2)[i]) * 3.0 * xPtr->data.F32[i+1] / (6.0 * H);
     601        // From (4)
     602        ((spline->spline[i])->coeff[2])-= (((spline->p_psDeriv2)[i+1]) * 3.0 * xPtr->data.F32[i] / (6.0 * H));
     603
     604        //
     605        // ******** Calculate 3-order term ********
     606        //
     607        // From (3)
     608        (spline->spline[i])->coeff[3] = -((spline->p_psDeriv2)[i]) / (6.0 * H);
     609        // From (4)
     610        ((spline->spline[i])->coeff[3])+=  ((spline->p_psDeriv2)[i+1]) / (6.0 * H);
     611
     612        psTrace(__func__, 6, "(spline->spline[%u])->coeff[0] is %f\n", i, (spline->spline[i])->coeff[0]);
     613        psTrace(__func__, 6, "(spline->spline[%u])->coeff[1] is %f\n", i, (spline->spline[i])->coeff[1]);
     614        psTrace(__func__, 6, "(spline->spline[%u])->coeff[2] is %f\n", i, (spline->spline[i])->coeff[2]);
     615        psTrace(__func__, 6, "(spline->spline[%u])->coeff[3] is %f\n", i, (spline->spline[i])->coeff[3]);
     616    }
     617
     618    if (PS_TYPE_F64 == x->type.type) {
     619        psFree(xF32);
     620    }
     621    if (PS_TYPE_F64 == y->type.type) {
     622        psFree(yF32);
     623    }
     624    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
     625    return(spline);
    1039626}
    1040627
    … …  
    1050637     the spline fit functions require F32 and F64.
    1051638 
    1052 XXX: This only works if spline0>knots if psF32.  Must add support for psU32 and
    1053 psF64.
     639XXX: This only works if spline0>knots if psF32.  Must we add support for psU32 and
     640psF64?
    1054641 *****************************************************************************/
    1055642float psSpline1DEval(
    1056643    const psSpline1D *spline,
    1057     float x
    1058 )
    1059 {
     644    float x)
     645{
     646    psTrace(__func__, 3, "---- %s(%f) begin ----\n", __func__, x);
    1060647    PS_ASSERT_PTR_NON_NULL(spline, NAN);
    1061648    PS_ASSERT_INT_NONNEGATIVE(spline->n, NAN);
    1062649    PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NAN);
    1063650
    1064     unsigned int binNum;
    1065     unsigned int n;
    1066 
    1067     n = spline->n;
    1068     //XXX    binNum = vectorBinDisectF32(spline->domains, (spline->n)+1, x);
    1069     binNum = vectorBinDisectF32(spline->knots->data.F32, (spline->n)+1, x);
     651    psS32 n = spline->n;
     652    psS32 binNum = vectorBinDisectF32(spline->knots->data.F32, (spline->n)+1, x);
    1070653    if (binNum < 0) {
     654        //
     655        // If x is outside the range of spline->knots, generate a warning
     656        // message, then return the left, or right, endpoint.
     657        //
    1071658        psLogMsg(__func__, PS_LOG_WARN,
    1072659                 "psSpline1DEval(): x ordinate (%f) is outside the spline range (%f - %f).",
    … …  
    1075662
    1076663        if (x < spline->knots->data.F32[0]) {
    1077             return(psPolynomial1DEval(spline->spline[0],
    1078                                       x));
     664            psF32 rcF32 = psPolynomial1DEval(spline->spline[0], x);
     665            psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32);
     666            return(rcF32);
    1079667        } else if (x > spline->knots->data.F32[n-1]) {
    1080             return(psPolynomial1DEval(spline->spline[n-1],
    1081                                       x));
     668            psF32 rcF32 = psPolynomial1DEval(spline->spline[n-1], x);
     669            psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32);
     670            return(rcF32);
    1082671        }
    1083672    }
    1084673
    1085     return(psPolynomial1DEval(spline->spline[binNum],
    1086                               x));
    1087 }
    1088 
    1089 // XXX: The spline eval functions require input and output to be F32.
    1090 // however the spline fit functions require F32 and F64.
     674    psF32 rcF32 = psPolynomial1DEval(spline->spline[binNum], x);
     675    psTrace(__func__, 3, "---- %s(%f) end ----\n", __func__, rcF32);
     676    return(rcF32);
     677}
     678
     679
     680/*****************************************************************************
     681 *****************************************************************************/
    1091682psVector *psSpline1DEvalVector(
    1092683    const psSpline1D *spline,
    1093     const psVector *x
    1094 )
    1095 {
     684    const psVector *x)
     685{
     686    psTrace(__func__, 3, "---- %s() begin ----\n", __func__);
    1096687    PS_ASSERT_PTR_NON_NULL(spline, NULL);
     688    PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL);
    1097689    PS_ASSERT_VECTOR_NON_NULL(x, NULL);
    1098690    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
    1099     PS_ASSERT_VECTOR_TYPE(spline->knots, PS_TYPE_F32, NULL);
    1100 
    1101     unsigned int i;
    1102     psVector *tmpVector;
    1103 
    1104     tmpVector = psVectorAlloc(x->n, PS_TYPE_F32);
     691    if (psTraceGetLevel(__func__) >= 6) {
     692        PS_VECTOR_PRINT_F32(x);
     693        // XXX: print spline
     694    }
     695
     696    psVector *tmpVector = psVectorAlloc(x->n, PS_TYPE_F32);
    1105697    if (x->type.type == PS_TYPE_F32) {
    1106         for (i=0;i<x->n;i++) {
    1107             tmpVector->data.F32[i] = psSpline1DEval(
    1108                                          spline,
    1109                                          x->data.F32[i]
    1110                                      );
     698        for (psS32 i=0;i<x->n;i++) {
     699            tmpVector->data.F32[i] = psSpline1DEval(spline, x->data.F32[i]);
    1111700        }
    1112701    } else if (x->type.type == PS_TYPE_F64) {
    1113         for (i=0;i<x->n;i++) {
    1114             tmpVector->data.F32[i] = psSpline1DEval(
    1115                                          spline,
    1116                                          (psF32) x->data.F64[i]
    1117                                      );
     702        for (psS32 i=0;i<x->n;i++) {
     703            tmpVector->data.F32[i] = psSpline1DEval(spline, (psF32) x->data.F64[i]);
    1118704        }
    1119     } else {
    1120         char* strType;
    1121         PS_TYPE_NAME(strType,x->type.type);
    1122         psError(PS_ERR_BAD_PARAMETER_TYPE,
    1123                 PS_ERRORTEXT_psSpline_TYPE_NOT_SUPPORTED,
    1124                 strType);
    1125         return(NULL);
    1126     }
    1127 
     705    }
     706
     707    psTrace(__func__, 3, "---- %s() end ----\n", __func__);
    1128708    return(tmpVector);
    1129709}
Note: See TracChangeset for help on using the changeset viewer.