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

@@ -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");
             }
-
+            return CHeading(interpolateAngle(headingBegin, headingEnd, m_simulationTimeFraction), headingEnd.getReferenceNorth());
-            if ((headingEnd - headingBegin).value(CAngleUnit::deg()) > 180)
-            {
-                headingEnd -= CHeading(360, CHeading::Magnetic, CAngleUnit::deg());
-            }
-
-            return CHeading((headingEnd - headingBegin)
-                            * m_simulationTimeFraction
-                            + headingBegin,
-                            headingBegin.getReferenceNorth());
         }
 
         CAngle CInterpolatorPbh::getPitch() const
         {
             // Interpolate Pitch: Pitch = (PitchB - PitchA) * t + PitchA
-            const CAngle pitchBegin = m_oldSituation.getPitch();
+            return interpolateAngle(m_oldSituation.getPitch(), m_newSituation.getPitch(), m_simulationTimeFraction);
-            const CAngle pitchEnd = m_newSituation.getPitch();
-            const CAngle pitch = (pitchEnd - pitchBegin) * m_simulationTimeFraction + pitchBegin;
-            return pitch;
         }
 
         CAngle CInterpolatorPbh::getBank() const
         {
             // Interpolate bank: Bank = (BankB - BankA) * t + BankA
-            const CAngle bankBegin = m_oldSituation.getBank();
+            return interpolateAngle(m_oldSituation.getBank(), m_newSituation.getBank(), m_simulationTimeFraction);
-            const CAngle bankEnd = m_newSituation.getBank();
-            const CAngle bank = (bankEnd - bankBegin) * m_simulationTimeFraction + bankBegin;
-            return bank;
         }
 
         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

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;