From 00356bbef68783cd135ffbe1b8af695ff0c758f8 Mon Sep 17 00:00:00 2001 From: Roland Winklmeier Date: Fri, 14 Mar 2014 17:51:45 +0100 Subject: [PATCH] Linear interpolator implementation refs #169 --- src/blackcore/interpolator_linear.cpp | 103 ++++++++++++++++++++++++++ src/blackcore/interpolator_linear.h | 47 ++++++++++++ src/blackmisc/geoearthangle.h | 29 +++++++- 3 files changed, 178 insertions(+), 1 deletion(-) create mode 100644 src/blackcore/interpolator_linear.cpp create mode 100644 src/blackcore/interpolator_linear.h diff --git a/src/blackcore/interpolator_linear.cpp b/src/blackcore/interpolator_linear.cpp new file mode 100644 index 000000000..61ba7dce2 --- /dev/null +++ b/src/blackcore/interpolator_linear.cpp @@ -0,0 +1,103 @@ +/* Copyright (C) 2013 VATSIM Community / contributors + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ + +#include "interpolator_linear.h" +#include "blackmisc/avaircraftsituation.h" +#include + +using namespace BlackMisc::Geo; +using namespace BlackMisc::Math; +using namespace BlackMisc::PhysicalQuantities; +using namespace BlackMisc::Aviation; + +namespace BlackCore +{ + void CInterpolatorLinear::initialize() + { + + } + + void CInterpolatorLinear::addAircraftSituation(const BlackMisc::Aviation::CAircraftSituation &situation) + { + m_aircraftSituationList.push_back(situation); + + // Delete packets older than 30 seconds + m_aircraftSituationList.removeBefore(QDateTime::currentDateTimeUtc().addSecs(-30)); + } + + bool CInterpolatorLinear::hasEnoughAircraftSituations() const + { + QDateTime currentTime = QDateTime::currentDateTimeUtc().addSecs(-6); + return !m_aircraftSituationList.findBefore(currentTime).isEmpty() && m_aircraftSituationList.size() > 1; + } + + CAircraftSituation CInterpolatorLinear::getCurrentSituation() + { + QDateTime currentTime = QDateTime::currentDateTimeUtc().addSecs(-6); + CAircraftSituationList situationsBefore = m_aircraftSituationList.findBefore(currentTime); + CAircraftSituationList situationsAfter = m_aircraftSituationList.findAfter(currentTime); + + Q_ASSERT_X(!situationsBefore.isEmpty(), "CInterpolatorLinear::getCurrentSituation()", "List previous situations is empty!"); + + CAircraftSituation beginSituation; + CAircraftSituation endSituation; + + // The first condition covers a situation, when there is now future packet. + // So we have to extrapolate. + if (situationsAfter.isEmpty()) + { + beginSituation = situationsBefore[situationsBefore.size() - 2]; + endSituation = situationsBefore[situationsBefore.size() - 1]; + } + else + { + beginSituation = situationsBefore.back(); + endSituation = situationsAfter.front(); + } + + CAircraftSituation currentSituation; + CCoordinateGeodetic currentPosition; + + // Time between start and end packet + double deltaTime = beginSituation.getTimestamp().msecsTo(endSituation.getTimestamp()); + + // Fraction of the deltaTime [0.0 - 1.0] + double simulationTime = beginSituation.getTimestamp().msecsTo(currentTime) / deltaTime; + + // Interpolate latitude: Lat = (LatB - LatA) * t + LatA + currentPosition.setLatitude((endSituation.getPosition().latitude() - beginSituation.getPosition().latitude()) + * simulationTime + + beginSituation.getPosition().latitude()); + + // Interpolate latitude: Lon = (LonB - LonA) * t + LonA + currentPosition.setLongitude((endSituation.getPosition().longitude() - beginSituation.getPosition().longitude()) + * simulationTime + + beginSituation.getPosition().longitude()); + currentSituation.setPosition(currentPosition); + + // Interpolate altitude: Alt = (AltB - AltA) * t + AltA + currentSituation.setAltitude(CAltitude((endSituation.getAltitude() - beginSituation.getAltitude()) + * simulationTime + + beginSituation.getAltitude(), + beginSituation.getAltitude().getReferenceDatum())); + + // Interpolate heading: HDG = (HdgB - HdgA) * t + HdgA + CHeading headingBegin = beginSituation.getHeading(); + CHeading headingEnd = endSituation.getHeading(); + + if ((headingEnd - headingBegin).value(CAngleUnit::deg()) < -180) + headingEnd += CHeading(360, CHeading::Magnetic, CAngleUnit::deg()); + + if ((headingEnd - headingBegin).value(CAngleUnit::deg()) > 180) + headingEnd -= CHeading(360, CHeading::Magnetic, CAngleUnit::deg()); + + currentSituation.setHeading(CHeading((headingEnd - headingBegin) + * simulationTime + + headingBegin, + headingBegin.getReferenceNorth())); + + return currentSituation; + } +} diff --git a/src/blackcore/interpolator_linear.h b/src/blackcore/interpolator_linear.h new file mode 100644 index 000000000..9e8c565ba --- /dev/null +++ b/src/blackcore/interpolator_linear.h @@ -0,0 +1,47 @@ +/* Copyright (C) 2013 VATSIM Community / contributors + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ + +#ifndef BLACKCORE_INTERPOLATOR_LINEAR_H +#define BLACKCORE_INTERPOLATOR_LINEAR_H + +#include "interpolator.h" +#include "blackmisc/avaircraftsituation.h" +#include "blackmisc/avaircraftsituationlist.h" +#include "blackmisc/sequence.h" +#include + +namespace BlackCore +{ + //! \brief Linear interpolator, calculation inbetween positions + class CInterpolatorLinear : public IInterpolator + { + public: + //! \brief Default constructor + CInterpolatorLinear() {} + + //! \brief Virtual destructor + virtual ~CInterpolatorLinear() {} + + //! Init object + virtual void initialize() override; + + //! \copydoc IInterpolator::addSituation() + virtual void addAircraftSituation(const BlackMisc::Aviation::CAircraftSituation &situation) override; + + //! \copydoc IInterpolator::hasEnoughAircraftSituations() + virtual bool hasEnoughAircraftSituations() const override; + + //! \copydoc IInterpolator::getCurrentSituation() + virtual BlackMisc::Aviation::CAircraftSituation getCurrentSituation() override; + + private: + BlackMisc::Aviation::CAircraftSituationList m_aircraftSituationList; + + }; + +} // namespace BlackCore + +#endif // guard + diff --git a/src/blackmisc/geoearthangle.h b/src/blackmisc/geoearthangle.h index 13f2fb207..19b330ad0 100644 --- a/src/blackmisc/geoearthangle.h +++ b/src/blackmisc/geoearthangle.h @@ -83,7 +83,22 @@ namespace BlackMisc return *this; } - //! \brief Greater operator > + /*! + * \brief Multiply operator *= + * \param multiply + * \return + */ + CEarthAngle operator *=(double multiply) + { + this->CAngle::operator *=(multiply); + return *this; + } + + /*! + * \brief Greater operator > + * \param latOrLon + * \return + */ bool operator >(const CEarthAngle &latOrLon) const { return this->CAngle::operator >(latOrLon); @@ -123,6 +138,18 @@ namespace BlackMisc return l; } + /*! + * \brief Multiply operator * + * \param multiply + * \return + */ + LATorLON operator *(double multiply) const + { + LATorLON l(*this); + l *= multiply; + return l; + } + //! \brief Register metadata static void registerMetadata();