From ccd987aeaedfca0245163d73834d220cffeac983 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sat, 8 Mar 2025 14:50:39 +0400 Subject: [PATCH] Start implementing optimized substepping (collision detection only once per frame) --- game/game.odin | 6 +- game/physics/collision/convex.odin | 6 +- game/physics/helpers.odin | 2 +- game/physics/scene.odin | 7 + game/physics/simulation.odin | 335 ++++++++++++++++------------- game_hot_reload.o | 0 6 files changed, 199 insertions(+), 157 deletions(-) create mode 100644 game_hot_reload.o diff --git a/game/game.odin b/game/game.odin index 60600f8..6dbe96a 100644 --- a/game/game.odin +++ b/game/game.odin @@ -75,7 +75,7 @@ Car :: struct { SOLVER_CONFIG :: physics.Solver_Config { timestep = 1.0 / 60, gravity = rl.Vector3{0, -9.8, 0}, - substreps_minus_one = 4 - 1, + substreps_minus_one = 8 - 1, } Game_Memory :: struct { @@ -277,8 +277,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { ) if true { - for x in 0 ..< 20 { - for y in 0 ..< 10 { + for x in 0 ..< 40 { + for y in 0 ..< 5 { physics.immediate_body( &world.physics_scene, &runtime_world.solver_state, diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index 8327d85..a0106c8 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -66,8 +66,8 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli if edge_separation > 0 { return } - biased_face_a_separation := face_query_a.separation - biased_face_b_separation := face_query_b.separation + biased_face_a_separation := face_query_a.separation + 0.3 + biased_face_b_separation := face_query_b.separation + 0.2 biased_edge_separation := edge_separation is_face_a_contact := biased_face_a_separation >= biased_edge_separation @@ -418,7 +418,7 @@ create_face_contact_manifold :: proc( for i in 0 ..< vert_count { d := signed_distance_plane(src_polygon[i], ref_plane) - if d <= 1e-03 { + if d <= 1e-02 { clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal j += 1 } diff --git a/game/physics/helpers.odin b/game/physics/helpers.odin index 0183aa4..5b6ea3a 100644 --- a/game/physics/helpers.odin +++ b/game/physics/helpers.odin @@ -103,7 +103,7 @@ body_get_convex_shape_world :: proc( case Internal_Shape_Convex: mesh = convex_container_get_mesh(&sim_state.convex_container, s.mesh) // TODO: make sure this works as intendent - mesh = halfedge.copy_mesh(mesh, context.temp_allocator) + mesh = halfedge.copy_mesh(mesh, allocator) } transform := diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 43e1ddd..9fb3dfd 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -32,6 +32,8 @@ Sim_State :: struct { Scene :: struct { simulation_states: []Sim_State, + // Speculative prediction state to find collisions + scratch_sim_state: Sim_State, simulation_state_index: i32, } @@ -50,6 +52,10 @@ Body :: struct { q: Quat, // Angular vel (omega) w: Vec3, + prev_x: Vec3, + prev_v: Vec3, + prev_q: Quat, + prev_w: Vec3, // Mass inv_mass: f32, // Moment of inertia @@ -388,5 +394,6 @@ destroy_physics_scene :: proc(scene: ^Scene) { for &sim_state in scene.simulation_states { destry_sim_state(&sim_state) } + destry_sim_state(&scene.scratch_sim_state) delete(scene.simulation_states) } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 4fe38ba..f79d969 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -2,9 +2,11 @@ package physics import "bvh" import "collision" +import "core:container/bit_array" import "core:fmt" import "core:math" import lg "core:math/linalg" +import "core:math/rand" import "core:slice" import "libs:tracy" @@ -46,32 +48,28 @@ MAX_STEPS :: 10 // TODO: move into scene.odin // Copy current state to next -prepare_next_sim_state :: proc(scene: ^Scene) { +sim_state_copy :: proc(dst: ^Sim_State, src: ^Sim_State) { tracy.Zone() - current_state := get_sim_state(scene) - next_state := get_next_sim_state(scene) + convex_container_reconcile(&src.convex_container) - convex_container_reconcile(¤t_state.convex_container) + dst.num_bodies = src.num_bodies + dst.first_free_body_plus_one = src.first_free_body_plus_one + dst.first_free_suspension_constraint_plus_one = src.first_free_suspension_constraint_plus_one - next_state.num_bodies = current_state.num_bodies - next_state.first_free_body_plus_one = current_state.first_free_body_plus_one - next_state.first_free_suspension_constraint_plus_one = - current_state.first_free_suspension_constraint_plus_one + resize(&dst.bodies, len(src.bodies)) + resize(&dst.suspension_constraints, len(src.suspension_constraints)) - resize(&next_state.bodies, len(current_state.bodies)) - resize(&next_state.suspension_constraints, len(current_state.suspension_constraints)) + dst.bodies_slice = dst.bodies[:] + dst.suspension_constraints_slice = dst.suspension_constraints[:] - next_state.bodies_slice = next_state.bodies[:] - next_state.suspension_constraints_slice = next_state.suspension_constraints[:] - - for i in 0 ..< len(next_state.bodies) { - next_state.bodies[i] = current_state.bodies[i] + for i in 0 ..< len(dst.bodies) { + dst.bodies[i] = src.bodies[i] } - for i in 0 ..< len(next_state.suspension_constraints) { - next_state.suspension_constraints[i] = current_state.suspension_constraints[i] + for i in 0 ..< len(dst.suspension_constraints) { + dst.suspension_constraints[i] = src.suspension_constraints[i] } - convex_container_copy(&next_state.convex_container, current_state.convex_container) + convex_container_copy(&dst.convex_container, src.convex_container) } Step_Mode :: enum { @@ -95,7 +93,8 @@ simulate :: proc( prune_immediate(scene, state) - prepare_next_sim_state(scene) + sim_state_copy(get_next_sim_state(scene), get_sim_state(scene)) + sim_state_copy(&scene.scratch_sim_state, get_sim_state(scene)) sim_state := get_next_sim_state(scene) @@ -171,6 +170,10 @@ simulate :: proc( } } + resize_soa(&sim_state.contact_pairs, 0) + predict_positions(&scene.scratch_sim_state, config, config.timestep) + find_collisions(&scene.scratch_sim_state, &sim_state.contact_pairs, potential_pairs) + // bvh.debug_draw_bvh_bounds(&sim_state_bvh, body_aabbs, 0) num_steps := 0 @@ -183,11 +186,11 @@ simulate :: proc( state.accumulated_time -= config.timestep if num_steps < MAX_STEPS { - simulate_step(sim_state, config, potential_pairs) + simulate_step(sim_state, config) } } case .Single: - simulate_step(get_next_sim_state(scene), config, potential_pairs) + simulate_step(get_next_sim_state(scene), config) num_steps += 1 } @@ -224,16 +227,109 @@ Contact_Pair :: struct { applied_normal_correction: [4]f32, } -simulate_step :: proc( +find_collisions :: proc( sim_state: ^Sim_State, - config: Solver_Config, + contact_pairs: ^#soa[dynamic]Contact_Pair, potential_pairs: []Potential_Pair, ) { tracy.Zone() - body_states := make_soa(#soa[]Body_Sim_State, len(sim_state.bodies), context.temp_allocator) + graph_color_bitmask: [4]bit_array.Bit_Array + for i in 0 ..< len(graph_color_bitmask) { + bit_array.init( + &graph_color_bitmask[i], + len(sim_state.bodies_slice), + 0, + context.temp_allocator, + ) + } - resize_soa(&sim_state.contact_pairs, 0) + for pair in potential_pairs { + i, j := int(pair[0]), int(pair[1]) + + body, body2 := &sim_state.bodies_slice[i], &sim_state.bodies_slice[j] + + assert(body.alive) + assert(body2.alive) + + aabb1, aabb2 := body_get_aabb(body), body_get_aabb(body2) + + // TODO: extract common math functions into a sane place + if !collision.test_aabb_vs_aabb( + {min = aabb1.center - aabb1.extent, max = aabb1.center + aabb1.extent}, + {min = aabb2.center - aabb2.extent, max = aabb2.center + aabb2.extent}, + ) { + continue + } + + 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 + 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 + + // 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], + ) + } + } + } +} + +predict_positions :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32) { + // Integrate positions and rotations + for &body in sim_state.bodies { + if body.alive { + body.prev_x = body.x + body.prev_v = body.v + body.prev_w = body.w + body.prev_q = body.q + + body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO + body.x += body.v * dt + + // NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ + q := body.q + + w := body.w + delta_rot := quaternion(x = w.x, y = w.y, z = w.z, w = 0) + delta_rot = delta_rot * q + q.x += 0.5 * dt * delta_rot.x + q.y += 0.5 * dt * delta_rot.y + q.z += 0.5 * dt * delta_rot.z + q.w += 0.5 * dt * delta_rot.w + q = lg.normalize0(q) + + body.q = q + } + } +} + +simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) { + tracy.Zone() substeps := config.substreps_minus_one + 1 @@ -241,120 +337,69 @@ simulate_step :: proc( inv_dt := 1.0 / dt for _ in 0 ..< substeps { - // Integrate positions and rotations - for &body, i in sim_state.bodies { - if body.alive { - body_states[i].prev_x = body.x - body_states[i].prev_v = body.v - body_states[i].prev_w = body.w - body_states[i].prev_q = body.q - - body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO - body.x += body.v * dt - - // NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ - q := body.q - - w := body.w - delta_rot := quaternion(x = w.x, y = w.y, z = w.z, w = 0) - delta_rot = delta_rot * q - q.x += 0.5 * dt * delta_rot.x - q.y += 0.5 * dt * delta_rot.y - q.z += 0.5 * dt * delta_rot.z - q.w += 0.5 * dt * delta_rot.w - q = lg.normalize0(q) - - body.q = q - } - } - + predict_positions(sim_state, config, dt) Body_Pair :: struct { a, b: int, } - handled_pairs := make_map(map[Body_Pair]bool, context.temp_allocator) + + // Shuffle contacts + { + num_contacts := len(sim_state.contact_pairs) + for i in 0 ..< num_contacts { + j := rand.int_max(num_contacts) + + if i != j { + tmp := sim_state.contact_pairs[i] + sim_state.contact_pairs[i] = sim_state.contact_pairs[j] + sim_state.contact_pairs[j] = tmp + } + } + } { - tracy.ZoneN("simulate_step::collisions") + 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 pair in potential_pairs { - i, j := int(pair[0]), int(pair[1]) - - body, body2 := &sim_state.bodies_slice[i], &sim_state.bodies_slice[j] - - assert(body.alive) - assert(body2.alive) - - aabb1, aabb2 := body_get_aabb(body), body_get_aabb(body2) - - // TODO: extract common math functions into a sane place - if !collision.test_aabb_vs_aabb( - {min = aabb1.center - aabb1.extent, max = aabb1.center + aabb1.extent}, - {min = aabb2.center - aabb2.extent, max = aabb2.center + aabb2.extent}, - ) { - continue + contact_pair^ = Contact_Pair { + a = contact_pair.a, + b = contact_pair.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, } - m1, m2 := - body_get_convex_shape_world(sim_state, body), - body_get_convex_shape_world(sim_state, body2) + manifold := &contact_pair.manifold - // Raw manifold has contact points in world space - raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2) + 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) - if collision { - new_contact_idx := len(sim_state.contact_pairs) - resize_soa(&sim_state.contact_pairs, new_contact_idx + 1) - contact_pair := &sim_state.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 + p_diff_normal := lg.dot(p2 - p1, manifold.normal) + separation := min(p_diff_normal, 0) - // 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) - - 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) - } + apply_correction(body, corr1, p1) + apply_correction(body2, corr2, p2) } } } @@ -400,7 +445,6 @@ simulate_step :: proc( manifold := contact_pair.manifold body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b) - i, j := int(contact_pair.a) - 1, int(contact_pair.b) - 1 for point_idx in 0 ..< manifold.points_len { lambda_norm := contact_pair.lambda_normal[point_idx] @@ -408,17 +452,11 @@ simulate_step :: proc( p1 := body_local_to_world(body, manifold.points_a[point_idx]) p2 := body_local_to_world(body2, manifold.points_b[point_idx]) prev_p1 := - body_states[i].prev_x + - lg.quaternion_mul_vector3( - body_states[i].prev_q, - manifold.points_a[point_idx], - ) + body.prev_x + + lg.quaternion_mul_vector3(body.prev_q, manifold.points_a[point_idx]) prev_p2 := - body_states[j].prev_x + - lg.quaternion_mul_vector3( - body_states[j].prev_q, - manifold.points_b[point_idx], - ) + body2.prev_x + + lg.quaternion_mul_vector3(body2.prev_q, manifold.points_b[point_idx]) p_diff_tangent := (p1 - prev_p1) - (p2 - prev_p2) p_diff_tangent -= lg.dot(p_diff_tangent, manifold.normal) * manifold.normal @@ -493,22 +531,20 @@ simulate_step :: proc( for _, i in sim_state.bodies_slice { body := &sim_state.bodies_slice[i] if body.alive { - body_solve_velocity(body, body_states[i].prev_x, body_states[i].prev_q, inv_dt) + body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) } } } // Restituion - { + if true { tracy.ZoneN("simulate_step::restitution") for &pair in sim_state.contact_pairs { - i, j := int(pair.a) - 1, int(pair.b) - 1 manifold := &pair.manifold body, body2 := get_body(sim_state, pair.a), get_body(sim_state, pair.b) - s1, s2 := body_states[i], body_states[j] - prev_q1, prev_q2 := s1.prev_q, s2.prev_q + 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 { @@ -521,8 +557,8 @@ simulate_step :: proc( r2 := lg.quaternion_mul_vector3(body2.q, manifold.points_b[point_idx]) prev_v := - (s1.prev_v + lg.cross(s1.prev_w, prev_r1)) - - (s2.prev_v + lg.cross(s2.prev_w, prev_r2)) + (body.prev_v + lg.cross(body.prev_w, prev_r1)) - + (body2.prev_v + lg.cross(body2.prev_w, prev_r2)) v := (body.v + lg.cross(body.w, r1)) - (body2.v + lg.cross(body2.w, r2)) prev_v_normal := lg.dot(prev_v, manifold.normal) @@ -614,22 +650,21 @@ simulate_step :: proc( } } + // Solve suspension velocity for _, i in sim_state.suspension_constraints { v := &sim_state.suspension_constraints_slice[i] if v.alive { - body_idx := int(v.body) - 1 body := get_body(sim_state, v.body) if body.alive && v.hit { - prev_x, prev_q := body_states[body_idx].prev_x, body_states[body_idx].prev_q + prev_x, prev_q := body.prev_x, body.prev_q wheel_world_pos := body_local_to_world(body, v.rel_pos) prev_wheel_world_pos := prev_x + lg.quaternion_mul_vector3(prev_q, v.rel_pos) vel_3d := (wheel_world_pos - prev_wheel_world_pos) * inv_dt dir := body_local_to_world_vec(body, v.rel_dir) - body_state := body_states[i32(v.body) - 1] // Spring damping if true { @@ -645,7 +680,7 @@ simulate_step :: proc( pos = wheel_world_pos, ) - body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt) + body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) } // Drive forces @@ -661,7 +696,7 @@ simulate_step :: proc( -forward, wheel_world_pos, ) - body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt) + body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) } // Lateral friction @@ -677,7 +712,7 @@ simulate_step :: proc( v.applied_impulse.x = impulse apply_correction(body, corr, v.hit_point) - body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt) + body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) } } } diff --git a/game_hot_reload.o b/game_hot_reload.o new file mode 100644 index 0000000..e69de29