Diff: CubicSpline.c

Revision:

7:e032ddec6ed5

Parent:

6:c4f36cee3ceb

Child:

8:e7d451bb4fd4

--- a/CubicSpline.c	Fri May 20 14:28:42 2016 +0000
+++ b/CubicSpline.c	Tue May 24 17:37:27 2016 +0000
@@ -1,6 +1,5 @@
 #define DEBUG
-#include "TRP105F_Spline.h"
-#include <cmath>
+#include "CubicSpline.h"
 
 //  To get voltage of TRP105F
 AnalogIn g_Sensor_Voltage(p16);
@@ -23,8 +22,8 @@
     _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));
+        _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
+        _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
     }
     //calibrateSensor();
 }
@@ -38,8 +37,8 @@
     _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));
+        _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
+        _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
     }
     //calibrateSensor();
 }
@@ -54,8 +53,8 @@
     _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));
+        _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
+        _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
     }
     //calibrateSensor();
 }
@@ -70,24 +69,6 @@
     }
 }
 
-unsigned short CubicSpline2d::getDistance()
-{
-    int idx;
-    unsigned short pv = 0;
-
-    //  low-pass filter
-    for(int i = 0; i < 10; i++)
-        pv += g_Sensor_Voltage.read_u16() / 10;
-
-    idx = _getNearest(_LIDX, _RIDX, pv);
-
-    if (idx != 0xFFFF)    //  unless occuring error
-        return _Set[idx].dst;
-    else
-        return 0xFFFF;
-}
-
-
 void CubicSpline2d::_sampleData()
 {
     int     tmp;
@@ -181,20 +162,21 @@
     _Sample_Set[i].t = 0;
     for(int i = 1; i < _Sample_Num; i++)
         _Sample_Set[i].t =
-            _Sample_Set[i-1]
+            _Sample_Set[i-1].t
             + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2)
                    +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2));
 }
 
+#define VERSION_C
 //
 //  Function to define _u_spline, specific constants of spline.
 //
-void CubicSpline2d::_makeModel(double* arg_t, double* arg_ft, double* arg_C[4], unsigned int arg_num)
+void CubicSpline2d::_makeModel(double* arg_t, double* arg_ft, double* arg_C[4], const unsigned int arg_num)
 {
-    //  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. 
+    //  arg_t :     t; The variable of f(t)
+    //  arg_ft:     f(t); The cubic poliminal in Interval-j.
+    //  arg_C[i]:   Ci; The coefficient of t^i of f(t) that defines Spline Model Poliminal 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)
@@ -202,7 +184,13 @@
     //  u[i] === d^2/dx^2(Spline f)[i]
     //  i:[0,N]
     //  u[0] = u[N] = 0
+#if defined (VERSION_C)
     double *u = (double*)malloc((arg_num    ) * sizeof(double));
+#elif defined (VERSION_C++)
+    double *u = new double[arg_num];
+#elif defined (VERSION_C++11)
+    std::array<double,arg_num> u;
+#endif
     //
     //  h[i] = x[i+1] - x[i]
     //  i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
@@ -293,50 +281,79 @@
     free(Upper);
     free(Lower);
 }
-
-//
-//  Function to return Voltage for distance.
 //
-unsigned short CubicSpline2d:: _getSplineYof(
-    double arg_x       //  the argument is supposed as distance [mm]
-)
+//  Fuction to return the value of Cubic polyminal f(t)
+//
+double  CubicSpline2d::_cubic_f(const double   arg_t, const double* arg_C[4])
 {
-    double y;       //  voltage calculated by spline polynomial
-    double a,b,c,d; //  which is specific constant of spline, and can be expressed with _u.
-    int itv = 0;    //  interval(section) of interpolation
-    //  the number of interval is less 1 than the number of sample sets,
-    //  which means the max number of interval is _Sample_num - 2.
-    if((double)(_Sample_Set[0].dst) <= arg_x) {
-        while (!((double)(_Sample_Set[itv].dst) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].dst))) {
-            itv++;
-            if(itv > _Sample_Num - 2) {
-                itv = _Sample_Num - 2;
-                break;
-            }
-        }
+    double ft;  //the value of Spline f(t).
+
+    ft = arg_C[3] * pow(arg_t, 3) + arg_C[2] * pow(arg_t, 2) + arg_C[1] * arg_t + arg_C[0];
+
+    return ft;
+}
+//
+//  Function to solve a cubic poliminal
+//  by using Gardano-Tartaglia formula
+//
+void _solve_cubic_f(
+    std::complex<double>* arg_t,
+    const double* arg_C[4],
+    const double  arg_ft)
+{
+    double c[3];
+    //f(t)  = arg_ft/arg_C[3]
+    //      = t^3 + c[2]*t^2 + c[1]*t + c[0].
+    for(int i = 0; i < 3; i++) {
+        c[i] = arg_C[i] / arg_C[3];
     }
-    a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst);
-    b = (double)(_u_spline[itv]) / 2.0;
-    c = (double)(_Sample_Set[itv + 1].vol - _Sample_Set[itv].vol) / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst)
-        -
-        (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0;
-    d = (double)(_Sample_Set[itv].vol);
-    //  cubic spline expression
-    y = a * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
-        +
-        b * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
-        +
-        c * (arg_x - (double)(_Sample_Set[itv].dst))
-        +
-        d;
+    //modify the formula
+    //t^3 + c[2]*t^2 + c[1]*t + (c[0] - ft) = 0.
+    c[0] -= arg_ft / argC[3];
+
+    //The values defined from coefficients of the formula
+    //that identify solutions
+    double p,q,d;
+    p = (   -pow(c[2], 2) + 3 * c[1]) / 9;
+    q = (2 * pow(c[2], 3) - 9 * c[2] * c[1] + 27 * c[0]) / 54;
+    d = - c[2] / 3;
+
+    //Discriminant section
+    double D;
+    D = pow(p, 3) + pow(q, 2);
+
+    //The values defined from p and q
+    //that idetify solutions
+    std::complex<double> u,v;
 
-#ifdef DEBUG2
-    g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
-    g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
-    g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
-#endif
+    //Real root only
+    if(D <= 0) {
+        u.real(-q);
+        u.imag(+sqrt(-D));
+        v.real(-q);
+        v.real(-sqrt(-D));
+    }
+    //One real root and two complex root
+    else {
+        u.real(-q+sqrt(D));
+        u.imag(0.0);
+        v.real(-q-sqrt(D));
+        v.real(0.0);
+    }
+    u = pow(u, 1/3);
+    v = pow(v, 1/3);
 
-    return ((unsigned short)(int)y);
+    //Cubic root of 1
+    std::complex<double> omega[3]= {
+        std::complex<double>( 1.0, 0.0),
+        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;
 }
 
 void CubicSpline2d::calibrateSensor()