CubicSpline.hpp

Go to the documentation of this file.

//==============================================================================
//
//  This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
//
//  The GNSSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 3.0 of the License, or
//  any later version.
//
//  The GNSSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GNSSTk; if not, write to the Free Software Foundation,
//  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin.
//  Copyright 2004-2022, The Board of Regents of The University of Texas System
//
//==============================================================================
 
//==============================================================================
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin, under contract to an agency or agencies
//  within the U.S. Department of Defense. The U.S. Government retains all
//  rights to use, duplicate, distribute, disclose, or release this software.
//
//  Pursuant to DoD Directive 523024
//
//  DISTRIBUTION STATEMENT A: This software has been approved for public
//                            release, distribution is unlimited.
//
//==============================================================================
 
#ifndef INCLUDE_CUBIC_SPLINE_HPP
#define INCLUDE_CUBIC_SPLINE_HPP
 
#include "Exception.hpp"
#include "StringUtils.hpp"
#include <vector>
 
namespace gnsstk
{
 
 
   template <class T> class CubicSpline
   {
   public:
      CubicSpline() : N(0) {}
 
      CubicSpline(const std::vector<T>& X, const std::vector<T>& Y)
      {
         initialize(X, Y);
      }
 
      void initialize(const std::vector<T>& X, const std::vector<T>& Y)
      {
         build(X, Y, false);
      }
 
      void initialize(const std::vector<T>& X, const std::vector<T>& Y,
                      const T DYDX1, const T DYDXN)
      {
         fd1 = DYDX1;
         fdN = DYDXN;
         build(X, Y, true);
      }
 
      bool testLimits(const T& x, T& y)
      {
         if (N == 0)
         {
            Exception e("Must call initialize() first");
            GNSSTK_THROW(e);
         }
         if (x <= X[0])
         {
            y = Y[0];
            return false;
         }
         if (x >= X[N - 1])
         {
            y = Y[N - 1];
            return false;
         }
         return true;
      }
 
      T evaluate(const T& x)
      {
         if (N == 0)
         {
            Exception e("Must call initialize() first");
            GNSSTK_THROW(e);
         }
         if (x < X[0] || x > X[N - 1])
         {
            Exception e(
               "Input value is outside range determined by initialize()");
            GNSSTK_THROW(e);
         }
 
         // find x in the array X
         int i, k;
         if (x == X[0])
         {
            return Y[0];
         }
         for (i = 1; i < N; i++)
         {
            if (x == X[i])
            {
               return Y[i];
            }
            if (X[i - 1] < x && x <= X[i])
            {
               k = i;
               break;
            }
         }
 
         // interpolate
         return interpolate(k, x);
      }
 
      std::vector<T> evaluate(const std::vector<T>& x)
      {
         if (N == 0)
         {
            Exception e("Must call initialize() first");
            GNSSTK_THROW(e);
         }
 
         int n = x.size();
         std::vector<T> y(n);
 
         int i, k = 0;
         for (i = 0; i < n; i++)
         {
            if (x[i] < X[0] || x[i] > X[N - 1])
            {
               Exception e(
                  std::string("Input value at index ") +
                  StringUtils::asString(i) +
                  std::string(" is outside range determined by initialize()"));
               GNSSTK_THROW(e);
            }
 
            // find x[i] in the array X: X[k-1] < x[i] <= X[k]
            if (k < N && x[i] > X[k])
            {
               while (x[i] > X[k])
                  k++; // move up
            }
            else if (k > 0 && x[i] < X[k - 1])
            {
               while (x[i] <= X[k - 1])
                  k--; // move down
            }
 
            if (x[i] == X[k])
            {
               y[i] = Y[k];
            }
            else if (x[i] == X[k - 1])
            {
               y[i] = Y[k - 1];
            }
            else
            {
               y[i] = interpolate(k, x[i]);
            }
         }
 
         return y;
      }
 
      int size() const { return S.size(); }
 
   private:
      void build(const std::vector<T>& x, const std::vector<T>& y, bool fixEnds)
      {
         int i;
 
         // clear old values
         X.clear();
         Y.clear();
         S.clear();
 
         // size of array S
         N = (x.size() < y.size() ? x.size() : y.size());
         if (N == 0)
         {
            Exception e("Input data array(s) empty");
            GNSSTK_THROW(e);
         }
 
         // resize arrays
         X = std::vector<T>(N);
         Y = std::vector<T>(N);
         S = std::vector<T>(N);
 
         // x must be strictly increasing; use this loop to copy out the data.
         X[0] = x[0];
         Y[0] = y[0];
         for (i = 1; i < N; i++)
         {
            if (x[i - 1] >= x[i])
            {
               Exception e("Input data array X is not strictly increasing");
               GNSSTK_THROW(e);
            }
            X[i] = x[i];
            Y[i] = y[i];
         }
 
         // min 4 points needed for cubic spline
         if (N <= 3)
         { // linear interpolation
            for (i = 0; i < N; i++)
               S[i] = T(0);
            return;
         }
 
         // fix derivatives at endpoints
         T dx1, dx2;
         if (!fixEnds)
         {
            // this fits parabola to endpoints
            dx1 = X[1] - X[0];
            dx2 = X[2] - X[0];
            fd1 = ((Y[1] - Y[0]) / (dx1 * dx1) - (Y[2] - Y[0]) / (dx2 * dx2)) /
                  (T(1) / dx1 - T(1) / dx2);
 
            dx1 = X[N - 2] - X[N - 1];
            dx2 = X[N - 3] - X[N - 1];
            fdN = ((Y[N - 2] - Y[N - 1]) / (dx1 * dx1) -
                   (Y[N - 3] - Y[N - 1]) / (dx2 * dx2)) /
                  (T(1) / dx1 - T(1) / dx2);
         }
 
         // second derivatives at endpoints...
         S[0] = T(6) * ((Y[1] - Y[0]) / (X[1] - X[0]) - fd1);
         S[N - 1] =
            T(6) * (fdN + (Y[N - 2] - Y[N - 1]) / (X[N - 1] - X[N - 2]));
 
         // ...and at point in between
         for (i = 1; i < N - 1; i++)
            S[i] =
               T(6) *
               (Y[i - 1] / (X[i] - X[i - 1]) -
                Y[i] * (T(1) / (X[i] - X[i - 1]) + T(1) / (X[i + 1] - X[i])) +
                Y[i + 1] / (X[i + 1] - X[i]));
 
         // finish with recursion
         std::vector<T> A(N);
         A[0] = T(2) * (X[1] - X[0]);
         A[1] = T(1.5) * (X[1] - X[0]) + T(2) * (X[2] - X[1]);
         S[1] -= T(0.5) * S[0];
         for (i = 2; i < N - 1; i++)
         {
            dx1  = (X[i] - X[i - 1]) / A[i - 1];
            A[i] = T(2) * (X[i + 1] - X[i - 1]) - dx1 * (X[i] - X[i - 1]);
            S[i] -= dx1 * S[i - 1];
         }
         dx1      = (X[N - 1] - X[N - 2]) / A[N - 2];
         A[N - 1] = (T(2) - dx1) * (X[N - 1] - X[N - 2]);
         S[N - 1] -= dx1 * S[N - 2];
 
         // back substitute to get second derivatives
         S[N - 1] /= A[N - 1];
         for (i = N - 2; i >= 0; i--)
         {
            S[i] -= (X[i + 1] - X[i]) * S[i + 1];
            S[i] /= A[i];
         }
      }
 
      T interpolate(const int k, const T x)
      {
         T dxr(X[k] - x), dxl(x - X[k - 1]), dx(X[k] - X[k - 1]);
         return ((dxl * (Y[k] - S[k] * dx * dx / T(6)) +
                  (S[k - 1] * dxr * dxr * dxr + S[k] * dxl * dxl * dxl) / T(6) +
                  dxr * (Y[k - 1] - S[k - 1] * dx * dx / T(6))) /
                 dx);
      }
 
      std::vector<T> X, Y;
 
      std::vector<T> S;
 
      T fd1, fdN;
 
      int N;
 
   }; // end class CubicSpline
 
 
} // namespace gnsstk
 
#endif