Diff: CubicSpline.c

Revision:

4:8db89b731133

Parent:

3:75f50dbedf1b

Child:

6:c4f36cee3ceb

diff -r 75f50dbedf1b -r 8db89b731133 CubicSpline.c
--- a/CubicSpline.c	Thu May 12 14:41:15 2016 +0000
+++ b/CubicSpline.c	Fri May 20 14:25:39 2016 +0000
@@ -1,5 +1,6 @@
 #define DEBUG
 #include "TRP105F_Spline.h"
+#include <cmath>
 
 //  To get voltage of TRP105F
 AnalogIn g_Sensor_Voltage(p16);
@@ -15,48 +16,61 @@
 DigitalOut led4(LED4);
 #endif
 
-Spline_2dCubic::Spline_2dCubic()
+CubicSpline2d::CubicSpline2d()
     :_Data_Input_Type(SYSTEM)
 {
     _Sample_Num = 5;
-    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
-    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
-
+    _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
+    //_u_param    = (double*)malloc(_Sample_Num * sizeof(double));
+    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));
+    }
     //calibrateSensor();
 }
 
-Spline_2dCubic::Spline_2dCubic(
+CubicSpline2d::CubicSpline2d(
     unsigned int arg_num
 )
     :_Data_Input_Type(SYSTEM)
 {
     _Sample_Num = arg_num;
-    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
-    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
-
+    _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
+    //_u_param    = (double*)malloc(_Sample_Num * sizeof(double));
+    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));
+    }
     //calibrateSensor();
 }
 
-Spline_2dCubic::Spline_2dCubic(
+CubicSpline2d::CubicSpline2d(
     unsigned int    arg_num,
     UseType         arg_useType
 )
     :_useType(arg_useType)
 {
     _Sample_Num = arg_num;
-    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
-    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
-
+    _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
+    //_u_param    = (double*)malloc(_Sample_Num * sizeof(double));
+    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));
+    }
     //calibrateSensor();
 }
 
-Spline_2dCubic::~Spline_2dCubic()
+CubicSpline2d::~CubicSpline2d()
 {
     free(_Sample_Set);
-    free(_u_spline);
+    //free(_u_param);
+    for(int i = 0; i < 4; i++) {
+        free(_C_x[i]);
+        free(_C_y[i]);
+    }
 }
 
-unsigned short TRP105FS::getDistance()
+unsigned short CubicSpline2d::getDistance()
 {
     int idx;
     unsigned short pv = 0;
@@ -74,157 +88,155 @@
 }
 
 
-void TRP105FS::_sampleData()
+void CubicSpline2d::_sampleData()
 {
     int     tmp;
     char    sig;
-    unsigned short tmp_vol;
-    Vector2d tmp_set[_Sample_Num];   // for bucket sort
+    Vxyt    tmp_set;
 
     //  For evry set,
     //  1, get dst data via serai com,
     //  2, get vol data,
     //  and then do same for next index set.
     for(int i = 0; i < _Sample_Num; i++) {
-        //
-        //  Recieve a Distance datus and store it into member
-        //
-        if(_Data_Input_Type == KEYBORD) {
-            g_Serial_Signal.putc('>');
-            _Sample_Set[i].dst = 0;
+        if(_useType == AsDebug) {
+            //
+            //  Recieve a Distance datus and store it into member
+            //
+            g_Serial_Signal.printf("X:");
+            _Sample_Set[i].x = 0;
             do {
                 sig = g_Serial_Signal.getc();
                 if('0' <= sig && sig <= '9') {
-                    _Sample_Set[i].dst = 10 * _Sample_Set[i].dst + sig - 48;
+                    _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
                     g_Serial_Signal.putc(char(sig));
                 } else if(sig == 0x08) {
-                    _Sample_Set[i].dst = 0;
+                    _Sample_Set[i].x = 0;
                     g_Serial_Signal.printf("[canseled!]");
                     g_Serial_Signal.putc('\n');
                     g_Serial_Signal.putc('>');
                 }
             } while (!(sig == 0x0a || sig == 0x0d));
             g_Serial_Signal.putc('\n');
-        } else { //_Data_Input_Type == SYSTEM
-            _Sample_Set[i].dst = g_Serial_Signal.getc();
+            //
+            //  Recieve a Voltage datus and store it into member
+            //
+            //  LOW PASS FILTERED
+            //  Get 10 data and store mean as a sample.
+            //  After get one original sample, system waits for 0.1 sec,
+            //  thus it takes 1 sec evry sampling.
+            _Sample_Set[i].y = 0;
+            for(int j = 0; j < 10; j++) {
+                tmp_set.y = g_Sensor_Voltage.read();
+#ifdef DEBUG
+                g_Serial_Signal.printf("%d,",tmp_set.y);
+#endif
+                _Sample_Set[i].y += (tmp_set.y / 10);
+                wait(0.1);
+            }
+#ifdef DEBUG
+            g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y);
+#endif
         }
 
-        //  if input data is over the bound calibrate it below
-        if (_Sample_Set[i].dst < 0)
-            _Sample_Set[i].dst = 0;
-        else if (_ENUM < _Sample_Set[i].dst)
-            _Sample_Set[i].dst = _ENUM;
-        //
-        //  Recieve a Voltage datus and store it into member
-        //
-        //  LOW PASS FILTERED
-        //  Get 10 data and store mean as a sample.
-        //  After get one original sample, system waits for 0.1 sec,
-        //  thus it takes 1 sec evry sampling.
-        _Sample_Set[i].vol = 0;
-        for(int j = 0; j < 10; j++) {
-            //unsigned short 's range [0 , 65535]
-            //the Number of significant figures of read voltage is 3 or 4.
-            tmp_vol = g_Sensor_Voltage.read_u16();
-#ifdef DEBUG
-            g_Serial_Signal.printf("%d,",tmp_vol);
-#endif
-            _Sample_Set[i].vol += (tmp_vol / 10);
-            wait(0.1);
-        }
-#ifdef DEBUG
-        g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].vol);
-#endif
+        //  if the input data is over the bound, it is calibrated
+        if (_Sample_Set[i].x < 0)
+            _Sample_Set[i].x = 0;
     }
     //
-    //  Sort set data array in distanceAscending order
+    //  Sort set data array in x-Ascending order
     //
-    //  Bucket sort
-    for(int i = 0; i < _ENUM; i++)
-        tmp_set[i].dst = 0xaaaa;
     tmp = 0;
-    for(int i = 0; i < _Sample_Num; i++) {
-        //  use dst as index for dst range [2,20]
-        if (tmp_set[_Sample_Set[i].dst].dst == 0xaaaa) {
-            tmp_set[_Sample_Set[i].dst].dst = _Sample_Set[i].dst;
-            tmp_set[_Sample_Set[i].dst].vol = _Sample_Set[i].vol;
-        } else { // if a same dst has been input, calcurate mean.
-            tmp_set[_Sample_Set[i].dst].vol += _Sample_Set[i].vol;
-            tmp_set[_Sample_Set[i].dst].vol /= 2;
-            tmp++;
+    for(    int i = 0              ; i < _Sumple_Num; i++) {
+        for(int j = _Sample_Num - 1; j < i+1        ; j++) {
+            //  use dst as index for dst range [2,20]
+            if (_Sample_Set[i].x > _Sample_set[j].x) {
+                tmp_set.x     = _Sample_Set[i].x;
+                tmp_set.y     = _Sample_Set[i].y;
+                _Sample_Set[i].x = _Sample_Set[j].x;
+                _Sample_Set[i].y = _Sample_Set[j].y;
+                _Sample_Set[j].x = tmp_set.x;
+                _Sample_Set[j].y = tmp_set.y;
+            }
+            // if a same dst has been input, calcurate mean.
+            else if (_Sample_Set[i].x == _Sample_set[j]) {
+                tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2;
+                _Sample_Set[i] = _Sample_Set[j] = tmp_set.y;
+                tmp++;
+            }
         }
     }
 #ifdef DEBUG
-    g_Serial_Signal.printf("%d\n", _Sample_Num );
+    g_Serial_Signal.printf("  _Sample_num: %d\n", _Sample_Num );
+    g_Serial_Signal.printf("-)        tmp: %d\n", tmp );
 #endif
     //  substruct tmp from number of sample.
     _Sample_Num -= tmp;
-
 #ifdef DEBUG
-    g_Serial_Signal.printf("tmp: %d\n", tmp );
+    g_Serial_Signal.printf("-----------------\n");
+    g_Serial_Signal.printf("  _Sample_num: %d\n", _Sample_Num );
 #endif
-    //  apply sort on _Sample_Set
-    tmp = 0;
-    for(int i = 0; i < _ENUM; i++) {
-        if(tmp_set[i].dst != 0xaaaa) {
-            _Sample_Set[i - tmp].dst = tmp_set[i].dst;
-            _Sample_Set[i - tmp].vol = tmp_set[i].vol;
-        } else //  if no data, skip it
-            tmp++;
-    }
+
+    //  generate t which is parameter related to x,y
+    _Sample_Set[i].t = 0;
+    for(int i = 1; i < _Sample_Num; i++)
+        _Sample_Set[i].t =
+            _Sample_Set[i-1]
+            + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2)
+                   +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2));
 }
 
 //
 //  Function to define _u_spline, specific constants of spline.
 //
-void TRP105FS::_makeSpline()
+void CubicSpline2d::_makeSpline(double* arg_t, double* arg_ft, double* arg_C[4], unsigned int arg_num)
 {
-    //  x: dst, distance
-    //  y: vol, voltage
+    //  arg_t :     t; parameter of x or y
+    //  arg_ft:     f(t); cubic poliminal. Value:<=> x or y.
+    //  arg_C[i]:   Ci; The parameter (set) that defines Spline Model Poliminal. Coefficient of t^i of f(t).
+    //  arg_num:    j in [0,_Sample_Num-1]; The number of interval. 
+    //  f(t)j = C3j*t^3 + C2j*t^2 + C1j*t + C0j
     //
     //  N: max of index <=> (_Sample_Num - 1)
     //
-    //  _u_spline[i] === d^2/dx^2(Spline f)[i]
+    //  u[i] === d^2/dx^2(Spline f)[i]
     //  i:[0,N]
-    //  _u_spline[0] = _u_spline[N] = 0
+    //  u[0] = u[N] = 0
+    double *u = (double*)malloc((arg_num    ) * sizeof(double));
     //
     //  h[i] = x[i+1] - x[i]
     //  i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
-    double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
-    //unsigned short *h __attribute__((at(0x20080000)));
-    //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
+    double *h = (double*)malloc((arg_num - 1) * sizeof(double));
+    //
     //  v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
     //  i:[0,N-2]
-    double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
-    //unsigned short *v __attribute__((at(0x20080100)));
-    //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
+    double *v = (double*)malloc((arg_num - 2) * sizeof(double));
+    //
     //  temporary array whose num of elm equals v array
-    double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
-    //unsigned short *w __attribute__((at(0x20080200)));
-    //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
+    double *w = (double*)malloc((arg_num - 2) * sizeof(double));
     //
-    //  [ 2(h[0]+h[1])  , h[1]          ,                                 O                 ]   [_u[1]  ]   [v[0]  ]
-    //  [ h[1]          , 2(h[1]+h[2])  , h[2]                                              ]   [_u[2]  ]   [v[1]  ]
-    //  [                       ...                                                         ] * [...    ] = [...   ]
-    //  [                   h[j]          , 2(h[j]+h[j+1])  , h[j+1]                        ]   [_u[j+1]]   [v[j]  ]
-    //  [                                   ...                                             ]   [ ...   ]   [ ...  ]
-    //  [                               h[N-3]        , 2(h[N-3]+h[N-2]), h[N-2]            ]   [_u[j+1]]   [v[j]  ]
-    //  [       O                                       h[N-2]          , 2(h[N-2]+h[N-1])  ]   [_u[N-1]]   [v[N-2]]
+    //  [ 2(h[0]+h[1])  , h[1]          ,                                 O                 ]   [u[1]  ]   [v[0]  ]
+    //  [ h[1]          , 2(h[1]+h[2])  , h[2]                                              ]   [u[2]  ]   [v[1]  ]
+    //  [                       ...                                                         ] * [...   ] = [...   ]
+    //  [                   h[j]          , 2(h[j]+h[j+1])  , h[j+1]                        ]   [u[j+1]]   [v[j]  ]
+    //  [                                   ...                                             ]   [ ...  ]   [ ...  ]
+    //  [                               h[N-3]        , 2(h[N-3]+h[N-2]), h[N-2]            ]   [u[j+1]]   [v[j]  ]
+    //  [       O                                       h[N-2]          , 2(h[N-2]+h[N-1])  ]   [u[N-1]]   [v[N-2]]
     //
     // For LU decomposition
-    double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
-    double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
+    double *Upper = (double*)malloc((arg_num - 2) * sizeof(double));
+    double *Lower = (double*)malloc((arg_num - 2) * sizeof(double));
 #ifdef DEBUG
-    _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
+    _printOutData(arg_t, arg_ft, arg_num, "\nargment set\n");
 #endif
-    for(int i = 0; i < _Sample_Num - 1; i++)
-        h[i] =  (double)(_Sample_Set[i + 1].dst - _Sample_Set[i].dst);
+    for(int i = 0; i < arg_num - 1; i++)
+        h[i] =  (double)(arg_t[i + 1] - arg_t[i]);
 
-    for(int i = 0; i < _Sample_Num - 2; i++)
+    for(int i = 0; i < arg_num - 2; i++)
         v[i] = 6 * (
-                   ((double)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol)) / h[i + 1]
+                   ((double)(arg_ft[i + 2] - arg_ft[i + 1])) / h[i + 1]
                    -
-                   ((double)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol))     / h[i]
+                   ((double)(arg_ft[i + 1] - arg_ft[i]))     / h[i]
                );
 
     //
@@ -232,7 +244,7 @@
     //
     Upper[0] = 2 * (h[0] + h[1]);
     Lower[0] = 0;
-    for (int i = 1; i < _Sample_Num - 2; i++) {
+    for (int i = 1; i < arg_num - 2; i++) {
         Lower[i] = h[i] / Upper[i - 1];
         Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
     }
@@ -242,40 +254,50 @@
     //  forward substitution
     //
     w[0] = v[0];
-    for (int i = 1; i < _Sample_Num - 2; i ++) {
+    for (int i = 1; i < arg_num - 2; i ++) {
         w[i] = v[i] - Lower[i] * w[i-1];
     }
 
-
     //
     //  backward substitution
     //
-    _u_spline[_Sample_Num - 2] =  w[_Sample_Num - 3]                         / Upper[_Sample_Num - 3];
-    for(int i = _Sample_Num - 3; i > 0; i--) {
-        _u_spline[i]           = (w[(i - 1)] -  h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
+    u[arg_num - 2] =  w[arg_num - 3]                     / Upper[arg_num - 3];
+    for(int i = arg_num - 3; i > 0; i--) {
+        u[i]       = (w[(i - 1)] -  h[(i)] * u[(i) + 1]) / Upper[(i - 1)];
     }
 
     // _u_spline[i] === d^2/dx^2(Spline f)[i]
-    _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;
+    u[0] = u[arg_num - 1] = 0.0;
 
 #ifdef DEBUG
-    _printOutData(h, _Sample_Num - 1, "h");
-    _printOutData(v, _Sample_Num - 2, "v");
-    _printOutData(w, _Sample_Num - 2, "w");
-    _printOutData(Upper, _Sample_Num - 2, "Upper");
-    _printOutData(Lower, _Sample_Num - 2, "Lower");
-    _printOutData(_u_spline, _Sample_Num, "u");
+    _printOutData(h,     arg_num - 1, "h");
+    _printOutData(v,     arg_num - 2, "v");
+    _printOutData(w,     arg_num - 2, "w");
+    _printOutData(Upper, arg_num - 2, "Upper");
+    _printOutData(Lower, arg_num - 2, "Lower");
+    _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]);
+    }
     free(h);
+    free(u);
     free(v);
     free(w);
     free(Upper);
     free(Lower);
 }
+
 //
 //  Function to return Voltage for distance.
 //
-unsigned short TRP105FS:: _getSplineYof(
+unsigned short CubicSpline2d:: _getSplineYof(
     double arg_x       //  the argument is supposed as distance [mm]
 )
 {
@@ -317,7 +339,7 @@
     return ((unsigned short)(int)y);
 }
 
-void TRP105FS::calibrateSensor()
+void CubicSpline2d::calibrateSensor()
 {
     _sampleData();
     _makeSpline();
@@ -332,7 +354,7 @@
     }
 }
 
-void TRP105FS::saveSetting()
+void CubicSpline2d::saveSetting()
 {
     FILE *fp;
 
@@ -358,12 +380,12 @@
 
 }
 
-void TRP105FS::printThresholds()
+void CubicSpline2d::printThresholds()
 {
     for(int i = 0; i < _ENUM; i++)
         g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
 }
-void TRP105FS::loadSetting()
+void CubicSpline2d::loadSetting()
 {
     FILE *fp;
     char tmp;
@@ -405,7 +427,7 @@
 }
 
 
-void TRP105FS::saveSetting(
+void CubicSpline2d::saveSetting(
     const char *filename
 )
 {
@@ -439,7 +461,7 @@
     free(filepath);
 }
 
-void TRP105FS::loadSetting(
+void CubicSpline2d::loadSetting(
     const char *filename
 )
 {
@@ -497,7 +519,7 @@
     free(filepath);
 }
 
-void TRP105FS::printOutData()
+void CubicSpline2d::printOutData()
 {
     FILE *fp;
 
@@ -513,7 +535,7 @@
     fclose(fp);
 
 }
-void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
+void CubicSpline2d::_printOutData(unsigned short *arg, int num, char* name)
 {
     FILE *fp;
     fp = fopen("/local/varlog.txt", "a");  // open file in add mode
@@ -524,7 +546,7 @@
     fprintf(fp, "\n");
     fclose(fp);
 }
-void TRP105FS::_printOutData(double *arg, int num, char* name)
+void CubicSpline2d::_printOutData(double *arg, int num, char* name)
 {
     FILE *fp;
 
@@ -536,7 +558,19 @@
     fprintf(fp, "\n");
     fclose(fp);
 }
-void TRP105FS::_printOutData(VDset *arg, int num, char* name)
+void CubicSpline2d::_printOutDataCouple(double *arg1, double *arg2, int num, char* name)
+{
+    FILE *fp;
+
+    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
+    fprintf(fp, "%10s\n", name);
+    for(int i = 0; i < num; i++) {
+        fprintf(fp, "(%.2f, %.2f)\n", arg1[i], arg2[i]);
+    }
+    fprintf(fp, "\n");
+    fclose(fp);
+}
+void CubicSpline2d::_printOutData(Vxyt *arg, int num, char* name)
 {
     FILE *fp;