Ref T429, UNIT test for PBH interpolator

tests/blackmisc/simulation/testinterpolatorlinear/testinterpolatorlinear.cpp

@@ -58,6 +58,9 @@ namespace BlackMiscTest
         //! Basic unit tests for interpolator
         void basicInterpolatorTests();
 
+        //! Interpolator PBH
+        void pbhInterpolatorTest();
+
     private:
         //! Test situation for testing
         static BlackMisc::Aviation::CAircraftSituation getTestSituation(const BlackMisc::Aviation::CCallsign &callsign, int number, qint64 ts, qint64 deltaT, qint64 offset);
@@ -74,9 +77,9 @@ namespace BlackMiscTest
         interpolator.markAsUnitTest();
 
         // fixed time so everything can be debugged
-        const qint64 ts =  1425000000000; // QDateTime::currentMSecsSinceEpoch();
+        const qint64 ts = 1425000000000; // QDateTime::currentMSecsSinceEpoch();
         const qint64 deltaT = 5000; // ms
-        const qint64 offset = 5000;
+        const qint64 offset = 5000; // ms
         for (int i = IRemoteAircraftProvider::MaxSituationsPerCallsign - 1; i >= 0; i--)
         {
             const CAircraftSituation s(getTestSituation(cs, i, ts, deltaT, offset));
@@ -101,7 +104,11 @@ namespace BlackMiscTest
         const CInterpolationAndRenderingSetupPerCallsign setup;
         double latOld = 360.0;
         double lngOld = 360.0;
-        for (qint64 currentTime = ts - 2 * deltaT + offset; currentTime < ts + offset; currentTime += (deltaT / 20))
+
+        qint64 from = ts - 2 * deltaT + offset;
+        qint64 to   = ts; // ts + offset is last value, but we have to consider offset
+        qint64 step = deltaT / 20;
+        for (qint64 currentTime = from; currentTime < to; currentTime += step)
         {
             // This will use time range
             // from:  ts - 2 * deltaT + offset
@@ -132,7 +139,11 @@ namespace BlackMiscTest
         interpolator.markAsUnitTest();
         for (int loops = 0; loops < 20; loops++)
         {
-            for (qint64 currentTime = startTimeMsSinceEpoch + offset; currentTime < ts + offset; currentTime += (deltaT / 20))
+            from = startTimeMsSinceEpoch + offset;
+            to   = ts; // ts + offset is last value, but we have to consider offset
+            step = deltaT / 20;
+
+            for (qint64 currentTime = from; currentTime < to; currentTime += step)
             {
                 // This will use range
                 // from:  ts - 2* deltaT + offset
@@ -168,6 +179,149 @@ namespace BlackMiscTest
         qDebug() << timeMs << "ms" << "for" << fetchedParts << "fetched parts";
     }
 
+    void CTestInterpolatorLinear::pbhInterpolatorTest()
+    {
+        const CCallsign cs("SWIFT");
+        CAircraftSituation s1 = getTestSituation(cs, 0, 0, 0, 0);
+        CAircraftSituation s2 = getTestSituation(cs, 5000, 0, 0, 0);
+        const CHeading heading1(0, CHeading::True, CAngleUnit::deg());
+        const CHeading heading2(120, CHeading::True, CAngleUnit::deg());
+        s1.setHeading(heading1);
+        s2.setHeading(heading2);
+        CInterpolatorPbh pbh(s1, s2);
+
+        const int steps = 10;
+        const double tfStep = 1.0 / steps;
+
+        double timeFraction = 0;
+        double lastDeg = 0;
+
+        for (int i = 0; i < steps; i++)
+        {
+            timeFraction = tfStep * i;
+            pbh.setTimeFraction(timeFraction);
+            const CHeading heading = pbh.getHeading();
+            const double h = heading.value(CAngleUnit::deg());
+            if (i < 1) { lastDeg = h; continue; }
+            QVERIFY2(h > lastDeg, "Expect increasing heading");
+            lastDeg = h;
+        }
+
+        // move from -90 -> 30 over 0
+        const CHeading heading3(270, CHeading::True, CAngleUnit::deg());
+        const CHeading heading4(30, CHeading::True, CAngleUnit::deg());
+        s1.setHeading(heading3);
+        s2.setHeading(heading4);
+        pbh.setSituations(s1, s2);
+
+        for (int i = 0; i < steps; i++)
+        {
+            timeFraction = tfStep * i;
+            pbh.setTimeFraction(timeFraction);
+            const CHeading heading = pbh.getHeading();
+            const double h = heading.value(CAngleUnit::deg());
+            if (i < 1) { lastDeg = h; continue; }
+            QVERIFY2(h > lastDeg, "Expect increasing heading");
+            lastDeg = h;
+        }
+
+        // move from -90 -> 170 over 180
+        const CHeading heading5(270, CHeading::True, CAngleUnit::deg());
+        const CHeading heading6(170, CHeading::True, CAngleUnit::deg());
+        s1.setHeading(heading5);
+        s2.setHeading(heading6);
+        pbh.setSituations(s1, s2);
+
+        for (int i = 0; i < steps; i++)
+        {
+            timeFraction = tfStep * i;
+            pbh.setTimeFraction(timeFraction);
+            const CHeading heading = pbh.getHeading();
+            const double h = CAngle::normalizeDegrees360(heading.value(CAngleUnit::deg()));
+            if (i < 1) { lastDeg = h; continue; }
+            QVERIFY2(h < lastDeg, "Expect increasing heading");
+            lastDeg = h;
+        }
+
+        // bank from 270 -> 30 over 0
+        s1.setHeading(heading5);
+        s2.setHeading(heading5);
+        const CAngle bank1(270, CAngleUnit::deg());
+        const CAngle bank2(30, CAngleUnit::deg());
+        s1.setBank(bank1);
+        s2.setBank(bank2);
+        pbh.setSituations(s1, s2);
+
+        for (int i = 0; i < steps; i++)
+        {
+            timeFraction = tfStep * i;
+            pbh.setTimeFraction(timeFraction);
+            const CAngle bank = pbh.getBank();
+            const double b = bank.value(CAngleUnit::deg());
+            if (i < 1) { lastDeg = b; continue; }
+            QVERIFY2(b > lastDeg, "Expect increasing bank");
+            lastDeg = b;
+        }
+
+        // bank from 170 -> 190 (-170) over 180
+        CAngle bank3(170, CAngleUnit::deg());
+        CAngle bank4(190, CAngleUnit::deg());
+        s1.setBank(bank3);
+        s2.setBank(bank4);
+        pbh.setSituations(s1, s2);
+
+        for (int i = 0; i < steps; i++)
+        {
+            timeFraction = tfStep * i;
+            pbh.setTimeFraction(timeFraction);
+            const CAngle bank = pbh.getBank();
+            const double b = CAngle::normalizeDegrees360(bank.value(CAngleUnit::deg()));
+            if (i < 1) { lastDeg = b; continue; }
+            QVERIFY2(b > lastDeg, "Expect increasing bank");
+            lastDeg = b;
+        }
+
+        // pitch from 30 -> -30 over 0
+        s1.setHeading(heading5);
+        s2.setHeading(heading5);
+        const CAngle pitch1(30, CAngleUnit::deg());
+        const CAngle pitch2(-30, CAngleUnit::deg());
+        s1.setPitch(pitch1);
+        s2.setPitch(pitch2);
+        pbh.setSituations(s1, s2);
+
+        for (int i = 0; i < steps; i++)
+        {
+            timeFraction = tfStep * i;
+            pbh.setTimeFraction(timeFraction);
+            const CAngle pitch = pbh.getPitch();
+            const double p = pitch.value(CAngleUnit::deg());
+            if (i < 1) { lastDeg = p; continue; }
+            QVERIFY2(p < lastDeg, "Expect decreasing pitch");
+            lastDeg = p;
+        }
+
+        // pitch from -30 -> 30 over 0
+        s1.setHeading(heading5);
+        s2.setHeading(heading5);
+        const CAngle pitch3(-30, CAngleUnit::deg());
+        const CAngle pitch4(30, CAngleUnit::deg());
+        s1.setPitch(pitch3);
+        s2.setPitch(pitch4);
+        pbh.setSituations(s1, s2);
+
+        for (int i = 0; i < steps; i++)
+        {
+            timeFraction = tfStep * i;
+            pbh.setTimeFraction(timeFraction);
+            const CAngle pitch = pbh.getPitch();
+            const double p = pitch.value(CAngleUnit::deg());
+            if (i < 1) { lastDeg = p; continue; }
+            QVERIFY2(p > lastDeg, "Expect increasing pitch");
+            lastDeg = p;
+        }
+    }
+
     CAircraftSituation CTestInterpolatorLinear::getTestSituation(const CCallsign &callsign, int number, qint64 ts, qint64 deltaT, qint64 offset)
     {
         const CAltitude alt(number, CAltitude::MeanSeaLevel, CLengthUnit::m());