From 6d5509890b42d49393484268d558d1f4d5542e4b Mon Sep 17 00:00:00 2001 From: Klaus Basan Date: Thu, 25 Jan 2018 23:20:45 +0100 Subject: [PATCH] Ref T238, moved spline x,y,z ... values to interpolant * only interpolant needs those values * encapsulated in struct PosArray * Interpolant needs no reference to interpolator (anymore) * const CInterpolatorPbh &pbh() --- .../simulation/interpolatorlinear.cpp | 12 +++ src/blackmisc/simulation/interpolatorlinear.h | 9 +- .../simulation/interpolatorspline.cpp | 87 ++++++++++++------- src/blackmisc/simulation/interpolatorspline.h | 46 +++++++--- 4 files changed, 109 insertions(+), 45 deletions(-) diff --git a/src/blackmisc/simulation/interpolatorlinear.cpp b/src/blackmisc/simulation/interpolatorlinear.cpp index 57d355ed9..42b7047dc 100644 --- a/src/blackmisc/simulation/interpolatorlinear.cpp +++ b/src/blackmisc/simulation/interpolatorlinear.cpp @@ -35,6 +35,18 @@ namespace BlackMisc { namespace Simulation { + CInterpolatorLinear::Interpolant::Interpolant(const CAircraftSituation &situation) : + m_situationsAvailable(1), m_oldSituation(situation), + m_pbh(0, situation, situation) + {} + + CInterpolatorLinear::Interpolant::Interpolant(const CAircraftSituation &situation1, const CAircraftSituation &situation2, double timeFraction) : + m_situationsAvailable(2), + m_oldSituation(situation1), m_newSituation(situation2), + m_simulationTimeFraction(timeFraction), + m_pbh(m_simulationTimeFraction, situation1, situation2) + {} + CInterpolatorLinear::Interpolant CInterpolatorLinear::getInterpolant(qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints, CInterpolationStatus &status, CInterpolationLogger::SituationLog &log) const { diff --git a/src/blackmisc/simulation/interpolatorlinear.h b/src/blackmisc/simulation/interpolatorlinear.h index 66b0af232..32ae5b0d7 100644 --- a/src/blackmisc/simulation/interpolatorlinear.h +++ b/src/blackmisc/simulation/interpolatorlinear.h @@ -43,10 +43,8 @@ namespace BlackMisc public: //! Constructor //! @{ - Interpolant(const Aviation::CAircraftSituation &situation) : - m_situationsAvailable(1), m_oldSituation(situation) {} - Interpolant(const Aviation::CAircraftSituation &situation1, const Aviation::CAircraftSituation &situation2, double time) : - m_situationsAvailable(2), m_oldSituation(situation1), m_newSituation(situation2), m_simulationTimeFraction(time) {} + Interpolant(const Aviation::CAircraftSituation &situation); + Interpolant(const Aviation::CAircraftSituation &situation1, const Aviation::CAircraftSituation &situation2, double timeFraction); //! @} //! Perform the interpolation @@ -56,13 +54,14 @@ namespace BlackMisc //! @} //! Interpolator for pitch, bank, heading, groundspeed - CInterpolatorPbh pbh() const { return { m_simulationTimeFraction, m_oldSituation, m_newSituation }; } + const CInterpolatorPbh &pbh() const { return m_pbh; } private: int m_situationsAvailable = 0; Aviation::CAircraftSituation m_oldSituation; Aviation::CAircraftSituation m_newSituation; double m_simulationTimeFraction = 0.0; + const CInterpolatorPbh m_pbh; }; //! Get the interpolant for the given time point diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp index c7329ae2b..f46813327 100644 --- a/src/blackmisc/simulation/interpolatorspline.cpp +++ b/src/blackmisc/simulation/interpolatorspline.cpp @@ -110,7 +110,7 @@ namespace BlackMisc if (situationsNewer.isEmpty() || situationsOlder.size() < 2) { - return { *this, 0 }; + return m_interpolant; } std::array s {{ *(situationsOlder.begin() + 1), *situationsOlder.begin(), *(situationsNewer.end() - 1) }}; @@ -125,9 +125,11 @@ namespace BlackMisc if (!hints.getElevationPlane().isNull()) { const CAltitude groundElevation = hints.getElevationPlane().getAltitude(); - s[0].setGroundElevation(groundElevation); - s[1].setGroundElevation(groundElevation); - s[2].setGroundElevation(groundElevation); + + // do not override existing values + s[0].setGroundElevationChecked(groundElevation); + s[1].setGroundElevationChecked(groundElevation); + s[2].setGroundElevationChecked(groundElevation); } const double a0 = s[0].getCorrectedAltitude(hints.getCGAboveGround()).value(); @@ -135,36 +137,36 @@ namespace BlackMisc const double a2 = s[2].getCorrectedAltitude(hints.getCGAboveGround()).value(); const std::array, 3> normals {{ s[0].getPosition().normalVectorDouble(), s[1].getPosition().normalVectorDouble(), s[2].getPosition().normalVectorDouble() }}; - x = {{ normals[0][0], normals[1][0], normals[2][0] }}; - y = {{ normals[0][1], normals[1][1], normals[2][1] }}; - z = {{ normals[0][2], normals[1][2], normals[2][2] }}; - a = {{ a0, a1, a2 }}; - t = {{ static_cast(s[0].getAdjustedMSecsSinceEpoch()), static_cast(s[1].getAdjustedMSecsSinceEpoch()), static_cast(s[2].getAdjustedMSecsSinceEpoch()) }}; + PosArray pa; + pa.x = {{ normals[0][0], normals[1][0], normals[2][0] }}; + pa.y = {{ normals[0][1], normals[1][1], normals[2][1] }}; + pa.z = {{ normals[0][2], normals[1][2], normals[2][2] }}; + pa.a = {{ a0, a1, a2 }}; + pa.t = {{ static_cast(s[0].getAdjustedMSecsSinceEpoch()), static_cast(s[1].getAdjustedMSecsSinceEpoch()), static_cast(s[2].getAdjustedMSecsSinceEpoch()) }}; - dx = getDerivatives(t, x); - dy = getDerivatives(t, y); - dz = getDerivatives(t, z); - da = getDerivatives(t, a); + pa.dx = getDerivatives(pa.t, pa.x); + pa.dy = getDerivatives(pa.t, pa.y); + pa.dz = getDerivatives(pa.t, pa.z); + pa.da = getDerivatives(pa.t, pa.a); m_prevSampleTime = situationsOlder.begin()->getAdjustedMSecsSinceEpoch(); m_nextSampleTime = (situationsNewer.end() - 1)->getAdjustedMSecsSinceEpoch(); - m_altitudeUnit = situationsOlder.begin()->getAltitude().getUnit(); - m_pbh = { *situationsOlder.begin(), *(situationsNewer.end() - 1) }; + m_interpolant = Interpolant(pa, situationsOlder.begin()->getAltitude().getUnit(), { *situationsOlder.begin(), *(situationsNewer.end() - 1) }); } - log.interpolator = 's'; - log.oldSituation = m_pbh.getOldSituation(); - log.newSituation = m_pbh.getNewSituation(); - status.setInterpolated(true); const double dt1 = static_cast(currentTimeMsSinceEpoc - m_prevSampleTime); const double dt2 = static_cast(m_nextSampleTime - m_prevSampleTime); const double timeFraction = dt1 / dt2; + log.interpolator = 's'; + log.oldSituation = m_interpolant.pbh().getOldSituation(); + log.newSituation = m_interpolant.pbh().getNewSituation(); log.deltaTimeMs = dt1; log.deltaTimeFractionMs = dt2; log.simulationTimeFraction = timeFraction; - m_pbh.setTimeFraction(timeFraction); - return { *this, currentTimeMsSinceEpoc }; + status.setInterpolated(true); + m_interpolant.setTimes(currentTimeMsSinceEpoc, timeFraction); + return m_interpolant; } CCoordinateGeodetic CInterpolatorSpline::Interpolant::interpolatePosition(const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints) const @@ -172,9 +174,9 @@ namespace BlackMisc Q_UNUSED(setup); Q_UNUSED(hints); - const double newX = evalSplineInterval(currentTimeMsSinceEpoc, i.t[1], i.t[2], i.x[1], i.x[2], i.dx[1], i.dx[2]); - const double newY = evalSplineInterval(currentTimeMsSinceEpoc, i.t[1], i.t[2], i.y[1], i.y[2], i.dy[1], i.dy[2]); - const double newZ = evalSplineInterval(currentTimeMsSinceEpoc, i.t[1], i.t[2], i.z[1], i.z[2], i.dz[1], i.dz[2]); + const double newX = evalSplineInterval(m_currentTimeMsSinceEpoc, m_pa.t[1], m_pa.t[2], m_pa.x[1], m_pa.x[2], m_pa.dx[1], m_pa.dx[2]); + const double newY = evalSplineInterval(m_currentTimeMsSinceEpoc, m_pa.t[1], m_pa.t[2], m_pa.y[1], m_pa.y[2], m_pa.dy[1], m_pa.dy[2]); + const double newZ = evalSplineInterval(m_currentTimeMsSinceEpoc, m_pa.t[1], m_pa.t[2], m_pa.z[1], m_pa.z[2], m_pa.dz[1], m_pa.dz[2]); CCoordinateGeodetic currentPosition; currentPosition.setNormalVector(newX, newY, newZ); @@ -186,9 +188,36 @@ namespace BlackMisc Q_UNUSED(setup); Q_UNUSED(hints); - const double newA = evalSplineInterval(currentTimeMsSinceEpoc, i.t[1], i.t[2], i.a[1], i.a[2], i.da[1], i.da[2]); - - return CAltitude(newA, i.m_altitudeUnit); + const double newA = evalSplineInterval(m_currentTimeMsSinceEpoc, m_pa.t[1], m_pa.t[2], m_pa.a[1], m_pa.a[2], m_pa.da[1], m_pa.da[2]); + return CAltitude(newA, m_altitudeUnit); } - } -} + + void CInterpolatorSpline::Interpolant::setTimes(qint64 currentTimeMs, double timeFraction) + { + m_currentTimeMsSinceEpoc = currentTimeMs; + m_pbh.setTimeFraction(timeFraction); + } + + void CInterpolatorSpline::PosArray::initToZero() + { + for (int i = 0; i < 3; i++) + { + x[i] = 0; y[i] = 0; z[i] = 0; + a[i] = 0; t[i] = 0; + dx[i] = 0; dy[i] = 0; dz[i] = 0; + da[i] = 0; + } + } + + const CInterpolatorSpline::PosArray &CInterpolatorSpline::PosArray::zeroPosArray() + { + static const PosArray pa = [] + { + PosArray p; + p.initToZero(); + return p; + }(); + return pa; + } + } // ns +} // ns diff --git a/src/blackmisc/simulation/interpolatorspline.h b/src/blackmisc/simulation/interpolatorspline.h index a2f722a76..6c4f422cd 100644 --- a/src/blackmisc/simulation/interpolatorspline.h +++ b/src/blackmisc/simulation/interpolatorspline.h @@ -30,16 +30,36 @@ namespace BlackMisc public: //! Constructor - CInterpolatorSpline(const BlackMisc::Aviation::CCallsign &callsign, QObject *parent = nullptr) : + CInterpolatorSpline(const Aviation::CCallsign &callsign, QObject *parent = nullptr) : CInterpolator("CInterpolatorSpline", callsign, parent) {} + //! Position arrays for interpolation + struct PosArray + { + //! Init all values to zero + void initToZero(); + + //! Zero initialized position array + static const PosArray &zeroPosArray(); + + //! 3 coordinates for spline interpolation + //! @{ + std::array x, y, z, a, t, dx, dy, dz, da; + //! @} + }; + //! Cubic function that performs the actual interpolation class Interpolant { public: + //! Default + Interpolant() : m_pa(PosArray::zeroPosArray()) {} + //! Constructor - Interpolant(const CInterpolatorSpline &interpolator, qint64 time) : i(interpolator), currentTimeMsSinceEpoc(time) {} + Interpolant( + const PosArray &pa, const PhysicalQuantities::CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh) : + m_pa(pa), m_altitudeUnit(altitudeUnit), m_pbh(pbh) {} //! Perform the interpolation //! @{ @@ -48,26 +68,30 @@ namespace BlackMisc //! @} //! Interpolator for pitch, bank, heading, groundspeed - CInterpolatorPbh pbh() const { return i.m_pbh; } + const CInterpolatorPbh &pbh() const { return m_pbh; } + + //! Set the time values + void setTimes(qint64 currentTimeMs, double timeFraction); private: - const CInterpolatorSpline &i; - qint64 currentTimeMsSinceEpoc = 0; + PosArray m_pa; + PhysicalQuantities::CLengthUnit m_altitudeUnit; + CInterpolatorPbh m_pbh; + qint64 m_currentTimeMsSinceEpoc { 0 }; }; //! Strategy used by CInterpolator::getInterpolatedSituation Interpolant getInterpolant( - qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetup &setup, + qint64 currentTimeMsSinceEpoc, + const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints, CInterpolationStatus &status, CInterpolationLogger::SituationLog &log); private: qint64 m_prevSampleTime = 0; qint64 m_nextSampleTime = 0; - PhysicalQuantities::CLengthUnit m_altitudeUnit; - std::array x, y, z, a, t, dx, dy, dz, da; - CInterpolatorPbh m_pbh; + Interpolant m_interpolant; }; - } -} + } // ns +} // ns #endif