From 4cc564334088cf52fbe2d323f10da1f91792c7ac Mon Sep 17 00:00:00 2001 From: Mathew Sutcliffe Date: Sat, 4 Feb 2017 18:34:07 +0000 Subject: [PATCH] refs #863 Further decomposed interpolator into granularly replaceable strategy methods. --- src/blackmisc/simulation/interpolator.cpp | 101 +++++++++++------- src/blackmisc/simulation/interpolator.h | 33 ++++++ .../simulation/interpolatorlinear.cpp | 42 +++++--- src/blackmisc/simulation/interpolatorlinear.h | 35 +++++- 4 files changed, 155 insertions(+), 56 deletions(-) diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index 2889aab17..42547e7dc 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/src/blackmisc/simulation/interpolator.cpp @@ -51,12 +51,20 @@ namespace BlackMisc // any data at all? if (m_aircraftSituations.isEmpty()) { return {}; } + CAircraftSituation currentSituation = m_aircraftSituations.front(); // data, split situations by time if (currentTimeMsSinceEpoc < 0) { currentTimeMsSinceEpoc = QDateTime::currentMSecsSinceEpoch(); } - // algorithm provided by derived class - CAircraftSituation currentSituation = derived()->getInterpolatedPosition(callsign, currentTimeMsSinceEpoc, setup, hints, status, log); + // interpolant function from derived class + auto interpolant = derived()->getInterpolant(currentTimeMsSinceEpoc, setup, hints, status, log); + + // succeeded so far? + if (!status.didInterpolationSucceed()) { return currentSituation; } + + // use derived interpolant function + currentSituation.setPosition(interpolant.interpolatePosition(setup, hints)); + currentSituation.setAltitude(interpolant.interpolateAltitude(setup, hints)); // Update current position by hints' elevation // * for XP provided by hints.getElevationProvider at current position @@ -93,42 +101,11 @@ namespace BlackMisc if (setup.isForcingFullInterpolation() || hints.isVtolAircraft() || status.hasChangedPosition()) { - // HINT: VTOL aircraft can change pitch/bank without changing position, planes cannot - // Interpolate heading: HDG = (HdgB - HdgA) * t + HdgA - const CHeading headingBegin = oldSituation.getHeading(); - CHeading headingEnd = newSituation.getHeading(); - - if ((headingEnd - headingBegin).value(CAngleUnit::deg()) < -180) - { - headingEnd += CHeading(360, CHeading::Magnetic, CAngleUnit::deg()); - } - - if ((headingEnd - headingBegin).value(CAngleUnit::deg()) > 180) - { - headingEnd -= CHeading(360, CHeading::Magnetic, CAngleUnit::deg()); - } - - currentSituation.setHeading(CHeading((headingEnd - headingBegin) - * simulationTimeFraction - + headingBegin, - headingBegin.getReferenceNorth())); - - // Interpolate Pitch: Pitch = (PitchB - PitchA) * t + PitchA - const CAngle pitchBegin = oldSituation.getPitch(); - const CAngle pitchEnd = newSituation.getPitch(); - const CAngle pitch = (pitchEnd - pitchBegin) * simulationTimeFraction + pitchBegin; - currentSituation.setPitch(pitch); - - // Interpolate bank: Bank = (BankB - BankA) * t + BankA - const CAngle bankBegin = oldSituation.getBank(); - const CAngle bankEnd = newSituation.getBank(); - const CAngle bank = (bankEnd - bankBegin) * simulationTimeFraction + bankBegin; - currentSituation.setBank(bank); - - currentSituation.setGroundSpeed((newSituation.getGroundSpeed() - oldSituation.getGroundSpeed()) - * simulationTimeFraction - + oldSituation.getGroundSpeed()); - + const auto pbh = interpolant.pbh(); + currentSituation.setHeading(pbh.getHeading()); + currentSituation.setPitch(pbh.getPitch()); + currentSituation.setBank(pbh.getBank()); + currentSituation.setGroundSpeed(pbh.getGroundSpeed()); status.setChangedPosition(true); } status.setInterpolationSucceeded(true); @@ -146,6 +123,54 @@ namespace BlackMisc return currentSituation; } + 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 = oldSituation.getHeading(); + CHeading headingEnd = newSituation.getHeading(); + + if ((headingEnd - headingBegin).value(CAngleUnit::deg()) < -180) + { + headingEnd += CHeading(360, CHeading::Magnetic, CAngleUnit::deg()); + } + + if ((headingEnd - headingBegin).value(CAngleUnit::deg()) > 180) + { + headingEnd -= CHeading(360, CHeading::Magnetic, CAngleUnit::deg()); + } + + return CHeading((headingEnd - headingBegin) + * simulationTimeFraction + + headingBegin, + headingBegin.getReferenceNorth()); + } + + CAngle CInterpolatorPbh::getPitch() const + { + // Interpolate Pitch: Pitch = (PitchB - PitchA) * t + PitchA + const CAngle pitchBegin = oldSituation.getPitch(); + const CAngle pitchEnd = newSituation.getPitch(); + const CAngle pitch = (pitchEnd - pitchBegin) * simulationTimeFraction + pitchBegin; + return pitch; + } + + CAngle CInterpolatorPbh::getBank() const + { + // Interpolate bank: Bank = (BankB - BankA) * t + BankA + const CAngle bankBegin = oldSituation.getBank(); + const CAngle bankEnd = newSituation.getBank(); + const CAngle bank = (bankEnd - bankBegin) * simulationTimeFraction + bankBegin; + return bank; + } + + CSpeed CInterpolatorPbh::getGroundSpeed() const + { + return (newSituation.getGroundSpeed() - oldSituation.getGroundSpeed()) + * simulationTimeFraction + + oldSituation.getGroundSpeed(); + } + template CAircraftParts CInterpolator::getInterpolatedParts(const CCallsign &callsign, qint64 currentTimeMsSinceEpoch, const CInterpolationAndRenderingSetup &setup, CPartsStatus &partsStatus, bool log) const diff --git a/src/blackmisc/simulation/interpolator.h b/src/blackmisc/simulation/interpolator.h index 018ef9abb..59e7f9060 100644 --- a/src/blackmisc/simulation/interpolator.h +++ b/src/blackmisc/simulation/interpolator.h @@ -154,6 +154,39 @@ namespace BlackMisc mutable QList m_interpolationLogs; //!< logs of interpolation }; + //! Simple interpolator for pitch, bank, heading, groundspeed + class BLACKMISC_EXPORT CInterpolatorPbh + { + public: + //! Constructor + //! @{ + CInterpolatorPbh() + {} + CInterpolatorPbh(const Aviation::CAircraftSituation &older, const Aviation::CAircraftSituation &newer) : + oldSituation(older), newSituation(newer) + {} + CInterpolatorPbh(double time, const Aviation::CAircraftSituation &older, const Aviation::CAircraftSituation &newer) : + simulationTimeFraction(time), oldSituation(older), newSituation(newer) + {} + //! @} + + //! Getter + //! @{ + Aviation::CHeading getHeading() const; + PhysicalQuantities::CAngle getPitch() const; + PhysicalQuantities::CAngle getBank() const; + PhysicalQuantities::CSpeed getGroundSpeed() const; + //! @} + + //! Change time fraction + void setTimeFraction(double tf) { simulationTimeFraction = tf; } + + private: + double simulationTimeFraction = 0.0; + Aviation::CAircraftSituation oldSituation; + Aviation::CAircraftSituation newSituation; + }; + //! Status of interpolation struct BLACKMISC_EXPORT CInterpolationStatus { diff --git a/src/blackmisc/simulation/interpolatorlinear.cpp b/src/blackmisc/simulation/interpolatorlinear.cpp index fa27d77ce..06ebf9781 100644 --- a/src/blackmisc/simulation/interpolatorlinear.cpp +++ b/src/blackmisc/simulation/interpolatorlinear.cpp @@ -11,7 +11,6 @@ #include "blackmisc/aviation/aircraftsituation.h" #include "blackmisc/aviation/aircraftsituationlist.h" #include "blackmisc/aviation/altitude.h" -#include "blackmisc/aviation/callsign.h" #include "blackmisc/geo/coordinategeodetic.h" #include "blackmisc/pq/length.h" #include "blackmisc/pq/physicalquantity.h" @@ -36,9 +35,12 @@ namespace BlackMisc { namespace Simulation { - CAircraftSituation CInterpolatorLinear::getInterpolatedSituation(const CCallsign &callsign, qint64 currentTimeMsSinceEpoc, + CInterpolatorLinear::Interpolant CInterpolatorLinear::getInterpolant(qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints, CInterpolationStatus &status, InterpolationLog &log) const { + Q_UNUSED(setup); + Q_UNUSED(hints); + // find the first situation not in the correct order, keep only the situations before that one // any updates in wrong chronological order are discounted const auto end = std::is_sorted_until(m_aircraftSituations.begin(), m_aircraftSituations.end(), [](auto && a, auto && b) { return b.getAdjustedMSecsSinceEpoch() < a.getAdjustedMSecsSinceEpoch(); }); @@ -112,32 +114,44 @@ namespace BlackMisc currentSituation.setTimeOffsetMs(oldSituation.getTimeOffsetMs() + (newSituation.getTimeOffsetMs() - oldSituation.getTimeOffsetMs()) * simulationTimeFraction); currentSituation.setMSecsSinceEpoch(oldSituation.getMSecsSinceEpoch() + deltaTimeFractionMs); + status.setChangedPosition(oldSituation.getPosition() != newSituation.getPosition() || oldSituation.getAltitude() != newSituation.getAltitude()); + status.setInterpolationSucceeded(true); + + log.oldSituation = oldSituation; + log.newSituation = newSituation; + return { oldSituation, newSituation, simulationTimeFraction }; + } + + CCoordinateGeodetic CInterpolatorLinear::Interpolant::interpolatePosition(const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints) const + { + Q_UNUSED(setup); + Q_UNUSED(hints); + const std::array oldVec(oldSituation.getPosition().normalVectorDouble()); const std::array newVec(newSituation.getPosition().normalVectorDouble()); // Interpolate position: pos = (posB - posA) * t + posA + CCoordinateGeodetic currentPosition; currentPosition.setNormalVector((newVec[0] - oldVec[0]) * simulationTimeFraction + oldVec[0], (newVec[1] - oldVec[1]) * simulationTimeFraction + oldVec[1], (newVec[2] - oldVec[2]) * simulationTimeFraction + oldVec[2]); + return currentPosition; + } - currentSituation.setPosition(currentPosition); + CAltitude CInterpolatorLinear::Interpolant::interpolateAltitude(const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints) const + { + Q_UNUSED(setup); + Q_UNUSED(hints); // Interpolate altitude: Alt = (AltB - AltA) * t + AltA // avoid underflow below ground elevation by using getCorrectedAltitude const CAltitude oldAlt(oldSituation.getCorrectedAltitude()); const CAltitude newAlt(newSituation.getCorrectedAltitude()); Q_ASSERT_X(oldAlt.getReferenceDatum() == CAltitude::MeanSeaLevel && oldAlt.getReferenceDatum() == newAlt.getReferenceDatum(), Q_FUNC_INFO, "mismatch in reference"); // otherwise no calculation is possible - currentSituation.setAltitude(CAltitude((newAlt - oldAlt) - * simulationTimeFraction - + oldAlt, - oldAlt.getReferenceDatum())); - - status.setChangedPosition(newVec != oldVec || oldAlt != newAlt); - status.setInterpolationSucceeded(true); - - log.oldSituation = oldSituation; - log.newSituation = newSituation; - return currentSituation; + return CAltitude((newAlt - oldAlt) + * simulationTimeFraction + + oldAlt, + oldAlt.getReferenceDatum()); } } // namespace } // namespace diff --git a/src/blackmisc/simulation/interpolatorlinear.h b/src/blackmisc/simulation/interpolatorlinear.h index d08d5b603..fca1e0e5a 100644 --- a/src/blackmisc/simulation/interpolatorlinear.h +++ b/src/blackmisc/simulation/interpolatorlinear.h @@ -36,10 +36,37 @@ namespace BlackMisc CInterpolator("CInterpolatorLinear", parent) {} - //! \copydoc IInterpolator::getInterpolatedSituation - BlackMisc::Aviation::CAircraftSituation getInterpolatedSituation( - const BlackMisc::Aviation::CCallsign &callsign, qint64 currentTimeSinceEpoc, const CInterpolationAndRenderingSetup &setup, - const BlackMisc::Simulation::CInterpolationHints &hints, CInterpolationStatus &status, InterpolationLog &log) const; + //! Linear function that performs the actual interpolation + class Interpolant + { + public: + //! Constructor + //! @{ + Interpolant(const Aviation::CAircraftSituation &situation) : + situationsAvailable(1), oldSituation(situation) {} + Interpolant(const Aviation::CAircraftSituation &situation1, const Aviation::CAircraftSituation &situation2, double time) : + situationsAvailable(2), oldSituation(situation1), newSituation(situation2), simulationTimeFraction(time) {} + //! @} + + //! Perform the interpolation + //! @{ + Geo::CCoordinateGeodetic interpolatePosition(const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints) const; + Aviation::CAltitude interpolateAltitude(const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints) const; + //! @} + + //! Interpolator for pitch, bank, heading, groundspeed + CInterpolatorPbh pbh() const { return { simulationTimeFraction, oldSituation, newSituation }; } + + private: + int situationsAvailable = 0; + Aviation::CAircraftSituation oldSituation; + Aviation::CAircraftSituation newSituation; + double simulationTimeFraction = 0.0; + }; + + //! Get the interpolant for the given time point + Interpolant getInterpolant(qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetup &setup, + const CInterpolationHints &hints, CInterpolationStatus &status, InterpolationLog &log) const; //! Log category static QString getLogCategory() { return "swift.interpolatorlinear"; }