fix(spatial): handle SE2 angles that round to the period (#86)

Author: Vamsi-2581

src/pgraph/PGraph.py

@@ -2058,14 +2058,17 @@ def prepare(q: NDArray) -> NDArray | None:  # noqa: F811
                     return None  # far outside the data: let the caller do it
                 if weight == 0:
                     return xy
-                return np.r_[xy, np.mod(weight * q[2], period)]
+                angle = np.mod(weight * q[2], period)
+                if angle >= period:
+                    angle = 0.0
+                return np.r_[xy, angle]
 
             if weight == 0:
                 tree = spatial.cKDTree(points[:, :2] - shift, boxsize=[size, size])
             else:
-                data = np.column_stack(
-                    [points[:, :2] - shift, np.mod(weight * points[:, 2], period)]
-                )
+                angle = np.mod(weight * points[:, 2], period)
+                angle[angle >= period] = 0.0
+                data = np.column_stack([points[:, :2] - shift, angle])
                 tree = spatial.cKDTree(data, boxsize=[size, size, period])
 
         self._kdtree = _KDTree(tree, index, p, prepare)

tests/test_spatial_index.py

@@ -168,6 +168,22 @@ def test_a_query_across_the_angle_wrap(self):
         self.assertEqual(v.name, "a")
         self.assertAlmostEqual(d, 0.2)
 
+    def test_a_negative_angle_that_rounds_to_the_period(self):
+        for metric in ("SE2", SE2Metric(0.3)):
+            with self.subTest(metric=metric):
+                for angle_is_in_graph in (True, False):
+                    with self.subTest(angle_is_in_graph=angle_is_in_graph):
+                        g = UGraph(dim=3, metric=metric)
+                        boundary_angle = -1e-17 if angle_is_in_graph else 0.0
+                        g.add_vertex([0.0, 0.0, boundary_angle], name="boundary")
+                        g.add_vertex([0.0, 0.0, 0.25], name="other")
+                        g.build_kdtree()
+
+                        query_angle = 0.0 if angle_is_in_graph else -1e-17
+                        query = [0.0, 0.0, query_angle]
+                        self.assertEqual(g.closest(query)[0].name, "boundary")
+                        self.assertEqual(g.near(query, 1e-12)[0][0].name, "boundary")
+
     def test_a_query_far_outside_is_still_right(self):
         g, P = points_graph(UGraph, dim=3, metric=SE2Metric(1.0))
         g.build_kdtree()