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@12:b3e07a2220bc, 2016-05-29 (annotated)

Committer:
aktk
Date:
Sun May 29 12:15:19 2016 +0000
Revision:
12:b3e07a2220bc
Parent:
11:a279e31d8499
Child:
13:9a51747773af
value of y dont modified except in ival:0. In ival:1~_Sample_Num-2 y = getY(x) all has the value of the biggest value of y in ival:0.

Who changed what in which revision?

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