From e8d5daba1bec66569b2b03980ae1187bcd678d04 Mon Sep 17 00:00:00 2001 From: Klaus Basan Date: Fri, 9 Nov 2018 02:56:00 +0100 Subject: [PATCH] Ref T429, PBH interpolator * make sure all angle interpolations interpolate "in the right direction" (like previously only for heaading) * one single function for angle interpolation --- src/blackmisc/simulation/interpolatorpbh.cpp | 65 +++++++++++++------- src/blackmisc/simulation/interpolatorpbh.h | 12 +++- 2 files changed, 54 insertions(+), 23 deletions(-) diff --git a/src/blackmisc/simulation/interpolatorpbh.cpp b/src/blackmisc/simulation/interpolatorpbh.cpp index b348c202c..b8b81df9c 100644 --- a/src/blackmisc/simulation/interpolatorpbh.cpp +++ b/src/blackmisc/simulation/interpolatorpbh.cpp @@ -8,7 +8,11 @@ */ #include "interpolatorpbh.h" +#include "interpolatorfunctions.h" +#include "blackmisc/verify.h" +#include "blackconfig/buildconfig.h" +using namespace BlackConfig; using namespace BlackMisc::Aviation; using namespace BlackMisc::PhysicalQuantities; @@ -16,45 +20,49 @@ namespace BlackMisc { namespace Simulation { + CAngle CInterpolatorPbh::interpolateAngle(const CAngle &begin, const CAngle &end, double timeFraction0to1) + { + // determine the right direction (to left, to right) we interpolate towards to + // -30 -> 30 => 60 (via 0) + // 30 -> -30 => -60 (via 0) + // 170 -> -170 => -340 (via 180) + // -170 -> 170 => 340 (via 180) + double deltaDeg = (end - begin).value(CAngleUnit::deg()); + if (deltaDeg > 180.0) { deltaDeg -= 360; } + else if (deltaDeg < -180.0) { deltaDeg += 360; } + + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + BLACK_VERIFY_X(isValidTimeFraction(timeFraction0to1), Q_FUNC_INFO, "0..1 fraction needed"); + } + + return begin + CAngle(timeFraction0to1 * deltaDeg, CAngleUnit::deg()); + } + CHeading CInterpolatorPbh::getHeading() const { // HINT: VTOL aircraft can change pitch/bank without changing position, planes cannot // Interpolate heading: HDG = (HdgB - HdgA) * t + HdgA const CHeading headingBegin = m_oldSituation.getHeading(); - CHeading headingEnd = m_newSituation.getHeading(); + const CHeading headingEnd = m_newSituation.getHeading(); - if ((headingEnd - headingBegin).value(CAngleUnit::deg()) < -180) + if (CBuildConfig::isLocalDeveloperDebugBuild()) { - headingEnd += CHeading(360, CHeading::Magnetic, CAngleUnit::deg()); + BLACK_VERIFY_X(headingBegin.getReferenceNorth() == headingEnd.getReferenceNorth(), Q_FUNC_INFO, "Need same reference"); } - - if ((headingEnd - headingBegin).value(CAngleUnit::deg()) > 180) - { - headingEnd -= CHeading(360, CHeading::Magnetic, CAngleUnit::deg()); - } - - return CHeading((headingEnd - headingBegin) - * m_simulationTimeFraction - + headingBegin, - headingBegin.getReferenceNorth()); + return CHeading(interpolateAngle(headingBegin, headingEnd, m_simulationTimeFraction), headingEnd.getReferenceNorth()); } CAngle CInterpolatorPbh::getPitch() const { // Interpolate Pitch: Pitch = (PitchB - PitchA) * t + PitchA - const CAngle pitchBegin = m_oldSituation.getPitch(); - const CAngle pitchEnd = m_newSituation.getPitch(); - const CAngle pitch = (pitchEnd - pitchBegin) * m_simulationTimeFraction + pitchBegin; - return pitch; + return interpolateAngle(m_oldSituation.getPitch(), m_newSituation.getPitch(), m_simulationTimeFraction); } CAngle CInterpolatorPbh::getBank() const { // Interpolate bank: Bank = (BankB - BankA) * t + BankA - const CAngle bankBegin = m_oldSituation.getBank(); - const CAngle bankEnd = m_newSituation.getBank(); - const CAngle bank = (bankEnd - bankBegin) * m_simulationTimeFraction + bankBegin; - return bank; + return interpolateAngle(m_oldSituation.getBank(), m_newSituation.getBank(), m_simulationTimeFraction); } CSpeed CInterpolatorPbh::getGroundSpeed() const @@ -63,5 +71,20 @@ namespace BlackMisc * m_simulationTimeFraction + m_oldSituation.getGroundSpeed(); } + + void CInterpolatorPbh::setSituations(const CAircraftSituation &older, const CAircraftSituation &newer) + { + m_oldSituation = older; + m_newSituation = newer; + } + + void CInterpolatorPbh::setTimeFraction(double tf) + { + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + BLACK_VERIFY_X(isValidTimeFraction(tf), Q_FUNC_INFO, "Time fraction needs to be 0-1"); + } + m_simulationTimeFraction = clampValidTimeFraction(tf); + } } // namespace } // namespace diff --git a/src/blackmisc/simulation/interpolatorpbh.h b/src/blackmisc/simulation/interpolatorpbh.h index 3a4f43104..dfbf86f72 100644 --- a/src/blackmisc/simulation/interpolatorpbh.h +++ b/src/blackmisc/simulation/interpolatorpbh.h @@ -16,13 +16,14 @@ #include "blackmisc/aviation/heading.h" #include "blackmisc/pq/angle.h" #include "blackmisc/pq/speed.h" +#include "blackmisc/blackmiscexport.h" namespace BlackMisc { namespace Simulation { //! Simple interpolator for pitch, bank, heading, groundspeed - class CInterpolatorPbh + class BLACKMISC_EXPORT CInterpolatorPbh { public: //! Constructor @@ -42,10 +43,17 @@ namespace BlackMisc const Aviation::CAircraftSituation &getNewSituation() const { return m_newSituation; } //! @} + //! Set situations + //! \remark mostly needed for UNIT tests + void setSituations(const Aviation::CAircraftSituation &older, const Aviation::CAircraftSituation &newer); + //! Change time fraction - void setTimeFraction(double tf) { m_simulationTimeFraction = tf; } + void setTimeFraction(double tf); private: + //! Interpolate angle + static PhysicalQuantities::CAngle interpolateAngle(const PhysicalQuantities::CAngle &begin, const PhysicalQuantities::CAngle &end, double timeFraction0to1); + double m_simulationTimeFraction = 0.0; Aviation::CAircraftSituation m_oldSituation; Aviation::CAircraftSituation m_newSituation;