flightsimulatorplatfrom/pilotclient
1
0
Fork 0
mirror of https://github.com/swift-project/pilotclient.git synced 2026-03-31 04:25:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

refs #863 Added cubic spline interpolator.

Browse Source
This commit is contained in:
Mathew Sutcliffe
2017-02-05 01:00:02 +00:00
parent 4cc5643340
commit 8d351d8279
6 changed files with 250 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/blackmisc/simulation/interpolator.cpp
View File

@@ -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 <typename Derived>
CAircraftSituation CInterpolator<Derived>::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 <typename Derived>
CAircraftParts CInterpolator<Derived>::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<CInterpolatorLinear>;
template class BLACKMISC_EXPORT_DEFINE_TEMPLATE CInterpolator<CInterpolatorSpline>;
//! \endcond
} // namespace
} // namespace

6
src/blackmisc/simulation/interpolator.h
View File

@@ -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<CInterpolatorLinear>;
extern template class BLACKMISC_EXPORT_DECLARE_TEMPLATE CInterpolator<CInterpolatorSpline>;
//! \endcond
} // namespace
} // namespace

164
src/blackmisc/simulation/interpolatorspline.cpp Normal file
View File

@@ -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 <size_t N>
std::array<double, N> solveTridiagonal(std::array<std::array<double, N>, N> &matrix, std::array<double, N> &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 <size_t N>
std::array<double, N> getDerivatives(const std::array<double, N> &x, const std::array<double, N> &y)
{
std::array<std::array<double, N>, N> a {{}};
std::array<double, N> 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<CAircraftSituation, 3> s {{ *(situationsOlder.begin() + 1), *situationsOlder.begin(), *(situationsNewer.end() - 1) }};
const std::array<std::array<double, 3>, 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<double>(s[0].getAdjustedMSecsSinceEpoch()), static_cast<double>(s[1].getAdjustedMSecsSinceEpoch()), static_cast<double>(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<double>(currentTimeMsSinceEpoc - m_prevSampleTime) / static_cast<double>(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);
}
}
}

74
src/blackmisc/simulation/interpolatorspline.h Normal file
View File

@@ -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 <QString>
#include <QtGlobal>
namespace BlackMisc
{
namespace Simulation
{
//! Cubic spline interpolator
class BLACKMISC_EXPORT CInterpolatorSpline : public CInterpolator<CInterpolatorSpline>
{
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<double, 3> x, y, z, a, t, dx, dy, dz, da;
CInterpolatorPbh pbh;
};
}
}
#endif

3
src/xbus/traffic.cpp
View File

@@ -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 <XPLM/XPLMProcessing.h>
#include <XPLM/XPLMUtilities.h>
@@ -41,7 +40,7 @@ namespace XBus
surfaces.lights.timeOffset = static_cast<quint16>(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

6
src/xbus/traffic.h
View File

@@ -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 <QObject>
#include <QHash>
#include <QVector>
@@ -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_);