55 lines
1.6 KiB
C++
Executable File
55 lines
1.6 KiB
C++
Executable File
#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);
|
||
}
|