Kyohei Shinomoto
add homotopy
Fork of
YNU_MPUd2h
by 
hige dura
Diff: main.cpp
- Revision:
- 6:3792b68b1fc9
- Parent:
- 5:192835ac6106
--- a/main.cpp Tue Jan 08 12:54:47 2013 +0000
+++ b/main.cpp Wed Jan 09 08:08:53 2013 +0000
@@ -122,6 +122,12 @@
float idet =0.0;
float t = 0.0;
float phf[mh]={0};
+ float ap[mt]={0.0};
+ float ps[mt]={0.0};
+ float xi[mt]={0.0};
+ float et[mt]={0.0};
+ float xi1[mt]={0.0};
+ float et1[mt]={0.0};
//********************* HOSHINO *********************
//********************* OHTSU ***********************
@@ -134,7 +140,7 @@
}
// ******************** waiting @LC ********************
- while(flag_lc==0){
+ while(flag_lc==1){
buf_char = mavc.getc();
buf_hex[0] = (unsigned int)buf_char;
for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; }
@@ -143,6 +149,860 @@
// ******************** homotopy ********************
+ ////SYOKIKATEIKAI////
+ for(int it=0;it<mt;it++){
+ double tm=dt1*it;
+ int i0=mv*it;
+ ph0[i0]=zer;
+ if(fabs(ps0)<=0.0000000001){
+ ph0[i0+1]=(ps0+1.0)*tm;
+ }
+ else{
+ ph0[i0+1] = ps0*tm;
+ }
+ ph0[i0+2]= xi0*tm;
+ ph0[i0+3]= et0*tm;
+ ph0[i0+4]=-hal;
+ ph0[i0+5]=-hal;
+ ph0[i0+6]=-hal;
+ ph0[i0+7]=-hal;
+ }
+ for(int it=0;it<mh;it++){
+ phi[it]=ph0[it];
+ }
+ for(int it=0;it<mh;it++){
+ phf[it]=ph0[it];
+ }
+
+ //rungekutta
+ for(int ir=0;ir<mt;ir++){
+ //k1
+
+ ////SABUNSHIKI//////
+ for(int it=0;it<mt;it++){
+ int i0=mv*it;
+ x1[0][it]=phi[i0+0];
+ x1[1][it]=phi[i0+1];
+ x1[2][it]=phi[i0+2];
+ x1[3][it]=phi[i0+3];
+ rl[0][it]=phi[i0+4];
+ rl[1][it]=phi[i0+5];
+ rl[2][it]=phi[i0+6];
+ rl[3][it]=phi[i0+7];
+ }
+
+ fn[0]=x1[0][0];
+ fn[1]=x1[1][0];
+ fn[2]=x1[2][0];
+ fn[3]=x1[3][0];
+ for(int it=0;it<md;it++){
+ int i0=mx+mv*it;
+ int it1=it+1;
+ double bkp = hal*(x1[0][it]+x1[0][it1]);
+ double bps = hal*(x1[1][it]+x1[1][it1]);
+ double br1 = hal*(rl[0][it]+rl[0][it1]);
+ double br2 = hal*(rl[1][it]+rl[1][it1]);
+ double br3 = hal*(rl[2][it]+rl[2][it1]);
+ double br4 = hal*(rl[3][it]+rl[3][it1]);
+ fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
+ fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
+ fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
+ fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
+ fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
+ fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
+ fn[i0+6] = rl[2][it1]-rl[2][it];
+ fn[i0+7] = rl[3][it1]-rl[3][it];
+ }
+ fn[mh-4]=x1[0][mt-1];
+ fn[mh-3]=x1[1][mt-1]-ps0;
+ fn[mh-2]=x1[2][mt-1]-xi0;
+ fn[mh-1]=x1[3][mt-1]-et0;
+
+
+ ////HENBIBUN////////////
+ //// I 0 //////
+ //// A B 0 ////
+ //// 0 A B 0 //
+ ///////////////
+ ////////////I 0
+ //I
+ fph[0][0]=one;
+ fph[1][1]=one;
+ fph[2][2]=one;
+ fph[3][3]=one;
+ //A
+ for(int iu=0;iu<md;iu++){
+ int i0=mx+mv*iu;
+ int j0=mv*iu;
+ int j1=j0+mv;
+ double bps=hal*(phi[j0+1]+phi[j1+1]);
+ double br3=hal*(phi[j0+6]+phi[j1+6]);
+ double br4=hal*(phi[j0+7]+phi[j1+7]);
+ double cps=cos(bps);
+ double sps=sin(bps);
+ for(int iv=0;iv<mv;iv++){
+ fph[i0+iv][j0+iv]=-one;
+ }
+ fph[i0+1][j0] = hdt;
+ fph[i0+2][j0+1] =-hdt*sps;
+ fph[i0+3][j0+1] = hdt*cps;
+ fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
+ fph[i0][j0+4] = hdt;
+ fph[i0+4][j0+5] =-hdt;
+ fph[i0+5][j0+6] = hdt*sps;
+ fph[i0+5][j0+7] =-hdt*cps;
+ }
+ //B
+ //j0=j0+mv;
+ for(int iu=0;iu<md;iu++){
+ int i0=mx+mv*iu;
+ int j0=mv*(iu+1);
+ int j1=j0+mv;
+ double bps=hal*(phi[j0+1]+phi[j1+1]);
+ double br3=hal*(phi[j0+6]+phi[j1+6]);
+ double br4=hal*(phi[j0+7]+phi[j1+7]);
+ double cps=cos(bps);
+ double sps=sin(bps);
+ for(int iv=0;iv<mv;iv++){
+ fph[i0+iv][j0+iv]=one;
+ }
+ fph[i0+1][j0]=hdt;
+ fph[i0+2][j0+1]=-hdt*sps;
+ fph[i0+3][j0+1]=hdt*cps;
+ fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
+ fph[i0][j0+4]=hdt;
+ fph[i0+4][j0+5]=-hdt;
+ fph[i0+5][j0+6]=hdt*sps;
+ fph[i0+5][j0+7]=-hdt*cps;
+ }
+ //I
+ fph[mh-4][mh-8]=one;
+ fph[mh-3][mh-7]=one;
+ fph[mh-2][mh-6]=one;
+ fph[mh-1][mh-5]=one;
+
+ //GYAKUGYOURETSU
+ for(int i=0;i<mh;i++){
+ for(int j=0;j<mh;j++){
+ fpi[i][j]=fph[i][j];
+ }
+ }
+ for(int j=0;j<mh;j++) {
+ for(int i=0;i<mh;i++) {
+ if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
+ }
+ }
+
+ for(int i=0;i<mh;i++) {
+ double q=wk[i];
+ // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
+ wk[i]=1.0/q;
+ if(q > qmax){qmax=q;}
+ }
+
+ if(eps < 0){eps=qmax*mh*ueps;}
+ rdet =1.0;
+ idet =0.0;
+ for(int k=0;k<mh;k++) {
+ double amax = 0.0;
+ int kmax = k;
+ for(int i=k;i<mh;i++) {
+ if(fabs(fpi[i][k])*wk[i] <= amax){}
+ else{
+ amax=fabs(fpi[i][k])*wk[i];
+ kmax=i;
+ }
+ }
+ //if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
+ rdet=rdet*fpi[kmax][k];
+ if(k != kmax){rdet=-rdet;}
+ double adet =fabs(rdet);
+ while(adet > 1.0){
+ adet = adet*0.0625;
+ idet = idet + 4;
+ }
+ if(adet >= 0.0625){}
+ else{
+ adet=adet*16.0;
+ idet=idet-4;
+ }
+ if(rdet < 0.0){adet=-adet;}
+ rdet=adet;
+ iwk[k]=kmax;
+ amax=-1.0/fpi[kmax][k];
+ t=fpi[kmax][k];
+ fpi[kmax][k]=fpi[k][k];
+ fpi[k][k]=t;
+ for(int j=0;j<mh;j++) {
+ if(j == k){}
+ else{
+ t=fpi[kmax][j]*amax;
+ fpi[kmax][j]=fpi[k][j];
+ if(fabs(t) <= 0){}
+ else{
+ for(int i=0;i<mh;i++) {
+ fpi[i][j]=fpi[i][j]+t*fpi[i][k];
+ }
+ }
+ fpi[k][j]=-t;
+ }
+ }
+ for(int i=0;i<mh;i++) {
+ fpi[i][k]=fpi[i][k]*amax;
+ }
+ fpi[k][k]=-amax;
+ }
+
+ for(int l=0;l<mh;l++) {
+ int k=mh-l-1;
+ if(k == iwk[k]){}
+ else{
+ int kmax=iwk[k];
+ for(int i=0;i<mh;i++) {
+ t=fpi[i][k];
+ fpi[i][k]=fpi[i][kmax];
+ fpi[i][kmax]=t;
+ }
+ }
+ }
+
+ //////function
+ for(int jh=0;jh<mh;jh++){
+ sum=zer;
+ for(int ih=0;ih<mh;ih++){
+ sum=sum-fpi[jh][ih]*fn[ih];
+ }
+ dph1[jh]=sum;
+ }
+
+ for(int it=0;it<mh;it++){
+ phi[it]= phf[it]+hal*dt1*dph1[it];
+ }
+
+
+ //k2
+ ////SABUNSHIKI//////
+ for(int it=0;it<mt;it++){
+ int i0=mv*it;
+ x1[0][it]=phi[i0+0];
+ x1[1][it]=phi[i0+1];
+ x1[2][it]=phi[i0+2];
+ x1[3][it]=phi[i0+3];
+ rl[0][it]=phi[i0+4];
+ rl[1][it]=phi[i0+5];
+ rl[2][it]=phi[i0+6];
+ rl[3][it]=phi[i0+7];
+ }
+
+ fn[0]=x1[0][0];
+ fn[1]=x1[1][0];
+ fn[2]=x1[2][0];
+ fn[3]=x1[3][0];
+ for(int it=0;it<md;it++){
+ int i0=mx+mv*it;
+ int it1=it+1;
+ double bkp = hal*(x1[0][it]+x1[0][it1]);
+ double bps = hal*(x1[1][it]+x1[1][it1]);
+ double br1 = hal*(rl[0][it]+rl[0][it1]);
+ double br2 = hal*(rl[1][it]+rl[1][it1]);
+ double br3 = hal*(rl[2][it]+rl[2][it1]);
+ double br4 = hal*(rl[3][it]+rl[3][it1]);
+ fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
+ fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
+ fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
+ fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
+ fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
+ fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
+ fn[i0+6] = rl[2][it1]-rl[2][it];
+ fn[i0+7] = rl[3][it1]-rl[3][it];
+ }
+ fn[mh-4]=x1[0][mt-1];
+ fn[mh-3]=x1[1][mt-1]-ps0;
+ fn[mh-2]=x1[2][mt-1]-xi0;
+ fn[mh-1]=x1[3][mt-1]-et0;
+
+
+ ////HENBIBUN////////////
+ //// I 0 //////
+ //// A B 0 ////
+ //// 0 A B 0 //
+ ///////////////
+ ////////////I 0
+ //I
+ fph[0][0]=one;
+ fph[1][1]=one;
+ fph[2][2]=one;
+ fph[3][3]=one;
+ //A
+ for(int iu=0;iu<md;iu++){
+ int i0=mx+mv*iu;
+ int j0=mv*iu;
+ int j1=j0+mv;
+ double bps=hal*(phi[j0+1]+phi[j1+1]);
+ double br3=hal*(phi[j0+6]+phi[j1+6]);
+ double br4=hal*(phi[j0+7]+phi[j1+7]);
+ double cps=cos(bps);
+ double sps=sin(bps);
+ for(int iv=0;iv<mv;iv++){
+ fph[i0+iv][j0+iv]=-one;
+ }
+ fph[i0+1][j0] = hdt;
+ fph[i0+2][j0+1] =-hdt*sps;
+ fph[i0+3][j0+1] = hdt*cps;
+ fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
+ fph[i0][j0+4] = hdt;
+ fph[i0+4][j0+5] =-hdt;
+ fph[i0+5][j0+6] = hdt*sps;
+ fph[i0+5][j0+7] =-hdt*cps;
+ }
+ //B
+ //j0=j0+mv;
+ for(int iu=0;iu<md;iu++){
+ int i0=mx+mv*iu;
+ int j0=mv*(iu+1);
+ int j1=j0+mv;
+ double bps=hal*(phi[j0+1]+phi[j1+1]);
+ double br3=hal*(phi[j0+6]+phi[j1+6]);
+ double br4=hal*(phi[j0+7]+phi[j1+7]);
+ double cps=cos(bps);
+ double sps=sin(bps);
+ for(int iv=0;iv<mv;iv++){
+ fph[i0+iv][j0+iv]=one;
+ }
+ fph[i0+1][j0]=hdt;
+ fph[i0+2][j0+1]=-hdt*sps;
+ fph[i0+3][j0+1]=hdt*cps;
+ fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
+ fph[i0][j0+4]=hdt;
+ fph[i0+4][j0+5]=-hdt;
+ fph[i0+5][j0+6]=hdt*sps;
+ fph[i0+5][j0+7]=-hdt*cps;
+ }
+ //I
+ fph[mh-4][mh-8]=one;
+ fph[mh-3][mh-7]=one;
+ fph[mh-2][mh-6]=one;
+ fph[mh-1][mh-5]=one;
+
+ //GYAKUGYOURETSU
+ for(int i=0;i<mh;i++){
+ for(int j=0;j<mh;j++){
+ fpi[i][j]=fph[i][j];
+ }
+ }
+ for(int j=0;j<mh;j++) {
+ for(int i=0;i<mh;i++) {
+ if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
+ }
+ }
+
+ for(int i=0;i<mh;i++) {
+ double q=wk[i];
+ // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
+ wk[i]=1.0/q;
+ if(q > qmax){qmax=q;}
+ }
+
+ if(eps < 0){eps=qmax*mh*ueps;}
+ rdet =1.0;
+ idet =0.0;
+ for(int k=0;k<mh;k++) {
+ double amax = 0.0;
+ int kmax = k;
+ for(int i=k;i<mh;i++) {
+ if(fabs(fpi[i][k])*wk[i] <= amax){}
+ else{
+ amax=fabs(fpi[i][k])*wk[i];
+ kmax=i;
+ }
+ }
+ // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
+ rdet=rdet*fpi[kmax][k];
+ if(k != kmax){rdet=-rdet;}
+ double adet =fabs(rdet);
+ while(adet > 1.0){
+ adet = adet*0.0625;
+ idet = idet + 4;
+ }
+ if(adet >= 0.0625){}
+ else{
+ adet=adet*16.0;
+ idet=idet-4;
+ }
+ if(rdet < 0.0){adet=-adet;}
+ rdet=adet;
+ iwk[k]=kmax;
+ amax=-1.0/fpi[kmax][k];
+ t=fpi[kmax][k];
+ fpi[kmax][k]=fpi[k][k];
+ fpi[k][k]=t;
+ for(int j=0;j<mh;j++) {
+ if(j == k){}
+ else{
+ t=fpi[kmax][j]*amax;
+ fpi[kmax][j]=fpi[k][j];
+ if(fabs(t) <= 0){}
+ else{
+ for(int i=0;i<mh;i++) {
+ fpi[i][j]=fpi[i][j]+t*fpi[i][k];
+ }
+ }
+ fpi[k][j]=-t;
+ }
+ }
+ for(int i=0;i<mh;i++) {
+ fpi[i][k]=fpi[i][k]*amax;
+ }
+ fpi[k][k]=-amax;
+ }
+
+ for(int l=0;l<mh;l++) {
+ int k=mh-l-1;
+ if(k == iwk[k]){}
+ else{
+ int kmax=iwk[k];
+ for(int i=0;i<mh;i++) {
+ t=fpi[i][k];
+ fpi[i][k]=fpi[i][kmax];
+ fpi[i][kmax]=t;
+ }
+ }
+ }
+
+ //////function
+ for(int jh=0;jh<mh;jh++){
+ sum=zer;
+ for(int ih=0;ih<mh;ih++){
+ sum=sum-fpi[jh][ih]*fn[ih];
+ }
+ dph2[jh]=sum;
+ }
+
+ //k3
+ for(int it=0;it<mh;it++){
+ phi[it]= phf[it]+hal*dt1*dph2[it];
+ }
+ ////SABUNSHIKI//////
+ for(int it=0;it<mt;it++){
+ int i0=mv*it;
+ x1[0][it]=phi[i0+0];
+ x1[1][it]=phi[i0+1];
+ x1[2][it]=phi[i0+2];
+ x1[3][it]=phi[i0+3];
+ rl[0][it]=phi[i0+4];
+ rl[1][it]=phi[i0+5];
+ rl[2][it]=phi[i0+6];
+ rl[3][it]=phi[i0+7];
+ }
+
+ fn[0]=x1[0][0];
+ fn[1]=x1[1][0];
+ fn[2]=x1[2][0];
+ fn[3]=x1[3][0];
+ for(int it=0;it<md;it++){
+ int i0=mx+mv*it;
+ int it1=it+1;
+ double bkp = hal*(x1[0][it]+x1[0][it1]);
+ double bps = hal*(x1[1][it]+x1[1][it1]);
+ double br1 = hal*(rl[0][it]+rl[0][it1]);
+ double br2 = hal*(rl[1][it]+rl[1][it1]);
+ double br3 = hal*(rl[2][it]+rl[2][it1]);
+ double br4 = hal*(rl[3][it]+rl[3][it1]);
+ fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
+ fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
+ fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
+ fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
+ fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
+ fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
+ fn[i0+6] = rl[2][it1]-rl[2][it];
+ fn[i0+7] = rl[3][it1]-rl[3][it];
+ }
+ fn[mh-4]=x1[0][mt-1];
+ fn[mh-3]=x1[1][mt-1]-ps0;
+ fn[mh-2]=x1[2][mt-1]-xi0;
+ fn[mh-1]=x1[3][mt-1]-et0;
+
+
+ ////HENBIBUN////////////
+ //// I 0 //////
+ //// A B 0 ////
+ //// 0 A B 0 //
+ ///////////////
+ ////////////I 0
+ //I
+ fph[0][0]=one;
+ fph[1][1]=one;
+ fph[2][2]=one;
+ fph[3][3]=one;
+ //A
+ for(int iu=0;iu<md;iu++){
+ int i0=mx+mv*iu;
+ int j0=mv*iu;
+ int j1=j0+mv;
+ double bps=hal*(phi[j0+1]+phi[j1+1]);
+ double br3=hal*(phi[j0+6]+phi[j1+6]);
+ double br4=hal*(phi[j0+7]+phi[j1+7]);
+ double cps=cos(bps);
+ double sps=sin(bps);
+ for(int iv=0;iv<mv;iv++){
+ fph[i0+iv][j0+iv]=-one;
+ }
+ fph[i0+1][j0] = hdt;
+ fph[i0+2][j0+1] =-hdt*sps;
+ fph[i0+3][j0+1] = hdt*cps;
+ fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
+ fph[i0][j0+4] = hdt;
+ fph[i0+4][j0+5] =-hdt;
+ fph[i0+5][j0+6] = hdt*sps;
+ fph[i0+5][j0+7] =-hdt*cps;
+ }
+ //B
+ //j0=j0+mv;
+ for(int iu=0;iu<md;iu++){
+ int i0=mx+mv*iu;
+ int j0=mv*(iu+1);
+ int j1=j0+mv;
+ double bps=hal*(phi[j0+1]+phi[j1+1]);
+ double br3=hal*(phi[j0+6]+phi[j1+6]);
+ double br4=hal*(phi[j0+7]+phi[j1+7]);
+ double cps=cos(bps);
+ double sps=sin(bps);
+ for(int iv=0;iv<mv;iv++){
+ fph[i0+iv][j0+iv]=one;
+ }
+ fph[i0+1][j0]=hdt;
+ fph[i0+2][j0+1]=-hdt*sps;
+ fph[i0+3][j0+1]=hdt*cps;
+ fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
+ fph[i0][j0+4]=hdt;
+ fph[i0+4][j0+5]=-hdt;
+ fph[i0+5][j0+6]=hdt*sps;
+ fph[i0+5][j0+7]=-hdt*cps;
+ }
+ //I
+ fph[mh-4][mh-8]=one;
+ fph[mh-3][mh-7]=one;
+ fph[mh-2][mh-6]=one;
+ fph[mh-1][mh-5]=one;
+
+ //GYAKUGYOURETSU
+ for(int i=0;i<mh;i++){
+ for(int j=0;j<mh;j++){
+ fpi[i][j]=fph[i][j];
+ }
+ }
+ for(int j=0;j<mh;j++) {
+ for(int i=0;i<mh;i++) {
+ if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
+ }
+ }
+
+ for(int i=0;i<mh;i++) {
+ double q=wk[i];
+ // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
+ wk[i]=1.0/q;
+ if(q > qmax){qmax=q;}
+ }
+
+ if(eps < 0){eps=qmax*mh*ueps;}
+ rdet =1.0;
+ idet =0.0;
+ for(int k=0;k<mh;k++) {
+ double amax = 0.0;
+ int kmax = k;
+ for(int i=k;i<mh;i++) {
+ if(fabs(fpi[i][k])*wk[i] <= amax){}
+ else{
+ amax=fabs(fpi[i][k])*wk[i];
+ kmax=i;
+ }
+ }
+ // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
+ rdet=rdet*fpi[kmax][k];
+ if(k != kmax){rdet=-rdet;}
+ double adet =fabs(rdet);
+ while(adet > 1.0){
+ adet = adet*0.0625;
+ idet = idet + 4;
+ }
+ if(adet >= 0.0625){}
+ else{
+ adet=adet*16.0;
+ idet=idet-4;
+ }
+ if(rdet < 0.0){adet=-adet;}
+ rdet=adet;
+ iwk[k]=kmax;
+ amax=-1.0/fpi[kmax][k];
+ t=fpi[kmax][k];
+ fpi[kmax][k]=fpi[k][k];
+ fpi[k][k]=t;
+ for(int j=0;j<mh;j++) {
+ if(j == k){}
+ else{
+ t=fpi[kmax][j]*amax;
+ fpi[kmax][j]=fpi[k][j];
+ if(fabs(t) <= 0){}
+ else{
+ for(int i=0;i<mh;i++) {
+ fpi[i][j]=fpi[i][j]+t*fpi[i][k];
+ }
+ }
+ fpi[k][j]=-t;
+ }
+ }
+ for(int i=0;i<mh;i++) {
+ fpi[i][k]=fpi[i][k]*amax;
+ }
+ fpi[k][k]=-amax;
+ }
+
+ for(int l=0;l<mh;l++) {
+ int k=mh-l-1;
+ if(k == iwk[k]){}
+ else{
+ int kmax=iwk[k];
+ for(int i=0;i<mh;i++) {
+ t=fpi[i][k];
+ fpi[i][k]=fpi[i][kmax];
+ fpi[i][kmax]=t;
+ }
+ }
+ }
+
+ //////function
+ for(int jh=0;jh<mh;jh++){
+ sum=zer;
+ for(int ih=0;ih<mh;ih++){
+ sum=sum-fpi[jh][ih]*fn[ih];
+ }
+ dph3[jh]=sum;
+ }
+
+ //k4
+
+ for(int it=0;it<mh;it++){
+ phi[it]= phf[it]+dt1*dph3[it];
+ }
+ ////SABUNSHIKI//////
+ for(int it=0;it<mt;it++){
+ int i0=mv*it;
+ x1[0][it]=phi[i0+0];
+ x1[1][it]=phi[i0+1];
+ x1[2][it]=phi[i0+2];
+ x1[3][it]=phi[i0+3];
+ rl[0][it]=phi[i0+4];
+ rl[1][it]=phi[i0+5];
+ rl[2][it]=phi[i0+6];
+ rl[3][it]=phi[i0+7];
+ }
+
+ fn[0]=x1[0][0];
+ fn[1]=x1[1][0];
+ fn[2]=x1[2][0];
+ fn[3]=x1[3][0];
+ for(int it=0;it<md;it++){
+ int i0=mx+mv*it;
+ int it1=it+1;
+ double bkp = hal*(x1[0][it]+x1[0][it1]);
+ double bps = hal*(x1[1][it]+x1[1][it1]);
+ double br1 = hal*(rl[0][it]+rl[0][it1]);
+ double br2 = hal*(rl[1][it]+rl[1][it1]);
+ double br3 = hal*(rl[2][it]+rl[2][it1]);
+ double br4 = hal*(rl[3][it]+rl[3][it1]);
+ fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
+ fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
+ fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
+ fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
+ fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
+ fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
+ fn[i0+6] = rl[2][it1]-rl[2][it];
+ fn[i0+7] = rl[3][it1]-rl[3][it];
+ }
+ fn[mh-4]=x1[0][mt-1];
+ fn[mh-3]=x1[1][mt-1]-ps0;
+ fn[mh-2]=x1[2][mt-1]-xi0;
+ fn[mh-1]=x1[3][mt-1]-et0;
+
+
+ ////HENBIBUN////////////
+ //// I 0 //////
+ //// A B 0 ////
+ //// 0 A B 0 //
+ ///////////////
+ ////////////I 0
+ //I
+ fph[0][0]=one;
+ fph[1][1]=one;
+ fph[2][2]=one;
+ fph[3][3]=one;
+ //A
+ for(int iu=0;iu<md;iu++){
+ int i0=mx+mv*iu;
+ int j0=mv*iu;
+ int j1=j0+mv;
+ double bps=hal*(phi[j0+1]+phi[j1+1]);
+ double br3=hal*(phi[j0+6]+phi[j1+6]);
+ double br4=hal*(phi[j0+7]+phi[j1+7]);
+ double cps=cos(bps);
+ double sps=sin(bps);
+ for(int iv=0;iv<mv;iv++){
+ fph[i0+iv][j0+iv]=-one;
+ }
+ fph[i0+1][j0] = hdt;
+ fph[i0+2][j0+1] =-hdt*sps;
+ fph[i0+3][j0+1] = hdt*cps;
+ fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
+ fph[i0][j0+4] = hdt;
+ fph[i0+4][j0+5] =-hdt;
+ fph[i0+5][j0+6] = hdt*sps;
+ fph[i0+5][j0+7] =-hdt*cps;
+ }
+ //B
+ //j0=j0+mv;
+ for(int iu=0;iu<md;iu++){
+ int i0=mx+mv*iu;
+ int j0=mv*(iu+1);
+ int j1=j0+mv;
+ double bps=hal*(phi[j0+1]+phi[j1+1]);
+ double br3=hal*(phi[j0+6]+phi[j1+6]);
+ double br4=hal*(phi[j0+7]+phi[j1+7]);
+ double cps=cos(bps);
+ double sps=sin(bps);
+ for(int iv=0;iv<mv;iv++){
+ fph[i0+iv][j0+iv]=one;
+ }
+ fph[i0+1][j0]=hdt;
+ fph[i0+2][j0+1]=-hdt*sps;
+ fph[i0+3][j0+1]=hdt*cps;
+ fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
+ fph[i0][j0+4]=hdt;
+ fph[i0+4][j0+5]=-hdt;
+ fph[i0+5][j0+6]=hdt*sps;
+ fph[i0+5][j0+7]=-hdt*cps;
+ }
+ //I
+ fph[mh-4][mh-8]=one;
+ fph[mh-3][mh-7]=one;
+ fph[mh-2][mh-6]=one;
+ fph[mh-1][mh-5]=one;
+
+ //GYAKUGYOURETSU
+ for(int i=0;i<mh;i++){
+ for(int j=0;j<mh;j++){
+ fpi[i][j]=fph[i][j];
+ }
+ }
+ for(int j=0;j<mh;j++) {
+ for(int i=0;i<mh;i++) {
+ if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
+ }
+ }
+
+ for(int i=0;i<mh;i++) {
+ double q=wk[i];
+ // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
+ wk[i]=1.0/q;
+ if(q > qmax){qmax=q;}
+ }
+
+ if(eps < 0){eps=qmax*mh*ueps;}
+ rdet =1.0;
+ idet =0.0;
+ for(int k=0;k<mh;k++) {
+ double amax = 0.0;
+ int kmax = k;
+ for(int i=k;i<mh;i++) {
+ if(fabs(fpi[i][k])*wk[i] <= amax){}
+ else{
+ amax=fabs(fpi[i][k])*wk[i];
+ kmax=i;
+ }
+ }
+ // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
+ rdet=rdet*fpi[kmax][k];
+ if(k != kmax){rdet=-rdet;}
+ double adet =fabs(rdet);
+ while(adet > 1.0){
+ adet = adet*0.0625;
+ idet = idet + 4;
+ }
+ if(adet >= 0.0625){}
+ else{
+ adet=adet*16.0;
+ idet=idet-4;
+ }
+ if(rdet < 0.0){adet=-adet;}
+ rdet=adet;
+ iwk[k]=kmax;
+ amax=-1.0/fpi[kmax][k];
+ t=fpi[kmax][k];
+ fpi[kmax][k]=fpi[k][k];
+ fpi[k][k]=t;
+ for(int j=0;j<mh;j++) {
+ if(j == k){}
+ else{
+ t=fpi[kmax][j]*amax;
+ fpi[kmax][j]=fpi[k][j];
+ if(fabs(t) <= 0){}
+ else{
+ for(int i=0;i<mh;i++) {
+ fpi[i][j]=fpi[i][j]+t*fpi[i][k];
+ }
+ }
+ fpi[k][j]=-t;
+ }
+ }
+ for(int i=0;i<mh;i++) {
+ fpi[i][k]=fpi[i][k]*amax;
+ }
+ fpi[k][k]=-amax;
+ }
+
+ for(int l=0;l<mh;l++) {
+ int k=mh-l-1;
+ if(k == iwk[k]){}
+ else{
+ int kmax=iwk[k];
+ for(int i=0;i<mh;i++) {
+ t=fpi[i][k];
+ fpi[i][k]=fpi[i][kmax];
+ fpi[i][kmax]=t;
+ }
+ }
+ }
+
+ //////function
+ for(int jh=0;jh<mh;jh++){
+ sum=zer;
+ for(int ih=0;ih<mh;ih++){
+ sum=sum-fpi[jh][ih]*fn[ih];
+ }
+ dph4[jh]=sum;
+ }
+
+ //saitekikai
+
+ for(int it=0;it<mh;it++){
+ phf[it]= phf[it]+dt1*(dph1[it]+2*dph2[it]+2*dph3[it]+dph4[it])/6.0;
+ }
+
+ }
+
+ for (int id=0; id< mt; ++id){
+ int ii = mt-id-1;
+ int i0 = mv*ii;
+ ap[id]=phi[i0+0];
+ ps[id]=phi[i0+1];
+ xi1[id]=phi[i0+2]*drf;
+ et1[id]=phi[i0+3]*drf;
+ xi[id]=xi1[id]-xi1[0];
+ et[id]=et1[id]-et1[0];
+ }
+ led1 = 1;
+ wait(1.0);
+
+ ///////////////////
flag_lc = 1; // end of homotopy
// ******************** homotopy ********************