mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-04-02 06:35:52 +08:00
83 lines
3.3 KiB
C++
83 lines
3.3 KiB
C++
// SPDX-FileCopyrightText: Copyright (C) 2018 swift Project Community / Contributors
|
|
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-swift-pilot-client-1
|
|
|
|
#include "blackmisc/simulation/interpolatorpbh.h"
|
|
#include "blackmisc/simulation/interpolatorfunctions.h"
|
|
#include "blackmisc/verify.h"
|
|
#include "blackconfig/buildconfig.h"
|
|
|
|
using namespace BlackConfig;
|
|
using namespace BlackMisc::Aviation;
|
|
using namespace BlackMisc::PhysicalQuantities;
|
|
|
|
namespace BlackMisc::Simulation
|
|
{
|
|
CAngle CInterpolatorPbh::interpolateAngle(const CAngle &begin, const CAngle &end, double timeFraction0to1)
|
|
{
|
|
// determine the right direction (to left, to right) we interpolate towards to
|
|
// -30 -> 30 => 60 (via 0)
|
|
// 30 -> -30 => -60 (via 0)
|
|
// 170 -> -170 => -340 (via 180)
|
|
// -170 -> 170 => 340 (via 180)
|
|
double deltaDeg = (end - begin).value(CAngleUnit::deg());
|
|
if (deltaDeg > 180.0) { deltaDeg -= 360; }
|
|
else if (deltaDeg < -180.0) { deltaDeg += 360; }
|
|
|
|
if (CBuildConfig::isLocalDeveloperDebugBuild())
|
|
{
|
|
BLACK_VERIFY_X(isAcceptableTimeFraction(timeFraction0to1), Q_FUNC_INFO, "0..1 fraction needed");
|
|
}
|
|
|
|
//! make sure to not end up we extrapolation
|
|
if (timeFraction0to1 >= 1.0) { return begin + CAngle(deltaDeg, CAngleUnit::deg()); }
|
|
if (timeFraction0to1 <= 0.0) { return begin; }
|
|
return begin + CAngle(timeFraction0to1 * deltaDeg, CAngleUnit::deg());
|
|
}
|
|
|
|
CHeading CInterpolatorPbh::getHeading() const
|
|
{
|
|
// HINT: VTOL aircraft can change pitch/bank without changing position, planes cannot
|
|
// Interpolate heading: HDG = (HdgB - HdgA) * t + HdgA
|
|
const CHeading headingBegin = m_oldSituation.getHeading();
|
|
const CHeading headingEnd = m_newSituation.getHeading();
|
|
|
|
if (CBuildConfig::isLocalDeveloperDebugBuild())
|
|
{
|
|
BLACK_VERIFY_X(headingBegin.getReferenceNorth() == headingEnd.getReferenceNorth(), Q_FUNC_INFO, "Need same reference");
|
|
}
|
|
return CHeading(interpolateAngle(headingBegin, headingEnd, m_simulationTimeFraction), headingEnd.getReferenceNorth());
|
|
}
|
|
|
|
CAngle CInterpolatorPbh::getPitch() const
|
|
{
|
|
// Interpolate Pitch: Pitch = (PitchB - PitchA) * t + PitchA
|
|
return interpolateAngle(m_oldSituation.getPitch(), m_newSituation.getPitch(), m_simulationTimeFraction);
|
|
}
|
|
|
|
CAngle CInterpolatorPbh::getBank() const
|
|
{
|
|
// Interpolate bank: Bank = (BankB - BankA) * t + BankA
|
|
return interpolateAngle(m_oldSituation.getBank(), m_newSituation.getBank(), m_simulationTimeFraction);
|
|
}
|
|
|
|
CSpeed CInterpolatorPbh::getGroundSpeed() const
|
|
{
|
|
return (m_newSituation.getGroundSpeed() - m_oldSituation.getGroundSpeed()) * m_simulationTimeFraction + m_oldSituation.getGroundSpeed();
|
|
}
|
|
|
|
void CInterpolatorPbh::setSituations(const CAircraftSituation &older, const CAircraftSituation &newer)
|
|
{
|
|
m_oldSituation = older;
|
|
m_newSituation = newer;
|
|
}
|
|
|
|
void CInterpolatorPbh::setTimeFraction(double tf)
|
|
{
|
|
if (CBuildConfig::isLocalDeveloperDebugBuild())
|
|
{
|
|
BLACK_VERIFY_X(isValidTimeFraction(tf), Q_FUNC_INFO, "Time fraction needs to be 0-1");
|
|
}
|
|
m_simulationTimeFraction = clampValidTimeFraction(tf);
|
|
}
|
|
} // namespace
|