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