From 2216d5bedb79d1abb80cba88d186500897c1ecdf Mon Sep 17 00:00:00 2001 From: mpragnay Date: Thu, 9 Apr 2026 13:29:56 -0400 Subject: [PATCH 1/4] Added collision types, at-fault collision percentage --- pufferlib/ocean/drive/binding.c | 6 +++ pufferlib/ocean/drive/datatypes.h | 22 +++++++++ pufferlib/ocean/drive/drive.h | 78 +++++++++++++++++++++++++++---- 3 files changed, 97 insertions(+), 9 deletions(-) diff --git a/pufferlib/ocean/drive/binding.c b/pufferlib/ocean/drive/binding.c index 75189f8ddf..f4c0462964 100644 --- a/pufferlib/ocean/drive/binding.c +++ b/pufferlib/ocean/drive/binding.c @@ -468,6 +468,12 @@ static int my_log(PyObject *dict, Log *log) { assign_to_dict(dict, "max_observation_distance", log->max_observation_distance); assign_to_dict(dict, "observation_coverage", log->observation_coverage); assign_to_dict(dict, "partner_obs_coverage", log->partner_obs_coverage); + float at_fault_collision_pct = + (log->collisions_per_agent > 0) ? log->at_fault_collision_count / log->collisions_per_agent : 0.0f; + assign_to_dict(dict, "at_fault_collision_pct", at_fault_collision_pct); + float not_at_fault_collision_pct = + (log->collisions_per_agent > 0) ? log->not_at_fault_collision_count / log->collisions_per_agent : 0.0f; + assign_to_dict(dict, "not_at_fault_collision_pct", not_at_fault_collision_pct); // assign_to_dict(dict, "avg_displacement_error", log->avg_displacement_error); return 0; } diff --git a/pufferlib/ocean/drive/datatypes.h b/pufferlib/ocean/drive/datatypes.h index 644571ef43..a5e5fba6e1 100644 --- a/pufferlib/ocean/drive/datatypes.h +++ b/pufferlib/ocean/drive/datatypes.h @@ -139,6 +139,27 @@ int unnormalize_traffic_light_state(int norm_state) { } } +/** + * @brief Categorizes the type of collision an agent is involved in. + * + * - STATIONARY_PARTNER_COLLISION: ego hit a stationary partner agent + * + * - STATIONARY_EGO_COLLISION: ego was stationary when hit by another agent + * + * - ACTIVE_FRONT_COLLISION: ego collided with something ahead of it + * + * - ACTIVE_REAR_COLLISION: ego was hit from behind while moving + * + * - ACTIVE_LATERAL_COLLISION: ego was involved in a side collision while moving + */ +typedef enum { + STATIONARY_AGENT_COLLISION, + STATIONARY_EGO_COLLISION, + ACTIVE_FRONT_COLLISION, + ACTIVE_REAR_COLLISION, + ACTIVE_LATERAL_COLLISION, +} CollisionType; + struct Agent { int id; int type; @@ -195,6 +216,7 @@ struct Agent { float goals_attempted_this_episode; // goals reached + last goal(if this segment can be judged as an attempt) int current_goal_reached; int collided_before_goal; + CollisionType current_collision_type; float init_goal_x; float init_goal_y; float init_goal_z; diff --git a/pufferlib/ocean/drive/drive.h b/pufferlib/ocean/drive/drive.h index 41f4af1a0c..b96bae4060 100644 --- a/pufferlib/ocean/drive/drive.h +++ b/pufferlib/ocean/drive/drive.h @@ -144,10 +144,12 @@ // Offsets #define COLLISION_RANGE 5 #define Z_RANGE 3 -#define Z_BUFFER 4.0f // 4.0m buffer for different z level checking -#define SPEED_LIMIT 20.0f // Hardcoded speed limit value -#define COMFORT_JERK_THRESHOLD 5.0f // For JERK model comfort -#define COMFORT_ACCEL_THRESHOLD 3.0f // For JERK and CLASSIC model comfort +#define Z_BUFFER 4.0f // 4.0m buffer for different z level checking +#define SPEED_LIMIT 20.0f // Hardcoded speed limit value +#define COMFORT_JERK_THRESHOLD 5.0f // For JERK model comfort +#define COMFORT_ACCEL_THRESHOLD 3.0f // For JERK and CLASSIC model comfort +#define STATIONARY_SPEED_THRESHOLD 0.05f // Speed below which we consider the agent stationary +#define HEAD_ON_COLLISION_ANGLE_THRESHOLD 30.0f // Degrees within which a collision is considered head-on // Metrics Heuristics #define MIN_GOAL_SEGMENT_TIME_TO_ANALYZE_AGENT 1.0f @@ -241,9 +243,11 @@ struct Log { float total_distance_travelled; float total_infractions; float avg_speed_per_agent; - float max_observation_distance; // average max observation distance - float observation_coverage; // percentage of entities in obs window seen on average - float partner_obs_coverage; // % of partners within radius that fit in the obs slots + float max_observation_distance; // average max observation distance + float observation_coverage; // percentage of entities in obs window seen on average + float partner_obs_coverage; // % of partners within radius that fit in the obs slots + float at_fault_collision_count; // count of at-fault collisions (ratio computed in my_log) + float not_at_fault_collision_count; // count of not at-fault collisions (ratio computed in my_log) }; typedef struct GridMapEntity GridMapEntity; @@ -1504,6 +1508,52 @@ int collision_check(Drive *env, int agent_idx) { return car_collided_with_index; } +float dot_product(float v1_x, float v1_y, float v2_x, float v2_y) { return v1_x * v2_x + v1_y * v2_y; } + +float get_partner_relative_angle(Drive *env, int ego_agent_idx, int partner_agent_idx) { + Agent *ego = &env->agents[ego_agent_idx]; + Agent *partner = &env->agents[partner_agent_idx]; + + float dx = partner->sim_x - ego->sim_x; + float dy = partner->sim_y - ego->sim_y; + float norm = sqrtf(dx * dx + dy * dy); + + float ego_x = cosf(ego->sim_heading); + float ego_y = sinf(ego->sim_heading); + + float relative_angle = acosf(dot_product(dx, dy, ego_x, ego_y) / norm); + + return relative_angle; +} + +void classify_collision_type(Drive *env, int ego_agent_idx, int collided_with_idx) { + Agent *ego = &env->agents[ego_agent_idx]; + Agent *collided_entity = &env->agents[collided_with_idx]; + + float ego_speed_magnitude = sqrtf(ego->sim_vx * ego->sim_vx + ego->sim_vy * ego->sim_vy); + float partner_speed_magnitude = + sqrtf(collided_entity->sim_vx * collided_entity->sim_vx + collided_entity->sim_vy * collided_entity->sim_vy); + + // Stationary cases + if (ego_speed_magnitude < STATIONARY_SPEED_THRESHOLD) { + ego->current_collision_type = STATIONARY_EGO_COLLISION; + return; + } else if (partner_speed_magnitude < STATIONARY_SPEED_THRESHOLD) { + ego->current_collision_type = STATIONARY_AGENT_COLLISION; + return; + } + + // Both agents are moving + float relative_angle = get_partner_relative_angle(env, ego_agent_idx, collided_with_idx); + if (relative_angle < HEAD_ON_COLLISION_ANGLE_THRESHOLD * (M_PI / 180.0f)) { + ego->current_collision_type = ACTIVE_FRONT_COLLISION; + } else if (relative_angle > (180.0f - HEAD_ON_COLLISION_ANGLE_THRESHOLD) * (M_PI / 180.0f)) { + ego->current_collision_type = ACTIVE_REAR_COLLISION; + } else { + ego->current_collision_type = ACTIVE_LATERAL_COLLISION; + } +} + bool check_line_intersection(float p1[2], float p2[2], float q1[2], float q2[2]) { if (fmax(p1[0], p2[0]) < fmin(q1[0], q2[0]) || fmin(p1[0], p2[0]) > fmax(q1[0], q2[0]) || fmax(p1[1], p2[1]) < fmin(q1[1], q2[1]) || fmin(p1[1], p2[1]) > fmax(q1[1], q2[1])) @@ -2588,10 +2638,20 @@ void compute_agent_metrics(Drive *env, int agent_idx) { // Check for vehicle collisions int car_collided_with_index = collision_check(env, agent_idx); - if (car_collided_with_index != -1) + if (car_collided_with_index != -1) { collided = VEHICLE_COLLISION; + agent->collision_state = collided; - agent->collision_state = collided; + classify_collision_type(env, agent_idx, car_collided_with_index); + + // Determine at-fault + CollisionType ct = env->agents[agent_idx].current_collision_type; + int at_fault = + (ct == STATIONARY_AGENT_COLLISION || ct == ACTIVE_FRONT_COLLISION || ct == ACTIVE_LATERAL_COLLISION); + int not_fault_ct = (ct == ACTIVE_REAR_COLLISION || ct == STATIONARY_EGO_COLLISION); + env->log.at_fault_collision_count += at_fault; + env->log.not_at_fault_collision_count += not_fault_ct; + } if (collided == VEHICLE_COLLISION) { if (env->collision_behavior == STOP_AGENT && !agent->stopped) { From 5a2a508c17ed5fd76fe27389e32de2763acb5467 Mon Sep 17 00:00:00 2001 From: mpragnay Date: Fri, 10 Apr 2026 10:47:48 -0400 Subject: [PATCH 2/4] Minor offroad check fix --- pufferlib/ocean/drive/drive.h | 1 + 1 file changed, 1 insertion(+) diff --git a/pufferlib/ocean/drive/drive.h b/pufferlib/ocean/drive/drive.h index b96bae4060..2e81d83f81 100644 --- a/pufferlib/ocean/drive/drive.h +++ b/pufferlib/ocean/drive/drive.h @@ -2666,6 +2666,7 @@ void compute_agent_metrics(Drive *env, int agent_idx) { } } if (collided == OFFROAD) { + agent->collision_state = OFFROAD; agent->metrics_array[OFFROAD_IDX] = 1.0f; if (env->offroad_behavior == STOP_AGENT && !agent->stopped) { agent->stopped = 1; From ac35676b5cdff1182933a3d7d81c1a15787f63f4 Mon Sep 17 00:00:00 2001 From: mpragnay Date: Sun, 12 Apr 2026 09:25:51 -0400 Subject: [PATCH 3/4] minor fixes --- pufferlib/ocean/drive/drive.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/pufferlib/ocean/drive/drive.h b/pufferlib/ocean/drive/drive.h index 2e81d83f81..58854af6ee 100644 --- a/pufferlib/ocean/drive/drive.h +++ b/pufferlib/ocean/drive/drive.h @@ -70,6 +70,7 @@ #define LANE_DISTANCE_NORMALIZATION 4.0f #define LANE_SWITCH_THRESHOLD 0.05f // Hysteresis: new lane must be 5% better to switch #define LANE_ALIGN_COS_THRESHOLD 0.5f +#define ZERO_THRESHOLD 1e-6f // Minimum distance to goal position #define MIN_DISTANCE_TO_GOAL 2.0f @@ -1518,6 +1519,10 @@ float get_partner_relative_angle(Drive *env, int ego_agent_idx, int partner_agen float dy = partner->sim_y - ego->sim_y; float norm = sqrtf(dx * dx + dy * dy); + if (norm < ZERO_THRESHOLD) { + return 0.0f; // Agents are at the same position, treat as head-on + } + float ego_x = cosf(ego->sim_heading); float ego_y = sinf(ego->sim_heading); From 1b9089ec1c6028f41ac6bef8b95efbe91a2c7dcb Mon Sep 17 00:00:00 2001 From: mpragnay Date: Sun, 12 Apr 2026 09:29:32 -0400 Subject: [PATCH 4/4] minor fixes --- pufferlib/ocean/drive/datatypes.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pufferlib/ocean/drive/datatypes.h b/pufferlib/ocean/drive/datatypes.h index a5e5fba6e1..b65eebdcde 100644 --- a/pufferlib/ocean/drive/datatypes.h +++ b/pufferlib/ocean/drive/datatypes.h @@ -142,7 +142,7 @@ int unnormalize_traffic_light_state(int norm_state) { /** * @brief Categorizes the type of collision an agent is involved in. * - * - STATIONARY_PARTNER_COLLISION: ego hit a stationary partner agent + * - STATIONARY_AGENT_COLLISION: ego hit a stationary partner agent * * - STATIONARY_EGO_COLLISION: ego was stationary when hit by another agent *