diff --git a/inire/router/astar.py b/inire/router/astar.py index 8f580bd..e7cccbd 100644 --- a/inire/router/astar.py +++ b/inire/router/astar.py @@ -229,9 +229,14 @@ class AStarRouter: if max_reach > self.config.min_straight_length: # milestone 1: exactly at max_reach (touching) straight_lengths.add(snap_search_grid(max_reach, snap)) - # milestone 2: 10um before max_reach (space to turn) - if max_reach > self.config.min_straight_length + 10.0: - straight_lengths.add(snap_search_grid(max_reach - 10.0, snap)) + # milestone 2: space to turn before collision + for radius in self.config.bend_radii: + if max_reach > radius + self.config.min_straight_length: + straight_lengths.add(snap_search_grid(max_reach - radius, snap)) + + # milestone 3: small buffer for tight maneuvering + if max_reach > self.config.min_straight_length + 5.0: + straight_lengths.add(snap_search_grid(max_reach - 5.0, snap)) visible_corners = self.visibility_manager.get_visible_corners(cp, max_dist=max_reach) for cx, cy, dist in visible_corners: @@ -251,10 +256,22 @@ class AStarRouter: target_dist = abs(target.x - cp.x) if target_dist <= max_reach and target_dist > self.config.min_straight_length: straight_lengths.add(snap_search_grid(target_dist, snap)) + # Space for turning: target_dist - R and target_dist - 2R + for radius in self.config.bend_radii: + if target_dist > radius + self.config.min_straight_length: + straight_lengths.add(snap_search_grid(target_dist - radius, snap)) + if target_dist > 2 * radius + self.config.min_straight_length: + straight_lengths.add(snap_search_grid(target_dist - 2 * radius, snap)) else: # Vertical target_dist = abs(target.y - cp.y) if target_dist <= max_reach and target_dist > self.config.min_straight_length: straight_lengths.add(snap_search_grid(target_dist, snap)) + # Space for turning: target_dist - R and target_dist - 2R + for radius in self.config.bend_radii: + if target_dist > radius + self.config.min_straight_length: + straight_lengths.add(snap_search_grid(target_dist - radius, snap)) + if target_dist > 2 * radius + self.config.min_straight_length: + straight_lengths.add(snap_search_grid(target_dist - 2 * radius, snap)) # NO standard samples here! Only milestones.