+ rnb interpolation (WIP)

utils/interpolate.cpp

@@ -33,24 +33,36 @@ static double get_step(const SInitOrbit& orbit)
 
 XInterpolator::XInterpolator() = default;
 
-XInterpolator::XInterpolator(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end)
+XInterpolator::XInterpolator(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, const bool calc_rnb)
 {
-    create(orbit, beg, end);
+    create(orbit, beg, end, calc_rnb);
 }
 
-XInterpolator::XInterpolator(const std::vector<IRes>& vec)
+XInterpolator::XInterpolator(
+    const std::vector<IRes>& vec,
+    const std::vector<std::tuple<double, double, double>>& rnb
+    )
 {
     std::vector<IRes> svec = vec;
     std::sort(svec.begin(), svec.end(), [](const IRes& a, const IRes& b) {
         return (a.get_date() < b.get_date());
     });
 
-    create(svec);
+    create(svec, rnb);
 }
 
-void XInterpolator::propagate_dir(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, const int dir, std::vector<IRes>& result)
+void XInterpolator::propagate_dir(
+    const SInitOrbit& orbit,
+    const TimeJD& beg,
+    const TimeJD& end,
+    const int dir,
+    const bool calc_rnb,
+    std::vector<IRes>& xyz,
+    std::vector<std::tuple<double, double, double>>& rnb
+)
 {
-    Propagator prop(orbit, false);
+    const bool variates = orbit.GetCovMtx().HasMtx() && orbit.GetMotionModel().GetType() == ST_PROP && calc_rnb;
+    Propagator prop(orbit, variates);
     const double step = get_step(orbit);
 
     bool exit = false;
@@ -64,31 +76,54 @@ void XInterpolator::propagate_dir(const SInitOrbit& orbit, const TimeJD& beg, co
 
         double age = DaysInterval(orbit.GetDate(), date);
 
+        if (!variates) {
             PhasePoint6D pos;
             prop.Propagate(date, pos);
-        result.emplace_back(pos.CoordsVel, date, age, orbit.GetId());
+            xyz.emplace_back(pos.CoordsVel, date, age, orbit.GetId());
+        }
+        else {
+            SInitOrbit norb;
+            prop.Propagate(date, norb);
+            xyz.emplace_back(norb.GetPhasePointJ2000().CoordsVel, date, age, orbit.GetId());
+
+            std::map<std::string, double> covrnb = norb.GetCovMtx().GetCovRnbMap(norb.GetPhasePointJ2000().CoordsVel);
+            rnb.emplace_back(covrnb["RR"], covrnb["NN"], covrnb["BB"]);
+        }
+
         date = ShiftDate(date, dir * step);
     }
 }
 
-void XInterpolator::propagate(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, int dir,
-                              std::vector<IRes>& result)
+void XInterpolator::propagate(
+    const SInitOrbit& orbit,
+    const TimeJD& beg,
+    const TimeJD& end,
+    const int dir,
+    const bool calc_rnb,
+    std::vector<IRes>& xyz,
+    std::vector<std::tuple<double, double, double>>& rnb
+)
 {
     if (dir != 0) {
-        propagate_dir(orbit, beg, end, dir, result);
+        propagate_dir(orbit, beg, end, dir, calc_rnb, xyz, rnb);
     }
     else {
-        propagate_dir(orbit, ShiftDate(orbit.GetDate(), +get_step(orbit) / 2.0), end, +1, result);
-        propagate_dir(orbit, ShiftDate(orbit.GetDate(), -get_step(orbit) / 2.0), beg, -1, result);
+        propagate_dir(orbit, ShiftDate(orbit.GetDate(), +get_step(orbit) / 2.0), end, +1, calc_rnb, xyz, rnb);
+        propagate_dir(orbit, ShiftDate(orbit.GetDate(), -get_step(orbit) / 2.0), beg, -1, calc_rnb, xyz, rnb);
     }
 
-    std::sort(result.begin(), result.end(), [](const IRes& a, const IRes& b) {
+    std::sort(xyz.begin(), xyz.end(), [](const IRes& a, const IRes& b) {
         return a.get_date() < b.get_date();
     });
 }
 
-void XInterpolator::calc_positions(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end,
-                                   std::vector<IRes>& result)
+void XInterpolator::calc_positions(
+    const SInitOrbit& orbit,
+    const TimeJD& beg,
+    const TimeJD& end,
+    const bool calc_rnb,
+    std::vector<IRes>& xyz,
+    std::vector<std::tuple<double, double, double>>& rnb)
 {
     const int n = 100;
     const int step = get_step(orbit);
@@ -97,13 +132,13 @@ void XInterpolator::calc_positions(const SInitOrbit& orbit, const TimeJD& beg, c
     const TimeJD end_ext = ShiftDate(end, +n * step);
 
     if (orbit.GetDate() <= beg_ext) {
-        propagate(orbit, beg_ext, end_ext, 1, result);
+        propagate(orbit, beg_ext, end_ext, 1, calc_rnb, xyz, rnb);
     }
     else if (orbit.GetDate() >= end_ext) {
-        propagate(orbit, end_ext, beg_ext, -1, result);
+        propagate(orbit, end_ext, beg_ext, -1, calc_rnb, xyz, rnb);
     }
     else {
-        propagate(orbit, beg_ext, end_ext, 0, result);
+        propagate(orbit, beg_ext, end_ext, 0, calc_rnb, xyz, rnb);
     }
 }
 
@@ -155,27 +190,51 @@ void finalize(ei& ephi)
     }
 }
 
-void XInterpolator::create(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end)
+void XInterpolator::create(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, const bool calc_rnb)
 {
-    std::vector<IRes> positions;
-    calc_positions(orbit, beg, end, positions);
+    std::vector<IRes> xyz;
+    std::vector<std::tuple<double, double, double>> rnb;
+    calc_positions(orbit, beg, end, calc_rnb, xyz, rnb);
 
-    create(positions);
+    create(xyz, rnb);
 }
 
-void XInterpolator::create(const std::vector<IRes>& positions)
+void XInterpolator::create(
+    const std::vector<IRes>& xyz, 
+    const std::vector<std::tuple<double, double, double>>& rnb)
 {
-    m_beg = positions.front().get_date();
-    m_end = positions.back().get_date();
+    m_beg = xyz.front().get_date();
+    m_end = xyz.back().get_date();
 
-    init(m_ephi, positions.size());
+    init(m_ephi, xyz.size());
 
-    for (const auto& p : positions) {
+    for (const auto& p : xyz) {
         fill(p.get_pos(), p.get_date().GetDays(), p.get_date().GetFraction(), m_ephi);
     }
 
     finalize(m_ephi);
-    m_positions = positions;
+    m_positions = xyz;
+
+    if(!rnb.empty()) {
+        init(m_rnb_ephi, rnb.size());
+
+        for(int i=0; i < static_cast<int>(rnb.size()); i++){
+            const auto& xyz_p = xyz[i];
+            const auto& rnb_p = rnb[i];
+            const Vect6 v(
+                std::get<0>(rnb_p),
+                std::get<1>(rnb_p),
+                std::get<2>(rnb_p),
+                0,
+                0,
+                0
+            );
+            fill(v, xyz_p.get_date().GetDays(), xyz_p.get_date().GetFraction(), m_rnb_ephi);
+        }
+
+        finalize(m_rnb_ephi);
+        m_rnb = rnb;
+    }
 }
 
 Vect6 XInterpolator::get_pos(const TimeJD& date, const bool tks) const

utils/interpolate.h

@@ -10,20 +10,46 @@ class XInterpolator
 {
 public:
     XInterpolator();
-    XInterpolator(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end);
-    XInterpolator(const std::vector<IRes>& vec);
+    XInterpolator(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, bool calc_rnb);
+    XInterpolator(
+        const std::vector<IRes>& vec,
+        const std::vector<std::tuple<double, double, double>>& rnb);
     Vect6 get_pos(const TimeJD& date, bool tks = false) const;
     TimeJD get_beg() const { return m_beg; }
     TimeJD get_end() const { return m_end; }
-    static void calc_positions(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, std::vector<IRes>& result);
+    static void calc_positions(
+        const SInitOrbit& orbit, 
+        const TimeJD& beg, 
+        const TimeJD& end,
+        bool calc_rnb,
+        std::vector<IRes>& xyz,
+        std::vector<std::tuple<double, double, double>>& rnb);
     void export_data() const;
     std::vector<IRes> m_positions;
+    std::vector<std::tuple<double, double, double>> m_rnb;
 private:
-    void create(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end);
-    void create(const std::vector<IRes>& positions);
-    static void propagate(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, int dir, std::vector<IRes>& result);
-    static void propagate_dir(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, int dir, std::vector<IRes>& result);
+    void create(const SInitOrbit& orbit, const TimeJD& beg, const TimeJD& end, bool calc_rnb);
+    void create(
+        const std::vector<IRes>& xyz,
+        const std::vector<std::tuple<double, double, double>>& rnb);
+    static void propagate(
+        const SInitOrbit& orbit,
+        const TimeJD& beg,
+        const TimeJD& end,
+        int dir,
+        const bool calc_rnb,
+        std::vector<IRes>& xyz,
+        std::vector<std::tuple<double, double, double>>& rnb);
+    static void propagate_dir(
+        const SInitOrbit& orbit,
+        const TimeJD& beg,
+        const TimeJD& end,
+        const int dir,
+        bool calc_rnb,
+        std::vector<IRes>& xyz,
+        std::vector<std::tuple<double, double, double>>& rnb);
     TimeJD m_beg;
     TimeJD m_end;
     ei m_ephi;
+    ei m_rnb_ephi;
 };

utils/orbBlock.cpp

@@ -53,14 +53,14 @@ OrbBlock::~OrbBlock()
     delete m_ip;
 }
 
-void OrbBlock::init_by_orbit()
+void OrbBlock::init_by_orbit(const bool calc_rnb)
 {
-    m_ip = new XInterpolator(m_orbit, get_beg(), get_end());
+    m_ip = new XInterpolator(m_orbit, get_beg(), get_end(), calc_rnb);
 }
 
 void OrbBlock::init_by_data()
 {
-    m_ip = new XInterpolator(m_data.get_vecs());
+    m_ip = new XInterpolator(m_data.get_vecs(), m_data.get_rnb());
 }
 
 void OrbBlock::load(const jsonval& parent)

utils/orbBlock.h

@@ -20,7 +20,7 @@ public:
     const SInitOrbit& get_orbit() const { return m_orbit; }
     double get_min_distance() const { return get_data().get_min_distance(); }
     std::string get_next_orbitid() const { return get_data().get_next_orbitid(); }
-    void init_by_orbit();
+    void init_by_orbit(bool calc_rnb);
     void init_by_data();
     const XInterpolator* get_interpolator() const { return m_ip; }
     void load(const jsonval& parent);

utils/orbBlocks.cpp

@@ -16,7 +16,9 @@ void OrbBlocksStore::init(
     const TimeJD* end,
     const bool filter_orbits,
     const bool min_dist_forced,
-    const bool interpolate)
+    const bool interpolate,
+    const bool calc_rnb
+    )
 {
     SInitOrbits sorted_orbits = orbits;
 
@@ -38,7 +40,7 @@ void OrbBlocksStore::init(
 
     if (interpolate) {
         for (auto& m_block : m_blocks) {
-            m_block.init_by_orbit();
+            m_block.init_by_orbit(calc_rnb);
         }
         m_initiated = true;
     }
@@ -50,9 +52,11 @@ void OrbBlocksStore::init(
     const TimeJD& end,
     const bool filter_orbits,
     const bool min_dist_forced,
-    const bool interpolate)
+    const bool interpolate,
+    const bool calc_rnb
+    )
 {
-    init(orbits, &beg, &end, filter_orbits, min_dist_forced, interpolate);
+    init(orbits, &beg, &end, filter_orbits, min_dist_forced, interpolate, calc_rnb);
 }
 
 void OrbBlocksStore::init()
@@ -224,8 +228,8 @@ private:
 
 void OrbBlocksStore::calc_min_dist(const SInitOrbit& orb1, const SInitOrbit& orb2, const TimeJD& beg, const TimeJD& end, TimeJD& result_jd, double& result_dist) const
 {
-    const XInterpolator ip1(orb1, beg, end);
-    const XInterpolator ip2(orb2, beg, end);
+    const XInterpolator ip1(orb1, beg, end, false);
+    const XInterpolator ip2(orb2, beg, end, false);
     MinDistEvaluator eval(ip1, ip2, beg);
 
     const double step = (int)(std::min(orb1.GetKeplerData().GetPeriod(), orb2.GetKeplerData().GetPeriod()) * 1000 / 20);

utils/orbBlocks.h

@@ -12,8 +12,8 @@ class OrbBlocksStore {
 public:
     OrbBlocksStore();
     OrbBlocksStore(const OrbBlocks& blocks);
-    void init(const SInitOrbits& orbits, const TimeJD* beg, const TimeJD* end, bool filter_orbits, bool min_dist_forced, bool interpolate);
-    void init(const SInitOrbits& orbits, const TimeJD& beg, const TimeJD& end, bool filter_orbits, bool min_dist_forced, bool interpolate);
+    void init(const SInitOrbits& orbits, const TimeJD* beg, const TimeJD* end, bool filter_orbits, bool min_dist_forced, bool interpolate, bool calc_rnb);
+    void init(const SInitOrbits& orbits, const TimeJD& beg, const TimeJD& end, bool filter_orbits, bool min_dist_forced, bool interpolate, bool calc_rnb);
     void init();
     SInitOrbits calc_orbits(std::vector<TimeJD> dates, bool variates) const;
     Vect6 get_pos(const TimeJD& date, bool tks = false) const;

utils/orb_block_data.h

@@ -37,6 +37,7 @@ public:
     const std::string& get_next_orbitid() const { return m_next_orbitid; }
     double get_min_distance() const { return m_mind; }
     const std::vector<IRes>& get_vecs() const { return m_vecs; }
+    const std::vector<std::tuple<double, double, double>>& get_rnb() const { return m_rnb; }
 private:
     std::string m_orbitid;
     std::string m_next_orbitid;
@@ -45,4 +46,5 @@ private:
     TimeJD m_end;
     double m_mind;
     std::vector<IRes> m_vecs;
+    std::vector<std::tuple<double, double, double>> m_rnb;
 };