jan/inire
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

fix straight endpoint collisions

Browse source
This commit is contained in:
Jan Petykiewicz 2026-03-18 00:06:41 -07:00
commit 064aed31a6
3 changed files with 46 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

2
examples/07_large_scale_routing.py
View file

@ -27,7 +27,7 @@ def main() -> None:
danger_map = DangerMap(bounds=bounds)
danger_map.precompute(obstacles)
evaluator = CostEvaluator(engine, danger_map, greedy_h_weight=2.0, unit_length_cost=0.1, bend_penalty=100.0, sbend_penalty=200.0, congestion_penalty=20.0)
evaluator = CostEvaluator(engine, danger_map, greedy_h_weight=2.0, unit_length_cost=0.1, bend_penalty=100.0, sbend_penalty=400.0, congestion_penalty=20.0)
router = AStarRouter(evaluator, node_limit=2000000, snap_size=5.0, bend_radii=[50.0], sbend_radii=[50.0], use_analytical_sbends=False)
pf = PathFinder(router, evaluator, max_iterations=50, base_congestion_penalty=20.0, congestion_multiplier=1.2)

2
inire/geometry/collision.py
View file

@ -326,7 +326,7 @@ class CollisionEngine:
t_min = max(tx_min, ty_min)
t_max = min(tx_max, ty_max)
if t_max < 0 or t_min > t_max or t_min > 1.0:
if t_max <= 1e-9 or t_min > t_max or t_min >= 1.0 - 1e-9:
continue
# If rectangle, slab is exact

40
inire/router/astar.py
View file

@ -256,22 +256,38 @@ 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))
# Space for turning: target_dist - R and target_dist - 2R
for radius in self.config.bend_radii:
l1 = target_dist - radius
if l1 > self.config.min_straight_length:
s_l1 = snap_search_grid(l1, snap)
if s_l1 <= max_reach and s_l1 > 0.1:
straight_lengths.add(s_l1)
l2 = target_dist - 2 * radius
if l2 > self.config.min_straight_length:
s_l2 = snap_search_grid(l2, snap)
if s_l2 <= max_reach and s_l2 > 0.1:
straight_lengths.add(s_l2)
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))
# Space for turning: target_dist - R and target_dist - 2R
for radius in self.config.bend_radii:
l1 = target_dist - radius
if l1 > self.config.min_straight_length:
s_l1 = snap_search_grid(l1, snap)
if s_l1 <= max_reach and s_l1 > 0.1:
straight_lengths.add(s_l1)
l2 = target_dist - 2 * radius
if l2 > self.config.min_straight_length:
s_l2 = snap_search_grid(l2, snap)
if s_l2 <= max_reach and s_l2 > 0.1:
straight_lengths.add(s_l2)
# NO standard samples here! Only milestones.