diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index a16024b0c..61ff04fd8 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/src/blackmisc/simulation/interpolator.cpp @@ -204,10 +204,14 @@ namespace BlackMisc currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation, interpolateGndFlag); if (currentSituation.isNull()) { break; } - if (CBuildConfig::isLocalDeveloperDebugBuild()) + // if we get here and the vector is invalid it means we haven't handled it correctly in one of the interpolators + if (!currentSituation.isValidVectorRange()) { - Q_ASSERT_X(currentSituation.isValidVectorRange(), Q_FUNC_INFO, "Invalid interpolation situation"); + if (CBuildConfig::isLocalDeveloperDebugBuild()) { BLACK_VERIFY_X(false, Q_FUNC_INFO, "Invalid interpolation situation"); } + return CAircraftSituation::null(); } + + // GND flag. if (!interpolateGndFlag) { currentSituation.guessOnGround(CAircraftSituationChange::null(), m_model); } // as we now have the position and can interpolate elevation diff --git a/src/blackmisc/simulation/interpolatorlinear.cpp b/src/blackmisc/simulation/interpolatorlinear.cpp index 0af25b5c1..e2726b3c3 100644 --- a/src/blackmisc/simulation/interpolatorlinear.cpp +++ b/src/blackmisc/simulation/interpolatorlinear.cpp @@ -8,6 +8,7 @@ */ #include "interpolatorlinear.h" +#include "interpolatorfunctions.h" #include "blackmisc/aviation/aircraftsituationlist.h" #include "blackmisc/aviation/altitude.h" #include "blackmisc/geo/coordinategeodetic.h" @@ -62,12 +63,24 @@ namespace BlackMisc const std::array oldVec(m_oldSituation.getPosition().normalVectorDouble()); const std::array newVec(m_newSituation.getPosition().normalVectorDouble()); + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + BLACK_VERIFY_X(CAircraftSituation::isValidVector(oldVec), Q_FUNC_INFO, "Invalid old vector"); + BLACK_VERIFY_X(CAircraftSituation::isValidVector(newVec), Q_FUNC_INFO, "Invalid new vector"); + BLACK_VERIFY_X(isValidTimeFraction(m_simulationTimeFraction), Q_FUNC_INFO, "Invalid fraction"); + } + // Interpolate position: pos = (posB - posA) * t + posA CCoordinateGeodetic newPosition; newPosition.setNormalVector((newVec[0] - oldVec[0]) * m_simulationTimeFraction + oldVec[0], (newVec[1] - oldVec[1]) * m_simulationTimeFraction + oldVec[1], (newVec[2] - oldVec[2]) * m_simulationTimeFraction + oldVec[2]); + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + BLACK_VERIFY_X(newPosition.isValidVectorRange(), Q_FUNC_INFO, "Invalid vector"); + } + // Interpolate altitude: Alt = (AltB - AltA) * t + AltA // avoid underflow below ground elevation by using getCorrectedAltitude const CAltitude oldAlt(m_oldSituation.getCorrectedAltitude()); diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp index 3498bd377..6bc91c85a 100644 --- a/src/blackmisc/simulation/interpolatorspline.cpp +++ b/src/blackmisc/simulation/interpolatorspline.cpp @@ -7,7 +7,8 @@ * contained in the LICENSE file. */ -#include "blackmisc/simulation/interpolatorspline.h" +#include "interpolatorspline.h" +#include "interpolatorfunctions.h" #include "blackmisc/network/fsdsetup.h" #include "blackmisc/logmessage.h" #include "blackmisc/verify.h" @@ -221,7 +222,7 @@ namespace BlackMisc m_nextSampleAdjustedTime = m_s[2].getAdjustedMSecsSinceEpoch(); // latest m_prevSampleTime = m_s[1].getMSecsSinceEpoch(); m_nextSampleTime = m_s[2].getMSecsSinceEpoch(); // latest - m_interpolant = CInterpolant(pa, altUnit, CInterpolatorPbh(m_s[1], m_s[2])); + m_interpolant = CInterpolant(pa, altUnit, CInterpolatorPbh(m_s[1], m_s[2])); // older, newer Q_ASSERT_X(m_prevSampleAdjustedTime < m_nextSampleAdjustedTime, Q_FUNC_INFO, "Wrong time order"); } @@ -245,7 +246,7 @@ namespace BlackMisc { BLACK_VERIFY_X(dt1 >= 0, Q_FUNC_INFO, "Expect postive dt1"); BLACK_VERIFY_X(dt2 > 0, Q_FUNC_INFO, "Expect postive dt2"); - BLACK_VERIFY_X(timeFraction < 1.01, Q_FUNC_INFO, "Expect fraction 0-1"); + BLACK_VERIFY_X(isValidTimeFraction(timeFraction), Q_FUNC_INFO, "Expect fraction 0-1"); } const qint64 interpolatedTime = m_prevSampleTime + qRound(timeFraction * dt2);