Repository: mishana/vectorized2d

Issue:
Add "Calc angle between vectors" method to Vector2D

Discussion:

Patch:
diff --git a/vectorized2d/vector2d.py b/vectorized2d/vector2d.py
index d33e6f6..f054ca7 100644
--- a/vectorized2d/vector2d.py
+++ b/vectorized2d/vector2d.py
@@ -84,6 +84,24 @@ class Vector2D(Array2D):
         new_direction = self.direction + rotation_angle
         return Vector2D(magnitude=self.norm, direction=new_direction)
 
+    @staticmethod
+    @njit
+    def _calc_angle_diff(direction_from: np.ndarray, direction_to: np.ndarray) -> np.ndarray:
+        raw_diff = direction_to - direction_from
+        diff = np.abs(raw_diff) % (2 * np.pi)
+        diff[diff > np.pi] = 2 * np.pi - diff[diff > np.pi]
+
+        mask = ((raw_diff >= -np.pi) & (raw_diff <= 0)) | ((raw_diff >= np.pi) & (raw_diff <= 2 * np.pi))
+        diff[mask] *= -1
+        return diff
+
+    def angle_to(self, v_towards: Vector2D):
+        """
+        Returns the angle between the current vector(s) and v_towards.
+        The angle is defined such that [(self.direction + angle) % 2*pi = v_towards.direction]
+        """
+        return self._calc_angle_diff(direction_from=self.direction, direction_to=v_towards.direction)
+
     @staticmethod
     @njit
     def _direction(v: Vector2D) -> np.ndarray:


Test patch:
diff --git a/tests/test_vector2d.py b/tests/test_vector2d.py
index 75fd5f0..c400c54 100644
--- a/tests/test_vector2d.py
+++ b/tests/test_vector2d.py
@@ -107,3 +107,17 @@ def test_rotation():
     expected_angles = (direction + rotation) % 360
 
     assert np.allclose(np.rad2deg(v_rotated.direction), expected_angles)
+
+
+def test_angle_to():
+    direction = np.random.random(size=(5000,)) * randint(1, 360) + 1
+    rotation = np.random.random(size=(5000,)) * randint(-180, 180)
+    magnitude = np.random.random(size=(5000,)) * randint(1, 20) + 1
+
+    v = Vector2D(magnitude=magnitude, direction=direction, direction_units=Vector2D.Units.DEGREES)
+    v_rotated = v.rotated(rotation_angle=rotation, rotation_units=Vector2D.Units.DEGREES)
+    angle_diff = v.angle_to(v_rotated)
+    assert np.allclose(np.rad2deg(angle_diff), rotation)
+
+    angle_diff = v_rotated.angle_to(v)
+    assert np.allclose(np.rad2deg(angle_diff), -rotation)
\ No newline at end of file