WIP. Added templated RK4 solver and some more framework code

gui/MainWindow.cpp

@@ -82,15 +82,27 @@ void MainWindow::on_testButton2_clicked()
 
    double ts = 0.01;
 
-   sim::RK4Solver velXSolver([=](double x, double t) -> double { return 0.0; });
-   velXSolver.setTimeStep(ts);
-   sim::RK4Solver velYSolver([=](double y, double t) -> double { return -9.8; });
-   velYSolver.setTimeStep(ts);
+   // X position/velocity. x[0] is X position, x[1] is X velocity
+   double x[2] = {0.0, initialVelocityX};
 
-   sim::RK4Solver posXSolver([=](double x, double t) -> double { return velXSolver.step(x, t); });
-   posXSolver.setTimeStep(ts);
-   sim::RK4Solver posYSolver([=](double y, double t) -> double { return velYSolver.step(x, t); });
-   posYSolver.setTimeStep(ts);
+   // Y position/velocity. y[0] is Y position, y[1] is Y velocity
+   double y[2] = {0.0, initialVelocityY};
+
+   auto xvelODE = [mass, dragCoeff](double, double* x) -> double
+      {
+
+         return -dragCoeff * 1.225 * 0.00774192 / (2.0 * mass) * x[1]*x[1]; };
+   auto xposODE = [](double, double* x) -> double { return x[1]; };
+   sim::RK4Solver xSolver(xposODE, xvelODE);
+   xSolver.setTimeStep(0.01);
+
+   auto yvelODE = [mass, dragCoeff](double, double* y) -> double
+      {
+
+         return -dragCoeff * 1.225 * 0.00774192 / (2.0 * mass) * y[1]*y[1] - 9.8; };
+   auto yposODE = [](double, double* y) -> double { return y[1]; };
+   sim::RK4Solver ySolver(yposODE, yvelODE);
+   ySolver.setTimeStep(0.01);
 
 
    // These can be solved independently for now. Maybe figure out how to merge them later
@@ -98,33 +110,42 @@ void MainWindow::on_testButton2_clicked()
    QTextStream cout(stdout);
    cout << "Initial X velocity: " << initialVelocityX << "\n";
    cout << "Initial Y velocity: " << initialVelocityY << "\n";
+   double resX[2];
+   double resY[2];
    for(size_t i = 0; i < maxTs; ++i)
    {
-      position.emplace_back(posXSolver.step(position[i].getX1(), i * ts),
-                            posYSolver.step(position[i].getX2(), i * ts),
-                            0.0);
+      xSolver.step(i * ts, x, resX);
+      ySolver.step(i * ts, y, resY);
+      position.emplace_back(resX[0], resY[0], 0.0);
+
+      x[0] = resX[0];
+      x[1] = resX[1];
+      y[0] = resY[0];
+      y[1] = resY[1];
 
       cout << "(" << position[i].getX1() << ", " << position[i].getX2() << ")\n";
+      if(y[0] < 0.0)
+         break;
 
    }
-
    auto& plot = ui->plotWindow;
    // generate some data:
-   QVector<double> x(position.size()), y(position.size());
-   for (int i = 0; i < x.size(); ++i)
+   QVector<double> xData(position.size()), yData(position.size());
+   for (int i = 0; i < xData.size(); ++i)
    {
-     x[i] = position[i].getX1();
-     y[i] = position[i].getX2();
+     xData[i] = position[i].getX1();
+     yData[i] = position[i].getX2();
    }
    // create graph and assign data to it:
    plot->addGraph();
-   plot->graph(0)->setData(x, y);
+   plot->graph(0)->setData(xData, yData);
    // give the axes some labels:
    plot->xAxis->setLabel("x");
    plot->yAxis->setLabel("y");
    // set axes ranges, so we see all data:
-   plot->xAxis->setRange(*std::min_element(std::begin(x), std::end(x)), *std::max_element(std::begin(x), std::end(x)));
-   plot->yAxis->setRange(*std::min_element(std::begin(y), std::end(y)), *std::max_element(std::begin(y), std::end(y)));
+   plot->xAxis->setRange(*std::min_element(std::begin(xData), std::end(xData)), *std::max_element(std::begin(xData), std::end(xData)));
+   plot->yAxis->setRange(*std::min_element(std::begin(yData), std::end(yData)), *std::max_element(std::begin(yData), std::end(yData)));
    plot->replot();
+
 }
 

gui/MainWindow.ui

@@ -99,7 +99,7 @@
         <string>Calculate Ballistic Trajectory</string>
        </property>
       </widget>
-      <widget class="QWidget" name="">
+      <widget class="QWidget" name="layoutWidget">
        <property name="geometry">
         <rect>
          <x>260</x>
@@ -117,7 +117,11 @@
          </widget>
         </item>
         <item row="0" column="1">
-         <widget class="QLineEdit" name="initialVelocity"/>
+         <widget class="QLineEdit" name="initialVelocity">
+          <property name="text">
+           <string>500.0</string>
+          </property>
+         </widget>
         </item>
         <item row="1" column="0">
          <widget class="QLabel" name="label_2">
@@ -127,7 +131,11 @@
          </widget>
         </item>
         <item row="1" column="1">
-         <widget class="QLineEdit" name="initialAngle"/>
+         <widget class="QLineEdit" name="initialAngle">
+          <property name="text">
+           <string>45.0</string>
+          </property>
+         </widget>
         </item>
         <item row="2" column="0">
          <widget class="QLabel" name="label_3">
@@ -137,7 +145,11 @@
          </widget>
         </item>
         <item row="2" column="1">
-         <widget class="QLineEdit" name="mass"/>
+         <widget class="QLineEdit" name="mass">
+          <property name="text">
+           <string>1.0</string>
+          </property>
+         </widget>
         </item>
         <item row="3" column="0">
          <widget class="QLabel" name="label_4">
@@ -147,7 +159,11 @@
          </widget>
         </item>
         <item row="3" column="1">
-         <widget class="QLineEdit" name="dragCoeff"/>
+         <widget class="QLineEdit" name="dragCoeff">
+          <property name="text">
+           <string>0.5</string>
+          </property>
+         </widget>
         </item>
        </layout>
       </widget>

model/Rocket.cpp

@@ -0,0 +1,6 @@
+#include "Rocket.h"
+
+Rocket::Rocket()
+{
+
+}

model/Rocket.h

@@ -0,0 +1,43 @@
+#ifndef ROCKET_H
+#define ROCKET_H
+
+#include "utils/math/Vector3.h"
+#include "utils/math/Quaternion.h"
+
+#include "sim/Propagator.h"
+
+#include <utility> // std::move
+#include <memory>
+
+class Rocket
+{
+public:
+   Rocket();
+
+   void setPosition(const math::Vector3& pos) { *position = pos; }
+   void setPosition(math::Vector3&& pos) { *position = std::move(pos); }
+
+   void setVelocity(const math::Vector3& vel) { *velocity = vel; }
+   void setVelocity(math::Vector3&& vel) { *velocity = std::move(vel); }
+
+   void setOrientation(const math::Quaternion& ori) { *orientation = ori; }
+   void setOrientation(math::Quaternion&& ori) { *orientation = std::move(ori); }
+
+   void setOrientationRate(const math::Quaternion& ori) { *orientationRate = ori; }
+   void setOrientationRate(math::Quaternion&& ori) { *orientationRate = std::move(ori); }
+
+   const math::Vector3& getPosition() const { return *position; }
+   const math::Vector3& getVelocity() const { return *velocity; }
+   const math::Quaternion& getOrientation() const { return *orientation; }
+   const math::Quaternion& getOrientationRate() const { return *orientation; }
+private:
+
+   std::shared_ptr<math::Vector3> position;
+   std::shared_ptr<math::Vector3> velocity;
+   std::shared_ptr<math::Quaternion> orientation;
+   std::shared_ptr<math::Quaternion> orientationRate;
+
+   sim::Propagator propagator;
+};
+
+#endif // ROCKET_H

qtrocket.pro

@@ -17,11 +17,11 @@ SOURCES += \
     gui/MainWindow.cpp \
     model/Motor.cpp \
     model/MotorCase.cpp \
+    model/Rocket.cpp \
     model/Thrustcurve.cpp \
     sim/AtmosphericModel.cpp \
     sim/GravityModel.cpp \
     sim/Propagator.cpp \
-    sim/RK4Solver.cpp \
     sim/SphericalGeoidModel.cpp \
     sim/SphericalGravityModel.cpp \
     sim/StateData.cpp \
@@ -43,6 +43,7 @@ HEADERS += \
     gui/qcustomplot.h \
     model/Motor.h \
     model/MotorCase.h \
+    model/Rocket.h \
     model/Thrustcurve.h \
     sim/AtmosphericModel.h \
     sim/DESolver.h \

sim/DESolver.h

@@ -11,7 +11,7 @@ public:
    virtual ~DESolver() {}
 
    virtual void setTimeStep(double ts) = 0;
-   virtual double step(double curVal, double t) const = 0;
+   virtual void step(double t, double* curVal, double* res ) = 0;
 };
 
 } // namespace sim

sim/Propagator.h

@@ -1,6 +1,9 @@
 #ifndef SIM_PROPAGATOR_H
 #define SIM_PROPAGATOR_H
 
+#include "sim/DESolver.h"
+
+#include <memory>
 
 namespace sim
 {
@@ -11,6 +14,8 @@ public:
     Propagator();
 
 private:
+
+   std::unique_ptr<sim::DESolver> solver;
 };
 
 } // namespace sim

sim/RK4Solver.cpp

@@ -1,19 +0,0 @@
-#include "RK4Solver.h"
-
-namespace sim {
-
-double RK4Solver::step(double curVal, double t) const
-{
-   double retVal(0.0);
-
-   double k1 = de(curVal, t);
-   double k2 = de(curVal + (dt * k1 / 2.0), t + dt / 2.0);
-   double k3 = de(curVal + (dt * k2 / 2.0), t + dt / 2.0);
-   double k4 = de(curVal + dt * k3, t + dt);
-   retVal = curVal + (1.0 / 6.0)*dt*(k1 + 2.0*k2 + 2.0*k3 + k4);
-
-   return retVal;
-}
-
-
-} // namespace sim

sim/RK4Solver.h

@@ -1,27 +1,93 @@
 #ifndef SIM_RK4SOLVER_H
 #define SIM_RK4SOLVER_H
 
-#include "DESolver.h"
-
 #include <functional>
+#include <vector>
+#include <limits>
+#include <cmath>
+
+#include "utils/Logger.h"
+#include "sim/DESolver.h"
 
 namespace sim {
 
+template<typename... Ts>
 class RK4Solver : public DESolver
 {
 public:
-   RK4Solver(std::function<double(double, double)> d) : de(d) {}
+
+   RK4Solver(Ts... funcs)
+   {
+      (odes.push_back(funcs), ...);
+
+   }
    virtual ~RK4Solver() {}
 
-   void setTimeStep(double inTs) override { dt = inTs; }
-   double step(double curVal, double t) const override;
+   void setTimeStep(double inTs) override { dt = inTs;  halfDT = dt / 2.0; }
+
+   void step(double t, double* curVal, double* res) override
+   {
+      if(dt == std::numeric_limits<double>::quiet_NaN())
+      {
+         utils::Logger::getInstance()->error("Calling RK4Solver without setting dt first is an error");
+         res[0] = std::numeric_limits<double>::quiet_NaN();
+      }
+
+      for(size_t i = 0; i < len; ++i)
+      {
+         k1[i] = odes[i](t, curVal);
+      }
+      // compute k2 values. This involves stepping the current values forward a half-step
+      // based on k1, so we do the stepping first
+      for(size_t i = 0; i < len; ++i)
+      {
+         temp[i] = curVal[i] + k1[i]*dt / 2.0;
+      }
+      for(size_t i = 0; i < len; ++i)
+      {
+         k2[i] = odes[i](t + halfDT, temp);
+      }
+      // repeat for k3
+      for(size_t i = 0; i < len; ++i)
+      {
+         temp[i] = curVal[i] + k2[i]*dt / 2.0;
+      }
+      for(size_t i = 0; i < len; ++i)
+      {
+         k3[i] = odes[i](t + halfDT, temp);
+      }
+
+      // now k4
+      for(size_t i = 0; i < len; ++i)
+      {
+         temp[i] = curVal[i] + k3[i]*dt;
+      }
+      for(size_t i = 0; i < len; ++i)
+      {
+         k4[i] = odes[i](t + dt, temp);
+      }
+
+      // now compute the result
+      for(size_t i = 0; i < len; ++i)
+      {
+         res[i] = curVal[i] + (dt / 6.0)*(k1[i] + 2.0*k2[i] + 2.0*k3[i] + k4[i]);
+      }
+
+   }
 
 private:
-   std::function<double(double, double)> de;
+   std::vector<std::function<double(double, double*)>> odes;
 
-   //double k1, k2, k3, k4;
+   static constexpr size_t len = sizeof...(Ts);
+   double k1[len];
+   double k2[len];
+   double k3[len];
+   double k4[len];
 
-   double dt;
+   double temp[len];
+
+   double dt = std::numeric_limits<double>::quiet_NaN();
+   double halfDT = 0.0;
 
 };
 

sim/StateData.h

@@ -2,6 +2,7 @@
 #define STATEDATA_H
 
 #include "utils/math/Vector3.h"
+#include "utils/math/Quaternion.h"
 
 /**
  * @brief The StateData class holds physical state data. Things such as position, velocity,
@@ -14,12 +15,11 @@ public:
    StateData();
 
 private:
-   math::Vector3 position;
-   math::Vector3 velocity;
-   math::Vector3 acceleration;
+   math::Vector3 position{0.0, 0.0, 0.0};
+   math::Vector3 velocity{0.0, 0.0, 0.0};
 
-   math::Vector3 orientation; // roll, pitch, yaw
-   math::Vector3 orientationVelocity; // roll-rate, pitch-rate, yaw-rate
+   math::Quaternion orientation{0.0, 0.0, 0.0, 0.0}; // roll, pitch, yaw
+   math::Quaternion orientationRate{0.0, 0.0, 0.0, 0.0}; // roll-rate, pitch-rate, yaw-rate
    // Necessary?
    //math::Vector3 orientationAccel;
 

utils/math/Quaternion.cpp

@@ -8,4 +8,16 @@ Quaternion::Quaternion()
 
 }
 
+Quaternion::Quaternion(double r, const Vector3& i)
+   : real(r),
+     imag(i)
+{
+}
+
+Quaternion::Quaternion(double r, double i1, double i2, double i3)
+   : real(r),
+     imag(i1, i2, i3)
+{
+}
+
 } // namespace math

utils/math/Quaternion.h

@@ -3,6 +3,8 @@
 
 #include "Vector3.h"
 
+#include <utility>
+
 namespace math
 {
 
@@ -10,17 +12,38 @@ class Quaternion
 {
 public:
     Quaternion();
+    ~Quaternion() {}
 
     Quaternion(double r, const Vector3& i);
     Quaternion(double r, double i1, double i2, double i3);
 
-    ~Quaternion();
+    Quaternion(const Quaternion&) = default;
+    Quaternion(Quaternion&&) = default;
 
-    Quaternion operator-();
-    Quaternion operator-(const Quaternion& rhs);
-    Quaternion operator+(const Quaternion& rhs);
-    Quaternion operator*(double s);
-    Quaternion operator*(const Quaternion& rhs);
+    Quaternion& operator=(const Quaternion& rhs)
+    {
+        if(this == &rhs)
+           return *this;
+        real = rhs.real;
+        imag = rhs.imag;
+        return *this;
+    }
+    Quaternion& operator=(Quaternion&& rhs)
+    {
+        if(this == &rhs)
+           return *this;
+        real = std::move(rhs.real);
+        imag = std::move(rhs.imag);
+        return *this;
+    }
+
+    /*
+    Quaternion operator-() {}
+    Quaternion operator-(const Quaternion& ) {}
+    Quaternion operator+(const Quaternion& ) {}
+    Quaternion operator*(double ) {}
+    Quaternion operator*(const Quaternion& ) {}
+    */
 
 private:
     double real;