lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Rewrite math::angle_difference without trigonometry

Browse source
This commit is contained in:
Nikita Lisitsa 2025-09-07 14:00:25 +03:00
parent 3b88908534
commit 08f62bdaf7
1 changed files with 12 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
libs/math/include/psemek/math/math.hpp
View file

@ -128,19 +128,22 @@ namespace psemek::math
return std::pair{x1, x2}; return std::pair{x1, x2};
} }
// Moves the angle to the range [-pi, pi]
template <typename T>
T normalize_angle(T a)
{
while (a < -static_cast<T>(pi))
a += static_cast<T>(2 * pi);
while (a > static_cast<T>(pi))
a -= static_cast<T>(2 * pi);
return a;
}
// returns (a1 - a0) // returns (a1 - a0)
template <typename T> template <typename T>
T angle_difference(T a0, T a1) T angle_difference(T a0, T a1)
{ {
T const x0 = std::cos(a0); return normalize_angle(a1 - 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);
} }
template <typename T> template <typename T>