MiscMath.hpp

Go to the documentation of this file.

#pragma ident "$Id: MiscMath.hpp 2974 2011-11-11 09:14:39Z yanweignss $"



#ifndef GPSTK_MISCMATH_HPP
#define GPSTK_MISCMATH_HPP

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk 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 2.1 of the License, or
//  any later version.
//
//  The GPSTk 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 GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software 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.
//
//=============================================================================

#include <vector>
#include "MathBase.hpp"
#include "DebugUtils.hpp"

namespace gpstk
{

   inline double Round(double x);

   template <class T>
   T LagrangeInterpolation(const std::vector<T>& X, const std::vector<T>& Y, T x)
   {
      GPSTK_ASSERT(X.size()==Y.size());

      T Yx(0.0);
      for(size_t i=0;i<X.size();i++)
      {
         if(x==X[i]) return Y[i];

         T Li(1.0);
         for(size_t 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(vector, vector, const T, T&)

   template <class T>
   T LagrangeFirstDerivative(const std::vector<T>& X, const std::vector<T>& Y, T x)
   {
      GPSTK_ASSERT(X.size()==Y.size());

      T dY1(0.0);
      for(size_t i=0; i<X.size(); i++)
      { 
         T temp1(1.0);
         T temp2(0.0);
         for(size_t j=0; j<X.size(); j++)
         {
            if(j==i) continue;

            T temp3(1.0);
            for(size_t k=0;k<X.size();k++)
            {
               if( (k!=i) && (k!=j) ) temp3 *= (x-X[k]);
            }

            temp1 *= (X[i]-X[j]); 
            temp2 += temp3;
         }

         dY1 += temp2/temp1*Y[i];
      }

      return dY1;
   }

   template <class T>
   T LagrangeSecondDerivative(const std::vector<T>& X, const std::vector<T>& Y, T x)
   {
      GPSTK_ASSERT(X.size()==Y.size());

      const size_t degree( X.size() );

      // First, compute interpolation factors
      typedef std::vector< T > vectorType;
      std::vector< vectorType > delta( degree, vectorType(degree, 0.0) );

      for (size_t i = 0; i < degree; ++i)
      {
         for (size_t j = 0; j < degree; ++j)
         {
            if (j != i) delta[i][j] = ( (x - X[j]) / (X[i] - X[j]) );
         }
      }

      double retVal(0.0);

      for( int i = 0; i < degree; ++i )
      {
         double sum( 0.0 );

         for( int m = 0; m < degree; ++m )
         {
            if( m != i )
            {
               double weight1( 1.0/(X[i]-X[m]) );
               double sum2( 0.0 );

               for( int j = 0; j < degree; ++j )
               {
                  if( (j != i) && (j != m) )
                  {
                     double weight2( 1.0/(X[i]-X[j]) );

                     for( int n = 0; n < degree; ++n )
                     {
                        if( (n != j) && (n != m) && (n != i) ) weight2 *= delta[i][n];
                     }
                     sum2 += weight2;
                  }
               }

               sum += sum2*weight1;
            }
         }

         retVal += Y[i] * sum;
      }

      return retVal;

   }  // End of 'lagrangeInterpolating2ndDerivative()'


   //=============================================================================

   template <class T>
   T LagrangeInterpolation(const std::vector<T>& X, const std::vector<T>& Y, const T& x, T& err)
   {
      err = 0.0;
      return LagrangeInterpolation(X,Y,x);

   }  // 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)
   {
      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) {
//std::cout << "Compute Q[" << i << "," << j << "=" << (i+(j*(j+1))/2) << "] = 1 ";
                  for(k=0; k<N; k++) {
                     if(k == i || k == j) continue;
//std::cout << " * (x-X[" << k << "])";
                     Q[i+(j*(j+1))/2] *= (x-X[k]);
                  }
//std::cout << " = " << Q[i+(j*(j+1))/2] << std::endl;
               }
            }
         }
      }
      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,
                                         T desiredPos )
   {

      int degree( pos.size() );

         // First, compute interpolation factors
      typedef std::vector< T > vectorType;
      std::vector< vectorType > delta( degree, vectorType(degree, 0.0) );


      for (int i = 0; i < degree; ++i)
      {
         for (int j = 0; j < degree; ++j)
         {
            if (j != i)
            {
               delta[i][j] = ( (desiredPos - pos[j]) / (pos[i] - pos[j]) );
            }
         }
      }

      double retVal(0.0);

      for( int i = 0; i < degree; ++i )
      {
         double sum( 0.0 );

         for( int m = 0; m < degree; ++m )
         {

            if( m != i )
            {

               double weight1( 1.0/(pos[i]-pos[m]) );
               double sum2( 0.0 );

               for( int j = 0; j < degree; ++j )
               {

                  if( (j != i) && (j != m) )
                  {

                     double weight2( 1.0/(pos[i]-pos[j]) );

                     for( int 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()'


   template <class T>
   T RSS (T aa, T bb, T cc)
   {
      T a(ABS(aa)), b(ABS(bb)), c(ABS(cc));
      if ( (a > b) && (a > c) )
         return a * SQRT(1 + (b/a)*(b/a) + (c/a)*(c/a));
      if ( (b > a) && (b > c) )
         return b * SQRT(1 + (a/b)*(a/b) + (c/b)*(c/b));
      if ( (c > b) && (c > a) )
         return c * SQRT(1 + (b/c)*(b/c) + (a/c)*(a/c));

      if (a == b)
      {
         if (b == c)
            return a * SQRT(T(3));
         a *= SQRT(T(2));
         if (a > c)
            return a * SQRT(1 + (c/a)*(c/a));
         else
            return c * SQRT(1 + (a/c)*(a/c));
      }
      if (a == c)
      {
         a *= SQRT(T(2));
         if (a > b)
            return a * SQRT(1 + (b/a)*(b/a));
         else
            return b * SQRT(1 + (a/b)*(a/b));
      }
      if (b == c)
      {
         b *= SQRT(T(2));
         if (b > a)
            return b * SQRT(1 + (a/b)*(a/b));
         else
            return a * SQRT(1 + (b/a)*(b/a));
      }

      return T(0);
   }

   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)
   {
#define swapValues(x,y) \
   { T temporalStorage; \
   temporalStorage = x; x = y; y = temporalStorage; }

      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) std::swap(a,b);
      if (a < c) swapValues(a,c);
      if (a < d) swapValues(a,d);

      return a * SQRT(1 + (b/a)*(b/a) + (c/a)*(c/a) + (d/a)*(d/a));
   }


   inline double Round(double x)
   {
      return double(std::floor(x+0.5));
   }

}  // namespace gpstk

#endif

---