Akifumi Takahashi / CubicSpline

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of TRP105F_Spline by Akifumi Takahashi

CubicSpline.c@8:e7d451bb4fd4, 2016-05-26 (annotated)

Committer:
aktk
Date:
Thu May 26 04:50:45 2016 +0000
Revision:
8:e7d451bb4fd4
Parent:
7:e032ddec6ed5 
function to save and load the data has been implemented.

Who changed what in which revision?

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