From c835925ca7f1782a0a93be04373453ebd08f5f86 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Thu, 11 Mar 2021 21:34:03 +0300
Subject: [PATCH] Add geom::angle_difference

---
 libs/geom/include/psemek/geom/math.hpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/libs/geom/include/psemek/geom/math.hpp b/libs/geom/include/psemek/geom/math.hpp
index 0c2d4b8c..9a10d075 100644
--- a/libs/geom/include/psemek/geom/math.hpp
+++ b/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);
+	}
+
 }