diff --git a/libs/geom/include/psemek/geom/math.hpp b/libs/geom/include/psemek/geom/math.hpp index 6894d080..573473b8 100644 --- a/libs/geom/include/psemek/geom/math.hpp +++ b/libs/geom/include/psemek/geom/math.hpp @@ -110,6 +110,7 @@ namespace psemek::geom return std::pair{x1, x2}; } + // returns (a1 - a0) template T angle_difference(T a0, T a1) { @@ -124,6 +125,17 @@ namespace psemek::geom return std::atan2(y, x); } + template + T rotate_towards(T value, T target, T max_difference) + { + auto d = angle_difference(value, target); + if (d > max_difference) + d = max_difference; + if (d < -max_difference) + d = -max_difference; + return value + d; + } + template T idiv(T x, T m) {