MiscMath.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 GNSSTK_MISCMATH_HPP
#define GNSSTK_MISCMATH_HPP
 
#include <cstring>   // for size_t
#include <vector>
#include "MathBase.hpp"
#include "Exception.hpp"
 
namespace gnsstk
{
 
 
   template <class T>
   T SimpleLagrangeInterpolation(const std::vector<T>& X,
                                 const std::vector<T>& Y,
                                 const T x)
   {
      if(Y.size() < X.size()) {
         GNSSTK_THROW(Exception("Input vectors must be of same size"));
      }
      size_t i,j;
      T Yx(0);
      for(i=0; i<X.size(); i++) {
         if(x==X[i]) return Y[i];
 
         T Li(1);
         for(j=0; j<X.size(); j++)
            if(i!=j)  Li *= (x-X[j])/(X[i]-X[j]);
 
         Yx += Li*Y[i];
      }
      return Yx;
   }  // end T LagrangeInterpolation(const vector, const vector, const T)
 
   template <class T>
   T LagrangeInterpolation(const std::vector<T>& X, const std::vector<T>& Y,
                           const T& x, T& err)
   {
      if(Y.size() < X.size() || X.size() < 4) {
         GNSSTK_THROW(Exception("Input vectors must be of same length, at least 4"));
      }
 
      std::size_t i,j,k;
      T y,del;
      std::vector<T> D,Q;
 
      err = T(0);
      k = X.size()/2;
      if(x == X[k]) return Y[k];
      if(x == X[k-1]) return Y[k-1];
      if(ABS(x-X[k-1]) < ABS(x-X[k])) k=k-1;
      for(i=0; i<X.size(); i++) {
         Q.push_back(Y[i]);
         D.push_back(Y[i]);
      }
      y = Y[k--];
      for(j=1; j<X.size(); j++) {
         for(i=0; i<X.size()-j; i++) {
            del = (Q[i+1]-D[i])/(X[i]-X[i+j]);
            D[i] = (X[i+j]-x)*del;
            Q[i] = (X[i]-x)*del;
         }
         err = (2*(k+1) < X.size()-j ? Q[k+1] : D[k--]);    // NOT 2*k
         y += err;
      }
      return y;
   }  // end T LagrangeInterpolation(vector, vector, const T, T&)
 
      // The following is a
      // Straightforward implementation of Lagrange polynomial and its derivative
      // { all sums are over index=0,N-1; Xi is short for X[i]; Lp is dL/dx;
      //   y(x) is the function being approximated. }
      // y(x) = SUM[Li(x)*Yi]
      // Li(x) = PROD(j!=i)[x-Xj] / PROD(j!=i)[Xi-Xj]
      // dy(x)/dx = SUM[Lpi(x)*Yi]
      // Lpi(x) = SUM(k!=i){PROD(j!=i,j!=k)[x-Xj]} / PROD(j!=i)[Xi-Xj]
      // Define Pi = PROD(j!=i)[x-Xj], Di = PROD(j!=i)[Xi-Xj],
      // Qij = PROD(k!=i,k!=j)[x-Xk] and Si = SUM(j!=i)Qij.
      // then Li(x) = Pi/Di, and Lpi(x) = Si/Di.
      // Qij is symmetric, there are only N(N+1)/2 - N of them, so store them
      // in a vector of length N(N+1)/2, where Qij==Q[i+j*(j+1)/2] (ignore i=j).
 
   template <class T>
   void LagrangeInterpolation(const std::vector<T>& X, const std::vector<T>& Y,
                              const T& x, T& y, T& dydx)
   {
      if(Y.size() < X.size() || X.size() < 4) {
         GNSSTK_THROW(Exception("Input vectors must be of same length, at least 4"));
      }
 
      std::size_t i,j,k,N=X.size(),M;
      M = (N*(N+1))/2;
      std::vector<T> P(N,T(1)),Q(M,T(1)),D(N,T(1));
      for(i=0; i<N; i++) {
         for(j=0; j<N; j++) {
            if(i != j) {
               P[i] *= x-X[j];
               D[i] *= X[i]-X[j];
               if(i < j) {
                  for(k=0; k<N; k++) {
                     if(k == i || k == j) continue;
                     Q[i+(j*(j+1))/2] *= (x-X[k]);
                  }
               }
            }
         }
      }
      y = dydx = T(0);
      for(i=0; i<N; i++) {
         y += Y[i]*(P[i]/D[i]);
         T S(0);
         for(k=0; k<N; k++) if(i != k) {
               if(k<i) S += Q[k+(i*(i+1))/2]/D[i];
               else    S += Q[i+(k*(k+1))/2]/D[i];
            }
         dydx += Y[i]*S;
      }
   }  // end void LagrangeInterpolation(vector, vector, const T, T&, T&)
 
 
   template <class T>
   T LagrangeInterpolating2ndDerivative(const std::vector<T>& pos,
                                        const std::vector<T>& val,
                                        const T desiredPos)
   {
      int degree(pos.size());
      int i,j,m,n;
 
         // First, compute interpolation factors
      typedef std::vector< T > vectorType;
      std::vector< vectorType > delta(degree, vectorType(degree, 0.0));
 
      for(i=0; i < degree; ++i) {
         for(j=0; j < degree; ++j) {
            if(j != i) {
               delta[i][j] = ((desiredPos - pos[j])/(pos[i] - pos[j]));
            }
         }
      }
 
      double retVal(0.0);
      for(i=0; i < degree; ++i) {
         double sum(0.0);
 
         for(m=0; m < degree; ++m) {
            if(m != i) {
               double weight1(1.0/(pos[i]-pos[m]));
               double sum2(0.0);
 
               for(j=0; j < degree; ++j) {
                  if((j != i) && (j != m)) {
                     double weight2(1.0/(pos[i]-pos[j]));
                     for(n=0; n < degree; ++n) {
                        if((n != j) && (n != m) && (n != i)) {
                           weight2 *= delta[i][n];
                        }
                     }
                     sum2 += weight2;
                  }
               }
               sum += sum2*weight1;
            }
         }
         retVal += val[i] * sum;
      }
 
      return retVal;
 
   }  // End of 'lagrangeInterpolating2ndDerivative()'
 
#define tswap(x,y) { T tmp; tmp = x; x = y; y = tmp; }
 
   template <class T>
   T RSS (T aa, T bb, T cc)
   {
      T a(ABS(aa)), b(ABS(bb)), c(ABS(cc));
      if(a < b) tswap(a,b);
      if(a < c) tswap(a,c);
      if(a == T(0)) return T(0);
      return a * SQRT(1 + (b/a)*(b/a) + (c/a)*(c/a));
   }
 
   template <class T>
   T RSS (T aa, T bb)
   {
      return RSS(aa,bb,T(0));
   }
 
   template <class T>
   T RSS (T aa, T bb, T cc, T dd)
   {
      T a(ABS(aa)), b(ABS(bb)), c(ABS(cc)), d(ABS(dd));
         // For numerical reason, let's just put the biggest in "a" (we are not sorting)
      if(a < b) tswap(a,b);
      if(a < c) tswap(a,c);
      if(a < d) tswap(a,d);
      if(a == T(0)) return T(0);
      return a * SQRT(1 + (b/a)*(b/a) + (c/a)*(c/a) + (d/a)*(d/a));
   }
 
#undef tswap
 
   inline double Round(double x)
   {
      return double(std::floor(x+0.5));
   }
 
 
}  // namespace gnsstk
 
#endif