File size: 3,170 Bytes
e516f1f beeea66 e516f1f | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 | from typing import Dict, Any, List, Tuple
def merge_paths(old_path: List[Tuple[int, int, int]], new_path: List[Tuple[int, int, int]]) -> List[Tuple[int, int, int]]:
"""
Merge two single-robot paths with global timesteps.
Assumes both paths already use global timesteps (offset by window's current_T).
Args:
old_path: Existing global path [(i, j, t), ...]
new_path: New global path [(i, j, t), ...]
Returns:
Merged path [(i, j, t), ...] with no position duplication
"""
if not old_path:
return new_path.copy()
if not new_path:
return old_path.copy()
merged = old_path.copy()
last_i, last_j, last_t = merged[-1]
first_i, first_j, first_t = new_path[0]
# Check if first position of new path duplicates last position of old path
if (last_i, last_j) == (first_i, first_j):
# Skip the duplicate position
merged.extend(new_path[1:])
else:
# No duplicate, just concatenate
merged.extend(new_path)
return merged
def clip_path_at_goal(coords: List[Tuple[int, int, int]], goal: Tuple[int, int]) -> List[Tuple[int, int, int]]:
"""
Trim the trailing steps where a single-robot path is already parked at
goal, keeping only the first arrival. Pure/output-only: does not touch
any solver or windowing state — callers decide whether/where to use it.
Args:
coords: Single robot's path [(i, j, t), ...], sorted by t
goal: Robot's goal position (i, j)
Returns:
Path truncated right after the first timestep the robot reaches
goal and never leaves again. Unchanged if the robot never parks
at goal for the remainder of the path.
"""
cut = len(coords)
for idx in range(len(coords) - 1, -1, -1):
i, j, _ = coords[idx]
if (i, j) != goal:
break
cut = idx
return coords[:cut + 1]
def decode_position(idx: int, problem) -> Tuple[int, int, int, int]:
"""
Decode variable index to position, time, and robot number.
Args:
idx: Variable index
problem: Problem instance
Returns:
Tuple of (i, j, t, robot_num) coordinates
"""
if problem.get_format_type() == "graph":
nodes_per_robot = len(problem.graph.nodes) * problem.T
robot_num = idx // nodes_per_robot
reduced_idx = idx % nodes_per_robot
t = reduced_idx // len(problem.graph.nodes)
graph_idx = reduced_idx % len(problem.graph.nodes)
pos = problem.graph.get_node_position(graph_idx)
return int(pos[0]), int(pos[1]), t, robot_num
M = problem.grid.M
N = problem.grid.N
T = problem.T
robot_num = idx // (M * N * T)
reduced_idx = idx % (M * N * T)
t = reduced_idx // (M * N)
pos = reduced_idx % (M * N)
i = pos // N
j = pos % N
return i, j, t, robot_num |