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