yal kaiyo / Mbed 2 deprecated 130111_YNU_MPU_3

kanazawa demonstration

Fork of 130111_YNU_MPU_1 by yal kaiyo

main.cpp@13:b444c425e194, 2013-01-13 (annotated)

Committer:
yal_kaiyo
Date:
Sun Jan 13 09:13:17 2013 +0000
Revision:
13:b444c425e194
Parent:
12:33085c983354
Child:
14:feea7e2a51b8
for meeting

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