Akifumi Takahashi / CubicSpline

This lib is considered to be used as a sensor's calibration program. Calibration with Spline Interpolation might be useful in the case that you want some model expressing relationship such like between a value of physical quantity and your sensor's voltage, but you cannot estimate a model such as liner, square, cubic polynomial, or sine curve. This makes (Parametric) Cubic Spline Polynomial Model (Coefficients of the polynomial) from some sample plots(e.g. sets of (value, voltage)). The inverse function (x,y)->(y,x) has been implemented so as to get analog data (not stepping or leveled data).

Fork of TRP105F_Spline by Akifumi Takahashi

CubicSpline.cpp@13:9a51747773af, 2016-05-30 (annotated)

Committer:
aktk
Date:
Mon May 30 09:18:50 2016 +0000
Revision:
13:9a51747773af
Parent:
12:b3e07a2220bc 
all function has briefly implemented.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aktk 9:1903c6f8d5a9 1 #include "CubicSpline.h"
aktk 9:1903c6f8d5a9 2
aktk 9:1903c6f8d5a9 3 // To get voltage of TRP105F
aktk 9:1903c6f8d5a9 4 AnalogIn g_Sensor_Voltage(p16);
aktk 9:1903c6f8d5a9 5 // To get sample distance via seral com
aktk 9:1903c6f8d5a9 6 Serial g_Serial_Signal(USBTX, USBRX);
aktk 9:1903c6f8d5a9 7
aktk 9:1903c6f8d5a9 8 LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる）
aktk 9:1903c6f8d5a9 9 // for debug
aktk 9:1903c6f8d5a9 10 #ifdef DEBUG
aktk 9:1903c6f8d5a9 11 DigitalOut led1(LED1);
aktk 9:1903c6f8d5a9 12 DigitalOut led2(LED2);
aktk 9:1903c6f8d5a9 13 DigitalOut led3(LED3);
aktk 9:1903c6f8d5a9 14 DigitalOut led4(LED4);
aktk 9:1903c6f8d5a9 15 #endif
aktk 9:1903c6f8d5a9 16
aktk 9:1903c6f8d5a9 17 CubicSpline2d::CubicSpline2d()
aktk 9:1903c6f8d5a9 18 :_useType(AsMODULE)
aktk 9:1903c6f8d5a9 19 {
aktk 9:1903c6f8d5a9 20 _Sample_Num = 5;
aktk 9:1903c6f8d5a9 21 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
aktk 9:1903c6f8d5a9 22 _Last_Point = (Vxyt) {
aktk 13:9a51747773af 23 0.0, 0.0, 0.0
aktk 9:1903c6f8d5a9 24 };
aktk 9:1903c6f8d5a9 25 for(int i = 0; i < 4; i++) {
aktk 9:1903c6f8d5a9 26 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 9:1903c6f8d5a9 27 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 9:1903c6f8d5a9 28 }
aktk 9:1903c6f8d5a9 29 //calibrateSensor();
aktk 9:1903c6f8d5a9 30 }
aktk 9:1903c6f8d5a9 31
aktk 9:1903c6f8d5a9 32 CubicSpline2d::CubicSpline2d(
aktk 9:1903c6f8d5a9 33 unsigned int arg_num
aktk 9:1903c6f8d5a9 34 )
aktk 9:1903c6f8d5a9 35 :_useType(AsMODULE)
aktk 9:1903c6f8d5a9 36 {
aktk 9:1903c6f8d5a9 37 _Sample_Num = arg_num;
aktk 9:1903c6f8d5a9 38 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
aktk 9:1903c6f8d5a9 39 _Last_Point = (Vxyt) {
aktk 13:9a51747773af 40 0.0, 0.0, 0.0
aktk 9:1903c6f8d5a9 41 };
aktk 9:1903c6f8d5a9 42 for(int i = 0; i < 4; i++) {
aktk 9:1903c6f8d5a9 43 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 9:1903c6f8d5a9 44 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 9:1903c6f8d5a9 45 }
aktk 9:1903c6f8d5a9 46 //calibrateSensor();
aktk 9:1903c6f8d5a9 47 }
aktk 9:1903c6f8d5a9 48
aktk 9:1903c6f8d5a9 49 CubicSpline2d::CubicSpline2d(
aktk 9:1903c6f8d5a9 50 unsigned int arg_num,
aktk 9:1903c6f8d5a9 51 UseType arg_useType
aktk 9:1903c6f8d5a9 52 )
aktk 9:1903c6f8d5a9 53 :_useType(arg_useType)
aktk 9:1903c6f8d5a9 54 {
aktk 9:1903c6f8d5a9 55 _Sample_Num = arg_num;
aktk 9:1903c6f8d5a9 56 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
aktk 9:1903c6f8d5a9 57 _Last_Point = (Vxyt) {
aktk 13:9a51747773af 58 0.0, 0.0, 0.0
aktk 9:1903c6f8d5a9 59 };
aktk 9:1903c6f8d5a9 60 for(int i = 0; i < 4; i++) {
aktk 9:1903c6f8d5a9 61 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 9:1903c6f8d5a9 62 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 9:1903c6f8d5a9 63 }
aktk 9:1903c6f8d5a9 64 //calibrateSensor();
aktk 9:1903c6f8d5a9 65 }
aktk 9:1903c6f8d5a9 66
aktk 9:1903c6f8d5a9 67 CubicSpline2d::~CubicSpline2d()
aktk 9:1903c6f8d5a9 68 {
aktk 9:1903c6f8d5a9 69 free(_Sample_Set);
aktk 9:1903c6f8d5a9 70 //free(_u_param);
aktk 9:1903c6f8d5a9 71 for(int i = 0; i < 4; i++) {
aktk 9:1903c6f8d5a9 72 free(_C_x[i]);
aktk 9:1903c6f8d5a9 73 free(_C_y[i]);
aktk 9:1903c6f8d5a9 74 }
aktk 9:1903c6f8d5a9 75 }
aktk 9:1903c6f8d5a9 76
aktk 9:1903c6f8d5a9 77 void CubicSpline2d::_sampleData()
aktk 9:1903c6f8d5a9 78 {
aktk 9:1903c6f8d5a9 79 int tmp;
aktk 9:1903c6f8d5a9 80 char sig;
aktk 9:1903c6f8d5a9 81 Vxyt tmp_set;
aktk 9:1903c6f8d5a9 82
aktk 9:1903c6f8d5a9 83 int floatflag = 0;
aktk 9:1903c6f8d5a9 84
aktk 9:1903c6f8d5a9 85 // For evry set,
aktk 9:1903c6f8d5a9 86 // 1, get dst data via serai com,
aktk 9:1903c6f8d5a9 87 // 2, get vol data,
aktk 9:1903c6f8d5a9 88 // and then do same for next index set.
aktk 9:1903c6f8d5a9 89 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 90 if(_useType == AsDEBUG) {
aktk 9:1903c6f8d5a9 91 //
aktk 9:1903c6f8d5a9 92 // Recieve a Distance datus and store it into member
aktk 9:1903c6f8d5a9 93 //
aktk 9:1903c6f8d5a9 94 g_Serial_Signal.printf("X:");
aktk 13:9a51747773af 95 _Sample_Set[i].x = 0.0;
aktk 9:1903c6f8d5a9 96 do {
aktk 9:1903c6f8d5a9 97 sig = g_Serial_Signal.getc();
aktk 9:1903c6f8d5a9 98 if('0' <= sig && sig <= '9') {
aktk 9:1903c6f8d5a9 99 if(floatflag == 0) {
aktk 13:9a51747773af 100 _Sample_Set[i].x = 10.0 * _Sample_Set[i].x + (double)(sig - 48);
aktk 9:1903c6f8d5a9 101 } else {
aktk 13:9a51747773af 102 _Sample_Set[i].x = _Sample_Set[i].x + (double)(sig - 48) * pow(10.0, (double)- floatflag);
aktk 9:1903c6f8d5a9 103 floatflag++;
aktk 9:1903c6f8d5a9 104 }
aktk 9:1903c6f8d5a9 105 g_Serial_Signal.putc(char(sig));
aktk 9:1903c6f8d5a9 106 } else if(sig == '.') {
aktk 9:1903c6f8d5a9 107 if(floatflag == 0) {
aktk 9:1903c6f8d5a9 108 floatflag = 1;
aktk 9:1903c6f8d5a9 109 g_Serial_Signal.putc(char(sig));
aktk 9:1903c6f8d5a9 110 }
aktk 9:1903c6f8d5a9 111 } else if(sig == 0x08) {
aktk 13:9a51747773af 112 _Sample_Set[i].x = 0.0;
aktk 9:1903c6f8d5a9 113 g_Serial_Signal.printf("[canseled!]");
aktk 9:1903c6f8d5a9 114 g_Serial_Signal.putc('\n');
aktk 9:1903c6f8d5a9 115 g_Serial_Signal.putc('>');
aktk 9:1903c6f8d5a9 116 }
aktk 9:1903c6f8d5a9 117 } while (!(sig == 0x0a || sig == 0x0d));
aktk 9:1903c6f8d5a9 118 floatflag = 0;
aktk 9:1903c6f8d5a9 119 g_Serial_Signal.putc('\n');
aktk 9:1903c6f8d5a9 120 g_Serial_Signal.printf("x:%f|",_Sample_Set[i].x);
aktk 9:1903c6f8d5a9 121 //
aktk 9:1903c6f8d5a9 122 // Recieve a Voltage datus and store it into member
aktk 9:1903c6f8d5a9 123 //
aktk 9:1903c6f8d5a9 124 // LOW PASS FILTERED
aktk 9:1903c6f8d5a9 125 // Get 10 data and store mean as a sample.
aktk 9:1903c6f8d5a9 126 // After get one original sample, system waits for 0.1 sec,
aktk 9:1903c6f8d5a9 127 // thus it takes 1 sec evry sampling.
aktk 13:9a51747773af 128 _Sample_Set[i].y = 0.0;
aktk 9:1903c6f8d5a9 129 for(int j = 0; j < 10; j++) {
aktk 9:1903c6f8d5a9 130 tmp_set.y = g_Sensor_Voltage.read();
aktk 9:1903c6f8d5a9 131 #ifdef DEBUG
aktk 9:1903c6f8d5a9 132 g_Serial_Signal.printf("%f,",tmp_set.y);
aktk 9:1903c6f8d5a9 133 #endif
aktk 13:9a51747773af 134 _Sample_Set[i].y += (tmp_set.y / 10.0);
aktk 9:1903c6f8d5a9 135 wait(0.1);
aktk 9:1903c6f8d5a9 136 }
aktk 9:1903c6f8d5a9 137 #ifdef DEBUG
aktk 9:1903c6f8d5a9 138 g_Serial_Signal.printf("(%f)\n",_Sample_Set[i].y);
aktk 9:1903c6f8d5a9 139 #endif
aktk 9:1903c6f8d5a9 140 }
aktk 9:1903c6f8d5a9 141
aktk 9:1903c6f8d5a9 142 // if the input data is over the bound, it is calibrated
aktk 13:9a51747773af 143 if (_Sample_Set[i].x < 0.0)
aktk 13:9a51747773af 144 _Sample_Set[i].x = 0.0;
aktk 9:1903c6f8d5a9 145 }
aktk 9:1903c6f8d5a9 146 //
aktk 9:1903c6f8d5a9 147 // Sort set data array in x-Ascending order
aktk 9:1903c6f8d5a9 148 //
aktk 9:1903c6f8d5a9 149 tmp = 0;
aktk 9:1903c6f8d5a9 150 for( int i = 0 ; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 151 for(int j = _Sample_Num - 1; i < j ; j--) {
aktk 9:1903c6f8d5a9 152 // use dst as index for dst range [2,20]
aktk 9:1903c6f8d5a9 153 if (_Sample_Set[i].x > _Sample_Set[j].x) {
aktk 9:1903c6f8d5a9 154 tmp_set.x = _Sample_Set[i].x;
aktk 9:1903c6f8d5a9 155 tmp_set.y = _Sample_Set[i].y;
aktk 9:1903c6f8d5a9 156 _Sample_Set[i].x = _Sample_Set[j].x;
aktk 9:1903c6f8d5a9 157 _Sample_Set[i].y = _Sample_Set[j].y;
aktk 9:1903c6f8d5a9 158 _Sample_Set[j].x = tmp_set.x;
aktk 9:1903c6f8d5a9 159 _Sample_Set[j].y = tmp_set.y;
aktk 9:1903c6f8d5a9 160 }
aktk 9:1903c6f8d5a9 161 // if a same dst has been input, calcurate mean.
aktk 9:1903c6f8d5a9 162 else if (_Sample_Set[i].x == _Sample_Set[j].x) {
aktk 13:9a51747773af 163 tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2.0;
aktk 9:1903c6f8d5a9 164 _Sample_Set[i].y = tmp_set.y;
aktk 9:1903c6f8d5a9 165 for(int k = j; k < _Sample_Num - 1; k++)
aktk 9:1903c6f8d5a9 166 _Sample_Set[k] = _Sample_Set[k+1];
aktk 9:1903c6f8d5a9 167 tmp++;
aktk 9:1903c6f8d5a9 168 }
aktk 9:1903c6f8d5a9 169 }
aktk 9:1903c6f8d5a9 170 }
aktk 9:1903c6f8d5a9 171 #ifdef DEBUG
aktk 9:1903c6f8d5a9 172 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 9:1903c6f8d5a9 173 g_Serial_Signal.printf("-) tmp: %d\n", tmp );
aktk 9:1903c6f8d5a9 174 #endif
aktk 9:1903c6f8d5a9 175 // substruct tmp from number of sample.
aktk 9:1903c6f8d5a9 176 _Sample_Num -= tmp;
aktk 9:1903c6f8d5a9 177 #ifdef DEBUG
aktk 9:1903c6f8d5a9 178 g_Serial_Signal.printf("-----------------\n");
aktk 9:1903c6f8d5a9 179 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 9:1903c6f8d5a9 180 #endif
aktk 9:1903c6f8d5a9 181
aktk 9:1903c6f8d5a9 182 // generate t which is parameter related to x,y
aktk 13:9a51747773af 183 _Sample_Set[0].t = 0.0;
aktk 9:1903c6f8d5a9 184 for(int i = 1; i < _Sample_Num; i++)
aktk 9:1903c6f8d5a9 185 _Sample_Set[i].t =
aktk 9:1903c6f8d5a9 186 _Sample_Set[i-1].t
aktk 13:9a51747773af 187 + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2.0)
aktk 13:9a51747773af 188 +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2.0));
aktk 9:1903c6f8d5a9 189 }
aktk 9:1903c6f8d5a9 190
aktk 9:1903c6f8d5a9 191 //
aktk 9:1903c6f8d5a9 192 // Function to define _u_spline, specific constants of spline.
aktk 9:1903c6f8d5a9 193 //
aktk 9:1903c6f8d5a9 194 void CubicSpline2d::_makeModel(const double* arg_sampled_t, const double* arg_sampled_ft, double* arg_C[4], const unsigned int arg_num)
aktk 9:1903c6f8d5a9 195 {
aktk 9:1903c6f8d5a9 196 // arg_t : t; The variable of f(t)
aktk 9:1903c6f8d5a9 197 // arg_ft: f(t); The cubic poliminal in Interval-j.
aktk 9:1903c6f8d5a9 198 // arg_C[i]: Ci; The coefficient of t^i of f(t) that defines Spline Model Poliminal f(t).
aktk 9:1903c6f8d5a9 199 // arg_num: j in [0,_Sample_Num-1]; The number of interval.
aktk 9:1903c6f8d5a9 200 // f(t)j = C3j*t^3 + C2j*t^2 + C1j*t + C0j
aktk 9:1903c6f8d5a9 201 //
aktk 9:1903c6f8d5a9 202 // N: max of index <=> (_Sample_Num - 1)
aktk 9:1903c6f8d5a9 203 //
aktk 9:1903c6f8d5a9 204 // u[i] === d^2/dx^2(Spline f)[i]
aktk 9:1903c6f8d5a9 205 // i:[0,N]
aktk 9:1903c6f8d5a9 206 // u[0] = u[N] = 0
aktk 9:1903c6f8d5a9 207 #if defined (VERSION_C)
aktk 9:1903c6f8d5a9 208 double *u = (double*)malloc((arg_num ) * sizeof(double));
aktk 9:1903c6f8d5a9 209 #elif defined (VERSION_Cpp)
aktk 9:1903c6f8d5a9 210 double *u = new double[arg_num];
aktk 9:1903c6f8d5a9 211 #elif defined (VERSION_Cpp11)
aktk 9:1903c6f8d5a9 212 std::array<double,arg_num> u;
aktk 9:1903c6f8d5a9 213 #endif
aktk 9:1903c6f8d5a9 214 //
aktk 9:1903c6f8d5a9 215 // h[i] = x[i+1] - x[i]
aktk 9:1903c6f8d5a9 216 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
aktk 9:1903c6f8d5a9 217 double *h = (double*)malloc((arg_num - 1) * sizeof(double));
aktk 9:1903c6f8d5a9 218 //
aktk 9:1903c6f8d5a9 219 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
aktk 9:1903c6f8d5a9 220 // i:[0,N-2]
aktk 9:1903c6f8d5a9 221 double *v = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 9:1903c6f8d5a9 222 //
aktk 9:1903c6f8d5a9 223 // temporary array whose num of elm equals v array
aktk 9:1903c6f8d5a9 224 double *w = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 9:1903c6f8d5a9 225 //
aktk 9:1903c6f8d5a9 226 // [ 2(h[0]+h[1]) , h[1] , O ] [u[1] ] [v[0] ]
aktk 9:1903c6f8d5a9 227 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [u[2] ] [v[1] ]
aktk 9:1903c6f8d5a9 228 // [ ... ] * [... ] = [... ]
aktk 9:1903c6f8d5a9 229 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [u[j+1]] [v[j] ]
aktk 9:1903c6f8d5a9 230 // [ ... ] [ ... ] [ ... ]
aktk 9:1903c6f8d5a9 231 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [u[j+1]] [v[j] ]
aktk 9:1903c6f8d5a9 232 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [u[N-1]] [v[N-2]]
aktk 9:1903c6f8d5a9 233 //
aktk 9:1903c6f8d5a9 234 // For LU decomposition
aktk 9:1903c6f8d5a9 235 double *Upper = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 9:1903c6f8d5a9 236 double *Lower = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 9:1903c6f8d5a9 237 #ifdef DEBUG_MAKE_MODEL
aktk 13:9a51747773af 238 _printOutDataCouple(arg_sampled_t, arg_sampled_ft, arg_num, "\nargument set\n");
aktk 9:1903c6f8d5a9 239 #endif
aktk 9:1903c6f8d5a9 240
aktk 9:1903c6f8d5a9 241 for(int i = 0; i < arg_num - 1; i++)
aktk 9:1903c6f8d5a9 242 h[i] = (double)(arg_sampled_t[i + 1] - arg_sampled_t[i]);
aktk 9:1903c6f8d5a9 243
aktk 9:1903c6f8d5a9 244 for(int i = 0; i < arg_num - 2; i++)
aktk 13:9a51747773af 245 v[i] = 6.0 * (
aktk 9:1903c6f8d5a9 246 ((double)(arg_sampled_ft[i + 2] - arg_sampled_ft[i + 1])) / h[i + 1]
aktk 9:1903c6f8d5a9 247 -
aktk 9:1903c6f8d5a9 248 ((double)(arg_sampled_ft[i + 1] - arg_sampled_ft[i])) / h[i]
aktk 9:1903c6f8d5a9 249 );
aktk 9:1903c6f8d5a9 250
aktk 9:1903c6f8d5a9 251 //
aktk 9:1903c6f8d5a9 252 // LU decomposition
aktk 9:1903c6f8d5a9 253 //
aktk 13:9a51747773af 254 Upper[0] = 2.0 * (h[0] + h[1]);
aktk 13:9a51747773af 255 Lower[0] = 0.0;
aktk 9:1903c6f8d5a9 256 for (int i = 1; i < arg_num - 2; i++) {
aktk 9:1903c6f8d5a9 257 Lower[i] = h[i] / Upper[i - 1];
aktk 13:9a51747773af 258 Upper[i] = 2.0 * (h[i] + h[i + 1]) - Lower[i] * h[i];
aktk 9:1903c6f8d5a9 259 }
aktk 9:1903c6f8d5a9 260 //
aktk 9:1903c6f8d5a9 261 // forward substitution
aktk 9:1903c6f8d5a9 262 //
aktk 9:1903c6f8d5a9 263 w[0] = v[0];
aktk 9:1903c6f8d5a9 264 for (int i = 1; i < arg_num - 2; i ++) {
aktk 9:1903c6f8d5a9 265 w[i] = v[i] - Lower[i] * w[i-1];
aktk 9:1903c6f8d5a9 266 }
aktk 9:1903c6f8d5a9 267 //
aktk 9:1903c6f8d5a9 268 // backward substitution
aktk 9:1903c6f8d5a9 269 //
aktk 9:1903c6f8d5a9 270 u[arg_num - 2] = w[arg_num - 3] / Upper[arg_num - 3];
aktk 9:1903c6f8d5a9 271 for(int i = arg_num - 3; i > 0; i--) {
aktk 9:1903c6f8d5a9 272 u[i] = (w[(i - 1)] - h[(i)] * u[(i) + 1]) / Upper[(i - 1)];
aktk 9:1903c6f8d5a9 273 }
aktk 9:1903c6f8d5a9 274 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 9:1903c6f8d5a9 275 u[0] = u[arg_num - 1] = 0.0;
aktk 9:1903c6f8d5a9 276 #ifdef DEBUG_MAKE_MODEL
aktk 9:1903c6f8d5a9 277 _printOutData(h, arg_num - 1, "h");
aktk 9:1903c6f8d5a9 278 _printOutData(v, arg_num - 2, "v");
aktk 9:1903c6f8d5a9 279 _printOutData(w, arg_num - 2, "w");
aktk 9:1903c6f8d5a9 280 _printOutData(Upper, arg_num - 2, "Upper");
aktk 9:1903c6f8d5a9 281 _printOutData(Lower, arg_num - 2, "Lower");
aktk 9:1903c6f8d5a9 282 _printOutData(u, arg_num , "u");
aktk 9:1903c6f8d5a9 283 #endif
aktk 9:1903c6f8d5a9 284
aktk 9:1903c6f8d5a9 285 for(int ival = 0; ival < arg_num - 1; ival++) {
aktk 9:1903c6f8d5a9 286 arg_C[3][ival] = (u[ival + 1] - u[ival]) / 6.0 / (arg_sampled_t[ival + 1] - arg_sampled_t[ival]);
aktk 9:1903c6f8d5a9 287 arg_C[2][ival] = (u[ival]) / 2.0;
aktk 9:1903c6f8d5a9 288 arg_C[1][ival] = (arg_sampled_ft[ival + 1] - arg_sampled_ft[ival]) / (arg_sampled_t[ival + 1] - arg_sampled_t[ival])
aktk 9:1903c6f8d5a9 289 -
aktk 9:1903c6f8d5a9 290 (arg_sampled_t[ival + 1] - arg_sampled_t[ival]) * (u[ival + 1] + 2.0 * u[ival]) / 6.0;
aktk 9:1903c6f8d5a9 291 arg_C[0][ival] = (arg_sampled_ft[ival]);
aktk 9:1903c6f8d5a9 292 }
aktk 9:1903c6f8d5a9 293 #ifdef DEBUG_MAKE_MODEL
aktk 9:1903c6f8d5a9 294 for(int ival = 0; ival < arg_num - 1; ival++) {
aktk 9:1903c6f8d5a9 295 for(int i = 0; i < 4; i++)
aktk 9:1903c6f8d5a9 296 g_Serial_Signal.printf("C[%d][%d]: %f\n", i, ival, arg_C[i][ival]);
aktk 9:1903c6f8d5a9 297 }
aktk 9:1903c6f8d5a9 298 #endif
aktk 9:1903c6f8d5a9 299
aktk 9:1903c6f8d5a9 300 free(h);
aktk 9:1903c6f8d5a9 301 free(u);
aktk 9:1903c6f8d5a9 302 free(v);
aktk 9:1903c6f8d5a9 303 free(w);
aktk 9:1903c6f8d5a9 304 free(Upper);
aktk 9:1903c6f8d5a9 305 free(Lower);
aktk 9:1903c6f8d5a9 306 }
aktk 9:1903c6f8d5a9 307 void CubicSpline2d::_makeModel(const double* arg_t, const double* arg_ft, double* arg_C[4])
aktk 9:1903c6f8d5a9 308 {
aktk 9:1903c6f8d5a9 309 _makeModel(arg_t, arg_ft, arg_C, _Sample_Num);
aktk 9:1903c6f8d5a9 310 }
aktk 13:9a51747773af 311
aktk 13:9a51747773af 312
aktk 13:9a51747773af 313 void CubicSpline2d::calibrateSensor()
aktk 13:9a51747773af 314 {
aktk 13:9a51747773af 315 double t[_Sample_Num];
aktk 13:9a51747773af 316 double ft[_Sample_Num];
aktk 13:9a51747773af 317
aktk 13:9a51747773af 318 _sampleData();
aktk 13:9a51747773af 319 _Last_Point = _Sample_Set[0];
aktk 13:9a51747773af 320
aktk 13:9a51747773af 321 for(int i = 0; i < _Sample_Num; i++) {
aktk 13:9a51747773af 322 t[i] = _Sample_Set[i].t;
aktk 13:9a51747773af 323 ft[i]= _Sample_Set[i].x;
aktk 13:9a51747773af 324 }
aktk 13:9a51747773af 325 _makeModel(t,ft,_C_x);
aktk 13:9a51747773af 326
aktk 13:9a51747773af 327 for(int i = 0; i < _Sample_Num; i++) {
aktk 13:9a51747773af 328 ft[i]= _Sample_Set[i].y;
aktk 13:9a51747773af 329 }
aktk 13:9a51747773af 330 _makeModel(t,ft,_C_y);
aktk 13:9a51747773af 331
aktk 13:9a51747773af 332 }
aktk 13:9a51747773af 333
aktk 13:9a51747773af 334
aktk 9:1903c6f8d5a9 335 //
aktk 9:1903c6f8d5a9 336 // Fuction to return the value of Cubic polynomial f(t)
aktk 9:1903c6f8d5a9 337 //
aktk 9:1903c6f8d5a9 338 double CubicSpline2d::_cubic_f(const double arg_t, const double arg_C[4])
aktk 9:1903c6f8d5a9 339 {
aktk 9:1903c6f8d5a9 340 double ft; //the value of Spline f(t).
aktk 9:1903c6f8d5a9 341
aktk 13:9a51747773af 342 ft = arg_C[3] * pow(arg_t, 3.0) + arg_C[2] * pow(arg_t, 2.0) + arg_C[1] * arg_t + arg_C[0];
aktk 9:1903c6f8d5a9 343
aktk 9:1903c6f8d5a9 344 return ft;
aktk 9:1903c6f8d5a9 345 }
aktk 13:9a51747773af 346
aktk 13:9a51747773af 347
aktk 9:1903c6f8d5a9 348 //
aktk 9:1903c6f8d5a9 349 // Function to solve a cubic polinomial
aktk 9:1903c6f8d5a9 350 // by using Gardano-Tartaglia formula
aktk 9:1903c6f8d5a9 351 //
aktk 9:1903c6f8d5a9 352 void CubicSpline2d::_solve_cubic_f(
aktk 13:9a51747773af 353 std::complex<double> arg_t[3],
aktk 9:1903c6f8d5a9 354 const double arg_C[4],
aktk 9:1903c6f8d5a9 355 const double arg_ft)
aktk 9:1903c6f8d5a9 356 {
aktk 9:1903c6f8d5a9 357 #ifdef DEBUG_SOLVE
aktk 9:1903c6f8d5a9 358 for(int i = 0; i < 4; i++)
aktk 9:1903c6f8d5a9 359 g_Serial_Signal.printf("C%d: %f\n", i, arg_C[i]);
aktk 9:1903c6f8d5a9 360 #endif
aktk 9:1903c6f8d5a9 361
aktk 13:9a51747773af 362 //arg_t: solution that's expected to be solved in this function.
aktk 13:9a51747773af 363 //t_sol: the solution that's actually wanted as Sprine's solution.
aktk 13:9a51747773af 364 //t0_ival: _Sample_Set[ival].t
aktk 13:9a51747773af 365 //arg_t = t_sol - t0_ival
aktk 13:9a51747773af 366 //=>
aktk 13:9a51747773af 367 //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0]
aktk 13:9a51747773af 368 // = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0]
aktk 9:1903c6f8d5a9 369 double c[3];
aktk 13:9a51747773af 370 //f(t) := arg_ft/arg_C[3]
aktk 13:9a51747773af 371 // = arg_t^3 + c[2]*arg_t^2 + c[1]*arg_t + c[0].
aktk 9:1903c6f8d5a9 372 for(int i = 0; i < 3; i++) {
aktk 9:1903c6f8d5a9 373 c[i] = arg_C[i] / arg_C[3];
aktk 9:1903c6f8d5a9 374 }
aktk 9:1903c6f8d5a9 375 //modify the formula
aktk 13:9a51747773af 376 //t^3 + c[2]*t^2 + c[1]*t + (c[0] - f(t)) = 0.
aktk 9:1903c6f8d5a9 377 c[0] -= arg_ft / arg_C[3];
aktk 9:1903c6f8d5a9 378 #ifdef DEBUG_SOLVE
aktk 9:1903c6f8d5a9 379 for(int i = 0; i < 3; i++)
aktk 9:1903c6f8d5a9 380 g_Serial_Signal.printf("c%d: %f\n", i, c[i]);
aktk 9:1903c6f8d5a9 381 #endif
aktk 9:1903c6f8d5a9 382
aktk 13:9a51747773af 383 std::complex<double> d;
aktk 13:9a51747773af 384 //d := c[2] / 3
aktk 13:9a51747773af 385 //T := t + d (== arg_t - t_iavl + c[2]/3)
aktk 13:9a51747773af 386 d = std::complex<double>(c[2] / 3.0, 0.0);
aktk 13:9a51747773af 387 //=> T^3 + 3pT + 2q = 0.
aktk 13:9a51747773af 388 double p,q;
aktk 9:1903c6f8d5a9 389 //The values defined from coefficients of the formula
aktk 9:1903c6f8d5a9 390 //that identify solutions
aktk 13:9a51747773af 391 p = ( -pow(c[2], 2.0) + 3.0 * c[1]) / 9.0;
aktk 13:9a51747773af 392 q = (2.0 * pow(c[2], 3.0) - 9.0 * c[2] * c[1] + 27.0 * c[0]) / 54.0;
aktk 9:1903c6f8d5a9 393
aktk 9:1903c6f8d5a9 394 //Discriminant section
aktk 9:1903c6f8d5a9 395 double D;
aktk 13:9a51747773af 396 D = pow(p, 3.0) + pow(q, 2.0);
aktk 9:1903c6f8d5a9 397 #ifdef DEBUG_SOLVE
aktk 9:1903c6f8d5a9 398 g_Serial_Signal.printf("p: %f\n", p);
aktk 9:1903c6f8d5a9 399 g_Serial_Signal.printf("q: %f\n", q);
aktk 9:1903c6f8d5a9 400 g_Serial_Signal.printf("D: %f\n", D);
aktk 9:1903c6f8d5a9 401 #endif
aktk 9:1903c6f8d5a9 402
aktk 13:9a51747773af 403 //For all T, u, there exsists v: T = u + v; increment degree of freedom.
aktk 13:9a51747773af 404 //Futhermore,
aktk 13:9a51747773af 405 // u^3 + v^3 - 2q = 0
aktk 13:9a51747773af 406 // uv + p = 0 <=> u^3v^3 = -p^3
aktk 13:9a51747773af 407 //those because: T = u + v, T^3 + 3pT + 2q = 0,
aktk 13:9a51747773af 408 //=> (u + v)^3 + 3p(u + v) + 2q = 0
aktk 13:9a51747773af 409 //=> u^3 + v^3 + 3(uv + p)(u+v) + 2q = 0
aktk 9:1903c6f8d5a9 410 //The values defined from p and q
aktk 9:1903c6f8d5a9 411 //that idetify solutions
aktk 9:1903c6f8d5a9 412 std::complex<double> u,v;
aktk 9:1903c6f8d5a9 413 //Real root only
aktk 13:9a51747773af 414 if(D <= 0.0) {
aktk 9:1903c6f8d5a9 415 u = std::complex<double>(-q, sqrt(-D));
aktk 9:1903c6f8d5a9 416 v = std::complex<double>(-q,-sqrt(-D));
aktk 11:a279e31d8499 417 //u = pow(u, 1/3);
aktk 11:a279e31d8499 418 //v = pow(v, 1/3);
aktk 12:b3e07a2220bc 419 u = std::exp(std::log(u)/std::complex<double>(3.0,0.0));
aktk 13:9a51747773af 420 v = std::exp(std::log(v)/std::complex<double>(3.0,0.0));
aktk 9:1903c6f8d5a9 421 }
aktk 9:1903c6f8d5a9 422 //One real root and two complex root
aktk 9:1903c6f8d5a9 423 else {
aktk 9:1903c6f8d5a9 424 u = std::complex<double>(-q+sqrt(D),0.0);
aktk 9:1903c6f8d5a9 425 v = std::complex<double>(-q-sqrt(D),0.0);
aktk 9:1903c6f8d5a9 426 u = std::complex<double>(cbrt(u.real()), 0.0);
aktk 9:1903c6f8d5a9 427 v = std::complex<double>(cbrt(v.real()), 0.0);
aktk 9:1903c6f8d5a9 428 }
aktk 9:1903c6f8d5a9 429 #ifdef DEBUG_SOLVE
aktk 9:1903c6f8d5a9 430 g_Serial_Signal.printf("u: %f + (%f)i\n", u.real(), u.imag());
aktk 9:1903c6f8d5a9 431 g_Serial_Signal.printf("v: %f + (%f)i\n", v.real(), v.imag());
aktk 13:9a51747773af 432 g_Serial_Signal.printf("d: %f + (%f)i\n", d.real(), d.imag());
aktk 9:1903c6f8d5a9 433 #endif
aktk 9:1903c6f8d5a9 434
aktk 9:1903c6f8d5a9 435 //Cubic root of 1
aktk 9:1903c6f8d5a9 436 std::complex<double> omega[3]= {
aktk 9:1903c6f8d5a9 437 std::complex<double>( 1.0, 0.0),
aktk 13:9a51747773af 438 std::complex<double>(-1.0/2.0, sqrt(3.0)/2.0),
aktk 13:9a51747773af 439 std::complex<double>(-1.0/2.0,-sqrt(3.0)/2.0)
aktk 9:1903c6f8d5a9 440 };
aktk 9:1903c6f8d5a9 441
aktk 9:1903c6f8d5a9 442 //Solution of the formula
aktk 13:9a51747773af 443 arg_t[0] = omega[0] * u + omega[0] * v - d;
aktk 13:9a51747773af 444 arg_t[1] = omega[1] * u + omega[2] * v - d;
aktk 13:9a51747773af 445 arg_t[2] = omega[2] * u + omega[1] * v - d;
aktk 9:1903c6f8d5a9 446
aktk 9:1903c6f8d5a9 447 #ifdef DEBUG_SOLVE
aktk 9:1903c6f8d5a9 448 for(int i = 0; i < 3; i++)
aktk 9:1903c6f8d5a9 449 g_Serial_Signal.printf("t%d: %f + (%f)i\n", i, arg_t[i].real(), arg_t[i].imag() );
aktk 9:1903c6f8d5a9 450 #endif
aktk 9:1903c6f8d5a9 451 }
aktk 9:1903c6f8d5a9 452
aktk 9:1903c6f8d5a9 453 double CubicSpline2d::getX(double arg_y)
aktk 9:1903c6f8d5a9 454 {
aktk 9:1903c6f8d5a9 455 double x;
aktk 9:1903c6f8d5a9 456 double C[4];
aktk 9:1903c6f8d5a9 457 double the_t;
aktk 9:1903c6f8d5a9 458 int the_i;
aktk 9:1903c6f8d5a9 459 std::complex<double>t_sol[3];
aktk 9:1903c6f8d5a9 460 std::vector<double> t_real;
aktk 9:1903c6f8d5a9 461 std::vector<int> t_ival;
aktk 9:1903c6f8d5a9 462
aktk 9:1903c6f8d5a9 463 #ifdef DEBUG_GETX
aktk 9:1903c6f8d5a9 464 g_Serial_Signal.printf(DEBUG_GETX);
aktk 9:1903c6f8d5a9 465 #endif
aktk 9:1903c6f8d5a9 466 // For the every Intervals of Spline,
aktk 9:1903c6f8d5a9 467 //it solves the polynomial defined by C[i] of the interval,
aktk 9:1903c6f8d5a9 468 //checks the solutions are real number,
aktk 9:1903c6f8d5a9 469 //and ckecks the solutions are in the interval.
aktk 9:1903c6f8d5a9 470 // And if not-excluded solutions are more than one,
aktk 9:1903c6f8d5a9 471 //it trys to find which one is more nearest to last point.
aktk 9:1903c6f8d5a9 472 for(int ival = 0; ival < _Sample_Num - 1; ival++) {
aktk 9:1903c6f8d5a9 473 for(int i = 0; i < 4; i++) C[i] = _C_y[i][ival];
aktk 9:1903c6f8d5a9 474 _solve_cubic_f(t_sol, C, arg_y);
aktk 13:9a51747773af 475 for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t;
aktk 9:1903c6f8d5a9 476 #ifdef DEBUG_GETX
aktk 13:9a51747773af 477 g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t);
aktk 9:1903c6f8d5a9 478 #endif
aktk 9:1903c6f8d5a9 479 for(int i = 0; i < 3; i++) {
aktk 9:1903c6f8d5a9 480 // regarding only real solution
aktk 9:1903c6f8d5a9 481 // acuracy (error range) is supposed +-10E-3 here(groundless)
aktk 9:1903c6f8d5a9 482 if(std::abs(t_sol[i].imag()) < 0.000001) {
aktk 13:9a51747773af 483 /* */ if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
aktk 13:9a51747773af 484 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 485 t_ival.push_back(ival);
aktk 13:9a51747773af 486 #ifdef DEBUG_GETX
aktk 13:9a51747773af 487 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
aktk 13:9a51747773af 488 #endif
aktk 9:1903c6f8d5a9 489 } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
aktk 13:9a51747773af 490 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 491 t_ival.push_back(ival);
aktk 13:9a51747773af 492 #ifdef DEBUG_GETX
aktk 13:9a51747773af 493 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
aktk 13:9a51747773af 494 #endif
aktk 13:9a51747773af 495 } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival + 1].t) {
aktk 9:1903c6f8d5a9 496 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 497 t_ival.push_back(ival);
aktk 12:b3e07a2220bc 498 #ifdef DEBUG_GETX
aktk 12:b3e07a2220bc 499 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
aktk 12:b3e07a2220bc 500 #endif
aktk 9:1903c6f8d5a9 501 }
aktk 9:1903c6f8d5a9 502 }
aktk 9:1903c6f8d5a9 503 }
aktk 9:1903c6f8d5a9 504 }
aktk 9:1903c6f8d5a9 505
aktk 13:9a51747773af 506 if(!t_real.empty()) {
aktk 10:607a68db6303 507 the_t = t_real[0];
aktk 10:607a68db6303 508 the_i = t_ival[0];
aktk 10:607a68db6303 509 //if t's size is bigger than 1
aktk 10:607a68db6303 510 for(int i = 1; i < t_real.size(); i++) {
aktk 10:607a68db6303 511 if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) {
aktk 10:607a68db6303 512 the_t = t_real[i];
aktk 10:607a68db6303 513 the_i = t_ival[i];
aktk 10:607a68db6303 514 }
aktk 9:1903c6f8d5a9 515 }
aktk 10:607a68db6303 516 } else {
aktk 12:b3e07a2220bc 517 #ifdef DEBUG_GETX
aktk 12:b3e07a2220bc 518 g_Serial_Signal.printf("LastPoint\n");
aktk 12:b3e07a2220bc 519 #endif
aktk 10:607a68db6303 520 the_t = _Last_Point.t;
aktk 12:b3e07a2220bc 521 for (int i = 0; i < _Sample_Num - 1; i++)
aktk 12:b3e07a2220bc 522 if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t)
aktk 10:607a68db6303 523 the_i = i;
aktk 9:1903c6f8d5a9 524 }
aktk 13:9a51747773af 525 /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t;
aktk 13:9a51747773af 526 else if (_Sample_Set[_Sample_Num - 1].t <= the_t) the_t = _Sample_Set[_Sample_Num - 1].t;
aktk 13:9a51747773af 527
aktk 9:1903c6f8d5a9 528 for(int i = 0; i < 4; i++) C[i] = _C_x[i][the_i];
aktk 9:1903c6f8d5a9 529 x = _cubic_f(the_t - _Sample_Set[the_i].t, C);
aktk 9:1903c6f8d5a9 530 #ifdef DEBUG_GETX
aktk 9:1903c6f8d5a9 531 g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)\n",the_t, the_i);
aktk 9:1903c6f8d5a9 532 #endif
aktk 12:b3e07a2220bc 533 _Last_Point = (Vxyt) {
aktk 12:b3e07a2220bc 534 x, arg_y, the_t
aktk 12:b3e07a2220bc 535 };
aktk 9:1903c6f8d5a9 536 return x;
aktk 9:1903c6f8d5a9 537 }
aktk 9:1903c6f8d5a9 538
aktk 9:1903c6f8d5a9 539 double CubicSpline2d::getY(double arg_x)
aktk 9:1903c6f8d5a9 540 {
aktk 12:b3e07a2220bc 541
aktk 9:1903c6f8d5a9 542 double y;
aktk 9:1903c6f8d5a9 543 double C[4];
aktk 9:1903c6f8d5a9 544 double the_t;
aktk 9:1903c6f8d5a9 545 int the_i;
aktk 9:1903c6f8d5a9 546 std::complex<double>t_sol[3];
aktk 9:1903c6f8d5a9 547 std::vector<double> t_real;
aktk 9:1903c6f8d5a9 548 std::vector<int> t_ival;
aktk 9:1903c6f8d5a9 549
aktk 9:1903c6f8d5a9 550 #ifdef DEBUG_GETY
aktk 9:1903c6f8d5a9 551 g_Serial_Signal.printf(DEBUG_GETY);
aktk 13:9a51747773af 552 g_Serial_Signal.printf("arg_x: %f\n", arg_x);
aktk 9:1903c6f8d5a9 553 #endif
aktk 9:1903c6f8d5a9 554 // For the every Intervals of Spline,
aktk 9:1903c6f8d5a9 555 //it solves the polynomial defined by C[i] of the interval,
aktk 9:1903c6f8d5a9 556 //checks the solutions are real number,
aktk 9:1903c6f8d5a9 557 //and ckecks the solutions are in the interval.
aktk 9:1903c6f8d5a9 558 // And if not-excluded solutions are more than one,
aktk 9:1903c6f8d5a9 559 //it trys to find which one is more nearest to last point.
aktk 9:1903c6f8d5a9 560 for(int ival = 0; ival < _Sample_Num - 1; ival++) {
aktk 9:1903c6f8d5a9 561 for(int i = 0; i < 4; i++) C[i] = _C_x[i][ival];
aktk 9:1903c6f8d5a9 562 _solve_cubic_f(t_sol, C, arg_x);
aktk 13:9a51747773af 563 for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t;
aktk 13:9a51747773af 564 //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0]
aktk 13:9a51747773af 565 // = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0]
aktk 12:b3e07a2220bc 566 #ifdef DEBUG_GETY
aktk 13:9a51747773af 567 g_Serial_Signal.printf("interval:%d \t %f <= t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t);
aktk 13:9a51747773af 568 for(int i = 0; i < 3; i++)
aktk 13:9a51747773af 569 g_Serial_Signal.printf("t%d \t %f + (%f)i\n", i, t_sol[i].real(), t_sol[i].imag());
aktk 12:b3e07a2220bc 570 #endif
aktk 9:1903c6f8d5a9 571 for(int i = 0; i < 3; i++) {
aktk 9:1903c6f8d5a9 572 // regarding only real solution
aktk 13:9a51747773af 573 // acuracy (error range) is supposed +-10E-6 here(groundless)
aktk 9:1903c6f8d5a9 574 if(std::abs(t_sol[i].imag()) < 0.000001) {
aktk 13:9a51747773af 575 /**/ if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
aktk 13:9a51747773af 576 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 577 t_ival.push_back(ival);
aktk 13:9a51747773af 578 #ifdef DEBUG_GETY
aktk 13:9a51747773af 579 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
aktk 13:9a51747773af 580 #endif
aktk 13:9a51747773af 581 }//
aktk 13:9a51747773af 582 else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
aktk 13:9a51747773af 583 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 584 t_ival.push_back(ival);
aktk 13:9a51747773af 585 #ifdef DEBUG_GETY
aktk 13:9a51747773af 586 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
aktk 13:9a51747773af 587 #endif
aktk 13:9a51747773af 588 }
aktk 13:9a51747773af 589 // There sometimes be a so fucking small error in solutions,
aktk 13:9a51747773af 590 // so acuracy is set at 10E-6(groundless)
aktk 13:9a51747773af 591 else if (static_cast<int>(_Sample_Set[ival].t * std::pow(10., 6.)) <= static_cast<int>(t_sol[i].real() * std::pow(10., 6.))
aktk 13:9a51747773af 592 && static_cast<int>(t_sol[i].real() * std::pow(10., 6.)) < static_cast<int>(_Sample_Set[ival + 1].t * std::pow(10., 6.))) {
aktk 9:1903c6f8d5a9 593 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 594 t_ival.push_back(ival);
aktk 12:b3e07a2220bc 595 #ifdef DEBUG_GETY
aktk 12:b3e07a2220bc 596 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
aktk 12:b3e07a2220bc 597 #endif
aktk 9:1903c6f8d5a9 598 }
aktk 9:1903c6f8d5a9 599 }
aktk 9:1903c6f8d5a9 600 }
aktk 12:b3e07a2220bc 601 }
aktk 9:1903c6f8d5a9 602
aktk 13:9a51747773af 603 if(!t_real.empty()) {
aktk 9:1903c6f8d5a9 604 the_t = t_real[0];
aktk 9:1903c6f8d5a9 605 the_i = t_ival[0];
aktk 9:1903c6f8d5a9 606 //if t's size is bigger than 1
aktk 9:1903c6f8d5a9 607 for(int i = 1; i < t_real.size(); i++) {
aktk 9:1903c6f8d5a9 608 if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) {
aktk 9:1903c6f8d5a9 609 the_t = t_real[i];
aktk 9:1903c6f8d5a9 610 the_i = t_ival[i];
aktk 9:1903c6f8d5a9 611 }
aktk 9:1903c6f8d5a9 612 }
aktk 13:9a51747773af 613 }
aktk 13:9a51747773af 614 //if no solution muched due to any errors
aktk 13:9a51747773af 615 else {
aktk 12:b3e07a2220bc 616 #ifdef DEBUG_GETY
aktk 12:b3e07a2220bc 617 g_Serial_Signal.printf("LastPoint\n");
aktk 12:b3e07a2220bc 618 #endif
aktk 12:b3e07a2220bc 619 the_t = _Last_Point.t;
aktk 12:b3e07a2220bc 620 for (int i = 0; i < _Sample_Num - 1; i++)
aktk 12:b3e07a2220bc 621 if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t)
aktk 12:b3e07a2220bc 622 the_i = i;
aktk 9:1903c6f8d5a9 623 }
aktk 13:9a51747773af 624
aktk 13:9a51747773af 625 //
aktk 13:9a51747773af 626 /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t;
aktk 13:9a51747773af 627 else if (_Sample_Set[_Sample_Num - 1].t < the_t) the_t = _Sample_Set[_Sample_Num - 1].t;
aktk 13:9a51747773af 628
aktk 13:9a51747773af 629 //
aktk 9:1903c6f8d5a9 630 for(int i = 0; i < 4; i++) C[i] = _C_y[i][the_i];
aktk 9:1903c6f8d5a9 631 y = _cubic_f(the_t - _Sample_Set[the_i].t, C);
aktk 9:1903c6f8d5a9 632 #ifdef DEBUG_GETY
aktk 12:b3e07a2220bc 633 g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)\n",the_t, the_i);
aktk 9:1903c6f8d5a9 634 #endif
aktk 12:b3e07a2220bc 635 _Last_Point = (Vxyt) {
aktk 12:b3e07a2220bc 636 y, arg_x, the_t
aktk 12:b3e07a2220bc 637 };
aktk 9:1903c6f8d5a9 638 return y;
aktk 9:1903c6f8d5a9 639 }
aktk 9:1903c6f8d5a9 640
aktk 9:1903c6f8d5a9 641
aktk 9:1903c6f8d5a9 642
aktk 9:1903c6f8d5a9 643 void CubicSpline2d::saveSetting()
aktk 9:1903c6f8d5a9 644 {
aktk 9:1903c6f8d5a9 645 FILE *fp;
aktk 9:1903c6f8d5a9 646
aktk 9:1903c6f8d5a9 647 fp = fopen("/local/savedata.log", "wb");
aktk 9:1903c6f8d5a9 648
aktk 9:1903c6f8d5a9 649 // Save _Sample_Num
aktk 9:1903c6f8d5a9 650 fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 651 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 652 // Save _Sample_Set
aktk 9:1903c6f8d5a9 653 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 654 fwrite(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 655 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 656 fwrite(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 657 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 658 fwrite(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 659 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 660 }
aktk 9:1903c6f8d5a9 661 // Save _C_x
aktk 9:1903c6f8d5a9 662 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 663 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 664 fwrite(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 665 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 666 }
aktk 9:1903c6f8d5a9 667 }
aktk 9:1903c6f8d5a9 668 // Save _C_y
aktk 9:1903c6f8d5a9 669 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 670 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 671 fwrite(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 672 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 673 }
aktk 9:1903c6f8d5a9 674 }
aktk 9:1903c6f8d5a9 675
aktk 9:1903c6f8d5a9 676 fclose(fp);
aktk 9:1903c6f8d5a9 677
aktk 9:1903c6f8d5a9 678 }
aktk 9:1903c6f8d5a9 679
aktk 9:1903c6f8d5a9 680 void CubicSpline2d::saveSetting(
aktk 9:1903c6f8d5a9 681 const char *filename
aktk 9:1903c6f8d5a9 682 )
aktk 9:1903c6f8d5a9 683 {
aktk 9:1903c6f8d5a9 684 FILE *fp;
aktk 9:1903c6f8d5a9 685 char *filepath;
aktk 9:1903c6f8d5a9 686 int fnnum = 0;
aktk 9:1903c6f8d5a9 687
aktk 9:1903c6f8d5a9 688 while (filename[fnnum] != 0) fnnum++;
aktk 9:1903c6f8d5a9 689 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 9:1903c6f8d5a9 690
aktk 9:1903c6f8d5a9 691 sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 692 fp = fopen(filepath, "wb");
aktk 9:1903c6f8d5a9 693
aktk 9:1903c6f8d5a9 694 // Save _Sample_Num
aktk 9:1903c6f8d5a9 695 fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 696 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 697 // Save _Sample_Set
aktk 9:1903c6f8d5a9 698 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 699 fwrite(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 700 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 701 fwrite(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 702 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 703 fwrite(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 704 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 705 }
aktk 9:1903c6f8d5a9 706 // Save _C_x
aktk 9:1903c6f8d5a9 707 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 708 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 709 fwrite(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 710 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 711 }
aktk 9:1903c6f8d5a9 712 }
aktk 9:1903c6f8d5a9 713 // Save _C_y
aktk 9:1903c6f8d5a9 714 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 715 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 716 fwrite(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 717 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 718 }
aktk 9:1903c6f8d5a9 719 }
aktk 9:1903c6f8d5a9 720
aktk 9:1903c6f8d5a9 721 fclose(fp);
aktk 9:1903c6f8d5a9 722 free(filepath);
aktk 9:1903c6f8d5a9 723 }
aktk 9:1903c6f8d5a9 724
aktk 9:1903c6f8d5a9 725 void CubicSpline2d::loadSetting()
aktk 9:1903c6f8d5a9 726 {
aktk 9:1903c6f8d5a9 727 FILE *fp;
aktk 9:1903c6f8d5a9 728 char tmp;
aktk 9:1903c6f8d5a9 729
aktk 9:1903c6f8d5a9 730 //sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 731 //fp = fopen(filepath, "rb");
aktk 9:1903c6f8d5a9 732 fp = fopen("/local/savedata.log", "rb");
aktk 9:1903c6f8d5a9 733
aktk 9:1903c6f8d5a9 734 // Load _Sample_Num
aktk 9:1903c6f8d5a9 735 fread(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 736 fread(&tmp, sizeof(char), 1, fp);
aktk 9:1903c6f8d5a9 737
aktk 9:1903c6f8d5a9 738 // Load _Sample_Set
aktk 9:1903c6f8d5a9 739 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 740 fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 741 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 742 fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 743 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 744 fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 745 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 746 }
aktk 9:1903c6f8d5a9 747
aktk 9:1903c6f8d5a9 748 // Load _C_x
aktk 9:1903c6f8d5a9 749 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 750 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 751 fread(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 752 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 753 }
aktk 9:1903c6f8d5a9 754 }
aktk 9:1903c6f8d5a9 755 // Load _C_y
aktk 9:1903c6f8d5a9 756 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 757 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 758 fread(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 759 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 760 }
aktk 9:1903c6f8d5a9 761 }
aktk 9:1903c6f8d5a9 762 fclose(fp);
aktk 9:1903c6f8d5a9 763 }
aktk 9:1903c6f8d5a9 764
aktk 9:1903c6f8d5a9 765
aktk 9:1903c6f8d5a9 766 void CubicSpline2d::loadSetting(
aktk 9:1903c6f8d5a9 767 const char *filename
aktk 9:1903c6f8d5a9 768 )
aktk 9:1903c6f8d5a9 769 {
aktk 9:1903c6f8d5a9 770 FILE *fp;
aktk 9:1903c6f8d5a9 771 char *filepath;
aktk 9:1903c6f8d5a9 772 char tmp;
aktk 9:1903c6f8d5a9 773 int fnnum = 0;
aktk 9:1903c6f8d5a9 774
aktk 9:1903c6f8d5a9 775 while (filename[fnnum] != 0) fnnum++;
aktk 9:1903c6f8d5a9 776 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 9:1903c6f8d5a9 777
aktk 9:1903c6f8d5a9 778 sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 779 fp = fopen(filepath, "rb");
aktk 9:1903c6f8d5a9 780
aktk 9:1903c6f8d5a9 781 // Load _Sample_Num
aktk 9:1903c6f8d5a9 782 fread(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 783 fread(&tmp, sizeof(char), 1, fp);
aktk 9:1903c6f8d5a9 784
aktk 9:1903c6f8d5a9 785 // Load _Sample_Set
aktk 9:1903c6f8d5a9 786 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 787 fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 788 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 789 fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 790 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 791 fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 792 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 793 }
aktk 9:1903c6f8d5a9 794
aktk 9:1903c6f8d5a9 795 // Load _C_x
aktk 9:1903c6f8d5a9 796 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 797 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 798 fread(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 799 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 800 }
aktk 9:1903c6f8d5a9 801 }
aktk 9:1903c6f8d5a9 802
aktk 9:1903c6f8d5a9 803 // Load _C_y
aktk 9:1903c6f8d5a9 804 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 805 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 806 fread(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 807 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 808 }
aktk 9:1903c6f8d5a9 809 }
aktk 9:1903c6f8d5a9 810 fclose(fp);
aktk 9:1903c6f8d5a9 811 free(filepath);
aktk 9:1903c6f8d5a9 812 }
aktk 9:1903c6f8d5a9 813
aktk 9:1903c6f8d5a9 814 void CubicSpline2d::printOutData()
aktk 9:1903c6f8d5a9 815 {
aktk 9:1903c6f8d5a9 816 FILE *fp;
aktk 13:9a51747773af 817 double x,y,t;
aktk 13:9a51747773af 818 double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100.0;
aktk 9:1903c6f8d5a9 819
aktk 9:1903c6f8d5a9 820 fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 9:1903c6f8d5a9 821
aktk 12:b3e07a2220bc 822 fprintf(fp, "x, y, (t)\n");
aktk 12:b3e07a2220bc 823 for(int ival = 0; ival < _Sample_Num - 1; ival++) {
aktk 12:b3e07a2220bc 824 fprintf(fp, "ival: %d \n", ival);
aktk 13:9a51747773af 825 for(x = _Sample_Set[ival].x; x <= _Sample_Set[ival + 1].x; x += d) {
aktk 12:b3e07a2220bc 826 y = getY(x);
aktk 13:9a51747773af 827 if(ival == 0)
aktk 13:9a51747773af 828 t = sqrt((x - _Sample_Set[ival].x)*(x - _Sample_Set[ival].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y));
aktk 13:9a51747773af 829 else
aktk 13:9a51747773af 830 t = _Sample_Set[ival-1].t + sqrt((x - _Sample_Set[ival-1].x)*(x - _Sample_Set[ival-1].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y));
aktk 13:9a51747773af 831 fprintf(fp, "%f,%f,(%f)\n", x, y, t);
aktk 9:1903c6f8d5a9 832 }
aktk 13:9a51747773af 833 fprintf(fp, "-----------------------------------------\n");
aktk 9:1903c6f8d5a9 834 }
aktk 9:1903c6f8d5a9 835
aktk 9:1903c6f8d5a9 836 fprintf(fp, "\nSample:dst, vol\n");
aktk 9:1903c6f8d5a9 837 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 838 fprintf(fp, "%f,%f,(%f)\n", _Sample_Set[i].x, _Sample_Set[i].y, _Sample_Set[i].t);
aktk 9:1903c6f8d5a9 839 }
aktk 9:1903c6f8d5a9 840 fclose(fp);
aktk 9:1903c6f8d5a9 841
aktk 9:1903c6f8d5a9 842 }
aktk 9:1903c6f8d5a9 843 void CubicSpline2d::_printOutData(const double *arg, const int num, const char* name)
aktk 9:1903c6f8d5a9 844 {
aktk 9:1903c6f8d5a9 845 FILE *fp;
aktk 9:1903c6f8d5a9 846
aktk 9:1903c6f8d5a9 847 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 848 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 849 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 850 fprintf(fp, "%.2f, ", arg[i]);
aktk 9:1903c6f8d5a9 851 }
aktk 9:1903c6f8d5a9 852 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 853 fclose(fp);
aktk 9:1903c6f8d5a9 854 }
aktk 9:1903c6f8d5a9 855 void CubicSpline2d::_printOutDataCouple(const double *arg1, const double *arg2, const int num, const char* name)
aktk 9:1903c6f8d5a9 856 {
aktk 9:1903c6f8d5a9 857 FILE *fp;
aktk 9:1903c6f8d5a9 858
aktk 9:1903c6f8d5a9 859 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 860 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 861 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 862 fprintf(fp, "(%.2f, %.2f)\n", arg1[i], arg2[i]);
aktk 9:1903c6f8d5a9 863 }
aktk 9:1903c6f8d5a9 864 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 865 fclose(fp);
aktk 9:1903c6f8d5a9 866 }
aktk 9:1903c6f8d5a9 867 void CubicSpline2d::_printOutData(const Vxyt *arg, int num, const char* name)
aktk 9:1903c6f8d5a9 868 {
aktk 9:1903c6f8d5a9 869 FILE *fp;
aktk 9:1903c6f8d5a9 870
aktk 9:1903c6f8d5a9 871 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 872 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 873 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 874 fprintf(fp, "%f, ", arg[i].y);
aktk 9:1903c6f8d5a9 875 }
aktk 9:1903c6f8d5a9 876 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 877 fclose(fp);
aktk 9:1903c6f8d5a9 878 }