From 08f62bdaf75ae5e6dbf5e8be7ce07098661cf84b Mon Sep 17 00:00:00 2001 From: lisyarus Date: Sun, 7 Sep 2025 14:00:25 +0300 Subject: [PATCH] Rewrite math::angle_difference without trigonometry --- libs/math/include/psemek/math/math.hpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/libs/math/include/psemek/math/math.hpp b/libs/math/include/psemek/math/math.hpp index 19e11e24..8438e6c7 100644 --- a/libs/math/include/psemek/math/math.hpp +++ b/libs/math/include/psemek/math/math.hpp @@ -128,19 +128,22 @@ namespace psemek::math return std::pair{x1, x2}; } + // Moves the angle to the range [-pi, pi] + template + T normalize_angle(T a) + { + while (a < -static_cast(pi)) + a += static_cast(2 * pi); + while (a > static_cast(pi)) + a -= static_cast(2 * pi); + return a; + } + // returns (a1 - a0) template T angle_difference(T a0, T a1) { - T const x0 = std::cos(a0); - T const x1 = std::cos(a1); - T const y0 = std::sin(a0); - T const y1 = std::sin(a1); - - T const x = x0 * x1 + y0 * y1; - T const y = x0 * y1 - y0 * x1; - - return std::atan2(y, x); + return normalize_angle(a1 - a0); } template