Initial commit

Lts/Tools/Trajectories/TrajectoryWidget/trajectorywarp.cpp

@@ -0,0 +1,54 @@
+#include "trajectorywarp.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
+
+QVector<TrajectoryPoint> TrajectoryWarp::warpToNewTarget(
+    const QVector<TrajectoryPoint>& original,
+    const QVector3D& newTarget)
+{
+    if (original.size() < 2)
+        return original;
+
+    QVector<TrajectoryPoint> result = original;
+
+    const QVector3D& oldTarget = original.last().position;
+    QVector3D delta = newTarget - oldTarget;
+
+    double t0 = original.first().time;
+    double tEnd = original.last().time;
+    double dt = tEnd - t0;
+
+    for (int i = 0; i < result.size(); ++i)
+    {
+        double s = (result[i].time - t0) / dt;
+        s = std::clamp(s, 0.0, 1.0);
+
+        double w = smoothStep(s);
+        result[i].position += delta * w;
+    }
+
+    return result;
+}
+
+double TrajectoryWarp::smoothStep(double x)
+{
+    return x * x * (3.0 - 2.0 * x);
+}