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
Embed:
(wiki syntax)
Show/hide line numbers
CubicSpline.cpp
00001 #include "CubicSpline.h" 00002 00003 // To get voltage of TRP105F 00004 AnalogIn g_Sensor_Voltage(p16); 00005 // To get sample distance via seral com 00006 Serial g_Serial_Signal(USBTX, USBRX); 00007 00008 LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる） 00009 // for debug 00010 #ifdef DEBUG 00011 DigitalOut led1(LED1); 00012 DigitalOut led2(LED2); 00013 DigitalOut led3(LED3); 00014 DigitalOut led4(LED4); 00015 #endif 00016 00017 CubicSpline2d::CubicSpline2d() 00018 :_useType(AsMODULE) 00019 { 00020 _Sample_Num = 5; 00021 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt)); 00022 _Last_Point = (Vxyt) { 00023 0.0, 0.0, 0.0 00024 }; 00025 for(int i = 0; i < 4; i++) { 00026 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));; 00027 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));; 00028 } 00029 //calibrateSensor(); 00030 } 00031 00032 CubicSpline2d::CubicSpline2d( 00033 unsigned int arg_num 00034 ) 00035 :_useType(AsMODULE) 00036 { 00037 _Sample_Num = arg_num; 00038 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt)); 00039 _Last_Point = (Vxyt) { 00040 0.0, 0.0, 0.0 00041 }; 00042 for(int i = 0; i < 4; i++) { 00043 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));; 00044 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));; 00045 } 00046 //calibrateSensor(); 00047 } 00048 00049 CubicSpline2d::CubicSpline2d( 00050 unsigned int arg_num, 00051 UseType arg_useType 00052 ) 00053 :_useType(arg_useType) 00054 { 00055 _Sample_Num = arg_num; 00056 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt)); 00057 _Last_Point = (Vxyt) { 00058 0.0, 0.0, 0.0 00059 }; 00060 for(int i = 0; i < 4; i++) { 00061 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));; 00062 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));; 00063 } 00064 //calibrateSensor(); 00065 } 00066 00067 CubicSpline2d::~CubicSpline2d() 00068 { 00069 free(_Sample_Set); 00070 //free(_u_param); 00071 for(int i = 0; i < 4; i++) { 00072 free(_C_x[i]); 00073 free(_C_y[i]); 00074 } 00075 } 00076 00077 void CubicSpline2d::_sampleData() 00078 { 00079 int tmp; 00080 char sig; 00081 Vxyt tmp_set; 00082 00083 int floatflag = 0; 00084 00085 // For evry set, 00086 // 1, get dst data via serai com, 00087 // 2, get vol data, 00088 // and then do same for next index set. 00089 for(int i = 0; i < _Sample_Num; i++) { 00090 if(_useType == AsDEBUG) { 00091 // 00092 // Recieve a Distance datus and store it into member 00093 // 00094 g_Serial_Signal.printf("X:"); 00095 _Sample_Set[i].x = 0.0; 00096 do { 00097 sig = g_Serial_Signal.getc(); 00098 if('0' <= sig && sig <= '9') { 00099 if(floatflag == 0) { 00100 _Sample_Set[i].x = 10.0 * _Sample_Set[i].x + (double)(sig - 48); 00101 } else { 00102 _Sample_Set[i].x = _Sample_Set[i].x + (double)(sig - 48) * pow(10.0, (double)- floatflag); 00103 floatflag++; 00104 } 00105 g_Serial_Signal.putc(char(sig)); 00106 } else if(sig == '.') { 00107 if(floatflag == 0) { 00108 floatflag = 1; 00109 g_Serial_Signal.putc(char(sig)); 00110 } 00111 } else if(sig == 0x08) { 00112 _Sample_Set[i].x = 0.0; 00113 g_Serial_Signal.printf("[canseled!]"); 00114 g_Serial_Signal.putc('\n'); 00115 g_Serial_Signal.putc('>'); 00116 } 00117 } while (!(sig == 0x0a || sig == 0x0d)); 00118 floatflag = 0; 00119 g_Serial_Signal.putc('\n'); 00120 g_Serial_Signal.printf("x:%f|",_Sample_Set[i].x); 00121 // 00122 // Recieve a Voltage datus and store it into member 00123 // 00124 // LOW PASS FILTERED 00125 // Get 10 data and store mean as a sample. 00126 // After get one original sample, system waits for 0.1 sec, 00127 // thus it takes 1 sec evry sampling. 00128 _Sample_Set[i].y = 0.0; 00129 for(int j = 0; j < 10; j++) { 00130 tmp_set.y = g_Sensor_Voltage.read(); 00131 #ifdef DEBUG 00132 g_Serial_Signal.printf("%f,",tmp_set.y); 00133 #endif 00134 _Sample_Set[i].y += (tmp_set.y / 10.0); 00135 wait(0.1); 00136 } 00137 #ifdef DEBUG 00138 g_Serial_Signal.printf("(%f)\n",_Sample_Set[i].y); 00139 #endif 00140 } 00141 00142 // if the input data is over the bound, it is calibrated 00143 if (_Sample_Set[i].x < 0.0) 00144 _Sample_Set[i].x = 0.0; 00145 } 00146 // 00147 // Sort set data array in x-Ascending order 00148 // 00149 tmp = 0; 00150 for( int i = 0 ; i < _Sample_Num; i++) { 00151 for(int j = _Sample_Num - 1; i < j ; j--) { 00152 // use dst as index for dst range [2,20] 00153 if (_Sample_Set[i].x > _Sample_Set[j].x) { 00154 tmp_set.x = _Sample_Set[i].x; 00155 tmp_set.y = _Sample_Set[i].y; 00156 _Sample_Set[i].x = _Sample_Set[j].x; 00157 _Sample_Set[i].y = _Sample_Set[j].y; 00158 _Sample_Set[j].x = tmp_set.x; 00159 _Sample_Set[j].y = tmp_set.y; 00160 } 00161 // if a same dst has been input, calcurate mean. 00162 else if (_Sample_Set[i].x == _Sample_Set[j].x) { 00163 tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2.0; 00164 _Sample_Set[i].y = tmp_set.y; 00165 for(int k = j; k < _Sample_Num - 1; k++) 00166 _Sample_Set[k] = _Sample_Set[k+1]; 00167 tmp++; 00168 } 00169 } 00170 } 00171 #ifdef DEBUG 00172 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num ); 00173 g_Serial_Signal.printf("-) tmp: %d\n", tmp ); 00174 #endif 00175 // substruct tmp from number of sample. 00176 _Sample_Num -= tmp; 00177 #ifdef DEBUG 00178 g_Serial_Signal.printf("-----------------\n"); 00179 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num ); 00180 #endif 00181 00182 // generate t which is parameter related to x,y 00183 _Sample_Set[0].t = 0.0; 00184 for(int i = 1; i < _Sample_Num; i++) 00185 _Sample_Set[i].t = 00186 _Sample_Set[i-1].t 00187 + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2.0) 00188 +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2.0)); 00189 } 00190 00191 // 00192 // Function to define _u_spline, specific constants of spline. 00193 // 00194 void CubicSpline2d::_makeModel(const double* arg_sampled_t, const double* arg_sampled_ft, double* arg_C[4], const unsigned int arg_num) 00195 { 00196 // arg_t : t; The variable of f(t) 00197 // arg_ft: f(t); The cubic poliminal in Interval-j. 00198 // arg_C[i]: Ci; The coefficient of t^i of f(t) that defines Spline Model Poliminal f(t). 00199 // arg_num: j in [0,_Sample_Num-1]; The number of interval. 00200 // f(t)j = C3j*t^3 + C2j*t^2 + C1j*t + C0j 00201 // 00202 // N: max of index <=> (_Sample_Num - 1) 00203 // 00204 // u[i] === d^2/dx^2(Spline f)[i] 00205 // i:[0,N] 00206 // u[0] = u[N] = 0 00207 #if defined (VERSION_C) 00208 double *u = (double*)malloc((arg_num ) * sizeof(double)); 00209 #elif defined (VERSION_Cpp) 00210 double *u = new double[arg_num]; 00211 #elif defined (VERSION_Cpp11) 00212 std::array<double,arg_num> u; 00213 #endif 00214 // 00215 // h[i] = x[i+1] - x[i] 00216 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1 00217 double *h = (double*)malloc((arg_num - 1) * sizeof(double)); 00218 // 00219 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i]) 00220 // i:[0,N-2] 00221 double *v = (double*)malloc((arg_num - 2) * sizeof(double)); 00222 // 00223 // temporary array whose num of elm equals v array 00224 double *w = (double*)malloc((arg_num - 2) * sizeof(double)); 00225 // 00226 // [ 2(h[0]+h[1]) , h[1] , O ] [u[1] ] [v[0] ] 00227 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [u[2] ] [v[1] ] 00228 // [ ... ] * [... ] = [... ] 00229 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [u[j+1]] [v[j] ] 00230 // [ ... ] [ ... ] [ ... ] 00231 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [u[j+1]] [v[j] ] 00232 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [u[N-1]] [v[N-2]] 00233 // 00234 // For LU decomposition 00235 double *Upper = (double*)malloc((arg_num - 2) * sizeof(double)); 00236 double *Lower = (double*)malloc((arg_num - 2) * sizeof(double)); 00237 #ifdef DEBUG_MAKE_MODEL 00238 _printOutDataCouple(arg_sampled_t, arg_sampled_ft, arg_num, "\nargument set\n"); 00239 #endif 00240 00241 for(int i = 0; i < arg_num - 1; i++) 00242 h[i] = (double)(arg_sampled_t[i + 1] - arg_sampled_t[i]); 00243 00244 for(int i = 0; i < arg_num - 2; i++) 00245 v[i] = 6.0 * ( 00246 ((double)(arg_sampled_ft[i + 2] - arg_sampled_ft[i + 1])) / h[i + 1] 00247 - 00248 ((double)(arg_sampled_ft[i + 1] - arg_sampled_ft[i])) / h[i] 00249 ); 00250 00251 // 00252 // LU decomposition 00253 // 00254 Upper[0] = 2.0 * (h[0] + h[1]); 00255 Lower[0] = 0.0; 00256 for (int i = 1; i < arg_num - 2; i++) { 00257 Lower[i] = h[i] / Upper[i - 1]; 00258 Upper[i] = 2.0 * (h[i] + h[i + 1]) - Lower[i] * h[i]; 00259 } 00260 // 00261 // forward substitution 00262 // 00263 w[0] = v[0]; 00264 for (int i = 1; i < arg_num - 2; i ++) { 00265 w[i] = v[i] - Lower[i] * w[i-1]; 00266 } 00267 // 00268 // backward substitution 00269 // 00270 u[arg_num - 2] = w[arg_num - 3] / Upper[arg_num - 3]; 00271 for(int i = arg_num - 3; i > 0; i--) { 00272 u[i] = (w[(i - 1)] - h[(i)] * u[(i) + 1]) / Upper[(i - 1)]; 00273 } 00274 // _u_spline[i] === d^2/dx^2(Spline f)[i] 00275 u[0] = u[arg_num - 1] = 0.0; 00276 #ifdef DEBUG_MAKE_MODEL 00277 _printOutData(h, arg_num - 1, "h"); 00278 _printOutData(v, arg_num - 2, "v"); 00279 _printOutData(w, arg_num - 2, "w"); 00280 _printOutData(Upper, arg_num - 2, "Upper"); 00281 _printOutData(Lower, arg_num - 2, "Lower"); 00282 _printOutData(u, arg_num , "u"); 00283 #endif 00284 00285 for(int ival = 0; ival < arg_num - 1; ival++) { 00286 arg_C[3][ival] = (u[ival + 1] - u[ival]) / 6.0 / (arg_sampled_t[ival + 1] - arg_sampled_t[ival]); 00287 arg_C[2][ival] = (u[ival]) / 2.0; 00288 arg_C[1][ival] = (arg_sampled_ft[ival + 1] - arg_sampled_ft[ival]) / (arg_sampled_t[ival + 1] - arg_sampled_t[ival]) 00289 - 00290 (arg_sampled_t[ival + 1] - arg_sampled_t[ival]) * (u[ival + 1] + 2.0 * u[ival]) / 6.0; 00291 arg_C[0][ival] = (arg_sampled_ft[ival]); 00292 } 00293 #ifdef DEBUG_MAKE_MODEL 00294 for(int ival = 0; ival < arg_num - 1; ival++) { 00295 for(int i = 0; i < 4; i++) 00296 g_Serial_Signal.printf("C[%d][%d]: %f\n", i, ival, arg_C[i][ival]); 00297 } 00298 #endif 00299 00300 free(h); 00301 free(u); 00302 free(v); 00303 free(w); 00304 free(Upper); 00305 free(Lower); 00306 } 00307 void CubicSpline2d::_makeModel(const double* arg_t, const double* arg_ft, double* arg_C[4]) 00308 { 00309 _makeModel(arg_t, arg_ft, arg_C, _Sample_Num); 00310 } 00311 00312 00313 void CubicSpline2d::calibrateSensor() 00314 { 00315 double t[_Sample_Num]; 00316 double ft[_Sample_Num]; 00317 00318 _sampleData(); 00319 _Last_Point = _Sample_Set[0]; 00320 00321 for(int i = 0; i < _Sample_Num; i++) { 00322 t[i] = _Sample_Set[i].t; 00323 ft[i]= _Sample_Set[i].x; 00324 } 00325 _makeModel(t,ft,_C_x); 00326 00327 for(int i = 0; i < _Sample_Num; i++) { 00328 ft[i]= _Sample_Set[i].y; 00329 } 00330 _makeModel(t,ft,_C_y); 00331 00332 } 00333 00334 00335 // 00336 // Fuction to return the value of Cubic polynomial f(t) 00337 // 00338 double CubicSpline2d::_cubic_f(const double arg_t, const double arg_C[4]) 00339 { 00340 double ft; //the value of Spline f(t). 00341 00342 ft = arg_C[3] * pow(arg_t, 3.0) + arg_C[2] * pow(arg_t, 2.0) + arg_C[1] * arg_t + arg_C[0]; 00343 00344 return ft; 00345 } 00346 00347 00348 // 00349 // Function to solve a cubic polinomial 00350 // by using Gardano-Tartaglia formula 00351 // 00352 void CubicSpline2d::_solve_cubic_f( 00353 std::complex<double> arg_t[3], 00354 const double arg_C[4], 00355 const double arg_ft) 00356 { 00357 #ifdef DEBUG_SOLVE 00358 for(int i = 0; i < 4; i++) 00359 g_Serial_Signal.printf("C%d: %f\n", i, arg_C[i]); 00360 #endif 00361 00362 //arg_t: solution that's expected to be solved in this function. 00363 //t_sol: the solution that's actually wanted as Sprine's solution. 00364 //t0_ival: _Sample_Set[ival].t 00365 //arg_t = t_sol - t0_ival 00366 //=> 00367 //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0] 00368 // = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0] 00369 double c[3]; 00370 //f(t) := arg_ft/arg_C[3] 00371 // = arg_t^3 + c[2]*arg_t^2 + c[1]*arg_t + c[0]. 00372 for(int i = 0; i < 3; i++) { 00373 c[i] = arg_C[i] / arg_C[3]; 00374 } 00375 //modify the formula 00376 //t^3 + c[2]*t^2 + c[1]*t + (c[0] - f(t)) = 0. 00377 c[0] -= arg_ft / arg_C[3]; 00378 #ifdef DEBUG_SOLVE 00379 for(int i = 0; i < 3; i++) 00380 g_Serial_Signal.printf("c%d: %f\n", i, c[i]); 00381 #endif 00382 00383 std::complex<double> d; 00384 //d := c[2] / 3 00385 //T := t + d (== arg_t - t_iavl + c[2]/3) 00386 d = std::complex<double>(c[2] / 3.0, 0.0); 00387 //=> T^3 + 3pT + 2q = 0. 00388 double p,q; 00389 //The values defined from coefficients of the formula 00390 //that identify solutions 00391 p = ( -pow(c[2], 2.0) + 3.0 * c[1]) / 9.0; 00392 q = (2.0 * pow(c[2], 3.0) - 9.0 * c[2] * c[1] + 27.0 * c[0]) / 54.0; 00393 00394 //Discriminant section 00395 double D; 00396 D = pow(p, 3.0) + pow(q, 2.0); 00397 #ifdef DEBUG_SOLVE 00398 g_Serial_Signal.printf("p: %f\n", p); 00399 g_Serial_Signal.printf("q: %f\n", q); 00400 g_Serial_Signal.printf("D: %f\n", D); 00401 #endif 00402 00403 //For all T, u, there exsists v: T = u + v; increment degree of freedom. 00404 //Futhermore, 00405 // u^3 + v^3 - 2q = 0 00406 // uv + p = 0 <=> u^3v^3 = -p^3 00407 //those because: T = u + v, T^3 + 3pT + 2q = 0, 00408 //=> (u + v)^3 + 3p(u + v) + 2q = 0 00409 //=> u^3 + v^3 + 3(uv + p)(u+v) + 2q = 0 00410 //The values defined from p and q 00411 //that idetify solutions 00412 std::complex<double> u,v; 00413 //Real root only 00414 if(D <= 0.0) { 00415 u = std::complex<double>(-q, sqrt(-D)); 00416 v = std::complex<double>(-q,-sqrt(-D)); 00417 //u = pow(u, 1/3); 00418 //v = pow(v, 1/3); 00419 u = std::exp(std::log(u)/std::complex<double>(3.0,0.0)); 00420 v = std::exp(std::log(v)/std::complex<double>(3.0,0.0)); 00421 } 00422 //One real root and two complex root 00423 else { 00424 u = std::complex<double>(-q+sqrt(D),0.0); 00425 v = std::complex<double>(-q-sqrt(D),0.0); 00426 u = std::complex<double>(cbrt(u.real()), 0.0); 00427 v = std::complex<double>(cbrt(v.real()), 0.0); 00428 } 00429 #ifdef DEBUG_SOLVE 00430 g_Serial_Signal.printf("u: %f + (%f)i\n", u.real(), u.imag()); 00431 g_Serial_Signal.printf("v: %f + (%f)i\n", v.real(), v.imag()); 00432 g_Serial_Signal.printf("d: %f + (%f)i\n", d.real(), d.imag()); 00433 #endif 00434 00435 //Cubic root of 1 00436 std::complex<double> omega[3]= { 00437 std::complex<double>( 1.0, 0.0), 00438 std::complex<double>(-1.0/2.0, sqrt(3.0)/2.0), 00439 std::complex<double>(-1.0/2.0,-sqrt(3.0)/2.0) 00440 }; 00441 00442 //Solution of the formula 00443 arg_t[0] = omega[0] * u + omega[0] * v - d; 00444 arg_t[1] = omega[1] * u + omega[2] * v - d; 00445 arg_t[2] = omega[2] * u + omega[1] * v - d; 00446 00447 #ifdef DEBUG_SOLVE 00448 for(int i = 0; i < 3; i++) 00449 g_Serial_Signal.printf("t%d: %f + (%f)i\n", i, arg_t[i].real(), arg_t[i].imag() ); 00450 #endif 00451 } 00452 00453 double CubicSpline2d::getX(double arg_y) 00454 { 00455 double x; 00456 double C[4]; 00457 double the_t; 00458 int the_i; 00459 std::complex<double>t_sol[3]; 00460 std::vector<double> t_real; 00461 std::vector<int> t_ival; 00462 00463 #ifdef DEBUG_GETX 00464 g_Serial_Signal.printf(DEBUG_GETX); 00465 #endif 00466 // For the every Intervals of Spline, 00467 //it solves the polynomial defined by C[i] of the interval, 00468 //checks the solutions are real number, 00469 //and ckecks the solutions are in the interval. 00470 // And if not-excluded solutions are more than one, 00471 //it trys to find which one is more nearest to last point. 00472 for(int ival = 0; ival < _Sample_Num - 1; ival++) { 00473 for(int i = 0; i < 4; i++) C[i] = _C_y[i][ival]; 00474 _solve_cubic_f(t_sol, C, arg_y); 00475 for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t; 00476 #ifdef DEBUG_GETX 00477 g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t); 00478 #endif 00479 for(int i = 0; i < 3; i++) { 00480 // regarding only real solution 00481 // acuracy (error range) is supposed +-10E-3 here(groundless) 00482 if(std::abs(t_sol[i].imag()) < 0.000001) { 00483 /* */ if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) { 00484 t_real.push_back(t_sol[i].real()); 00485 t_ival.push_back(ival); 00486 #ifdef DEBUG_GETX 00487 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival); 00488 #endif 00489 } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) { 00490 t_real.push_back(t_sol[i].real()); 00491 t_ival.push_back(ival); 00492 #ifdef DEBUG_GETX 00493 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival); 00494 #endif 00495 } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival + 1].t) { 00496 t_real.push_back(t_sol[i].real()); 00497 t_ival.push_back(ival); 00498 #ifdef DEBUG_GETX 00499 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival); 00500 #endif 00501 } 00502 } 00503 } 00504 } 00505 00506 if(!t_real.empty()) { 00507 the_t = t_real[0]; 00508 the_i = t_ival[0]; 00509 //if t's size is bigger than 1 00510 for(int i = 1; i < t_real.size(); i++) { 00511 if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) { 00512 the_t = t_real[i]; 00513 the_i = t_ival[i]; 00514 } 00515 } 00516 } else { 00517 #ifdef DEBUG_GETX 00518 g_Serial_Signal.printf("LastPoint\n"); 00519 #endif 00520 the_t = _Last_Point.t; 00521 for (int i = 0; i < _Sample_Num - 1; i++) 00522 if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t) 00523 the_i = i; 00524 } 00525 /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t; 00526 else if (_Sample_Set[_Sample_Num - 1].t <= the_t) the_t = _Sample_Set[_Sample_Num - 1].t; 00527 00528 for(int i = 0; i < 4; i++) C[i] = _C_x[i][the_i]; 00529 x = _cubic_f(the_t - _Sample_Set[the_i].t, C); 00530 #ifdef DEBUG_GETX 00531 g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)\n",the_t, the_i); 00532 #endif 00533 _Last_Point = (Vxyt) { 00534 x, arg_y, the_t 00535 }; 00536 return x; 00537 } 00538 00539 double CubicSpline2d::getY(double arg_x) 00540 { 00541 00542 double y; 00543 double C[4]; 00544 double the_t; 00545 int the_i; 00546 std::complex<double>t_sol[3]; 00547 std::vector<double> t_real; 00548 std::vector<int> t_ival; 00549 00550 #ifdef DEBUG_GETY 00551 g_Serial_Signal.printf(DEBUG_GETY); 00552 g_Serial_Signal.printf("arg_x: %f\n", arg_x); 00553 #endif 00554 // For the every Intervals of Spline, 00555 //it solves the polynomial defined by C[i] of the interval, 00556 //checks the solutions are real number, 00557 //and ckecks the solutions are in the interval. 00558 // And if not-excluded solutions are more than one, 00559 //it trys to find which one is more nearest to last point. 00560 for(int ival = 0; ival < _Sample_Num - 1; ival++) { 00561 for(int i = 0; i < 4; i++) C[i] = _C_x[i][ival]; 00562 _solve_cubic_f(t_sol, C, arg_x); 00563 for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t; 00564 //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0] 00565 // = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0] 00566 #ifdef DEBUG_GETY 00567 g_Serial_Signal.printf("interval:%d \t %f <= t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t); 00568 for(int i = 0; i < 3; i++) 00569 g_Serial_Signal.printf("t%d \t %f + (%f)i\n", i, t_sol[i].real(), t_sol[i].imag()); 00570 #endif 00571 for(int i = 0; i < 3; i++) { 00572 // regarding only real solution 00573 // acuracy (error range) is supposed +-10E-6 here(groundless) 00574 if(std::abs(t_sol[i].imag()) < 0.000001) { 00575 /**/ if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) { 00576 t_real.push_back(t_sol[i].real()); 00577 t_ival.push_back(ival); 00578 #ifdef DEBUG_GETY 00579 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival); 00580 #endif 00581 }// 00582 else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) { 00583 t_real.push_back(t_sol[i].real()); 00584 t_ival.push_back(ival); 00585 #ifdef DEBUG_GETY 00586 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival); 00587 #endif 00588 } 00589 // There sometimes be a so fucking small error in solutions, 00590 // so acuracy is set at 10E-6(groundless) 00591 else if (static_cast<int>(_Sample_Set[ival].t * std::pow(10., 6.)) <= static_cast<int>(t_sol[i].real() * std::pow(10., 6.)) 00592 && static_cast<int>(t_sol[i].real() * std::pow(10., 6.)) < static_cast<int>(_Sample_Set[ival + 1].t * std::pow(10., 6.))) { 00593 t_real.push_back(t_sol[i].real()); 00594 t_ival.push_back(ival); 00595 #ifdef DEBUG_GETY 00596 g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival); 00597 #endif 00598 } 00599 } 00600 } 00601 } 00602 00603 if(!t_real.empty()) { 00604 the_t = t_real[0]; 00605 the_i = t_ival[0]; 00606 //if t's size is bigger than 1 00607 for(int i = 1; i < t_real.size(); i++) { 00608 if(std::abs(t_real[i] - _Last_Point.t) < std::abs(the_t - _Last_Point.t)) { 00609 the_t = t_real[i]; 00610 the_i = t_ival[i]; 00611 } 00612 } 00613 } 00614 //if no solution muched due to any errors 00615 else { 00616 #ifdef DEBUG_GETY 00617 g_Serial_Signal.printf("LastPoint\n"); 00618 #endif 00619 the_t = _Last_Point.t; 00620 for (int i = 0; i < _Sample_Num - 1; i++) 00621 if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t) 00622 the_i = i; 00623 } 00624 00625 // 00626 /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t; 00627 else if (_Sample_Set[_Sample_Num - 1].t < the_t) the_t = _Sample_Set[_Sample_Num - 1].t; 00628 00629 // 00630 for(int i = 0; i < 4; i++) C[i] = _C_y[i][the_i]; 00631 y = _cubic_f(the_t - _Sample_Set[the_i].t, C); 00632 #ifdef DEBUG_GETY 00633 g_Serial_Signal.printf("(the_t, the_i):= (%f , %d)\n",the_t, the_i); 00634 #endif 00635 _Last_Point = (Vxyt) { 00636 y, arg_x, the_t 00637 }; 00638 return y; 00639 } 00640 00641 00642 00643 void CubicSpline2d::saveSetting() 00644 { 00645 FILE *fp; 00646 00647 fp = fopen("/local/savedata.log", "wb"); 00648 00649 // Save _Sample_Num 00650 fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp); 00651 fputc(0x3b, fp); 00652 // Save _Sample_Set 00653 for(int i = 0; i < _Sample_Num; i++) { 00654 fwrite(&_Sample_Set[i].x, sizeof(double), 1, fp); 00655 fputc(0x2c, fp); 00656 fwrite(&_Sample_Set[i].y, sizeof(double), 1, fp); 00657 fputc(0x2c, fp); 00658 fwrite(&_Sample_Set[i].t, sizeof(double), 1, fp); 00659 fputc(0x3b, fp); 00660 } 00661 // Save _C_x 00662 for(int i = 0; i < _Sample_Num - 1; i++) { 00663 for(int j = 0; j < 4; j++) { 00664 fwrite(&_C_x[j][i], sizeof(double), 1, fp); 00665 fputc((j != 3)? 0x2c : 0x3b, fp); 00666 } 00667 } 00668 // Save _C_y 00669 for(int i = 0; i < _Sample_Num - 1; i++) { 00670 for(int j = 0; j < 4; j++) { 00671 fwrite(&_C_y[j][i], sizeof(double), 1, fp); 00672 fputc((j != 3)? 0x2c : 0x3b, fp); 00673 } 00674 } 00675 00676 fclose(fp); 00677 00678 } 00679 00680 void CubicSpline2d::saveSetting( 00681 const char *filename 00682 ) 00683 { 00684 FILE *fp; 00685 char *filepath; 00686 int fnnum = 0; 00687 00688 while (filename[fnnum] != 0) fnnum++; 00689 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char. 00690 00691 sprintf(filepath, "/local/%s", filename); 00692 fp = fopen(filepath, "wb"); 00693 00694 // Save _Sample_Num 00695 fwrite(&_Sample_Num, sizeof(unsigned int), 1, fp); 00696 fputc(0x3b, fp); 00697 // Save _Sample_Set 00698 for(int i = 0; i < _Sample_Num; i++) { 00699 fwrite(&_Sample_Set[i].x, sizeof(double), 1, fp); 00700 fputc(0x2c, fp); 00701 fwrite(&_Sample_Set[i].y, sizeof(double), 1, fp); 00702 fputc(0x2c, fp); 00703 fwrite(&_Sample_Set[i].t, sizeof(double), 1, fp); 00704 fputc(0x3b, fp); 00705 } 00706 // Save _C_x 00707 for(int i = 0; i < _Sample_Num - 1; i++) { 00708 for(int j = 0; j < 4; j++) { 00709 fwrite(&_C_x[j][i], sizeof(double), 1, fp); 00710 fputc((j != 3)? 0x2c : 0x3b, fp); 00711 } 00712 } 00713 // Save _C_y 00714 for(int i = 0; i < _Sample_Num - 1; i++) { 00715 for(int j = 0; j < 4; j++) { 00716 fwrite(&_C_y[j][i], sizeof(double), 1, fp); 00717 fputc((j != 3)? 0x2c : 0x3b, fp); 00718 } 00719 } 00720 00721 fclose(fp); 00722 free(filepath); 00723 } 00724 00725 void CubicSpline2d::loadSetting() 00726 { 00727 FILE *fp; 00728 char tmp; 00729 00730 //sprintf(filepath, "/local/%s", filename); 00731 //fp = fopen(filepath, "rb"); 00732 fp = fopen("/local/savedata.log", "rb"); 00733 00734 // Load _Sample_Num 00735 fread(&_Sample_Num, sizeof(unsigned int), 1, fp); 00736 fread(&tmp, sizeof(char), 1, fp); 00737 00738 // Load _Sample_Set 00739 for(int i = 0; i < _Sample_Num; i++) { 00740 fread(&_Sample_Set[i].x, sizeof(double), 1, fp); 00741 fread(&tmp, sizeof(char),1,fp); 00742 fread(&_Sample_Set[i].y, sizeof(double), 1, fp); 00743 fread(&tmp, sizeof(char),1,fp); 00744 fread(&_Sample_Set[i].t, sizeof(double), 1, fp); 00745 fread(&tmp, sizeof(char),1,fp); 00746 } 00747 00748 // Load _C_x 00749 for(int i = 0; i < _Sample_Num - 1; i++) { 00750 for(int j = 0; j < 4; j++) { 00751 fread(&_C_x[j][i], sizeof(double), 1, fp); 00752 fread(&tmp, sizeof(char),1,fp); 00753 } 00754 } 00755 // Load _C_y 00756 for(int i = 0; i < _Sample_Num - 1; i++) { 00757 for(int j = 0; j < 4; j++) { 00758 fread(&_C_y[j][i], sizeof(double), 1, fp); 00759 fread(&tmp, sizeof(char),1,fp); 00760 } 00761 } 00762 fclose(fp); 00763 } 00764 00765 00766 void CubicSpline2d::loadSetting( 00767 const char *filename 00768 ) 00769 { 00770 FILE *fp; 00771 char *filepath; 00772 char tmp; 00773 int fnnum = 0; 00774 00775 while (filename[fnnum] != 0) fnnum++; 00776 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char. 00777 00778 sprintf(filepath, "/local/%s", filename); 00779 fp = fopen(filepath, "rb"); 00780 00781 // Load _Sample_Num 00782 fread(&_Sample_Num, sizeof(unsigned int), 1, fp); 00783 fread(&tmp, sizeof(char), 1, fp); 00784 00785 // Load _Sample_Set 00786 for(int i = 0; i < _Sample_Num; i++) { 00787 fread(&_Sample_Set[i].x, sizeof(double), 1, fp); 00788 fread(&tmp, sizeof(char),1,fp); 00789 fread(&_Sample_Set[i].y, sizeof(double), 1, fp); 00790 fread(&tmp, sizeof(char),1,fp); 00791 fread(&_Sample_Set[i].t, sizeof(double), 1, fp); 00792 fread(&tmp, sizeof(char),1,fp); 00793 } 00794 00795 // Load _C_x 00796 for(int i = 0; i < _Sample_Num - 1; i++) { 00797 for(int j = 0; j < 4; j++) { 00798 fread(&_C_x[j][i], sizeof(double), 1, fp); 00799 fread(&tmp, sizeof(char),1,fp); 00800 } 00801 } 00802 00803 // Load _C_y 00804 for(int i = 0; i < _Sample_Num - 1; i++) { 00805 for(int j = 0; j < 4; j++) { 00806 fread(&_C_y[j][i], sizeof(double), 1, fp); 00807 fread(&tmp, sizeof(char),1,fp); 00808 } 00809 } 00810 fclose(fp); 00811 free(filepath); 00812 } 00813 00814 void CubicSpline2d::printOutData() 00815 { 00816 FILE *fp; 00817 double x,y,t; 00818 double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100.0; 00819 00820 fp = fopen("/local/log.txt", "w"); // open file in writing mode 00821 00822 fprintf(fp, "x, y, (t)\n"); 00823 for(int ival = 0; ival < _Sample_Num - 1; ival++) { 00824 fprintf(fp, "ival: %d \n", ival); 00825 for(x = _Sample_Set[ival].x; x <= _Sample_Set[ival + 1].x; x += d) { 00826 y = getY(x); 00827 if(ival == 0) 00828 t = sqrt((x - _Sample_Set[ival].x)*(x - _Sample_Set[ival].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y)); 00829 else 00830 t = _Sample_Set[ival-1].t + sqrt((x - _Sample_Set[ival-1].x)*(x - _Sample_Set[ival-1].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y)); 00831 fprintf(fp, "%f,%f,(%f)\n", x, y, t); 00832 } 00833 fprintf(fp, "-----------------------------------------\n"); 00834 } 00835 00836 fprintf(fp, "\nSample:dst, vol\n"); 00837 for(int i = 0; i < _Sample_Num; i++) { 00838 fprintf(fp, "%f,%f,(%f)\n", _Sample_Set[i].x, _Sample_Set[i].y, _Sample_Set[i].t); 00839 } 00840 fclose(fp); 00841 00842 } 00843 void CubicSpline2d::_printOutData(const double *arg, const int num, const char* name) 00844 { 00845 FILE *fp; 00846 00847 fp = fopen("/local/varlog.txt", "a"); // open file in add mode 00848 fprintf(fp, "%10s\n", name); 00849 for(int i = 0; i < num; i++) { 00850 fprintf(fp, "%.2f, ", arg[i]); 00851 } 00852 fprintf(fp, "\n"); 00853 fclose(fp); 00854 } 00855 void CubicSpline2d::_printOutDataCouple(const double *arg1, const double *arg2, const int num, const char* name) 00856 { 00857 FILE *fp; 00858 00859 fp = fopen("/local/varlog.txt", "a"); // open file in add mode 00860 fprintf(fp, "%10s\n", name); 00861 for(int i = 0; i < num; i++) { 00862 fprintf(fp, "(%.2f, %.2f)\n", arg1[i], arg2[i]); 00863 } 00864 fprintf(fp, "\n"); 00865 fclose(fp); 00866 } 00867 void CubicSpline2d::_printOutData(const Vxyt *arg, int num, const char* name) 00868 { 00869 FILE *fp; 00870 00871 fp = fopen("/local/varlog.txt", "a"); // open file in add mode 00872 fprintf(fp, "%10s\n", name); 00873 for(int i = 0; i < num; i++) { 00874 fprintf(fp, "%f, ", arg[i].y); 00875 } 00876 fprintf(fp, "\n"); 00877 fclose(fp); 00878 }
Generated on Tue Jul 19 2022 03:11:56 by
1.7.2