predict the planned waypoint if a heading is given

include/aman/types/Inbound.h

@@ -39,12 +39,14 @@ namespace aman {
         UtcTime::Point               m_waypointEstimatedTimeOfArrival;
         Length                       m_trackmiles;
         Time                         m_flighttime;
+        std::string                  m_predictedWaypoint;
 
         void updatePrediction(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSchedule& inbound);
         Velocity indicatedAirspeed(const Length& altitude) const noexcept;
         Velocity groundSpeed(const Length& altitude, const Velocity& ias, const Angle& heading);
         void createWindTables(const google::protobuf::RepeatedPtrField<aman::WindData>& wind);
         void predictETA(const EuroScopePlugIn::CRadarTarget& target, const EuroScopePlugIn::CFlightPlanPositionPredictions& predictions);
+        std::string findNextWaypointOnHeading(EuroScopePlugIn::CFlightPlan& plan);
 
     public:
         Inbound(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSchedule& inbound, const google::protobuf::RepeatedPtrField<aman::WindData>& wind);
@@ -58,6 +60,7 @@ namespace aman {
         const Time& timeToLose() const noexcept;
         const Length& trackmiles() const noexcept;
         const Time& flighttime() const noexcept;
+        const std::string& predictedWaypoint() const noexcept;
 
         static int matchToPredictedPath(const EuroScopePlugIn::CFlightPlanPositionPredictions& predictions, const GeoCoordinate& position,
                                         const Velocity& groundspeed, Length& trackmiles);

src/types/Inbound.cpp

@@ -11,6 +11,10 @@
 
 #include <Windows.h>
 
+#pragma warning(push, 0)
+#include <Eigen/Geometry>
+#pragma warning(pop)
+#include <GeographicLib/Gnomonic.hpp>
 #include <gsl/gsl>
 
 #include <aman/helper/String.h>
@@ -35,7 +39,8 @@ Inbound::Inbound(EuroScopePlugIn::CRadarTarget& target, const aman::AircraftSche
         m_arrivalRoute(),
         m_timeToLose(),
         m_trackmiles(),
-        m_flighttime() {
+        m_flighttime(),
+        m_predictedWaypoint() {
     this->createWindTables(wind);
     this->updatePrediction(target, inbound);
     auto flightplan = target.GetCorrelatedFlightPlan();
@@ -332,6 +337,90 @@ void Inbound::update(EuroScopePlugIn::CRadarTarget& target) {
     this->predictETA(target, target.GetCorrelatedFlightPlan().GetPositionPredictions());
 }
 
+std::string Inbound::findNextWaypointOnHeading(EuroScopePlugIn::CFlightPlan& plan) {
+    auto target = plan.GetCorrelatedRadarTarget();
+    const auto course(static_cast<float>(target.GetTrackHeading()) * degree);
+
+    const GeoCoordinate currentPosition(static_cast<float>(target.GetPosition().GetPosition().m_Longitude) * degree,
+                                        static_cast<float>(target.GetPosition().GetPosition().m_Latitude) * degree);
+    const auto projectedPosition = currentPosition.projection(course, 300_nm);
+
+    /* calculate the average to get a good cartesian projection point */
+    Angle avgLongitude = currentPosition.longitude(), avgLatitude = currentPosition.latitude();
+    avgLongitude += projectedPosition.longitude();
+    avgLatitude += projectedPosition.latitude();
+
+    for (const auto& waypoint : std::as_const(this->m_arrivalRoute)) {
+        avgLongitude += waypoint.position().longitude();
+        avgLatitude += waypoint.position().latitude();
+    }
+
+    avgLongitude = avgLongitude / static_cast<float>(this->m_arrivalRoute.size() + 2);
+    avgLatitude = avgLatitude / static_cast<float>(this->m_arrivalRoute.size() + 2);
+
+    GeographicLib::Gnomonic projection(GeographicLib::Geodesic::WGS84());
+    Eigen::Vector2f start, end;
+
+    /* create the flight line to find the intersections */
+    projection.Forward(avgLatitude.convert(degree), avgLongitude.convert(degree),
+                       currentPosition.latitude().convert(degree), currentPosition.longitude().convert(degree),
+                       start[0], start[1]);
+    projection.Forward(avgLatitude.convert(degree), avgLongitude.convert(degree),
+                       projectedPosition.latitude().convert(degree), projectedPosition.longitude().convert(degree),
+                       end[0], end[1]);
+
+    Angle offset = 380.0_deg;
+    std::size_t waypointIdx = this->m_arrivalRoute.size();
+    const auto flightLine = Eigen::Hyperplane<float, 2>::Through(start, end);
+    for (std::size_t i = 0; i < this->m_arrivalRoute.size() - 1; ++i) {
+        const auto& waypointStart = gsl::at(this->m_arrivalRoute, i);
+        const auto& waypointEnd = gsl::at(this->m_arrivalRoute, i + 1);
+
+        projection.Forward(avgLatitude.convert(degree), avgLongitude.convert(degree),
+                           waypointStart.position().latitude().convert(degree), waypointStart.position().longitude().convert(degree),
+                           start[0], start[1]);
+        projection.Forward(avgLatitude.convert(degree), avgLongitude.convert(degree),
+                           waypointEnd.position().latitude().convert(degree), waypointEnd.position().longitude().convert(degree),
+                           end[0], end[1]);
+
+        /* get the intersection point */
+        const auto segment = Eigen::Hyperplane<float, 2>::Through(start, end);
+        const auto intersection = flightLine.intersection(segment);
+
+        /*
+         * - check if the intersection is inside the segment
+         * idea:
+         *   - the controller can use wrong headings due to weather and maybe misinterpretations
+         *   - allow an error of 20% for the line to find the best waypoint
+         */
+        const auto segmentLength = (start - end).norm() * 1.2f;
+        const auto intersectionStartLength = (start - intersection).norm();
+        const auto intersectionEndLength = (end - intersection).norm();
+
+        /* outside the segment */
+        if (intersectionStartLength > segmentLength || intersectionEndLength > segmentLength)
+            continue;
+
+        /* check if the offset is better than the last one */
+        const auto deltaAngle = __normalize(waypointStart.position().bearingTo(waypointEnd.position()) - course).abs();
+        if (deltaAngle < offset) {
+            offset = deltaAngle;
+
+            /* find the closer waypoint */
+            if (intersectionStartLength < intersectionEndLength)
+                waypointIdx = i;
+            else
+                waypointIdx = i + 1;
+        }
+    }
+
+    /* found a good waypoint */
+    if (this->m_arrivalRoute.size() > waypointIdx)
+        return gsl::at(this->m_arrivalRoute, waypointIdx).name();
+    else
+        return gsl::at(this->m_arrivalRoute, 0).name();
+}
+
 void Inbound::update(EuroScopePlugIn::CFlightPlan& plan) {
     this->m_nextStarWaypoint = 0;
 
@@ -342,6 +431,10 @@ void Inbound::update(EuroScopePlugIn::CFlightPlan& plan) {
 
     /* find the point on the route */
     std::string direct(plan.GetControllerAssignedData().GetDirectToPointName());
+    if (0 == direct.length())
+        direct = this->findNextWaypointOnHeading(plan);
+    this->m_predictedWaypoint = direct;
+
     auto route = plan.GetExtractedRoute();
     int starEntry = route.GetPointsNumber(), directEntry = route.GetPointsNumber();
 
@@ -436,3 +529,7 @@ const Length& Inbound::trackmiles() const noexcept {
 const Time& Inbound::flighttime() const noexcept {
     return this->m_flighttime;
 }
+
+const std::string& Inbound::predictedWaypoint() const noexcept {
+    return this->m_predictedWaypoint;
+}