libctsupport/interpolator.cpp

   1 /*****************************************************************************
   2 **  This is part of the CTSim program
   3 **  Copyright (c) 1983-2001 Kevin Rosenberg
   4 **
   5 **  $Id: interpolator.cpp,v 1.4 2003/03/23 18:37:42 kevin Exp $
   6 **
   7 **  This program is free software; you can redistribute it and/or modify
   8 **  it under the terms of the GNU General Public License (version 2) as
   9 **  published by the Free Software Foundation.
  10 **
  11 **  This program is distributed in the hope that it will be useful,
  12 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 **  GNU General Public License for more details.
  15 **
  16 **  You should have received a copy of the GNU General Public License
  17 **  along with this program; if not, write to the Free Software
  18 **  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19 ******************************************************************************/
  20
  21
  22 #include "ctsupport.h"
  23 #include "interpolator.h"
  24
  25
  26 CubicPolyInterpolator::CubicPolyInterpolator (const double* const y, const int n)
  27   : m_pdY(y), m_n(n)
  28 {
  29   if (m_n < 2)
  30     sys_error (ERR_SEVERE, "Too few points (%d) in CubicPolyInterpolator", m_n);
  31 }
  32
  33 CubicPolyInterpolator::~CubicPolyInterpolator ()
  34 {
  35 }
  36
  37
  38 double
  39 CubicPolyInterpolator::interpolate (double x)
  40 {
  41   int lo = static_cast<int>(floor(x)) - 1;
  42   int hi = lo + 3;
  43
  44   if (lo < -1) {
  45 #ifdef DEBUG
  46     sys_error (ERR_WARNING, "x=%f, out of range [CubicPolyInterpolator]", x);
  47 #endif
  48     return (0);
  49   } else if (lo == -1)  // linear interpolate at between x = 0 & 1
  50     return m_pdY[0] + x * (m_pdY[1] - m_pdY[0]);
  51
  52   if (hi > m_n) {
  53 #ifdef DEBUG
  54     sys_error (ERR_WARNING, "x=%f, out of range [CubicPolyInterpolator]", x);
  55 #endif
  56     return (0);
  57   } else if (hi == m_n) {// linear interpolate between x = (n-2) and (n-1)
  58     double frac = x - (lo + 1);
  59     return m_pdY[m_n - 2] + frac * (m_pdY[m_n - 1] - m_pdY[m_n - 2]);
  60   }
  61
  62   // Lagrange formula for N=4 (cubic)
  63
  64   double xd_0 = x - lo;
  65   double xd_1 = x - (lo + 1);
  66   double xd_2 = x - (lo + 2);
  67   double xd_3 = x - (lo + 3);
  68
  69   static double oneSixth = (1. / 6.);
  70
  71   double y = xd_1 * xd_2 * xd_3 * -oneSixth * m_pdY[lo];
  72   y += xd_0 * xd_2 * xd_3 * 0.5 * m_pdY[lo+1];
  73   y += xd_0 * xd_1 * xd_3 * -0.5 * m_pdY[lo+2];
  74   y += xd_0 * xd_1 * xd_2 * oneSixth * m_pdY[lo+3];
  75
  76   return (y);
  77 }
  78
  79
  80
  81 CubicSplineInterpolator::CubicSplineInterpolator (const double* const y, const int n)
  82   : m_pdY(y), m_n(n)
  83 {
  84   // Precalculate 2nd derivative of y and put in m_pdY2
  85   // Calculated by solving set of simultaneous CubicSpline spline equations
  86   // Only n-2 CubicSpline spline equations, but able to make two more
  87   // equations by setting second derivative to 0 at ends
  88
  89   m_pdY2 = new double [n];
  90   m_pdY2[0] = 0;   // second deriviative = 0 at beginning and end
  91   m_pdY2[n-1] = 0;
  92
  93   double* temp = new double [n - 1];
  94   temp[0] = 0;
  95   int i;
  96   for (i = 1; i < n - 1; i++) {
  97     double t = 2 + (0.5 * m_pdY2[i-1]);
  98     temp[i] = y[i+1] + y[i-1] - y[i] - y[i];
  99     temp[i] = (3 * temp[i] - 0.5 * temp[i-1]) / t;
 100     m_pdY2[i] = -0.5 / t;
 101   }
 102
 103   for (i = n - 2; i >= 0; i--)
 104     m_pdY2[i] = temp[i] + m_pdY2[i] * m_pdY2[i + 1];
 105
 106   delete temp;
 107 }
 108
 109 CubicSplineInterpolator::~CubicSplineInterpolator ()
 110 {
 111   delete m_pdY2;
 112 }
 113
 114
 115 double
 116 CubicSplineInterpolator::interpolate (double x)
 117 {
 118   const static double oneSixth = (1. / 6.);
 119   int lo = static_cast<int>(floor(x));
 120   int hi = lo + 1;
 121
 122   if (lo < 0 || hi >= m_n) {
 123 #ifdef DEBUG
 124     sys_error (ERR_SEVERE, "x out of bounds [CubicSplineInterpolator::interpolate]");
 125 #endif
 126     return (0);
 127   }
 128
 129   double loFr = hi - x;
 130   double hiFr = 1 - loFr;
 131   double y = loFr * m_pdY[lo] + hiFr * m_pdY[hi];
 132   y += oneSixth * ((loFr*loFr*loFr - loFr) * m_pdY2[lo] + (hiFr*hiFr*hiFr - hiFr) * m_pdY2[hi]);
 133
 134   return y;
 135 }
 136
 137
 138
 139