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@7:e032ddec6ed5, 2016-05-24 (annotated)

Committer:
aktk
Date:
Tue May 24 17:37:27 2016 +0000
Revision:
7:e032ddec6ed5
Parent:
6:c4f36cee3ceb
Child:
8:e7d451bb4fd4
function to solve cubic polynomial formula 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 4:8db89b731133 23 //_u_param = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:8db89b731133 24 for(int i = 0; i < _4; i++) {
aktk 7:e032ddec6ed5 25 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 7:e032ddec6ed5 26 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 4:8db89b731133 27 }
aktk 3:75f50dbedf1b 28 //calibrateSensor();
aktk 3:75f50dbedf1b 29 }
aktk 3:75f50dbedf1b 30
aktk 4:8db89b731133 31 CubicSpline2d::CubicSpline2d(
aktk 3:75f50dbedf1b 32 unsigned int arg_num
aktk 3:75f50dbedf1b 33 )
aktk 3:75f50dbedf1b 34 :_Data_Input_Type(SYSTEM)
aktk 3:75f50dbedf1b 35 {
aktk 3:75f50dbedf1b 36 _Sample_Num = arg_num;
aktk 4:8db89b731133 37 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
aktk 4:8db89b731133 38 //_u_param = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:8db89b731133 39 for(int i = 0; i < 4; i++) {
aktk 7:e032ddec6ed5 40 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 7:e032ddec6ed5 41 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 4:8db89b731133 42 }
aktk 3:75f50dbedf1b 43 //calibrateSensor();
aktk 3:75f50dbedf1b 44 }
aktk 3:75f50dbedf1b 45
aktk 4:8db89b731133 46 CubicSpline2d::CubicSpline2d(
aktk 3:75f50dbedf1b 47 unsigned int arg_num,
aktk 3:75f50dbedf1b 48 UseType arg_useType
aktk 3:75f50dbedf1b 49 )
aktk 3:75f50dbedf1b 50 :_useType(arg_useType)
aktk 3:75f50dbedf1b 51 {
aktk 3:75f50dbedf1b 52 _Sample_Num = arg_num;
aktk 4:8db89b731133 53 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
aktk 4:8db89b731133 54 //_u_param = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:8db89b731133 55 for(int i = 0; i < 4; i++) {
aktk 7:e032ddec6ed5 56 _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 7:e032ddec6ed5 57 _C_y[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
aktk 4:8db89b731133 58 }
aktk 3:75f50dbedf1b 59 //calibrateSensor();
aktk 3:75f50dbedf1b 60 }
aktk 3:75f50dbedf1b 61
aktk 4:8db89b731133 62 CubicSpline2d::~CubicSpline2d()
aktk 3:75f50dbedf1b 63 {
aktk 3:75f50dbedf1b 64 free(_Sample_Set);
aktk 4:8db89b731133 65 //free(_u_param);
aktk 4:8db89b731133 66 for(int i = 0; i < 4; i++) {
aktk 4:8db89b731133 67 free(_C_x[i]);
aktk 4:8db89b731133 68 free(_C_y[i]);
aktk 4:8db89b731133 69 }
aktk 3:75f50dbedf1b 70 }
aktk 3:75f50dbedf1b 71
aktk 4:8db89b731133 72 void CubicSpline2d::_sampleData()
aktk 3:75f50dbedf1b 73 {
aktk 3:75f50dbedf1b 74 int tmp;
aktk 3:75f50dbedf1b 75 char sig;
aktk 4:8db89b731133 76 Vxyt tmp_set;
aktk 3:75f50dbedf1b 77
aktk 3:75f50dbedf1b 78 // For evry set,
aktk 3:75f50dbedf1b 79 // 1, get dst data via serai com,
aktk 3:75f50dbedf1b 80 // 2, get vol data,
aktk 3:75f50dbedf1b 81 // and then do same for next index set.
aktk 3:75f50dbedf1b 82 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:8db89b731133 83 if(_useType == AsDebug) {
aktk 4:8db89b731133 84 //
aktk 4:8db89b731133 85 // Recieve a Distance datus and store it into member
aktk 4:8db89b731133 86 //
aktk 4:8db89b731133 87 g_Serial_Signal.printf("X:");
aktk 4:8db89b731133 88 _Sample_Set[i].x = 0;
aktk 3:75f50dbedf1b 89 do {
aktk 3:75f50dbedf1b 90 sig = g_Serial_Signal.getc();
aktk 3:75f50dbedf1b 91 if('0' <= sig && sig <= '9') {
aktk 4:8db89b731133 92 _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
aktk 3:75f50dbedf1b 93 g_Serial_Signal.putc(char(sig));
aktk 3:75f50dbedf1b 94 } else if(sig == 0x08) {
aktk 4:8db89b731133 95 _Sample_Set[i].x = 0;
aktk 3:75f50dbedf1b 96 g_Serial_Signal.printf("[canseled!]");
aktk 3:75f50dbedf1b 97 g_Serial_Signal.putc('\n');
aktk 3:75f50dbedf1b 98 g_Serial_Signal.putc('>');
aktk 3:75f50dbedf1b 99 }
aktk 3:75f50dbedf1b 100 } while (!(sig == 0x0a || sig == 0x0d));
aktk 3:75f50dbedf1b 101 g_Serial_Signal.putc('\n');
aktk 4:8db89b731133 102 //
aktk 4:8db89b731133 103 // Recieve a Voltage datus and store it into member
aktk 4:8db89b731133 104 //
aktk 4:8db89b731133 105 // LOW PASS FILTERED
aktk 4:8db89b731133 106 // Get 10 data and store mean as a sample.
aktk 4:8db89b731133 107 // After get one original sample, system waits for 0.1 sec,
aktk 4:8db89b731133 108 // thus it takes 1 sec evry sampling.
aktk 4:8db89b731133 109 _Sample_Set[i].y = 0;
aktk 4:8db89b731133 110 for(int j = 0; j < 10; j++) {
aktk 4:8db89b731133 111 tmp_set.y = g_Sensor_Voltage.read();
aktk 4:8db89b731133 112 #ifdef DEBUG
aktk 4:8db89b731133 113 g_Serial_Signal.printf("%d,",tmp_set.y);
aktk 4:8db89b731133 114 #endif
aktk 4:8db89b731133 115 _Sample_Set[i].y += (tmp_set.y / 10);
aktk 4:8db89b731133 116 wait(0.1);
aktk 4:8db89b731133 117 }
aktk 4:8db89b731133 118 #ifdef DEBUG
aktk 4:8db89b731133 119 g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y);
aktk 4:8db89b731133 120 #endif
aktk 3:75f50dbedf1b 121 }
aktk 3:75f50dbedf1b 122
aktk 4:8db89b731133 123 // if the input data is over the bound, it is calibrated
aktk 4:8db89b731133 124 if (_Sample_Set[i].x < 0)
aktk 4:8db89b731133 125 _Sample_Set[i].x = 0;
aktk 3:75f50dbedf1b 126 }
aktk 3:75f50dbedf1b 127 //
aktk 4:8db89b731133 128 // Sort set data array in x-Ascending order
aktk 3:75f50dbedf1b 129 //
aktk 3:75f50dbedf1b 130 tmp = 0;
aktk 4:8db89b731133 131 for( int i = 0 ; i < _Sumple_Num; i++) {
aktk 4:8db89b731133 132 for(int j = _Sample_Num - 1; j < i+1 ; j++) {
aktk 4:8db89b731133 133 // use dst as index for dst range [2,20]
aktk 4:8db89b731133 134 if (_Sample_Set[i].x > _Sample_set[j].x) {
aktk 4:8db89b731133 135 tmp_set.x = _Sample_Set[i].x;
aktk 4:8db89b731133 136 tmp_set.y = _Sample_Set[i].y;
aktk 4:8db89b731133 137 _Sample_Set[i].x = _Sample_Set[j].x;
aktk 4:8db89b731133 138 _Sample_Set[i].y = _Sample_Set[j].y;
aktk 4:8db89b731133 139 _Sample_Set[j].x = tmp_set.x;
aktk 4:8db89b731133 140 _Sample_Set[j].y = tmp_set.y;
aktk 4:8db89b731133 141 }
aktk 4:8db89b731133 142 // if a same dst has been input, calcurate mean.
aktk 4:8db89b731133 143 else if (_Sample_Set[i].x == _Sample_set[j]) {
aktk 4:8db89b731133 144 tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2;
aktk 4:8db89b731133 145 _Sample_Set[i] = _Sample_Set[j] = tmp_set.y;
aktk 4:8db89b731133 146 tmp++;
aktk 4:8db89b731133 147 }
aktk 3:75f50dbedf1b 148 }
aktk 3:75f50dbedf1b 149 }
aktk 3:75f50dbedf1b 150 #ifdef DEBUG
aktk 4:8db89b731133 151 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 4:8db89b731133 152 g_Serial_Signal.printf("-) tmp: %d\n", tmp );
aktk 3:75f50dbedf1b 153 #endif
aktk 3:75f50dbedf1b 154 // substruct tmp from number of sample.
aktk 3:75f50dbedf1b 155 _Sample_Num -= tmp;
aktk 3:75f50dbedf1b 156 #ifdef DEBUG
aktk 4:8db89b731133 157 g_Serial_Signal.printf("-----------------\n");
aktk 4:8db89b731133 158 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 3:75f50dbedf1b 159 #endif
aktk 4:8db89b731133 160
aktk 4:8db89b731133 161 // generate t which is parameter related to x,y
aktk 4:8db89b731133 162 _Sample_Set[i].t = 0;
aktk 4:8db89b731133 163 for(int i = 1; i < _Sample_Num; i++)
aktk 4:8db89b731133 164 _Sample_Set[i].t =
aktk 7:e032ddec6ed5 165 _Sample_Set[i-1].t
aktk 4:8db89b731133 166 + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2)
aktk 4:8db89b731133 167 +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2));
aktk 3:75f50dbedf1b 168 }
aktk 3:75f50dbedf1b 169
aktk 7:e032ddec6ed5 170 #define VERSION_C
aktk 3:75f50dbedf1b 171 //
aktk 3:75f50dbedf1b 172 // Function to define _u_spline, specific constants of spline.
aktk 3:75f50dbedf1b 173 //
aktk 7:e032ddec6ed5 174 void CubicSpline2d::_makeModel(double* arg_t, double* arg_ft, double* arg_C[4], const unsigned int arg_num)
aktk 3:75f50dbedf1b 175 {
aktk 7:e032ddec6ed5 176 // arg_t : t; The variable of f(t)
aktk 7:e032ddec6ed5 177 // arg_ft: f(t); The cubic poliminal in Interval-j.
aktk 7:e032ddec6ed5 178 // arg_C[i]: Ci; The coefficient of t^i of f(t) that defines Spline Model Poliminal f(t).
aktk 7:e032ddec6ed5 179 // arg_num: j in [0,_Sample_Num-1]; The number of interval.
aktk 4:8db89b731133 180 // f(t)j = C3j*t^3 + C2j*t^2 + C1j*t + C0j
aktk 3:75f50dbedf1b 181 //
aktk 3:75f50dbedf1b 182 // N: max of index <=> (_Sample_Num - 1)
aktk 3:75f50dbedf1b 183 //
aktk 4:8db89b731133 184 // u[i] === d^2/dx^2(Spline f)[i]
aktk 3:75f50dbedf1b 185 // i:[0,N]
aktk 4:8db89b731133 186 // u[0] = u[N] = 0
aktk 7:e032ddec6ed5 187 #if defined (VERSION_C)
aktk 4:8db89b731133 188 double *u = (double*)malloc((arg_num ) * sizeof(double));
aktk 7:e032ddec6ed5 189 #elif defined (VERSION_C++)
aktk 7:e032ddec6ed5 190 double *u = new double[arg_num];
aktk 7:e032ddec6ed5 191 #elif defined (VERSION_C++11)
aktk 7:e032ddec6ed5 192 std::array<double,arg_num> u;
aktk 7:e032ddec6ed5 193 #endif
aktk 3:75f50dbedf1b 194 //
aktk 3:75f50dbedf1b 195 // h[i] = x[i+1] - x[i]
aktk 3:75f50dbedf1b 196 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
aktk 4:8db89b731133 197 double *h = (double*)malloc((arg_num - 1) * sizeof(double));
aktk 4:8db89b731133 198 //
aktk 3:75f50dbedf1b 199 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
aktk 3:75f50dbedf1b 200 // i:[0,N-2]
aktk 4:8db89b731133 201 double *v = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 4:8db89b731133 202 //
aktk 3:75f50dbedf1b 203 // temporary array whose num of elm equals v array
aktk 4:8db89b731133 204 double *w = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 3:75f50dbedf1b 205 //
aktk 4:8db89b731133 206 // [ 2(h[0]+h[1]) , h[1] , O ] [u[1] ] [v[0] ]
aktk 4:8db89b731133 207 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [u[2] ] [v[1] ]
aktk 4:8db89b731133 208 // [ ... ] * [... ] = [... ]
aktk 4:8db89b731133 209 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [u[j+1]] [v[j] ]
aktk 4:8db89b731133 210 // [ ... ] [ ... ] [ ... ]
aktk 4:8db89b731133 211 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [u[j+1]] [v[j] ]
aktk 4:8db89b731133 212 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [u[N-1]] [v[N-2]]
aktk 3:75f50dbedf1b 213 //
aktk 3:75f50dbedf1b 214 // For LU decomposition
aktk 4:8db89b731133 215 double *Upper = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 4:8db89b731133 216 double *Lower = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 3:75f50dbedf1b 217 #ifdef DEBUG
aktk 4:8db89b731133 218 _printOutData(arg_t, arg_ft, arg_num, "\nargment set\n");
aktk 3:75f50dbedf1b 219 #endif
aktk 4:8db89b731133 220 for(int i = 0; i < arg_num - 1; i++)
aktk 4:8db89b731133 221 h[i] = (double)(arg_t[i + 1] - arg_t[i]);
aktk 3:75f50dbedf1b 222
aktk 4:8db89b731133 223 for(int i = 0; i < arg_num - 2; i++)
aktk 3:75f50dbedf1b 224 v[i] = 6 * (
aktk 4:8db89b731133 225 ((double)(arg_ft[i + 2] - arg_ft[i + 1])) / h[i + 1]
aktk 3:75f50dbedf1b 226 -
aktk 4:8db89b731133 227 ((double)(arg_ft[i + 1] - arg_ft[i])) / h[i]
aktk 3:75f50dbedf1b 228 );
aktk 3:75f50dbedf1b 229
aktk 3:75f50dbedf1b 230 //
aktk 3:75f50dbedf1b 231 // LU decomposition
aktk 3:75f50dbedf1b 232 //
aktk 3:75f50dbedf1b 233 Upper[0] = 2 * (h[0] + h[1]);
aktk 3:75f50dbedf1b 234 Lower[0] = 0;
aktk 4:8db89b731133 235 for (int i = 1; i < arg_num - 2; i++) {
aktk 3:75f50dbedf1b 236 Lower[i] = h[i] / Upper[i - 1];
aktk 3:75f50dbedf1b 237 Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
aktk 3:75f50dbedf1b 238 }
aktk 3:75f50dbedf1b 239
aktk 3:75f50dbedf1b 240
aktk 3:75f50dbedf1b 241 //
aktk 3:75f50dbedf1b 242 // forward substitution
aktk 3:75f50dbedf1b 243 //
aktk 3:75f50dbedf1b 244 w[0] = v[0];
aktk 4:8db89b731133 245 for (int i = 1; i < arg_num - 2; i ++) {
aktk 3:75f50dbedf1b 246 w[i] = v[i] - Lower[i] * w[i-1];
aktk 3:75f50dbedf1b 247 }
aktk 3:75f50dbedf1b 248
aktk 3:75f50dbedf1b 249 //
aktk 3:75f50dbedf1b 250 // backward substitution
aktk 3:75f50dbedf1b 251 //
aktk 4:8db89b731133 252 u[arg_num - 2] = w[arg_num - 3] / Upper[arg_num - 3];
aktk 4:8db89b731133 253 for(int i = arg_num - 3; i > 0; i--) {
aktk 4:8db89b731133 254 u[i] = (w[(i - 1)] - h[(i)] * u[(i) + 1]) / Upper[(i - 1)];
aktk 3:75f50dbedf1b 255 }
aktk 3:75f50dbedf1b 256
aktk 3:75f50dbedf1b 257 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 4:8db89b731133 258 u[0] = u[arg_num - 1] = 0.0;
aktk 3:75f50dbedf1b 259
aktk 3:75f50dbedf1b 260 #ifdef DEBUG
aktk 4:8db89b731133 261 _printOutData(h, arg_num - 1, "h");
aktk 4:8db89b731133 262 _printOutData(v, arg_num - 2, "v");
aktk 4:8db89b731133 263 _printOutData(w, arg_num - 2, "w");
aktk 4:8db89b731133 264 _printOutData(Upper, arg_num - 2, "Upper");
aktk 4:8db89b731133 265 _printOutData(Lower, arg_num - 2, "Lower");
aktk 4:8db89b731133 266 _printOutData(u, arg_num , "u");
aktk 3:75f50dbedf1b 267 #endif
aktk 4:8db89b731133 268
aktk 4:8db89b731133 269 for(int itv = 0; itv < arg_num - 1; itv++) {
aktk 4:8db89b731133 270 C[3][itv] = (u[itv + 1] - u[itv]) / 6.0 / (arg_t[itv + 1] - arg_t[itv]);
aktk 4:8db89b731133 271 C[2][itv] = (u[itv]) / 2.0;
aktk 4:8db89b731133 272 C[1][itv] = (arg_ft[itv + 1] - arg_ft[itv]) / (arg_t[itv + 1] - arg_t[itv])
aktk 4:8db89b731133 273 -
aktk 4:8db89b731133 274 (arg_t[itv + 1] - arg_t[itv]) * (u[itv + 1] + 2.0 * u[itv]) / 6.0;
aktk 4:8db89b731133 275 C[0][itv] = (arg_ft[itv]);
aktk 4:8db89b731133 276 }
aktk 3:75f50dbedf1b 277 free(h);
aktk 4:8db89b731133 278 free(u);
aktk 3:75f50dbedf1b 279 free(v);
aktk 3:75f50dbedf1b 280 free(w);
aktk 3:75f50dbedf1b 281 free(Upper);
aktk 3:75f50dbedf1b 282 free(Lower);
aktk 3:75f50dbedf1b 283 }
aktk 3:75f50dbedf1b 284 //
aktk 7:e032ddec6ed5 285 // Fuction to return the value of Cubic polyminal f(t)
aktk 7:e032ddec6ed5 286 //
aktk 7:e032ddec6ed5 287 double CubicSpline2d::_cubic_f(const double arg_t, const double* arg_C[4])
aktk 3:75f50dbedf1b 288 {
aktk 7:e032ddec6ed5 289 double ft; //the value of Spline f(t).
aktk 7:e032ddec6ed5 290
aktk 7:e032ddec6ed5 291 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 292
aktk 7:e032ddec6ed5 293 return ft;
aktk 7:e032ddec6ed5 294 }
aktk 7:e032ddec6ed5 295 //
aktk 7:e032ddec6ed5 296 // Function to solve a cubic poliminal
aktk 7:e032ddec6ed5 297 // by using Gardano-Tartaglia formula
aktk 7:e032ddec6ed5 298 //
aktk 7:e032ddec6ed5 299 void _solve_cubic_f(
aktk 7:e032ddec6ed5 300 std::complex<double>* arg_t,
aktk 7:e032ddec6ed5 301 const double* arg_C[4],
aktk 7:e032ddec6ed5 302 const double arg_ft)
aktk 7:e032ddec6ed5 303 {
aktk 7:e032ddec6ed5 304 double c[3];
aktk 7:e032ddec6ed5 305 //f(t) = arg_ft/arg_C[3]
aktk 7:e032ddec6ed5 306 // = t^3 + c[2]*t^2 + c[1]*t + c[0].
aktk 7:e032ddec6ed5 307 for(int i = 0; i < 3; i++) {
aktk 7:e032ddec6ed5 308 c[i] = arg_C[i] / arg_C[3];
aktk 3:75f50dbedf1b 309 }
aktk 7:e032ddec6ed5 310 //modify the formula
aktk 7:e032ddec6ed5 311 //t^3 + c[2]*t^2 + c[1]*t + (c[0] - ft) = 0.
aktk 7:e032ddec6ed5 312 c[0] -= arg_ft / argC[3];
aktk 7:e032ddec6ed5 313
aktk 7:e032ddec6ed5 314 //The values defined from coefficients of the formula
aktk 7:e032ddec6ed5 315 //that identify solutions
aktk 7:e032ddec6ed5 316 double p,q,d;
aktk 7:e032ddec6ed5 317 p = ( -pow(c[2], 2) + 3 * c[1]) / 9;
aktk 7:e032ddec6ed5 318 q = (2 * pow(c[2], 3) - 9 * c[2] * c[1] + 27 * c[0]) / 54;
aktk 7:e032ddec6ed5 319 d = - c[2] / 3;
aktk 7:e032ddec6ed5 320
aktk 7:e032ddec6ed5 321 //Discriminant section
aktk 7:e032ddec6ed5 322 double D;
aktk 7:e032ddec6ed5 323 D = pow(p, 3) + pow(q, 2);
aktk 7:e032ddec6ed5 324
aktk 7:e032ddec6ed5 325 //The values defined from p and q
aktk 7:e032ddec6ed5 326 //that idetify solutions
aktk 7:e032ddec6ed5 327 std::complex<double> u,v;
aktk 3:75f50dbedf1b 328
aktk 7:e032ddec6ed5 329 //Real root only
aktk 7:e032ddec6ed5 330 if(D <= 0) {
aktk 7:e032ddec6ed5 331 u.real(-q);
aktk 7:e032ddec6ed5 332 u.imag(+sqrt(-D));
aktk 7:e032ddec6ed5 333 v.real(-q);
aktk 7:e032ddec6ed5 334 v.real(-sqrt(-D));
aktk 7:e032ddec6ed5 335 }
aktk 7:e032ddec6ed5 336 //One real root and two complex root
aktk 7:e032ddec6ed5 337 else {
aktk 7:e032ddec6ed5 338 u.real(-q+sqrt(D));
aktk 7:e032ddec6ed5 339 u.imag(0.0);
aktk 7:e032ddec6ed5 340 v.real(-q-sqrt(D));
aktk 7:e032ddec6ed5 341 v.real(0.0);
aktk 7:e032ddec6ed5 342 }
aktk 7:e032ddec6ed5 343 u = pow(u, 1/3);
aktk 7:e032ddec6ed5 344 v = pow(v, 1/3);
aktk 3:75f50dbedf1b 345
aktk 7:e032ddec6ed5 346 //Cubic root of 1
aktk 7:e032ddec6ed5 347 std::complex<double> omega[3]= {
aktk 7:e032ddec6ed5 348 std::complex<double>( 1.0, 0.0),
aktk 7:e032ddec6ed5 349 std::complex<double>(-1/2, sqrt(3)/2),
aktk 7:e032ddec6ed5 350 std::complex<double>(-1/2,-sqrt(3)/2)
aktk 7:e032ddec6ed5 351 };
aktk 7:e032ddec6ed5 352
aktk 7:e032ddec6ed5 353 //Solution of the formula
aktk 7:e032ddec6ed5 354 arg_t[0] = omega[0] * u + omega[0] * v + d;
aktk 7:e032ddec6ed5 355 arg_t[1] = omega[1] * u + omega[2] * v + d;
aktk 7:e032ddec6ed5 356 arg_t[2] = omega[2] * u + omega[1] * v + d;
aktk 3:75f50dbedf1b 357 }
aktk 3:75f50dbedf1b 358
aktk 4:8db89b731133 359 void CubicSpline2d::calibrateSensor()
aktk 3:75f50dbedf1b 360 {
aktk 3:75f50dbedf1b 361 _sampleData();
aktk 3:75f50dbedf1b 362 _makeSpline();
aktk 3:75f50dbedf1b 363
aktk 3:75f50dbedf1b 364 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 365 _Set[i].dst = i;
aktk 3:75f50dbedf1b 366 _Set[i].vol = _getSplineYof((double)(_Set[i].dst));
aktk 3:75f50dbedf1b 367 _Threshold[i] = _getSplineYof((double)(_Set[i].dst) + 0.5);
aktk 3:75f50dbedf1b 368 #ifdef DEBUG2
aktk 3:75f50dbedf1b 369 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 3:75f50dbedf1b 370 #endif
aktk 3:75f50dbedf1b 371 }
aktk 3:75f50dbedf1b 372 }
aktk 3:75f50dbedf1b 373
aktk 4:8db89b731133 374 void CubicSpline2d::saveSetting()
aktk 3:75f50dbedf1b 375 {
aktk 3:75f50dbedf1b 376 FILE *fp;
aktk 3:75f50dbedf1b 377
aktk 3:75f50dbedf1b 378 fp = fopen("/local/savedata.log", "wb");
aktk 3:75f50dbedf1b 379
aktk 3:75f50dbedf1b 380 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 381 fwrite(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 382 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 383 fwrite(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 384 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 385 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 386 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 387 }
aktk 3:75f50dbedf1b 388 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 3:75f50dbedf1b 389 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 390 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 391 fwrite(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 392 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 393 fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 394 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 395 }
aktk 3:75f50dbedf1b 396 fclose(fp);
aktk 3:75f50dbedf1b 397
aktk 3:75f50dbedf1b 398 }
aktk 3:75f50dbedf1b 399
aktk 4:8db89b731133 400 void CubicSpline2d::printThresholds()
aktk 3:75f50dbedf1b 401 {
aktk 3:75f50dbedf1b 402 for(int i = 0; i < _ENUM; i++)
aktk 3:75f50dbedf1b 403 g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
aktk 3:75f50dbedf1b 404 }
aktk 4:8db89b731133 405 void CubicSpline2d::loadSetting()
aktk 3:75f50dbedf1b 406 {
aktk 3:75f50dbedf1b 407 FILE *fp;
aktk 3:75f50dbedf1b 408 char tmp;
aktk 3:75f50dbedf1b 409
aktk 3:75f50dbedf1b 410 //sprintf(filepath, "/local/%s", filename);
aktk 3:75f50dbedf1b 411 //fp = fopen(filepath, "rb");
aktk 3:75f50dbedf1b 412 fp = fopen("/local/savedata.log", "rb");
aktk 3:75f50dbedf1b 413 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 414
aktk 3:75f50dbedf1b 415 fread(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 416 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 417 #ifdef DEBUG2
aktk 3:75f50dbedf1b 418 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp);
aktk 3:75f50dbedf1b 419 #endif
aktk 3:75f50dbedf1b 420
aktk 3:75f50dbedf1b 421 fread(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 422 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 423 #ifdef DEBUG2
aktk 3:75f50dbedf1b 424 g_Serial_Signal.printf("%d%c", _Set[i].vol, tmp);
aktk 3:75f50dbedf1b 425 #endif
aktk 3:75f50dbedf1b 426
aktk 3:75f50dbedf1b 427 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 428 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 429 #ifdef DEBUG2
aktk 3:75f50dbedf1b 430 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 3:75f50dbedf1b 431 #endif
aktk 3:75f50dbedf1b 432 }
aktk 3:75f50dbedf1b 433
aktk 3:75f50dbedf1b 434 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 435 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 436
aktk 3:75f50dbedf1b 437 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 438 fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 439 fread(&tmp, sizeof(char),1,fp);
aktk 3:75f50dbedf1b 440 fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 441 fread(&tmp, sizeof(char),1,fp);
aktk 3:75f50dbedf1b 442 }
aktk 3:75f50dbedf1b 443 fclose(fp);
aktk 3:75f50dbedf1b 444 }
aktk 3:75f50dbedf1b 445
aktk 3:75f50dbedf1b 446
aktk 4:8db89b731133 447 void CubicSpline2d::saveSetting(
aktk 3:75f50dbedf1b 448 const char *filename
aktk 3:75f50dbedf1b 449 )
aktk 3:75f50dbedf1b 450 {
aktk 3:75f50dbedf1b 451 FILE *fp;
aktk 3:75f50dbedf1b 452 char *filepath;
aktk 3:75f50dbedf1b 453 int fnnum = 0;
aktk 3:75f50dbedf1b 454
aktk 3:75f50dbedf1b 455 while (filename[fnnum] != 0) fnnum++;
aktk 3:75f50dbedf1b 456 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 3:75f50dbedf1b 457
aktk 3:75f50dbedf1b 458 sprintf(filepath, "/local/%s", filename);
aktk 3:75f50dbedf1b 459 fp = fopen(filepath, "wb");
aktk 3:75f50dbedf1b 460
aktk 3:75f50dbedf1b 461 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 462 fwrite(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 463 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 464 fwrite(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 465 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 466 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 467 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 468 }
aktk 3:75f50dbedf1b 469 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 3:75f50dbedf1b 470 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 471 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 472 fwrite(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 473 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 474 fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 475 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 476 }
aktk 3:75f50dbedf1b 477 fclose(fp);
aktk 3:75f50dbedf1b 478 free(filepath);
aktk 3:75f50dbedf1b 479 }
aktk 3:75f50dbedf1b 480
aktk 4:8db89b731133 481 void CubicSpline2d::loadSetting(
aktk 3:75f50dbedf1b 482 const char *filename
aktk 3:75f50dbedf1b 483 )
aktk 3:75f50dbedf1b 484 {
aktk 3:75f50dbedf1b 485 FILE *fp;
aktk 3:75f50dbedf1b 486 char *filepath;
aktk 3:75f50dbedf1b 487 char tmp;
aktk 3:75f50dbedf1b 488 int fnnum = 0;
aktk 3:75f50dbedf1b 489
aktk 3:75f50dbedf1b 490 while (filename[fnnum] != 0) fnnum++;
aktk 3:75f50dbedf1b 491 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 3:75f50dbedf1b 492
aktk 3:75f50dbedf1b 493 sprintf(filepath, "/local/%s", filename);
aktk 3:75f50dbedf1b 494 fp = fopen(filepath, "rb");
aktk 3:75f50dbedf1b 495 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 496
aktk 3:75f50dbedf1b 497 fread(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 498 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 499 #ifdef DEBUG3
aktk 3:75f50dbedf1b 500 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp);
aktk 3:75f50dbedf1b 501 #endif
aktk 3:75f50dbedf1b 502
aktk 3:75f50dbedf1b 503 fread(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 504 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 505 #ifdef DEBUG3
aktk 3:75f50dbedf1b 506 g_Serial_Signal.printf("%d%c", _Set[i].vol, tmp);
aktk 3:75f50dbedf1b 507 #endif
aktk 3:75f50dbedf1b 508
aktk 3:75f50dbedf1b 509 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 510 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 511 #ifdef DEBUG3
aktk 3:75f50dbedf1b 512 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 3:75f50dbedf1b 513 #endif
aktk 3:75f50dbedf1b 514 }
aktk 3:75f50dbedf1b 515
aktk 3:75f50dbedf1b 516 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 517 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 518 #ifdef DEBUG3
aktk 3:75f50dbedf1b 519 g_Serial_Signal.printf("%d%c\n",_Sample_Num, tmp);
aktk 3:75f50dbedf1b 520 #endif
aktk 3:75f50dbedf1b 521
aktk 3:75f50dbedf1b 522 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 523 fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 524 fread(&tmp, sizeof(char),1,fp);
aktk 3:75f50dbedf1b 525 #ifdef DEBUG3
aktk 3:75f50dbedf1b 526 g_Serial_Signal.printf("%d%c", _Sample_Set[i].dst, tmp);
aktk 3:75f50dbedf1b 527 #endif
aktk 3:75f50dbedf1b 528
aktk 3:75f50dbedf1b 529 fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 530 fread(&tmp, sizeof(char),1,fp);
aktk 3:75f50dbedf1b 531 #ifdef DEBUG3
aktk 3:75f50dbedf1b 532 g_Serial_Signal.printf("%d%c", _Sample_Set[i].vol, tmp);
aktk 3:75f50dbedf1b 533 #endif
aktk 3:75f50dbedf1b 534 }
aktk 3:75f50dbedf1b 535 fclose(fp);
aktk 3:75f50dbedf1b 536 free(filepath);
aktk 3:75f50dbedf1b 537 }
aktk 3:75f50dbedf1b 538
aktk 4:8db89b731133 539 void CubicSpline2d::printOutData()
aktk 3:75f50dbedf1b 540 {
aktk 3:75f50dbedf1b 541 FILE *fp;
aktk 3:75f50dbedf1b 542
aktk 3:75f50dbedf1b 543 fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 3:75f50dbedf1b 544 fprintf(fp, "dst, vol,(threshold)\n");
aktk 3:75f50dbedf1b 545 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 546 fprintf(fp, "%d,%d,(%d)\n", _Set[i].dst, _Set[i].vol, _Threshold[i]);
aktk 3:75f50dbedf1b 547 }
aktk 3:75f50dbedf1b 548 fprintf(fp, "\nSample:dst, vol\n");
aktk 3:75f50dbedf1b 549 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 550 fprintf(fp, "%d,%d\n", _Sample_Set[i].dst, _Sample_Set[i].vol);
aktk 3:75f50dbedf1b 551 }
aktk 3:75f50dbedf1b 552 fclose(fp);
aktk 3:75f50dbedf1b 553
aktk 3:75f50dbedf1b 554 }
aktk 4:8db89b731133 555 void CubicSpline2d::_printOutData(unsigned short *arg, int num, char* name)
aktk 3:75f50dbedf1b 556 {
aktk 3:75f50dbedf1b 557 FILE *fp;
aktk 3:75f50dbedf1b 558 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 3:75f50dbedf1b 559 fprintf(fp, "%10s\n", name);
aktk 3:75f50dbedf1b 560 for(int i = 0; i < num; i++) {
aktk 3:75f50dbedf1b 561 fprintf(fp, "%d, ", arg[i]);
aktk 3:75f50dbedf1b 562 }
aktk 3:75f50dbedf1b 563 fprintf(fp, "\n");
aktk 3:75f50dbedf1b 564 fclose(fp);
aktk 3:75f50dbedf1b 565 }
aktk 4:8db89b731133 566 void CubicSpline2d::_printOutData(double *arg, int num, char* name)
aktk 3:75f50dbedf1b 567 {
aktk 3:75f50dbedf1b 568 FILE *fp;
aktk 3:75f50dbedf1b 569
aktk 3:75f50dbedf1b 570 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 3:75f50dbedf1b 571 fprintf(fp, "%10s\n", name);
aktk 3:75f50dbedf1b 572 for(int i = 0; i < num; i++) {
aktk 3:75f50dbedf1b 573 fprintf(fp, "%.2f, ", arg[i]);
aktk 3:75f50dbedf1b 574 }
aktk 3:75f50dbedf1b 575 fprintf(fp, "\n");
aktk 3:75f50dbedf1b 576 fclose(fp);
aktk 3:75f50dbedf1b 577 }
aktk 4:8db89b731133 578 void CubicSpline2d::_printOutDataCouple(double *arg1, double *arg2, int num, char* name)
aktk 4:8db89b731133 579 {
aktk 4:8db89b731133 580 FILE *fp;
aktk 4:8db89b731133 581
aktk 4:8db89b731133 582 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 4:8db89b731133 583 fprintf(fp, "%10s\n", name);
aktk 4:8db89b731133 584 for(int i = 0; i < num; i++) {
aktk 4:8db89b731133 585 fprintf(fp, "(%.2f, %.2f)\n", arg1[i], arg2[i]);
aktk 4:8db89b731133 586 }
aktk 4:8db89b731133 587 fprintf(fp, "\n");
aktk 4:8db89b731133 588 fclose(fp);
aktk 4:8db89b731133 589 }
aktk 4:8db89b731133 590 void CubicSpline2d::_printOutData(Vxyt *arg, int num, char* name)
aktk 3:75f50dbedf1b 591 {
aktk 3:75f50dbedf1b 592 FILE *fp;
aktk 3:75f50dbedf1b 593
aktk 3:75f50dbedf1b 594 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 3:75f50dbedf1b 595 fprintf(fp, "%10s\n", name);
aktk 3:75f50dbedf1b 596 for(int i = 0; i < num; i++) {
aktk 3:75f50dbedf1b 597 fprintf(fp, "%d, ", arg[i].vol);
aktk 3:75f50dbedf1b 598 }
aktk 3:75f50dbedf1b 599 fprintf(fp, "\n");
aktk 3:75f50dbedf1b 600 fclose(fp);
aktk 3:75f50dbedf1b 601 }