diff --git a/gui/MainWindow.cpp b/gui/MainWindow.cpp index 9c0e1b3..fa13924 100644 --- a/gui/MainWindow.cpp +++ b/gui/MainWindow.cpp @@ -4,6 +4,7 @@ #include "utils/math/Vector3.h" #include "sim/RK4Solver.h" +#include "model/Rocket.h" #include @@ -74,66 +75,21 @@ void MainWindow::on_testButton2_clicked() double initialVelocityX = initialVelocity * std::cos(initialAngle / 57.2958); double initialVelocityY = initialVelocity * std::sin(initialAngle / 57.2958); - math::Vector3 initialVelVector(initialVelocityX, initialVelocityY, 0.0); - std::vector position; - position.emplace_back(0.0, 0.0, 0.0); + Rocket rocket; + std::vector initialState = {0.0, 0.0, 0.0, initialVelocityX, initialVelocityY, 0.0}; + rocket.setInitialState(initialState); + rocket.setMass(mass); + rocket.setDragCoefficient(dragCoeff); + rocket.launch(); - std::vector velocity; - velocity.push_back(initialVelVector); - - double ts = 0.01; - - // X position/velocity. x[0] is X position, x[1] is X velocity - std::vector x = {0.0, initialVelocityX}; - - // Y position/velocity. y[0] is Y position, y[1] is Y velocity - std::vector y = {0.0, initialVelocityY}; - - auto xvelODE = [mass, dragCoeff](double, const std::vector& x) -> double - { - - return -dragCoeff * 1.225 * 0.00774192 / (2.0 * mass) * x[1]*x[1]; }; - auto xposODE = [](double, const std::vector& x) -> double { return x[1]; }; - sim::RK4Solver xSolver(xposODE, xvelODE); - xSolver.setTimeStep(0.01); - - auto yvelODE = [mass, dragCoeff](double, const std::vector& y) -> double - { - - return -dragCoeff * 1.225 * 0.00774192 / (2.0 * mass) * y[1]*y[1] - 9.8; }; - auto yposODE = [](double, const std::vector& 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 - size_t maxTs = std::ceil(100.0 / ts); - QTextStream cout(stdout); - cout << "Initial X velocity: " << initialVelocityX << "\n"; - cout << "Initial Y velocity: " << initialVelocityY << "\n"; - std::vector resX(2); - std::vector resY(2); - for(size_t i = 0; i < maxTs; ++i) - { - xSolver.step(i * ts, x, resX); - ySolver.step(i * ts, y, resY); - position.emplace_back(resX[0], resY[0], 0.0); - - x = resX; - y = resY; - - cout << "(" << position[i].getX1() << ", " << position[i].getX2() << ")\n"; - if(y[0] < 0.0) - break; - - } + const std::vector>& res = rocket.getStates(); auto& plot = ui->plotWindow; // generate some data: - QVector xData(position.size()), yData(position.size()); + QVector xData(res.size()), yData(res.size()); for (int i = 0; i < xData.size(); ++i) { - xData[i] = position[i].getX1(); - yData[i] = position[i].getX2(); + xData[i] = res[i][0]; + yData[i] = res[i][1]; } // create graph and assign data to it: plot->addGraph(); diff --git a/model/Rocket.cpp b/model/Rocket.cpp index 6e8b268..47e6931 100644 --- a/model/Rocket.cpp +++ b/model/Rocket.cpp @@ -1,8 +1,13 @@ #include "Rocket.h" -Rocket::Rocket() +Rocket::Rocket() : propagator(this) { propagator.setTimeStep(0.01); //propagator.set } + +void Rocket::launch() +{ + propagator.runUntilTerminate(); +} diff --git a/model/Rocket.h b/model/Rocket.h index fc99477..d4e0183 100644 --- a/model/Rocket.h +++ b/model/Rocket.h @@ -1,9 +1,6 @@ #ifndef ROCKET_H #define ROCKET_H -#include "utils/math/Vector3.h" -#include "utils/math/Quaternion.h" - #include "sim/Propagator.h" #include // std::move @@ -14,30 +11,21 @@ class Rocket public: Rocket(); - void setPosition(const math::Vector3& pos) { *position = pos; } - void setPosition(math::Vector3&& pos) { *position = std::move(pos); } + void launch(); + const std::vector>& getStates() const { return propagator.getStates(); } - void setVelocity(const math::Vector3& vel) { *velocity = vel; } - void setVelocity(math::Vector3&& vel) { *velocity = std::move(vel); } + void setInitialState(const std::vector& initState) { propagator.setInitialState(initState); } - void setOrientation(const math::Quaternion& ori) { *orientation = ori; } - void setOrientation(math::Quaternion&& ori) { *orientation = std::move(ori); } + double getMass() const { return mass; } + void setMass(double m) { mass = m;} - 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; } + void setDragCoefficient(double d) { dragCoeff = d; } + double getDragCoefficient() const { return dragCoeff; } private: - std::shared_ptr position; - std::shared_ptr velocity; - std::shared_ptr orientation; - std::shared_ptr orientationRate; - sim::Propagator propagator; + double dragCoeff; + double mass; }; #endif // ROCKET_H diff --git a/sim/Propagator.cpp b/sim/Propagator.cpp index cb58536..17a29a2 100644 --- a/sim/Propagator.cpp +++ b/sim/Propagator.cpp @@ -1,13 +1,17 @@ +#include #include "Propagator.h" #include "sim/RK4Solver.h" +#include "model/Rocket.h" #include +#include namespace sim { -Propagator::Propagator() - : integrator() +Propagator::Propagator(Rocket* r) + : integrator(), + rocket(r) { @@ -30,6 +34,7 @@ Propagator::Propagator() integrator->setTimeStep(timeStep); + saveStates = true; } Propagator::~Propagator() @@ -38,20 +43,51 @@ Propagator::~Propagator() void Propagator::runUntilTerminate() { - while(true) + + QTextStream out(stdout); + std::size_t j = 0; + while(true && j < 100000) { // nextState gets overwritten - integrator->step(currentTime, currentState, nextState); - std::swap(currentState, nextState); + integrator->step(currentTime, currentState, tempRes); + std::size_t size = currentState.size(); + for(size_t i = 0; i < size; ++i) + { + currentState[i] = tempRes[i]; + tempRes[i] = 0; + } + + //std::swap(currentState, nextState); if(saveStates) { states.push_back(currentState); } + out << currentTime << ": (" + << currentState[0] << ", " + << currentState[1] << ", " + << currentState[2] << ", " + << currentState[3] << ", " + << currentState[4] << ", " + << currentState[5] << ")\n"; if(currentState[1] < 0.0) break; + j++; currentTime += timeStep; } } +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::getTorqueP() { return 0.0; } +double Propagator::getTorqueQ() { return 0.0; } +double Propagator::getTorqueR() { return 0.0; } + } // namespace sim diff --git a/sim/Propagator.h b/sim/Propagator.h index bbe79d7..0d40799 100644 --- a/sim/Propagator.h +++ b/sim/Propagator.h @@ -6,18 +6,26 @@ #include #include +// Forward declare +class Rocket; + namespace sim { class Propagator { public: - Propagator(); + Propagator(Rocket* r); ~Propagator(); - void setInitialState(std::vector& initialState) + void setInitialState(const std::vector& initialState) { - currentState = initialState; + currentState.resize(initialState.size()); + for(std::size_t i = 0; i < initialState.size(); ++i) + { + currentState[i] = initialState[i]; + } + } const std::vector& getCurrentState() const @@ -36,23 +44,27 @@ public: void setTimeStep(double ts) { timeStep = ts; } + void setSaveStats(bool s) { saveStates = s; } + private: - double getForceX() { return 0.0; } - double getForceY() { return 0.0; } - double getForceZ() { return 0.0; } + double getForceX(); + double getForceY(); + double getForceZ(); - double getTorqueP() { return 0.0; } - double getTorqueQ() { return 0.0; } - double getTorqueR() { return 0.0; } + double getTorqueP(); + double getTorqueQ(); + double getTorqueR(); - double getMass() { return 0.0; } + double getMass(); //private: std::unique_ptr integrator; + Rocket* rocket; + std::vector currentState{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - std::vector nextState{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector tempRes{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; bool saveStates{true}; double currentTime{0.0}; double timeStep{0.01};