From a2ad9e490a5e145c3ec01761cd112e0ea2be6011 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 9 Mar 2025 15:04:13 +0400 Subject: [PATCH] Refactor collision detection to support persistent contacts across frames --- game/physics/debug.odin | 2 +- game/physics/scene.odin | 31 +++++- game/physics/simulation.odin | 183 +++++++++++++++++++---------------- 3 files changed, 129 insertions(+), 87 deletions(-) diff --git a/game/physics/debug.odin b/game/physics/debug.odin index d640b92..a6b0405 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -109,7 +109,7 @@ draw_debug_scene :: proc(scene: ^Scene) { } if false { - for &contact, contact_idx in sim_state.contact_pairs { + for &contact, contact_idx in sim_state.contact_container.contacts { points_a := contact.manifold.points_a points_b := contact.manifold.points_b points_a_slice, points_b_slice := diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 9fb3dfd..28bb396 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -13,6 +13,32 @@ AABB :: struct { extent: Vec3, } +Contact_Pair :: [2]i32 + +make_contact_pair :: proc(body_a: i32, body_b: i32) -> Contact_Pair { + return {min(body_a, body_b), max(body_a, body_b)} +} + +Contact_Container :: struct { + // body index pair to contact index + lookup: map[Contact_Pair]i32, + contacts: #soa[dynamic]Contact, +} + +contact_container_copy :: proc(dst: ^Contact_Container, src: Contact_Container) { + clear(&dst.lookup) + reserve(&dst.lookup, cap(src.lookup)) + resize_soa(&dst.contacts, len(src.contacts)) + + for k, v in src.lookup { + dst.lookup[k] = v + } + + for i in 0 ..< len(src.contacts) { + dst.contacts[i] = src.contacts[i] + } +} + Sim_State :: struct { bodies: #soa[dynamic]Body, suspension_constraints: #soa[dynamic]Suspension_Constraint, @@ -26,7 +52,7 @@ Sim_State :: struct { first_free_suspension_constraint_plus_one: i32, // Persistent stuff for simulation - contact_pairs: #soa[dynamic]Contact_Pair, + contact_container: Contact_Container, convex_container: Convex_Container, } @@ -386,7 +412,8 @@ _get_first_free_body :: proc(sim_state: ^Sim_State) -> i32 { destry_sim_state :: proc(sim_state: ^Sim_State) { delete_soa(sim_state.bodies) delete_soa(sim_state.suspension_constraints) - delete_soa(sim_state.contact_pairs) + delete_soa(sim_state.contact_container.contacts) + delete_map(sim_state.contact_container.lookup) convex_container_destroy(&sim_state.convex_container) } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 85a91f3..37c922c 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -70,6 +70,7 @@ sim_state_copy :: proc(dst: ^Sim_State, src: ^Sim_State) { dst.suspension_constraints[i] = src.suspension_constraints[i] } + contact_container_copy(&dst.contact_container, src.contact_container) convex_container_copy(&dst.convex_container, src.convex_container) } @@ -78,8 +79,6 @@ Step_Mode :: enum { Single, } -Potential_Pair :: [2]u16 - // Top Level Acceleration Structure TLAS :: struct { bvh_tree: bvh.BVH, @@ -122,9 +121,8 @@ build_tlas :: proc(sim_state: ^Sim_State, config: Solver_Config) -> TLAS { } // TODO: free intermediate temp allocs -find_potential_pairs :: proc(sim_state: ^Sim_State, tlas: ^TLAS) -> []Potential_Pair { +find_new_contacts :: proc(sim_state: ^Sim_State, tlas: ^TLAS) { tracy.Zone() - potential_pairs_map := make(map[Potential_Pair]bool, context.temp_allocator) for i in 0 ..< len(sim_state.bodies_slice) { assert(i <= int(max(u16))) @@ -136,34 +134,30 @@ find_potential_pairs :: proc(sim_state: ^Sim_State, tlas: ^TLAS) -> []Potential_ it := bvh.iterator_intersect_leaf(&tlas.bvh_tree, body_aabb) for leaf_node in bvh.iterator_intersect_leaf_next(&it) { - for i in 0 ..< leaf_node.prim_len { - other_body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + i] + for j in 0 ..< leaf_node.prim_len { + other_body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + j] prim_aabb := tlas.body_aabbs[other_body_idx] - if body_idx != other_body_idx && bvh.test_aabb_vs_aabb(body_aabb, prim_aabb) { - pair := Potential_Pair { - min(body_idx, other_body_idx), - max(body_idx, other_body_idx), + pair := make_contact_pair(i32(body_idx), i32(other_body_idx)) + if body_idx != other_body_idx && + bvh.test_aabb_vs_aabb(body_aabb, prim_aabb) && + !(pair in sim_state.contact_container.lookup) { + + new_contact_idx := len(sim_state.contact_container.contacts) + resize_soa(&sim_state.contact_container.contacts, new_contact_idx + 1) + contact := &sim_state.contact_container.contacts[new_contact_idx] + + contact^ = Contact { + a = Body_Handle(i + 1), + b = Body_Handle(other_body_idx + 1), } - potential_pairs_map[pair] = true + sim_state.contact_container.lookup[pair] = i32(new_contact_idx) } } } } } - - potential_pairs := make([]Potential_Pair, len(potential_pairs_map), context.temp_allocator) - - { - i := 0 - for p in potential_pairs_map { - potential_pairs[i] = p - i += 1 - } - } - - return potential_pairs } // Outer simulation loop for fixed timestepping @@ -216,7 +210,7 @@ GLOBAL_PLANE :: collision.Plane { dist = 0, } -Contact_Pair :: struct { +Contact :: struct { a, b: Body_Handle, prev_x_a, prev_x_b: Vec3, prev_q_a, prev_q_b: Quat, @@ -228,11 +222,7 @@ Contact_Pair :: struct { applied_normal_correction: [4]f32, } -find_collisions :: proc( - sim_state: ^Sim_State, - contact_pairs: ^#soa[dynamic]Contact_Pair, - potential_pairs: []Potential_Pair, -) { +update_contacts :: proc(sim_state: ^Sim_State) { tracy.Zone() graph_color_bitmask: [4]bit_array.Bit_Array @@ -245,14 +235,26 @@ find_collisions :: proc( ) } - for pair in potential_pairs { - i, j := int(pair[0]), int(pair[1]) + for contact_idx in 0 ..< len(sim_state.contact_container.contacts) { + contact := &sim_state.contact_container.contacts[contact_idx] + + i, j := i32(contact.a) - 1, i32(contact.b) - 1 body, body2 := &sim_state.bodies_slice[i], &sim_state.bodies_slice[j] assert(body.alive) assert(body2.alive) + contact.prev_x_a = body.x + contact.prev_x_b = body2.x + contact.prev_q_a = body.q + contact.prev_q_b = body2.q + contact.manifold = {} + contact.lambda_normal = 0 + contact.lambda_tangent = 0 + contact.applied_static_friction = false + contact.applied_normal_correction = 0 + aabb1, aabb2 := body_get_aabb(body), body_get_aabb(body2) // TODO: extract common math functions into a sane place @@ -271,19 +273,8 @@ find_collisions :: proc( raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2) if collision { - new_contact_idx := len(contact_pairs) - resize_soa(contact_pairs, new_contact_idx + 1) - contact_pair := &contact_pairs[new_contact_idx] - contact_pair^ = Contact_Pair { - a = Body_Handle(i + 1), - b = Body_Handle(j + 1), - prev_x_a = body.x, - prev_x_b = body2.x, - prev_q_a = body.q, - prev_q_b = body2.q, - manifold = raw_manifold, - } - manifold := &contact_pair.manifold + manifold := &contact.manifold + manifold^ = raw_manifold // Convert manifold contact from world to local space for point_idx in 0 ..< manifold.points_len { @@ -338,21 +329,21 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ { tracy.ZoneN("simulate_step::solve_collisions") - for i in 0 ..< len(sim_state.contact_pairs) { - contact_pair := &sim_state.contact_pairs[i] - body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b) + for i in 0 ..< len(sim_state.contact_container.contacts) { + contact := &sim_state.contact_container.contacts[i] + body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) - contact_pair^ = Contact_Pair { - a = contact_pair.a, - b = contact_pair.b, + contact^ = Contact { + a = contact.a, + b = contact.b, prev_x_a = body.x, prev_x_b = body2.x, prev_q_a = body.q, prev_q_b = body2.q, - manifold = contact_pair.manifold, + manifold = contact.manifold, } - manifold := &contact_pair.manifold + manifold := &contact.manifold for point_idx in 0 ..< manifold.points_len { p1, p2 := manifold.points_a[point_idx], manifold.points_b[point_idx] @@ -372,9 +363,9 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ p2, ) if ok { - contact_pair.applied_normal_correction[point_idx] = -separation - contact_pair.applied_corrections += 1 - contact_pair.lambda_normal[point_idx] = lambda_norm + contact.applied_normal_correction[point_idx] = -separation + contact.applied_corrections += 1 + contact.lambda_normal[point_idx] = lambda_norm apply_correction(body, corr1, p1) apply_correction(body2, corr2, p2) @@ -391,12 +382,12 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ context.user_ptr = sim_state slice.sort_by( sim_state.contact_pairs[:sim_state.contact_pairs_len], - proc(c1, c2: Contact_Pair) -> bool { + proc(c1, c2: Contact) -> bool { sim_state := cast(^Sim_State)context.user_ptr find_min_contact_y :: proc( scene: ^Sim_State, - c: Contact_Pair, + c: Contact, ) -> ( min_contact_y: f32, ) { @@ -419,12 +410,12 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ ) } - for &contact_pair in sim_state.contact_pairs { - manifold := contact_pair.manifold - body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b) + for &contact in sim_state.contact_container.contacts { + manifold := contact.manifold + body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) for point_idx in 0 ..< manifold.points_len { - lambda_norm := contact_pair.lambda_normal[point_idx] + lambda_norm := contact.lambda_normal[point_idx] if lambda_norm != 0 { p1 := body_local_to_world(body, manifold.points_a[point_idx]) p2 := body_local_to_world(body2, manifold.points_b[point_idx]) @@ -457,8 +448,8 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ STATIC_FRICTION :: 0 if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm { - contact_pair.applied_static_friction[point_idx] = true - contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent + contact.applied_static_friction[point_idx] = true + contact.lambda_tangent[point_idx] = delta_lambda_tangent apply_correction(body, corr1_tangent, p1) apply_correction(body2, corr2_tangent, p2) @@ -516,15 +507,15 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ if true { tracy.ZoneN("simulate_step::restitution") - for &pair in sim_state.contact_pairs { - manifold := &pair.manifold + for &contact in sim_state.contact_container.contacts { + manifold := &contact.manifold - body, body2 := get_body(sim_state, pair.a), get_body(sim_state, pair.b) + body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) prev_q1, prev_q2 := body.prev_q, body2.prev_q for point_idx in 0 ..< manifold.points_len { - if pair.lambda_normal[point_idx] == 0 { + if contact.lambda_normal[point_idx] == 0 { continue } prev_r1 := lg.quaternion_mul_vector3(prev_q1, manifold.points_a[point_idx]) @@ -571,13 +562,13 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ if true { tracy.ZoneN("simulate_step::dynamic_friction") - for &pair in sim_state.contact_pairs { - manifold := &pair.manifold - body1 := get_body(sim_state, pair.a) - body2 := get_body(sim_state, pair.b) + for &contact in sim_state.contact_container.contacts { + manifold := &contact.manifold + body1 := get_body(sim_state, contact.a) + body2 := get_body(sim_state, contact.b) - for point_idx in 0 ..< pair.manifold.points_len { - if pair.applied_static_friction[point_idx] || pair.lambda_normal == 0 { + for point_idx in 0 ..< contact.manifold.points_len { + if contact.applied_static_friction[point_idx] || contact.lambda_normal == 0 { continue } p1, p2 := @@ -608,7 +599,7 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_ min( dt * DYNAMIC_FRICTION * - abs(pair.lambda_normal[point_idx] / (dt * dt)), + abs(contact.lambda_normal[point_idx] / (dt * dt)), v_tangent_len / w, ) @@ -709,7 +700,7 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d // for i in 0 ..< len(sim_state.contact_pairs) { // contact_pair := &sim_state.contact_pairs[i] - // + // // } for i in 0 ..< len(sim_state.bodies_slice) { @@ -743,30 +734,54 @@ simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Confi dt := config.timestep / f32(substeps) inv_dt := 1.0 / dt - resize_soa(&sim_state.contact_pairs, 0) tlas := build_tlas(sim_state, config) - potential_pairs := find_potential_pairs(sim_state, &tlas) + find_new_contacts(sim_state, &tlas) + + { + tracy.ZoneN("simulate_step::remove_invalid_contacts") + i := 0 + for i < len(sim_state.contact_container.contacts) { + contact := sim_state.contact_container.contacts[i] + + aabb_a := tlas.body_aabbs[int(contact.a) - 1] + aabb_b := tlas.body_aabbs[int(contact.b) - 1] + + if !bvh.test_aabb_vs_aabb(aabb_a, aabb_b) { + removed_pair := make_contact_pair(i32(contact.a) - 1, i32(contact.b) - 1) + delete_key(&sim_state.contact_container.lookup, removed_pair) + + unordered_remove_soa(&sim_state.contact_container.contacts, i) + + if i < len(sim_state.contact_container.contacts) { + moved_contact := &sim_state.contact_container.contacts[i] + moved_pair := make_contact_pair( + i32(moved_contact.a) - 1, + i32(moved_contact.b) - 1, + ) + sim_state.contact_container.lookup[moved_pair] = i32(i) + } + } else { + i += 1 + } + } + } + + update_contacts(sim_state) Solver :: enum { XPBD, PGS, } - solver := Solver.PGS + solver := Solver.XPBD switch solver { case .XPBD: - sim_state_copy(&scene.scratch_sim_state, get_sim_state(scene)) - xpbd_predict_positions(&scene.scratch_sim_state, config, config.timestep) - find_collisions(&scene.scratch_sim_state, &sim_state.contact_pairs, potential_pairs) - for _ in 0 ..< substeps { xpbd_substep(sim_state, config, dt, inv_dt) } case .PGS: - find_collisions(sim_state, &sim_state.contact_pairs, potential_pairs) - for _ in 0 ..< substeps { pgs_substep(sim_state, config, dt, inv_dt) }