yal kaiyo / Mbed 2 deprecated 130111_YNU_MPU_3

kanazawa demonstration

Fork of 130111_YNU_MPU_1 by yal kaiyo

main.cpp@12:33085c983354, 2013-01-11 (annotated)

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