Initial commit

Lts/Services/AreaEffectSimulation/Trajectory/trajectory_transform.cpp

@@ -0,0 +1,86 @@
+/**
+ * @RDE_HEADER@
+ * TrajectoryTransform
+ * $Id$
+ * $HeadURL$
+ * $Author$
+ * Created by Gerlinde Rudolphus, ESZS, 2026
+ */
+
+#include "trajectory_transform.h"
+
+// Algorithmus: Zeitgewichtete Bézier-Korrektur (empfohlen)
+//               Kerngedanke
+//               Die ursprüngliche Trajektorie bleibt die Basis
+//               Eine glatte Korrekturkurve wird addiert
+//               Die Korrektur ist:
+//                                   0 am Start
+//                                   maximal am Ziel
+//                                   zeitlich C¹-stetig
+//                                   Formel
+//                                   Für jeden Punkt:
+//                                                     Pneu(t) = Palt(t) + w(t) · ΔZ
+//       mit
+//       ΔZ = Zneu − Zalt
+//     w(t) = 3s² − 2s³    (smoothstep)
+//     s = (t − t0) / (tEnd − t0)
+//  Dadurch:
+//              Start bleibt exakt gleich
+//              Zielpunkt wird exakt erreicht
+//              Bewegung bleibt glatt
+
+namespace AreaEffectSimulation
+{
+namespace TrajectoryTransform
+{
+
+
+double smoothStep(double x)
+{
+  return x * x * (3.0 - 2.0 * x);
+}
+
+template <typename T>
+T clamp(T v, T lo, T hi) {
+  return (v < lo) ? lo : (v > hi) ? hi : v; }
+
+
+
+QVector<TrajectoryPoint> transformToNewTarget(
+    const QVector<TrajectoryPoint>& originalTrajMetrical,
+    const LtsUtils::QPoint3D& newTargetMetrical)
+{
+  if (originalTrajMetrical.size() < 2)
+    return originalTrajMetrical;
+
+  QVector<TrajectoryPoint> result = originalTrajMetrical;
+
+  const LtsUtils::QPoint3D &oldTarget = originalTrajMetrical.last().position;
+  const LtsUtils::QPoint3D &delta = newTargetMetrical - oldTarget;
+
+  const double t0 = originalTrajMetrical.first().time;
+  const double tEnd = originalTrajMetrical.last().time;
+  const double dt = tEnd - t0;
+
+  for (int i = 0; i < result.size(); ++i)
+  {
+    double s = (result[i].time - t0) / dt;
+    s = clamp(s, 0.0, 1.0);
+
+    double w = smoothStep(s);
+    result[i].position += delta * w;
+  }
+
+  return result;
+}
+
+} // namespace TrajectoryTransform
+} // namespace AreaEffectSimulation
+
+
+std::ostream &operator<< (std::ostream   &stream, const AreaEffectSimulation::TrajectoryPoint &value)
+{
+  stream << "TrajectoryPoint(" << "position=" << value.position << "; time=" << value.time;
+  return stream;
+}
+