Refactor collision detection to support persistent contacts across frames

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 :=

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)
 }
 

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 {
+				for j in 0 ..< leaf_node.prim_len {
-					other_body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + i]
+					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 := make_contact_pair(i32(body_idx), i32(other_body_idx))
-						pair := Potential_Pair {
+					if body_idx != other_body_idx &&
-							min(body_idx, other_body_idx),
+					   bvh.test_aabb_vs_aabb(body_aabb, prim_aabb) &&
-							max(body_idx, other_body_idx),
+					   !(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(
+update_contacts :: proc(sim_state: ^Sim_State) {
-	sim_state: ^Sim_State,
-	contact_pairs: ^#soa[dynamic]Contact_Pair,
-	potential_pairs: []Potential_Pair,
-) {
 	tracy.Zone()
 
 	graph_color_bitmask: [4]bit_array.Bit_Array
@@ -245,14 +235,26 @@ find_collisions :: proc(
 		)
 	}
 
-	for pair in potential_pairs {
+	for contact_idx in 0 ..< len(sim_state.contact_container.contacts) {
-		i, j := int(pair[0]), int(pair[1])
+		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)
+			manifold := &contact.manifold
-			resize_soa(contact_pairs, new_contact_idx + 1)
+			manifold^ = raw_manifold
-			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
 
 			// 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) {
+		for i in 0 ..< len(sim_state.contact_container.contacts) {
-			contact_pair := &sim_state.contact_pairs[i]
+			contact := &sim_state.contact_container.contacts[i]
-			body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
+			body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b)
 
-			contact_pair^ = Contact_Pair {
+			contact^ = Contact {
-				a        = contact_pair.a,
+				a        = contact.a,
-				b        = contact_pair.b,
+				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.applied_normal_correction[point_idx] = -separation
-					contact_pair.applied_corrections += 1
+					contact.applied_corrections += 1
-					contact_pair.lambda_normal[point_idx] = lambda_norm
+					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 {
+		for &contact in sim_state.contact_container.contacts {
-			manifold := contact_pair.manifold
+			manifold := contact.manifold
-			body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
+			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.applied_static_friction[point_idx] = true
-							contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent
+							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 {
+		for &contact in sim_state.contact_container.contacts {
-			manifold := &pair.manifold
+			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 {
+		for &contact in sim_state.contact_container.contacts {
-			manifold := &pair.manifold
+			manifold := &contact.manifold
-			body1 := get_body(sim_state, pair.a)
+			body1 := get_body(sim_state, contact.a)
-			body2 := get_body(sim_state, pair.b)
+			body2 := get_body(sim_state, contact.b)
 
-			for point_idx in 0 ..< pair.manifold.points_len {
+			for point_idx in 0 ..< contact.manifold.points_len {
-				if pair.applied_static_friction[point_idx] || pair.lambda_normal == 0 {
+				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,
 							)
 
@@ -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)
 		}