Fold latitude angles correctly across complete turns

Author: xiehuanyi

spatialmath/base/vectors.py

@@ -567,13 +567,10 @@ def wrap_mpi2_pi2(theta: ArrayLike) -> Union[float, NDArray]:
 
     """
     theta = getvector(theta)
-    n = theta / np.pi * 2
-    if isinstance(n, np.ndarray):
-        n = n.astype(int)
-    else:
-        n = np.fix(n).astype(int)
-
-    y = np.where(np.bitwise_and(n, 1) == 0, theta - n * np.pi, n * np.pi - theta)
+    theta = np.fmod(theta, 2 * np.pi)
+    theta = np.where(theta > np.pi, theta - 2 * np.pi, theta)
+    theta = np.where(theta < -np.pi, theta + 2 * np.pi, theta)
+    y = np.where(np.abs(theta) <= np.pi / 2, theta, np.copysign(np.pi, theta) - theta)
     if isinstance(y, np.ndarray) and len(y) == 1:
         return float(y[0])
     else:

tests/base/test_vectors.py

@@ -278,6 +278,30 @@ def test_angdiff(self):
         nt.assert_array_almost_equal(x, [0, 0, 0])
         self.assertIsInstance(x, np.ndarray)
 
+    def test_latitude_wrap_multiple_turns(self):
+        angles = pi * np.arange(-4, 4.5, 0.5)
+        expected = pi * np.array(
+            [0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0]
+        )
+        nt.assert_allclose(wrap_mpi2_pi2(angles), expected, atol=1e-14)
+        nt.assert_allclose(angle_wrap(angles, mode="-pi/2:pi/2"), expected, atol=1e-14)
+        for angle, folded in zip(angles, expected):
+            self.assertAlmostEqual(wrap_mpi2_pi2(angle), folded)
+
+    def test_latitude_wrap_boundaries_and_small_angles(self):
+        epsilon = 1e-12
+        angles = np.array(
+            [-pi / 2 - epsilon, -pi / 2 + epsilon, pi / 2 - epsilon, pi / 2 + epsilon]
+        )
+        expected = np.array(
+            [-pi / 2 + epsilon, -pi / 2 + epsilon, pi / 2 - epsilon, pi / 2 - epsilon]
+        )
+        nt.assert_allclose(wrap_mpi2_pi2(angles), expected, rtol=0, atol=1e-15)
+        nt.assert_allclose(
+            wrap_mpi2_pi2([1e-16, -1e-16]), [1e-16, -1e-16], rtol=0, atol=0
+        )
+        self.assertEqual(wrap_mpi2_pi2([]).size, 0)
+
     def test_wrap(self):
         self.assertAlmostEqual(wrap_0_2pi(0), 0)
         self.assertAlmostEqual(wrap_0_2pi(2 * pi), 0)