asc node fix

utils/AscNodeOrbit.cpp

@@ -3,6 +3,7 @@
 #include "DrPeriod.h"
 #include "GreenwichFrame.h"
 #include "ECIFrame.h"
+#include "METEFrame.h"
 #include "XMLFunctions.h"
 #include "mathconst.h"
 
@@ -27,7 +28,7 @@ void AscNodeOrbit::calc_asc_node(Propagator& prop, const TimeJD& date, const boo
         //if(!GetAscNodeYZ(*prop.GetPredictor()->GetPredictor(), prop.GetPredictor()->GetTime(), result, GetZ, GetVZ)){
         //	return false;
         //};
-
+               
         SInitOrbit ascNodeOrbit;
         if (!jump_arg_of_lat(date, prop, 0, gcs_node, ascNodeOrbit)) {
             throw Exp() << "internal error in ascending node method 1";
@@ -37,6 +38,10 @@ void AscNodeOrbit::calc_asc_node(Propagator& prop, const TimeJD& date, const boo
 
         if (DaysInterval(date, result.T) * 86400 > 1) {
             // TODO sometimes date lower result.T less than 1 sec
+            //SInitOrbit ascNodeOrbit;
+            //if (!jump_arg_of_lat(date, prop, 0, gcs_node, ascNodeOrbit)) {
+            //    throw Exp() << "internal error in ascending node method 1";
+            //}
             throw Exp() << "internal error in ascending node method 2 " << DaysInterval(date, result.T)*86400;
         }
 
@@ -50,9 +55,6 @@ void AscNodeOrbit::calc_asc_node(Propagator& prop, const TimeJD& date, const boo
 
 void AscNodeOrbit::calc_desc_node(Propagator& prop, const TimeJD& date, const bool vzmode, const bool gcs_node, PhasePoint6D& result) const
 {
-//    std::string date_s = DateToStr(date);
-//    std::cout << date_s << "\n";
-
     if (vzmode) {
         //if(!GetAscNodeYZ(*prop.GetPredictor()->GetPredictor(), prop.GetPredictor()->GetTime(), result, GetZ, GetVZ)){
         //	return false;
@@ -64,11 +66,9 @@ void AscNodeOrbit::calc_desc_node(Propagator& prop, const TimeJD& date, const bo
         }
 
         result = ascNodeOrbit.GetPhasePointJ2000();
-
-        //std::string res_s = DateToStr(result.T);
-
+                
         if (date < result.T) {
-            throw Exp() << "internal error in descending node method 2 " << DaysInterval(date, result.T) * 86400;;
+            throw Exp() << "internal error in descending node method 2 " << DaysInterval(date, result.T) * 86400;
         }
 
     }
@@ -136,44 +136,71 @@ double AscNodeOrbit::get_kep_time(double TA, double e, double w) const
 
 bool AscNodeOrbit::jump_arg_of_lat(const TimeJD& date, Propagator& prop, double dstU, bool gcs_node, SInitOrbit& result) const
 {
-    int maxInter = 20;
+    const int maxInter = 20;
     int iter = 0;
-    bool nearestAscendingNode = false;
-    double u_tol = 0.001;
-    double time_tol = 0.001;
+    const bool nearestAscendingNode = false;
+    const double u_tol = 0.001;
+    const double time_tol = 0.001;
 
     PhasePoint6D p;
     prop.Propagate(date, p);
 
     if (!nearestAscendingNode) {
         Kepler kep;
-        kep.SetXYZ(p.CoordsVel);
-        prop.Propagate(kep.GetTimeFromAN(p.T), p);
+        PhasePoint6D pt = p;
+        
+        //if (gcs_node) {
+        //    pt.Convert(METEFrame(pt.T));
+        //}
+
+        kep.SetXYZ(pt.CoordsVel);
+        prop.Propagate(kep.GetTimeFromAN(pt.T), p);
     }
 
     while (true) {
-        Kepler kep;
+        double u;
+        double e;
+        double arg_of_per;
+        double period;
+
+        //std::string date_s = DateToStr(p.T);
+        //std::cout << date_s << "\n";
+
         if (gcs_node) {
             PhasePoint6D gcs = p;
-            gcs.Convert(GreenwichFrame());
-            kep.SetXYZ(gcs.CoordsVel);
+            gcs.Convert(ECIFrame(gcs.T));
+            Kepler gcs_kep;
+            gcs_kep.SetXYZ(gcs.CoordsVel);
+            u = gcs_kep.U;
+            arg_of_per = gcs_kep.GetArgP();
+            e = gcs_kep.GetEcc();
+            period = gcs_kep.GetPeriod();
         }
         else {
+            Kepler kep;
             kep.SetXYZ(p.CoordsVel);
+            u = kep.U;
+            e = kep.GetEcc();
+            arg_of_per = kep.GetArgP();
+            period = kep.GetPeriod();
         }
-
-        if (fabs(Distance(0, kep.U, 0, dstU)) * rad2deg < u_tol) {
+                
+        if (fabs(Distance(0, u, 0, dstU)) * rad2deg < u_tol) {
             break;
         }
-
-        double e = kep.GetEcc();
-        double u = kep.U;
-        double period = kep.GetPeriod();
+        
         double w = 86400 / period / 1000 * Pi2 / 1440 / 60;
-        double t = get_kep_time((u - kep.GetArgP()), e, w);
-        double t2 = get_kep_time((dstU - kep.GetArgP()), e, w);
+        double t = get_kep_time((u - arg_of_per), e, w);
+        double t2 = get_kep_time((dstU - arg_of_per), e, w);
         double step = t2 - t;
 
+        if (step > period * 1000 / 2) {
+            step = step - period * 1000;
+        }
+        else if (step < -period * 1000 / 2) {
+            step = period * 1000 - step;
+        }
+
         if (fabs(step) < time_tol) {
             break;
         }