QuadraticInterpolator.h

#ifndef CARMA_UTIL_QUADRATICINTERPOLATOR_H
#define CARMA_UTIL_QUADRATICINTERPOLATOR_H

#if 0
#include <iostream>
#endif
#include <pthread.h>


// Define pi

//#define pi 3.1415926535897932384626433832795028841971693993751

// Define 2*pi 

//#define twopi 6.2831853071795864769252867665590057683943387987502

#define QP_ANGLE_FN(fn)  double (fn)(double angle)

namespace carma {
  namespace util {
    
    class QuadraticInterpolator {
      
    public:

      static const double pi_;
      static const double twopi_;
      
      enum QuadType {
        
        QP_NORMAL,
        
        QP_SIGNED_ANGLE,
        
        QP_POSITIVE_ANGLE
      };
      
      //------------------------------------------------------------
      // Public methods
      //------------------------------------------------------------
      
      virtual ~QuadraticInterpolator();
      
      void empty();
      
      void extend(double x, double y);
      
      double evaluate(double x);
      
      double gradient(double x);

      void lock();

      void unlock();

      bool tryLock();

      bool canBracket(double x);

      unsigned getNpt();

      double getXmin();

      double getXmax();

      void getXvals( double xVals[3], unsigned * numXvals );

#if 0

      friend std::ostream& operator<<(std::ostream& os, QuadraticInterpolator& quad);
#endif
      void print();

    protected:

      QuadType type_;

      QuadraticInterpolator();
      
      void setEmptyValue(double emptyValue);

    private:
      
      //------------------------------------------------------------
      // Resources for locking this container
      //------------------------------------------------------------

      bool mutexIsReady_;

      pthread_mutex_t mutex_;

      //------------------------------------------------------------
      // Structs used by this class.
      //------------------------------------------------------------

      struct QuadSample {
        double x;
        double y;
      };
      
      struct QuadData {
        
        int npt;          
        
        QuadSample s[3]; 
        
        void init();
      };
      
      //------------------------------------------------------------
      // Private data members
      //------------------------------------------------------------
      
      double emptyValue_;      // The value to use when npt==0 
      double x0_;              // A value which will be subtracted off
                               // of the x-value to maintain accuracy
      double a_,b_,c_;         // The coefficients of the quadratic
                               // equation that interpolates x[],y[]
      QuadSample s_[3];        // The samples to interpolate between 
      int npt_;                // The number of values in x[] and y[]
      
      //------------------------------------------------------------
      // Private methods
      //------------------------------------------------------------

      virtual double fixAngle(double angle); 

      void get(QuadData* data);
      
      void set(QuadData* data);
      
      double extendAngle(double a, double b);
    };
  };
};

#endif