Add geom::angle_difference

libs/geom/include/psemek/geom/math.hpp

@@ -104,4 +104,18 @@ namespace psemek::geom
 		return std::pair{x1, x2};
 	}
 
+	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);
+	}
+
 }