Ref T261, interpolator changes

* use CAircraftSituationChange
* recalculate interpolant if situation changed (due to elevation update)
* keep extra info string for debugging
* improved situation checking

src/blackmisc/simulation/interpolator.cpp

@@ -54,6 +54,15 @@ namespace BlackMisc
             this->attachLogger(logger);
         }
 
+        template<typename Derived>
+        CLength CInterpolator<Derived>::getAndFetchModelCG()
+        {
+            const CLength cg = this->getCG(m_callsign);
+            m_model.setCG(cg);
+            m_model.setCallsign(m_callsign);
+            return cg;
+        }
+
         template<typename Derived>
         void CInterpolator<Derived>::deferredInit()
         {
@@ -72,24 +81,24 @@ namespace BlackMisc
             if (!newer.isNull())  { situations.push_back(newer); }
             if (!oldest.isNull()) { situations.push_back(oldest); }
 
-            const bool sort1 = situations.isSortedLatestFirst();
-            BLACK_VERIFY_X(sort1, Q_FUNC_INFO, "Wrong timestamp order");
-            const bool sort2 = situations.isSortedAdjustedLatestFirst();
-            BLACK_VERIFY_X(sort2, Q_FUNC_INFO, "Wrong adjusted timestamp order");
-
-            bool details = true;
-            if (setup.isAircraftPartsEnabled())
+            const bool sorted = situations.isSortedAdjustedLatestFirstWithoutNullPositions();
+            if (CBuildConfig::isLocalDeveloperDebugBuild())
             {
-                if (situations.containsOnGroundDetails(CAircraftSituation::InFromParts))
-                {
-                    // if a client supports parts, all situations are supposed to be parts based
-                    details = situations.areAllOnGroundDetailsSame(CAircraftSituation::InFromParts);
-                    BLACK_VERIFY_X(details, Q_FUNC_INFO, "Once gnd.from parts -> always gnd. from parts");
-                }
+                BLACK_VERIFY_X(sorted, Q_FUNC_INFO, "Wrong adjusted timestamp order");
+            }
+
+            if (setup.isNull() || !setup.isAircraftPartsEnabled()) { return sorted; }
+
+            bool details = false;
+            if (situations.containsOnGroundDetails(CAircraftSituation::InFromParts))
+            {
+                // if a client supports parts, all situations are supposed to be parts based
+                details = situations.areAllOnGroundDetailsSame(CAircraftSituation::InFromParts);
+                BLACK_VERIFY_X(details, Q_FUNC_INFO, "Once gnd.from parts -> always gnd. from parts");
             }
 
             // result
-            return sort1 && sort2 && details;
+            return sorted && details;
         }
 
         template <typename Derived>
@@ -108,14 +117,25 @@ namespace BlackMisc
             Q_ASSERT_X(!m_callsign.isEmpty(), Q_FUNC_INFO, "Missing callsign");
 
             // this code is used by linear and spline interpolator
+            m_interpolatedSituationsCounter++;
             status.reset();
             SituationLog log;
             const bool doLogging = this->hasAttachedLogger() && setup.logInterpolation();
 
             // any data at all?
             const CAircraftSituationList situations = this->remoteAircraftSituations(m_callsign);
-            if (situations.isEmpty()) { return CAircraftSituation(m_callsign); }
-            CAircraftSituation currentSituation = m_lastInterpolation.isNull() ? situations.front() : m_lastInterpolation;
+            const int situationsCount = situations.size();
+            status.setSituationsCount(situationsCount);
+            if (situations.isEmpty())
+            {
+                status.setExtraInfo(this->isAircraftInRange(m_callsign) ?
+                                    QString("No situations, but remote aircraft '%1'").arg(m_callsign.asString()) :
+                                    QString("Unknown remote aircraft: '%1'").arg(m_callsign.asString()));
+                return CAircraftSituation(m_callsign);
+            }
+
+            const CAircraftSituation latest = situations.front();
+            CAircraftSituation currentSituation = m_lastInterpolation.isNull() ? latest : m_lastInterpolation;
             if (currentSituation.getCallsign() != m_callsign)
             {
                 BLACK_VERIFY_X(false, Q_FUNC_INFO, "Wrong callsign");
@@ -130,7 +150,8 @@ namespace BlackMisc
             }
 
             // fetch CG once
-            if (m_cg.isNull()) { m_cg = this->getCG(m_callsign); }
+            const CLength cg = latest.hasCG() ? latest.getCG() : this->getAndFetchModelCG();
+            currentSituation.setCG(cg);
 
             // data, split situations by time
             if (currentTimeMsSinceEpoc < 0) { currentTimeMsSinceEpoc = QDateTime::currentMSecsSinceEpoch(); }
@@ -157,14 +178,14 @@ namespace BlackMisc
             // use derived interpolant function
             const bool interpolateGndFlag = pbh.getNewSituation().hasGroundDetailsForGndInterpolation() && pbh.getOldSituation().hasGroundDetailsForGndInterpolation();
             currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation, interpolateGndFlag);
-            if (!interpolateGndFlag) { currentSituation.guessOnGround(m_model.isVtol(), m_cg); }
+            if (!interpolateGndFlag) { currentSituation.guessOnGround(CAircraftSituationChange::null(), m_model); }
 
             // correct itself
             CAircraftSituation::AltitudeCorrection altCorrection = CAircraftSituation::NoCorrection;
             if (!interpolateGndFlag && currentSituation.getOnGroundDetails() != CAircraftSituation::OnGroundByGuessing)
             {
                 // just in case
-                altCorrection = currentSituation.correctAltitude(m_cg, true);
+                altCorrection = currentSituation.correctAltitude(cg, true);
             }
 
             // status
@@ -180,9 +201,11 @@ namespace BlackMisc
                 log.groundFactor = currentSituation.getOnGroundFactor();
                 log.altCorrection = CAircraftSituation::altitudeCorrectionToString(altCorrection);
                 log.situationCurrent = currentSituation;
+                log.change = m_situationChange;
                 log.usedSetup = setup;
                 log.elevationInfo = elv.arg(elvStats.first).arg(elvStats.second);
-                log.cgAboveGround = m_cg;
+                log.cgAboveGround = cg;
+                log.sceneryOffset = m_currentSceneryOffset;
                 m_logger->logInterpolation(log);
             }
 
@@ -287,15 +310,7 @@ namespace BlackMisc
             if (!partsStatus.isSupportingParts())
             {
                 // check if model has been thru model matching
-                if (m_model.hasModelString())
-                {
-                    parts.guessParts(this->getLastInterpolatedSituation(), m_model.isVtol(), m_model.getEngineCount());
-                }
-                else
-                {
-                    // default guess
-                    parts.guessParts(this->getLastInterpolatedSituation());
-                }
+                parts.guessParts(this->getLastInterpolatedSituation(), m_situationChange, m_model);
             }
             this->logParts(currentTimeMsSinceEpoch, parts, 0, false, log);
             return parts;
@@ -355,6 +370,17 @@ namespace BlackMisc
                     m_model = model;
                 }
             }
+            if (m_model.getCG().isNull())
+            {
+                const CLength cg(this->getCG(m_callsign));
+                if (!cg.isNull()) { m_model.setCG(cg); }
+            }
+            m_model.setCallsign(m_callsign);
+        }
+
+        void CInterpolationStatus::setExtraInfo(const QString &info)
+        {
+            m_extraInfo = info;
         }
 
         void CInterpolationStatus::setInterpolatedAndCheckSituation(bool succeeded, const CAircraftSituation &situation)
@@ -365,7 +391,7 @@ namespace BlackMisc
 
         void CInterpolationStatus::checkIfValidSituation(const CAircraftSituation &situation)
         {
-            m_isValidSituation = !situation.isGeodeticHeightNull() && !situation.isPositionNull();
+            m_isValidSituation = !situation.isPositionOrAltitudeNull();
         }
 
         bool CInterpolationStatus::hasValidInterpolatedSituation() const
@@ -375,14 +401,20 @@ namespace BlackMisc
 
         void CInterpolationStatus::reset()
         {
+            m_extraInfo.clear();
             m_isValidSituation = false;
             m_isInterpolated = false;
+            m_situations = -1;
         }
 
         QString CInterpolationStatus::toQString() const
         {
             return QStringLiteral("Interpolated: ") % boolToYesNo(m_isInterpolated) %
-                   QStringLiteral(" | situation valid: ") % boolToYesNo(m_isValidSituation);
+                   QStringLiteral(" | situations: ") % QString::number(m_situations) %
+                   QStringLiteral(" | situation valid: ") % boolToYesNo(m_isValidSituation) %
+                   (
+                       m_extraInfo.isEmpty() ? QStringLiteral("") : QStringLiteral(" info: ") % m_extraInfo
+                   );
         }
 
         bool CPartsStatus::allTrue() const

src/blackmisc/simulation/interpolator.h

@@ -17,7 +17,7 @@
 #include "blackmisc/simulation/interpolationsetupprovider.h"
 #include "blackmisc/simulation/simulationenvironmentprovider.h"
 #include "blackmisc/simulation/aircraftmodel.h"
-#include "blackmisc/aviation/aircraftpartslist.h"
+#include "blackmisc/aviation/aircraftsituationchange.h"
 #include "blackmisc/aviation/aircraftsituation.h"
 #include "blackmisc/aviation/aircraftpartslist.h"
 #include "blackmisc/aviation/callsign.h"
@@ -99,47 +99,25 @@ namespace BlackMisc
                           ISimulationEnvironmentProvider *simEnvProvider, IInterpolationSetupProvider *setupProvider, IRemoteAircraftProvider *p3,
                           CInterpolationLogger *logger);
 
-            const Aviation::CCallsign    m_callsign; //!< corresponding callsign
-            PhysicalQuantities::CLength  m_cg { 0, nullptr } ; //!< fetched once, stays constant
-            Aviation::CAircraftSituation m_lastInterpolation { Aviation::CAircraftSituation::null() }; //!< latest interpolation
+            //! Center of gravity
+            const PhysicalQuantities::CLength &getModelCG() const { return m_model.getCG(); }
+
+            //! Center of gravity, fetched from provider in case needed
+            PhysicalQuantities::CLength getAndFetchModelCG();
+
+            const Aviation::CCallsign  m_callsign; //!< corresponding callsign
             CAircraftModel m_model; //!< corresponding model
+            Aviation::CAircraftSituation m_lastInterpolation { Aviation::CAircraftSituation::null() }; //!< latest interpolation
+            Aviation::CAircraftSituationChange m_situationChange; //!< situations change
             qint64 m_situationsLastModifiedUsed { -1 }; //!< based on situations last updated
-
-            //! Equal double values?
-            static bool doubleEpsilonEqual(double d1, double d2)
-            {
-                return qAbs(d1 - d2) < std::numeric_limits<double>::epsilon();
-            }
-
-            //! Both on ground
-            static bool gfEqualOnGround(double oldGroundFactor, double newGroundFactor)
-            {
-                return doubleEpsilonEqual(1.0, oldGroundFactor) && doubleEpsilonEqual(1.0, newGroundFactor);
-            }
-
-            //! Both not on ground
-            static bool gfEqualAirborne(double oldGroundFactor, double newGroundFactor)
-            {
-                return doubleEpsilonEqual(0.0, oldGroundFactor) && doubleEpsilonEqual(0.0, newGroundFactor);
-            }
-
-            //! Plane is starting
-            static bool gfStarting(double oldGroundFactor, double newGroundFactor)
-            {
-                return doubleEpsilonEqual(0.0, oldGroundFactor) && doubleEpsilonEqual(1.0, newGroundFactor);
-            }
-
-            //! Plane is landing
-            static bool gfLanding(double oldGroundFactor, double newGroundFactor)
-            {
-                return doubleEpsilonEqual(1.0, oldGroundFactor) && doubleEpsilonEqual(0.0, newGroundFactor);
-            }
+            int m_interpolatedSituationsCounter = 0; //!< counter for each interpolated situations: statistics, every n-th interpolation ....
 
             //! Verify gnd flag, times, ... true means "OK"
-            bool verifyInterpolationSituations(const Aviation::CAircraftSituation &oldest, const Aviation::CAircraftSituation &newer, const Aviation::CAircraftSituation &latest, const CInterpolationAndRenderingSetupPerCallsign &setup);
+            bool verifyInterpolationSituations(const Aviation::CAircraftSituation &oldest, const Aviation::CAircraftSituation &newer, const Aviation::CAircraftSituation &latest,
+                                               const CInterpolationAndRenderingSetupPerCallsign &setup = CInterpolationAndRenderingSetupPerCallsign::null());
 
         private:
-            CInterpolationLogger *m_logger = nullptr;
+            CInterpolationLogger *m_logger = nullptr; //!< optional interpolation logger
             QTimer m_initTimer; //!< timer to init model, will be deleted when interpolator is deleted and cancel the call
 
             //! Log parts
@@ -192,6 +170,12 @@ namespace BlackMisc
             //! Set succeeded
             void setInterpolated(bool interpolated) { m_isInterpolated = interpolated; }
 
+            //! Set situations count
+            void setSituationsCount(int count) { m_situations = count; }
+
+            //! Extra info
+            void setExtraInfo(const QString &info);
+
             //! Set succeeded
             void setInterpolatedAndCheckSituation(bool succeeded, const Aviation::CAircraftSituation &situation);
 
@@ -213,6 +197,8 @@ namespace BlackMisc
         private:
             bool m_isInterpolated = false;   //!< position is interpolated (means enough values, etc.)
             bool m_isValidSituation = false; //!< is valid situation
+            int  m_situations = -1;          //!< number of situations used for interpolation
+            QString m_extraInfo; //!< optional details
         };
 
         //! Status regarding parts

src/blackmisc/simulation/interpolatorlinear.cpp

@@ -81,11 +81,11 @@ namespace BlackMisc
                 const double newGroundFactor = m_newSituation.getOnGroundFactor();
                 do
                 {
-                    if (gfEqualAirborne(oldGroundFactor, newGroundFactor)) { newSituation.setOnGround(false); break; }
-                    if (gfEqualOnGround(oldGroundFactor, newGroundFactor)) { newSituation.setOnGround(true); break; }
+                    if (CAircraftSituation::isGfEqualAirborne(oldGroundFactor, newGroundFactor)) { newSituation.setOnGround(false); break; }
+                    if (CAircraftSituation::isGfEqualOnGround(oldGroundFactor, newGroundFactor)) { newSituation.setOnGround(true); break; }
                     const double groundFactor = (newGroundFactor - oldGroundFactor) * m_simulationTimeFraction + oldGroundFactor;
                     newSituation.setOnGroundFactor(groundFactor);
-                    newSituation.setOnGroundFromGroundFactorFromInterpolation();
+                    newSituation.setOnGroundFromGroundFactorFromInterpolation(groundInterpolationFactor());
                 }
                 while (false);
             }
@@ -103,15 +103,19 @@ namespace BlackMisc
             // with the latest updates of T243 the order and the offsets are supposed to be correct
             // so even mixing fast/slow updates shall work
             const CAircraftSituationList validSituations = this->remoteAircraftSituations(m_callsign); // if needed, we could also copy here
-            m_situationsLastModifiedUsed = this->situationsLastModified(m_callsign);
-
-            // checks
             if (!CBuildConfig::isReleaseBuild())
             {
-                BLACK_VERIFY_X(validSituations.isSortedAdjustedLatestFirst(), Q_FUNC_INFO, "Wrong sort order");
+                BLACK_VERIFY_X(validSituations.isSortedAdjustedLatestFirstWithoutNullPositions(), Q_FUNC_INFO, "Wrong sort order");
                 Q_ASSERT_X(validSituations.size() <= IRemoteAircraftProvider::MaxSituationsPerCallsign, Q_FUNC_INFO, "Wrong size");
             }
 
+            const qint64 tsLastModified = this->situationsLastModified(m_callsign);
+            if (m_situationsLastModifiedUsed < tsLastModified || m_situationChange.isNull())
+            {
+                m_situationsLastModifiedUsed = tsLastModified;
+                m_situationChange = CAircraftSituationChange(validSituations, true, true);
+            }
+
             // find the first situation earlier than the current time
             const auto pivot = std::partition_point(validSituations.begin(), validSituations.end(), [ = ](auto &&s) { return s.getAdjustedMSecsSinceEpoch() > currentTimeMsSinceEpoc; });
             const auto situationsNewer = makeRange(validSituations.begin(), pivot);

src/blackmisc/simulation/interpolatorspline.cpp

@@ -119,14 +119,15 @@ namespace BlackMisc
                 // with the latest updates of T243 the order and the offsets are supposed to be correct
                 // so even mixing fast/slow updates shall work
                 const CAircraftSituationList validSituations = this->remoteAircraftSituations(m_callsign);
+                if (CBuildConfig::isLocalDeveloperDebugBuild())
+                {
+                    BLACK_VERIFY_X(validSituations.isSortedAdjustedLatestFirstWithoutNullPositions(), Q_FUNC_INFO, "Wrong sort order");
+                    BLACK_VERIFY_X(validSituations.size() <= IRemoteAircraftProvider::MaxSituationsPerCallsign, Q_FUNC_INFO, "Wrong size");
+                }
+
                 situationsSize = validSituations.size();
                 m_situationsLastModifiedUsed = lastModified;
-
-                if (!CBuildConfig::isReleaseBuild())
-                {
-                    Q_ASSERT_X(validSituations.isSortedAdjustedLatestFirst(), Q_FUNC_INFO, "Wrong sort order");
-                    Q_ASSERT_X(validSituations.size() <= IRemoteAircraftProvider::MaxSituationsPerCallsign, Q_FUNC_INFO, "Wrong size");
-                }
+                m_situationChange = CAircraftSituationChange(validSituations, true, true);
 
                 // find the first situation earlier than the current time
                 const auto pivot = std::partition_point(validSituations.begin(), validSituations.end(), [ = ](auto &&s) { return s.getAdjustedMSecsSinceEpoch() > currentTimeMsSinceEpoc; });
@@ -136,10 +137,18 @@ namespace BlackMisc
                 // m_s[0] .. oldest -> m_[2] .. latest
                 if (situationsNewer.isEmpty() || situationsOlder.size() < 2) { return m_interpolant; }
                 m_s = std::array<CAircraftSituation, 3> {{ *(situationsOlder.begin() + 1), *situationsOlder.begin(), *(situationsNewer.end() - 1) }};
+                this->verifyInterpolationSituations(m_s[0], m_s[1], m_s[2], setup); // oldest -> latest
 
                 // we interpolate from 1 -> 2, 0 for smoother interpolation
                 if (newStep && !m_lastInterpolation.isNull())
                 {
+                    const bool verified = this->verifyInterpolationSituations(m_s[0], m_lastInterpolation, m_s[2]); // oldest -> latest, only verify order
+                    if (!verified)
+                    {
+                        static const QString vm("m0-2 (oldest latest) %1 %2 (%3) %4");
+                        const QString vmValues = vm.arg(m_s[0].getAdjustedMSecsSinceEpoch()).arg(m_s[1].getAdjustedMSecsSinceEpoch()).arg(m_lastInterpolation.getAdjustedMSecsSinceEpoch()).arg(m_s[2].getAdjustedMSecsSinceEpoch());
+                        Q_UNUSED(vmValues);
+                    }
                     m_s[1] = m_lastInterpolation; // true only for the moment we create a new step
                 }
 
@@ -161,9 +170,10 @@ namespace BlackMisc
                 // - and the elevation remains (almost) constant for a wider area
                 // - during flying the ground elevation not really matters
                 this->updateElevations();
-                const double a0 = m_s[0].getCorrectedAltitude(m_cg).value(); // oldest
-                const double a1 = m_s[1].getCorrectedAltitude(m_cg).value();
-                const double a2 = m_s[2].getCorrectedAltitude(m_cg).value(); // latest
+                const CLength cg(m_s[2].hasCG() ? m_s[2].getCG() : this->getAndFetchModelCG());
+                const double a0 = m_s[0].getCorrectedAltitude(cg).value(); // oldest
+                const double a1 = m_s[1].getCorrectedAltitude(cg).value();
+                const double a2 = m_s[2].getCorrectedAltitude(cg).value(); // latest
                 pa.a = {{ a0, a1, a2 }};
                 pa.gnd = {{ m_s[0].getOnGroundFactor(), m_s[1].getOnGroundFactor(), m_s[2].getOnGroundFactor() }};
                 pa.da = getDerivatives(pa.t, pa.a);
@@ -182,9 +192,6 @@ namespace BlackMisc
                 m_nextSampleTime = m_s[2].getMSecsSinceEpoch(); // latest
                 m_interpolant = Interpolant(pa, m_s[2].getAltitudeUnit(), CInterpolatorPbh(m_s[1], m_s[2]));
                 Q_ASSERT_X(m_prevSampleAdjustedTime < m_nextSampleAdjustedTime, Q_FUNC_INFO, "Wrong time order");
-
-                // VERIFY
-                this->verifyInterpolationSituations(m_s[0], m_s[1], m_s[2], setup); // oldest -> latest
             }
 
             // Example:
@@ -290,15 +297,15 @@ namespace BlackMisc
             if (interpolateGndFactor)
             {
                 const double gnd1 = m_pa.gnd[1];
-                const double gnd2 = m_pa.gnd[2];
+                const double gnd2 = m_pa.gnd[2]; // latest
                 do
                 {
                     newSituation.setOnGroundDetails(CAircraftSituation::OnGroundByInterpolation);
-                    if (gfEqualAirborne(gnd1, gnd2)) { newSituation.setOnGround(false); break; }
-                    if (gfEqualOnGround(gnd1, gnd2)) { newSituation.setOnGround(true); break; }
+                    if (CAircraftSituation::isGfEqualAirborne(gnd1, gnd2)) { newSituation.setOnGround(false); break; }
+                    if (CAircraftSituation::isGfEqualOnGround(gnd1, gnd2)) { newSituation.setOnGround(true); break; }
                     const double newGnd = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, gnd1, gnd2, m_pa.dgnd[1], m_pa.dgnd[2]);
                     newSituation.setOnGroundFactor(newGnd);
-                    newSituation.setOnGroundFromGroundFactorFromInterpolation();
+                    newSituation.setOnGroundFromGroundFactorFromInterpolation(groundInterpolationFactor());
                 }
                 while (false);
             }