Multi-Agent Collision Simulator (Unicycle Kinematics)

Problem Statement

In autonomous driving simulation, tracking dynamic obstacles and verifying that trajectories remain collision-free are fundamental tasks. You are tasked with implementing a discrete-time 2D multi-agent simulation engine using the standard unicycle kinematic model.

Each agent $i$ is modeled as a circular disc with radius radius_i and state (x, y, theta):

• x, y: 2D Cartesian position in meters.
• theta: Heading angle in radians measured counter-clockwise from the positive x-axis.

At each time step k (from k = 0 to k = steps - 1), agent i receives control input (v, omega):

• v: Forward linear velocity in m/s.
• omega: Angular velocity in rad/s.

Kinematics & Discrete Update

Using Forward Euler integration with step size dt, an agent's state at step k + 1 is updated from step k via:

x[k+1]     = x[k] + v[k] * cos(theta[k]) * dt
y[k+1]     = y[k] + v[k] * sin(theta[k]) * dt
theta[k+1] = theta[k] + omega[k] * dt

Collision Detection

A collision occurs between agent i and agent j (i != j) at any discrete time step if the Euclidean distance between their centers is less than or equal to the sum of their radii:

sqrt((x_i - x_j)^2 + (y_i - y_j)^2) <= radius_i + radius_j

Check for collisions at the initial state (step = 0), and after every update step step = 1, ..., steps.

Implement the function find_first_collision that simulates the system and returns the earliest time step and the pairs of agents that collide. If multiple distinct pairs collide at that same earliest time step, return all such pairs sorted in ascending order by their IDs.

Function Signature

def find_first_collision(
    agents: list[dict],
    controls: list[list[tuple[float, float]]],
    dt: float,
    steps: int
) -> tuple[int, list[tuple[int, int]]] | None:
    """
    Simulates multi-agent unicycle trajectories and returns the first collision.
    
    agents: list of dicts with keys 'id' (int), 'x' (float), 'y' (float), 
            'theta' (float), 'radius' (float).
    controls: controls[i][k] contains (v, omega) for agents[i] at step k.
    dt: simulation time delta in seconds.
    steps: number of discrete time steps to simulate (controls has length == steps).
    
    Returns:
      (first_step, [(id_a, id_b), ...]) where id_a < id_b, sorted lexicographically.
      Returns None if no collision occurs across steps 0 through steps.
    """
    pass

Examples

Example 1

agents = [
    {"id": 0, "x": 0.0, "y": 0.0, "theta": 0.0, "radius": 1.0},
    {"id": 1, "x": 5.0, "y": 0.0, "theta": 3.141592653589793, "radius": 1.0}
]
# Agent 0 moves right at 2 m/s; Agent 1 moves left at 2 m/s. dt = 1.0, steps = 2
controls = [
    [(2.0, 0.0), (2.0, 0.0)],
    [(2.0, 0.0), (2.0, 0.0)]
]
dt = 1.0
steps = 2

# At step 0: dist = 5.0 > 2.0 (no collision)
# At step 1: Agent 0 is at (2, 0), Agent 1 is at (3, 0). dist = 1.0 <= 2.0 (collision)
# Output: (1, [(0, 1)])

Example 2

agents = [
    {"id": 10, "x": 0.0, "y": 0.0, "theta": 0.0, "radius": 0.5},
    {"id": 20, "x": 10.0, "y": 10.0, "theta": 1.57, "radius": 0.5}
]
controls = [
    [(1.0, 0.0)],
    [(1.0, 0.0)]
]
dt = 0.5
steps = 1
# Output: None

Constraints

• 2 0.0
• Radii are strictly positive (radius > 0).
• Coordinates and velocities are finite floats.