Initial commit

Lts/Services/AreaEffectSimulation/TrajectorySplitter/trajectory_splitter.h

@@ -0,0 +1,211 @@
+/**
+  * @RDE_HEADER@
+  * TrajectorySplitter
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  * Created by Norbert Thoden, ESVG, 2026-04-10
+  */
+
+#pragma once
+
+//#include <LtsCoordinate/ltscoordinate_export.h>
+
+#include <Core/core.hpp>
+#include <QVector>
+
+
+namespace AreaEffectSimulation
+{
+
+// E01562-294: Falsche Darstellung Flugkurve SMArt
+// At SMArt ADLER (or SAA?) generates a polyline which is "dividing":
+//        x  x
+//     x        x
+//   x        x   x
+// x         x     x
+// | Start   | T1  | T2   (Target 1 & Target 2)
+// Same diagram, but with order
+//        x4  x5
+//     x3        x6 (duplicate)
+//   x2        x7   x10
+// x1         x8     x11
+//                    x12
+// The relativeTime step backward between x8 and x10.
+
+//using TRAJECTORY_POINT_TYPE = Core::RelativePositionRelativeTime;
+//using TRAJECTORY_TYPE       = std::vector<Core::RelativePositionRelativeTime>;
+
+//!
+//! \brief The TrajectorySplitter class splits a combined/special vector into single trajectories.
+//! See UnitTestTrajectorySplitter
+//!
+template<typename TRAJECTORY_TYPE>
+class TrajectorySplitter
+{
+public:
+
+//  friend class UnitTestTrajectorySplitter;
+
+  explicit TrajectorySplitter(const TRAJECTORY_TYPE &trajectory)
+    : m_trajectory(trajectory)
+  {}
+
+  TRAJECTORY_TYPE getTrunk() const
+  {
+    if (!init())
+    {
+      return {};
+    }
+
+    TRAJECTORY_TYPE trunk(m_trajectory.begin(), m_trajectory.begin()+m_branchIndex);
+    return trunk;
+  }
+
+  TRAJECTORY_TYPE getBranch1() const
+  {
+    if (!init())
+    {
+      return {};
+    }
+
+    const int idx = findIndexOfEndOfFirstBranchInternal(m_trajectory);
+
+    const TRAJECTORY_TYPE branch1(m_trajectory.begin()+m_branchIndex, m_trajectory.begin()+idx+1);
+    return branch1;
+  }
+
+  TRAJECTORY_TYPE getBranch2() const
+  {
+    if (!init())
+    {
+      return {};
+    }
+
+    const int idx = findIndexOfEndOfFirstBranchInternal(m_trajectory);
+
+    const TRAJECTORY_TYPE branch2(m_trajectory.begin()+idx+1, m_trajectory.end());
+    return branch2;
+  }
+
+  TRAJECTORY_TYPE getTrajectory1() const
+  {
+    if (!init())
+    {
+      return {};
+    }
+
+    auto complete = getTrunk();
+    const auto &branch = getBranch1();
+    complete.insert(complete.end(), branch.begin(), branch.end());
+    return complete;
+  }
+
+  TRAJECTORY_TYPE getTrajectory2() const
+  {
+    if (!init())
+    {
+      return {};
+    }
+
+    auto complete = getTrunk();
+    const auto &branch = getBranch2();
+    complete.insert(complete.end(), branch.begin(), branch.end());
+    return complete;
+  }
+
+  size_t rawTrajectorySize() const { return m_trajectory.size(); }
+
+protected:
+  static qint64 findDuplicateTimeInternal(const TRAJECTORY_TYPE &trajectory)
+  {
+    // extract relativeTimes, sort, find adjacent duplicates
+    QVector<qint64> relativeTimes;
+    relativeTimes.reserve(static_cast<int>(trajectory.size()));
+
+    for (const auto &posTimeAware : trajectory)
+    {
+      relativeTimes.append(posTimeAware.relativeTime());
+    }
+
+    std::sort(relativeTimes.begin(), relativeTimes.end());
+
+    auto it = std::adjacent_find(relativeTimes.begin(), relativeTimes.end());
+
+    if (it == relativeTimes.end())
+    {
+      return -1;   // not found
+    }
+
+    const qint64 duplicateRelativeTime = *it;
+    return duplicateRelativeTime;
+  }
+  static int findDuplicateBranchIndexInternal(const TRAJECTORY_TYPE &trajectory)
+  {
+    const qint64 duplicateRelativeTime = findDuplicateTimeInternal(trajectory);
+
+    if (duplicateRelativeTime < 0)
+    {
+      return -1;   // not found
+    }
+
+    // find first occurance:
+    int idx = 0;
+    {
+      for (const auto &posTimeAware : trajectory)
+      {
+        const qint64 currentTime = posTimeAware.relativeTime();
+
+        if (duplicateRelativeTime == currentTime)
+        {
+          break;
+        }
+        ++idx;
+      } // for
+    }
+    return idx;
+  }
+  static int findIndexOfEndOfFirstBranchInternal(const TRAJECTORY_TYPE &trajectory)
+  {
+    int idx = -1;
+    {
+      qint64 currentTime = std::numeric_limits<qint64>::min();
+      for (const auto &posTimeAware : trajectory)
+      {
+        if (posTimeAware.relativeTime() < currentTime)
+        {
+          break;
+        }
+        currentTime = posTimeAware.relativeTime();
+        ++idx;
+      } // for
+    }
+    return idx;
+  }
+
+  bool init() const
+  {
+    // already calculated?
+    if (m_branchIndex > 0)
+    {
+      return true;
+    }
+
+    // sanity check:
+    if (m_trajectory.size() < 3)
+    {
+      return false;
+    }
+
+    m_branchIndex = findDuplicateBranchIndexInternal(m_trajectory);
+
+    return (m_branchIndex >= 0);
+  }
+
+private:
+  TRAJECTORY_TYPE m_trajectory;
+  mutable int m_branchIndex{0};
+};
+
+} // namespace AreaEffectSimulation
+