| 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] |
|
|
| |
| if (last_i, last_j) == (first_i, first_j): |
| |
| merged.extend(new_path[1:]) |
| else: |
| |
| 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 |
