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

Committer:
aktk
Date:
Sun May 29 09:23:24 2016 +0000
Revision:
10:607a68db6303
Parent:
9:1903c6f8d5a9
Child:
11:a279e31d8499
in _solve_cubic_f, only if t_sol is in a interval, it is push_back()ed into t_real. ; And, if no t_real member, return last point.

Who changed what in which revision?

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