Use BVH to find potential collision pairs before simulation steps

game/game.odin

@@ -277,8 +277,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 
 		if true {
 
-			for x in 0 ..< 1 {
+			for x in 0 ..< 10 {
-				for y in -3 ..< 4 {
+				for y in -3 ..< 10 {
 					physics.immediate_body(
 						&world.physics_scene,
 						&runtime_world.solver_state,

game/physics/bvh/bvh.odin

@@ -135,6 +135,7 @@ build_bvh_from_aabbs :: proc(
 ) -> (
 	bvh: BVH,
 ) {
+	tracy.Zone()
 	bvh.nodes, _ = mem.make_aligned([]Node, len(aabbs) * 2 - 1, size_of(Node), allocator)
 	bvh.primitives = make([]u16, len(aabbs), allocator)
 
@@ -264,6 +265,61 @@ Collision :: struct {
 	triangle_tests: int,
 }
 
+Iterator_Intersect_Leaf :: struct {
+	bvh:              ^BVH,
+	nodes_to_process: queue.Queue(i32),
+	bounds:           AABB,
+}
+
+iterator_intersect_leaf :: proc(bvh: ^BVH, bounds: AABB) -> (it: Iterator_Intersect_Leaf) {
+	it.bvh = bvh
+	it.bounds = bounds
+	queue.init(&it.nodes_to_process, queue.DEFAULT_CAPACITY, context.temp_allocator)
+	queue.push_back(&it.nodes_to_process, 0)
+
+	return it
+}
+
+test_aabb_vs_aabb :: proc(a, b: AABB) -> bool {
+	// Exit with no intersection if separated along an axis
+	if a.max[0] < b.min[0] || a.min[0] > b.max[0] do return false
+	if a.max[1] < b.min[1] || a.min[1] > b.max[1] do return false
+	if a.max[2] < b.min[2] || a.min[2] > b.max[2] do return false
+	// Overlapping on all axes means AABBs are intersecting
+	return true
+}
+
+
+iterator_intersect_leaf_next :: proc(
+	it: ^Iterator_Intersect_Leaf,
+) -> (
+	node: Node,
+	node_idx: int,
+	ok: bool,
+) {
+	for queue.len(it.nodes_to_process) > 0 {
+		cur_node_idx := queue.pop_front(&it.nodes_to_process)
+		assert(cur_node_idx < it.bvh.nodes_used)
+
+		cur_node := &it.bvh.nodes[cur_node_idx]
+
+		if test_aabb_vs_aabb(cur_node.aabb, it.bounds) {
+			if is_leaf_node(cur_node^) {
+				node = cur_node^
+				node_idx = int(cur_node_idx)
+				ok = true
+				break
+			} else {
+				left_node := cur_node.child_or_prim_start
+
+				queue.push_back_elems(&it.nodes_to_process, left_node, left_node + 1)
+			}
+		}
+	}
+
+	return
+}
+
 traverse_bvh_ray_mesh :: proc(bvh: ^BVH, mesh: Mesh, ray: Ray, out_collision: ^Collision) -> bool {
 	tracy.Zone()
 

game/physics/bvh/debug.odin

@@ -6,6 +6,7 @@ import "core:fmt"
 import "core:log"
 import lg "core:math/linalg"
 import "game:debug"
+import "libs:tracy"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
 
@@ -89,6 +90,8 @@ debug_draw_bvh_bounds_mesh :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_
 }
 
 debug_draw_bvh_bounds :: proc(bvh: ^BVH, primitive_bounds: []AABB, node_index: int) {
+	tracy.Zone()
+
 	old_width := rlgl.GetLineWidth()
 	rlgl.SetLineWidth(4)
 	defer rlgl.SetLineWidth(old_width)

game/physics/collision/convex.odin

@@ -4,6 +4,7 @@ import "core:log"
 import "core:math"
 import lg "core:math/linalg"
 import "game:halfedge"
+import "libs:tracy"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
 
@@ -48,21 +49,20 @@ Contact_Manifold :: struct {
 }
 
 convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, collision: bool) {
+	tracy.Zone()
+
 	face_query_a := query_separation_face_directions(a, b)
 	if face_query_a.separation > 0 {
-		log.debugf("face a separation: %v", face_query_a.separation)
 		return
 	}
 	face_query_b := query_separation_face_directions(b, a)
 	if face_query_b.separation > 0 {
-		log.debugf("face b separation: %v", face_query_b.separation)
 		return
 	}
 
 	edge_separation, edge_a, edge_b, edge_separating_plane := query_separation_edges(a, b)
 	_, _ = edge_a, edge_b
 	if edge_separation > 0 {
-		log.debugf("edge separation: %v", edge_separation)
 		return
 	}
 	biased_face_a_separation := face_query_a.separation
@@ -95,6 +95,7 @@ Face_Query :: struct {
 }
 
 query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) {
+	tracy.Zone()
 	result.separation = min(f32)
 	for face, f in a.faces {
 		index := a.edges[face.edge].origin
@@ -171,6 +172,8 @@ query_separation_edges :: proc(
 	b_edge: halfedge.Edge_Index,
 	separating_plane: Plane,
 ) {
+	tracy.Zone()
+
 	separation = min(f32)
 	a_edge = -1
 	b_edge = -1
@@ -181,7 +184,11 @@ query_separation_edges :: proc(
 	success_step: int
 
 	Edge_Pair :: [2]halfedge.Edge_Index
-	checked_pairs := make(map[Edge_Pair]bool, context.temp_allocator)
+	checked_pairs := make_map_cap(
+		map[Edge_Pair]bool,
+		len(a.edges) * len(b.edges),
+		context.temp_allocator,
+	)
 
 	for edge_a, edge_a_idx in a.edges {
 		for edge_b, edge_b_idx in b.edges {
@@ -190,6 +197,8 @@ query_separation_edges :: proc(
 				continue
 			}
 
+			tracy.ZoneN("collision.query_separation_edges::check_single_pair")
+
 			checked_pairs[pair] = true
 			if edge_a.twin >= 0 {
 				checked_pairs[{edge_a.twin, halfedge.Edge_Index(edge_b_idx)}] = true
@@ -210,7 +219,6 @@ query_separation_edges :: proc(
 				continue
 			}
 
-
 			edge_a_origin, _ := halfedge.get_edge_points(a, edge_a)
 			if lg.dot(axis, edge_a_origin - a.center) < 0 {
 				axis = -axis
@@ -298,6 +306,8 @@ create_face_contact_manifold :: proc(
 ) -> (
 	manifold: Contact_Manifold,
 ) {
+	tracy.Zone()
+
 	is_ref_a := face_query_a.separation > face_query_b.separation
 	ref_face_query := is_ref_a ? face_query_a : face_query_b
 	ref_convex := is_ref_a ? a : b
@@ -548,6 +558,8 @@ create_edge_contact_manifold :: proc(
 ) -> (
 	manifold: Contact_Manifold,
 ) {
+	tracy.Zone()
+
 	a1, a2 := halfedge.get_edge_points(a, a.edges[edge_a])
 	b1, b2 := halfedge.get_edge_points(b, b.edges[edge_b])
 

game/physics/immediate.odin

@@ -1,6 +1,7 @@
 package physics
 
 import "core:log"
+import "libs:tracy"
 
 _ :: log
 
@@ -71,6 +72,7 @@ immediate_suspension_constraint :: proc(
 }
 
 prune_immediate :: proc(scene: ^Scene, state: ^Solver_State) {
+	tracy.Zone()
 	prune_immediate_bodies(scene, state)
 	prune_immediate_suspension_constraints(scene, state)
 }

game/physics/simulation.odin

@@ -46,6 +46,7 @@ MAX_STEPS :: 10
 // TODO: move into scene.odin
 // Copy current state to next
 prepare_next_sim_state :: proc(scene: ^Scene) {
+	tracy.Zone()
 	current_state := get_sim_state(scene)
 	next_state := get_next_sim_state(scene)
 
@@ -77,6 +78,8 @@ Step_Mode :: enum {
 	Single,
 }
 
+Potential_Pair :: [2]u16
+
 // Outer simulation loop for fixed timestepping
 simulate :: proc(
 	scene: ^Scene,
@@ -86,6 +89,7 @@ simulate :: proc(
 	commit := true, // commit = false is a special mode for debugging physics stepping to allow rerunning the same step each frame
 	step_mode := Step_Mode.Accumulated_Time,
 ) {
+	tracy.Zone()
 	assert(config.timestep > 0)
 
 	prune_immediate(scene, state)
@@ -121,6 +125,51 @@ simulate :: proc(
 
 	sim_state_bvh := bvh.build_bvh_from_aabbs(body_aabbs, body_indices, context.temp_allocator)
 
+	potential_pairs_map := make(map[Potential_Pair]bool, context.temp_allocator)
+
+	{
+		tracy.ZoneN("physics.simulate::find_potential_pairs")
+
+		for i in 0 ..< len(sim_state.bodies_slice) {
+			assert(i <= int(max(u16)))
+			body_idx := u16(i)
+			body := &sim_state.bodies_slice[i]
+
+			if body.alive {
+				body_aabb := body_aabbs[i]
+				it := bvh.iterator_intersect_leaf(&sim_state_bvh, body_aabb)
+
+				for leaf_node in bvh.iterator_intersect_leaf_next(&it) {
+					for i in 0 ..< leaf_node.prim_len {
+						other_body_idx :=
+							sim_state_bvh.primitives[leaf_node.child_or_prim_start + i]
+						prim_aabb := 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),
+							}
+
+							potential_pairs_map[pair] = true
+						}
+					}
+				}
+			}
+		}
+	}
+
+	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
+		}
+	}
+
 	bvh.debug_draw_bvh_bounds(&sim_state_bvh, body_aabbs, 0)
 
 	switch step_mode {
@@ -133,11 +182,11 @@ simulate :: proc(
 			state.accumulated_time -= config.timestep
 
 			if num_steps < MAX_STEPS {
-				simulate_step(sim_state, config)
+				simulate_step(sim_state, config, potential_pairs)
 			}
 		}
 	case .Single:
-		simulate_step(get_next_sim_state(scene), config)
+		simulate_step(get_next_sim_state(scene), config, potential_pairs)
 	}
 
 	if commit {
@@ -173,7 +222,11 @@ Contact_Pair :: struct {
 	applied_normal_correction: [4]f32,
 }
 
-simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
+simulate_step :: proc(
+	sim_state: ^Sim_State,
+	config: Solver_Config,
+	potential_pairs: []Potential_Pair,
+) {
 	tracy.Zone()
 
 	body_states := make([]Body_Sim_State, len(sim_state.bodies), context.temp_allocator)
@@ -222,80 +275,72 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
 		{
 			tracy.ZoneN("simulate_step::collisions")
 
-			for _, i in sim_state.bodies {
+			for pair in potential_pairs {
-				body := &sim_state.bodies_slice[i]
+				i, j := int(pair[0]), int(pair[1])
-				if body.alive {
-					for _, j in sim_state.bodies {
-						body2 := &sim_state.bodies_slice[j]
 
-						if i != j &&
+				body, body2 := &sim_state.bodies_slice[i], &sim_state.bodies_slice[j]
-						   body2.alive &&
-						   !handled_pairs[{a = min(i, j), b = max(i, j)}] {
-							m1, m2 :=
-								body_get_convex_shape_world(sim_state, body),
-								body_get_convex_shape_world(sim_state, body2)
 
-							// Raw manifold has contact points in world space
+				assert(body.alive)
-							raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
+				assert(body2.alive)
 
-							if collision {
+				m1, m2 :=
-								contact_pair := &sim_state.contact_pairs[sim_state.contact_pairs_len]
+					body_get_convex_shape_world(sim_state, body),
-								contact_pair^ = Contact_Pair {
+					body_get_convex_shape_world(sim_state, body2)
-									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,
-								}
-								sim_state.contact_pairs_len += 1
-								manifold := &contact_pair.manifold
 
-								// Convert manifold contact from world to local space
+				// Raw manifold has contact points in world space
-								for point_idx in 0 ..< manifold.points_len {
+				raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
-									manifold.points_a[point_idx] = body_world_to_local(
-										body,
-										manifold.points_a[point_idx],
-									)
-									manifold.points_b[point_idx] = body_world_to_local(
-										body2,
-										manifold.points_b[point_idx],
-									)
-								}
-								for point_idx in 0 ..< manifold.points_len {
-									p1, p2 :=
-										manifold.points_a[point_idx], manifold.points_b[point_idx]
-									p1, p2 =
-										body_local_to_world(body, p1),
-										body_local_to_world(body2, p2)
 
-									p_diff_normal := lg.dot(p2 - p1, manifold.normal)
+				if collision {
-									separation := min(p_diff_normal, 0)
+					contact_pair := &sim_state.contact_pairs[sim_state.contact_pairs_len]
+					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,
+					}
+					sim_state.contact_pairs_len += 1
+					manifold := &contact_pair.manifold
 
-									handled_pairs[{a = min(i, j), b = max(i, j)}] = true
+					// Convert manifold contact from world to local space
+					for point_idx in 0 ..< manifold.points_len {
+						manifold.points_a[point_idx] = body_world_to_local(
+							body,
+							manifold.points_a[point_idx],
+						)
+						manifold.points_b[point_idx] = body_world_to_local(
+							body2,
+							manifold.points_b[point_idx],
+						)
+					}
+					for point_idx in 0 ..< manifold.points_len {
+						p1, p2 := manifold.points_a[point_idx], manifold.points_b[point_idx]
+						p1, p2 = body_local_to_world(body, p1), body_local_to_world(body2, p2)
 
-									lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
+						p_diff_normal := lg.dot(p2 - p1, manifold.normal)
-										dt,
+						separation := min(p_diff_normal, 0)
-										body,
-										body2,
-										0,
-										separation,
-										-manifold.normal,
-										p1,
-										p2,
-									)
-									if ok {
-										contact_pair.applied_normal_correction[point_idx] =
-										-separation
-										contact_pair.applied_corrections += 1
-										contact_pair.lambda_normal[point_idx] = lambda_norm
 
-										apply_correction(body, corr1, p1)
+						handled_pairs[{a = min(i, j), b = max(i, j)}] = true
-										apply_correction(body2, corr2, p2)
+
-									}
+						lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
-								}
+							dt,
-							}
+							body,
+							body2,
+							0,
+							separation,
+							-manifold.normal,
+							p1,
+							p2,
+						)
+						if ok {
+							contact_pair.applied_normal_correction[point_idx] = -separation
+							contact_pair.applied_corrections += 1
+							contact_pair.lambda_normal[point_idx] = lambda_norm
+
+							apply_correction(body, corr1, p1)
+							apply_correction(body2, corr2, p2)
 						}
 					}
 				}