diff --git a/AGENTIC_PR b/AGENTIC_PR new file mode 100644 index 0000000000..e5cc143622 --- /dev/null +++ b/AGENTIC_PR @@ -0,0 +1,4 @@ +Devant la ligne, un instant de repos, +Le feu veille au passage des autos. +Quand vient le vert, la route se dessine, +Et chaque trajet poursuit sa ligne. diff --git a/pufferlib/ocean/drive/drive.h b/pufferlib/ocean/drive/drive.h index 7599963e0c..8270581945 100644 --- a/pufferlib/ocean/drive/drive.h +++ b/pufferlib/ocean/drive/drive.h @@ -1774,9 +1774,74 @@ static bool check_corner_boxes_overlap(float corners_a[4][2], float corners_b[4] return true; } +static bool check_front_bumper_crossing(Agent *agent, TrafficControlElement *tc) { + float line_dx = tc->stop_line[3] - tc->stop_line[0]; + float line_dy = tc->stop_line[4] - tc->stop_line[1]; + float line_len = sqrtf(line_dx * line_dx + line_dy * line_dy); + if (line_len <= 0.0f) { + return false; + } + float mid_x = (tc->stop_line[0] + tc->stop_line[3]) * 0.5f; + float mid_y = (tc->stop_line[1] + tc->stop_line[4]) * 0.5f; + float line_ux = line_dx / line_len, line_uy = line_dy / line_len; + // Normal oriented along travel direction, so s < 0 is before the line. + float normal_x = -line_uy, normal_y = line_ux; + if (normal_x * cosf(tc->heading) + normal_y * sinf(tc->heading) < 0.0f) { + normal_x = -normal_x; + normal_y = -normal_y; + } + + // Use physical dimensions, not the observation-only perceived size margin. + float prev_corners[4][2], cur_corners[4][2]; + compute_bounding_box_corners( + agent->prev_x, + agent->prev_y, + agent->prev_cos_heading, + agent->prev_sin_heading, + agent->sim_length / 2.0f, + agent->sim_width / 2.0f, + prev_corners); + compute_bounding_box_corners( + agent->sim_x, + agent->sim_y, + agent->cos_heading, + agent->sin_heading, + agent->sim_length / 2.0f, + agent->sim_width / 2.0f, + cur_corners); + + // Corners 0 and 1 form the front bumper. Both must start before the line: + // the trailing corner, center, and rear must not report the same entry again. + float s_prev[2], s_cur[2]; + for (int corner_idx = 0; corner_idx < 2; corner_idx++) { + s_prev[corner_idx] + = (prev_corners[corner_idx][0] - mid_x) * normal_x + (prev_corners[corner_idx][1] - mid_y) * normal_y; + s_cur[corner_idx] + = (cur_corners[corner_idx][0] - mid_x) * normal_x + (cur_corners[corner_idx][1] - mid_y) * normal_y; + } + if (fmaxf(s_prev[0], s_prev[1]) >= 0.0f) { + return false; + } + for (int corner_idx = 0; corner_idx < 2; corner_idx++) { + if (s_cur[corner_idx] < 0.0f) { + continue; + } + float crossing_frac = s_prev[corner_idx] / (s_prev[corner_idx] - s_cur[corner_idx]); + float cross_x + = prev_corners[corner_idx][0] + crossing_frac * (cur_corners[corner_idx][0] - prev_corners[corner_idx][0]); + float cross_y + = prev_corners[corner_idx][1] + crossing_frac * (cur_corners[corner_idx][1] - prev_corners[corner_idx][1]); + float lateral = (cross_x - mid_x) * line_ux + (cross_y - mid_y) * line_uy; + if (fabsf(lateral) <= 0.5f * line_len + RED_LIGHT_LATERAL_EXTENSION_M) { + return true; + } + } + return false; +} + static bool check_red_light_violation(Drive *env, int agent_idx) { Agent *agent = &env->agents[agent_idx]; - // Vehicle center crosses on red; the laterally extended line cannot be driven + // Front bumper crosses on red; the laterally extended line cannot be driven // around, and the heading gate exempts exiting/opposing traffic. for (int i = 0; i < env->num_traffic_elements; i++) { TrafficControlElement *tc = &env->traffic_elements[i]; @@ -1804,29 +1869,7 @@ static bool check_red_light_violation(Drive *env, int agent_idx) { continue; } - float line_dx = tc->stop_line[3] - tc->stop_line[0]; - float line_dy = tc->stop_line[4] - tc->stop_line[1]; - float line_len = sqrtf(line_dx * line_dx + line_dy * line_dy); - if (line_len <= 0.0f) { - continue; - } - float line_ux = line_dx / line_len, line_uy = line_dy / line_len; - // normal oriented along travel direction, so s < 0 is before the line - float normal_x = -line_uy, normal_y = line_ux; - if (normal_x * cosf(tc->heading) + normal_y * sinf(tc->heading) < 0.0f) { - normal_x = -normal_x; - normal_y = -normal_y; - } - float s_prev = (agent->prev_x - mid_x) * normal_x + (agent->prev_y - mid_y) * normal_y; - float s_cur = (agent->sim_x - mid_x) * normal_x + (agent->sim_y - mid_y) * normal_y; - if (!(s_prev < 0.0f && s_cur >= 0.0f)) { - continue; - } - float crossing_frac = s_prev / (s_prev - s_cur); - float cross_x = agent->prev_x + crossing_frac * (agent->sim_x - agent->prev_x); - float cross_y = agent->prev_y + crossing_frac * (agent->sim_y - agent->prev_y); - float lateral = (cross_x - mid_x) * line_ux + (cross_y - mid_y) * line_uy; - if (fabsf(lateral) <= 0.5f * line_len + RED_LIGHT_LATERAL_EXTENSION_M) { + if (check_front_bumper_crossing(agent, tc)) { return true; } } diff --git a/tests/drive/test_drive_red_light.c b/tests/drive/test_drive_red_light.c index 7791179fd3..85b7830ed3 100644 --- a/tests/drive/test_drive_red_light.c +++ b/tests/drive/test_drive_red_light.c @@ -34,7 +34,7 @@ static Drive drive_test_light_env(TrafficControlElement *tc) { return env; } -// Violation = vehicle center crossing the line; stationary/backward motion never flags. +// Agent length 4: the front bumper is 2 m ahead of the center. static Agent drive_test_driving_agent(float prev_x, float cur_x, float y) { Agent agent = drive_test_agent(prev_x, y, 0.0f); agent.sim_x = cur_x; @@ -47,20 +47,20 @@ static int test_red_crossing_flags(void) { TrafficControlElement tc = drive_test_light(); Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_RED); - Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 0.0f); + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f); env.agents = &agent; EXPECT_TRUE(check_red_light_violation(&env, 0)); return 0; } -static int test_straddle_then_proceed_on_red_flags(void) { +static int test_front_already_across_no_flag(void) { TrafficControlElement tc = drive_test_light(); Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_RED); - // nose already over the line when the light turned red; proceeding completes entry + // The entry event has already happened; crossing with the center must not fire again. Agent agent = drive_test_driving_agent(-1.5f, 0.5f, 0.0f); env.agents = &agent; - EXPECT_TRUE(check_red_light_violation(&env, 0)); + EXPECT_TRUE(!check_red_light_violation(&env, 0)); return 0; } @@ -68,12 +68,12 @@ static int test_green_crossing_then_red_inside_no_flag(void) { TrafficControlElement tc = drive_test_light(); Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_GREEN); - Agent agent = drive_test_driving_agent(1.5f, 2.5f, 0.0f); + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f); env.agents = &agent; EXPECT_TRUE(!check_red_light_violation(&env, 0)); - // light turns red once the agent is fully inside the junction + // The front entered on green; red must not penalize the later center crossing. set_light(TRAFFIC_CONTROL_STATE_RED); - agent = drive_test_driving_agent(4.0f, 5.0f, 0.0f); + agent = drive_test_driving_agent(-1.5f, 0.5f, 0.0f); EXPECT_TRUE(!check_red_light_violation(&env, 0)); return 0; } @@ -94,8 +94,12 @@ static int test_crossing_fires_once(void) { TrafficControlElement tc = drive_test_light(); Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_RED); - Agent agent = drive_test_driving_agent(2.5f, 3.5f, 0.0f); + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f); env.agents = &agent; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + agent = drive_test_driving_agent(-1.5f, 0.5f, 0.0f); + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + agent = drive_test_driving_agent(0.5f, 2.5f, 0.0f); EXPECT_TRUE(!check_red_light_violation(&env, 0)); return 0; } @@ -105,7 +109,7 @@ static int test_opposing_lane_bypass_flags(void) { Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_RED); // crossing 5 m beside the painted line (opposing lane) is still a violation - Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 5.0f); + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 5.0f); env.agents = &agent; EXPECT_TRUE(check_red_light_violation(&env, 0)); return 0; @@ -115,7 +119,7 @@ static int test_crossing_beyond_virtual_extension_no_flag(void) { TrafficControlElement tc = drive_test_light(); Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_RED); - Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 18.0f); + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 18.1f); env.agents = &agent; EXPECT_TRUE(!check_red_light_violation(&env, 0)); return 0; @@ -138,8 +142,9 @@ static int test_diagonal_entry_flags(void) { Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_RED); // 1.2 rad off the light heading is still entering (< pi/2 gate) - Agent agent = drive_test_agent(-0.5f, 0.0f, 1.2f); - agent.sim_x = 0.5f; + // Only the leading corner crosses: even the bumper midpoint stays before x = 0. + Agent agent = drive_test_agent(-2.0f, 0.0f, 1.2f); + agent.sim_x = -1.5f; env.agents = &agent; EXPECT_TRUE(check_red_light_violation(&env, 0)); return 0; @@ -161,7 +166,7 @@ static int test_different_z_level_no_flag(void) { TrafficControlElement tc = drive_test_light(); Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_RED); - Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 0.0f); + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f); agent.sim_z = 6.0f; env.agents = &agent; EXPECT_TRUE(!check_red_light_violation(&env, 0)); @@ -172,7 +177,7 @@ static int test_no_lane_assignment_still_flags(void) { TrafficControlElement tc = drive_test_light(); Drive env = drive_test_light_env(&tc); set_light(TRAFFIC_CONTROL_STATE_RED); - Agent agent = drive_test_driving_agent(-0.5f, 0.5f, 0.0f); + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f); agent.current_lane_idx = -1; agent.previous_lane_idx = -1; env.agents = &agent; @@ -180,10 +185,126 @@ static int test_no_lane_assignment_still_flags(void) { return 0; } +static int test_approach_and_reverse_no_flag(void) { + TrafficControlElement tc = drive_test_light(); + Drive env = drive_test_light_env(&tc); + set_light(TRAFFIC_CONTROL_STATE_RED); + Agent agent = drive_test_driving_agent(-3.0f, -2.1f, 0.0f); + env.agents = &agent; + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + agent = drive_test_driving_agent(-2.1f, -2.1f, 0.0f); + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + agent = drive_test_driving_agent(-1.5f, -2.5f, 0.0f); + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + return 0; +} + +static int test_front_touch_and_large_step_flag(void) { + TrafficControlElement tc = drive_test_light(); + Drive env = drive_test_light_env(&tc); + set_light(TRAFFIC_CONTROL_STATE_RED); + Agent agent = drive_test_driving_agent(-2.5f, -2.0f, 0.0f); + env.agents = &agent; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + agent = drive_test_driving_agent(-10.0f, 10.0f, 0.0f); + EXPECT_TRUE(check_red_light_violation(&env, 0)); + return 0; +} + +static int test_heading_change_uses_previous_pose(void) { + TrafficControlElement tc = drive_test_light(); + Drive env = drive_test_light_env(&tc); + set_light(TRAFFIC_CONTROL_STATE_RED); + Agent agent = drive_test_agent(-1.8f, 0.0f, 1.2f); + agent.sim_heading = 0.4f; + agent.cos_heading = cosf(agent.sim_heading); + agent.sin_heading = sinf(agent.sim_heading); + env.agents = &agent; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + return 0; +} + +static int test_trailing_front_corner_does_not_retrigger(void) { + TrafficControlElement tc = drive_test_light(); + Drive env = drive_test_light_env(&tc); + set_light(TRAFFIC_CONTROL_STATE_RED); + Agent agent = drive_test_agent(-2.0f, 0.0f, 1.2f); + agent.sim_x = -1.5f; + env.agents = &agent; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + copy_pose_to_prev(&agent); + agent.sim_x = 1.0f; + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + return 0; +} + +static int test_front_edge_overlaps_lateral_limit(void) { + TrafficControlElement tc = drive_test_light(); + Drive env = drive_test_light_env(&tc); + set_light(TRAFFIC_CONTROL_STATE_RED); + // Center is outside the 17 m half-span, but the right front corner is inside. + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 17.5f); + env.agents = &agent; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + return 0; +} + +static int test_translated_rotated_line_and_reversed_endpoints(void) { + TrafficControlElement tc = drive_test_light(); + tc.stop_line[0] = 8.0f; + tc.stop_line[1] = 20.0f; + tc.stop_line[3] = 12.0f; + tc.stop_line[4] = 20.0f; + tc.heading = (float) M_PI / 2.0f; + Drive env = drive_test_light_env(&tc); + set_light(TRAFFIC_CONTROL_STATE_RED); + Agent agent = drive_test_agent(10.0f, 17.5f, tc.heading); + agent.sim_y = 18.5f; + env.agents = &agent; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + tc.stop_line[0] = 12.0f; + tc.stop_line[3] = 8.0f; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + return 0; +} + +static int test_physical_length_and_perceived_margin(void) { + TrafficControlElement tc = drive_test_light(); + Drive env = drive_test_light_env(&tc); + set_light(TRAFFIC_CONTROL_STATE_RED); + Agent agent = drive_test_driving_agent(-3.5f, -2.5f, 0.0f); + env.agents = &agent; + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + env.eval_mode = 1; + env.control_mode = CONTROL_MODE_SDC_ONLY; + env.eval_perceived_size_margin_m = 2.0f; + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + agent.sim_length = 6.0f; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + env.eval_perceived_size_margin_m = 0.0f; + EXPECT_TRUE(check_red_light_violation(&env, 0)); + return 0; +} + +static int test_non_red_or_missing_state_no_flag(void) { + TrafficControlElement tc = drive_test_light(); + Drive env = drive_test_light_env(&tc); + Agent agent = drive_test_driving_agent(-2.5f, -1.5f, 0.0f); + env.agents = &agent; + set_light(TRAFFIC_CONTROL_STATE_GREEN); + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + set_light(TRAFFIC_CONTROL_STATE_YELLOW); + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + set_light(TRAFFIC_CONTROL_STATE_RED); + env.timestep = tc.state_size; + EXPECT_TRUE(!check_red_light_violation(&env, 0)); + return 0; +} + int main(void) { int failures = 0; RUN_TEST(test_red_crossing_flags); - RUN_TEST(test_straddle_then_proceed_on_red_flags); + RUN_TEST(test_front_already_across_no_flag); RUN_TEST(test_green_crossing_then_red_inside_no_flag); RUN_TEST(test_stationary_straddle_no_flag); RUN_TEST(test_crossing_fires_once); @@ -194,5 +315,13 @@ int main(void) { RUN_TEST(test_perpendicular_crossing_no_flag); RUN_TEST(test_different_z_level_no_flag); RUN_TEST(test_no_lane_assignment_still_flags); + RUN_TEST(test_approach_and_reverse_no_flag); + RUN_TEST(test_front_touch_and_large_step_flag); + RUN_TEST(test_heading_change_uses_previous_pose); + RUN_TEST(test_trailing_front_corner_does_not_retrigger); + RUN_TEST(test_front_edge_overlaps_lateral_limit); + RUN_TEST(test_translated_rotated_line_and_reversed_endpoints); + RUN_TEST(test_physical_length_and_perceived_margin); + RUN_TEST(test_non_red_or_missing_state_no_flag); return test_summary(failures); }