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

Committer:
aktk
Date:
Sun May 29 09:31:44 2016 +0000
Revision:
11:a279e31d8499
Parent:
10:607a68db6303
Child:
12:b3e07a2220bc
In _solve_cubic_f, pow(u,1/3) -> exp(log(u)/3)

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 11:a279e31d8499 379 //u = pow(u, 1/3);
aktk 11:a279e31d8499 380 //v = pow(v, 1/3);
aktk 11:a279e31d8499 381 u = std::exp(std::log(u)/3);
aktk 11:a279e31d8499 382 v = std::exp(std::log(u)/3);
aktk 9:1903c6f8d5a9 383 }
aktk 9:1903c6f8d5a9 384 //One real root and two complex root
aktk 9:1903c6f8d5a9 385 else {
aktk 9:1903c6f8d5a9 386 u = std::complex<double>(-q+sqrt(D),0.0);
aktk 9:1903c6f8d5a9 387 v = std::complex<double>(-q-sqrt(D),0.0);
aktk 9:1903c6f8d5a9 388 u = std::complex<double>(cbrt(u.real()), 0.0);
aktk 9:1903c6f8d5a9 389 v = std::complex<double>(cbrt(v.real()), 0.0);
aktk 9:1903c6f8d5a9 390 }
aktk 9:1903c6f8d5a9 391 #ifdef DEBUG_SOLVE
aktk 9:1903c6f8d5a9 392 g_Serial_Signal.printf("u: %f + (%f)i\n", u.real(), u.imag());
aktk 9:1903c6f8d5a9 393 g_Serial_Signal.printf("v: %f + (%f)i\n", v.real(), v.imag());
aktk 9:1903c6f8d5a9 394 #endif
aktk 9:1903c6f8d5a9 395
aktk 9:1903c6f8d5a9 396 //Cubic root of 1
aktk 9:1903c6f8d5a9 397 std::complex<double> omega[3]= {
aktk 9:1903c6f8d5a9 398 std::complex<double>( 1.0, 0.0),
aktk 9:1903c6f8d5a9 399 std::complex<double>(-1/2, sqrt(3.0)/2),
aktk 9:1903c6f8d5a9 400 std::complex<double>(-1/2,-sqrt(3.0)/2)
aktk 9:1903c6f8d5a9 401 };
aktk 9:1903c6f8d5a9 402
aktk 9:1903c6f8d5a9 403 //Solution of the formula
aktk 9:1903c6f8d5a9 404 arg_t[0] = omega[0] * u + omega[0] * v + d;
aktk 9:1903c6f8d5a9 405 arg_t[1] = omega[1] * u + omega[2] * v + d;
aktk 9:1903c6f8d5a9 406 arg_t[2] = omega[2] * u + omega[1] * v + d;
aktk 9:1903c6f8d5a9 407
aktk 9:1903c6f8d5a9 408 #ifdef DEBUG_SOLVE
aktk 9:1903c6f8d5a9 409 for(int i = 0; i < 3; i++)
aktk 9:1903c6f8d5a9 410 g_Serial_Signal.printf("t%d: %f + (%f)i\n", i, arg_t[i].real(), arg_t[i].imag() );
aktk 9:1903c6f8d5a9 411 #endif
aktk 9:1903c6f8d5a9 412 }
aktk 9:1903c6f8d5a9 413
aktk 9:1903c6f8d5a9 414 #define DEBUG_GETX "DEBUG_GETX\n"
aktk 9:1903c6f8d5a9 415 double CubicSpline2d::getX(double arg_y)
aktk 9:1903c6f8d5a9 416 {
aktk 9:1903c6f8d5a9 417 double x;
aktk 9:1903c6f8d5a9 418 double C[4];
aktk 9:1903c6f8d5a9 419 double the_t;
aktk 9:1903c6f8d5a9 420 int the_i;
aktk 9:1903c6f8d5a9 421 std::complex<double>t_sol[3];
aktk 9:1903c6f8d5a9 422 std::vector<double> t_real;
aktk 9:1903c6f8d5a9 423 std::vector<int> t_ival;
aktk 9:1903c6f8d5a9 424
aktk 9:1903c6f8d5a9 425 #ifdef DEBUG_GETX
aktk 9:1903c6f8d5a9 426 g_Serial_Signal.printf(DEBUG_GETX);
aktk 9:1903c6f8d5a9 427 #endif
aktk 9:1903c6f8d5a9 428 // For the every Intervals of Spline,
aktk 9:1903c6f8d5a9 429 //it solves the polynomial defined by C[i] of the interval,
aktk 9:1903c6f8d5a9 430 //checks the solutions are real number,
aktk 9:1903c6f8d5a9 431 //and ckecks the solutions are in the interval.
aktk 9:1903c6f8d5a9 432 // And if not-excluded solutions are more than one,
aktk 9:1903c6f8d5a9 433 //it trys to find which one is more nearest to last point.
aktk 9:1903c6f8d5a9 434 for(int ival = 0; ival < _Sample_Num - 1; ival++) {
aktk 9:1903c6f8d5a9 435 for(int i = 0; i < 4; i++) C[i] = _C_y[i][ival];
aktk 9:1903c6f8d5a9 436 _solve_cubic_f(t_sol, C, arg_y);
aktk 9:1903c6f8d5a9 437 #ifdef DEBUG_GETX
aktk 9:1903c6f8d5a9 438 g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival+1].t);
aktk 9:1903c6f8d5a9 439 #endif
aktk 9:1903c6f8d5a9 440 for(int i = 0; i < 3; i++) {
aktk 9:1903c6f8d5a9 441 // regarding only real solution
aktk 9:1903c6f8d5a9 442 // acuracy (error range) is supposed +-10E-3 here(groundless)
aktk 9:1903c6f8d5a9 443 if(std::abs(t_sol[i].imag()) < 0.000001) {
aktk 10:607a68db6303 444 /* if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
aktk 9:1903c6f8d5a9 445 t_real.push_back(_Sample_Set[ival].t);
aktk 9:1903c6f8d5a9 446 t_ival.push_back(ival);
aktk 9:1903c6f8d5a9 447 } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
aktk 9:1903c6f8d5a9 448 t_real.push_back(_Sample_Set[ival + 1].t);
aktk 9:1903c6f8d5a9 449 t_ival.push_back(ival);
aktk 10:607a68db6303 450 } else*/ if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
aktk 9:1903c6f8d5a9 451 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 452 t_ival.push_back(ival);
aktk 9:1903c6f8d5a9 453 }
aktk 9:1903c6f8d5a9 454 #ifdef DEBUG_GETX
aktk 9:1903c6f8d5a9 455 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
aktk 9:1903c6f8d5a9 456 #endif
aktk 9:1903c6f8d5a9 457 }
aktk 9:1903c6f8d5a9 458 }
aktk 9:1903c6f8d5a9 459 }
aktk 9:1903c6f8d5a9 460
aktk 10:607a68db6303 461 if(t_real.size > 0) {
aktk 10:607a68db6303 462 the_t = t_real[0];
aktk 10:607a68db6303 463 the_i = t_ival[0];
aktk 10:607a68db6303 464 //if t's size is bigger than 1
aktk 10:607a68db6303 465 for(int i = 1; i < t_real.size(); i++) {
aktk 10:607a68db6303 466 if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) {
aktk 10:607a68db6303 467 the_t = t_real[i];
aktk 10:607a68db6303 468 the_i = t_ival[i];
aktk 10:607a68db6303 469 }
aktk 9:1903c6f8d5a9 470 }
aktk 10:607a68db6303 471 } else {
aktk 10:607a68db6303 472 the_t = _Last_Point.t;
aktk 10:607a68db6303 473 for (int i = 0; i < _Sumple_Num - 1; i++)
aktk 10:607a68db6303 474 if(_Sumple_Set[i].t <= the_t && the_t <= _Sample_Set[i+1])
aktk 10:607a68db6303 475 the_i = i;
aktk 9:1903c6f8d5a9 476 }
aktk 9:1903c6f8d5a9 477 for(int i = 0; i < 4; i++) C[i] = _C_x[i][the_i];
aktk 9:1903c6f8d5a9 478 x = _cubic_f(the_t - _Sample_Set[the_i].t, C);
aktk 9:1903c6f8d5a9 479 #ifdef DEBUG_GETX
aktk 9:1903c6f8d5a9 480 g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)\n",the_t, the_i);
aktk 9:1903c6f8d5a9 481 #endif
aktk 9:1903c6f8d5a9 482
aktk 9:1903c6f8d5a9 483 return x;
aktk 9:1903c6f8d5a9 484 }
aktk 9:1903c6f8d5a9 485
aktk 9:1903c6f8d5a9 486 #define DEBUG_GETY "DEBUG_GETY\n"
aktk 9:1903c6f8d5a9 487 double CubicSpline2d::getY(double arg_x)
aktk 9:1903c6f8d5a9 488 {
aktk 9:1903c6f8d5a9 489 double y;
aktk 9:1903c6f8d5a9 490 double C[4];
aktk 9:1903c6f8d5a9 491 double the_t;
aktk 9:1903c6f8d5a9 492 int the_i;
aktk 9:1903c6f8d5a9 493 std::complex<double>t_sol[3];
aktk 9:1903c6f8d5a9 494 std::vector<double> t_real;
aktk 9:1903c6f8d5a9 495 std::vector<int> t_ival;
aktk 9:1903c6f8d5a9 496
aktk 9:1903c6f8d5a9 497 #ifdef DEBUG_GETY
aktk 9:1903c6f8d5a9 498 g_Serial_Signal.printf(DEBUG_GETY);
aktk 9:1903c6f8d5a9 499 #endif
aktk 9:1903c6f8d5a9 500 // For the every Intervals of Spline,
aktk 9:1903c6f8d5a9 501 //it solves the polynomial defined by C[i] of the interval,
aktk 9:1903c6f8d5a9 502 //checks the solutions are real number,
aktk 9:1903c6f8d5a9 503 //and ckecks the solutions are in the interval.
aktk 9:1903c6f8d5a9 504 // And if not-excluded solutions are more than one,
aktk 9:1903c6f8d5a9 505 //it trys to find which one is more nearest to last point.
aktk 9:1903c6f8d5a9 506 for(int ival = 0; ival < _Sample_Num - 1; ival++) {
aktk 9:1903c6f8d5a9 507 for(int i = 0; i < 4; i++) C[i] = _C_x[i][ival];
aktk 9:1903c6f8d5a9 508 _solve_cubic_f(t_sol, C, arg_x);
aktk 9:1903c6f8d5a9 509 for(int i = 0; i < 3; i++) {
aktk 9:1903c6f8d5a9 510 // regarding only real solution
aktk 9:1903c6f8d5a9 511 // acuracy (error range) is supposed +-10E-3 here(groundless)
aktk 9:1903c6f8d5a9 512 if(std::abs(t_sol[i].imag()) < 0.000001) {
aktk 9:1903c6f8d5a9 513 /* */ if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
aktk 9:1903c6f8d5a9 514 t_real.push_back(_Sample_Set[ival].t);
aktk 9:1903c6f8d5a9 515 t_ival.push_back(ival);
aktk 9:1903c6f8d5a9 516 } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
aktk 9:1903c6f8d5a9 517 t_real.push_back(_Sample_Set[ival + 1].t);
aktk 9:1903c6f8d5a9 518 t_ival.push_back(ival);
aktk 9:1903c6f8d5a9 519 } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
aktk 9:1903c6f8d5a9 520 t_real.push_back(t_sol[i].real());
aktk 9:1903c6f8d5a9 521 t_ival.push_back(ival);
aktk 9:1903c6f8d5a9 522 }
aktk 9:1903c6f8d5a9 523 }
aktk 9:1903c6f8d5a9 524 }
aktk 9:1903c6f8d5a9 525
aktk 9:1903c6f8d5a9 526
aktk 9:1903c6f8d5a9 527 the_t = t_real[0];
aktk 9:1903c6f8d5a9 528 the_i = t_ival[0];
aktk 9:1903c6f8d5a9 529 //if t's size is bigger than 1
aktk 9:1903c6f8d5a9 530 for(int i = 1; i < t_real.size(); i++) {
aktk 9:1903c6f8d5a9 531 if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) {
aktk 9:1903c6f8d5a9 532 the_t = t_real[i];
aktk 9:1903c6f8d5a9 533 the_i = t_ival[i];
aktk 9:1903c6f8d5a9 534 }
aktk 9:1903c6f8d5a9 535 }
aktk 9:1903c6f8d5a9 536 }
aktk 9:1903c6f8d5a9 537
aktk 9:1903c6f8d5a9 538 for(int i = 0; i < 4; i++) C[i] = _C_y[i][the_i];
aktk 9:1903c6f8d5a9 539 y = _cubic_f(the_t - _Sample_Set[the_i].t, C);
aktk 9:1903c6f8d5a9 540 #ifdef DEBUG_GETY
aktk 9:1903c6f8d5a9 541 g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)i\n",the_t, the_i);
aktk 9:1903c6f8d5a9 542 #endif
aktk 9:1903c6f8d5a9 543
aktk 9:1903c6f8d5a9 544 return y;
aktk 9:1903c6f8d5a9 545 }
aktk 9:1903c6f8d5a9 546
aktk 9:1903c6f8d5a9 547
aktk 9:1903c6f8d5a9 548 void CubicSpline2d::calibrateSensor()
aktk 9:1903c6f8d5a9 549 {
aktk 9:1903c6f8d5a9 550 double t[_Sample_Num];
aktk 9:1903c6f8d5a9 551 double ft[_Sample_Num];
aktk 9:1903c6f8d5a9 552
aktk 9:1903c6f8d5a9 553 _sampleData();
aktk 9:1903c6f8d5a9 554 _Last_Point = _Sample_Set[0];
aktk 9:1903c6f8d5a9 555
aktk 9:1903c6f8d5a9 556 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 557 t[i] = _Sample_Set[i].t;
aktk 9:1903c6f8d5a9 558 ft[i]= _Sample_Set[i].x;
aktk 9:1903c6f8d5a9 559 }
aktk 9:1903c6f8d5a9 560 _makeModel(t,ft,_C_x);
aktk 9:1903c6f8d5a9 561 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 562 ft[i]= _Sample_Set[i].y;
aktk 9:1903c6f8d5a9 563 }
aktk 9:1903c6f8d5a9 564 _makeModel(t,ft,_C_y);
aktk 9:1903c6f8d5a9 565
aktk 9:1903c6f8d5a9 566 }
aktk 9:1903c6f8d5a9 567
aktk 9:1903c6f8d5a9 568 void CubicSpline2d::saveSetting()
aktk 9:1903c6f8d5a9 569 {
aktk 9:1903c6f8d5a9 570 FILE *fp;
aktk 9:1903c6f8d5a9 571
aktk 9:1903c6f8d5a9 572 fp = fopen("/local/savedata.log", "wb");
aktk 9:1903c6f8d5a9 573
aktk 9:1903c6f8d5a9 574 // Save _Sample_Num
aktk 9:1903c6f8d5a9 575 fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 576 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 577 // Save _Sample_Set
aktk 9:1903c6f8d5a9 578 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 579 fwrite(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 580 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 581 fwrite(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 582 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 583 fwrite(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 584 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 585 }
aktk 9:1903c6f8d5a9 586 // Save _C_x
aktk 9:1903c6f8d5a9 587 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 588 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 589 fwrite(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 590 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 591 }
aktk 9:1903c6f8d5a9 592 }
aktk 9:1903c6f8d5a9 593 // Save _C_y
aktk 9:1903c6f8d5a9 594 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 595 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 596 fwrite(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 597 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 598 }
aktk 9:1903c6f8d5a9 599 }
aktk 9:1903c6f8d5a9 600
aktk 9:1903c6f8d5a9 601 fclose(fp);
aktk 9:1903c6f8d5a9 602
aktk 9:1903c6f8d5a9 603 }
aktk 9:1903c6f8d5a9 604
aktk 9:1903c6f8d5a9 605 void CubicSpline2d::saveSetting(
aktk 9:1903c6f8d5a9 606 const char *filename
aktk 9:1903c6f8d5a9 607 )
aktk 9:1903c6f8d5a9 608 {
aktk 9:1903c6f8d5a9 609 FILE *fp;
aktk 9:1903c6f8d5a9 610 char *filepath;
aktk 9:1903c6f8d5a9 611 int fnnum = 0;
aktk 9:1903c6f8d5a9 612
aktk 9:1903c6f8d5a9 613 while (filename[fnnum] != 0) fnnum++;
aktk 9:1903c6f8d5a9 614 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 9:1903c6f8d5a9 615
aktk 9:1903c6f8d5a9 616 sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 617 fp = fopen(filepath, "wb");
aktk 9:1903c6f8d5a9 618
aktk 9:1903c6f8d5a9 619 // Save _Sample_Num
aktk 9:1903c6f8d5a9 620 fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 621 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 622 // Save _Sample_Set
aktk 9:1903c6f8d5a9 623 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 624 fwrite(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 625 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 626 fwrite(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 627 fputc(0x2c, fp);
aktk 9:1903c6f8d5a9 628 fwrite(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 629 fputc(0x3b, fp);
aktk 9:1903c6f8d5a9 630 }
aktk 9:1903c6f8d5a9 631 // Save _C_x
aktk 9:1903c6f8d5a9 632 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 633 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 634 fwrite(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 635 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 636 }
aktk 9:1903c6f8d5a9 637 }
aktk 9:1903c6f8d5a9 638 // Save _C_y
aktk 9:1903c6f8d5a9 639 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 640 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 641 fwrite(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 642 fputc((j != 3)? 0x2c : 0x3b, fp);
aktk 9:1903c6f8d5a9 643 }
aktk 9:1903c6f8d5a9 644 }
aktk 9:1903c6f8d5a9 645
aktk 9:1903c6f8d5a9 646 fclose(fp);
aktk 9:1903c6f8d5a9 647 free(filepath);
aktk 9:1903c6f8d5a9 648 }
aktk 9:1903c6f8d5a9 649
aktk 9:1903c6f8d5a9 650 void CubicSpline2d::loadSetting()
aktk 9:1903c6f8d5a9 651 {
aktk 9:1903c6f8d5a9 652 FILE *fp;
aktk 9:1903c6f8d5a9 653 char tmp;
aktk 9:1903c6f8d5a9 654
aktk 9:1903c6f8d5a9 655 //sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 656 //fp = fopen(filepath, "rb");
aktk 9:1903c6f8d5a9 657 fp = fopen("/local/savedata.log", "rb");
aktk 9:1903c6f8d5a9 658
aktk 9:1903c6f8d5a9 659 // Load _Sample_Num
aktk 9:1903c6f8d5a9 660 fread(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 661 fread(&tmp, sizeof(char), 1, fp);
aktk 9:1903c6f8d5a9 662
aktk 9:1903c6f8d5a9 663 // Load _Sample_Set
aktk 9:1903c6f8d5a9 664 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 665 fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 666 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 667 fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 668 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 669 fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 670 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 671 }
aktk 9:1903c6f8d5a9 672
aktk 9:1903c6f8d5a9 673 // Load _C_x
aktk 9:1903c6f8d5a9 674 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 675 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 676 fread(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 677 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 678 }
aktk 9:1903c6f8d5a9 679 }
aktk 9:1903c6f8d5a9 680 // Load _C_y
aktk 9:1903c6f8d5a9 681 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 682 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 683 fread(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 684 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 685 }
aktk 9:1903c6f8d5a9 686 }
aktk 9:1903c6f8d5a9 687 fclose(fp);
aktk 9:1903c6f8d5a9 688 }
aktk 9:1903c6f8d5a9 689
aktk 9:1903c6f8d5a9 690
aktk 9:1903c6f8d5a9 691 void CubicSpline2d::loadSetting(
aktk 9:1903c6f8d5a9 692 const char *filename
aktk 9:1903c6f8d5a9 693 )
aktk 9:1903c6f8d5a9 694 {
aktk 9:1903c6f8d5a9 695 FILE *fp;
aktk 9:1903c6f8d5a9 696 char *filepath;
aktk 9:1903c6f8d5a9 697 char tmp;
aktk 9:1903c6f8d5a9 698 int fnnum = 0;
aktk 9:1903c6f8d5a9 699
aktk 9:1903c6f8d5a9 700 while (filename[fnnum] != 0) fnnum++;
aktk 9:1903c6f8d5a9 701 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 9:1903c6f8d5a9 702
aktk 9:1903c6f8d5a9 703 sprintf(filepath, "/local/%s", filename);
aktk 9:1903c6f8d5a9 704 fp = fopen(filepath, "rb");
aktk 9:1903c6f8d5a9 705
aktk 9:1903c6f8d5a9 706 // Load _Sample_Num
aktk 9:1903c6f8d5a9 707 fread(&_Sample_Num, sizeof(unsigned int), 1, fp);
aktk 9:1903c6f8d5a9 708 fread(&tmp, sizeof(char), 1, fp);
aktk 9:1903c6f8d5a9 709
aktk 9:1903c6f8d5a9 710 // Load _Sample_Set
aktk 9:1903c6f8d5a9 711 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 712 fread(&_Sample_Set[i].x, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 713 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 714 fread(&_Sample_Set[i].y, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 715 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 716 fread(&_Sample_Set[i].t, sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 717 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 718 }
aktk 9:1903c6f8d5a9 719
aktk 9:1903c6f8d5a9 720 // Load _C_x
aktk 9:1903c6f8d5a9 721 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 722 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 723 fread(&_C_x[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 724 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 725 }
aktk 9:1903c6f8d5a9 726 }
aktk 9:1903c6f8d5a9 727
aktk 9:1903c6f8d5a9 728 // Load _C_y
aktk 9:1903c6f8d5a9 729 for(int i = 0; i < _Sample_Num - 1; i++) {
aktk 9:1903c6f8d5a9 730 for(int j = 0; j < 4; j++) {
aktk 9:1903c6f8d5a9 731 fread(&_C_y[j][i], sizeof(double), 1, fp);
aktk 9:1903c6f8d5a9 732 fread(&tmp, sizeof(char),1,fp);
aktk 9:1903c6f8d5a9 733 }
aktk 9:1903c6f8d5a9 734 }
aktk 9:1903c6f8d5a9 735 fclose(fp);
aktk 9:1903c6f8d5a9 736 free(filepath);
aktk 9:1903c6f8d5a9 737 }
aktk 9:1903c6f8d5a9 738
aktk 9:1903c6f8d5a9 739 void CubicSpline2d::printOutData()
aktk 9:1903c6f8d5a9 740 {
aktk 9:1903c6f8d5a9 741 FILE *fp;
aktk 9:1903c6f8d5a9 742 double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100;
aktk 9:1903c6f8d5a9 743
aktk 9:1903c6f8d5a9 744 fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 9:1903c6f8d5a9 745
aktk 9:1903c6f8d5a9 746 fprintf(fp, "x, y\n");
aktk 9:1903c6f8d5a9 747 for(int ival = 0; ival < _Sample_Num; ival++) {
aktk 9:1903c6f8d5a9 748 for(double x = _Sample_Set[ival].x; x < _Sample_Set[ival+1].x; x += d) {
aktk 9:1903c6f8d5a9 749 fprintf(fp, "%f,%f\n", x, getY(x));
aktk 9:1903c6f8d5a9 750 }
aktk 9:1903c6f8d5a9 751 }
aktk 9:1903c6f8d5a9 752
aktk 9:1903c6f8d5a9 753 fprintf(fp, "\nSample:dst, vol\n");
aktk 9:1903c6f8d5a9 754 for(int i = 0; i < _Sample_Num; i++) {
aktk 9:1903c6f8d5a9 755 fprintf(fp, "%f,%f,(%f)\n", _Sample_Set[i].x, _Sample_Set[i].y, _Sample_Set[i].t);
aktk 9:1903c6f8d5a9 756 }
aktk 9:1903c6f8d5a9 757 fclose(fp);
aktk 9:1903c6f8d5a9 758
aktk 9:1903c6f8d5a9 759 }
aktk 9:1903c6f8d5a9 760 void CubicSpline2d::_printOutData(const double *arg, const int num, const char* name)
aktk 9:1903c6f8d5a9 761 {
aktk 9:1903c6f8d5a9 762 FILE *fp;
aktk 9:1903c6f8d5a9 763
aktk 9:1903c6f8d5a9 764 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 765 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 766 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 767 fprintf(fp, "%.2f, ", arg[i]);
aktk 9:1903c6f8d5a9 768 }
aktk 9:1903c6f8d5a9 769 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 770 fclose(fp);
aktk 9:1903c6f8d5a9 771 }
aktk 9:1903c6f8d5a9 772 void CubicSpline2d::_printOutDataCouple(const double *arg1, const double *arg2, const int num, const char* name)
aktk 9:1903c6f8d5a9 773 {
aktk 9:1903c6f8d5a9 774 FILE *fp;
aktk 9:1903c6f8d5a9 775
aktk 9:1903c6f8d5a9 776 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 777 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 778 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 779 fprintf(fp, "(%.2f, %.2f)\n", arg1[i], arg2[i]);
aktk 9:1903c6f8d5a9 780 }
aktk 9:1903c6f8d5a9 781 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 782 fclose(fp);
aktk 9:1903c6f8d5a9 783 }
aktk 9:1903c6f8d5a9 784 void CubicSpline2d::_printOutData(const Vxyt *arg, int num, const char* name)
aktk 9:1903c6f8d5a9 785 {
aktk 9:1903c6f8d5a9 786 FILE *fp;
aktk 9:1903c6f8d5a9 787
aktk 9:1903c6f8d5a9 788 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 9:1903c6f8d5a9 789 fprintf(fp, "%10s\n", name);
aktk 9:1903c6f8d5a9 790 for(int i = 0; i < num; i++) {
aktk 9:1903c6f8d5a9 791 fprintf(fp, "%f, ", arg[i].y);
aktk 9:1903c6f8d5a9 792 }
aktk 9:1903c6f8d5a9 793 fprintf(fp, "\n");
aktk 9:1903c6f8d5a9 794 fclose(fp);
aktk 9:1903c6f8d5a9 795 }