diff --git a/game/game.odin b/game/game.odin index b4a5172..ec7d251 100644 --- a/game/game.odin +++ b/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 y in -3 ..< 4 { + for x in 0 ..< 10 { + for y in -3 ..< 10 { physics.immediate_body( &world.physics_scene, &runtime_world.solver_state, diff --git a/game/physics/bvh/bvh.odin b/game/physics/bvh/bvh.odin index 51d426c..4572ac2 100644 --- a/game/physics/bvh/bvh.odin +++ b/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() diff --git a/game/physics/bvh/debug.odin b/game/physics/bvh/debug.odin index 40b4122..7c5b4bc 100644 --- a/game/physics/bvh/debug.odin +++ b/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) diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index 7b1d1ad..db1aa6e 100644 --- a/game/physics/collision/convex.odin +++ b/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]) diff --git a/game/physics/immediate.odin b/game/physics/immediate.odin index d77924d..aa48446 100644 --- a/game/physics/immediate.odin +++ 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) } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 3178048..3e62db6 100644 --- a/game/physics/simulation.odin +++ b/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 { - body := &sim_state.bodies_slice[i] - if body.alive { - for _, j in sim_state.bodies { - body2 := &sim_state.bodies_slice[j] + for pair in potential_pairs { + i, j := int(pair[0]), int(pair[1]) - if i != 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) + body, body2 := &sim_state.bodies_slice[i], &sim_state.bodies_slice[j] - // Raw manifold has contact points in world space - raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2) + assert(body.alive) + assert(body2.alive) - if collision { - 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 + m1, m2 := + body_get_convex_shape_world(sim_state, body), + body_get_convex_shape_world(sim_state, body2) - // 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) + // Raw manifold has contact points in world space + raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2) - p_diff_normal := lg.dot(p2 - p1, manifold.normal) - separation := min(p_diff_normal, 0) + if collision { + 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( - 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 + p_diff_normal := lg.dot(p2 - p1, manifold.normal) + separation := min(p_diff_normal, 0) - apply_correction(body, corr1, p1) - apply_correction(body2, corr2, p2) - } - } - } + handled_pairs[{a = min(i, j), b = max(i, j)}] = true + + 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) } } }