diff --git a/libs/math/include/psemek/math/math.hpp b/libs/math/include/psemek/math/math.hpp index a19ef28d..6f3f194a 100644 --- a/libs/math/include/psemek/math/math.hpp +++ b/libs/math/include/psemek/math/math.hpp @@ -235,4 +235,17 @@ namespace psemek::math return T{1} / T{2} - std::sin(std::asin(T{1} - T{2} * x) / T{3}); } + // Stateless piecewise-quadratic spline acceleration + // based on current velocity, current distance to target, + // and max acceleration + template + T quadratic_spline_acceleration(T velocity, T distance, T max_acceleration) + { + T max_velocity = std::sqrt(2.f * std::abs(distance) * max_acceleration); + return max_acceleration + * (distance < 0.f ? -1.f : 1.f) + * ((std::abs(velocity) < max_velocity || (distance > 0.f) != (velocity > 0.f))? 1.f : -1.f) + ; + } + }