Diff: main.cpp

Revision:

16:a33fe7d99a0f

Parent:

15:571b34671309

Child:

17:3cd1afc27ca7

--- a/main.cpp	Mon Jan 14 12:02:15 2013 +0000
+++ b/main.cpp	Wed Jan 16 13:04:38 2013 +0000
@@ -73,7 +73,7 @@
     float   drf     =   100;    //syuutandaunrenji
     float   ht0     =   0;
     float   gm0     =   20*pi/180;
-    float   ua      =   10; //initial horizontal velosity
+    float   ua      =   1; //initial horizontal velosity
     //terminal condition
     float   htf     =   5;    
     float   gmf     =   5*pi/180;
@@ -118,7 +118,7 @@
     
     //homotopy
     //initial condition
-    float rps0 = 90.0;
+    float rps0 =  0.0;
     float rxi0 = -60.0;    //terminal xi
     float ret0 = -20.0;    //terminal et
     
@@ -171,7 +171,7 @@
     float   zet2    =   0.7;
     
     float   gmr     =   20;
-    float   vmr     =   0;
+    float   vmr     =   16;
     float   pse     =   0;
     float   phir    =   0;
     float   ete     =   0;   
@@ -1135,11 +1135,8 @@
             // ******************** initialization *******************
             
             // ******************** interpolation ********************
-            //xir ni hennkannseneba
 
-            //filter henko
-
-            if (xis != xis1 && fabs(xis-xis1)<50) {
+            if (fabs(xis- xis1)>1 && fabs(xis-xis1)<50) {
                 s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1);
                 ua = 0.5*(ua+sqrt(s)/(time-time1));
                 drc = drc+ua*(time-time1);
@@ -1172,6 +1169,7 @@
             if(c >=0 && c1 >= 0){
             c0=c/(dr[i+1]-dr[i]);
             c1=c1/(dr[i+1]-dr[i]);
+            apc=c0*ap[i+1]+c1*ap[i];
             psc=c0*ps[i+1]+c1*ps[i];
             xic=c0*xi[i+1]+c1*xi[i];
             etc=c0*et[i+1]+c1*et[i];
@@ -1198,10 +1196,7 @@
             phff=atan(apc*v2/grv);
             phic=-gkp*ete-gkd*pse-phff;
             avpc=(phic-phic00)/dt;
-            phic00=phic;
-            
-            
-            
+            phic00=phic;                      
             //vertical
             
             a22=grv*sin(gmr*dtr)/vmr;
@@ -1210,21 +1205,14 @@
             b21=swg*rho*vmr*dclda*cos(phir*dtr)/(2*wgt);
             aaa=a32*b21;
             ck1=(a22+2*zet2*omg2)/aaa;
-            ck2=(a23*a32+omg2*omg2)/aaa;
-            
-            
-            
+            ck2=(a23*a32+omg2*omg2)/aaa;         
             
             dhdt=(hte-hte00)/dt;
-            
-            
+                   
             // to translate deg/s to rad/s, add *pi/180. TORATANI 
             avqc=(ck1*dhdt+ck2*hte)/tt2;
             hte00=hte;
-            
-            
-
-            
+                       
             com[0]  =   avpc;
             com[1]  =   avqc;
             com[2]  =   avrc;