yal kaiyo / Mbed 2 deprecated 130111_YNU_MPU_3

kanazawa demonstration

Fork of 130111_YNU_MPU_1 by yal kaiyo

main.cpp@8:de86cf9ccb89, 2013-01-10 (annotated)

Committer:
higedura
Date:
Thu Jan 10 14:43:14 2013 +0000
Revision:
8:de86cf9ccb89
Parent:
7:05a718fdef74
Child:
9:a56c340f47c9
130110_add_tx_DecToAscii

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