diff --git a/gui/MainWindow.cpp b/gui/MainWindow.cpp index 8ff61db..d7fecc7 100644 --- a/gui/MainWindow.cpp +++ b/gui/MainWindow.cpp @@ -83,24 +83,24 @@ void MainWindow::on_testButton2_clicked() double ts = 0.01; // X position/velocity. x[0] is X position, x[1] is X velocity - double x[2] = {0.0, initialVelocityX}; + std::vector x = {0.0, initialVelocityX}; // Y position/velocity. y[0] is Y position, y[1] is Y velocity - double y[2] = {0.0, initialVelocityY}; + std::vector y = {0.0, initialVelocityY}; - auto xvelODE = [mass, dragCoeff](double, double* x) -> double + 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, double* x) -> double { return 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, double* y) -> double + 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, double* y) -> double { return y[1]; }; + auto yposODE = [](double, const std::vector& y) -> double { return y[1]; }; sim::RK4Solver ySolver(yposODE, yvelODE); ySolver.setTimeStep(0.01); @@ -110,18 +110,16 @@ 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]; + 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[0] = resX[0]; - x[1] = resX[1]; - y[0] = resY[0]; - y[1] = resY[1]; + x = resX; + y = resY; cout << "(" << position[i].getX1() << ", " << position[i].getX2() << ")\n"; if(y[0] < 0.0) diff --git a/main.cpp b/main.cpp index db1c204..3b3b466 100644 --- a/main.cpp +++ b/main.cpp @@ -4,7 +4,9 @@ #include #include -int main(int argc, char *argv[]) +#include + +void worker(int argc, char* argv[], int& ret) { QApplication a(argc, argv); a.setWindowIcon(QIcon(":/qtrocket.png")); @@ -26,5 +28,37 @@ int main(int argc, char *argv[]) // Go! MainWindow w; w.show(); - return a.exec(); + ret = a.exec(); + +} + +int main(int argc, char *argv[]) +{ + /* + QApplication a(argc, argv); + a.setWindowIcon(QIcon(":/qtrocket.png")); + + // Start translation component. + // TODO: Only support US English at the moment. Anyone want to help translate? + QTranslator translator; + const QStringList uiLanguages = QLocale::system().uiLanguages(); + for (const QString &locale : uiLanguages) + { + const QString baseName = "qtrocket_" + QLocale(locale).name(); + if (translator.load(":/i18n/" + baseName)) + { + a.installTranslator(&translator); + break; + } + } + + // Go! + //MainWindow w; + //w.show(); + //return a.exec(); + */ + int ret = 0; + std::thread guiThread(worker, argc, argv, std::ref(ret)); + guiThread.join(); + return ret; } diff --git a/model/Rocket.cpp b/model/Rocket.cpp index 90c3e92..6e8b268 100644 --- a/model/Rocket.cpp +++ b/model/Rocket.cpp @@ -2,5 +2,7 @@ Rocket::Rocket() { + propagator.setTimeStep(0.01); + //propagator.set } diff --git a/qtrocket.pro b/qtrocket.pro index 5af0822..bae0fb1 100644 --- a/qtrocket.pro +++ b/qtrocket.pro @@ -59,6 +59,7 @@ HEADERS += \ utils/BinMap.h \ utils/CurlConnection.h \ utils/Logger.h \ + utils/TSQueue.h \ utils/ThreadPool.h \ utils/ThrustCurveAPI.h \ utils/math/Constants.h \ diff --git a/sim/DESolver.h b/sim/DESolver.h index f1cb655..2ee33e9 100644 --- a/sim/DESolver.h +++ b/sim/DESolver.h @@ -1,6 +1,8 @@ #ifndef SIM_DESOLVER_H #define SIM_DESOLVER_H +#include + namespace sim { @@ -11,7 +13,7 @@ public: virtual ~DESolver() {} virtual void setTimeStep(double ts) = 0; - virtual void step(double t, double* curVal, double* res ) = 0; + virtual void step(double t, const std::vector& curVal, std::vector& res ) = 0; }; } // namespace sim diff --git a/sim/Propagator.cpp b/sim/Propagator.cpp index 2a072b0..cb58536 100644 --- a/sim/Propagator.cpp +++ b/sim/Propagator.cpp @@ -2,27 +2,56 @@ #include "sim/RK4Solver.h" +#include + namespace sim { Propagator::Propagator() + : integrator() + { -// solver = std::make_unique( -// new(RK4Solver(/* xvel */ [this](double, double* x) -> double { return }))) + + // This is a little strange, but I have to populate the integrator unique_ptr + // with reset. make_unique() doesn't work because the compiler can't seem to + // 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: (x, y, z, xdot, ydot, zdot) - - integrator = std::make_unique(new RK4Solver( - /* dvx/dt */ [this](double, double* ) -> double { return getForceX() / getMass(); }, - /* dx/dt */ [this](double, double* s) -> double {return s[3]; }, - /* dvy/dt */ [this](double, double* ) -> double { return getForceY() / getMass() }, - /* dy/dt */ [this](double, double* s) -> double {return s[4]; }, - /* dvz/dt */ [this](double, double* ) -> double { return getForceZ() / getMass() }, - /* dz/dt */ [this](double, double* s) -> double {return s[5]; })); + integrator.reset(new RK4Solver( + /* dx/dt */ [](double, const std::vector& s) -> double {return s[3]; }, + /* dy/dt */ [](double, const std::vector& s) -> double {return s[4]; }, + /* dz/dt */ [](double, const std::vector& s) -> double {return s[5]; }, + /* dvx/dt */ [this](double, const std::vector& ) -> double { return getForceX() / getMass(); }, + /* dvy/dt */ [this](double, const std::vector& ) -> double { return getForceY() / getMass(); }, + /* dvz/dt */ [this](double, const std::vector& ) -> double { return getForceZ() / getMass(); })); + integrator->setTimeStep(timeStep); +} +Propagator::~Propagator() +{ +} +void Propagator::runUntilTerminate() +{ + while(true) + { + // nextState gets overwritten + integrator->step(currentTime, currentState, nextState); + std::swap(currentState, nextState); + if(saveStates) + { + states.push_back(currentState); + } + if(currentState[1] < 0.0) + break; + + currentTime += timeStep; + } } } // namespace sim diff --git a/sim/Propagator.h b/sim/Propagator.h index dfcfaf5..bbe79d7 100644 --- a/sim/Propagator.h +++ b/sim/Propagator.h @@ -4,6 +4,7 @@ #include "sim/DESolver.h" #include +#include namespace sim { @@ -12,23 +13,50 @@ class Propagator { public: Propagator(); + ~Propagator(); - double getForceX(); - double getForceY(); - double getForceZ(); + void setInitialState(std::vector& initialState) + { + currentState = initialState; + } - double getTorqueP(); - double getTorqueQ(); - double getTorqueR(); + const std::vector& getCurrentState() const + { + return currentState; + } - double getMass(); + void runUntilTerminate(); + + void retainStates(bool s) + { + saveStates = s; + } + + const std::vector>& getStates() const { return states; } + + void setTimeStep(double ts) { timeStep = ts; } private: + double getForceX() { return 0.0; } + double getForceY() { return 0.0; } + double getForceZ() { return 0.0; } + + double getTorqueP() { return 0.0; } + double getTorqueQ() { return 0.0; } + double getTorqueR() { return 0.0; } + + double getMass() { return 0.0; } + +//private: std::unique_ptr integrator; - double currentState[6]{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - double nextState[6]{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + 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}; + bool saveStates{true}; + double currentTime{0.0}; + double timeStep{0.01}; + std::vector> states; }; } // namespace sim diff --git a/sim/RK4Solver.h b/sim/RK4Solver.h index be3f7d5..ec065bc 100644 --- a/sim/RK4Solver.h +++ b/sim/RK4Solver.h @@ -19,13 +19,14 @@ public: RK4Solver(Ts... funcs) { (odes.push_back(funcs), ...); + temp.resize(sizeof...(Ts)); } virtual ~RK4Solver() {} void setTimeStep(double inTs) override { dt = inTs; halfDT = dt / 2.0; } - void step(double t, double* curVal, double* res) override + void step(double t, const std::vector& curVal, std::vector& res) override { if(dt == std::numeric_limits::quiet_NaN()) { @@ -76,7 +77,7 @@ public: } private: - std::vector> odes; + std::vector&)>> odes; static constexpr size_t len = sizeof...(Ts); double k1[len]; @@ -84,7 +85,7 @@ private: double k3[len]; double k4[len]; - double temp[len]; + std::vector temp; double dt = std::numeric_limits::quiet_NaN(); double halfDT = 0.0; diff --git a/sim/StateData.h b/sim/StateData.h index adb654d..33ea7ef 100644 --- a/sim/StateData.h +++ b/sim/StateData.h @@ -14,6 +14,7 @@ class StateData public: StateData(); + private: math::Vector3 position{0.0, 0.0, 0.0}; math::Vector3 velocity{0.0, 0.0, 0.0}; @@ -23,6 +24,8 @@ private: // Necessary? //math::Vector3 orientationAccel; + // This is an array because the integrator expects it + double data[6]; }; diff --git a/utils/TSQueue.h b/utils/TSQueue.h new file mode 100644 index 0000000..7862c4a --- /dev/null +++ b/utils/TSQueue.h @@ -0,0 +1,83 @@ +#ifndef TSQUEUE_H +#define TSQUEUE_H + +#include +#include +#include +#include + +/** + * @brief The TSQueue class is a very basic thread-safe queue + */ +template +class TSQueue +{ +public: + TSQueue() + : mtx(), + q(), + cv() + {} + + void push(T newVal) + { + std::lock_guard lck(mtx); + q.push(newVal); + cv.notify_one(); + } + + void waitAndPop(T& val) + { + std::unique_lock lck(mtx); + cv.wait(lck, [this]{return !q.empty(); }); + val = std::move(q.front()); + q.pop(); + } + + std::shared_ptr waitAndPop() + { + std::unique_lock lck(mtx); + cv.wait(lck, [this] { return !q.empty(); }); + std::shared_ptr res(std::make_shared(std::move(q.front()))); + q.pop(); + return res; + } + + bool tryPop(T& val) + { + std::unique_lock lck(mtx); + if(q.empty()) + { + return false; + } + val = std::move(q.front()); + q.pop(); + return true; + } + + std::shared_ptr tryPop() + { + std::unique_lock lck(mtx); + if(q.empty()) + { + return std::shared_ptr(); // nullptr + } + std::shared_ptr retVal(std::move(q.front())); + q.pop(); + return retVal; + } + + bool empty() const + { + std::lock_guard lck(mtx); + return q.empty(); + } + + +private: + mutable std::mutex mtx; + std::queue q; + std::condition_variable cv; +}; + +#endif // TSQUEUE_H diff --git a/utils/ThreadPool.cpp b/utils/ThreadPool.cpp index 6f03add..d3e16d7 100644 --- a/utils/ThreadPool.cpp +++ b/utils/ThreadPool.cpp @@ -1,6 +1,45 @@ #include "ThreadPool.h" -ThreadPool::ThreadPool() -{ +#include +ThreadPool::ThreadPool() + : done(false), + q(), + threads(), + joiner(threads) +{ + const std::size_t threadCount = std::thread::hardware_concurrency(); + + try + { + for(size_t i = 0; i < threadCount; ++i) + { + threads.push_back(std::thread(&ThreadPool::worker, this)); + } + } + catch(...) + { + done = true; + throw; + } +} + +ThreadPool::~ThreadPool() +{ + done = true; +} +void ThreadPool::worker() +{ + while(!done) + { + std::function task; + if(q.tryPop(task)) + { + task(); + } + else + { + std::this_thread::yield(); + } + } } diff --git a/utils/ThreadPool.h b/utils/ThreadPool.h index fd75a4e..459a560 100644 --- a/utils/ThreadPool.h +++ b/utils/ThreadPool.h @@ -2,6 +2,12 @@ #define THREADPOOL_H #include +#include +#include +#include + +#include "TSQueue.h" + /** * @brief A basic ThreadPool class @@ -10,8 +16,46 @@ class ThreadPool { public: ThreadPool(); + ~ThreadPool(); + + template + void submit(FunctionType f) + { + q.push(std::function(f)); + } + private: + class JoinThreads + { + public: + explicit JoinThreads(std::vector& inThreads) + : threads(inThreads) + {} + + ~JoinThreads() + { + for(auto& i : threads) + { + if(i.joinable()) + { + i.join(); + } + } + } + private: + std::vector& threads; + }; + std::atomic_bool done; + TSQueue> q; + std::vector threads; + + JoinThreads joiner; + + void worker(); + }; + + #endif // THREADPOOL_H