CubicSpline.h

00001 /**
00002  *  Spline_Cubic.h,.cpp
00003  *
00004  *  Author: aktk, aktk.j.uec@gmail.com
00005  *  Tokyo, Japan.
00006  *
00007  *  LOG:
00008  *  ver.0   2016/02.12 - 2016/02/16
00009  *      TRP105F_Spline.h,.cpp(Ver.2.1)
00010  *  ver.1   2016/05.12
00011  */
00012 #ifndef Cubic_Spline_H
00013 #define Cubic_Spline_H
00014 
00015 #define DEBUG
00016 #define VERSION_C
00017 //#define DEBUG_MAKE_MODEL
00018 //#define DEBUG_SOLVE
00019 //#define DEBUG_GETX "DEBUG_GETX\n"
00020 //#define DEBUG_GETY "DEBUG_GETY\n"
00021 
00022 #include "mbed.h"
00023 #include <cmath>
00024 #include <complex>
00025 #include <vector>
00026 
00027 //  Vector Element Type
00028 typedef struct {
00029     double x; //
00030     double y; //
00031     double t; //    use as pramameter of x,y.
00032 } Vxyt;
00033 
00034 enum UseType {
00035     AsDEBUG,
00036     AsMODULE
00037 };
00038 
00039 class CubicSpline2d
00040 {
00041 public:
00042     //  Constraction
00043     CubicSpline2d();
00044     CubicSpline2d(unsigned int);
00045     CubicSpline2d(unsigned int, UseType);
00046     //  Destraction
00047     ~CubicSpline2d();
00048     //  Functions
00049     double  getX(double arg_y);
00050     double  getY(double arg_x);
00051     void    calibrateSensor();
00052     void    saveSetting();
00053     void    saveSetting(const char *filename);
00054     void    loadSetting();
00055     void    loadSetting(const char *filename);
00056     void    printOutData();
00057 
00058 private:
00059     //
00060     //  Variables
00061     //
00062     UseType      _useType;
00063     unsigned int _Sample_Num;   //  the number of samples for derive spline
00064     Vxyt*        _Sample_Set;
00065     double*      _C_x[4];    //x = Spline-f(t) = _C_x[0]  + _C_x[1]t  + _C_x[2]t^2  + _C_x[3]t^3
00066     double*      _C_y[4];    //y = Spline-f(t) = _C_y[0]  + _C_y[1]t  + _C_y[2]t^2  + _C_y[3]t^3
00067     Vxyt        _Last_Point;
00068     //
00069     //
00070     //
00071     //
00072     //  For calibration
00073     //
00074     //  sampling data for calibration
00075     void    _sampleData();
00076     //  generate a vector of _u_params which is used for Cubic spline model
00077     void    _makeModel(
00078         const double* arg_sampled_t,
00079         const double* arg_sampled_ft,
00080         /*-*/ double* arg_C[4],
00081         const unsigned int arg_num
00082     );
00083     void    _makeModel(
00084         const double* arg_sampled_t,
00085         const double* arg_sampled_ft,
00086         /*-*/ double* arg_C[4]
00087     );
00088     //
00089     //  For calculation
00090     //
00091     //  Fuction to return the value of Cubic polynomial f(t)
00092     double  _cubic_f(
00093         const double  arg_t,
00094         const double  arg_C[4]
00095     );
00096     //  Function to solve a cubic polinomial
00097     //  by using Gardano-Tartaglia formula
00098     void _solve_cubic_f(
00099         std::complex<double>* arg_t,
00100         const double  arg_C[4],
00101         const double  arg_ft
00102     );
00103     //
00104     //  For debug
00105     //
00106     void    _printOutData(
00107         const unsigned short*   arg,
00108         const int               num,
00109         const char*             name);
00110     void    _printOutData(
00111         const Vxyt*             arg,
00112         const int               num,
00113         const char*             name);
00114     void    _printOutData(
00115         const double*           arg, 
00116         const int               num,
00117         const char*             name);
00118     void    _printOutDataCouple(
00119         const double*           arg1,
00120         const double*           arg2,
00121         const int               num,
00122         const char*             name);
00123 };
00124 #endif