Temporal (swept) collision & geometry checks#487
Conversation
- Introduced new functions for computing distances and checking intersections, including `compute_point_to_segment_distance` and `check_segment_intersects_aabb`. - Refactored existing collision checks to utilize new distance calculations and streamline logic for moving agents. - Updated agent properties during spawning and movement to maintain accurate previous positions for collision checks. - Improved handling of stop line crossings by integrating moving box checks. - Removed redundant corner calculations in favor of bounding box corner computations. - Ensured static probes maintain consistent previous and current positions to simplify geometry checks.
There was a problem hiding this comment.
Pull request overview
This PR upgrades Drive’s geometry/infraction logic from single-pose (static) tests to swept (temporal) collision and crossing checks by tracking each agent’s previous-step pose and using shared segment-vs-AABB kernels. This aims to prevent tunneling at high speed / large dt and consolidate geometry logic into a smaller set of reusable primitives.
Changes:
- Added per-agent previous pose (
prev_*) and cached bounding radius to support swept checks and faster collision candidate pruning. - Introduced shared geometry kernels (segment-vs-AABB, moving-segment vs moving-box, moving OBB collision) and rewired collision / stop-line / offroad / goal checks to use them.
- Updated IDM probing to use the shared swept kernels (static probes set
prev == cur).
Reviewed changes
Copilot reviewed 3 out of 3 changed files in this pull request and generated 3 comments.
| File | Description |
|---|---|
pufferlib/ocean/drive/datatypes.h |
Extends Agent with prev_* pose fields and cached radius used by swept geometry and quick-check pruning. |
pufferlib/ocean/drive/drive.h |
Adds the swept geometry kernels and routes collision/infractions/goal checks through them; updates spawn/reset/movers to populate prev_* and radius. |
pufferlib/ocean/drive/idm.h |
Updates IDM static probe pose initialization and red-light probe logic to work with swept kernels. |
Comments suppressed due to low confidence (1)
pufferlib/ocean/drive/idm.h:146
idm_make_sample_agentexpandssim_length/sim_widthbut leavesradiuscopied fromego, sosample.radiusno longer matches its box dimensions. Even if it isn't used today, it breaks the new invariant thatradiusreflects current L/W and is easy to keep consistent.
sample.sim_length = ego->sim_length + 2.0f * IDM_BBOX_MARGIN;
sample.sim_width = ego->sim_width + 2.0f * IDM_BBOX_MARGIN;
sample.removed = 0;
sample.sim_valid = 1;
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
… and agent performance
| float sim_length; // Bounding box length | ||
| float sim_width; // Bounding box width | ||
| float sim_height; // Bounding box height | ||
| float radius; // 0.5 * sqrt(L^2 + W^2); refreshed when L/W change |
There was a problem hiding this comment.
not obvious what this corresponds to? The smallest enclosing circle of the vehicle?
| return sqrtf(dx * dx + dy * dy); | ||
| } | ||
|
|
||
| // Distance from point (px, py) to segment (x0, y0)->(x1, y1). |
There was a problem hiding this comment.
comment underdefined; is it the minimum distance between a point and a segment?
There was a problem hiding this comment.
I'm assuming it is intended to be that though
Summary
Replaces the static, single-pose geometry tests with swept (temporal) checks that account for motion between the previous and current sim pose. At high
dtor high speed, a static OBB test can miss a collision/crossing that happened between two states (tunnelling); this PR closes that gap by sweeping each box/segment over its per-step motion.All geometric checks now route through one shared segment-vs-AABB kernel (GIGAFLOW §A.5 style) instead of the old ad-hoc
check_line_intersection+ per-corner loops.What changed
Datatypes (
datatypes.h)Agentgainsprev_x/prev_y/prev_cos_heading/prev_sin_heading(pose at start of the step) and a cachedradius(0.5*sqrt(L²+W²), refreshed only when L/W change).Core geometry (
drive.h)compute_point_to_segment_distance,compute_bounding_box_corners,check_segment_intersects_aabb(Liang–Barsky slab clip),check_segment_crosses_moving_box(swept-quad test),check_moving_obb_collision.project_point_to_local/project_vector_to_local(explicit center+heading), with thinproject_*_to_ego_framewrappers for the common ego case.collision_check: pair quick-check now uses cached radii + per-step displacement (radius_a + radius_b + margin + disp_a + disp_b) instead of a flat 15 m cutoff, then runs the moving-OBB test.prev→cur, so a largedtcan't jump the goal disc).check_line_intersection,compute_agent_corners,check_agent_corners_cross_stop_line,check_spawn_red_light_violation, and theCOLLISION_QUICK_CHECK_DISTmacro.prev_*pose is captured inmove_dynamics,move_expert, spawn,set_start_position, andc_reset.IDM (
idm.h)idm_make_sample_agent,idm_update_sample_agent_pose) setprev == curso the swept kernels degenerate cleanly to static box tests.Notes / follow-ups
prev == cur(a static box is just a zero-length sweep), so spawn/IDM share the same kernel as moving agents.move_idmdoes not yet refreshprev_*per step — IDM agents' swept span can grow stale. Worth addressing as a follow-up (centralizeprev_*capture at thec_stepdispatch layer so no mover can forget it).Testing
build_ext), no new compiler warnings.🤖 Generated with Claude Code