From 22c4e79f52e86e72d92f874f3ef5194954fc83b9 Mon Sep 17 00:00:00 2001 From: Travis Hunter Date: Thu, 18 May 2023 19:58:34 -0600 Subject: [PATCH] WIP --- CMakeLists.txt | 9 +-- QtRocket.h | 2 +- gui/AnalysisWindow.cpp | 4 +- main.cpp | 43 +------------ model/Rocket.cpp | 4 +- model/Rocket.h | 5 +- sim/DESolver.h | 6 +- sim/GravityModel.h | 2 +- sim/Propagator.cpp | 139 +++++++++++++++++++++++++++++++++-------- sim/Propagator.h | 62 +++++++++++------- sim/RK4Solver.h | 87 +++++++++++--------------- sim/StateData.h | 27 +++++++- utils/math/MathTypes.h | 14 ++++- 13 files changed, 244 insertions(+), 160 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c3c574..b4932ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(qtrocket VERSION 0.1 LANGUAGES CXX) +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) include(FetchContent) @@ -42,13 +44,13 @@ FetchContent_Declare(eigen GIT_TAG 3.4.0) FetchContent_MakeAvailable(eigen) +# Add qtrocket subdirectories. These are components that will be linked in + set(CMAKE_AUTOUIC ON) set(CMAKE_AUTOMOC ON) set(CMAKE_AUTORCC ON) -set(CMAKE_CXX_STANDARD 20) -set(CMAKE_CXX_STANDARD_REQUIRED ON) if(WIN32) set(CMAKE_PREFIX_PATH $ENV{QTDIR}) @@ -112,6 +114,7 @@ set(PROJECT_SOURCES sim/SphericalGravityModel.cpp sim/SphericalGravityModel.h sim/StateData.h + sim/TestRK4Solver.h sim/USStandardAtmosphere.cpp sim/USStandardAtmosphere.h sim/WindModel.cpp @@ -162,10 +165,8 @@ else() ) endif() - #qt5_create_translation(QM_FILES ${CMAKE_SOURCE_DIR} ${TS_FILES}) endif() -#target_link_libraries(qtrocket PRIVATE Qt${QT_VERSION_MAJOR}::Widgets Qt${Qt_VERSION_MAJOR}::PrintSupport libcurl jsoncpp_static fmt::fmt-header-only) target_link_libraries(qtrocket PRIVATE Qt6::Widgets Qt6::PrintSupport libcurl jsoncpp_static fmt::fmt-header-only eigen) set_target_properties(qtrocket PROPERTIES diff --git a/QtRocket.h b/QtRocket.h index 02e301b..eaf2381 100644 --- a/QtRocket.h +++ b/QtRocket.h @@ -59,7 +59,7 @@ 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>>& getStates() const { return rocket.second->getStates(); } + const std::vector>& getStates() const { return rocket.second->getStates(); } /** * @brief setInitialState sets the initial state of the Rocket. diff --git a/gui/AnalysisWindow.cpp b/gui/AnalysisWindow.cpp index 92c4e66..b8096f8 100644 --- a/gui/AnalysisWindow.cpp +++ b/gui/AnalysisWindow.cpp @@ -36,7 +36,7 @@ AnalysisWindow::~AnalysisWindow() void AnalysisWindow::onButton_plotAltitude_clicked() { QtRocket* qtRocket = QtRocket::getInstance(); - const std::vector>>& res = qtRocket->getStates(); + const std::vector>& res = qtRocket->getStates(); auto& plot = ui->plotWidget; plot->clearGraphs(); plot->setInteraction(QCP::iRangeDrag, true); @@ -63,7 +63,7 @@ void AnalysisWindow::onButton_plotAltitude_clicked() void AnalysisWindow::onButton_plotVelocity_clicked() { QtRocket* qtRocket = QtRocket::getInstance(); - const std::vector>>& res = qtRocket->getStates(); + const std::vector>& res = qtRocket->getStates(); auto& plot = ui->plotWidget; plot->clearGraphs(); plot->setInteraction(QCP::iRangeDrag, true); diff --git a/main.cpp b/main.cpp index e0d1885..02d1e70 100644 --- a/main.cpp +++ b/main.cpp @@ -23,45 +23,4 @@ int main(int argc, char *argv[]) int retVal = qtrocket->run(argc, argv); logger->debug("Returning"); return retVal; -} - - - -/* - * This was the old main when it directly started the QtGui. It worked. The new way of - * starting the gui through QtRocket->run() also seems to work, but I'm leaving this here for - * now in case there are unforeseen issues with starting the gui in a separate thread via - * QtRocket::run() - * - -#include "gui/MainWindow.h" - -#include -#include -#include - -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(); -} -*/ +} \ No newline at end of file diff --git a/model/Rocket.cpp b/model/Rocket.cpp index 144553f..7bf0fa5 100644 --- a/model/Rocket.cpp +++ b/model/Rocket.cpp @@ -1,3 +1,5 @@ + +// qtrocket headers #include "Rocket.h" #include "QtRocket.h" @@ -16,7 +18,7 @@ void Rocket::setMotorModel(const model::MotorModel& motor) mm = motor; } -bool Rocket::terminateCondition(const std::pair>& cond) +bool Rocket::terminateCondition(const std::pair& cond) { // Terminate propagation when the z coordinate drops below zero if(cond.second[2] < 0) diff --git a/model/Rocket.h b/model/Rocket.h index 5473840..2823843 100644 --- a/model/Rocket.h +++ b/model/Rocket.h @@ -15,6 +15,7 @@ #include "model/ThrustCurve.h" #include "model/MotorModel.h" #include "sim/Propagator.h" +#include "utils/math/MathTypes.h" /** * @brief The Rocket class holds all rocket components @@ -86,7 +87,7 @@ public: * @param cond time/state pair * @return true if the passed-in time/state satisfies the terminate condition */ - bool terminateCondition(const std::pair>& cond); + bool terminateCondition(const std::pair& cond); /** * @brief setName sets the rocket name @@ -98,8 +99,6 @@ public: private: - - std::vector bodyFrameState{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; std::string name; /// Rocket name double dragCoeff; /// @todo get rid of this, should be dynamically calculated double mass; /// @todo get rid of this, should be dynamically computed, but is the current rocket mass diff --git a/sim/DESolver.h b/sim/DESolver.h index 36d04fa..54a4dee 100644 --- a/sim/DESolver.h +++ b/sim/DESolver.h @@ -9,9 +9,13 @@ // 3rd party headers /// \endcond +// qtrocket headers +#include "utils/math/MathTypes.h" + namespace sim { +template class DESolver { public: @@ -29,7 +33,7 @@ public: * the RK4 solver independently as a general tool, this interface is needed * here unfortunately. */ - virtual void step(const std::vector& curVal, std::vector& res, double t = 0.0) = 0; + virtual std::pair step(T& state, T& rate) = 0; }; } // namespace sim diff --git a/sim/GravityModel.h b/sim/GravityModel.h index 5f4085b..718dad1 100644 --- a/sim/GravityModel.h +++ b/sim/GravityModel.h @@ -14,7 +14,7 @@ public: virtual ~GravityModel() {} virtual Vector3 getAccel(double x, double y, double z) = 0; - Vector3 getAccel(const Vector3& t) { return this->getAccel(t[0], t[1], t[2]); } + Vector3 getAccel(const Vector3& t) { return this->getAccel(t.x(), t.y(), t.z()); } }; } // namespace sim diff --git a/sim/Propagator.cpp b/sim/Propagator.cpp index bb46605..c8344fb 100644 --- a/sim/Propagator.cpp +++ b/sim/Propagator.cpp @@ -24,9 +24,10 @@ namespace sim { Propagator::Propagator(std::shared_ptr r) : linearIntegrator(), - //orientationIntegrator(), - rocket(r) - + orientationIntegrator(), + rocket(r), + currentBodyState(), + worldFrameState() { @@ -38,23 +39,78 @@ Propagator::Propagator(std::shared_ptr r) // and pass it a freshly allocated RK4Solver pointer // The state vector has components of the form: - linearIntegrator.reset(new RK4Solver( - /* dx/dt */ [](const std::vector& s, double) -> double {return s[3]; }, - /* dy/dt */ [](const std::vector& s, double) -> double {return s[4]; }, - /* dz/dt */ [](const std::vector& s, double) -> double {return s[5]; }, - /* dvx/dt */ [this](const std::vector&, double ) -> double { return getForceX() / getMass(); }, - /* dvy/dt */ [this](const std::vector&, double ) -> double { return getForceY() / getMass(); }, - /* dvz/dt */ [this](const std::vector&, double ) -> double { return getForceZ() / getMass(); })); + + // Linear velocity and acceleration + std::function(Vector3&, Vector3&)> linearODE = [this](Vector3& state, Vector3& rate) -> std::pair + { + Vector3 dPosition; + Vector3 dVelocity; + // dx/dt + dPosition[0] = rate[0]; + + // dy/dt + 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(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( -// /* dpitch/dt */ [](double, const std::vector& s) -> double { return s[3]; }, -// /* dyaw/dt */ [](double, const std::vector& s) -> double { return s[4]; }, -// /* droll/dt */ [](double, const std::vector& s) -> double { return s[5]; }, -// /* dpitchRate/dt */ [this](double, const std::vector& s) -> double { return (getTorqueP() - s[1] * s[2] * (getIroll() - getIyaw())) / getIpitch(); }, -// /* dyawRate/dt */ [this](double, const std::vector& s) -> double { return (getTorqueQ() - s[0] * s[2] * (getIpitch() - getIroll())) / getIyaw(); }, -// /* drollRate/dt */ [this](double, const std::vector& s) -> double { return (getTorqueR() - s[0] * s[1] * (getIyaw() - getIpitch())) / getIroll(); })); - +// /* dyawRate/dt */ [this](const std::vector& s, double) -> double +// { return getTorqueP() / getIyaw(); }, +// /* dpitchRate/dt */ [this](const std::vector& s, double) -> double +// { return getTorqueQ() / getIpitch(); }, +// /* drollRate/dt */ [this](const std::vector& s, double) -> double +// { return getTorqueR() / getIroll(); }, +// /* q1Dot */ [](const std::vector& 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& 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& 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& s, double) -> double +// { +// double retVal = s[3]*s[0] + s[4]*s[1] + s[5]*s[2]; +// return -0.5 * retVal; +// })); // orientationIntegrator->setTimeStep(timeStep); saveStates = true; @@ -71,15 +127,22 @@ void Propagator::runUntilTerminate() while(true) { - // tempRes gets overwritten - linearIntegrator->step(currentState, tempRes); - std::swap(currentState, tempRes); + // Reset the body frame positions to zero since the origin of the body frame is the CM + currentBodyState[0] = 0.0; + currentBodyState[1] = 0.0; + currentBodyState[2] = 0.0; + + // tempRes gets overwritten + tempRes = linearIntegrator->step(currentBodyState); + + std::swap(currentBodyState, tempRes); + if(saveStates) { - states.push_back(std::make_pair(currentTime, currentState)); + states.push_back(std::make_pair(currentTime, currentBodyState)); } - if(rocket->terminateCondition(std::make_pair(currentTime, currentState))) + if(rocket->terminateCondition(std::make_pair(currentTime, currentBodyState))) break; currentTime += timeStep; @@ -101,20 +164,20 @@ double Propagator::getMass() double Propagator::getForceX() { QtRocket* qtrocket = QtRocket::getInstance(); - return (currentState[3] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState[2])/ 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[3]* currentState[3]; + return (currentBodyState[3] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2])/ 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[3]* currentBodyState[3]; } double Propagator::getForceY() { QtRocket* qtrocket = QtRocket::getInstance(); - return (currentState[4] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[4]* currentState[4]; + return (currentBodyState[4] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[4]* currentBodyState[4]; } double Propagator::getForceZ() { QtRocket* qtrocket = QtRocket::getInstance(); - double gravity = (qtrocket->getEnvironment()->getGravityModel()->getAccel(currentState[0], currentState[1], currentState[2]))[2]; - double airDrag = (currentState[5] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentState[5]* currentState[5]; + double gravity = (qtrocket->getEnvironment()->getGravityModel()->getAccel(currentBodyState[0], currentBodyState[1], currentBodyState[2]))[2]; + double airDrag = (currentBodyState[5] >= 0 ? -1.0 : 1.0) * qtrocket->getEnvironment()->getAtmosphericModel()->getDensity(currentBodyState[2]) / 2.0 * 0.008107 * rocket->getDragCoefficient() * currentBodyState[5]* currentBodyState[5]; double thrust = rocket->getThrust(currentTime); return gravity + airDrag + thrust; } @@ -123,4 +186,28 @@ 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(currentBodyState[0], + currentBodyState[1], + currentBodyState[2]); + Vector3 gravityVector{gravityAccel[0], + gravityAccel[1], + gravityAccel[2]}; + + + Quaternion q{currentOrientation[3], + currentOrientation[4], + currentOrientation[5], + currentOrientation[6]}; + + Vector3 res = q * gravityVector; + + + return Vector3{0.0, 0.0, 0.0}; + + +} + } // namespace sim diff --git a/sim/Propagator.h b/sim/Propagator.h index 916251d..d2ef7ca 100644 --- a/sim/Propagator.h +++ b/sim/Propagator.h @@ -12,6 +12,8 @@ // qtrocket headers #include "sim/DESolver.h" +#include "utils/math/MathTypes.h" +#include "sim/StateData.h" // Forward declare @@ -29,17 +31,16 @@ public: void setInitialState(const std::vector& initialState) { - currentState.resize(initialState.size()); for(std::size_t i = 0; i < initialState.size(); ++i) { - currentState[i] = initialState[i]; + currentBodyState[i] = initialState[i]; } } - const std::vector& getCurrentState() const + const Vector6& getCurrentState() const { - return currentState; + return currentBodyState; } void runUntilTerminate(); @@ -49,43 +50,56 @@ public: saveStates = s; } - const std::vector>>& getStates() const { return states; } + const std::vector>& 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 getMass(); + double getForceX(); + double getForceY(); + double getForceZ(); - double getTorqueP(); - double getTorqueQ(); - double getTorqueR(); + double getTorqueP(); + double getTorqueQ(); + double getTorqueR(); - double getIpitch() { return 1.0; } - double getIyaw() { return 1.0; } - double getIroll() { return 1.0; } + double getIpitch() { return 1.0; } + double getIyaw() { return 1.0; } + double getIroll() { return 1.0; } -//private: - - std::unique_ptr linearIntegrator; - //std::unique_ptr orientationIntegrator; + std::unique_ptr> linearIntegrator; + std::unique_ptr> orientationIntegrator; std::shared_ptr rocket; + StateData worldFrameState; + //StateData bodyFrameState; + Vector3 currentBodyPosition{0.0, 0.0, 0.0}; + Vector3 currentBodyVelocity{0.0, 0.0, 0.0}; + Vector3 nextBodyPosition{0.0, 0.0, 0.0}; + Vector3 nextBodyVelocity{0.0, 0.0, 0.0}; - std::vector currentState{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, 0.0}; + std::vector currentWorldState{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + Vector3 currentGravity{0.0, 0.0, 0.0}; + Vector3 currentWindSpeed{0.0, 0.0, 0.0}; + + + // orientation vectors in the form (yawDot, pitchDot, rollDot, q1, q2, q3, q4) + Quaternion currentOrientation{0.0, 0.0, 0.0, 0.0}; + Quaternion currentOrientationRate{0.0, 0.0, 0.0, 0.0}; + Quaternion nextOrientation{0.0, 0.0, 0.0, 0.0}; + Quaternion nextOrientationRate{0.0, 0.0, 0.0, 0.0}; bool saveStates{true}; double currentTime{0.0}; double timeStep{0.01}; - std::vector>> states; + std::vector> states; + + Vector3 getCurrentGravity(); }; } // namespace sim diff --git a/sim/RK4Solver.h b/sim/RK4Solver.h index ec481da..46a13ec 100644 --- a/sim/RK4Solver.h +++ b/sim/RK4Solver.h @@ -16,6 +16,7 @@ // qtrocket headers #include "sim/DESolver.h" #include "utils/Logger.h" +#include "utils/math/MathTypes.h" namespace sim { @@ -28,81 +29,67 @@ namespace sim { * * @tparam Ts */ -template -class RK4Solver : public DESolver +template +class RK4Solver : public DESolver { public: - RK4Solver(Ts... funcs) + RK4Solver(std::function(T&, T&)> func) { - (odes.push_back(funcs), ...); - temp.resize(sizeof...(Ts)); - + // This only works for Eigen Vector types. + // TODO: Figure out how to make this slightly more generic, but for now + // we're only using this for Vector3 and Quaternion types + static_assert(std::is_same::value + || std::is_same::value, + "You can only use Vector3 or Quaternion valued functions in RK4Solver"); + + odes = func; } virtual ~RK4Solver() {} void setTimeStep(double inTs) override { dt = inTs; halfDT = dt / 2.0; } - void step(const std::vector& curVal, std::vector& res, double t = 0.0) override + std::pair step(T& state, T& rate) override { + std::pair res; if(dt == std::numeric_limits::quiet_NaN()) { utils::Logger::getInstance()->error("Calling RK4Solver without setting dt first is an error"); - res[0] = std::numeric_limits::quiet_NaN(); + return res; } - for(size_t i = 0; i < len; ++i) - { - k1[i] = odes[i](curVal, t); - } + std::tie(k1State, k1Rate) = odes(state, rate); // 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](temp, t + halfDT); - } - // 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](temp, t + halfDT); - } + std::tie(tempState, tempRate) = std::make_pair(state + k1State*halfDT, rate + k1Rate*halfDT); + std::tie(k2State, k2Rate) = odes(tempState, tempRate); - // 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](temp, t + dt); - } + std::tie(tempState, tempRate) = std::make_pair(state + k2State*halfDT, rate + k2Rate*halfDT); + std::tie(k3State, k3Rate) = odes(tempState, tempRate); - // 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]); - } + std::tie(tempState, tempRate) = std::make_pair(state + k3State*dt, rate + k3Rate*dt); + std::tie(k4State, k4Rate) = odes(tempState, tempRate); + res = std::make_pair(state + (dt / 6.0)*(k1State + 2.0*k2State + 2.0*k3State + k4State), + rate + (dt / 6.0)*(k1Rate + 2.0*k2Rate + 2.0*k3Rate + k4Rate)); + + return res; } private: - std::vector&, double)>> odes; + std::function(T&, T&)> odes; - static constexpr size_t len = sizeof...(Ts); - double k1[len]; - double k2[len]; - double k3[len]; - double k4[len]; + T k1State; + T k2State; + T k3State; + T k4State; + T k1Rate; + T k2Rate; + T k3Rate; + T k4Rate; - std::vector temp; + T tempState; + T tempRate; double dt = std::numeric_limits::quiet_NaN(); double halfDT = 0.0; diff --git a/sim/StateData.h b/sim/StateData.h index f0a747b..f20aede 100644 --- a/sim/StateData.h +++ b/sim/StateData.h @@ -1,6 +1,13 @@ #ifndef STATEDATA_H #define STATEDATA_H +/// \cond +// C headers +// C++ headers +#include +// 3rd party headers +/// \endcond + // qtrocket headers #include "utils/math/MathTypes.h" @@ -15,17 +22,33 @@ public: StateData() {} ~StateData() {} + std::vector getPosStdVector() + { + return std::vector{position[0], position[1], position[2]}; + } + std::vector getVelStdVector() + { + return std::vector{velocity[0], velocity[1], velocity[2]}; + } + /// TODO: Put these behind an interface // private: + + // These are 4-vectors so quaternion multiplication works out. The last term (scalar) is always + // zero. I could just use quaternions here, but I want to make it clear that these aren't + // rotations, they're vectors Vector3 position{0.0, 0.0, 0.0}; Vector3 velocity{0.0, 0.0, 0.0}; - Quaternion orientation{0.0, 0.0, 0.0, 0.0}; /// (scalar, vector) - Quaternion orientationRate{0.0, 0.0, 0.0, 0.0}; /// (scalar, vector) + Quaternion orientation{0.0, 0.0, 0.0, 0.0}; /// (vector, scalar) + Quaternion orientationRate{0.0, 0.0, 0.0, 0.0}; /// (vector, scalar) Matrix3 dcm{{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; /// Euler angles are yaw-pitch-roll, and (3-2-1) order + /// yaw - psi + /// pitch - theta + /// roll - phi Vector3 eulerAngles{0.0, 0.0, 0.0}; // This is an array because the integrator expects it diff --git a/utils/math/MathTypes.h b/utils/math/MathTypes.h index 41551bb..9cb354c 100644 --- a/utils/math/MathTypes.h +++ b/utils/math/MathTypes.h @@ -6,10 +6,18 @@ /// This is not in any namespace. These typedefs are intended to be used throughout QtRocket, /// so keeping them in the global namespace seems to make sense. -typedef Eigen::Matrix3d Matrix3; -typedef Eigen::Matrix4d Matrix4; -typedef Eigen::Vector3d Vector3; +template +using MyMatrix = Eigen::Matrix; + +template +using MyVector = Eigen::Matrix; + typedef Eigen::Quaterniond Quaternion; +using Matrix3 = MyMatrix<3>; +using Matrix4 = MyMatrix<4>; + +using Vector3 = MyVector<3>; +using Vector6 = MyVector<6>; #endif // UTILS_MATH_MATHTYPES_H \ No newline at end of file