Diff: main.cpp

Revision:

6:3792b68b1fc9

Parent:

5:192835ac6106

Child:

7:05a718fdef74

diff -r 192835ac6106 -r 3792b68b1fc9 main.cpp
--- 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 ********************