python-motion-planning 2.0.dev2__py3-none-any.whl → 2.1__py3-none-any.whl

python_motion_planning/common/env/map/grid.py

@@ -1,20 +1,358 @@
 """
 @file: grid.py
 @author: Wu Maojia
-@update: 2025.12.20
+@update: 2026.9.11
 """
 from itertools import product
-from typing import Iterable, Union, Tuple, Callable, List, Dict
+from typing import Iterable, Union, Tuple, List, Dict
 import time
 
 import numpy as np
 from scipy import ndimage
 
+try:
+    from numba import njit as _numba_njit
+except Exception:  # pragma: no cover - keeps import compatibility without numba.
+    _numba_njit = None
+
 from python_motion_planning.common.env.map.base_map import BaseMap
 from python_motion_planning.common.env import Node, TYPES
 from python_motion_planning.common.utils.geometry import Geometry
 
 
+def _njit(*args, **kwargs):
+    if _numba_njit is None:
+        if args and callable(args[0]):
+            return args[0]
+
+        def decorator(func):
+            return func
+
+        return decorator
+
+    return _numba_njit(*args, **kwargs)
+
+
+@_njit(cache=True)
+def _grid_flat_index(point: np.ndarray, shape: np.ndarray) -> int:
+    idx = 0
+    for d in range(shape.size):
+        idx = idx * shape[d] + point[d]
+    return idx
+
+
+@_njit(cache=True)
+def _grid_within_bounds(point: np.ndarray, shape: np.ndarray) -> bool:
+    for d in range(shape.size):
+        if point[d] < 0 or point[d] >= shape[d]:
+            return False
+    return True
+
+
+@_njit(cache=True)
+def _grid_is_expandable(
+    point: np.ndarray,
+    src_point: np.ndarray,
+    has_src_point: bool,
+    shape: np.ndarray,
+    type_map: np.ndarray,
+    esdf: np.ndarray,
+    obstacle_type: int,
+    inflation_type: int,
+    strict_collision: bool,
+) -> bool:
+    if not _grid_within_bounds(point, shape):
+        return False
+
+    point_idx = _grid_flat_index(point, shape)
+    src_idx = 0
+    src_is_inflation = False
+    if has_src_point:
+        src_idx = _grid_flat_index(src_point, shape)
+        src_is_inflation = type_map[src_idx] == inflation_type
+
+    point_type = type_map[point_idx]
+    if point_type == obstacle_type or (
+        point_type == inflation_type
+        and not (src_is_inflation and esdf[point_idx] >= esdf[src_idx])
+    ):
+        return False
+
+    if not has_src_point or not strict_collision:
+        return True
+
+    dim = point.size
+    if dim == 2:
+        dx = point[0] - src_point[0]
+        dy = point[1] - src_point[1]
+        if abs(dx) > 1 or abs(dy) > 1 or dx == 0 or dy == 0:
+            return True
+
+        side_idx = point[0] * shape[1] + src_point[1]
+        side_type = type_map[side_idx]
+        if side_type == obstacle_type or (
+            side_type == inflation_type
+            and not (src_is_inflation and esdf[side_idx] >= esdf[src_idx])
+        ):
+            return False
+
+        side_idx = src_point[0] * shape[1] + point[1]
+        side_type = type_map[side_idx]
+        return side_type != obstacle_type and (
+            side_type != inflation_type
+            or (src_is_inflation and esdf[side_idx] >= esdf[src_idx])
+        )
+
+    changed_mask = 0
+    for d in range(dim):
+        diff = point[d] - src_point[d]
+        if abs(diff) > 1:
+            return True
+        if diff != 0:
+            changed_mask |= 1 << d
+
+    if not changed_mask & (changed_mask - 1):
+        return True
+
+    subset = (changed_mask - 1) & changed_mask
+    while subset:
+        side_idx = 0
+        for d in range(dim):
+            side = point[d] if subset & (1 << d) else src_point[d]
+            side_idx = side_idx * shape[d] + side
+
+        side_type = type_map[side_idx]
+        if side_type == obstacle_type or (
+            side_type == inflation_type
+            and not (src_is_inflation and esdf[side_idx] >= esdf[src_idx])
+        ):
+            return False
+        subset = (subset - 1) & changed_mask
+
+    return True
+
+
+@_njit(cache=True)
+def _grid_world_to_map_int(
+    point: np.ndarray,
+    bounds: np.ndarray,
+    resolution: float,
+    shape: np.ndarray,
+) -> np.ndarray:
+    point_map = np.empty(shape.size, dtype=np.int64)
+    inv_resolution = 1.0 / resolution
+    for d in range(shape.size):
+        value = int(round((point[d] - bounds[d, 0]) * inv_resolution - 0.5))
+        if value < 0:
+            value = 0
+        elif value >= shape[d]:
+            value = shape[d] - 1
+        point_map[d] = value
+    return point_map
+
+
+@_njit(cache=True)
+def _grid_line_of_sight(p1: np.ndarray, p2: np.ndarray) -> np.ndarray:
+    dim = p1.size
+    delta = np.empty(dim, dtype=np.int64)
+    abs_delta = np.empty(dim, dtype=np.int64)
+    delta2 = np.empty(dim, dtype=np.int64)
+
+    primary_axis = 0
+    max_delta = 0
+    for d in range(dim):
+        delta[d] = p2[d] - p1[d]
+        abs_delta[d] = abs(delta[d])
+        delta2[d] = 2 * abs_delta[d]
+        if abs_delta[d] > max_delta:
+            max_delta = abs_delta[d]
+            primary_axis = d
+
+    primary_step = 1 if delta[primary_axis] > 0 else -1
+    steps = abs_delta[primary_axis]
+    result = np.empty((steps + 1, dim), dtype=np.int64)
+    current = p1.copy()
+
+    for d in range(dim):
+        result[0, d] = current[d]
+
+    error = np.zeros(dim, dtype=np.int64)
+    for i in range(1, steps + 1):
+        current[primary_axis] += primary_step
+
+        for d in range(dim):
+            if d == primary_axis:
+                continue
+
+            error[d] += delta2[d]
+            # Reverse the tie-break when traversing the primary axis backwards.
+            if error[d] > abs_delta[primary_axis] or (
+                error[d] == abs_delta[primary_axis] and primary_step < 0
+            ):
+                current[d] += 1 if delta[d] > 0 else -1
+                error[d] -= delta2[primary_axis]
+
+        for d in range(dim):
+            result[i, d] = current[d]
+
+    return result
+
+
+@_njit(cache=True)
+def _grid_in_collision(
+    p1: np.ndarray,
+    p2: np.ndarray,
+    shape: np.ndarray,
+    type_map: np.ndarray,
+    esdf: np.ndarray,
+    obstacle_type: int,
+    inflation_type: int,
+    strict_collision: bool,
+) -> bool:
+    if not _grid_is_expandable(p1, p1, False, shape, type_map, esdf, obstacle_type, inflation_type, False):
+        return True
+    if not _grid_is_expandable(p2, p1, True, shape, type_map, esdf, obstacle_type, inflation_type, False):
+        return True
+
+    dim = p1.size
+    same_point = True
+    for d in range(dim):
+        if p1[d] != p2[d]:
+            same_point = False
+            break
+    if same_point:
+        return False
+
+    delta = np.empty(dim, dtype=np.int64)
+    abs_delta = np.empty(dim, dtype=np.int64)
+    delta2 = np.empty(dim, dtype=np.int64)
+
+    primary_axis = 0
+    max_delta = 0
+    for d in range(dim):
+        delta[d] = p2[d] - p1[d]
+        abs_delta[d] = abs(delta[d])
+        delta2[d] = 2 * abs_delta[d]
+        if abs_delta[d] > max_delta:
+            max_delta = abs_delta[d]
+            primary_axis = d
+
+    primary_step = 1 if delta[primary_axis] > 0 else -1
+    steps = abs_delta[primary_axis]
+    current = p1.copy()
+    last_point = np.empty(dim, dtype=np.int64)
+    error = np.zeros(dim, dtype=np.int64)
+
+    for _ in range(steps):
+        for d in range(dim):
+            last_point[d] = current[d]
+
+        current[primary_axis] += primary_step
+
+        for d in range(dim):
+            if d == primary_axis:
+                continue
+
+            error[d] += delta2[d]
+            # Keep the same cells as _grid_line_of_sight in both directions.
+            if error[d] > abs_delta[primary_axis] or (
+                error[d] == abs_delta[primary_axis] and primary_step < 0
+            ):
+                current[d] += 1 if delta[d] > 0 else -1
+                error[d] -= delta2[primary_axis]
+
+        if not _grid_is_expandable(
+            current,
+            last_point,
+            True,
+            shape,
+            type_map,
+            esdf,
+            obstacle_type,
+            inflation_type,
+            strict_collision,
+        ):
+            return True
+
+    return False
+
+
+@_njit(cache=True)
+def _grid_neighbor_positions_and_mask(
+    current: np.ndarray,
+    offsets: np.ndarray,
+    shape: np.ndarray,
+    type_map: np.ndarray,
+    esdf: np.ndarray,
+    obstacle_type: int,
+    inflation_type: int,
+    strict_collision: bool,
+) -> Tuple[np.ndarray, np.ndarray]:
+    node_num = offsets.shape[0]
+    dim = offsets.shape[1]
+    positions = np.empty((node_num, dim), dtype=np.int64)
+    mask = np.zeros(node_num, dtype=np.bool_)
+    neighbor = np.empty(dim, dtype=np.int64)
+
+    for i in range(node_num):
+        for d in range(dim):
+            neighbor[d] = current[d] + offsets[i, d]
+            positions[i, d] = neighbor[d]
+
+        mask[i] = _grid_is_expandable(
+            neighbor,
+            current,
+            True,
+            shape,
+            type_map,
+            esdf,
+            obstacle_type,
+            inflation_type,
+            strict_collision,
+        )
+
+    return positions, mask
+
+
+@_njit(cache=True)
+def _grid_path_map_to_world(points: np.ndarray, bounds: np.ndarray, resolution: float) -> np.ndarray:
+    path_world = np.empty((points.shape[0], points.shape[1]), dtype=np.float64)
+    for i in range(points.shape[0]):
+        for d in range(points.shape[1]):
+            path_world[i, d] = (points[i, d] + 0.5) * resolution + bounds[d, 0]
+    return path_world
+
+
+@_njit(cache=True)
+def _grid_path_world_to_map_float(points: np.ndarray, bounds: np.ndarray, resolution: float) -> np.ndarray:
+    path_map = np.empty((points.shape[0], points.shape[1]), dtype=np.float64)
+    inv_resolution = 1.0 / resolution
+    for i in range(points.shape[0]):
+        for d in range(points.shape[1]):
+            path_map[i, d] = (points[i, d] - bounds[d, 0]) * inv_resolution - 0.5
+    return path_map
+
+
+@_njit(cache=True)
+def _grid_path_world_to_map_int(
+    points: np.ndarray,
+    bounds: np.ndarray,
+    resolution: float,
+    shape: np.ndarray,
+) -> np.ndarray:
+    path_map = np.empty((points.shape[0], shape.size), dtype=np.int64)
+    inv_resolution = 1.0 / resolution
+    for i in range(points.shape[0]):
+        for d in range(shape.size):
+            value = int(round((points[i, d] - bounds[d, 0]) * inv_resolution - 0.5))
+            if value < 0:
+                value = 0
+            elif value >= shape[d]:
+                value = shape[d] - 1
+            path_map[i, d] = value
+    return path_map
+
+
 class GridTypeMap:
     """
     Class for Grid Type Map. It is like a np.ndarray, except that its shape and dtype are fixed.
@@ -47,6 +385,7 @@ class GridTypeMap:
         self._data = np.asarray(type_map)
         self._shape = self._data.shape
         self._dtype = self._data.dtype
+        self._lazy_flags_ = 3
         
         self._dtype_options = [np.int8, np.int16, np.int32, np.int64]
         if self._dtype not in self._dtype_options:
@@ -63,6 +402,7 @@ class GridTypeMap:
 
     def __setitem__(self, idx, value):
         self._data[idx] = value
+        self._lazy_flags_ = 3
 
     @property
     def data(self) -> np.ndarray:
@@ -89,6 +429,7 @@ class Grid(BaseMap):
         resolution: resolution of the grid map
         type_map: initial type map of the grid map (its shape must be the same as the converted grid map shape, and its dtype must be int)
         inflation_radius: radius of the inflation
+        strict_collision: whether diagonal steps beside obstacles or inflation are collisions (default: True)
 
     Examples:
         >>> grid_map = Grid(bounds=[[0, 51], [0, 31]], resolution=0.5)
@@ -137,7 +478,7 @@ class Grid(BaseMap):
 
         >>> grid_map[1, 0] = TYPES.OBSTACLE     # place an obstacle
         >>> grid_map.get_neighbors(Node((0, 0)))    # limited within the bounds
-        [Node((0, 1), (0, 0), 0, 0), Node((1, 1), (0, 0), 0, 0)]
+        [Node((0, 1), (0, 0), 0, 0)]
 
         >>> grid_map.get_neighbors(Node((grid_map.shape[0] - 1, grid_map.shape[1] - 1)), diagonal=False)  # limited within the boundss
         [Node((100, 61), (101, 61), 0, 0), Node((101, 60), (101, 61), 0, 0)]
@@ -152,15 +493,31 @@ class Grid(BaseMap):
         False
 
         >>> grid_map[1, 3] = TYPES.OBSTACLE
-        >>> grid_map.update_esdf()
         >>> grid_map.in_collision((1, 2), (3, 6))
         True
+
+        >>> grid_map = Grid(bounds=[[0, 3], [0, 3]])
+        >>> grid_map[1, 0] = TYPES.OBSTACLE
+        >>> grid_map.in_collision((0, 0), (1, 1))
+        True
+        >>> grid_map.strict_collision = False
+        >>> grid_map.in_collision((0, 0), (1, 1))
+        False
+
+        >>> grid_map = Grid(bounds=[[0, 4], [0, 4]])
+        >>> grid_map[2, :] = TYPES.OBSTACLE
+        >>> grid_map.is_connected((0, 0), (3, 3))
+        False
+        >>> grid_map[2, 1] = TYPES.FREE
+        >>> grid_map.is_connected((0, 0), (3, 3))
+        True
     """
     def __init__(self, 
                 bounds: Iterable = [[0, 30], [0, 40]], 
                 resolution: float = 1.0, 
                 type_map: Union[GridTypeMap, np.ndarray] = None,
                 inflation_radius: float = 0.0,
+                strict_collision: bool = True,
                 ) -> None:
         super().__init__(bounds)
 
@@ -168,22 +525,24 @@ class Grid(BaseMap):
         shape = tuple([int((self.bounds[i, 1] - self.bounds[i, 0]) / self.resolution) for i in range(self.dim)])
 
         if type_map is None:
-            self.type_map = GridTypeMap(np.zeros(shape, dtype=np.int8))
+            self._type_map = GridTypeMap(np.zeros(shape, dtype=np.int8))
         else:
             if type_map.shape != shape:
                 raise ValueError("Shape must be {} instead of {} with given bounds={} and resolution={}".format(shape, type_map.shape, self.bounds, self.resolution))
 
             if isinstance(type_map, GridTypeMap):
-                self.type_map = type_map
+                self._type_map = type_map
             elif isinstance(type_map, np.ndarray):
-                self.type_map = GridTypeMap(type_map)        
+                self._type_map = GridTypeMap(type_map)
             else:
                 raise ValueError("Type map must be GridTypeMap or numpy.ndarray instead of {}".format(type(type_map)))
 
+        self._shape_array = np.asarray(self.shape, dtype=np.int64)
         self._precompute_offsets()
         
         self._esdf = np.zeros(self.shape, dtype=np.float32)
-        # self.update_esdf()    # updated in self.inflate_obstacles()
+        self._connectivity_map = np.zeros(self.shape, dtype=np.int32)
+        self.strict_collision = strict_collision
 
         self.inflation_radius = inflation_radius
         if self.inflation_radius >= 1:
@@ -198,28 +557,79 @@ class Grid(BaseMap):
     @property
     def resolution(self) -> float:
         return self._resolution
+
+    @property
+    def type_map(self) -> GridTypeMap:
+        return self._type_map
     
     @property
     def shape(self) -> tuple:
-        return self.type_map.shape
+        return self._type_map.shape
     
     @property
     def dtype(self) -> np.dtype:
-        return self.type_map.dtype
+        return self._type_map.dtype
     
     @property
     def esdf(self) -> np.ndarray:
+        if self._esdf_lazy_flag_:
+            self.update_esdf()
+            self._esdf_lazy_flag_ = False
         return self._esdf
+
+    @property
+    def connectivity_map(self) -> np.ndarray:
+        if self._connectivity_lazy_flag_:
+            self.update_connectivity()
+            self._connectivity_lazy_flag_ = False
+        return self._connectivity_map
     
     @property
     def data(self) -> np.ndarray:
-        return self.type_map.data
+        return self._type_map.data
     
     def __getitem__(self, idx):
-        return self.type_map[idx]
+        return self._type_map[idx]
 
     def __setitem__(self, idx, value):
-        self.type_map[idx] = value
+        self._type_map[idx] = value
+
+    @property
+    def _esdf_lazy_flag_(self) -> bool:
+        return bool(self._type_map._lazy_flags_ & 1)
+
+    @_esdf_lazy_flag_.setter
+    def _esdf_lazy_flag_(self, value: bool) -> None:
+        if value:
+            self._type_map._lazy_flags_ |= 1
+        else:
+            self._type_map._lazy_flags_ &= ~1
+
+    @property
+    def _connectivity_lazy_flag_(self) -> bool:
+        return bool(self._type_map._lazy_flags_ & 2)
+
+    @_connectivity_lazy_flag_.setter
+    def _connectivity_lazy_flag_(self, value: bool) -> None:
+        if value:
+            self._type_map._lazy_flags_ |= 2
+        else:
+            self._type_map._lazy_flags_ &= ~2
+
+    @property
+    def strict_collision(self) -> bool:
+        return self._strict_collision
+
+    @strict_collision.setter
+    def strict_collision(self, value: bool) -> None:
+        self._strict_collision = value
+        self._connectivity_lazy_flag_ = True
+
+    def _type_map_flat(self) -> np.ndarray:
+        return np.ravel(self._type_map.data)
+
+    def _esdf_flat(self) -> np.ndarray:
+        return np.ravel(self.esdf)
 
     def map_to_world(self, point: tuple) -> Tuple[float, ...]:
         """
@@ -234,7 +644,9 @@ class Grid(BaseMap):
         if len(point) != self.dim:
             raise ValueError("Point dimension does not match map dimension.")
 
-        return tuple((x + 0.5) * self.resolution + float(self.bounds[i, 0]) for i, x in enumerate(point))
+        bounds = self.bounds
+        resolution = self.resolution
+        return tuple(float((point[d] + 0.5) * resolution + bounds[d, 0]) for d in range(self.dim))
 
     def world_to_map(self, point: Tuple[float, ...], discrete: bool = True) -> tuple:
         """
@@ -250,10 +662,13 @@ class Grid(BaseMap):
         if len(point) != self.dim:
             raise ValueError("Point dimension does not match map dimension.")
         
-        point_map = tuple((x - float(self.bounds[i, 0])) * (1.0 / self.resolution) - 0.5 for i, x in enumerate(point))
         if discrete:
-            point_map = self.point_float_to_int(point_map)
-        return point_map
+            point_map = _grid_world_to_map_int(np.asarray(point, dtype=np.float64), self.bounds, self.resolution, self._shape_array)
+            return tuple(int(x) for x in point_map)
+        else:
+            inv_resolution = 1.0 / self.resolution
+            bounds = self.bounds
+            return tuple(float((point[d] - bounds[d, 0]) * inv_resolution - 0.5) for d in range(self.dim))
 
     def get_distance(self, p1: Tuple[int, int], p2: Tuple[int, int]) -> float:
         """
@@ -278,18 +693,32 @@ class Grid(BaseMap):
         Returns:
             bool: True if the point is within the bounds of the map, False otherwise.
         """
-        # if point.dim != self.dim:
-        #     raise ValueError("Point dimension does not match map dimension.")
-
-        # return all(0 <= point[i] < self.shape[i] for i in range(self.dim))
-        dim = self.dim
+        if len(point) != self.dim:
+            return False
         shape = self.shape
-        
-        for i in range(dim):
-            if not (0 <= point[i] < shape[i]):
+        for d in range(self.dim):
+            if point[d] < 0 or point[d] >= shape[d]:
                 return False
         return True
 
+    def is_connected(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
+        """
+        Check whether two points belong to the same free-space component.
+
+        Args:
+            p1: First point.
+            p2: Second point.
+
+        Returns:
+            connected: True if the two points are connected, False otherwise.
+        """
+        if not self.within_bounds(p1) or not self.within_bounds(p2):
+            raise ValueError("Points are out of bounds or invalid.")
+
+        connectivity_map = self.connectivity_map
+        component = connectivity_map[p1]
+        return component != 0 and component == connectivity_map[p2]
+
     def is_expandable(self, point: Tuple[int, ...], src_point: Tuple[int, ...] = None) -> bool:
         """
         Check if a point is expandable.
@@ -301,13 +730,21 @@ class Grid(BaseMap):
         Returns:
             expandable: True if the point is expandable, False otherwise.
         """
-        if not self.within_bounds(point):
-            return False
-        if src_point is not None:
-            if self.type_map[src_point] == TYPES.INFLATION and self._esdf[point] >= self._esdf[src_point]:
-                return True
-                
-        return not self.type_map[point] == TYPES.OBSTACLE and not self.type_map[point] == TYPES.INFLATION
+        point_array = np.asarray(point, dtype=np.int64)
+        has_src_point = src_point is not None
+        src_array = point_array if src_point is None else np.asarray(src_point, dtype=np.int64)
+
+        return _grid_is_expandable(
+            point_array,
+            src_array,
+            has_src_point,
+            self._shape_array,
+            self._type_map_flat(),
+            self._esdf_flat(),
+            TYPES.OBSTACLE,
+            TYPES.INFLATION,
+            self.strict_collision,
+        )
 
     def get_neighbors(self, 
                     node: Node, 
@@ -326,18 +763,28 @@ class Grid(BaseMap):
         if node.dim != self.dim:
             raise ValueError("Node dimension does not match map dimension.")
         
-        offsets = self._diagonal_offsets if diagonal else self._orthogonal_offsets
-        
-        # Generate all neighbor positions
-        # neighbor_positions = current_pos + offsets
-        neighbors = [node + offset for offset in offsets]
-        filtered_neighbors = []
-
-        for neighbor in neighbors:
-            if self.is_expandable(neighbor.current, node.current):
-                filtered_neighbors.append(neighbor)
-        
-        return filtered_neighbors
+        positions, mask = self._get_neighbor_arrays(node, diagonal)
+
+        return [
+            Node(tuple(positions[i].tolist()), node.current, node.g, node.h)
+            for i in range(positions.shape[0])
+            if mask[i]
+        ]
+
+    def _get_neighbor_arrays(self, node: Node, diagonal: bool = True) -> Tuple[np.ndarray, np.ndarray]:
+        """Get candidate neighbor positions and their expandable mask."""
+        offsets = self._diagonal_offsets_array if diagonal else self._orthogonal_offsets_array
+        positions, mask = _grid_neighbor_positions_and_mask(
+            np.asarray(node.current, dtype=np.int64),
+            offsets,
+            self._shape_array,
+            self._type_map_flat(),
+            self._esdf_flat(),
+            TYPES.OBSTACLE,
+            TYPES.INFLATION,
+            self.strict_collision,
+        )
+        return positions, mask
 
     def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> List[Tuple[int, ...]]:
         """
@@ -350,45 +797,13 @@ class Grid(BaseMap):
         Returns:
             points: List of point on the line of sight.
         """
-        p1 = np.array(p1)
-        p2 = np.array(p2)
+        p1_array = np.asarray(p1, dtype=np.int64)
+        p2_array = np.asarray(p2, dtype=np.int64)
+        if p1_array.shape != p2_array.shape:
+            p2_array - p1_array
 
-        dim = len(p1)
-        delta = p2 - p1
-        abs_delta = np.abs(delta)
-        
-        # Determine the main direction axis (the dimension with the greatest change)
-        primary_axis = np.argmax(abs_delta)
-        primary_step = 1 if delta[primary_axis] > 0 else -1
-        
-        # Initialize the error variable
-        error = np.zeros(dim, dtype=int)
-        delta2 = 2 * abs_delta
-        
-        # Calculate the number of steps and initialize the current point
-        steps = abs_delta[primary_axis]
-        current = p1
-        
-        # Allocate the result array
-        result = []
-        result.append(tuple(int(x) for x in current))
-        
-        for i in range(1, steps + 1):
-            current[primary_axis] += primary_step
-            
-            # Update the error for the primary dimension
-            for d in range(dim):
-                if d == primary_axis:
-                    continue
-                    
-                error[d] += delta2[d]
-                if error[d] > abs_delta[primary_axis]:
-                    current[d] += 1 if delta[d] > 0 else -1
-                    error[d] -= delta2[primary_axis]
-            
-            result.append(tuple(int(x) for x in current))
-
-        return result
+        points = _grid_line_of_sight(p1_array, p2_array)
+        return [tuple(int(x) for x in points[i]) for i in range(points.shape[0])]
 
     def in_collision(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
         """
@@ -401,51 +816,21 @@ class Grid(BaseMap):
         Returns:
             in_collision: True if the line of sight is in collision, False otherwise.
         """
-        if not self.is_expandable(p1) or not self.is_expandable(p2, p1):
-            return True
-
-        # Corner Case: Start and end points are the same
-        if p1 == p2:
-            return False
-        
-        p1 = np.array(p1)
-        p2 = np.array(p2)
-
-        # Calculate delta and absolute delta
-        delta = p2 - p1
-        abs_delta = np.abs(delta)
-        
-        # Determine the primary axis (the dimension with the greatest change)
-        primary_axis = np.argmax(abs_delta)
-        primary_step = 1 if delta[primary_axis] > 0 else -1
-        
-        # Initialize the error variable
-        error = np.zeros_like(delta, dtype=np.int32)
-        delta2 = 2 * abs_delta
-        
-        # calculate the number of steps and initialize the current point
-        steps = abs_delta[primary_axis]
-        current = p1
-        
-        for _ in range(steps):
-            last_point = current.copy()
-            current[primary_axis] += primary_step
-            
-            # Update the error for the primary dimension
-            for d in range(len(delta)):
-                if d == primary_axis:
-                    continue
-                    
-                error[d] += delta2[d]
-                if error[d] > abs_delta[primary_axis]:
-                    current[d] += 1 if delta[d] > 0 else -1
-                    error[d] -= delta2[primary_axis]
-
-            # Check the current point
-            if not self.is_expandable(tuple(current), tuple(last_point)):
-                return True
-        
-        return False
+        p1_array = np.asarray(p1, dtype=np.int64)
+        p2_array = np.asarray(p2, dtype=np.int64)
+        if p1_array.shape != p2_array.shape:
+            p2_array - p1_array
+
+        return _grid_in_collision(
+            p1_array,
+            p2_array,
+            self._shape_array,
+            self._type_map_flat(),
+            self._esdf_flat(),
+            TYPES.OBSTACLE,
+            TYPES.INFLATION,
+            self.strict_collision,
+        )
 
     def fill_boundary_with_obstacles(self) -> None:
         """
@@ -456,12 +841,12 @@ class Grid(BaseMap):
             # First boundary (start index)
             slices_start = [slice(None)] * self.dim
             slices_start[d] = 0
-            self.type_map[tuple(slices_start)] = TYPES.OBSTACLE
+            self._type_map[tuple(slices_start)] = TYPES.OBSTACLE
             
             # Last boundary (end index)
             slices_end = [slice(None)] * self.dim
             slices_end[d] = -1
-            self.type_map[tuple(slices_end)] = TYPES.OBSTACLE
+            self._type_map[tuple(slices_end)] = TYPES.OBSTACLE
 
     def inflate_obstacles(self, radius: float = 1.0) -> None:
         """
@@ -470,9 +855,8 @@ class Grid(BaseMap):
         Args:
             radius: Radius of the inflation.
         """
-        self.update_esdf()
-        mask = (self.esdf <= radius) & (self.type_map.data == TYPES.FREE)
-        self.type_map[mask] = TYPES.INFLATION
+        mask = (self.esdf <= radius) & (self._type_map.data == TYPES.FREE)
+        self._type_map[mask] = TYPES.INFLATION
         self.inflation_radius = radius
 
     def fill_expands(self, expands: Dict[Tuple[int, ...], Node]) -> None:
@@ -483,9 +867,9 @@ class Grid(BaseMap):
             expands: List of expands.
         """
         for expand in expands.keys():
-            if self.type_map[expand] != TYPES.FREE:
+            if self._type_map[expand] != TYPES.FREE:
                 continue
-            self.type_map[expand] = TYPES.EXPAND
+            self._type_map[expand] = TYPES.EXPAND
 
     def update_esdf(self) -> None:
         """
@@ -493,7 +877,7 @@ class Grid(BaseMap):
         - Obstacle grid ESDF = 0
         - Free grid ESDF > 0. The value is the di/stance to the nearest obstacle
         """
-        obstacle_mask = (self.type_map.data == TYPES.OBSTACLE)
+        obstacle_mask = (self._type_map.data == TYPES.OBSTACLE)
         free_mask = ~obstacle_mask
 
         # distance to obstacles
@@ -503,6 +887,16 @@ class Grid(BaseMap):
 
         self._esdf = dist_outside.astype(np.float32)
         self._esdf[obstacle_mask] = -dist_inside[obstacle_mask]
+        self._esdf_lazy_flag_ = False
+
+    def update_connectivity(self) -> None:
+        """Update the free-space connected component map."""
+        type_map = self._type_map.data
+        free_mask = (type_map != TYPES.OBSTACLE) & (type_map != TYPES.INFLATION)
+        connectivity = 1 if self.strict_collision else self.dim
+        structure = ndimage.generate_binary_structure(self.dim, connectivity)
+        ndimage.label(free_mask, structure=structure, output=self._connectivity_map)
+        self._connectivity_lazy_flag_ = False
 
     def path_map_to_world(self, path: List[tuple]) -> List[Tuple[float, ...]]:
         """
@@ -514,7 +908,16 @@ class Grid(BaseMap):
         Returns:
             path: a list of world coordinates
         """
-        return [self.map_to_world(p) for p in path]
+        path = list(path)
+        if not path:
+            return []
+
+        points = np.asarray(path, dtype=np.float64)
+        if points.ndim != 2 or points.shape[1] != self.dim:
+            raise ValueError("Point dimension does not match map dimension.")
+
+        path_world = _grid_path_map_to_world(points, self.bounds, self.resolution)
+        return [tuple(float(x) for x in path_world[i]) for i in range(path_world.shape[0])]
 
     def path_world_to_map(self, path: List[Tuple[float, ...]], discrete: bool = True) -> List[tuple]:
         """
@@ -527,7 +930,20 @@ class Grid(BaseMap):
         Returns:
             path: a list of map coordinates
         """
-        return [self.world_to_map(p, discrete) for p in path]
+        path = list(path)
+        if not path:
+            return []
+
+        points = np.asarray(path, dtype=np.float64)
+        if points.ndim != 2 or points.shape[1] != self.dim:
+            raise ValueError("Point dimension does not match map dimension.")
+
+        if discrete:
+            path_map = _grid_path_world_to_map_int(points, self.bounds, self.resolution, self._shape_array)
+            return [tuple(int(x) for x in path_map[i]) for i in range(path_map.shape[0])]
+        else:
+            path_map = _grid_path_world_to_map_float(points, self.bounds, self.resolution)
+            return [tuple(float(x) for x in path_map[i]) for i in range(path_map.shape[0])]
 
     def point_float_to_int(self, point: Tuple[float, ...]) -> Tuple[int, ...]:
         """
@@ -539,24 +955,29 @@ class Grid(BaseMap):
         Returns:
             point: a point in integer coordinates
         """
+        shape = self.shape
         point_int = []
         for d in range(self.dim):
-            point_int.append(max(0, min(self.shape[d] - 1, int(round(point[d])))))
-        point_int = tuple(point_int)
-        return point_int
+            value = round(point[d])
+            if value < 0:
+                value = 0
+            elif value >= shape[d]:
+                value = shape[d] - 1
+            point_int.append(value)
+        return tuple(point_int)
 
     def _precompute_offsets(self):
         # Generate all possible offsets (-1, 0, +1) in each dimension
-        self._diagonal_offsets = np.array(np.meshgrid(*[[-1, 0, 1]]*self.dim), dtype=self.dtype).T.reshape(-1, self.dim)
+        self._diagonal_offsets_array = np.array(np.meshgrid(*[[-1, 0, 1]]*self.dim), dtype=np.int64).T.reshape(-1, self.dim)
         # Remove the zero offset (current node itself)
-        self._diagonal_offsets = self._diagonal_offsets[np.any(self._diagonal_offsets != 0, axis=1)]
+        self._diagonal_offsets_array = self._diagonal_offsets_array[np.any(self._diagonal_offsets_array != 0, axis=1)]
         # self._diagonal_offsets = [Node((offset.tolist(), dtype=self.dtype)) for offset in self._diagonal_offsets]
-        self._diagonal_offsets = [Node(tuple(offset.tolist())) for offset in self._diagonal_offsets]
+        self._diagonal_offsets = [Node(tuple(offset.tolist())) for offset in self._diagonal_offsets_array]
 
         # Generate only orthogonal offsets (one dimension changes by ±1)
-        self._orthogonal_offsets = np.zeros((2*self.dim, self.dim), dtype=self.dtype)
+        self._orthogonal_offsets_array = np.zeros((2*self.dim, self.dim), dtype=np.int64)
         for d in range(self.dim):
-            self._orthogonal_offsets[2*d, d] = 1
-            self._orthogonal_offsets[2*d+1, d] = -1
+            self._orthogonal_offsets_array[2*d, d] = 1
+            self._orthogonal_offsets_array[2*d+1, d] = -1
         # self._orthogonal_offsets = [Node((offset.tolist(), dtype=self.dtype)) for offset in self._orthogonal_offsets]
-        self._orthogonal_offsets = [Node(tuple(offset.tolist())) for offset in self._orthogonal_offsets]
+        self._orthogonal_offsets = [Node(tuple(offset.tolist())) for offset in self._orthogonal_offsets_array]