flightsimulatorplatfrom/pilotclient
1
0
Fork 0
mirror of https://github.com/swift-project/pilotclient.git synced 2026-04-17 10:55:32 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor: Clarify variable names in linear interpolator

Browse Source
This commit is contained in:
Lars Toenning
2024-01-04 12:33:43 +01:00
parent 784bf75e0d
commit 1a6d7fba28
2 changed files with 71 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
src/blackmisc/simulation/interpolatorlinear.cpp
View File

@@ -29,19 +29,19 @@ using namespace BlackMisc::Simulation;
namespace BlackMisc::Simulation namespace BlackMisc::Simulation
{ {
CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &oldSituation) : IInterpolant(1, CInterpolatorPbh(0, oldSituation, oldSituation)), CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &startSituation) : IInterpolant(1, CInterpolatorPbh(0, startSituation, startSituation)),
m_oldSituation(oldSituation) m_startSituation(startSituation)
{} {}
CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &oldSituation, const CInterpolatorPbh &pbh) : IInterpolant(1, pbh), CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &startSituation, const CInterpolatorPbh &pbh) : IInterpolant(1, pbh),
m_oldSituation(oldSituation) m_startSituation(startSituation)
{} {}
CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation, double timeFraction, qint64 interpolatedTime) : IInterpolant(interpolatedTime, 2), CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &startSituation, const CAircraftSituation &endSituation, double timeFraction, qint64 interpolatedTime) : IInterpolant(interpolatedTime, 2),
m_oldSituation(oldSituation), m_newSituation(newSituation), m_startSituation(startSituation), m_endSituation(endSituation),
m_simulationTimeFraction(timeFraction) m_simulationTimeFraction(timeFraction)
{ {
m_pbh = CInterpolatorPbh(m_simulationTimeFraction, oldSituation, newSituation); m_pbh = CInterpolatorPbh(m_simulationTimeFraction, startSituation, endSituation);
} }
void CInterpolatorLinear::anchor() void CInterpolatorLinear::anchor()
@@ -49,76 +49,73 @@ namespace BlackMisc::Simulation
CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const
{ {
const std::array<double, 3> oldVec(m_oldSituation.getPosition().normalVectorDouble()); const std::array<double, 3> startVec(m_startSituation.getPosition().normalVectorDouble());
const std::array<double, 3> newVec(m_newSituation.getPosition().normalVectorDouble()); const std::array<double, 3> endVec(m_endSituation.getPosition().normalVectorDouble());
if (CBuildConfig::isLocalDeveloperDebugBuild()) if (CBuildConfig::isLocalDeveloperDebugBuild())
{ {
BLACK_VERIFY_X(CAircraftSituation::isValidVector(oldVec), Q_FUNC_INFO, "Invalid old vector"); BLACK_VERIFY_X(CAircraftSituation::isValidVector(startVec), Q_FUNC_INFO, "Invalid start vector");
BLACK_VERIFY_X(CAircraftSituation::isValidVector(newVec), Q_FUNC_INFO, "Invalid new vector"); BLACK_VERIFY_X(CAircraftSituation::isValidVector(endVec), Q_FUNC_INFO, "Invalid end vector");
BLACK_VERIFY_X(isAcceptableTimeFraction(m_simulationTimeFraction), Q_FUNC_INFO, "Invalid fraction"); BLACK_VERIFY_X(isAcceptableTimeFraction(m_simulationTimeFraction), Q_FUNC_INFO, "Invalid fraction");
} }
// Interpolate position: pos = (posB - posA) * t + posA // Interpolate position: pos = (posB - posA) * t + posA
CCoordinateGeodetic newPosition; CCoordinateGeodetic interpolatedPosition;
const double tf = clampValidTimeFraction(m_simulationTimeFraction); const double tf = clampValidTimeFraction(m_simulationTimeFraction);
newPosition.setNormalVector((newVec[0] - oldVec[0]) * tf + oldVec[0], interpolatedPosition.setNormalVector((endVec[0] - startVec[0]) * tf + startVec[0],
(newVec[1] - oldVec[1]) * tf + oldVec[1], (endVec[1] - startVec[1]) * tf + startVec[1],
(newVec[2] - oldVec[2]) * tf + oldVec[2]); (endVec[2] - startVec[2]) * tf + startVec[2]);
if (CBuildConfig::isLocalDeveloperDebugBuild()) 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 // Interpolate altitude: Alt = (AltB - AltA) * t + AltA
// avoid underflow below ground elevation by using getCorrectedAltitude // avoid underflow below ground elevation by using getCorrectedAltitude
const CAltitude oldAlt(m_oldSituation.getCorrectedAltitude()); const CAltitude oldAlt(m_startSituation.getCorrectedAltitude());
const CAltitude newAlt(m_newSituation.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 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, const CAltitude altitude((newAlt - oldAlt) * tf + oldAlt,
oldAlt.getReferenceDatum()); oldAlt.getReferenceDatum());
CAircraftSituation newSituation(situation); CAircraftSituation interpolatedSituation(situation);
newSituation.setPosition(newPosition); interpolatedSituation.setPosition(interpolatedPosition);
newSituation.setAltitude(altitude); interpolatedSituation.setAltitude(altitude);
newSituation.setMSecsSinceEpoch(this->getInterpolatedTime()); interpolatedSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
if (interpolateGndFactor) if (interpolateGndFactor)
{ {
const double oldGroundFactor = m_oldSituation.getOnGroundFactor(); const double startGroundFactor = m_startSituation.getOnGroundFactor();
const double newGroundFactor = m_newSituation.getOnGroundFactor(); const double endGroundFactor = m_endSituation.getOnGroundFactor();
do if (CAircraftSituation::isGfEqualAirborne(startGroundFactor, endGroundFactor))
{ {
if (CAircraftSituation::isGfEqualAirborne(oldGroundFactor, newGroundFactor)) interpolatedSituation.setOnGround(false);
{ }
newSituation.setOnGround(false); else if (CAircraftSituation::isGfEqualOnGround(startGroundFactor, endGroundFactor))
break; {
} interpolatedSituation.setOnGround(true);
if (CAircraftSituation::isGfEqualOnGround(oldGroundFactor, newGroundFactor)) }
{ else
newSituation.setOnGround(true); {
break; const double interpolatedGroundFactor = (endGroundFactor - startGroundFactor) * tf + startGroundFactor;
} interpolatedSituation.setOnGroundFactor(interpolatedGroundFactor);
const double groundFactor = (newGroundFactor - oldGroundFactor) * tf + oldGroundFactor; interpolatedSituation.setOnGroundFromGroundFactorFromInterpolation(groundInterpolationFactor());
newSituation.setOnGroundFactor(groundFactor);
newSituation.setOnGroundFromGroundFactorFromInterpolation(groundInterpolationFactor());
} }
while (false);
} }
return newSituation; return interpolatedSituation;
} }
CInterpolatorLinear::CInterpolant CInterpolatorLinear::getInterpolant(SituationLog &log) CInterpolatorLinear::CInterpolant CInterpolatorLinear::getInterpolant(SituationLog &log)
{ {
// set default situations // set default situations
CAircraftSituation oldSituation = m_interpolant.getOldSituation(); CAircraftSituation startSituation = m_interpolant.getStartSituation();
CAircraftSituation newSituation = m_interpolant.getNewSituation(); 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 updated = m_situationsLastModifiedUsed < m_situationsLastModified;
const bool newSplit = newSituation.getAdjustedMSecsSinceEpoch() < m_currentTimeMsSinceEpoch; const bool newSplit = endSituation.getAdjustedMSecsSinceEpoch() < m_currentTimeMsSinceEpoch;
const bool recalculate = updated || newSplit; const bool recalculate = updated || newSplit;
if (recalculate) if (recalculate)
@@ -162,33 +159,33 @@ namespace BlackMisc::Simulation
} }
// extrapolate from two before situations // extrapolate from two before situations
oldSituation = *(situationsOlder.begin() + 1); // before newest startSituation = *(situationsOlder.begin() + 1); // before newest
newSituation = situationsOlder.front(); // newest endSituation = situationsOlder.front(); // newest
} }
else else
{ {
oldSituation = situationsOlder.front(); // first oldest (aka newest oldest) startSituation = situationsOlder.front(); // first oldest (aka newest oldest)
newSituation = *(situationsNewer.end() - 1); // latest newest (aka oldest of newer block) endSituation = *(situationsNewer.end() - 1); // latest newest (aka oldest of newer block)
Q_ASSERT(oldSituation.getAdjustedMSecsSinceEpoch() < newSituation.getAdjustedMSecsSinceEpoch()); Q_ASSERT(startSituation.getAdjustedMSecsSinceEpoch() < endSituation.getAdjustedMSecsSinceEpoch());
} }
// adjust ground if required // adjust ground if required
if (!oldSituation.canLikelySkipNearGroundInterpolation() && !oldSituation.hasGroundElevation()) if (!startSituation.canLikelySkipNearGroundInterpolation() && !startSituation.hasGroundElevation())
{ {
const CElevationPlane planeOld = this->findClosestElevationWithinRange(oldSituation, CElevationPlane::singlePointRadius()); const CElevationPlane planeOld = this->findClosestElevationWithinRange(startSituation, CElevationPlane::singlePointRadius());
oldSituation.setGroundElevationChecked(planeOld, CAircraftSituation::FromCache); startSituation.setGroundElevationChecked(planeOld, CAircraftSituation::FromCache);
} }
if (!newSituation.canLikelySkipNearGroundInterpolation() && !newSituation.hasGroundElevation()) if (!endSituation.canLikelySkipNearGroundInterpolation() && !endSituation.hasGroundElevation())
{ {
const CElevationPlane planeNew = this->findClosestElevationWithinRange(newSituation, CElevationPlane::singlePointRadius()); const CElevationPlane planeNew = this->findClosestElevationWithinRange(endSituation, CElevationPlane::singlePointRadius());
newSituation.setGroundElevationChecked(planeNew, CAircraftSituation::FromCache); endSituation.setGroundElevationChecked(planeNew, CAircraftSituation::FromCache);
} }
} // modified situations } // 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 // 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"); Q_ASSERT_X(sampleDeltaTimeMs >= 0, Q_FUNC_INFO, "Negative delta time");
log.interpolator = 'l'; 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 // < 0 should not happen due to the split, > 1 can happen if new values are delayed beyond split time
// 1) values > 1 mean extrapolation // 1) values > 1 mean extrapolation
// 2) values > 2 mean no new situations coming in // 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); double simulationTimeFraction = qMax(1.0 - (distanceToSplitTimeMs / sampleDeltaTimeMs), 0.0);
if (simulationTimeFraction >= 1.0) if (simulationTimeFraction >= 1.0)
{ {
@@ -205,10 +202,10 @@ namespace BlackMisc::Simulation
} }
const double deltaTimeFractionMs = sampleDeltaTimeMs * simulationTimeFraction; 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 // 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); currentSituation.setMSecsSinceEpoch(interpolatedTime);
m_currentInterpolationStatus.setInterpolatedAndCheckSituation(true, currentSituation); m_currentInterpolationStatus.setInterpolatedAndCheckSituation(true, currentSituation);
@@ -220,12 +217,12 @@ namespace BlackMisc::Simulation
log.deltaSampleTimesMs = sampleDeltaTimeMs; log.deltaSampleTimesMs = sampleDeltaTimeMs;
log.tsInterpolated = interpolatedTime; log.tsInterpolated = interpolatedTime;
log.interpolationSituations.clear(); log.interpolationSituations.clear();
log.interpolationSituations.push_back(oldSituation); // oldest at front log.interpolationSituations.push_back(startSituation); // oldest at front
log.interpolationSituations.push_back(newSituation); // latest at back log.interpolationSituations.push_back(endSituation); // latest at back
log.interpolantRecalc = recalculate; log.interpolantRecalc = recalculate;
} }
m_interpolant = { oldSituation, newSituation, simulationTimeFraction, interpolatedTime }; m_interpolant = { startSituation, endSituation, simulationTimeFraction, interpolatedTime };
m_interpolant.setRecalculated(recalculate); m_interpolant.setRecalculated(recalculate);
return m_interpolant; return m_interpolant;

20
src/blackmisc/simulation/interpolatorlinear.h
View File

@@ -41,10 +41,10 @@ namespace BlackMisc
public: public:
//! @{ //! @{
//! Constructor //! Constructor
CInterpolant() {} CInterpolant() = default;
CInterpolant(const Aviation::CAircraftSituation &oldSituation); CInterpolant(const Aviation::CAircraftSituation &startSituation);
CInterpolant(const Aviation::CAircraftSituation &oldSituation, const CInterpolatorPbh &pbh); CInterpolant(const Aviation::CAircraftSituation &startSituation, const CInterpolatorPbh &pbh);
CInterpolant(const Aviation::CAircraftSituation &oldSituation, const Aviation::CAircraftSituation &newSituation, double timeFraction, qint64 interpolatedTime); CInterpolant(const Aviation::CAircraftSituation &startSituation, const Aviation::CAircraftSituation &endSituation, double timeFraction, qint64 interpolatedTime);
//! @} //! @}
//! Perform the interpolation //! Perform the interpolation
@@ -53,15 +53,15 @@ namespace BlackMisc
//! \return \p situation with interpolated position and altitude, updated timestamp and possibly interpolated GND factor //! \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; Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation, bool interpolateGndFactor) const;
//! Old situation //! Start situation
const Aviation::CAircraftSituation &getOldSituation() const { return m_oldSituation; } const Aviation::CAircraftSituation &getStartSituation() const { return m_startSituation; }
//! New situation //! End situation
const Aviation::CAircraftSituation &getNewSituation() const { return m_newSituation; } const Aviation::CAircraftSituation &getEndSituation() const { return m_endSituation; }
private: private:
Aviation::CAircraftSituation m_oldSituation; Aviation::CAircraftSituation m_startSituation;
Aviation::CAircraftSituation m_newSituation; Aviation::CAircraftSituation m_endSituation;
double m_simulationTimeFraction = 0.0; //!< 0..1 double m_simulationTimeFraction = 0.0; //!< 0..1
}; };