87 lines
2.3 KiB
C++
Executable File
87 lines
2.3 KiB
C++
Executable File
/**
|
||
* @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;
|
||
}
|
||
|