Fold latitude angles correctly across complete turns (#241)

`wrap_mpi2_pi2()` returns latitude angles outside its documented
`[-pi/2, pi/2]` range after complete turns: `wrap_mpi2_pi2(2*pi)`
currently returns `-2*pi`, and `3*pi/2` is left unchanged.

Reduce complete turns while preserving the sign and precision of small
angles, then reflect the remaining angle about the poles. Scalar/vector
return behavior is unchanged, and the general `angle_wrap(...,
mode="-pi/2:pi/2")` dispatcher receives the same correction.

Regression tests cover positive/negative complete and half turns from
-4*pi through4*pi, scalar/vector forms, the dispatcher, both sides of
each pole, exact1e-16 angles and empty vectors. The multiple-turn
regression fails before the fix. All20 vector tests pass after it; the
broader base suite passes149 tests with1 skip and5 existing warnings.
The repository-pinned Black23.10.0 and `git diff --check` pass.

Prepared with AI assistance.

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)