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 <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)
 	template <typename T>
 	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 <typename T>