yal kaiyo
kanazawa demonstration
Fork of
130111_YNU_MPU_1
by 
yal kaiyo
main.cpp@7:05a718fdef74, 2013-01-10 (annotated)
- Committer:
- kenkenpapurika
- Date:
- Thu Jan 10 05:59:12 2013 +0000
- Revision:
- 7:05a718fdef74
- Parent:
- 6:3792b68b1fc9
- Child:
- 8:de86cf9ccb89
??????????????????????????
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| higedura | 0:9aa9be24636a | 1 | #include "mbed.h" |
| higedura | 0:9aa9be24636a | 2 | #include "SDFileSystem.h" |
| higedura | 0:9aa9be24636a | 3 | |
| higedura | 0:9aa9be24636a | 4 | #define pi 3.1416 |
| higedura | 0:9aa9be24636a | 5 | #define data 6 |
| higedura | 0:9aa9be24636a | 6 | |
| khoshino | 2:30f96c159d9c | 7 | #define zer 0.0 |
| khoshino | 2:30f96c159d9c | 8 | #define hal 0.5 |
| khoshino | 2:30f96c159d9c | 9 | #define one 1.0 |
| khoshino | 2:30f96c159d9c | 10 | #define dtr 0.01745 |
| khoshino | 2:30f96c159d9c | 11 | #define rtd 57.3 |
| khoshino | 2:30f96c159d9c | 12 | //parameter// |
| khoshino | 2:30f96c159d9c | 13 | #define dt1 0.2 // 1/N |
| khoshino | 2:30f96c159d9c | 14 | #define hdt 0.1 // 1/2N |
| khoshino | 2:30f96c159d9c | 15 | #define md 6 //kizami N |
| khoshino | 2:30f96c159d9c | 16 | #define mt 7 //md+1 |
| khoshino | 2:30f96c159d9c | 17 | #define mx 4 |
| khoshino | 2:30f96c159d9c | 18 | #define mv 8 |
| khoshino | 2:30f96c159d9c | 19 | #define mh 56 //mt*mv |
| khoshino | 2:30f96c159d9c | 20 | |
| higedura | 0:9aa9be24636a | 21 | Serial pc(USBTX, USBRX); |
| higedura | 0:9aa9be24636a | 22 | DigitalOut led1(LED1); |
| higedura | 0:9aa9be24636a | 23 | DigitalOut led2(LED2); |
| higedura | 0:9aa9be24636a | 24 | DigitalOut led3(LED3); |
| higedura | 0:9aa9be24636a | 25 | DigitalOut led4(LED4); |
| higedura | 0:9aa9be24636a | 26 | |
| higedura | 0:9aa9be24636a | 27 | SDFileSystem sd(p5, p6, p7, p8, "sd"); |
| higedura | 0:9aa9be24636a | 28 | Serial mavc(p9, p10); |
| higedura | 0:9aa9be24636a | 29 | DigitalIn stop_sd(p11); |
| higedura | 0:9aa9be24636a | 30 | |
| higedura | 0:9aa9be24636a | 31 | int main() { |
| higedura | 1:ca296cfa603b | 32 | |
| higedura | 1:ca296cfa603b | 33 | // ******************** TORATANI ******************** |
| higedura | 1:ca296cfa603b | 34 | //pc.baud(921600); |
| higedura | 0:9aa9be24636a | 35 | mavc.baud(115200); |
| higedura | 0:9aa9be24636a | 36 | |
| higedura | 5:192835ac6106 | 37 | int flag_lc = 0; |
| higedura | 5:192835ac6106 | 38 | int flag_cm = 0; |
| higedura | 5:192835ac6106 | 39 | int loop = 1200; // for 2 minuts |
| khoshino | 4:037fab837823 | 40 | |
| higedura | 0:9aa9be24636a | 41 | char buf_char; |
| higedura | 0:9aa9be24636a | 42 | unsigned int buf_hex[30] = {0}; |
| higedura | 0:9aa9be24636a | 43 | unsigned int buf_dec[10] = {0}; |
| higedura | 0:9aa9be24636a | 44 | |
| higedura | 0:9aa9be24636a | 45 | // rx |
| higedura | 0:9aa9be24636a | 46 | double acc[3] = {0}; |
| higedura | 0:9aa9be24636a | 47 | double gyro[3] = {0}; |
| higedura | 0:9aa9be24636a | 48 | double azi; |
| higedura | 0:9aa9be24636a | 49 | double alt; |
| higedura | 0:9aa9be24636a | 50 | double GPS[2] = {0}; |
| higedura | 0:9aa9be24636a | 51 | |
| higedura | 0:9aa9be24636a | 52 | // tx |
| higedura | 0:9aa9be24636a | 53 | double com[3] = {0}; |
| higedura | 0:9aa9be24636a | 54 | char H; |
| higedura | 0:9aa9be24636a | 55 | char D[data]; |
| higedura | 0:9aa9be24636a | 56 | char test[9]; |
| higedura | 0:9aa9be24636a | 57 | H = 0x02; // header of dateset2 |
| higedura | 0:9aa9be24636a | 58 | D[0] = 0xff; D[1] = 0xff; // p_com |
| higedura | 0:9aa9be24636a | 59 | D[2] = 0x7f; D[3] = 0xff; // q_com |
| higedura | 0:9aa9be24636a | 60 | D[4] = 0x00; D[5] = 0x00; // r_com |
| higedura | 1:ca296cfa603b | 61 | // ******************** TORATANI ******************** |
| higedura | 0:9aa9be24636a | 62 | |
| khoshino | 2:30f96c159d9c | 63 | // ******************** HOSHINO ********************* |
| khoshino | 2:30f96c159d9c | 64 | float time = 0; //time |
| khoshino | 4:037fab837823 | 65 | float xis = 0; |
| khoshino | 4:037fab837823 | 66 | float ets = 0; |
| khoshino | 2:30f96c159d9c | 67 | float psis = 0; |
| khoshino | 2:30f96c159d9c | 68 | float x0 = 0; |
| khoshino | 2:30f96c159d9c | 69 | float y0 = 0; |
| khoshino | 4:037fab837823 | 70 | float drf = 800; //syuutandaunrenji |
| khoshino | 4:037fab837823 | 71 | float ht0 = 200; |
| khoshino | 4:037fab837823 | 72 | float htf = 10; |
| khoshino | 2:30f96c159d9c | 73 | float gm0 = 20*pi/180; |
| khoshino | 2:30f96c159d9c | 74 | float gmf = 5*pi/180; |
| khoshino | 4:037fab837823 | 75 | float b0 = ht0; |
| khoshino | 2:30f96c159d9c | 76 | float b1 = tan(gm0); |
| khoshino | 4:037fab837823 | 77 | float b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf); |
| khoshino | 4:037fab837823 | 78 | float b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf); |
| khoshino | 4:037fab837823 | 79 | float htc = 0; |
| khoshino | 4:037fab837823 | 80 | float x = 0; //down range |
| khoshino | 2:30f96c159d9c | 81 | float ua = 10; //initial horizontal velosity |
| khoshino | 4:037fab837823 | 82 | float xis1 = 0; |
| khoshino | 4:037fab837823 | 83 | float ets1 = 0; |
| khoshino | 4:037fab837823 | 84 | float xir = 0; |
| khoshino | 4:037fab837823 | 85 | float etr = 0; |
| khoshino | 2:30f96c159d9c | 86 | float drc = 0; |
| khoshino | 2:30f96c159d9c | 87 | float dr = 0; |
| khoshino | 4:037fab837823 | 88 | float dt = 0.1; |
| khoshino | 4:037fab837823 | 89 | float psr = 0; |
| khoshino | 4:037fab837823 | 90 | float htr = 0; |
| khoshino | 4:037fab837823 | 91 | float avp = 0; |
| khoshino | 4:037fab837823 | 92 | float avq = 0; |
| khoshino | 4:037fab837823 | 93 | float avr = 0; |
| khoshino | 2:30f96c159d9c | 94 | float s = 0; |
| khoshino | 2:30f96c159d9c | 95 | |
| khoshino | 2:30f96c159d9c | 96 | //homotopy |
| khoshino | 2:30f96c159d9c | 97 | float rps0 = 90.0; |
| khoshino | 4:037fab837823 | 98 | float rxi0 = -600.0; //terminal xi |
| khoshino | 4:037fab837823 | 99 | float ret0 = -200.0; //terminal et |
| khoshino | 2:30f96c159d9c | 100 | float ps0 = rps0*dtr; |
| khoshino | 2:30f96c159d9c | 101 | float xi0 = rxi0/drf; |
| khoshino | 2:30f96c159d9c | 102 | float et0 = ret0/drf; |
| khoshino | 2:30f96c159d9c | 103 | float eps = 0.00000001; |
| khoshino | 2:30f96c159d9c | 104 | float ueps = 0.000001; |
| khoshino | 2:30f96c159d9c | 105 | float ier = 0.0; |
| khoshino | 2:30f96c159d9c | 106 | float sum = 0.0; |
| khoshino | 4:037fab837823 | 107 | float ph0[mh] = {0}; |
| khoshino | 4:037fab837823 | 108 | float phi[mh] = {0}; |
| khoshino | 4:037fab837823 | 109 | float fn[mh] = {0}; |
| khoshino | 4:037fab837823 | 110 | float wk[mh] = {0}; |
| khoshino | 2:30f96c159d9c | 111 | int iwk[mh] = {0}; |
| khoshino | 4:037fab837823 | 112 | float dph1[mh] = {0}; |
| khoshino | 4:037fab837823 | 113 | float dph2[mh] = {0}; |
| khoshino | 4:037fab837823 | 114 | float dph3[mh] = {0}; |
| khoshino | 4:037fab837823 | 115 | float dph4[mh] = {0}; |
| khoshino | 4:037fab837823 | 116 | float fph[mh][mh] = {0}; |
| khoshino | 4:037fab837823 | 117 | float fpi[mh][mh] = {0}; |
| khoshino | 4:037fab837823 | 118 | float x1[4][mt] = {0}; |
| khoshino | 4:037fab837823 | 119 | float rl[4][mt] = {0}; |
| khoshino | 2:30f96c159d9c | 120 | float qmax=0.0; |
| khoshino | 2:30f96c159d9c | 121 | float rdet =1.0; |
| khoshino | 2:30f96c159d9c | 122 | float idet =0.0; |
| khoshino | 2:30f96c159d9c | 123 | float t = 0.0; |
| khoshino | 4:037fab837823 | 124 | float phf[mh]={0}; |
| khoshino | 6:3792b68b1fc9 | 125 | float ap[mt]={0.0}; |
| khoshino | 6:3792b68b1fc9 | 126 | float ps[mt]={0.0}; |
| khoshino | 6:3792b68b1fc9 | 127 | float xi[mt]={0.0}; |
| khoshino | 6:3792b68b1fc9 | 128 | float et[mt]={0.0}; |
| khoshino | 6:3792b68b1fc9 | 129 | float xi1[mt]={0.0}; |
| khoshino | 6:3792b68b1fc9 | 130 | float et1[mt]={0.0}; |
| kenkenpapurika | 7:05a718fdef74 | 131 | float psc; |
| kenkenpapurika | 7:05a718fdef74 | 132 | float etc; |
| khoshino | 2:30f96c159d9c | 133 | //********************* HOSHINO ********************* |
| kenkenpapurika | 7:05a718fdef74 | 134 | |
| kenkenpapurika | 7:05a718fdef74 | 135 | //********************* OHTSU *********************** |
| kenkenpapurika | 7:05a718fdef74 | 136 | //teigi |
| kenkenpapurika | 7:05a718fdef74 | 137 | float grv=9.80665; |
| kenkenpapurika | 7:05a718fdef74 | 138 | float swg=0.04695; |
| kenkenpapurika | 7:05a718fdef74 | 139 | float wgt=0.76; |
| kenkenpapurika | 7:05a718fdef74 | 140 | float rho=1.22547328574415; |
| kenkenpapurika | 7:05a718fdef74 | 141 | |
| khoshino | 2:30f96c159d9c | 142 | |
| kenkenpapurika | 7:05a718fdef74 | 143 | float tt1=0.8; |
| kenkenpapurika | 7:05a718fdef74 | 144 | float omg1=1.0; |
| kenkenpapurika | 7:05a718fdef74 | 145 | float zet1=0.7; |
| kenkenpapurika | 7:05a718fdef74 | 146 | float tt2=0.37; |
| kenkenpapurika | 7:05a718fdef74 | 147 | float omg2=2.6; |
| kenkenpapurika | 7:05a718fdef74 | 148 | float zet2=0.7; |
| kenkenpapurika | 7:05a718fdef74 | 149 | |
| kenkenpapurika | 7:05a718fdef74 | 150 | float gmr=20; |
| kenkenpapurika | 7:05a718fdef74 | 151 | float vmr=0; |
| kenkenpapurika | 7:05a718fdef74 | 152 | float pse; |
| kenkenpapurika | 7:05a718fdef74 | 153 | float ete; |
| kenkenpapurika | 7:05a718fdef74 | 154 | float hte; |
| kenkenpapurika | 7:05a718fdef74 | 155 | |
| kenkenpapurika | 7:05a718fdef74 | 156 | float hte00; |
| kenkenpapurika | 7:05a718fdef74 | 157 | float dhdt; |
| kenkenpapurika | 7:05a718fdef74 | 158 | |
| kenkenpapurika | 7:05a718fdef74 | 159 | float v2; |
| kenkenpapurika | 7:05a718fdef74 | 160 | |
| kenkenpapurika | 7:05a718fdef74 | 161 | float gkp; |
| kenkenpapurika | 7:05a718fdef74 | 162 | float gkd; |
| kenkenpapurika | 7:05a718fdef74 | 163 | |
| kenkenpapurika | 7:05a718fdef74 | 164 | float avpc; |
| kenkenpapurika | 7:05a718fdef74 | 165 | float avqc; |
| higedura | 5:192835ac6106 | 166 | |
| khoshino | 4:037fab837823 | 167 | //********************* OHTSU *********************** |
| khoshino | 4:037fab837823 | 168 | |
| higedura | 0:9aa9be24636a | 169 | FILE *fp = fopen("/sd/sdtest.txt", "w"); |
| higedura | 0:9aa9be24636a | 170 | if(fp == NULL) { |
| higedura | 0:9aa9be24636a | 171 | error("Could not open file for write\n"); |
| higedura | 0:9aa9be24636a | 172 | } |
| higedura | 0:9aa9be24636a | 173 | |
| higedura | 5:192835ac6106 | 174 | // ******************** waiting @LC ******************** |
| khoshino | 6:3792b68b1fc9 | 175 | while(flag_lc==1){ |
| higedura | 5:192835ac6106 | 176 | buf_char = mavc.getc(); |
| higedura | 5:192835ac6106 | 177 | buf_hex[0] = (unsigned int)buf_char; |
| higedura | 5:192835ac6106 | 178 | for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; } |
| higedura | 5:192835ac6106 | 179 | if(buf_hex[29]==0x40 && buf_hex[28]==0x4C && buf_hex[27]==0x43 && buf_hex[26]==0x0D){ |
| higedura | 5:192835ac6106 | 180 | // ******************** waiting @LC ******************** |
| higedura | 5:192835ac6106 | 181 | |
| higedura | 5:192835ac6106 | 182 | // ******************** homotopy ******************** |
| higedura | 5:192835ac6106 | 183 | |
| khoshino | 6:3792b68b1fc9 | 184 | ////SYOKIKATEIKAI//// |
| khoshino | 6:3792b68b1fc9 | 185 | for(int it=0;it<mt;it++){ |
| khoshino | 6:3792b68b1fc9 | 186 | double tm=dt1*it; |
| khoshino | 6:3792b68b1fc9 | 187 | int i0=mv*it; |
| khoshino | 6:3792b68b1fc9 | 188 | ph0[i0]=zer; |
| khoshino | 6:3792b68b1fc9 | 189 | if(fabs(ps0)<=0.0000000001){ |
| khoshino | 6:3792b68b1fc9 | 190 | ph0[i0+1]=(ps0+1.0)*tm; |
| khoshino | 6:3792b68b1fc9 | 191 | } |
| khoshino | 6:3792b68b1fc9 | 192 | else{ |
| khoshino | 6:3792b68b1fc9 | 193 | ph0[i0+1] = ps0*tm; |
| khoshino | 6:3792b68b1fc9 | 194 | } |
| khoshino | 6:3792b68b1fc9 | 195 | ph0[i0+2]= xi0*tm; |
| khoshino | 6:3792b68b1fc9 | 196 | ph0[i0+3]= et0*tm; |
| khoshino | 6:3792b68b1fc9 | 197 | ph0[i0+4]=-hal; |
| khoshino | 6:3792b68b1fc9 | 198 | ph0[i0+5]=-hal; |
| khoshino | 6:3792b68b1fc9 | 199 | ph0[i0+6]=-hal; |
| khoshino | 6:3792b68b1fc9 | 200 | ph0[i0+7]=-hal; |
| khoshino | 6:3792b68b1fc9 | 201 | } |
| khoshino | 6:3792b68b1fc9 | 202 | for(int it=0;it<mh;it++){ |
| khoshino | 6:3792b68b1fc9 | 203 | phi[it]=ph0[it]; |
| khoshino | 6:3792b68b1fc9 | 204 | } |
| khoshino | 6:3792b68b1fc9 | 205 | for(int it=0;it<mh;it++){ |
| khoshino | 6:3792b68b1fc9 | 206 | phf[it]=ph0[it]; |
| khoshino | 6:3792b68b1fc9 | 207 | } |
| khoshino | 6:3792b68b1fc9 | 208 | |
| khoshino | 6:3792b68b1fc9 | 209 | //rungekutta |
| khoshino | 6:3792b68b1fc9 | 210 | for(int ir=0;ir<mt;ir++){ |
| khoshino | 6:3792b68b1fc9 | 211 | //k1 |
| khoshino | 6:3792b68b1fc9 | 212 | |
| khoshino | 6:3792b68b1fc9 | 213 | ////SABUNSHIKI////// |
| khoshino | 6:3792b68b1fc9 | 214 | for(int it=0;it<mt;it++){ |
| khoshino | 6:3792b68b1fc9 | 215 | int i0=mv*it; |
| khoshino | 6:3792b68b1fc9 | 216 | x1[0][it]=phi[i0+0]; |
| khoshino | 6:3792b68b1fc9 | 217 | x1[1][it]=phi[i0+1]; |
| khoshino | 6:3792b68b1fc9 | 218 | x1[2][it]=phi[i0+2]; |
| khoshino | 6:3792b68b1fc9 | 219 | x1[3][it]=phi[i0+3]; |
| khoshino | 6:3792b68b1fc9 | 220 | rl[0][it]=phi[i0+4]; |
| khoshino | 6:3792b68b1fc9 | 221 | rl[1][it]=phi[i0+5]; |
| khoshino | 6:3792b68b1fc9 | 222 | rl[2][it]=phi[i0+6]; |
| khoshino | 6:3792b68b1fc9 | 223 | rl[3][it]=phi[i0+7]; |
| khoshino | 6:3792b68b1fc9 | 224 | } |
| khoshino | 6:3792b68b1fc9 | 225 | |
| khoshino | 6:3792b68b1fc9 | 226 | fn[0]=x1[0][0]; |
| khoshino | 6:3792b68b1fc9 | 227 | fn[1]=x1[1][0]; |
| khoshino | 6:3792b68b1fc9 | 228 | fn[2]=x1[2][0]; |
| khoshino | 6:3792b68b1fc9 | 229 | fn[3]=x1[3][0]; |
| khoshino | 6:3792b68b1fc9 | 230 | for(int it=0;it<md;it++){ |
| khoshino | 6:3792b68b1fc9 | 231 | int i0=mx+mv*it; |
| khoshino | 6:3792b68b1fc9 | 232 | int it1=it+1; |
| khoshino | 6:3792b68b1fc9 | 233 | double bkp = hal*(x1[0][it]+x1[0][it1]); |
| khoshino | 6:3792b68b1fc9 | 234 | double bps = hal*(x1[1][it]+x1[1][it1]); |
| khoshino | 6:3792b68b1fc9 | 235 | double br1 = hal*(rl[0][it]+rl[0][it1]); |
| khoshino | 6:3792b68b1fc9 | 236 | double br2 = hal*(rl[1][it]+rl[1][it1]); |
| khoshino | 6:3792b68b1fc9 | 237 | double br3 = hal*(rl[2][it]+rl[2][it1]); |
| khoshino | 6:3792b68b1fc9 | 238 | double br4 = hal*(rl[3][it]+rl[3][it1]); |
| khoshino | 6:3792b68b1fc9 | 239 | fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1; |
| khoshino | 6:3792b68b1fc9 | 240 | fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp; |
| khoshino | 6:3792b68b1fc9 | 241 | fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps); |
| khoshino | 6:3792b68b1fc9 | 242 | fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps); |
| khoshino | 6:3792b68b1fc9 | 243 | fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2; |
| khoshino | 6:3792b68b1fc9 | 244 | fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps)); |
| khoshino | 6:3792b68b1fc9 | 245 | fn[i0+6] = rl[2][it1]-rl[2][it]; |
| khoshino | 6:3792b68b1fc9 | 246 | fn[i0+7] = rl[3][it1]-rl[3][it]; |
| khoshino | 6:3792b68b1fc9 | 247 | } |
| khoshino | 6:3792b68b1fc9 | 248 | fn[mh-4]=x1[0][mt-1]; |
| khoshino | 6:3792b68b1fc9 | 249 | fn[mh-3]=x1[1][mt-1]-ps0; |
| khoshino | 6:3792b68b1fc9 | 250 | fn[mh-2]=x1[2][mt-1]-xi0; |
| khoshino | 6:3792b68b1fc9 | 251 | fn[mh-1]=x1[3][mt-1]-et0; |
| khoshino | 6:3792b68b1fc9 | 252 | |
| khoshino | 6:3792b68b1fc9 | 253 | |
| khoshino | 6:3792b68b1fc9 | 254 | ////HENBIBUN//////////// |
| khoshino | 6:3792b68b1fc9 | 255 | //// I 0 ////// |
| khoshino | 6:3792b68b1fc9 | 256 | //// A B 0 //// |
| khoshino | 6:3792b68b1fc9 | 257 | //// 0 A B 0 // |
| khoshino | 6:3792b68b1fc9 | 258 | /////////////// |
| khoshino | 6:3792b68b1fc9 | 259 | ////////////I 0 |
| khoshino | 6:3792b68b1fc9 | 260 | //I |
| khoshino | 6:3792b68b1fc9 | 261 | fph[0][0]=one; |
| khoshino | 6:3792b68b1fc9 | 262 | fph[1][1]=one; |
| khoshino | 6:3792b68b1fc9 | 263 | fph[2][2]=one; |
| khoshino | 6:3792b68b1fc9 | 264 | fph[3][3]=one; |
| khoshino | 6:3792b68b1fc9 | 265 | //A |
| khoshino | 6:3792b68b1fc9 | 266 | for(int iu=0;iu<md;iu++){ |
| khoshino | 6:3792b68b1fc9 | 267 | int i0=mx+mv*iu; |
| khoshino | 6:3792b68b1fc9 | 268 | int j0=mv*iu; |
| khoshino | 6:3792b68b1fc9 | 269 | int j1=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 270 | double bps=hal*(phi[j0+1]+phi[j1+1]); |
| khoshino | 6:3792b68b1fc9 | 271 | double br3=hal*(phi[j0+6]+phi[j1+6]); |
| khoshino | 6:3792b68b1fc9 | 272 | double br4=hal*(phi[j0+7]+phi[j1+7]); |
| khoshino | 6:3792b68b1fc9 | 273 | double cps=cos(bps); |
| khoshino | 6:3792b68b1fc9 | 274 | double sps=sin(bps); |
| khoshino | 6:3792b68b1fc9 | 275 | for(int iv=0;iv<mv;iv++){ |
| khoshino | 6:3792b68b1fc9 | 276 | fph[i0+iv][j0+iv]=-one; |
| khoshino | 6:3792b68b1fc9 | 277 | } |
| khoshino | 6:3792b68b1fc9 | 278 | fph[i0+1][j0] = hdt; |
| khoshino | 6:3792b68b1fc9 | 279 | fph[i0+2][j0+1] =-hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 280 | fph[i0+3][j0+1] = hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 281 | fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps); |
| khoshino | 6:3792b68b1fc9 | 282 | fph[i0][j0+4] = hdt; |
| khoshino | 6:3792b68b1fc9 | 283 | fph[i0+4][j0+5] =-hdt; |
| khoshino | 6:3792b68b1fc9 | 284 | fph[i0+5][j0+6] = hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 285 | fph[i0+5][j0+7] =-hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 286 | } |
| khoshino | 6:3792b68b1fc9 | 287 | //B |
| khoshino | 6:3792b68b1fc9 | 288 | //j0=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 289 | for(int iu=0;iu<md;iu++){ |
| khoshino | 6:3792b68b1fc9 | 290 | int i0=mx+mv*iu; |
| khoshino | 6:3792b68b1fc9 | 291 | int j0=mv*(iu+1); |
| khoshino | 6:3792b68b1fc9 | 292 | int j1=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 293 | double bps=hal*(phi[j0+1]+phi[j1+1]); |
| khoshino | 6:3792b68b1fc9 | 294 | double br3=hal*(phi[j0+6]+phi[j1+6]); |
| khoshino | 6:3792b68b1fc9 | 295 | double br4=hal*(phi[j0+7]+phi[j1+7]); |
| khoshino | 6:3792b68b1fc9 | 296 | double cps=cos(bps); |
| khoshino | 6:3792b68b1fc9 | 297 | double sps=sin(bps); |
| khoshino | 6:3792b68b1fc9 | 298 | for(int iv=0;iv<mv;iv++){ |
| khoshino | 6:3792b68b1fc9 | 299 | fph[i0+iv][j0+iv]=one; |
| khoshino | 6:3792b68b1fc9 | 300 | } |
| khoshino | 6:3792b68b1fc9 | 301 | fph[i0+1][j0]=hdt; |
| khoshino | 6:3792b68b1fc9 | 302 | fph[i0+2][j0+1]=-hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 303 | fph[i0+3][j0+1]=hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 304 | fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps); |
| khoshino | 6:3792b68b1fc9 | 305 | fph[i0][j0+4]=hdt; |
| khoshino | 6:3792b68b1fc9 | 306 | fph[i0+4][j0+5]=-hdt; |
| khoshino | 6:3792b68b1fc9 | 307 | fph[i0+5][j0+6]=hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 308 | fph[i0+5][j0+7]=-hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 309 | } |
| khoshino | 6:3792b68b1fc9 | 310 | //I |
| khoshino | 6:3792b68b1fc9 | 311 | fph[mh-4][mh-8]=one; |
| khoshino | 6:3792b68b1fc9 | 312 | fph[mh-3][mh-7]=one; |
| khoshino | 6:3792b68b1fc9 | 313 | fph[mh-2][mh-6]=one; |
| khoshino | 6:3792b68b1fc9 | 314 | fph[mh-1][mh-5]=one; |
| khoshino | 6:3792b68b1fc9 | 315 | |
| khoshino | 6:3792b68b1fc9 | 316 | //GYAKUGYOURETSU |
| khoshino | 6:3792b68b1fc9 | 317 | for(int i=0;i<mh;i++){ |
| khoshino | 6:3792b68b1fc9 | 318 | for(int j=0;j<mh;j++){ |
| khoshino | 6:3792b68b1fc9 | 319 | fpi[i][j]=fph[i][j]; |
| khoshino | 6:3792b68b1fc9 | 320 | } |
| khoshino | 6:3792b68b1fc9 | 321 | } |
| khoshino | 6:3792b68b1fc9 | 322 | for(int j=0;j<mh;j++) { |
| khoshino | 6:3792b68b1fc9 | 323 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 324 | if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); } |
| khoshino | 6:3792b68b1fc9 | 325 | } |
| khoshino | 6:3792b68b1fc9 | 326 | } |
| khoshino | 6:3792b68b1fc9 | 327 | |
| khoshino | 6:3792b68b1fc9 | 328 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 329 | double q=wk[i]; |
| khoshino | 6:3792b68b1fc9 | 330 | // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");} |
| khoshino | 6:3792b68b1fc9 | 331 | wk[i]=1.0/q; |
| khoshino | 6:3792b68b1fc9 | 332 | if(q > qmax){qmax=q;} |
| khoshino | 6:3792b68b1fc9 | 333 | } |
| khoshino | 6:3792b68b1fc9 | 334 | |
| khoshino | 6:3792b68b1fc9 | 335 | if(eps < 0){eps=qmax*mh*ueps;} |
| khoshino | 6:3792b68b1fc9 | 336 | rdet =1.0; |
| khoshino | 6:3792b68b1fc9 | 337 | idet =0.0; |
| khoshino | 6:3792b68b1fc9 | 338 | for(int k=0;k<mh;k++) { |
| khoshino | 6:3792b68b1fc9 | 339 | double amax = 0.0; |
| khoshino | 6:3792b68b1fc9 | 340 | int kmax = k; |
| khoshino | 6:3792b68b1fc9 | 341 | for(int i=k;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 342 | if(fabs(fpi[i][k])*wk[i] <= amax){} |
| khoshino | 6:3792b68b1fc9 | 343 | else{ |
| khoshino | 6:3792b68b1fc9 | 344 | amax=fabs(fpi[i][k])*wk[i]; |
| khoshino | 6:3792b68b1fc9 | 345 | kmax=i; |
| khoshino | 6:3792b68b1fc9 | 346 | } |
| khoshino | 6:3792b68b1fc9 | 347 | } |
| khoshino | 6:3792b68b1fc9 | 348 | //if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");} |
| khoshino | 6:3792b68b1fc9 | 349 | rdet=rdet*fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 350 | if(k != kmax){rdet=-rdet;} |
| khoshino | 6:3792b68b1fc9 | 351 | double adet =fabs(rdet); |
| khoshino | 6:3792b68b1fc9 | 352 | while(adet > 1.0){ |
| khoshino | 6:3792b68b1fc9 | 353 | adet = adet*0.0625; |
| khoshino | 6:3792b68b1fc9 | 354 | idet = idet + 4; |
| khoshino | 6:3792b68b1fc9 | 355 | } |
| khoshino | 6:3792b68b1fc9 | 356 | if(adet >= 0.0625){} |
| khoshino | 6:3792b68b1fc9 | 357 | else{ |
| khoshino | 6:3792b68b1fc9 | 358 | adet=adet*16.0; |
| khoshino | 6:3792b68b1fc9 | 359 | idet=idet-4; |
| khoshino | 6:3792b68b1fc9 | 360 | } |
| khoshino | 6:3792b68b1fc9 | 361 | if(rdet < 0.0){adet=-adet;} |
| khoshino | 6:3792b68b1fc9 | 362 | rdet=adet; |
| khoshino | 6:3792b68b1fc9 | 363 | iwk[k]=kmax; |
| khoshino | 6:3792b68b1fc9 | 364 | amax=-1.0/fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 365 | t=fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 366 | fpi[kmax][k]=fpi[k][k]; |
| khoshino | 6:3792b68b1fc9 | 367 | fpi[k][k]=t; |
| khoshino | 6:3792b68b1fc9 | 368 | for(int j=0;j<mh;j++) { |
| khoshino | 6:3792b68b1fc9 | 369 | if(j == k){} |
| khoshino | 6:3792b68b1fc9 | 370 | else{ |
| khoshino | 6:3792b68b1fc9 | 371 | t=fpi[kmax][j]*amax; |
| khoshino | 6:3792b68b1fc9 | 372 | fpi[kmax][j]=fpi[k][j]; |
| khoshino | 6:3792b68b1fc9 | 373 | if(fabs(t) <= 0){} |
| khoshino | 6:3792b68b1fc9 | 374 | else{ |
| khoshino | 6:3792b68b1fc9 | 375 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 376 | fpi[i][j]=fpi[i][j]+t*fpi[i][k]; |
| khoshino | 6:3792b68b1fc9 | 377 | } |
| khoshino | 6:3792b68b1fc9 | 378 | } |
| khoshino | 6:3792b68b1fc9 | 379 | fpi[k][j]=-t; |
| khoshino | 6:3792b68b1fc9 | 380 | } |
| khoshino | 6:3792b68b1fc9 | 381 | } |
| khoshino | 6:3792b68b1fc9 | 382 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 383 | fpi[i][k]=fpi[i][k]*amax; |
| khoshino | 6:3792b68b1fc9 | 384 | } |
| khoshino | 6:3792b68b1fc9 | 385 | fpi[k][k]=-amax; |
| khoshino | 6:3792b68b1fc9 | 386 | } |
| khoshino | 6:3792b68b1fc9 | 387 | |
| khoshino | 6:3792b68b1fc9 | 388 | for(int l=0;l<mh;l++) { |
| khoshino | 6:3792b68b1fc9 | 389 | int k=mh-l-1; |
| khoshino | 6:3792b68b1fc9 | 390 | if(k == iwk[k]){} |
| khoshino | 6:3792b68b1fc9 | 391 | else{ |
| khoshino | 6:3792b68b1fc9 | 392 | int kmax=iwk[k]; |
| khoshino | 6:3792b68b1fc9 | 393 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 394 | t=fpi[i][k]; |
| khoshino | 6:3792b68b1fc9 | 395 | fpi[i][k]=fpi[i][kmax]; |
| khoshino | 6:3792b68b1fc9 | 396 | fpi[i][kmax]=t; |
| khoshino | 6:3792b68b1fc9 | 397 | } |
| khoshino | 6:3792b68b1fc9 | 398 | } |
| khoshino | 6:3792b68b1fc9 | 399 | } |
| khoshino | 6:3792b68b1fc9 | 400 | |
| khoshino | 6:3792b68b1fc9 | 401 | //////function |
| khoshino | 6:3792b68b1fc9 | 402 | for(int jh=0;jh<mh;jh++){ |
| khoshino | 6:3792b68b1fc9 | 403 | sum=zer; |
| khoshino | 6:3792b68b1fc9 | 404 | for(int ih=0;ih<mh;ih++){ |
| khoshino | 6:3792b68b1fc9 | 405 | sum=sum-fpi[jh][ih]*fn[ih]; |
| khoshino | 6:3792b68b1fc9 | 406 | } |
| khoshino | 6:3792b68b1fc9 | 407 | dph1[jh]=sum; |
| khoshino | 6:3792b68b1fc9 | 408 | } |
| khoshino | 6:3792b68b1fc9 | 409 | |
| khoshino | 6:3792b68b1fc9 | 410 | for(int it=0;it<mh;it++){ |
| khoshino | 6:3792b68b1fc9 | 411 | phi[it]= phf[it]+hal*dt1*dph1[it]; |
| khoshino | 6:3792b68b1fc9 | 412 | } |
| khoshino | 6:3792b68b1fc9 | 413 | |
| khoshino | 6:3792b68b1fc9 | 414 | |
| khoshino | 6:3792b68b1fc9 | 415 | //k2 |
| khoshino | 6:3792b68b1fc9 | 416 | ////SABUNSHIKI////// |
| khoshino | 6:3792b68b1fc9 | 417 | for(int it=0;it<mt;it++){ |
| khoshino | 6:3792b68b1fc9 | 418 | int i0=mv*it; |
| khoshino | 6:3792b68b1fc9 | 419 | x1[0][it]=phi[i0+0]; |
| khoshino | 6:3792b68b1fc9 | 420 | x1[1][it]=phi[i0+1]; |
| khoshino | 6:3792b68b1fc9 | 421 | x1[2][it]=phi[i0+2]; |
| khoshino | 6:3792b68b1fc9 | 422 | x1[3][it]=phi[i0+3]; |
| khoshino | 6:3792b68b1fc9 | 423 | rl[0][it]=phi[i0+4]; |
| khoshino | 6:3792b68b1fc9 | 424 | rl[1][it]=phi[i0+5]; |
| khoshino | 6:3792b68b1fc9 | 425 | rl[2][it]=phi[i0+6]; |
| khoshino | 6:3792b68b1fc9 | 426 | rl[3][it]=phi[i0+7]; |
| khoshino | 6:3792b68b1fc9 | 427 | } |
| khoshino | 6:3792b68b1fc9 | 428 | |
| khoshino | 6:3792b68b1fc9 | 429 | fn[0]=x1[0][0]; |
| khoshino | 6:3792b68b1fc9 | 430 | fn[1]=x1[1][0]; |
| khoshino | 6:3792b68b1fc9 | 431 | fn[2]=x1[2][0]; |
| khoshino | 6:3792b68b1fc9 | 432 | fn[3]=x1[3][0]; |
| khoshino | 6:3792b68b1fc9 | 433 | for(int it=0;it<md;it++){ |
| khoshino | 6:3792b68b1fc9 | 434 | int i0=mx+mv*it; |
| khoshino | 6:3792b68b1fc9 | 435 | int it1=it+1; |
| khoshino | 6:3792b68b1fc9 | 436 | double bkp = hal*(x1[0][it]+x1[0][it1]); |
| khoshino | 6:3792b68b1fc9 | 437 | double bps = hal*(x1[1][it]+x1[1][it1]); |
| khoshino | 6:3792b68b1fc9 | 438 | double br1 = hal*(rl[0][it]+rl[0][it1]); |
| khoshino | 6:3792b68b1fc9 | 439 | double br2 = hal*(rl[1][it]+rl[1][it1]); |
| khoshino | 6:3792b68b1fc9 | 440 | double br3 = hal*(rl[2][it]+rl[2][it1]); |
| khoshino | 6:3792b68b1fc9 | 441 | double br4 = hal*(rl[3][it]+rl[3][it1]); |
| khoshino | 6:3792b68b1fc9 | 442 | fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1; |
| khoshino | 6:3792b68b1fc9 | 443 | fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp; |
| khoshino | 6:3792b68b1fc9 | 444 | fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps); |
| khoshino | 6:3792b68b1fc9 | 445 | fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps); |
| khoshino | 6:3792b68b1fc9 | 446 | fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2; |
| khoshino | 6:3792b68b1fc9 | 447 | fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps)); |
| khoshino | 6:3792b68b1fc9 | 448 | fn[i0+6] = rl[2][it1]-rl[2][it]; |
| khoshino | 6:3792b68b1fc9 | 449 | fn[i0+7] = rl[3][it1]-rl[3][it]; |
| khoshino | 6:3792b68b1fc9 | 450 | } |
| khoshino | 6:3792b68b1fc9 | 451 | fn[mh-4]=x1[0][mt-1]; |
| khoshino | 6:3792b68b1fc9 | 452 | fn[mh-3]=x1[1][mt-1]-ps0; |
| khoshino | 6:3792b68b1fc9 | 453 | fn[mh-2]=x1[2][mt-1]-xi0; |
| khoshino | 6:3792b68b1fc9 | 454 | fn[mh-1]=x1[3][mt-1]-et0; |
| khoshino | 6:3792b68b1fc9 | 455 | |
| khoshino | 6:3792b68b1fc9 | 456 | |
| khoshino | 6:3792b68b1fc9 | 457 | ////HENBIBUN//////////// |
| khoshino | 6:3792b68b1fc9 | 458 | //// I 0 ////// |
| khoshino | 6:3792b68b1fc9 | 459 | //// A B 0 //// |
| khoshino | 6:3792b68b1fc9 | 460 | //// 0 A B 0 // |
| khoshino | 6:3792b68b1fc9 | 461 | /////////////// |
| khoshino | 6:3792b68b1fc9 | 462 | ////////////I 0 |
| khoshino | 6:3792b68b1fc9 | 463 | //I |
| khoshino | 6:3792b68b1fc9 | 464 | fph[0][0]=one; |
| khoshino | 6:3792b68b1fc9 | 465 | fph[1][1]=one; |
| khoshino | 6:3792b68b1fc9 | 466 | fph[2][2]=one; |
| khoshino | 6:3792b68b1fc9 | 467 | fph[3][3]=one; |
| khoshino | 6:3792b68b1fc9 | 468 | //A |
| khoshino | 6:3792b68b1fc9 | 469 | for(int iu=0;iu<md;iu++){ |
| khoshino | 6:3792b68b1fc9 | 470 | int i0=mx+mv*iu; |
| khoshino | 6:3792b68b1fc9 | 471 | int j0=mv*iu; |
| khoshino | 6:3792b68b1fc9 | 472 | int j1=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 473 | double bps=hal*(phi[j0+1]+phi[j1+1]); |
| khoshino | 6:3792b68b1fc9 | 474 | double br3=hal*(phi[j0+6]+phi[j1+6]); |
| khoshino | 6:3792b68b1fc9 | 475 | double br4=hal*(phi[j0+7]+phi[j1+7]); |
| khoshino | 6:3792b68b1fc9 | 476 | double cps=cos(bps); |
| khoshino | 6:3792b68b1fc9 | 477 | double sps=sin(bps); |
| khoshino | 6:3792b68b1fc9 | 478 | for(int iv=0;iv<mv;iv++){ |
| khoshino | 6:3792b68b1fc9 | 479 | fph[i0+iv][j0+iv]=-one; |
| khoshino | 6:3792b68b1fc9 | 480 | } |
| khoshino | 6:3792b68b1fc9 | 481 | fph[i0+1][j0] = hdt; |
| khoshino | 6:3792b68b1fc9 | 482 | fph[i0+2][j0+1] =-hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 483 | fph[i0+3][j0+1] = hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 484 | fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps); |
| khoshino | 6:3792b68b1fc9 | 485 | fph[i0][j0+4] = hdt; |
| khoshino | 6:3792b68b1fc9 | 486 | fph[i0+4][j0+5] =-hdt; |
| khoshino | 6:3792b68b1fc9 | 487 | fph[i0+5][j0+6] = hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 488 | fph[i0+5][j0+7] =-hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 489 | } |
| khoshino | 6:3792b68b1fc9 | 490 | //B |
| khoshino | 6:3792b68b1fc9 | 491 | //j0=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 492 | for(int iu=0;iu<md;iu++){ |
| khoshino | 6:3792b68b1fc9 | 493 | int i0=mx+mv*iu; |
| khoshino | 6:3792b68b1fc9 | 494 | int j0=mv*(iu+1); |
| khoshino | 6:3792b68b1fc9 | 495 | int j1=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 496 | double bps=hal*(phi[j0+1]+phi[j1+1]); |
| khoshino | 6:3792b68b1fc9 | 497 | double br3=hal*(phi[j0+6]+phi[j1+6]); |
| khoshino | 6:3792b68b1fc9 | 498 | double br4=hal*(phi[j0+7]+phi[j1+7]); |
| khoshino | 6:3792b68b1fc9 | 499 | double cps=cos(bps); |
| khoshino | 6:3792b68b1fc9 | 500 | double sps=sin(bps); |
| khoshino | 6:3792b68b1fc9 | 501 | for(int iv=0;iv<mv;iv++){ |
| khoshino | 6:3792b68b1fc9 | 502 | fph[i0+iv][j0+iv]=one; |
| khoshino | 6:3792b68b1fc9 | 503 | } |
| khoshino | 6:3792b68b1fc9 | 504 | fph[i0+1][j0]=hdt; |
| khoshino | 6:3792b68b1fc9 | 505 | fph[i0+2][j0+1]=-hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 506 | fph[i0+3][j0+1]=hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 507 | fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps); |
| khoshino | 6:3792b68b1fc9 | 508 | fph[i0][j0+4]=hdt; |
| khoshino | 6:3792b68b1fc9 | 509 | fph[i0+4][j0+5]=-hdt; |
| khoshino | 6:3792b68b1fc9 | 510 | fph[i0+5][j0+6]=hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 511 | fph[i0+5][j0+7]=-hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 512 | } |
| khoshino | 6:3792b68b1fc9 | 513 | //I |
| khoshino | 6:3792b68b1fc9 | 514 | fph[mh-4][mh-8]=one; |
| khoshino | 6:3792b68b1fc9 | 515 | fph[mh-3][mh-7]=one; |
| khoshino | 6:3792b68b1fc9 | 516 | fph[mh-2][mh-6]=one; |
| khoshino | 6:3792b68b1fc9 | 517 | fph[mh-1][mh-5]=one; |
| khoshino | 6:3792b68b1fc9 | 518 | |
| khoshino | 6:3792b68b1fc9 | 519 | //GYAKUGYOURETSU |
| khoshino | 6:3792b68b1fc9 | 520 | for(int i=0;i<mh;i++){ |
| khoshino | 6:3792b68b1fc9 | 521 | for(int j=0;j<mh;j++){ |
| khoshino | 6:3792b68b1fc9 | 522 | fpi[i][j]=fph[i][j]; |
| khoshino | 6:3792b68b1fc9 | 523 | } |
| khoshino | 6:3792b68b1fc9 | 524 | } |
| khoshino | 6:3792b68b1fc9 | 525 | for(int j=0;j<mh;j++) { |
| khoshino | 6:3792b68b1fc9 | 526 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 527 | if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); } |
| khoshino | 6:3792b68b1fc9 | 528 | } |
| khoshino | 6:3792b68b1fc9 | 529 | } |
| khoshino | 6:3792b68b1fc9 | 530 | |
| khoshino | 6:3792b68b1fc9 | 531 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 532 | double q=wk[i]; |
| khoshino | 6:3792b68b1fc9 | 533 | // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");} |
| khoshino | 6:3792b68b1fc9 | 534 | wk[i]=1.0/q; |
| khoshino | 6:3792b68b1fc9 | 535 | if(q > qmax){qmax=q;} |
| khoshino | 6:3792b68b1fc9 | 536 | } |
| khoshino | 6:3792b68b1fc9 | 537 | |
| khoshino | 6:3792b68b1fc9 | 538 | if(eps < 0){eps=qmax*mh*ueps;} |
| khoshino | 6:3792b68b1fc9 | 539 | rdet =1.0; |
| khoshino | 6:3792b68b1fc9 | 540 | idet =0.0; |
| khoshino | 6:3792b68b1fc9 | 541 | for(int k=0;k<mh;k++) { |
| khoshino | 6:3792b68b1fc9 | 542 | double amax = 0.0; |
| khoshino | 6:3792b68b1fc9 | 543 | int kmax = k; |
| khoshino | 6:3792b68b1fc9 | 544 | for(int i=k;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 545 | if(fabs(fpi[i][k])*wk[i] <= amax){} |
| khoshino | 6:3792b68b1fc9 | 546 | else{ |
| khoshino | 6:3792b68b1fc9 | 547 | amax=fabs(fpi[i][k])*wk[i]; |
| khoshino | 6:3792b68b1fc9 | 548 | kmax=i; |
| khoshino | 6:3792b68b1fc9 | 549 | } |
| khoshino | 6:3792b68b1fc9 | 550 | } |
| khoshino | 6:3792b68b1fc9 | 551 | // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");} |
| khoshino | 6:3792b68b1fc9 | 552 | rdet=rdet*fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 553 | if(k != kmax){rdet=-rdet;} |
| khoshino | 6:3792b68b1fc9 | 554 | double adet =fabs(rdet); |
| khoshino | 6:3792b68b1fc9 | 555 | while(adet > 1.0){ |
| khoshino | 6:3792b68b1fc9 | 556 | adet = adet*0.0625; |
| khoshino | 6:3792b68b1fc9 | 557 | idet = idet + 4; |
| khoshino | 6:3792b68b1fc9 | 558 | } |
| khoshino | 6:3792b68b1fc9 | 559 | if(adet >= 0.0625){} |
| khoshino | 6:3792b68b1fc9 | 560 | else{ |
| khoshino | 6:3792b68b1fc9 | 561 | adet=adet*16.0; |
| khoshino | 6:3792b68b1fc9 | 562 | idet=idet-4; |
| khoshino | 6:3792b68b1fc9 | 563 | } |
| khoshino | 6:3792b68b1fc9 | 564 | if(rdet < 0.0){adet=-adet;} |
| khoshino | 6:3792b68b1fc9 | 565 | rdet=adet; |
| khoshino | 6:3792b68b1fc9 | 566 | iwk[k]=kmax; |
| khoshino | 6:3792b68b1fc9 | 567 | amax=-1.0/fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 568 | t=fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 569 | fpi[kmax][k]=fpi[k][k]; |
| khoshino | 6:3792b68b1fc9 | 570 | fpi[k][k]=t; |
| khoshino | 6:3792b68b1fc9 | 571 | for(int j=0;j<mh;j++) { |
| khoshino | 6:3792b68b1fc9 | 572 | if(j == k){} |
| khoshino | 6:3792b68b1fc9 | 573 | else{ |
| khoshino | 6:3792b68b1fc9 | 574 | t=fpi[kmax][j]*amax; |
| khoshino | 6:3792b68b1fc9 | 575 | fpi[kmax][j]=fpi[k][j]; |
| khoshino | 6:3792b68b1fc9 | 576 | if(fabs(t) <= 0){} |
| khoshino | 6:3792b68b1fc9 | 577 | else{ |
| khoshino | 6:3792b68b1fc9 | 578 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 579 | fpi[i][j]=fpi[i][j]+t*fpi[i][k]; |
| khoshino | 6:3792b68b1fc9 | 580 | } |
| khoshino | 6:3792b68b1fc9 | 581 | } |
| khoshino | 6:3792b68b1fc9 | 582 | fpi[k][j]=-t; |
| khoshino | 6:3792b68b1fc9 | 583 | } |
| khoshino | 6:3792b68b1fc9 | 584 | } |
| khoshino | 6:3792b68b1fc9 | 585 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 586 | fpi[i][k]=fpi[i][k]*amax; |
| khoshino | 6:3792b68b1fc9 | 587 | } |
| khoshino | 6:3792b68b1fc9 | 588 | fpi[k][k]=-amax; |
| khoshino | 6:3792b68b1fc9 | 589 | } |
| khoshino | 6:3792b68b1fc9 | 590 | |
| khoshino | 6:3792b68b1fc9 | 591 | for(int l=0;l<mh;l++) { |
| khoshino | 6:3792b68b1fc9 | 592 | int k=mh-l-1; |
| khoshino | 6:3792b68b1fc9 | 593 | if(k == iwk[k]){} |
| khoshino | 6:3792b68b1fc9 | 594 | else{ |
| khoshino | 6:3792b68b1fc9 | 595 | int kmax=iwk[k]; |
| khoshino | 6:3792b68b1fc9 | 596 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 597 | t=fpi[i][k]; |
| khoshino | 6:3792b68b1fc9 | 598 | fpi[i][k]=fpi[i][kmax]; |
| khoshino | 6:3792b68b1fc9 | 599 | fpi[i][kmax]=t; |
| khoshino | 6:3792b68b1fc9 | 600 | } |
| khoshino | 6:3792b68b1fc9 | 601 | } |
| khoshino | 6:3792b68b1fc9 | 602 | } |
| khoshino | 6:3792b68b1fc9 | 603 | |
| khoshino | 6:3792b68b1fc9 | 604 | //////function |
| khoshino | 6:3792b68b1fc9 | 605 | for(int jh=0;jh<mh;jh++){ |
| khoshino | 6:3792b68b1fc9 | 606 | sum=zer; |
| khoshino | 6:3792b68b1fc9 | 607 | for(int ih=0;ih<mh;ih++){ |
| khoshino | 6:3792b68b1fc9 | 608 | sum=sum-fpi[jh][ih]*fn[ih]; |
| khoshino | 6:3792b68b1fc9 | 609 | } |
| khoshino | 6:3792b68b1fc9 | 610 | dph2[jh]=sum; |
| khoshino | 6:3792b68b1fc9 | 611 | } |
| khoshino | 6:3792b68b1fc9 | 612 | |
| khoshino | 6:3792b68b1fc9 | 613 | //k3 |
| khoshino | 6:3792b68b1fc9 | 614 | for(int it=0;it<mh;it++){ |
| khoshino | 6:3792b68b1fc9 | 615 | phi[it]= phf[it]+hal*dt1*dph2[it]; |
| khoshino | 6:3792b68b1fc9 | 616 | } |
| khoshino | 6:3792b68b1fc9 | 617 | ////SABUNSHIKI////// |
| khoshino | 6:3792b68b1fc9 | 618 | for(int it=0;it<mt;it++){ |
| khoshino | 6:3792b68b1fc9 | 619 | int i0=mv*it; |
| khoshino | 6:3792b68b1fc9 | 620 | x1[0][it]=phi[i0+0]; |
| khoshino | 6:3792b68b1fc9 | 621 | x1[1][it]=phi[i0+1]; |
| khoshino | 6:3792b68b1fc9 | 622 | x1[2][it]=phi[i0+2]; |
| khoshino | 6:3792b68b1fc9 | 623 | x1[3][it]=phi[i0+3]; |
| khoshino | 6:3792b68b1fc9 | 624 | rl[0][it]=phi[i0+4]; |
| khoshino | 6:3792b68b1fc9 | 625 | rl[1][it]=phi[i0+5]; |
| khoshino | 6:3792b68b1fc9 | 626 | rl[2][it]=phi[i0+6]; |
| khoshino | 6:3792b68b1fc9 | 627 | rl[3][it]=phi[i0+7]; |
| khoshino | 6:3792b68b1fc9 | 628 | } |
| khoshino | 6:3792b68b1fc9 | 629 | |
| khoshino | 6:3792b68b1fc9 | 630 | fn[0]=x1[0][0]; |
| khoshino | 6:3792b68b1fc9 | 631 | fn[1]=x1[1][0]; |
| khoshino | 6:3792b68b1fc9 | 632 | fn[2]=x1[2][0]; |
| khoshino | 6:3792b68b1fc9 | 633 | fn[3]=x1[3][0]; |
| khoshino | 6:3792b68b1fc9 | 634 | for(int it=0;it<md;it++){ |
| khoshino | 6:3792b68b1fc9 | 635 | int i0=mx+mv*it; |
| khoshino | 6:3792b68b1fc9 | 636 | int it1=it+1; |
| khoshino | 6:3792b68b1fc9 | 637 | double bkp = hal*(x1[0][it]+x1[0][it1]); |
| khoshino | 6:3792b68b1fc9 | 638 | double bps = hal*(x1[1][it]+x1[1][it1]); |
| khoshino | 6:3792b68b1fc9 | 639 | double br1 = hal*(rl[0][it]+rl[0][it1]); |
| khoshino | 6:3792b68b1fc9 | 640 | double br2 = hal*(rl[1][it]+rl[1][it1]); |
| khoshino | 6:3792b68b1fc9 | 641 | double br3 = hal*(rl[2][it]+rl[2][it1]); |
| khoshino | 6:3792b68b1fc9 | 642 | double br4 = hal*(rl[3][it]+rl[3][it1]); |
| khoshino | 6:3792b68b1fc9 | 643 | fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1; |
| khoshino | 6:3792b68b1fc9 | 644 | fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp; |
| khoshino | 6:3792b68b1fc9 | 645 | fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps); |
| khoshino | 6:3792b68b1fc9 | 646 | fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps); |
| khoshino | 6:3792b68b1fc9 | 647 | fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2; |
| khoshino | 6:3792b68b1fc9 | 648 | fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps)); |
| khoshino | 6:3792b68b1fc9 | 649 | fn[i0+6] = rl[2][it1]-rl[2][it]; |
| khoshino | 6:3792b68b1fc9 | 650 | fn[i0+7] = rl[3][it1]-rl[3][it]; |
| khoshino | 6:3792b68b1fc9 | 651 | } |
| khoshino | 6:3792b68b1fc9 | 652 | fn[mh-4]=x1[0][mt-1]; |
| khoshino | 6:3792b68b1fc9 | 653 | fn[mh-3]=x1[1][mt-1]-ps0; |
| khoshino | 6:3792b68b1fc9 | 654 | fn[mh-2]=x1[2][mt-1]-xi0; |
| khoshino | 6:3792b68b1fc9 | 655 | fn[mh-1]=x1[3][mt-1]-et0; |
| khoshino | 6:3792b68b1fc9 | 656 | |
| khoshino | 6:3792b68b1fc9 | 657 | |
| khoshino | 6:3792b68b1fc9 | 658 | ////HENBIBUN//////////// |
| khoshino | 6:3792b68b1fc9 | 659 | //// I 0 ////// |
| khoshino | 6:3792b68b1fc9 | 660 | //// A B 0 //// |
| khoshino | 6:3792b68b1fc9 | 661 | //// 0 A B 0 // |
| khoshino | 6:3792b68b1fc9 | 662 | /////////////// |
| khoshino | 6:3792b68b1fc9 | 663 | ////////////I 0 |
| khoshino | 6:3792b68b1fc9 | 664 | //I |
| khoshino | 6:3792b68b1fc9 | 665 | fph[0][0]=one; |
| khoshino | 6:3792b68b1fc9 | 666 | fph[1][1]=one; |
| khoshino | 6:3792b68b1fc9 | 667 | fph[2][2]=one; |
| khoshino | 6:3792b68b1fc9 | 668 | fph[3][3]=one; |
| khoshino | 6:3792b68b1fc9 | 669 | //A |
| khoshino | 6:3792b68b1fc9 | 670 | for(int iu=0;iu<md;iu++){ |
| khoshino | 6:3792b68b1fc9 | 671 | int i0=mx+mv*iu; |
| khoshino | 6:3792b68b1fc9 | 672 | int j0=mv*iu; |
| khoshino | 6:3792b68b1fc9 | 673 | int j1=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 674 | double bps=hal*(phi[j0+1]+phi[j1+1]); |
| khoshino | 6:3792b68b1fc9 | 675 | double br3=hal*(phi[j0+6]+phi[j1+6]); |
| khoshino | 6:3792b68b1fc9 | 676 | double br4=hal*(phi[j0+7]+phi[j1+7]); |
| khoshino | 6:3792b68b1fc9 | 677 | double cps=cos(bps); |
| khoshino | 6:3792b68b1fc9 | 678 | double sps=sin(bps); |
| khoshino | 6:3792b68b1fc9 | 679 | for(int iv=0;iv<mv;iv++){ |
| khoshino | 6:3792b68b1fc9 | 680 | fph[i0+iv][j0+iv]=-one; |
| khoshino | 6:3792b68b1fc9 | 681 | } |
| khoshino | 6:3792b68b1fc9 | 682 | fph[i0+1][j0] = hdt; |
| khoshino | 6:3792b68b1fc9 | 683 | fph[i0+2][j0+1] =-hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 684 | fph[i0+3][j0+1] = hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 685 | fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps); |
| khoshino | 6:3792b68b1fc9 | 686 | fph[i0][j0+4] = hdt; |
| khoshino | 6:3792b68b1fc9 | 687 | fph[i0+4][j0+5] =-hdt; |
| khoshino | 6:3792b68b1fc9 | 688 | fph[i0+5][j0+6] = hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 689 | fph[i0+5][j0+7] =-hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 690 | } |
| khoshino | 6:3792b68b1fc9 | 691 | //B |
| khoshino | 6:3792b68b1fc9 | 692 | //j0=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 693 | for(int iu=0;iu<md;iu++){ |
| khoshino | 6:3792b68b1fc9 | 694 | int i0=mx+mv*iu; |
| khoshino | 6:3792b68b1fc9 | 695 | int j0=mv*(iu+1); |
| khoshino | 6:3792b68b1fc9 | 696 | int j1=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 697 | double bps=hal*(phi[j0+1]+phi[j1+1]); |
| khoshino | 6:3792b68b1fc9 | 698 | double br3=hal*(phi[j0+6]+phi[j1+6]); |
| khoshino | 6:3792b68b1fc9 | 699 | double br4=hal*(phi[j0+7]+phi[j1+7]); |
| khoshino | 6:3792b68b1fc9 | 700 | double cps=cos(bps); |
| khoshino | 6:3792b68b1fc9 | 701 | double sps=sin(bps); |
| khoshino | 6:3792b68b1fc9 | 702 | for(int iv=0;iv<mv;iv++){ |
| khoshino | 6:3792b68b1fc9 | 703 | fph[i0+iv][j0+iv]=one; |
| khoshino | 6:3792b68b1fc9 | 704 | } |
| khoshino | 6:3792b68b1fc9 | 705 | fph[i0+1][j0]=hdt; |
| khoshino | 6:3792b68b1fc9 | 706 | fph[i0+2][j0+1]=-hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 707 | fph[i0+3][j0+1]=hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 708 | fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps); |
| khoshino | 6:3792b68b1fc9 | 709 | fph[i0][j0+4]=hdt; |
| khoshino | 6:3792b68b1fc9 | 710 | fph[i0+4][j0+5]=-hdt; |
| khoshino | 6:3792b68b1fc9 | 711 | fph[i0+5][j0+6]=hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 712 | fph[i0+5][j0+7]=-hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 713 | } |
| khoshino | 6:3792b68b1fc9 | 714 | //I |
| khoshino | 6:3792b68b1fc9 | 715 | fph[mh-4][mh-8]=one; |
| khoshino | 6:3792b68b1fc9 | 716 | fph[mh-3][mh-7]=one; |
| khoshino | 6:3792b68b1fc9 | 717 | fph[mh-2][mh-6]=one; |
| khoshino | 6:3792b68b1fc9 | 718 | fph[mh-1][mh-5]=one; |
| khoshino | 6:3792b68b1fc9 | 719 | |
| khoshino | 6:3792b68b1fc9 | 720 | //GYAKUGYOURETSU |
| khoshino | 6:3792b68b1fc9 | 721 | for(int i=0;i<mh;i++){ |
| khoshino | 6:3792b68b1fc9 | 722 | for(int j=0;j<mh;j++){ |
| khoshino | 6:3792b68b1fc9 | 723 | fpi[i][j]=fph[i][j]; |
| khoshino | 6:3792b68b1fc9 | 724 | } |
| khoshino | 6:3792b68b1fc9 | 725 | } |
| khoshino | 6:3792b68b1fc9 | 726 | for(int j=0;j<mh;j++) { |
| khoshino | 6:3792b68b1fc9 | 727 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 728 | if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); } |
| khoshino | 6:3792b68b1fc9 | 729 | } |
| khoshino | 6:3792b68b1fc9 | 730 | } |
| khoshino | 6:3792b68b1fc9 | 731 | |
| khoshino | 6:3792b68b1fc9 | 732 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 733 | double q=wk[i]; |
| khoshino | 6:3792b68b1fc9 | 734 | // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");} |
| khoshino | 6:3792b68b1fc9 | 735 | wk[i]=1.0/q; |
| khoshino | 6:3792b68b1fc9 | 736 | if(q > qmax){qmax=q;} |
| khoshino | 6:3792b68b1fc9 | 737 | } |
| khoshino | 6:3792b68b1fc9 | 738 | |
| khoshino | 6:3792b68b1fc9 | 739 | if(eps < 0){eps=qmax*mh*ueps;} |
| khoshino | 6:3792b68b1fc9 | 740 | rdet =1.0; |
| khoshino | 6:3792b68b1fc9 | 741 | idet =0.0; |
| khoshino | 6:3792b68b1fc9 | 742 | for(int k=0;k<mh;k++) { |
| khoshino | 6:3792b68b1fc9 | 743 | double amax = 0.0; |
| khoshino | 6:3792b68b1fc9 | 744 | int kmax = k; |
| khoshino | 6:3792b68b1fc9 | 745 | for(int i=k;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 746 | if(fabs(fpi[i][k])*wk[i] <= amax){} |
| khoshino | 6:3792b68b1fc9 | 747 | else{ |
| khoshino | 6:3792b68b1fc9 | 748 | amax=fabs(fpi[i][k])*wk[i]; |
| khoshino | 6:3792b68b1fc9 | 749 | kmax=i; |
| khoshino | 6:3792b68b1fc9 | 750 | } |
| khoshino | 6:3792b68b1fc9 | 751 | } |
| khoshino | 6:3792b68b1fc9 | 752 | // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");} |
| khoshino | 6:3792b68b1fc9 | 753 | rdet=rdet*fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 754 | if(k != kmax){rdet=-rdet;} |
| khoshino | 6:3792b68b1fc9 | 755 | double adet =fabs(rdet); |
| khoshino | 6:3792b68b1fc9 | 756 | while(adet > 1.0){ |
| khoshino | 6:3792b68b1fc9 | 757 | adet = adet*0.0625; |
| khoshino | 6:3792b68b1fc9 | 758 | idet = idet + 4; |
| khoshino | 6:3792b68b1fc9 | 759 | } |
| khoshino | 6:3792b68b1fc9 | 760 | if(adet >= 0.0625){} |
| khoshino | 6:3792b68b1fc9 | 761 | else{ |
| khoshino | 6:3792b68b1fc9 | 762 | adet=adet*16.0; |
| khoshino | 6:3792b68b1fc9 | 763 | idet=idet-4; |
| khoshino | 6:3792b68b1fc9 | 764 | } |
| khoshino | 6:3792b68b1fc9 | 765 | if(rdet < 0.0){adet=-adet;} |
| khoshino | 6:3792b68b1fc9 | 766 | rdet=adet; |
| khoshino | 6:3792b68b1fc9 | 767 | iwk[k]=kmax; |
| khoshino | 6:3792b68b1fc9 | 768 | amax=-1.0/fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 769 | t=fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 770 | fpi[kmax][k]=fpi[k][k]; |
| khoshino | 6:3792b68b1fc9 | 771 | fpi[k][k]=t; |
| khoshino | 6:3792b68b1fc9 | 772 | for(int j=0;j<mh;j++) { |
| khoshino | 6:3792b68b1fc9 | 773 | if(j == k){} |
| khoshino | 6:3792b68b1fc9 | 774 | else{ |
| khoshino | 6:3792b68b1fc9 | 775 | t=fpi[kmax][j]*amax; |
| khoshino | 6:3792b68b1fc9 | 776 | fpi[kmax][j]=fpi[k][j]; |
| khoshino | 6:3792b68b1fc9 | 777 | if(fabs(t) <= 0){} |
| khoshino | 6:3792b68b1fc9 | 778 | else{ |
| khoshino | 6:3792b68b1fc9 | 779 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 780 | fpi[i][j]=fpi[i][j]+t*fpi[i][k]; |
| khoshino | 6:3792b68b1fc9 | 781 | } |
| khoshino | 6:3792b68b1fc9 | 782 | } |
| khoshino | 6:3792b68b1fc9 | 783 | fpi[k][j]=-t; |
| khoshino | 6:3792b68b1fc9 | 784 | } |
| khoshino | 6:3792b68b1fc9 | 785 | } |
| khoshino | 6:3792b68b1fc9 | 786 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 787 | fpi[i][k]=fpi[i][k]*amax; |
| khoshino | 6:3792b68b1fc9 | 788 | } |
| khoshino | 6:3792b68b1fc9 | 789 | fpi[k][k]=-amax; |
| khoshino | 6:3792b68b1fc9 | 790 | } |
| khoshino | 6:3792b68b1fc9 | 791 | |
| khoshino | 6:3792b68b1fc9 | 792 | for(int l=0;l<mh;l++) { |
| khoshino | 6:3792b68b1fc9 | 793 | int k=mh-l-1; |
| khoshino | 6:3792b68b1fc9 | 794 | if(k == iwk[k]){} |
| khoshino | 6:3792b68b1fc9 | 795 | else{ |
| khoshino | 6:3792b68b1fc9 | 796 | int kmax=iwk[k]; |
| khoshino | 6:3792b68b1fc9 | 797 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 798 | t=fpi[i][k]; |
| khoshino | 6:3792b68b1fc9 | 799 | fpi[i][k]=fpi[i][kmax]; |
| khoshino | 6:3792b68b1fc9 | 800 | fpi[i][kmax]=t; |
| khoshino | 6:3792b68b1fc9 | 801 | } |
| khoshino | 6:3792b68b1fc9 | 802 | } |
| khoshino | 6:3792b68b1fc9 | 803 | } |
| khoshino | 6:3792b68b1fc9 | 804 | |
| khoshino | 6:3792b68b1fc9 | 805 | //////function |
| khoshino | 6:3792b68b1fc9 | 806 | for(int jh=0;jh<mh;jh++){ |
| khoshino | 6:3792b68b1fc9 | 807 | sum=zer; |
| khoshino | 6:3792b68b1fc9 | 808 | for(int ih=0;ih<mh;ih++){ |
| khoshino | 6:3792b68b1fc9 | 809 | sum=sum-fpi[jh][ih]*fn[ih]; |
| khoshino | 6:3792b68b1fc9 | 810 | } |
| khoshino | 6:3792b68b1fc9 | 811 | dph3[jh]=sum; |
| khoshino | 6:3792b68b1fc9 | 812 | } |
| khoshino | 6:3792b68b1fc9 | 813 | |
| khoshino | 6:3792b68b1fc9 | 814 | //k4 |
| khoshino | 6:3792b68b1fc9 | 815 | |
| khoshino | 6:3792b68b1fc9 | 816 | for(int it=0;it<mh;it++){ |
| khoshino | 6:3792b68b1fc9 | 817 | phi[it]= phf[it]+dt1*dph3[it]; |
| khoshino | 6:3792b68b1fc9 | 818 | } |
| khoshino | 6:3792b68b1fc9 | 819 | ////SABUNSHIKI////// |
| khoshino | 6:3792b68b1fc9 | 820 | for(int it=0;it<mt;it++){ |
| khoshino | 6:3792b68b1fc9 | 821 | int i0=mv*it; |
| khoshino | 6:3792b68b1fc9 | 822 | x1[0][it]=phi[i0+0]; |
| khoshino | 6:3792b68b1fc9 | 823 | x1[1][it]=phi[i0+1]; |
| khoshino | 6:3792b68b1fc9 | 824 | x1[2][it]=phi[i0+2]; |
| khoshino | 6:3792b68b1fc9 | 825 | x1[3][it]=phi[i0+3]; |
| khoshino | 6:3792b68b1fc9 | 826 | rl[0][it]=phi[i0+4]; |
| khoshino | 6:3792b68b1fc9 | 827 | rl[1][it]=phi[i0+5]; |
| khoshino | 6:3792b68b1fc9 | 828 | rl[2][it]=phi[i0+6]; |
| khoshino | 6:3792b68b1fc9 | 829 | rl[3][it]=phi[i0+7]; |
| khoshino | 6:3792b68b1fc9 | 830 | } |
| khoshino | 6:3792b68b1fc9 | 831 | |
| khoshino | 6:3792b68b1fc9 | 832 | fn[0]=x1[0][0]; |
| khoshino | 6:3792b68b1fc9 | 833 | fn[1]=x1[1][0]; |
| khoshino | 6:3792b68b1fc9 | 834 | fn[2]=x1[2][0]; |
| khoshino | 6:3792b68b1fc9 | 835 | fn[3]=x1[3][0]; |
| khoshino | 6:3792b68b1fc9 | 836 | for(int it=0;it<md;it++){ |
| khoshino | 6:3792b68b1fc9 | 837 | int i0=mx+mv*it; |
| khoshino | 6:3792b68b1fc9 | 838 | int it1=it+1; |
| khoshino | 6:3792b68b1fc9 | 839 | double bkp = hal*(x1[0][it]+x1[0][it1]); |
| khoshino | 6:3792b68b1fc9 | 840 | double bps = hal*(x1[1][it]+x1[1][it1]); |
| khoshino | 6:3792b68b1fc9 | 841 | double br1 = hal*(rl[0][it]+rl[0][it1]); |
| khoshino | 6:3792b68b1fc9 | 842 | double br2 = hal*(rl[1][it]+rl[1][it1]); |
| khoshino | 6:3792b68b1fc9 | 843 | double br3 = hal*(rl[2][it]+rl[2][it1]); |
| khoshino | 6:3792b68b1fc9 | 844 | double br4 = hal*(rl[3][it]+rl[3][it1]); |
| khoshino | 6:3792b68b1fc9 | 845 | fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1; |
| khoshino | 6:3792b68b1fc9 | 846 | fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp; |
| khoshino | 6:3792b68b1fc9 | 847 | fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps); |
| khoshino | 6:3792b68b1fc9 | 848 | fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps); |
| khoshino | 6:3792b68b1fc9 | 849 | fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2; |
| khoshino | 6:3792b68b1fc9 | 850 | fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps)); |
| khoshino | 6:3792b68b1fc9 | 851 | fn[i0+6] = rl[2][it1]-rl[2][it]; |
| khoshino | 6:3792b68b1fc9 | 852 | fn[i0+7] = rl[3][it1]-rl[3][it]; |
| khoshino | 6:3792b68b1fc9 | 853 | } |
| khoshino | 6:3792b68b1fc9 | 854 | fn[mh-4]=x1[0][mt-1]; |
| khoshino | 6:3792b68b1fc9 | 855 | fn[mh-3]=x1[1][mt-1]-ps0; |
| khoshino | 6:3792b68b1fc9 | 856 | fn[mh-2]=x1[2][mt-1]-xi0; |
| khoshino | 6:3792b68b1fc9 | 857 | fn[mh-1]=x1[3][mt-1]-et0; |
| khoshino | 6:3792b68b1fc9 | 858 | |
| khoshino | 6:3792b68b1fc9 | 859 | |
| khoshino | 6:3792b68b1fc9 | 860 | ////HENBIBUN//////////// |
| khoshino | 6:3792b68b1fc9 | 861 | //// I 0 ////// |
| khoshino | 6:3792b68b1fc9 | 862 | //// A B 0 //// |
| khoshino | 6:3792b68b1fc9 | 863 | //// 0 A B 0 // |
| khoshino | 6:3792b68b1fc9 | 864 | /////////////// |
| khoshino | 6:3792b68b1fc9 | 865 | ////////////I 0 |
| khoshino | 6:3792b68b1fc9 | 866 | //I |
| khoshino | 6:3792b68b1fc9 | 867 | fph[0][0]=one; |
| khoshino | 6:3792b68b1fc9 | 868 | fph[1][1]=one; |
| khoshino | 6:3792b68b1fc9 | 869 | fph[2][2]=one; |
| khoshino | 6:3792b68b1fc9 | 870 | fph[3][3]=one; |
| khoshino | 6:3792b68b1fc9 | 871 | //A |
| khoshino | 6:3792b68b1fc9 | 872 | for(int iu=0;iu<md;iu++){ |
| khoshino | 6:3792b68b1fc9 | 873 | int i0=mx+mv*iu; |
| khoshino | 6:3792b68b1fc9 | 874 | int j0=mv*iu; |
| khoshino | 6:3792b68b1fc9 | 875 | int j1=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 876 | double bps=hal*(phi[j0+1]+phi[j1+1]); |
| khoshino | 6:3792b68b1fc9 | 877 | double br3=hal*(phi[j0+6]+phi[j1+6]); |
| khoshino | 6:3792b68b1fc9 | 878 | double br4=hal*(phi[j0+7]+phi[j1+7]); |
| khoshino | 6:3792b68b1fc9 | 879 | double cps=cos(bps); |
| khoshino | 6:3792b68b1fc9 | 880 | double sps=sin(bps); |
| khoshino | 6:3792b68b1fc9 | 881 | for(int iv=0;iv<mv;iv++){ |
| khoshino | 6:3792b68b1fc9 | 882 | fph[i0+iv][j0+iv]=-one; |
| khoshino | 6:3792b68b1fc9 | 883 | } |
| khoshino | 6:3792b68b1fc9 | 884 | fph[i0+1][j0] = hdt; |
| khoshino | 6:3792b68b1fc9 | 885 | fph[i0+2][j0+1] =-hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 886 | fph[i0+3][j0+1] = hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 887 | fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps); |
| khoshino | 6:3792b68b1fc9 | 888 | fph[i0][j0+4] = hdt; |
| khoshino | 6:3792b68b1fc9 | 889 | fph[i0+4][j0+5] =-hdt; |
| khoshino | 6:3792b68b1fc9 | 890 | fph[i0+5][j0+6] = hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 891 | fph[i0+5][j0+7] =-hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 892 | } |
| khoshino | 6:3792b68b1fc9 | 893 | //B |
| khoshino | 6:3792b68b1fc9 | 894 | //j0=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 895 | for(int iu=0;iu<md;iu++){ |
| khoshino | 6:3792b68b1fc9 | 896 | int i0=mx+mv*iu; |
| khoshino | 6:3792b68b1fc9 | 897 | int j0=mv*(iu+1); |
| khoshino | 6:3792b68b1fc9 | 898 | int j1=j0+mv; |
| khoshino | 6:3792b68b1fc9 | 899 | double bps=hal*(phi[j0+1]+phi[j1+1]); |
| khoshino | 6:3792b68b1fc9 | 900 | double br3=hal*(phi[j0+6]+phi[j1+6]); |
| khoshino | 6:3792b68b1fc9 | 901 | double br4=hal*(phi[j0+7]+phi[j1+7]); |
| khoshino | 6:3792b68b1fc9 | 902 | double cps=cos(bps); |
| khoshino | 6:3792b68b1fc9 | 903 | double sps=sin(bps); |
| khoshino | 6:3792b68b1fc9 | 904 | for(int iv=0;iv<mv;iv++){ |
| khoshino | 6:3792b68b1fc9 | 905 | fph[i0+iv][j0+iv]=one; |
| khoshino | 6:3792b68b1fc9 | 906 | } |
| khoshino | 6:3792b68b1fc9 | 907 | fph[i0+1][j0]=hdt; |
| khoshino | 6:3792b68b1fc9 | 908 | fph[i0+2][j0+1]=-hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 909 | fph[i0+3][j0+1]=hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 910 | fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps); |
| khoshino | 6:3792b68b1fc9 | 911 | fph[i0][j0+4]=hdt; |
| khoshino | 6:3792b68b1fc9 | 912 | fph[i0+4][j0+5]=-hdt; |
| khoshino | 6:3792b68b1fc9 | 913 | fph[i0+5][j0+6]=hdt*sps; |
| khoshino | 6:3792b68b1fc9 | 914 | fph[i0+5][j0+7]=-hdt*cps; |
| khoshino | 6:3792b68b1fc9 | 915 | } |
| khoshino | 6:3792b68b1fc9 | 916 | //I |
| khoshino | 6:3792b68b1fc9 | 917 | fph[mh-4][mh-8]=one; |
| khoshino | 6:3792b68b1fc9 | 918 | fph[mh-3][mh-7]=one; |
| khoshino | 6:3792b68b1fc9 | 919 | fph[mh-2][mh-6]=one; |
| khoshino | 6:3792b68b1fc9 | 920 | fph[mh-1][mh-5]=one; |
| khoshino | 6:3792b68b1fc9 | 921 | |
| khoshino | 6:3792b68b1fc9 | 922 | //GYAKUGYOURETSU |
| khoshino | 6:3792b68b1fc9 | 923 | for(int i=0;i<mh;i++){ |
| khoshino | 6:3792b68b1fc9 | 924 | for(int j=0;j<mh;j++){ |
| khoshino | 6:3792b68b1fc9 | 925 | fpi[i][j]=fph[i][j]; |
| khoshino | 6:3792b68b1fc9 | 926 | } |
| khoshino | 6:3792b68b1fc9 | 927 | } |
| khoshino | 6:3792b68b1fc9 | 928 | for(int j=0;j<mh;j++) { |
| khoshino | 6:3792b68b1fc9 | 929 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 930 | if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); } |
| khoshino | 6:3792b68b1fc9 | 931 | } |
| khoshino | 6:3792b68b1fc9 | 932 | } |
| khoshino | 6:3792b68b1fc9 | 933 | |
| khoshino | 6:3792b68b1fc9 | 934 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 935 | double q=wk[i]; |
| khoshino | 6:3792b68b1fc9 | 936 | // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");} |
| khoshino | 6:3792b68b1fc9 | 937 | wk[i]=1.0/q; |
| khoshino | 6:3792b68b1fc9 | 938 | if(q > qmax){qmax=q;} |
| khoshino | 6:3792b68b1fc9 | 939 | } |
| khoshino | 6:3792b68b1fc9 | 940 | |
| khoshino | 6:3792b68b1fc9 | 941 | if(eps < 0){eps=qmax*mh*ueps;} |
| khoshino | 6:3792b68b1fc9 | 942 | rdet =1.0; |
| khoshino | 6:3792b68b1fc9 | 943 | idet =0.0; |
| khoshino | 6:3792b68b1fc9 | 944 | for(int k=0;k<mh;k++) { |
| khoshino | 6:3792b68b1fc9 | 945 | double amax = 0.0; |
| khoshino | 6:3792b68b1fc9 | 946 | int kmax = k; |
| khoshino | 6:3792b68b1fc9 | 947 | for(int i=k;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 948 | if(fabs(fpi[i][k])*wk[i] <= amax){} |
| khoshino | 6:3792b68b1fc9 | 949 | else{ |
| khoshino | 6:3792b68b1fc9 | 950 | amax=fabs(fpi[i][k])*wk[i]; |
| khoshino | 6:3792b68b1fc9 | 951 | kmax=i; |
| khoshino | 6:3792b68b1fc9 | 952 | } |
| khoshino | 6:3792b68b1fc9 | 953 | } |
| khoshino | 6:3792b68b1fc9 | 954 | // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");} |
| khoshino | 6:3792b68b1fc9 | 955 | rdet=rdet*fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 956 | if(k != kmax){rdet=-rdet;} |
| khoshino | 6:3792b68b1fc9 | 957 | double adet =fabs(rdet); |
| khoshino | 6:3792b68b1fc9 | 958 | while(adet > 1.0){ |
| khoshino | 6:3792b68b1fc9 | 959 | adet = adet*0.0625; |
| khoshino | 6:3792b68b1fc9 | 960 | idet = idet + 4; |
| khoshino | 6:3792b68b1fc9 | 961 | } |
| khoshino | 6:3792b68b1fc9 | 962 | if(adet >= 0.0625){} |
| khoshino | 6:3792b68b1fc9 | 963 | else{ |
| khoshino | 6:3792b68b1fc9 | 964 | adet=adet*16.0; |
| khoshino | 6:3792b68b1fc9 | 965 | idet=idet-4; |
| khoshino | 6:3792b68b1fc9 | 966 | } |
| khoshino | 6:3792b68b1fc9 | 967 | if(rdet < 0.0){adet=-adet;} |
| khoshino | 6:3792b68b1fc9 | 968 | rdet=adet; |
| khoshino | 6:3792b68b1fc9 | 969 | iwk[k]=kmax; |
| khoshino | 6:3792b68b1fc9 | 970 | amax=-1.0/fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 971 | t=fpi[kmax][k]; |
| khoshino | 6:3792b68b1fc9 | 972 | fpi[kmax][k]=fpi[k][k]; |
| khoshino | 6:3792b68b1fc9 | 973 | fpi[k][k]=t; |
| khoshino | 6:3792b68b1fc9 | 974 | for(int j=0;j<mh;j++) { |
| khoshino | 6:3792b68b1fc9 | 975 | if(j == k){} |
| khoshino | 6:3792b68b1fc9 | 976 | else{ |
| khoshino | 6:3792b68b1fc9 | 977 | t=fpi[kmax][j]*amax; |
| khoshino | 6:3792b68b1fc9 | 978 | fpi[kmax][j]=fpi[k][j]; |
| khoshino | 6:3792b68b1fc9 | 979 | if(fabs(t) <= 0){} |
| khoshino | 6:3792b68b1fc9 | 980 | else{ |
| khoshino | 6:3792b68b1fc9 | 981 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 982 | fpi[i][j]=fpi[i][j]+t*fpi[i][k]; |
| khoshino | 6:3792b68b1fc9 | 983 | } |
| khoshino | 6:3792b68b1fc9 | 984 | } |
| khoshino | 6:3792b68b1fc9 | 985 | fpi[k][j]=-t; |
| khoshino | 6:3792b68b1fc9 | 986 | } |
| khoshino | 6:3792b68b1fc9 | 987 | } |
| khoshino | 6:3792b68b1fc9 | 988 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 989 | fpi[i][k]=fpi[i][k]*amax; |
| khoshino | 6:3792b68b1fc9 | 990 | } |
| khoshino | 6:3792b68b1fc9 | 991 | fpi[k][k]=-amax; |
| khoshino | 6:3792b68b1fc9 | 992 | } |
| khoshino | 6:3792b68b1fc9 | 993 | |
| khoshino | 6:3792b68b1fc9 | 994 | for(int l=0;l<mh;l++) { |
| khoshino | 6:3792b68b1fc9 | 995 | int k=mh-l-1; |
| khoshino | 6:3792b68b1fc9 | 996 | if(k == iwk[k]){} |
| khoshino | 6:3792b68b1fc9 | 997 | else{ |
| khoshino | 6:3792b68b1fc9 | 998 | int kmax=iwk[k]; |
| khoshino | 6:3792b68b1fc9 | 999 | for(int i=0;i<mh;i++) { |
| khoshino | 6:3792b68b1fc9 | 1000 | t=fpi[i][k]; |
| khoshino | 6:3792b68b1fc9 | 1001 | fpi[i][k]=fpi[i][kmax]; |
| khoshino | 6:3792b68b1fc9 | 1002 | fpi[i][kmax]=t; |
| khoshino | 6:3792b68b1fc9 | 1003 | } |
| khoshino | 6:3792b68b1fc9 | 1004 | } |
| khoshino | 6:3792b68b1fc9 | 1005 | } |
| khoshino | 6:3792b68b1fc9 | 1006 | |
| khoshino | 6:3792b68b1fc9 | 1007 | //////function |
| khoshino | 6:3792b68b1fc9 | 1008 | for(int jh=0;jh<mh;jh++){ |
| khoshino | 6:3792b68b1fc9 | 1009 | sum=zer; |
| khoshino | 6:3792b68b1fc9 | 1010 | for(int ih=0;ih<mh;ih++){ |
| khoshino | 6:3792b68b1fc9 | 1011 | sum=sum-fpi[jh][ih]*fn[ih]; |
| khoshino | 6:3792b68b1fc9 | 1012 | } |
| khoshino | 6:3792b68b1fc9 | 1013 | dph4[jh]=sum; |
| khoshino | 6:3792b68b1fc9 | 1014 | } |
| khoshino | 6:3792b68b1fc9 | 1015 | |
| khoshino | 6:3792b68b1fc9 | 1016 | //saitekikai |
| khoshino | 6:3792b68b1fc9 | 1017 | |
| khoshino | 6:3792b68b1fc9 | 1018 | for(int it=0;it<mh;it++){ |
| khoshino | 6:3792b68b1fc9 | 1019 | phf[it]= phf[it]+dt1*(dph1[it]+2*dph2[it]+2*dph3[it]+dph4[it])/6.0; |
| khoshino | 6:3792b68b1fc9 | 1020 | } |
| khoshino | 6:3792b68b1fc9 | 1021 | |
| khoshino | 6:3792b68b1fc9 | 1022 | } |
| khoshino | 6:3792b68b1fc9 | 1023 | |
| khoshino | 6:3792b68b1fc9 | 1024 | for (int id=0; id< mt; ++id){ |
| khoshino | 6:3792b68b1fc9 | 1025 | int ii = mt-id-1; |
| khoshino | 6:3792b68b1fc9 | 1026 | int i0 = mv*ii; |
| khoshino | 6:3792b68b1fc9 | 1027 | ap[id]=phi[i0+0]; |
| khoshino | 6:3792b68b1fc9 | 1028 | ps[id]=phi[i0+1]; |
| khoshino | 6:3792b68b1fc9 | 1029 | xi1[id]=phi[i0+2]*drf; |
| khoshino | 6:3792b68b1fc9 | 1030 | et1[id]=phi[i0+3]*drf; |
| khoshino | 6:3792b68b1fc9 | 1031 | xi[id]=xi1[id]-xi1[0]; |
| khoshino | 6:3792b68b1fc9 | 1032 | et[id]=et1[id]-et1[0]; |
| khoshino | 6:3792b68b1fc9 | 1033 | } |
| khoshino | 6:3792b68b1fc9 | 1034 | led1 = 1; |
| khoshino | 6:3792b68b1fc9 | 1035 | wait(1.0); |
| khoshino | 6:3792b68b1fc9 | 1036 | |
| khoshino | 6:3792b68b1fc9 | 1037 | /////////////////// |
| higedura | 5:192835ac6106 | 1038 | flag_lc = 1; // end of homotopy |
| higedura | 5:192835ac6106 | 1039 | // ******************** homotopy ******************** |
| higedura | 5:192835ac6106 | 1040 | |
| higedura | 5:192835ac6106 | 1041 | } |
| higedura | 5:192835ac6106 | 1042 | } |
| higedura | 5:192835ac6106 | 1043 | |
| higedura | 5:192835ac6106 | 1044 | while(flag_cm<loop){ |
| higedura | 0:9aa9be24636a | 1045 | |
| higedura | 0:9aa9be24636a | 1046 | // ******************** sequence No.1 ******************** |
| higedura | 0:9aa9be24636a | 1047 | buf_char = mavc.getc(); |
| higedura | 0:9aa9be24636a | 1048 | buf_hex[0] = (unsigned int)buf_char; |
| higedura | 0:9aa9be24636a | 1049 | for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; } |
| higedura | 0:9aa9be24636a | 1050 | if(buf_hex[29]==0x40 && buf_hex[28]==0x43 && buf_hex[27]==0x4D && buf_hex[26]==0x0D){ |
| higedura | 0:9aa9be24636a | 1051 | // ******************** sequence No.1 ******************** |
| higedura | 0:9aa9be24636a | 1052 | |
| higedura | 0:9aa9be24636a | 1053 | // ******************** sequence No.2 ******************** |
| higedura | 0:9aa9be24636a | 1054 | // send to mavc |
| higedura | 0:9aa9be24636a | 1055 | mavc.putc(H); |
| higedura | 0:9aa9be24636a | 1056 | for(int i=0;i<data;i++){ mavc.putc(D[i]); } |
| higedura | 0:9aa9be24636a | 1057 | //for(int i=0;i<9;i++){ mavc.putc(test[i]); } |
| higedura | 0:9aa9be24636a | 1058 | // ******************** sequence No.2 ******************** |
| higedura | 0:9aa9be24636a | 1059 | |
| higedura | 0:9aa9be24636a | 1060 | // ******************** sequence No.3 ******************** |
| higedura | 0:9aa9be24636a | 1061 | // buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 | |
| higedura | 0:9aa9be24636a | 1062 | // |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY | |
| higedura | 0:9aa9be24636a | 1063 | // | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] | |
| higedura | 0:9aa9be24636a | 1064 | // transrating hex to dec |
| higedura | 0:9aa9be24636a | 1065 | for(int i=0;i<10;i++){ buf_dec[i] = buf_hex[24-i*2]*256+buf_hex[23-i*2]; } |
| higedura | 0:9aa9be24636a | 1066 | // wakariyasuku surutame bunnkatsu, honnrai ha matrix ga yoi |
| higedura | 0:9aa9be24636a | 1067 | // -> saisyuteki niha matrix ni simasu? |
| higedura | 0:9aa9be24636a | 1068 | for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec[i]-32768)/65535*20*9.8; } |
| higedura | 0:9aa9be24636a | 1069 | for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec[i+3]-32768)/65535*600/180*pi; } |
| higedura | 0:9aa9be24636a | 1070 | azi = (double)buf_dec[6]/65535*2*pi; |
| higedura | 0:9aa9be24636a | 1071 | alt = ((double)buf_dec[7]-32768)*0.1; |
| higedura | 0:9aa9be24636a | 1072 | for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec[i+8]-32768)*0.1; } |
| higedura | 0:9aa9be24636a | 1073 | // ******************** sequence No.3 ******************** |
| higedura | 0:9aa9be24636a | 1074 | |
| khoshino | 2:30f96c159d9c | 1075 | // ******************** initialization ******************* |
| khoshino | 2:30f96c159d9c | 1076 | if(t == 0){ |
| khoshino | 4:037fab837823 | 1077 | x0 = GPS[0]; |
| khoshino | 4:037fab837823 | 1078 | y0 = GPS[1]; |
| khoshino | 4:037fab837823 | 1079 | ps0 = azi; |
| khoshino | 2:30f96c159d9c | 1080 | } |
| khoshino | 4:037fab837823 | 1081 | xis = GPS[0] - x0; |
| khoshino | 4:037fab837823 | 1082 | ets = GPS[1] - y0; |
| khoshino | 4:037fab837823 | 1083 | psr = azi - ps0; |
| khoshino | 4:037fab837823 | 1084 | htr = alt; |
| khoshino | 4:037fab837823 | 1085 | avp = gyro[0]; |
| khoshino | 4:037fab837823 | 1086 | avq = gyro[1]; |
| khoshino | 4:037fab837823 | 1087 | avr = gyro[2]; |
| khoshino | 2:30f96c159d9c | 1088 | // ******************** initialization ******************* |
| khoshino | 2:30f96c159d9c | 1089 | |
| khoshino | 2:30f96c159d9c | 1090 | // ******************** interpolation ******************** |
| khoshino | 2:30f96c159d9c | 1091 | dr = dr + ua * dt; |
| khoshino | 4:037fab837823 | 1092 | xir = xir + ua * dt * cos(psr); |
| khoshino | 4:037fab837823 | 1093 | etr = etr + ua * dt * sin(psr); |
| khoshino | 2:30f96c159d9c | 1094 | |
| khoshino | 4:037fab837823 | 1095 | if (xis != xis1 || fabs(xis-xis1)<50) { |
| khoshino | 4:037fab837823 | 1096 | s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1); |
| khoshino | 2:30f96c159d9c | 1097 | ua = 0.5*(ua+sqrt(s)); |
| khoshino | 2:30f96c159d9c | 1098 | drc = drc+ua; |
| khoshino | 2:30f96c159d9c | 1099 | dr= drc; |
| khoshino | 4:037fab837823 | 1100 | xir=xis; |
| khoshino | 4:037fab837823 | 1101 | etr=ets; |
| khoshino | 2:30f96c159d9c | 1102 | } |
| khoshino | 4:037fab837823 | 1103 | xis1=xis; |
| khoshino | 4:037fab837823 | 1104 | ets1=ets; |
| khoshino | 2:30f96c159d9c | 1105 | // ******************** interpolation ******************** |
| khoshino | 2:30f96c159d9c | 1106 | |
| khoshino | 2:30f96c159d9c | 1107 | // ******************** guidance law WO homo ************* |
| khoshino | 4:037fab837823 | 1108 | htc = b0 + b1*dr + b2*dr*dr + b3*dr*dr*dr; |
| khoshino | 2:30f96c159d9c | 1109 | |
| khoshino | 2:30f96c159d9c | 1110 | /* |
| khoshino | 2:30f96c159d9c | 1111 | c = drp-dr[i]; |
| khoshino | 2:30f96c159d9c | 1112 | c1= dr[i+1]-drp; |
| khoshino | 2:30f96c159d9c | 1113 | |
| khoshino | 2:30f96c159d9c | 1114 | if(c >=0 && c1 >= 0) |
| khoshino | 2:30f96c159d9c | 1115 | { |
| khoshino | 2:30f96c159d9c | 1116 | c0=c/(dr[i+1]-dr[i]); |
| khoshino | 2:30f96c159d9c | 1117 | c1=c1/(dr[i+1]-dr[i]); |
| khoshino | 2:30f96c159d9c | 1118 | psc=c0*ps[i+1]+c1*ps[i]; |
| khoshino | 2:30f96c159d9c | 1119 | xic=c0*xi[i+1]+c1*xi[i]; |
| khoshino | 2:30f96c159d9c | 1120 | etc=c0*et[i+1]+c1*et[i]; |
| khoshino | 2:30f96c159d9c | 1121 | break; |
| khoshino | 2:30f96c159d9c | 1122 | } |
| khoshino | 4:037fab837823 | 1123 | *///kurikaesituika |
| khoshino | 2:30f96c159d9c | 1124 | |
| khoshino | 2:30f96c159d9c | 1125 | // ******************** guidance law WO homo ************* |
| khoshino | 2:30f96c159d9c | 1126 | |
| khoshino | 2:30f96c159d9c | 1127 | // ******************** control law ********************** |
| higedura | 0:9aa9be24636a | 1128 | // transrating dec to hex |
| higedura | 0:9aa9be24636a | 1129 | //com |
| kenkenpapurika | 7:05a718fdef74 | 1130 | |
| kenkenpapurika | 7:05a718fdef74 | 1131 | pse=(psc-psr)*dtr; |
| kenkenpapurika | 7:05a718fdef74 | 1132 | ete=etc-etr; |
| kenkenpapurika | 7:05a718fdef74 | 1133 | hte=htc-htr; |
| kenkenpapurika | 7:05a718fdef74 | 1134 | |
| kenkenpapurika | 7:05a718fdef74 | 1135 | //horizontal |
| kenkenpapurika | 7:05a718fdef74 | 1136 | gkp=omg1*omg1/grv; |
| kenkenpapurika | 7:05a718fdef74 | 1137 | gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr; |
| kenkenpapurika | 7:05a718fdef74 | 1138 | avpc=1/tt1*(gkd*pse+gkp*ete);// |
| kenkenpapurika | 7:05a718fdef74 | 1139 | |
| kenkenpapurika | 7:05a718fdef74 | 1140 | //vertical |
| kenkenpapurika | 7:05a718fdef74 | 1141 | v2=vmr*vmr; |
| kenkenpapurika | 7:05a718fdef74 | 1142 | dhdt=(hte-hte00)/dt; |
| kenkenpapurika | 7:05a718fdef74 | 1143 | |
| kenkenpapurika | 7:05a718fdef74 | 1144 | avqc=-1/tt2*(2/(0.084*rho*v2*swg))*(wgt*grv*cos(gmr*dtr)-wgt*(2*zet2*omg2*dhdt+omg2*omg2*hte));// |
| kenkenpapurika | 7:05a718fdef74 | 1145 | hte00=hte; |
| kenkenpapurika | 7:05a718fdef74 | 1146 | |
| kenkenpapurika | 7:05a718fdef74 | 1147 | |
| khoshino | 2:30f96c159d9c | 1148 | // ******************** control law ********************** |
| higedura | 0:9aa9be24636a | 1149 | |
| higedura | 3:17236ad0ff1e | 1150 | // !!! HOSHINO !!! fprintf add guidance, velocity and control |
| higedura | 3:17236ad0ff1e | 1151 | fprintf(fp, "%7.4f %7.4f %7.4f %7.3f %7.3f %7.3f %8.5f %8.1f %8.1f %8.1f\r\n", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], azi, alt, GPS[0], GPS[1]); |
| khoshino | 4:037fab837823 | 1152 | |
| higedura | 5:192835ac6106 | 1153 | flag_cm++; |
| khoshino | 4:037fab837823 | 1154 | |
| higedura | 0:9aa9be24636a | 1155 | } |
| higedura | 3:17236ad0ff1e | 1156 | |
| higedura | 3:17236ad0ff1e | 1157 | time = time + 0.1; |
| higedura | 3:17236ad0ff1e | 1158 | |
| higedura | 0:9aa9be24636a | 1159 | } |
| higedura | 0:9aa9be24636a | 1160 | |
| higedura | 0:9aa9be24636a | 1161 | fclose(fp); |
| higedura | 0:9aa9be24636a | 1162 | led1 = 1; led2 = 1; led3 = 1; led4 = 1; |
| higedura | 0:9aa9be24636a | 1163 | |
| higedura | 0:9aa9be24636a | 1164 | } |