Diff: CubicSpline.c

Revision:

8:e7d451bb4fd4

Parent:

7:e032ddec6ed5

--- a/CubicSpline.c	Tue May 24 17:37:27 2016 +0000
+++ b/CubicSpline.c	Thu May 26 04:50:45 2016 +0000
@@ -20,7 +20,9 @@
 {
     _Sample_Num = 5;
     _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
-    //_u_param    = (double*)malloc(_Sample_Num * sizeof(double));
+    _Last_Point = (Vxyt) {
+        0,0,0
+    };
     for(int i = 0; i < _4; i++) {
         _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
         _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -35,7 +37,9 @@
 {
     _Sample_Num = arg_num;
     _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
-    //_u_param    = (double*)malloc(_Sample_Num * sizeof(double));
+    _Last_Point = (Vxyt) {
+        0,0,0
+    };
     for(int i = 0; i < 4; i++) {
         _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
         _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -51,7 +55,9 @@
 {
     _Sample_Num = arg_num;
     _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
-    //_u_param    = (double*)malloc(_Sample_Num * sizeof(double));
+    _Last_Point = (Vxyt) {
+        0,0,0
+    };
     for(int i = 0; i < 4; i++) {
         _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
         _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -266,13 +272,13 @@
     _printOutData(u,     arg_num    , "u");
 #endif
 
-    for(int itv = 0; itv < arg_num - 1; itv++) {
-        C[3][itv] = (u[itv + 1] - u[itv]) / 6.0 / (arg_t[itv + 1] - arg_t[itv]);
-        C[2][itv] = (u[itv]) / 2.0;
-        C[1][itv] = (arg_ft[itv + 1] - arg_ft[itv]) / (arg_t[itv + 1] - arg_t[itv])
-                    -
-                    (arg_t[itv + 1]  - arg_t[itv])  * (u[itv + 1] + 2.0 * u[itv]) / 6.0;
-        C[0][itv] = (arg_ft[itv]);
+    for(int ival = 0; ival < arg_num - 1; ival++) {
+        C[3][ival] = (u[ival + 1] - u[ival]) / 6.0 / (arg_t[ival + 1] - arg_t[ival]);
+        C[2][ival] = (u[ival]) / 2.0;
+        C[1][ival] = (arg_ft[ival + 1] - arg_ft[ival]) / (arg_t[ival + 1] - arg_t[ival])
+                     -
+                     (arg_t[ival + 1]  - arg_t[ival])  * (u[ival + 1] + 2.0 * u[ival]) / 6.0;
+        C[0][ival] = (arg_ft[ival]);
     }
     free(h);
     free(u);
@@ -282,9 +288,9 @@
     free(Lower);
 }
 //
-//  Fuction to return the value of Cubic polyminal f(t)
+//  Fuction to return the value of Cubic polynomial f(t)
 //
-double  CubicSpline2d::_cubic_f(const double   arg_t, const double* arg_C[4])
+double  CubicSpline2d::_cubic_f(const double   arg_t, const double arg_C[4])
 {
     double ft;  //the value of Spline f(t).
 
@@ -293,12 +299,12 @@
     return ft;
 }
 //
-//  Function to solve a cubic poliminal
+//  Function to solve a cubic polinomial
 //  by using Gardano-Tartaglia formula
 //
-void _solve_cubic_f(
+void CubicSpline2d::_solve_cubic_f(
     std::complex<double>* arg_t,
-    const double* arg_C[4],
+    const double  arg_C[4],
     const double  arg_ft)
 {
     double c[3];
@@ -349,26 +355,137 @@
         std::complex<double>(-1/2, sqrt(3)/2),
         std::complex<double>(-1/2,-sqrt(3)/2)
     };
-    
+
     //Solution of the formula
     arg_t[0] = omega[0] * u + omega[0] * v + d;
     arg_t[1] = omega[1] * u + omega[2] * v + d;
     arg_t[2] = omega[2] * u + omega[1] * v + d;
 }
 
+double  CubicSpline2d::getX(double arg_y)
+{
+    double x;
+    double C[4];
+    double the_t;
+    int    the_i;
+    std::complex<double>t_sol[3];
+    std::vector<double> t_real;
+    std::vector<int>    t_ival;
+
+    //  For the every Intervals of Spline,
+    //it solves the polynomial defined by C[i] of the interval,
+    //checks the solutions are real number,
+    //and ckecks the solutions are in the interval.
+    //  And if not-excluded solutions are more than one,
+    //it trys to find which one is more nearest to last point.
+    for(int ival = 0; ival < _Sample_Num - 1; ival++) {
+        for(int i = 0; i < 4; i++) C[i] = _C_y[i][ival];
+        _solve_cubic_f(t_sol, C, arg_y);
+        for(int i = 0; i < 3; i++) {
+            //  regarding only real solution
+            //  acuracy (error range) is supposed +-10E-3 here(groundless)
+            if(std::abs(t_sol[i].imag()) < 0.001) {
+                /*  */ if (ival == 0               && t_sol[i].real() < _Sample_Set[ival].t) {
+                    t_real.push_back(_Sample_Set[ival].t);
+                    t_ival.push_back(ival);
+                } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
+                    t_real.push_back(_Sample_Set[ival + 1].t);
+                    t_ival.push_back(ival);
+                } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
+                    t_real.push_back(t_sol[i].real());
+                    t_ival.push_back(ival);
+                }
+            }
+        }
+
+
+        the_t = t_real[0];
+        the_i = t_ival[0];
+        //if t's size is bigger than 1
+        for(int i = 1; i < t_real.size(); i++) {
+            if(std::abs(t_real[i] - _Last_Point.t) < std::abs(t - _Last_Point.t)) {
+                the_t = t_real[i];
+                the_i = t_ival[i];
+            }
+        }
+        for(int i = 0; i < 4; i++) C[i] = _C_y[i][the_i];
+        x = _cubic_f(the_t, C);
+    }
+
+    return x;
+}
+double  CubicSpline2d::getY(double arg_x)
+{
+    double y;
+    double C[4];
+    double the_t;
+    int    the_i;
+    std::complex<double>t_sol[3];
+    std::vector<double> t_real;
+    std::vector<int>    t_ival;
+
+    //  For the every Intervals of Spline,
+    //it solves the polynomial defined by C[i] of the interval,
+    //checks the solutions are real number,
+    //and ckecks the solutions are in the interval.
+    //  And if not-excluded solutions are more than one,
+    //it trys to find which one is more nearest to last point.
+    for(int ival = 0; ival < _Sample_Num - 1; ival++) {
+        for(int i = 0; i < 4; i++) C[i] = _C_x[i][ival];
+        _solve_cubic_f(t_sol, C, arg_x);
+        for(int i = 0; i < 3; i++) {
+            //  regarding only real solution
+            //  acuracy (error range) is supposed +-10E-3 here(groundless)
+            if(std::abs(t_sol[i].imag()) < 0.001) {
+                /*  */ if (ival == 0               && t_sol[i].real() < _Sample_Set[ival].t) {
+                    t_real.push_back(_Sample_Set[ival].t);
+                    t_ival.push_back(ival);
+                } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
+                    t_real.push_back(_Sample_Set[ival + 1].t);
+                    t_ival.push_back(ival);
+                } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
+                    t_real.push_back(t_sol[i].real());
+                    t_ival.push_back(ival);
+                }
+            }
+        }
+
+
+        the_t = t_real[0];
+        the_i = t_ival[0];
+        //if t's size is bigger than 1
+        for(int i = 1; i < t_real.size(); i++) {
+            if(std::abs(t_real[i] - _Last_Point.t) < std::abs(t - _Last_Point.t)) {
+                the_t = t_real[i];
+                the_i = t_ival[i];
+            }
+        }
+        for(int i = 0; i < 4; i++) C[i] = _C_x[i][the_i];
+        y = _cubic_f(the_t, C);
+    }
+
+    return y;
+}
+
+
 void CubicSpline2d::calibrateSensor()
 {
+    double t[_Sample_Num];
+    double ft[_Sample_Num];
+    
     _sampleData();
-    _makeSpline();
-
-    for(int i = 0; i < _ENUM; i++) {
-        _Set[i].dst     = i;
-        _Set[i].vol     = _getSplineYof((double)(_Set[i].dst));
-        _Threshold[i]   = _getSplineYof((double)(_Set[i].dst) + 0.5);
-#ifdef DEBUG2
-        g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
-#endif
+    _Last_Point = _Sample_Set[0];
+    
+    for(int i = 0; i < _Sample_Num; i++){
+        t[i] = _Sample_Set[i].t;
+        ft[i]= _Sample_Set[i].x;
     }
+    _makeModel(t,ft,_C_x);
+    for(int i = 0; i < _Sample_Num; i++){
+        ft[i]= _Sample_Set[i].y;
+    }
+    _makeModel(t,ft,_C_y);
+    
 }
 
 void CubicSpline2d::saveSetting()
@@ -377,73 +494,37 @@
 
     fp = fopen("/local/savedata.log", "wb");
 
-    for(int i = 0; i < _ENUM; i++) {
-        fwrite(&_Set[i].dst,    sizeof(unsigned short), 1, fp);
+    //  Save _Sample_Num
+    fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
+    fputc(0x3b, fp);
+    //  Save _Sample_Set
+    for(int i = 0; i < _Sample_Num; i++) {
+        fwrite(&_Sample_Set[i].x,    sizeof(double), 1, fp);
         fputc(0x2c, fp);
-        fwrite(&_Set[i].vol,    sizeof(unsigned short), 1, fp);
+        fwrite(&_Sample_Set[i].y,    sizeof(double), 1, fp);
         fputc(0x2c, fp);
-        fwrite(&_Threshold[i],  sizeof(unsigned short), 1, fp);
+        fwrite(&_Sample_Set[i].t,    sizeof(double), 1, fp);
         fputc(0x3b, fp);
     }
-    fwrite(&_Sample_Num, sizeof(int), 1, fp);
-    fputc(0x3b, fp);
-    for(int i = 0; i < _Sample_Num; i++) {
-        fwrite(&_Sample_Set[i].dst,    sizeof(unsigned short), 1, fp);
-        fputc(0x2c, fp);
-        fwrite(&_Sample_Set[i].vol,    sizeof(unsigned short), 1, fp);
-        fputc(0x3b, fp);
+    //  Save _C_x
+    for(int i = 0; i < _Sample_Num - 1; i++){
+        for(int j = 0; j < 4; j++){
+            fwrite(&_C_x[j][i],    sizeof(double), 1, fp);
+            fputc((j != 3)? 0x2c : 0x3b, fp);
+        }
     }
+    //  Save _C_y
+    for(int i = 0; i < _Sample_Num - 1; i++){
+        for(int j = 0; j < 4; j++){
+            fwrite(&_C_y[j][i],    sizeof(double), 1, fp);
+            fputc((j != 3)? 0x2c : 0x3b, fp);
+        }
+    }
+    
     fclose(fp);
 
 }
 
-void CubicSpline2d::printThresholds()
-{
-    for(int i = 0; i < _ENUM; i++)
-        g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
-}
-void CubicSpline2d::loadSetting()
-{
-    FILE *fp;
-    char tmp;
-
-    //sprintf(filepath, "/local/%s", filename);
-    //fp = fopen(filepath, "rb");
-    fp = fopen("/local/savedata.log", "rb");
-    for(int i = 0; i < _ENUM; i++) {
-
-        fread(&_Set[i].dst,     sizeof(unsigned short), 1, fp);
-        fread(&tmp,             sizeof(char),           1, fp);
-#ifdef DEBUG2
-        g_Serial_Signal.printf("%d%c",  _Set[i].dst, tmp);
-#endif
-
-        fread(&_Set[i].vol,     sizeof(unsigned short), 1, fp);
-        fread(&tmp,             sizeof(char), 1, fp);
-#ifdef DEBUG2
-        g_Serial_Signal.printf("%d%c",  _Set[i].vol, tmp);
-#endif
-
-        fread(&_Threshold[i],   sizeof(unsigned short), 1, fp);
-        fread(&tmp,             sizeof(char), 1, fp);
-#ifdef DEBUG2
-        g_Serial_Signal.printf("%d%c\n",_Threshold[i],      tmp);
-#endif
-    }
-
-    fread(&_Sample_Num, sizeof(unsigned short),  1, fp);
-    fread(&tmp,         sizeof(char), 1, fp);
-
-    for(int i = 0; i < _Sample_Num; i++) {
-        fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
-        fread(&tmp, sizeof(char),1,fp);
-        fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
-        fread(&tmp, sizeof(char),1,fp);
-    }
-    fclose(fp);
-}
-
-
 void CubicSpline2d::saveSetting(
     const char *filename
 )
@@ -457,27 +538,78 @@
 
     sprintf(filepath, "/local/%s", filename);
     fp = fopen(filepath, "wb");
-
-    for(int i = 0; i < _ENUM; i++) {
-        fwrite(&_Set[i].dst,    sizeof(unsigned short), 1, fp);
+    
+    //  Save _Sample_Num
+    fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
+    fputc(0x3b, fp);
+    //  Save _Sample_Set
+    for(int i = 0; i < _Sample_Num; i++) {
+        fwrite(&_Sample_Set[i].x,    sizeof(double), 1, fp);
         fputc(0x2c, fp);
-        fwrite(&_Set[i].vol,    sizeof(unsigned short), 1, fp);
+        fwrite(&_Sample_Set[i].y,    sizeof(double), 1, fp);
         fputc(0x2c, fp);
-        fwrite(&_Threshold[i],  sizeof(unsigned short), 1, fp);
+        fwrite(&_Sample_Set[i].t,    sizeof(double), 1, fp);
         fputc(0x3b, fp);
     }
-    fwrite(&_Sample_Num, sizeof(int), 1, fp);
-    fputc(0x3b, fp);
-    for(int i = 0; i < _Sample_Num; i++) {
-        fwrite(&_Sample_Set[i].dst,    sizeof(unsigned short), 1, fp);
-        fputc(0x2c, fp);
-        fwrite(&_Sample_Set[i].vol,    sizeof(unsigned short), 1, fp);
-        fputc(0x3b, fp);
+    //  Save _C_x
+    for(int i = 0; i < _Sample_Num - 1; i++){
+        for(int j = 0; j < 4; j++){
+            fwrite(&_C_x[j][i],    sizeof(double), 1, fp);
+            fputc((j != 3)? 0x2c : 0x3b, fp);
+        }
     }
+    //  Save _C_y
+    for(int i = 0; i < _Sample_Num - 1; i++){
+        for(int j = 0; j < 4; j++){
+            fwrite(&_C_y[j][i],    sizeof(double), 1, fp);
+            fputc((j != 3)? 0x2c : 0x3b, fp);
+        }
+    }
+    
     fclose(fp);
     free(filepath);
 }
 
+void CubicSpline2d::loadSetting()
+{
+    FILE *fp;
+    char tmp;
+
+    //sprintf(filepath, "/local/%s", filename);
+    //fp = fopen(filepath, "rb");
+    fp = fopen("/local/savedata.log", "rb");
+    
+    //  Load _Sample_Num
+    fread(&_Sample_Num, sizeof(unsigned short),  1, fp);
+    fread(&tmp,         sizeof(char), 1, fp);
+
+    //  Load _Sample_Set
+    for(int i = 0; i < _Sample_Num; i++) {
+        fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+        fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+        fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+    }
+    
+    //  Load _C_x
+    for(int i = 0; i < _Sample_Num - 1; i++) {
+        for(int j = 0; j < 4; j++){
+        fread(&_C_x[j][i], sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+    }
+    
+    //  Load _C_y
+    for(int i = 0; i < _Sample_Num - 1; i++) {
+        for(int j = 0; j < 4; j++){
+        fread(&_C_y[j][i], sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+    }
+    fclose(fp);
+}
+
+
 void CubicSpline2d::loadSetting(
     const char *filename
 )
@@ -492,45 +624,33 @@
 
     sprintf(filepath, "/local/%s", filename);
     fp = fopen(filepath, "rb");
-    for(int i = 0; i < _ENUM; i++) {
 
-        fread(&_Set[i].dst,     sizeof(unsigned short), 1, fp);
-        fread(&tmp,             sizeof(char), 1, fp);
-#ifdef DEBUG3
-        g_Serial_Signal.printf("%d%c",  _Set[i].dst, tmp);
-#endif
+    //  Load _Sample_Num
+    fread(&_Sample_Num, sizeof(unsigned short),  1, fp);
+    fread(&tmp,         sizeof(char), 1, fp);
 
-        fread(&_Set[i].vol,     sizeof(unsigned short), 1, fp);
-        fread(&tmp,             sizeof(char), 1, fp);
-#ifdef DEBUG3
-        g_Serial_Signal.printf("%d%c",  _Set[i].vol, tmp);
-#endif
-
-        fread(&_Threshold[i],   sizeof(unsigned short), 1, fp);
-        fread(&tmp,             sizeof(char), 1, fp);
-#ifdef DEBUG3
-        g_Serial_Signal.printf("%d%c\n",_Threshold[i],      tmp);
-#endif
+    //  Load _Sample_Set
+    for(int i = 0; i < _Sample_Num; i++) {
+        fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+        fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+        fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
     }
-
-    fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
-    fread(&tmp,         sizeof(char),           1, fp);
-#ifdef DEBUG3
-    g_Serial_Signal.printf("%d%c\n",_Sample_Num,      tmp);
-#endif
-
-    for(int i = 0; i < _Sample_Num; i++) {
-        fread(&_Sample_Set[i].dst,  sizeof(unsigned short), 1, fp);
-        fread(&tmp,                 sizeof(char),1,fp);
-#ifdef DEBUG3
-        g_Serial_Signal.printf("%d%c",  _Sample_Set[i].dst, tmp);
-#endif
-
-        fread(&_Sample_Set[i].vol,  sizeof(unsigned short), 1, fp);
-        fread(&tmp,                 sizeof(char),1,fp);
-#ifdef DEBUG3
-        g_Serial_Signal.printf("%d%c",  _Sample_Set[i].vol, tmp);
-#endif
+    
+    //  Load _C_x
+    for(int i = 0; i < _Sample_Num - 1; i++) {
+        for(int j = 0; j < 4; j++){
+        fread(&_C_x[j][i], sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+    }
+    
+    //  Load _C_y
+    for(int i = 0; i < _Sample_Num - 1; i++) {
+        for(int j = 0; j < 4; j++){
+        fread(&_C_y[j][i], sizeof(double), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
     }
     fclose(fp);
     free(filepath);