yal kaiyo / Mbed 2 deprecated YNU_MPU_tora

for hige

Fork of 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?

UserRevisionLine numberNew 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 }