From 1a6d7fba28a04c7c3fc93d8bd0cba62060fe7125 Mon Sep 17 00:00:00 2001 From: Lars Toenning Date: Thu, 4 Jan 2024 12:33:43 +0100 Subject: [PATCH] refactor: Clarify variable names in linear interpolator --- .../simulation/interpolatorlinear.cpp | 125 +++++++++--------- src/blackmisc/simulation/interpolatorlinear.h | 20 +-- 2 files changed, 71 insertions(+), 74 deletions(-) diff --git a/src/blackmisc/simulation/interpolatorlinear.cpp b/src/blackmisc/simulation/interpolatorlinear.cpp index 9860c3c66..fd7b4b9f6 100644 --- a/src/blackmisc/simulation/interpolatorlinear.cpp +++ b/src/blackmisc/simulation/interpolatorlinear.cpp @@ -29,19 +29,19 @@ using namespace BlackMisc::Simulation; namespace BlackMisc::Simulation { - CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &oldSituation) : IInterpolant(1, CInterpolatorPbh(0, oldSituation, oldSituation)), - m_oldSituation(oldSituation) + CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &startSituation) : IInterpolant(1, CInterpolatorPbh(0, startSituation, startSituation)), + m_startSituation(startSituation) {} - CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &oldSituation, const CInterpolatorPbh &pbh) : IInterpolant(1, pbh), - m_oldSituation(oldSituation) + CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &startSituation, const CInterpolatorPbh &pbh) : IInterpolant(1, pbh), + m_startSituation(startSituation) {} - CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation, double timeFraction, qint64 interpolatedTime) : IInterpolant(interpolatedTime, 2), - m_oldSituation(oldSituation), m_newSituation(newSituation), - m_simulationTimeFraction(timeFraction) + CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &startSituation, const CAircraftSituation &endSituation, double timeFraction, qint64 interpolatedTime) : IInterpolant(interpolatedTime, 2), + m_startSituation(startSituation), m_endSituation(endSituation), + m_simulationTimeFraction(timeFraction) { - m_pbh = CInterpolatorPbh(m_simulationTimeFraction, oldSituation, newSituation); + m_pbh = CInterpolatorPbh(m_simulationTimeFraction, startSituation, endSituation); } void CInterpolatorLinear::anchor() @@ -49,76 +49,73 @@ namespace BlackMisc::Simulation CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const { - const std::array oldVec(m_oldSituation.getPosition().normalVectorDouble()); - const std::array newVec(m_newSituation.getPosition().normalVectorDouble()); + const std::array startVec(m_startSituation.getPosition().normalVectorDouble()); + const std::array endVec(m_endSituation.getPosition().normalVectorDouble()); if (CBuildConfig::isLocalDeveloperDebugBuild()) { - BLACK_VERIFY_X(CAircraftSituation::isValidVector(oldVec), Q_FUNC_INFO, "Invalid old vector"); - BLACK_VERIFY_X(CAircraftSituation::isValidVector(newVec), Q_FUNC_INFO, "Invalid new vector"); + BLACK_VERIFY_X(CAircraftSituation::isValidVector(startVec), Q_FUNC_INFO, "Invalid start vector"); + BLACK_VERIFY_X(CAircraftSituation::isValidVector(endVec), Q_FUNC_INFO, "Invalid end vector"); BLACK_VERIFY_X(isAcceptableTimeFraction(m_simulationTimeFraction), Q_FUNC_INFO, "Invalid fraction"); } // Interpolate position: pos = (posB - posA) * t + posA - CCoordinateGeodetic newPosition; + CCoordinateGeodetic interpolatedPosition; const double tf = clampValidTimeFraction(m_simulationTimeFraction); - newPosition.setNormalVector((newVec[0] - oldVec[0]) * tf + oldVec[0], - (newVec[1] - oldVec[1]) * tf + oldVec[1], - (newVec[2] - oldVec[2]) * tf + oldVec[2]); + interpolatedPosition.setNormalVector((endVec[0] - startVec[0]) * tf + startVec[0], + (endVec[1] - startVec[1]) * tf + startVec[1], + (endVec[2] - startVec[2]) * tf + startVec[2]); if (CBuildConfig::isLocalDeveloperDebugBuild()) { - BLACK_VERIFY_X(newPosition.isValidVectorRange(), Q_FUNC_INFO, "Invalid vector"); + BLACK_VERIFY_X(interpolatedPosition.isValidVectorRange(), Q_FUNC_INFO, "Invalid vector"); } // Interpolate altitude: Alt = (AltB - AltA) * t + AltA // avoid underflow below ground elevation by using getCorrectedAltitude - const CAltitude oldAlt(m_oldSituation.getCorrectedAltitude()); - const CAltitude newAlt(m_newSituation.getCorrectedAltitude()); + const CAltitude oldAlt(m_startSituation.getCorrectedAltitude()); + const CAltitude newAlt(m_endSituation.getCorrectedAltitude()); Q_ASSERT_X(oldAlt.getReferenceDatum() == CAltitude::MeanSeaLevel && oldAlt.getReferenceDatum() == newAlt.getReferenceDatum(), Q_FUNC_INFO, "mismatch in reference"); // otherwise no calculation is possible const CAltitude altitude((newAlt - oldAlt) * tf + oldAlt, oldAlt.getReferenceDatum()); - CAircraftSituation newSituation(situation); - newSituation.setPosition(newPosition); - newSituation.setAltitude(altitude); - newSituation.setMSecsSinceEpoch(this->getInterpolatedTime()); + CAircraftSituation interpolatedSituation(situation); + interpolatedSituation.setPosition(interpolatedPosition); + interpolatedSituation.setAltitude(altitude); + interpolatedSituation.setMSecsSinceEpoch(this->getInterpolatedTime()); if (interpolateGndFactor) { - const double oldGroundFactor = m_oldSituation.getOnGroundFactor(); - const double newGroundFactor = m_newSituation.getOnGroundFactor(); - do + const double startGroundFactor = m_startSituation.getOnGroundFactor(); + const double endGroundFactor = m_endSituation.getOnGroundFactor(); + if (CAircraftSituation::isGfEqualAirborne(startGroundFactor, endGroundFactor)) { - if (CAircraftSituation::isGfEqualAirborne(oldGroundFactor, newGroundFactor)) - { - newSituation.setOnGround(false); - break; - } - if (CAircraftSituation::isGfEqualOnGround(oldGroundFactor, newGroundFactor)) - { - newSituation.setOnGround(true); - break; - } - const double groundFactor = (newGroundFactor - oldGroundFactor) * tf + oldGroundFactor; - newSituation.setOnGroundFactor(groundFactor); - newSituation.setOnGroundFromGroundFactorFromInterpolation(groundInterpolationFactor()); + interpolatedSituation.setOnGround(false); + } + else if (CAircraftSituation::isGfEqualOnGround(startGroundFactor, endGroundFactor)) + { + interpolatedSituation.setOnGround(true); + } + else + { + const double interpolatedGroundFactor = (endGroundFactor - startGroundFactor) * tf + startGroundFactor; + interpolatedSituation.setOnGroundFactor(interpolatedGroundFactor); + interpolatedSituation.setOnGroundFromGroundFactorFromInterpolation(groundInterpolationFactor()); } - while (false); } - return newSituation; + return interpolatedSituation; } CInterpolatorLinear::CInterpolant CInterpolatorLinear::getInterpolant(SituationLog &log) { // set default situations - CAircraftSituation oldSituation = m_interpolant.getOldSituation(); - CAircraftSituation newSituation = m_interpolant.getNewSituation(); + CAircraftSituation startSituation = m_interpolant.getStartSituation(); + CAircraftSituation endSituation = m_interpolant.getEndSituation(); - Q_ASSERT_X(newSituation.getAdjustedMSecsSinceEpoch() >= oldSituation.getAdjustedMSecsSinceEpoch(), Q_FUNC_INFO, "Wrong order"); + Q_ASSERT_X(endSituation.getAdjustedMSecsSinceEpoch() >= startSituation.getAdjustedMSecsSinceEpoch(), Q_FUNC_INFO, "Wrong order"); const bool updated = m_situationsLastModifiedUsed < m_situationsLastModified; - const bool newSplit = newSituation.getAdjustedMSecsSinceEpoch() < m_currentTimeMsSinceEpoch; + const bool newSplit = endSituation.getAdjustedMSecsSinceEpoch() < m_currentTimeMsSinceEpoch; const bool recalculate = updated || newSplit; if (recalculate) @@ -162,33 +159,33 @@ namespace BlackMisc::Simulation } // extrapolate from two before situations - oldSituation = *(situationsOlder.begin() + 1); // before newest - newSituation = situationsOlder.front(); // newest + startSituation = *(situationsOlder.begin() + 1); // before newest + endSituation = situationsOlder.front(); // newest } else { - oldSituation = situationsOlder.front(); // first oldest (aka newest oldest) - newSituation = *(situationsNewer.end() - 1); // latest newest (aka oldest of newer block) - Q_ASSERT(oldSituation.getAdjustedMSecsSinceEpoch() < newSituation.getAdjustedMSecsSinceEpoch()); + startSituation = situationsOlder.front(); // first oldest (aka newest oldest) + endSituation = *(situationsNewer.end() - 1); // latest newest (aka oldest of newer block) + Q_ASSERT(startSituation.getAdjustedMSecsSinceEpoch() < endSituation.getAdjustedMSecsSinceEpoch()); } // adjust ground if required - if (!oldSituation.canLikelySkipNearGroundInterpolation() && !oldSituation.hasGroundElevation()) + if (!startSituation.canLikelySkipNearGroundInterpolation() && !startSituation.hasGroundElevation()) { - const CElevationPlane planeOld = this->findClosestElevationWithinRange(oldSituation, CElevationPlane::singlePointRadius()); - oldSituation.setGroundElevationChecked(planeOld, CAircraftSituation::FromCache); + const CElevationPlane planeOld = this->findClosestElevationWithinRange(startSituation, CElevationPlane::singlePointRadius()); + startSituation.setGroundElevationChecked(planeOld, CAircraftSituation::FromCache); } - if (!newSituation.canLikelySkipNearGroundInterpolation() && !newSituation.hasGroundElevation()) + if (!endSituation.canLikelySkipNearGroundInterpolation() && !endSituation.hasGroundElevation()) { - const CElevationPlane planeNew = this->findClosestElevationWithinRange(newSituation, CElevationPlane::singlePointRadius()); - newSituation.setGroundElevationChecked(planeNew, CAircraftSituation::FromCache); + const CElevationPlane planeNew = this->findClosestElevationWithinRange(endSituation, CElevationPlane::singlePointRadius()); + endSituation.setGroundElevationChecked(planeNew, CAircraftSituation::FromCache); } } // modified situations - CAircraftSituation currentSituation(oldSituation); // also sets ground elevation if available + CAircraftSituation currentSituation(startSituation); // also sets ground elevation if available // Time between start and end packet - const qint64 sampleDeltaTimeMs = newSituation.getAdjustedMSecsSinceEpoch() - oldSituation.getAdjustedMSecsSinceEpoch(); + const qint64 sampleDeltaTimeMs = endSituation.getAdjustedMSecsSinceEpoch() - startSituation.getAdjustedMSecsSinceEpoch(); Q_ASSERT_X(sampleDeltaTimeMs >= 0, Q_FUNC_INFO, "Negative delta time"); log.interpolator = 'l'; @@ -196,7 +193,7 @@ namespace BlackMisc::Simulation // < 0 should not happen due to the split, > 1 can happen if new values are delayed beyond split time // 1) values > 1 mean extrapolation // 2) values > 2 mean no new situations coming in - const double distanceToSplitTimeMs = newSituation.getAdjustedMSecsSinceEpoch() - m_currentTimeMsSinceEpoch; + const double distanceToSplitTimeMs = endSituation.getAdjustedMSecsSinceEpoch() - m_currentTimeMsSinceEpoch; double simulationTimeFraction = qMax(1.0 - (distanceToSplitTimeMs / sampleDeltaTimeMs), 0.0); if (simulationTimeFraction >= 1.0) { @@ -205,10 +202,10 @@ namespace BlackMisc::Simulation } const double deltaTimeFractionMs = sampleDeltaTimeMs * simulationTimeFraction; - const qint64 interpolatedTime = oldSituation.getMSecsSinceEpoch() + qRound(deltaTimeFractionMs); + const qint64 interpolatedTime = startSituation.getMSecsSinceEpoch() + qRound(deltaTimeFractionMs); // Ref T297 adjust offset time, but this already the interpolated situation - currentSituation.setTimeOffsetMs(oldSituation.getTimeOffsetMs() + qRound((newSituation.getTimeOffsetMs() - oldSituation.getTimeOffsetMs()) * simulationTimeFraction)); + currentSituation.setTimeOffsetMs(startSituation.getTimeOffsetMs() + qRound((endSituation.getTimeOffsetMs() - startSituation.getTimeOffsetMs()) * simulationTimeFraction)); currentSituation.setMSecsSinceEpoch(interpolatedTime); m_currentInterpolationStatus.setInterpolatedAndCheckSituation(true, currentSituation); @@ -220,12 +217,12 @@ namespace BlackMisc::Simulation log.deltaSampleTimesMs = sampleDeltaTimeMs; log.tsInterpolated = interpolatedTime; log.interpolationSituations.clear(); - log.interpolationSituations.push_back(oldSituation); // oldest at front - log.interpolationSituations.push_back(newSituation); // latest at back + log.interpolationSituations.push_back(startSituation); // oldest at front + log.interpolationSituations.push_back(endSituation); // latest at back log.interpolantRecalc = recalculate; } - m_interpolant = { oldSituation, newSituation, simulationTimeFraction, interpolatedTime }; + m_interpolant = { startSituation, endSituation, simulationTimeFraction, interpolatedTime }; m_interpolant.setRecalculated(recalculate); return m_interpolant; diff --git a/src/blackmisc/simulation/interpolatorlinear.h b/src/blackmisc/simulation/interpolatorlinear.h index 56ee024cb..8d58aed7e 100644 --- a/src/blackmisc/simulation/interpolatorlinear.h +++ b/src/blackmisc/simulation/interpolatorlinear.h @@ -41,10 +41,10 @@ namespace BlackMisc public: //! @{ //! Constructor - CInterpolant() {} - CInterpolant(const Aviation::CAircraftSituation &oldSituation); - CInterpolant(const Aviation::CAircraftSituation &oldSituation, const CInterpolatorPbh &pbh); - CInterpolant(const Aviation::CAircraftSituation &oldSituation, const Aviation::CAircraftSituation &newSituation, double timeFraction, qint64 interpolatedTime); + CInterpolant() = default; + CInterpolant(const Aviation::CAircraftSituation &startSituation); + CInterpolant(const Aviation::CAircraftSituation &startSituation, const CInterpolatorPbh &pbh); + CInterpolant(const Aviation::CAircraftSituation &startSituation, const Aviation::CAircraftSituation &endSituation, double timeFraction, qint64 interpolatedTime); //! @} //! Perform the interpolation @@ -53,15 +53,15 @@ namespace BlackMisc //! \return \p situation with interpolated position and altitude, updated timestamp and possibly interpolated GND factor Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation, bool interpolateGndFactor) const; - //! Old situation - const Aviation::CAircraftSituation &getOldSituation() const { return m_oldSituation; } + //! Start situation + const Aviation::CAircraftSituation &getStartSituation() const { return m_startSituation; } - //! New situation - const Aviation::CAircraftSituation &getNewSituation() const { return m_newSituation; } + //! End situation + const Aviation::CAircraftSituation &getEndSituation() const { return m_endSituation; } private: - Aviation::CAircraftSituation m_oldSituation; - Aviation::CAircraftSituation m_newSituation; + Aviation::CAircraftSituation m_startSituation; + Aviation::CAircraftSituation m_endSituation; double m_simulationTimeFraction = 0.0; //!< 0..1 };