YNU_MPU_tora - for hige

Users » yal_kaiyo » Code » YNU_MPU_tora

yal kaiyo / Mbed 2 deprecated YNU_MPU_tora

for hige

main.cpp@6:3792b68b1fc9, 2013-01-09 (annotated)

Committer:: khoshino
Date:: Wed Jan 09 08:08:53 2013 +0000
Revision:: 6:3792b68b1fc9
Parent:: 5:192835ac6106
Child:: 7:05a718fdef74

add homotomy

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	21 Jan 2013
Imports:	1
Forks:	0
Commits:	18
Dependents:	0
Dependencies:	0
Followers:	1

main.cpp@6:3792b68b1fc9, 2013-01-09 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning