Substantial refactor and cleanup of Propagator and Rocket class (now RocketModel class)

QtRocket.cpp

@@ -80,7 +80,7 @@ QtRocket::QtRocket()
    setEnvironment(std::make_shared<sim::Environment>());
 
    rocket.first =
-      std::make_shared<model::Rocket>();
+      std::make_shared<model::RocketModel>();
    
    rocket.second =
       std::make_shared<sim::Propagator>(rocket.first);
@@ -113,7 +113,7 @@ int QtRocket::run(int argc, char* argv[])
 void QtRocket::launchRocket()
 {
    // initialize the propagator
-   rocket.second->clearStates();
+   rocket.first->clearStates();
    rocket.second->setCurrentTime(0.0);
 
    // start the rocket motor
@@ -128,8 +128,3 @@ void QtRocket::addMotorModels(std::vector<model::MotorModel>& m)
    motorDatabase->addMotorModels(m);
    // TODO: Now clear any duplicates?
 }
-
-void QtRocket::appendState(const StateData& state)
-{
-    states.emplace_back(state);
-}

QtRocket.h

@@ -14,7 +14,7 @@
 
 // qtrocket headers
 #include "model/MotorModel.h"
-#include "model/Rocket.h"
+#include "model/RocketModel.h"
 #include "sim/Environment.h"
 #include "sim/Propagator.h"
 #include "utils/Logger.h"
@@ -43,13 +43,13 @@ public:
 
    std::shared_ptr<sim::Environment> getEnvironment() { return environment; }
    void setTimeStep(double t) { rocket.second->setTimeStep(t); }
-   std::shared_ptr<model::Rocket> getRocket() { return rocket.first; }
+   std::shared_ptr<model::RocketModel> getRocket() { return rocket.first; }
 
    std::shared_ptr<utils::MotorModelDatabase> getMotorDatabase() { return motorDatabase; }
 
    void addMotorModels(std::vector<model::MotorModel>& m);
 
-   void addRocket(std::shared_ptr<model::Rocket> r) { rocket.first = r; }
+   void addRocket(std::shared_ptr<model::RocketModel> r) { rocket.first = r; rocket.second = std::make_shared<sim::Propagator>(r); }
 
    void setEnvironment(std::shared_ptr<sim::Environment> e) { environment = e; }
 
@@ -58,15 +58,13 @@ public:
     * @brief getStates returns a vector of time/state pairs generated during launch()
     * @return vector of pairs of doubles, where the first value is a time and the second a state vector
     */
-   const std::vector<std::pair<double, StateData>>& getStates() const { return rocket.second->getStates(); }
+   const std::vector<std::pair<double, StateData>>& getStates() const { return rocket.first->getStates(); }
 
    /**
     * @brief setInitialState sets the initial state of the Rocket.
     * @param initState initial state vector (x, y, z, xDot, yDot, zDot, pitch, yaw, roll, pitchDot, yawDot, rollDot)
     */
-   void setInitialState(const StateData& initState) { rocket.second->setInitialState(initState); }
+   void setInitialState(const StateData& initState) { rocket.first->setInitialState(initState); }
-
-   void appendState(const StateData& state);
 
 private:
    QtRocket();
@@ -80,7 +78,8 @@ private:
 
    utils::Logger* logger;
 
-   std::pair<std::shared_ptr<model::Rocket>, std::shared_ptr<sim::Propagator>> rocket;
+   using Rocket = std::pair<std::shared_ptr<model::RocketModel>, std::shared_ptr<sim::Propagator>>;
+   Rocket rocket;
 
    std::shared_ptr<sim::Environment> environment;
    std::shared_ptr<utils::MotorModelDatabase> motorDatabase;
@@ -92,8 +91,6 @@ private:
    // Table of state data
    std::vector<StateData> states;
 
-
-
 };
 
 #endif // QTROCKET_H

gui/AnalysisWindow.cpp

@@ -91,7 +91,7 @@ void AnalysisWindow::onButton_plotVelocity_clicked()
 
 void AnalysisWindow::onButton_plotMotorCurve_clicked()
 {
-   std::shared_ptr<model::Rocket> rocket = QtRocket::getInstance()->getRocket();
+   std::shared_ptr<model::RocketModel> rocket = QtRocket::getInstance()->getRocket();
    model::MotorModel motor = rocket->getMotorModel();
    ThrustCurve tc = motor.getThrustCurve();
 

gui/MainWindow.cpp

@@ -20,7 +20,7 @@
 #include "gui/MainWindow.h"
 #include "gui/ThrustCurveMotorSelector.h"
 #include "gui/SimOptionsWindow.h"
-#include "model/Rocket.h"
+#include "model/RocketModel.h"
 #include "utils/RSEDatabaseLoader.h"
 
 

model/CMakeLists.txt

@@ -7,8 +7,8 @@ add_library(model
    Part.h
    Propagatable.cpp
    Propagatable.h
-   Rocket.cpp
+   RocketModel.cpp
-   Rocket.h
+   RocketModel.h
    ThrustCurve.cpp
    ThrustCurve.h
    InertiaTensors.h)

model/Propagatable.h

@@ -30,11 +30,27 @@ public:
 
    virtual bool terminateCondition(double t) = 0;
 
+   void setCurrentState(const StateData& st) { currentState = st; }
+   const StateData& getCurrentState() { return currentState; }
+
+   const StateData& getInitialState() { return initialState; }
+   void setInitialState(const StateData& init) { initialState = init; }
+
+   void appendState(double t, const StateData& st) { states.emplace_back(t, st); }
+
+   const std::vector<std::pair<double, StateData>>& getStates() { return states; }
+
+   void clearStates() { states.clear(); }
+
 protected:
 
    sim::Aero aeroData;
 
-   StateData state;
+   StateData initialState;
+   StateData currentState;
+   StateData nextState;
+
+   std::vector<std::pair<double, StateData>> states;
 };
 
 }

model/RocketModel.cpp

@@ -1,41 +1,41 @@
 
 // qtrocket headers
-#include "Rocket.h"
+#include "RocketModel.h"
 #include "QtRocket.h"
 #include "InertiaTensors.h"
 
 namespace model
-{ 
+{
 
-Rocket::Rocket()
+RocketModel::RocketModel()
     : topPart("NoseCone", InertiaTensors::SolidSphere(1.0), 1.0, {0.0, 0.0, 1.0})
 {
 
 }
 
 
-double Rocket::getMass(double t)
+double RocketModel::getMass(double t)
 {
     double mass = mm.getMass(t);
     mass += topPart.getCompositeMass(t);
     return mass;
 }
 
-Matrix3 Rocket::getInertiaTensor(double)
+Matrix3 RocketModel::getInertiaTensor(double)
 {
     return topPart.getCompositeI();
 }
 
-bool Rocket::terminateCondition(double)
+bool RocketModel::terminateCondition(double)
 {
    // Terminate propagation when the z coordinate drops below zero
-    if(state.position[2] < 0)
+    if(currentState.position[2] < 0)
         return true;
     else
         return false;
 }
 
-Vector3 Rocket::getForces(double t)
+Vector3 RocketModel::getForces(double t)
 {
     // Get thrust
     // Assume that thrust is always through the center of mass and in the rocket's Z-axis
@@ -45,7 +45,7 @@ Vector3 Rocket::getForces(double t)
     // Get gravity
     auto gravityModel = QtRocket::getInstance()->getEnvironment()->getGravityModel();
 
-    Vector3 gravity = gravityModel->getAccel(state.position)*getMass(t);
+    Vector3 gravity = gravityModel->getAccel(currentState.position)*getMass(t);
 
     forces += gravity;
 
@@ -55,23 +55,23 @@ Vector3 Rocket::getForces(double t)
     return forces;
 }
 
-Vector3 Rocket::getTorques(double t)
+Vector3 RocketModel::getTorques(double t)
 {
     return Vector3{0.0, 0.0, 0.0};
 
 }
 
-double Rocket::getThrust(double t)
+double RocketModel::getThrust(double t)
 {
    return mm.getThrust(t);
 }
 
-void Rocket::launch()
+void RocketModel::launch()
 {
    mm.startMotor(0.0);
 }
 
-void Rocket::setMotorModel(const model::MotorModel& motor)
+void RocketModel::setMotorModel(const model::MotorModel& motor)
 {
    mm = motor;
 }

model/RocketModel.h

@@ -1,5 +1,5 @@
-#ifndef ROCKET_H
+#ifndef ROCKETMODEL_H
-#define ROCKET_H
+#define ROCKETMODEL_H
 
 /// \cond
 // C headers
@@ -28,19 +28,19 @@ namespace model
  * @brief The Rocket class holds all rocket components
  *
  */
-class Rocket : public Propagatable
+class RocketModel : public Propagatable
 {
 public:
     /**
     * @brief Rocket class constructor
     */
-   Rocket();
+   RocketModel();
 
    /**
     * @brief Rocket class destructor
     * 
     */
-   virtual ~Rocket() {}
+   virtual ~RocketModel() {}
 
     /**
     * @brief launch Propagates the Rocket object until termination,
@@ -113,4 +113,4 @@ private:
 };
 
 } // namespace model
-#endif // ROCKET_H
+#endif // ROCKETMODEL_H

sim/Propagator.cpp

@@ -13,121 +13,36 @@
 // qtrocket headers
 #include "Propagator.h"
 
-#include "QtRocket.h"
-#include "model/Rocket.h"
 #include "sim/RK4Solver.h"
 #include "utils/Logger.h"
 
-
+namespace sim
-namespace sim {
-
-Propagator::Propagator(std::shared_ptr<model::Rocket> r)
-   : linearIntegrator(),
-     //orientationIntegrator(),
-     object(r),
-     currentState(),
-     nextState(),
-     currentGravity(),
-     currentWindSpeed(),
-     saveStates(true),
-     currentTime(0.0),
-     timeStep(0.01),
-     states()
 {
 
+Propagator::Propagator(std::shared_ptr<model::Propagatable> r)
+   : linearIntegrator(),
+     object(r),
+     saveStates(true),
+     timeStep(0.01)
+{
+    // Linear velocity and acceleration
+    std::function<std::pair<Vector3, Vector3>(Vector3&, Vector3&)> linearODEs = [this](Vector3& state, Vector3& rate) -> std::pair<Vector3, Vector3>
+    {
+        Vector3 dPosition;
+        Vector3 dVelocity;
+        // dx/dt
+        dPosition = rate;
 
-   // This is a little strange, but I have to populate the integrator unique_ptr
+        // dvx/dt
-   // with reset. make_unique() doesn't work because the compiler can't seem to
+        dVelocity = object->getForces(currentTime) / object->getMass(currentTime);
-   // deduce the template parameters correctly, and I don't want to specify them
-   // manually either. RK4Solver constructor is perfectly capable of deducing it's
-   // template types, and it derives from DESolver, so we can just reset the unique_ptr
-   // and pass it a freshly allocated RK4Solver pointer
 
-   // The state vector has components of the form:
+        return std::make_pair(dPosition, dVelocity);
+    };
 
-   // Linear velocity and acceleration
+    linearIntegrator.reset(new RK4Solver<Vector3>(linearODEs));
-   std::function<std::pair<Vector3, Vector3>(Vector3&, Vector3&)> linearODE = [this](Vector3& state, Vector3& rate) -> std::pair<Vector3, Vector3>
+    linearIntegrator->setTimeStep(timeStep);
-   {
-      Vector3 dPosition;
-      Vector3 dVelocity;
-      // dx/dt
-      dPosition[0] = rate[0];
 
-      // dy/dt
+    saveStates = true;
-      dPosition[1] = rate[1];
-
-      // dz/dt
-      dPosition[2] = rate[2];
-
-      // dvx/dt
-      dVelocity[0] = getForceX() / getMass();
-
-      // dvy/dt
-      dVelocity[1] = getForceY() / getMass();
-
-      // dvz/dt
-      dVelocity[2] = getForceZ() / getMass();
-
-      return std::make_pair(dPosition, dVelocity);
-
-   };
-
-   linearIntegrator.reset(new RK4Solver<Vector3>(linearODE));
-   linearIntegrator->setTimeStep(timeStep);
-   
-   // This is pure quaternion
-   // This formula is taken from https://www.vectornav.com/resources/inertial-navigation-primer/math-fundamentals/math-attitudetran
-   //
-   // Convention for the quaternions is (vector, scalar). So q4 is scalar term
-   //
-   // q1Dot = 1/2 * [(q4*omega1) - (q3*omega2) + (q2*omega3)]
-   // q2Dot = 1/2 * [(q3*omega1) + (q4*omega2) - (q1*omega3)]
-   // q3Dot = 1/2 * [(-q2*omega1) + (q1*omega2) + (q4*omega3)]
-   // q4Dot = -1/2 *[(q1*omega1) + (q2*omega2) + (q3*omega3)]
-   //
-   // omega1 = yawRate
-   // omega2 = pitchRate
-   // omega3 = rollRate
-   //
-//   orientationIntegrator.reset(new RK4Solver(
-//       /* dyawRate/dt    */ [this](const std::vector<double>& s, double) -> double
-//       { return getTorqueP() / getIyaw(); },
-//       /* dpitchRate/dt  */ [this](const std::vector<double>& s, double) -> double
-//       { return getTorqueQ() / getIpitch(); },
-//       /* drollRate/dt   */ [this](const std::vector<double>& s, double) -> double
-//       { return getTorqueR() / getIroll(); },
-//       /* q1Dot */ [](const std::vector<double>& s, double) -> double
-//       {
-//           double retVal = s[6]*s[0] - s[5]*s[1] + s[4]*s[2];
-//           return 0.5 * retVal;
-//       },
-//       /* q2Dot */ [](const std::vector<double>& s, double) -> double
-//       {
-//           double retVal = s[5]*s[0] + s[6]*s[1] - s[3]*s[2];
-//           return 0.5 * retVal;
-//       },
-//       /* q3Dot */ [](const std::vector<double>& s, double) -> double
-//       {
-//           double retVal = -s[4]*s[0] + s[3]*s[1] + s[6]*s[2];
-//           return 0.5 * retVal;
-//       },
-//       /* q4Dot */ [](const std::vector<double>& s, double) -> double
-//       {
-//           double retVal = s[3]*s[0] + s[4]*s[1] + s[5]*s[2];
-//           return -0.5 * retVal;
-//       }));
-//   orientationIntegrator->setTimeStep(timeStep);
-
-//  std::function<std::pair<Quaternion, Quaternion>(Quaternion&, Quaternion&)> orientationODE =
-//  [this](Quaternion& qOri, Quaternion& qRate) -> std::pair<Quaternion, Quaternion>
-//  {
-//     Quaternion dOri;
-//     Quaternion dOriRate;
-
-//     Matrix4
-//  }
-
-  saveStates = true;
 }
 
 Propagator::~Propagator()
@@ -136,89 +51,40 @@ Propagator::~Propagator()
 
 void Propagator::runUntilTerminate()
 {
-   std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
+    std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();
-   std::chrono::steady_clock::time_point endTime;
+    std::chrono::steady_clock::time_point endTime;
 
-   currentState.position = {0.0, 0.0, 0.0};
+    Vector3 currentPosition;
-   while(true)
+    Vector3 currentVelocity;
-   {
+    Vector3 nextPosition;
+    Vector3 nextVelocity;
+    while(true)
+    {
+        currentPosition = object->getCurrentState().position;
+        currentVelocity = object->getCurrentState().velocity;
 
-      // tempRes gets overwritten
+        std::tie(nextPosition, nextVelocity) = linearIntegrator->step(currentPosition, currentVelocity);
-      std::tie(nextState.position, nextState.velocity) = linearIntegrator->step(currentState.position, currentState.velocity);
 
-      //tempRes = linearIntegrator->step(currentBodyState);
+        StateData nextState;
+        nextState.position = nextPosition;
+        nextState.velocity = nextVelocity;
+        object->setCurrentState(nextState);
 
-      
+        if(saveStates)
-      if(saveStates)
+        {
-      {
+            object->appendState(currentTime, nextState);
-         states.push_back(std::make_pair(currentTime, nextState));
+        }
-      }
+        if(object->terminateCondition(currentTime))
-      if(object->terminateCondition(currentTime))
+            break;
-         break;
 
-      currentTime += timeStep;
+        currentTime += timeStep;
-      currentState = nextState;
+    }
-   }
+    endTime = std::chrono::steady_clock::now();
-   endTime = std::chrono::steady_clock::now();
-
-   std::stringstream duration;
-   duration << "runUntilTerminate time (microseconds): ";
-   duration << std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTime).count();
-   utils::Logger::getInstance()->debug(duration.str());
-
-}
-
-double Propagator::getMass()
-{
-    return object->getMass(currentTime);
-}
-
-double Propagator::getForceX()
-{
-    return 0.0;
-    //QtRocket* qtrocket = QtRocket::getInstance();
-    //return (currentState.velocity[0] >= 0 ? -1.0 : 1.0) *  qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2])/ 2.0 * 0.008107 * object->getDragCoefficient() * currentState.velocity[0]* currentState.velocity[0];
-}
-
-double Propagator::getForceY()
-{
-    return 0.0;
-    //QtRocket* qtrocket = QtRocket::getInstance();
-    //return (currentState.velocity[1] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2]) / 2.0 * 0.008107 * object->getDragCoefficient() * currentState.velocity[1]* currentState.velocity[1];
-}
-
-double Propagator::getForceZ()
-{
-    return 0.0;
-    //QtRocket* qtrocket = QtRocket::getInstance();
-    //double gravity = (qtrocket->getEnvironment()->getGravityModel()->getAccel(currentState.position[0], currentState.position[1], currentState.position[2]))[2];
-    //double airDrag = (currentState.velocity[2] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState.position[2]) / 2.0 * 0.008107 * object->getDragCoefficient() * currentState.velocity[2]* currentState.velocity[2];
-    //double thrust  = object->getThrust(currentTime);
-    //return gravity + airDrag + thrust;
-}
-
-double Propagator::getTorqueP() { return 0.0; }
-double Propagator::getTorqueQ() { return 0.0; }
-double Propagator::getTorqueR() { return 0.0; }
-
-Vector3 Propagator::getCurrentGravity()
-{
-   auto gravityModel = QtRocket::getInstance()->getEnvironment()->getGravityModel();
-   auto gravityAccel = gravityModel->getAccel(currentState.position[0],
-                                              currentState.position[1],
-                                              currentState.position[2]);
-   Vector3 gravityVector{gravityAccel[0],
-                         gravityAccel[1],
-                         gravityAccel[2]};
-   
-
-   Quaternion q = currentState.orientation;
-
-   Vector3 res = q * gravityVector;
-   
-
-   return Vector3{0.0, 0.0, 0.0};
 
+    std::stringstream duration;
+    duration << "runUntilTerminate time (microseconds): ";
+    duration << std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTime).count();
+    utils::Logger::getInstance()->debug(duration.str());
 
 }
 

sim/Propagator.h

@@ -5,7 +5,6 @@
 // C headers
 // C++ headers
 #include <memory>
-#include <vector>
 
 // 3rd party headers
 /// \endcond
@@ -30,17 +29,17 @@ namespace sim
 class Propagator
 {
 public:
-    Propagator(std::shared_ptr<model::Rocket> r);
+    Propagator(std::shared_ptr<model::Propagatable> o);
     ~Propagator();
 
     void setInitialState(const StateData& initialState)
     {
-           currentState = initialState;
+        object->setInitialState(initialState);
     }
 
     const StateData& getCurrentState() const
     {
-       return currentState;
+        return object->getCurrentState();
     }
 
     void runUntilTerminate();
@@ -50,44 +49,21 @@ public:
        saveStates = s;
     }
 
-    const std::vector<std::pair<double, StateData>>& getStates() const { return states; }
-
-    void clearStates() { states.clear(); }
     void setCurrentTime(double t) { currentTime = t; }
     void setTimeStep(double ts) { timeStep = ts; }
     void setSaveStats(bool s) { saveStates = s; }
 
 private:
-   double getMass();
-   double getForceX();
-   double getForceY();
-   double getForceZ();
-
-   double getTorqueP(); // yaw
-   double getTorqueQ(); // pitch
-   double getTorqueR(); // roll
-
-   double getIyaw()   { return 1.0; }
-   double getIpitch() { return 1.0; }
-   double getIroll()  { return 1.0; }
 
    std::unique_ptr<sim::RK4Solver<Vector3>> linearIntegrator;
 //   std::unique_ptr<sim::RK4Solver<Quaternion>> orientationIntegrator;
 
    std::shared_ptr<model::Propagatable> object;
 
-   StateData currentState;
-   StateData nextState;
-
-   Vector3 currentGravity{0.0, 0.0, 0.0};
-   Vector3 currentWindSpeed{0.0, 0.0, 0.0};
-
    bool saveStates{true};
    double currentTime{0.0};
    double timeStep{0.01};
-   std::vector<std::pair<double, StateData>> states;
 
-   Vector3 getCurrentGravity();
 };
 
 } // namespace sim