diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index 42547e7dc..aa2844e82 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/src/blackmisc/simulation/interpolator.cpp @@ -11,6 +11,7 @@ #include "blackconfig/buildconfig.h" #include "blackmisc/simulation/interpolationhints.h" #include "blackmisc/simulation/interpolatorlinear.h" +#include "blackmisc/simulation/interpolatorspline.h" #include "blackmisc/aviation/callsign.h" #include "blackmisc/aviation/heading.h" #include "blackmisc/pq/angle.h" @@ -44,7 +45,7 @@ namespace BlackMisc template CAircraftSituation CInterpolator::getInterpolatedSituation(const CCallsign &callsign, qint64 currentTimeMsSinceEpoc, - const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints, CInterpolationStatus &status) const + const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints, CInterpolationStatus &status) { status.reset(); InterpolationLog log; @@ -173,7 +174,7 @@ namespace BlackMisc template CAircraftParts CInterpolator::getInterpolatedParts(const CCallsign &callsign, qint64 currentTimeMsSinceEpoch, - const CInterpolationAndRenderingSetup &setup, CPartsStatus &partsStatus, bool log) const + const CInterpolationAndRenderingSetup &setup, CPartsStatus &partsStatus, bool log) { Q_UNUSED(setup); partsStatus.reset(); @@ -557,6 +558,7 @@ namespace BlackMisc // https://isocpp.org/wiki/faq/templates#separate-template-fn-defn-from-decl //! \cond PRIVATE template class BLACKMISC_EXPORT_DEFINE_TEMPLATE CInterpolator; + template class BLACKMISC_EXPORT_DEFINE_TEMPLATE CInterpolator; //! \endcond } // namespace } // namespace diff --git a/src/blackmisc/simulation/interpolator.h b/src/blackmisc/simulation/interpolator.h index 59e7f9060..daf77790d 100644 --- a/src/blackmisc/simulation/interpolator.h +++ b/src/blackmisc/simulation/interpolator.h @@ -30,6 +30,7 @@ namespace BlackMisc { class CInterpolationHints; class CInterpolatorLinear; + class CInterpolatorSpline; struct CInterpolationStatus; struct CPartsStatus; @@ -44,12 +45,12 @@ namespace BlackMisc //! Current interpolated situation BlackMisc::Aviation::CAircraftSituation getInterpolatedSituation( const BlackMisc::Aviation::CCallsign &callsign, qint64 currentTimeSinceEpoc, - const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints, CInterpolationStatus &status) const; + const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints, CInterpolationStatus &status); //! Parts before given offset time (aka pending parts) BlackMisc::Aviation::CAircraftParts getInterpolatedParts( const Aviation::CCallsign &callsign, qint64 cutoffTime, - const CInterpolationAndRenderingSetup &setup, CPartsStatus &partsStatus, bool log = false) const; + const CInterpolationAndRenderingSetup &setup, CPartsStatus &partsStatus, bool log = false); //! Add a new aircraft situation void addAircraftSituation(const BlackMisc::Aviation::CAircraftSituation &situation); @@ -236,6 +237,7 @@ namespace BlackMisc //! \cond PRIVATE extern template class BLACKMISC_EXPORT_DECLARE_TEMPLATE CInterpolator; + extern template class BLACKMISC_EXPORT_DECLARE_TEMPLATE CInterpolator; //! \endcond } // namespace } // namespace diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp new file mode 100644 index 000000000..def673207 --- /dev/null +++ b/src/blackmisc/simulation/interpolatorspline.cpp @@ -0,0 +1,164 @@ +/* Copyright (C) 2017 + * swift project Community / Contributors + * + * This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level + * directory of this distribution and at http://www.swift-project.org/license.html. No part of swift project, + * including this file, may be copied, modified, propagated, or distributed except according to the terms + * contained in the LICENSE file. + */ + +#include "blackmisc/simulation/interpolatorspline.h" +#include "blackmisc/simulation/interpolationhints.h" +#include "blackmisc/logmessage.h" + +using namespace BlackMisc::Aviation; +using namespace BlackMisc::Geo; +using namespace BlackMisc::Math; +using namespace BlackMisc::PhysicalQuantities; +using namespace BlackMisc::Simulation; + +namespace BlackMisc +{ + namespace Simulation + { + namespace + { + //! \private https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm + template + std::array solveTridiagonal(std::array, N> &matrix, std::array &d) + { + const auto a = [&matrix](auto i) -> double& { return matrix[i][i-1]; }; // subdiagonal + const auto b = [&matrix](auto i) -> double& { return matrix[i][i ]; }; // main diagonal + const auto c = [&matrix](auto i) -> double& { return matrix[i][i+1]; }; // superdiagonal + + // forward sweep + c(0) /= b(0); + d[0] /= b(0); + for (size_t i = 1; i < N; ++i) + { + const double denom = b(i) - a(i) * c(i - 1); + if (i < N-1) { c(i) /= denom; } + d[i] = (d[i] - a(i) * d[i - 1]) / denom; + } + + // back substitution + for (int i = N - 2; i >= 0; --i) + { + d[i] -= c(i) * d[i+1]; + } + return d; + } + + //! \private Linear equation expressed as tridiagonal matrix. + //! https://en.wikipedia.org/wiki/Spline_interpolation + //! http://blog.ivank.net/interpolation-with-cubic-splines.html + template + std::array getDerivatives(const std::array &x, const std::array &y) + { + std::array, N> a {{}}; + std::array b {{}}; + + a[0][0] = 2.0 / (x[1] - x[0]); + a[0][1] = 1.0 / (x[1] - x[0]); + b[0] = 3.0 * (y[1] - y[0]) / ((x[1] - x[0]) * (x[1] - x[0])); + + a[N-1][N-2] = 1.0 / (x[N-1] - x[N-2]); + a[N-1][N-1] = 2.0 / (x[N-1] - x[N-2]); + b[N-1] = 3.0 * (y[N-1] - y[N-2]) / ((x[N-1] - x[N-2]) * (x[N-1] - x[N-2])); + + for (size_t i = 1; i < N - 1; ++i) + { + a[i][i-1] = 1.0 / (x[i] - x[i-1]); + a[i][i ] = 2.0 / (x[i] - x[i-1]) + 2.0 / (x[i+1] - x[i]); + a[i][i+1] = 1.0 / (x[i+1] - x[i]); + b[i] = 3.0 * (y[i] - y[i-1]) / ((x[i] - x[i-1]) * (x[i] - x[i-1])) + + 3.0 * (y[i+1] - y[i]) / ((x[i+1] - x[i]) * (x[i+1] - x[i])); + } + solveTridiagonal(a, b); + return b; + } + + //! \private Cubic interpolation. + double evalSplineInterval(double x, double x0, double x1, double y0, double y1, double k0, double k1) + { + const double t = (x - x0) / (x1 - x0); + const double a = k0 * (x1 - x0) - (y1 - y0); + const double b = -k1 * (x1 - x0) + (y1 - y0); + const double y = (1 - t) * y0 + t * y1 + t * (1 - t) * (a * (1 - t) + b * t); + return y; + } + } + + CInterpolatorSpline::Interpolant CInterpolatorSpline::getInterpolant(qint64 currentTimeMsSinceEpoc, + const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints, CInterpolationStatus &status, InterpolationLog &log) + { + Q_UNUSED(hints); + Q_UNUSED(setup); + Q_UNUSED(log); + + // recalculate derivatives only if they changed + if (currentTimeMsSinceEpoc > m_nextSampleTime) + { + // find the first situation not in the correct order, keep only the situations before that one + const auto end = std::is_sorted_until(m_aircraftSituations.begin(), m_aircraftSituations.end(), [](auto && a, auto && b) { return b.getAdjustedMSecsSinceEpoch() < a.getAdjustedMSecsSinceEpoch(); }); + const auto validSituations = makeRange(m_aircraftSituations.begin(), end); + + // 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); + const auto situationsOlder = makeRange(pivot, validSituations.end()); + + if (situationsNewer.isEmpty() || situationsOlder.size() < 2) { return { *this, 0 }; } + + const std::array s {{ *(situationsOlder.begin() + 1), *situationsOlder.begin(), *(situationsNewer.end() - 1) }}; + + 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 = {{ s[0].getAltitude().value(), s[1].getAltitude().value(), s[2].getAltitude().value() }}; + 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); + + m_prevSampleTime = situationsOlder.begin()->getAdjustedMSecsSinceEpoch(); + m_nextSampleTime = (situationsNewer.end() - 1)->getAdjustedMSecsSinceEpoch(); + m_altitudeUnit = situationsOlder.begin()->getAltitude().getUnit(); + pbh = { *situationsOlder.begin(), *(situationsNewer.end() - 1) }; + } + + status.setInterpolationSucceeded(true); + status.setChangedPosition(true); + pbh.setTimeFraction(static_cast(currentTimeMsSinceEpoc - m_prevSampleTime) / static_cast(m_nextSampleTime - m_prevSampleTime)); + + return { *this, currentTimeMsSinceEpoc }; + } + + CCoordinateGeodetic CInterpolatorSpline::Interpolant::interpolatePosition(const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints) const + { + 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]); + + CCoordinateGeodetic currentPosition; + currentPosition.setNormalVector(newX, newY, newZ); + return currentPosition; + } + + CAltitude CInterpolatorSpline::Interpolant::interpolateAltitude(const CInterpolationAndRenderingSetup &setup, const CInterpolationHints &hints) const + { + 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); + } + } +} diff --git a/src/blackmisc/simulation/interpolatorspline.h b/src/blackmisc/simulation/interpolatorspline.h new file mode 100644 index 000000000..790f5d257 --- /dev/null +++ b/src/blackmisc/simulation/interpolatorspline.h @@ -0,0 +1,74 @@ +/* Copyright (C) 2017 + * swift project Community / Contributors + * + * This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level + * directory of this distribution and at http://www.swift-project.org/license.html. No part of swift project, + * including this file, may be copied, modified, propagated, or distributed except according to the terms + * contained in the LICENSE file. + */ + +//! \file + +#ifndef BLACKMISC_SIMULATION_INTERPOLATORSPLINE_H +#define BLACKMISC_SIMULATION_INTERPOLATORSPLINE_H + +#include "blackmisc/simulation/interpolator.h" +#include "blackmisc/blackmiscexport.h" +#include "blackmisc/aviation/aircraftsituation.h" +#include +#include + +namespace BlackMisc +{ + namespace Simulation + { + //! Cubic spline interpolator + class BLACKMISC_EXPORT CInterpolatorSpline : public CInterpolator + { + Q_OBJECT + + public: + //! Constructor + CInterpolatorSpline(QObject *parent = nullptr) : + CInterpolator("CInterpolatorSpline", parent) + {} + + //! Cubic function that performs the actual interpolation + class Interpolant + { + public: + //! Constructor + Interpolant(const CInterpolatorSpline &interpolator, qint64 time) : i(interpolator), currentTimeMsSinceEpoc(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 i.pbh; } + + private: + const CInterpolatorSpline &i; + qint64 currentTimeMsSinceEpoc = 0; + }; + + //! Strategy used by CInterpolator::getInterpolatedSituation + Interpolant getInterpolant(qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetup &setup, + const CInterpolationHints &hints, CInterpolationStatus &status, InterpolationLog &log); + + //! Log category + static QString getLogCategory() { return "swift.interpolatorspline"; } + + 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 pbh; + }; + } +} + +#endif diff --git a/src/xbus/traffic.cpp b/src/xbus/traffic.cpp index e1cad7c36..c372dfc8c 100644 --- a/src/xbus/traffic.cpp +++ b/src/xbus/traffic.cpp @@ -16,7 +16,6 @@ #include "utils.h" #include "blackmisc/simulation/interpolator.h" #include "blackmisc/simulation/interpolationhints.h" -#include "blackmisc/simulation/interpolatorlinear.h" #include "XPMPMultiplayer.h" #include #include @@ -41,7 +40,7 @@ namespace XBus surfaces.lights.timeOffset = static_cast(qrand() % 0xffff); } - BlackMisc::Simulation::CInterpolationHints CTraffic::Plane::hints() const + BlackMisc::Simulation::CInterpolationHints CTraffic::Plane::hints() { // \todo MS 865 CInterpolationAndRenderingSetup allows to setup interpolation in the GUI, e.g. // also to disable aircraft parts / or logging parts (log file). I wonder if you want to consider it here diff --git a/src/xbus/traffic.h b/src/xbus/traffic.h index 732428bd2..18c482d74 100644 --- a/src/xbus/traffic.h +++ b/src/xbus/traffic.h @@ -16,7 +16,7 @@ #include "terrainprobe.h" #include "blackmisc/aviation/aircraftsituationlist.h" #include "blackmisc/aviation/aircraftpartslist.h" -#include "blackmisc/simulation/interpolatorlinear.h" +#include "blackmisc/simulation/interpolatorspline.h" #include #include #include @@ -131,9 +131,9 @@ namespace XBus bool hasSurfaces = false; bool hasXpdr = false; char label[32] {}; - BlackMisc::Simulation::CInterpolatorLinear interpolator; + BlackMisc::Simulation::CInterpolatorSpline interpolator; CTerrainProbe terrainProbe; - BlackMisc::Simulation::CInterpolationHints hints() const; + BlackMisc::Simulation::CInterpolationHints hints(); XPMPPlaneSurfaces_t surfaces; XPMPPlaneRadar_t xpdr; Plane(void *id_, QString callsign_, QString aircraftIcao_, QString airlineIcao_, QString livery_);