From 0734d6a013ca30b25c3bf384e70e7aac92b49164 Mon Sep 17 00:00:00 2001
From: Travis Hunter <travis@travishunter.io>
Date: Mon, 10 Apr 2023 19:19:11 -0600
Subject: [PATCH] improvements to propagator getForceXYZ methods. The
 propagator now reaches out to the QtRocket singleton for current gravity data
 at each timestep. Can easily do the same for atmospheric density next

---
 QtRocket.h                 |  3 +++
 gui/MainWindow.cpp         |  6 +++---
 sim/ConstantGravityModel.h |  2 +-
 sim/Propagator.cpp         | 25 ++++++++++++++++++++-----
 sim/Propagator.h           |  2 ++
 5 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/QtRocket.h b/QtRocket.h
index fbdd6e0..0a388b2 100644
--- a/QtRocket.h
+++ b/QtRocket.h
@@ -30,6 +30,9 @@ public:
 
    void runSim();
 
+   std::shared_ptr<sim::GravityModel> getGravityModel() { return gravity; }
+   std::shared_ptr<sim::AtmosphericModel> getAtmosphereModel() { return atmosphere; }
+
 private:
    QtRocket();
 
diff --git a/gui/MainWindow.cpp b/gui/MainWindow.cpp
index fa13924..a1bc241 100644
--- a/gui/MainWindow.cpp
+++ b/gui/MainWindow.cpp
@@ -74,9 +74,9 @@ void MainWindow::on_testButton2_clicked()
             ui->rocketPartButtons->findChild<QLineEdit*>(QString("dragCoeff"))->text().toDouble();
 
    double initialVelocityX = initialVelocity * std::cos(initialAngle / 57.2958);
-   double initialVelocityY = initialVelocity * std::sin(initialAngle / 57.2958);
+   double initialVelocityZ = initialVelocity * std::sin(initialAngle / 57.2958);
    Rocket rocket;
-   std::vector<double> initialState = {0.0, 0.0, 0.0, initialVelocityX, initialVelocityY, 0.0};
+   std::vector<double> initialState = {0.0, 0.0, 0.0, initialVelocityX, 0.0, initialVelocityZ};
    rocket.setInitialState(initialState);
    rocket.setMass(mass);
    rocket.setDragCoefficient(dragCoeff);
@@ -89,7 +89,7 @@ void MainWindow::on_testButton2_clicked()
    for (int i = 0; i < xData.size(); ++i)
    {
      xData[i] = res[i][0];
-     yData[i] = res[i][1];
+     yData[i] = res[i][2];
    }
    // create graph and assign data to it:
    plot->addGraph();
diff --git a/sim/ConstantGravityModel.h b/sim/ConstantGravityModel.h
index 1774884..3252c39 100644
--- a/sim/ConstantGravityModel.h
+++ b/sim/ConstantGravityModel.h
@@ -12,7 +12,7 @@ public:
 
     virtual ~ConstantGravityModel() {}
 
-    std::tuple<double, double, double> getAccel(double, double, double) override { return std::make_tuple(0.0, 0.0, 9.8); }
+    std::tuple<double, double, double> getAccel(double, double, double) override { return std::make_tuple(0.0, 0.0, -9.8); }
 };
 
 } // namespace sim
diff --git a/sim/Propagator.cpp b/sim/Propagator.cpp
index 7d33d33..79b3b79 100644
--- a/sim/Propagator.cpp
+++ b/sim/Propagator.cpp
@@ -3,6 +3,7 @@
 
 #include "sim/RK4Solver.h"
 #include "model/Rocket.h"
+#include "QtRocket.h"
 
 #include <utility>
 #include <QTextStream>
@@ -11,7 +12,8 @@ namespace sim {
 
 Propagator::Propagator(Rocket* r)
    : integrator(),
-     rocket(r)
+     rocket(r),
+     qtrocket(QtRocket::getInstance())
 
 {
 
@@ -71,7 +73,7 @@ void Propagator::runUntilTerminate()
           << currentState[3] << ", "
           << currentState[4] << ", "
           << currentState[5] << ")\n";
-      if(currentState[1] < 0.0)
+      if(currentState[2] < 0.0)
          break;
 
        j++;
@@ -84,9 +86,22 @@ double Propagator::getMass()
     return rocket->getMass();
 }
 
-double Propagator::getForceX() { return -1.225 / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[3]* currentState[3]; }
-double Propagator::getForceY() { return -1.225 / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[4]* currentState[4] -9.8; }
-double Propagator::getForceZ() { return 0; }
+double Propagator::getForceX()
+{
+    return -1.225 / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[3]* currentState[3];
+}
+
+double Propagator::getForceY()
+{
+    return -1.225 / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[4]* currentState[4];
+}
+
+double Propagator::getForceZ()
+{
+    double gravity = std::get<2>(qtrocket->getGravityModel()->getAccel(currentState[0], currentState[1], currentState[2]));
+    double airDrag = -1.225 / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[5]* currentState[5];
+    return gravity + airDrag;
+}
 
 double Propagator::getTorqueP() { return 0.0; }
 double Propagator::getTorqueQ() { return 0.0; }
diff --git a/sim/Propagator.h b/sim/Propagator.h
index 0d40799..8ee9dc5 100644
--- a/sim/Propagator.h
+++ b/sim/Propagator.h
@@ -8,6 +8,7 @@
 
 // Forward declare
 class Rocket;
+class QtRocket;
 
 namespace sim
 {
@@ -62,6 +63,7 @@ private:
    std::unique_ptr<sim::DESolver> integrator;
 
    Rocket* rocket;
+   QtRocket* qtrocket;
 
    std::vector<double> currentState{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    std::vector<double> tempRes{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};