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@9:1903c6f8d5a9, 2016-05-29 (annotated)

Committer:
aktk
Date:
Sun May 29 09:14:54 2016 +0000
Revision:
9:1903c6f8d5a9
Child:
10:607a68db6303
debugging: (the_t, the_i) now comes to (0,0) unexpectedly

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 9:1903c6f8d5a9 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 9:1903c6f8d5a9 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 9:1903c6f8d5a9 459 the_t = t_real[0];
aktk 9:1903c6f8d5a9 460 the_i = t_ival[0];
aktk 9:1903c6f8d5a9 461 //if t's size is bigger than 1
aktk 9:1903c6f8d5a9 462 for(int i = 1; i < t_real.size(); i++) {
aktk 9:1903c6f8d5a9 463 if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) {
aktk 9:1903c6f8d5a9 464 the_t = t_real[i];
aktk 9:1903c6f8d5a9 465 the_i = t_ival[i];
aktk 9:1903c6f8d5a9 466 }
aktk 9:1903c6f8d5a9 467 }
aktk 9:1903c6f8d5a9 468 for(int i = 0; i < 4; i++) C[i] = _C_x[i][the_i];
aktk 9:1903c6f8d5a9 469 x = _cubic_f(the_t - _Sample_Set[the_i].t, C);
aktk 9:1903c6f8d5a9 470 #ifdef DEBUG_GETX
aktk 9:1903c6f8d5a9 471 g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)\n",the_t, the_i);
aktk 9:1903c6f8d5a9 472 #endif
aktk 9:1903c6f8d5a9 473
aktk 9:1903c6f8d5a9 474 return x;
aktk 9:1903c6f8d5a9 475 }
aktk 9:1903c6f8d5a9 476
aktk 9:1903c6f8d5a9 477 #define DEBUG_GETY "DEBUG_GETY\n"
aktk 9:1903c6f8d5a9 478 double CubicSpline2d::getY(double arg_x)
aktk 9:1903c6f8d5a9 479 {
aktk 9:1903c6f8d5a9 480 double y;
aktk 9:1903c6f8d5a9 481 double C[4];
aktk 9:1903c6f8d5a9 482 double the_t;
aktk 9:1903c6f8d5a9 483 int the_i;
aktk 9:1903c6f8d5a9 484 std::complex<double>t_sol[3];
aktk 9:1903c6f8d5a9 485 std::vector<double> t_real;
aktk 9:1903c6f8d5a9 486 std::vector<int> t_ival;
aktk 9:1903c6f8d5a9 487
aktk 9:1903c6f8d5a9 488 #ifdef DEBUG_GETY
aktk 9:1903c6f8d5a9 489 g_Serial_Signal.printf(DEBUG_GETY);
aktk 9:1903c6f8d5a9 490 #endif
aktk 9:1903c6f8d5a9 491 // For the every Intervals of Spline,
aktk 9:1903c6f8d5a9 492 //it solves the polynomial defined by C[i] of the interval,
aktk 9:1903c6f8d5a9 493 //checks the solutions are real number,
aktk 9:1903c6f8d5a9 494 //and ckecks the solutions are in the interval.
aktk 9:1903c6f8d5a9 495 // And if not-excluded solutions are more than one,
aktk 9:1903c6f8d5a9 496 //it trys to find which one is more nearest to last point.
aktk 9:1903c6f8d5a9 497 for(int ival = 0; ival < _Sample_Num - 1; ival++) {
aktk 9:1903c6f8d5a9 498 for(int i = 0; i < 4; i++) C[i] = _C_x[i][ival];
aktk 9:1903c6f8d5a9 499 _solve_cubic_f(t_sol, C, arg_x);
aktk 9:1903c6f8d5a9 500 for(int i = 0; i < 3; i++) {
aktk 9:1903c6f8d5a9 501 // regarding only real solution
aktk 9:1903c6f8d5a9 502 // acuracy (error range) is supposed +-10E-3 here(groundless)
aktk 9:1903c6f8d5a9 503 if(std::abs(t_sol[i].imag()) < 0.000001) {
aktk 9:1903c6f8d5a9 504 /* */ if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
aktk 9:1903c6f8d5a9 505 t_real.push_back(_Sample_Set[ival].t);
aktk 9:1903c6f8d5a9 506 t_ival.push_back(ival);
aktk 9:1903c6f8d5a9 507 } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
aktk 9:1903c6f8d5a9 508 t_real.push_back(_Sample_Set[ival + 1].t);
aktk 9:1903c6f8d5a9 509 t_ival.push_back(ival);
aktk 9:1903c6f8d5a9 510 } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
aktk 9:1903c6f8d5a9 511 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 512 t_ival.push_back(ival);
aktk 9:1903c6f8d5a9 513 }
aktk 9:1903c6f8d5a9 514 }
aktk 9:1903c6f8d5a9 515 }
aktk 9:1903c6f8d5a9 516
aktk 9:1903c6f8d5a9 517
aktk 9:1903c6f8d5a9 518 the_t = t_real[0];
aktk 9:1903c6f8d5a9 519 the_i = t_ival[0];
aktk 9:1903c6f8d5a9 520 //if t's size is bigger than 1
aktk 9:1903c6f8d5a9 521 for(int i = 1; i < t_real.size(); i++) {
aktk 9:1903c6f8d5a9 522 if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) {
aktk 9:1903c6f8d5a9 523 the_t = t_real[i];
aktk 9:1903c6f8d5a9 524 the_i = t_ival[i];
aktk 9:1903c6f8d5a9 525 }
aktk 9:1903c6f8d5a9 526 }
aktk 9:1903c6f8d5a9 527 }
aktk 9:1903c6f8d5a9 528
aktk 9:1903c6f8d5a9 529 for(int i = 0; i < 4; i++) C[i] = _C_y[i][the_i];
aktk 9:1903c6f8d5a9 530 y = _cubic_f(the_t - _Sample_Set[the_i].t, C);
aktk 9:1903c6f8d5a9 531 #ifdef DEBUG_GETY
aktk 9:1903c6f8d5a9 532 g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)i\n",the_t, the_i);
aktk 9:1903c6f8d5a9 533 #endif
aktk 9:1903c6f8d5a9 534
aktk 9:1903c6f8d5a9 535 return y;
aktk 9:1903c6f8d5a9 536 }
aktk 9:1903c6f8d5a9 537
aktk 9:1903c6f8d5a9 538
aktk 9:1903c6f8d5a9 539 void CubicSpline2d::calibrateSensor()
aktk 9:1903c6f8d5a9 540 {
aktk 9:1903c6f8d5a9 541 double t[_Sample_Num];
aktk 9:1903c6f8d5a9 542 double ft[_Sample_Num];
aktk 9:1903c6f8d5a9 543
aktk 9:1903c6f8d5a9 544 _sampleData();
aktk 9:1903c6f8d5a9 545 _Last_Point = _Sample_Set[0];
aktk 9:1903c6f8d5a9 546
aktk 9:1903c6f8d5a9 547 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 548 t[i] = _Sample_Set[i].t;
aktk 9:1903c6f8d5a9 549 ft[i]= _Sample_Set[i].x;
aktk 9:1903c6f8d5a9 550 }
aktk 9:1903c6f8d5a9 551 _makeModel(t,ft,_C_x);
aktk 9:1903c6f8d5a9 552 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 553 ft[i]= _Sample_Set[i].y;
aktk 9:1903c6f8d5a9 554 }
aktk 9:1903c6f8d5a9 555 _makeModel(t,ft,_C_y);
aktk 9:1903c6f8d5a9 556
aktk 9:1903c6f8d5a9 557 }
aktk 9:1903c6f8d5a9 558
aktk 9:1903c6f8d5a9 559 void CubicSpline2d::saveSetting()
aktk 9:1903c6f8d5a9 560 {
aktk 9:1903c6f8d5a9 561 FILE *fp;
aktk 9:1903c6f8d5a9 562
aktk 9:1903c6f8d5a9 563 fp = fopen("/local/savedata.log", "wb");
aktk 9:1903c6f8d5a9 564
aktk 9:1903c6f8d5a9 565 // Save _Sample_Num
aktk 9:1903c6f8d5a9 566 fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 567 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 568 // Save _Sample_Set
aktk 9:1903c6f8d5a9 569 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 570 fwrite(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 571 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 572 fwrite(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 573 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 574 fwrite(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 575 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 576 }
aktk 9:1903c6f8d5a9 577 // Save _C_x
aktk 9:1903c6f8d5a9 578 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 579 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 580 fwrite(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 581 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 582 }
aktk 9:1903c6f8d5a9 583 }
aktk 9:1903c6f8d5a9 584 // Save _C_y
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_y[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
aktk 9:1903c6f8d5a9 592 fclose(fp);
aktk 9:1903c6f8d5a9 593
aktk 9:1903c6f8d5a9 594 }
aktk 9:1903c6f8d5a9 595
aktk 9:1903c6f8d5a9 596 void CubicSpline2d::saveSetting(
aktk 9:1903c6f8d5a9 597 const char *filename
aktk 9:1903c6f8d5a9 598 )
aktk 9:1903c6f8d5a9 599 {
aktk 9:1903c6f8d5a9 600 FILE *fp;
aktk 9:1903c6f8d5a9 601 char *filepath;
aktk 9:1903c6f8d5a9 602 int fnnum = 0;
aktk 9:1903c6f8d5a9 603
aktk 9:1903c6f8d5a9 604 while (filename[fnnum] != 0) fnnum++;
aktk 9:1903c6f8d5a9 605 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 9:1903c6f8d5a9 606
aktk 9:1903c6f8d5a9 607 sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 608 fp = fopen(filepath, "wb");
aktk 9:1903c6f8d5a9 609
aktk 9:1903c6f8d5a9 610 // Save _Sample_Num
aktk 9:1903c6f8d5a9 611 fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 612 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 613 // Save _Sample_Set
aktk 9:1903c6f8d5a9 614 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 615 fwrite(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 616 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 617 fwrite(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 618 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 619 fwrite(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 620 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 621 }
aktk 9:1903c6f8d5a9 622 // Save _C_x
aktk 9:1903c6f8d5a9 623 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 624 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 625 fwrite(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 626 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 627 }
aktk 9:1903c6f8d5a9 628 }
aktk 9:1903c6f8d5a9 629 // Save _C_y
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_y[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
aktk 9:1903c6f8d5a9 637 fclose(fp);
aktk 9:1903c6f8d5a9 638 free(filepath);
aktk 9:1903c6f8d5a9 639 }
aktk 9:1903c6f8d5a9 640
aktk 9:1903c6f8d5a9 641 void CubicSpline2d::loadSetting()
aktk 9:1903c6f8d5a9 642 {
aktk 9:1903c6f8d5a9 643 FILE *fp;
aktk 9:1903c6f8d5a9 644 char tmp;
aktk 9:1903c6f8d5a9 645
aktk 9:1903c6f8d5a9 646 //sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 647 //fp = fopen(filepath, "rb");
aktk 9:1903c6f8d5a9 648 fp = fopen("/local/savedata.log", "rb");
aktk 9:1903c6f8d5a9 649
aktk 9:1903c6f8d5a9 650 // Load _Sample_Num
aktk 9:1903c6f8d5a9 651 fread(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 652 fread(&tmp, sizeof(char), 1, fp);
aktk 9:1903c6f8d5a9 653
aktk 9:1903c6f8d5a9 654 // Load _Sample_Set
aktk 9:1903c6f8d5a9 655 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 656 fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 657 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 658 fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 659 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 660 fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 661 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 662 }
aktk 9:1903c6f8d5a9 663
aktk 9:1903c6f8d5a9 664 // Load _C_x
aktk 9:1903c6f8d5a9 665 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 666 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 667 fread(&_C_x[j][i], 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_y
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_y[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 fclose(fp);
aktk 9:1903c6f8d5a9 679 }
aktk 9:1903c6f8d5a9 680
aktk 9:1903c6f8d5a9 681
aktk 9:1903c6f8d5a9 682 void CubicSpline2d::loadSetting(
aktk 9:1903c6f8d5a9 683 const char *filename
aktk 9:1903c6f8d5a9 684 )
aktk 9:1903c6f8d5a9 685 {
aktk 9:1903c6f8d5a9 686 FILE *fp;
aktk 9:1903c6f8d5a9 687 char *filepath;
aktk 9:1903c6f8d5a9 688 char tmp;
aktk 9:1903c6f8d5a9 689 int fnnum = 0;
aktk 9:1903c6f8d5a9 690
aktk 9:1903c6f8d5a9 691 while (filename[fnnum] != 0) fnnum++;
aktk 9:1903c6f8d5a9 692 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 9:1903c6f8d5a9 693
aktk 9:1903c6f8d5a9 694 sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 695 fp = fopen(filepath, "rb");
aktk 9:1903c6f8d5a9 696
aktk 9:1903c6f8d5a9 697 // Load _Sample_Num
aktk 9:1903c6f8d5a9 698 fread(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 699 fread(&tmp, sizeof(char), 1, fp);
aktk 9:1903c6f8d5a9 700
aktk 9:1903c6f8d5a9 701 // Load _Sample_Set
aktk 9:1903c6f8d5a9 702 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 703 fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 704 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 705 fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 706 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 707 fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 708 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 709 }
aktk 9:1903c6f8d5a9 710
aktk 9:1903c6f8d5a9 711 // Load _C_x
aktk 9:1903c6f8d5a9 712 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 713 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 714 fread(&_C_x[j][i], 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
aktk 9:1903c6f8d5a9 719 // Load _C_y
aktk 9:1903c6f8d5a9 720 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 721 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 722 fread(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 723 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 724 }
aktk 9:1903c6f8d5a9 725 }
aktk 9:1903c6f8d5a9 726 fclose(fp);
aktk 9:1903c6f8d5a9 727 free(filepath);
aktk 9:1903c6f8d5a9 728 }
aktk 9:1903c6f8d5a9 729
aktk 9:1903c6f8d5a9 730 void CubicSpline2d::printOutData()
aktk 9:1903c6f8d5a9 731 {
aktk 9:1903c6f8d5a9 732 FILE *fp;
aktk 9:1903c6f8d5a9 733 double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100;
aktk 9:1903c6f8d5a9 734
aktk 9:1903c6f8d5a9 735 fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 9:1903c6f8d5a9 736
aktk 9:1903c6f8d5a9 737 fprintf(fp, "x, y\n");
aktk 9:1903c6f8d5a9 738 for(int ival = 0; ival < _Sample_Num; ival++) {
aktk 9:1903c6f8d5a9 739 for(double x = _Sample_Set[ival].x; x < _Sample_Set[ival+1].x; x += d) {
aktk 9:1903c6f8d5a9 740 fprintf(fp, "%f,%f\n", x, getY(x));
aktk 9:1903c6f8d5a9 741 }
aktk 9:1903c6f8d5a9 742 }
aktk 9:1903c6f8d5a9 743
aktk 9:1903c6f8d5a9 744 fprintf(fp, "\nSample:dst, vol\n");
aktk 9:1903c6f8d5a9 745 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 746 fprintf(fp, "%f,%f,(%f)\n", _Sample_Set[i].x, _Sample_Set[i].y, _Sample_Set[i].t);
aktk 9:1903c6f8d5a9 747 }
aktk 9:1903c6f8d5a9 748 fclose(fp);
aktk 9:1903c6f8d5a9 749
aktk 9:1903c6f8d5a9 750 }
aktk 9:1903c6f8d5a9 751 void CubicSpline2d::_printOutData(const double *arg, const int num, const char* name)
aktk 9:1903c6f8d5a9 752 {
aktk 9:1903c6f8d5a9 753 FILE *fp;
aktk 9:1903c6f8d5a9 754
aktk 9:1903c6f8d5a9 755 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 756 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 757 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 758 fprintf(fp, "%.2f, ", arg[i]);
aktk 9:1903c6f8d5a9 759 }
aktk 9:1903c6f8d5a9 760 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 761 fclose(fp);
aktk 9:1903c6f8d5a9 762 }
aktk 9:1903c6f8d5a9 763 void CubicSpline2d::_printOutDataCouple(const double *arg1, const double *arg2, const int num, const char* name)
aktk 9:1903c6f8d5a9 764 {
aktk 9:1903c6f8d5a9 765 FILE *fp;
aktk 9:1903c6f8d5a9 766
aktk 9:1903c6f8d5a9 767 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 768 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 769 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 770 fprintf(fp, "(%.2f, %.2f)\n", arg1[i], arg2[i]);
aktk 9:1903c6f8d5a9 771 }
aktk 9:1903c6f8d5a9 772 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 773 fclose(fp);
aktk 9:1903c6f8d5a9 774 }
aktk 9:1903c6f8d5a9 775 void CubicSpline2d::_printOutData(const Vxyt *arg, int num, const char* name)
aktk 9:1903c6f8d5a9 776 {
aktk 9:1903c6f8d5a9 777 FILE *fp;
aktk 9:1903c6f8d5a9 778
aktk 9:1903c6f8d5a9 779 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 780 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 781 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 782 fprintf(fp, "%f, ", arg[i].y);
aktk 9:1903c6f8d5a9 783 }
aktk 9:1903c6f8d5a9 784 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 785 fclose(fp);
aktk 9:1903c6f8d5a9 786 }