diff --git a/game/game.odin b/game/game.odin index 793423b..a62bcf3 100644 --- a/game/game.odin +++ b/game/game.odin @@ -1243,7 +1243,7 @@ draw_world :: proc(world: ^World) { } if world.debug_state.draw_physics_scene { - physics.draw_debug_scene(&world.physics_scene, &phys_debug_state) + physics.draw_debug_scene(&world.physics_scene, SOLVER_CONFIG, &phys_debug_state) } diff --git a/game/physics/debug.odin b/game/physics/debug.odin index f5a4d42..922b040 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -60,7 +60,7 @@ init_debug_state :: proc(debug_state: ^Debug_State) { debug_state.selected_contacts = make(map[int]struct {}, context.temp_allocator) } -draw_debug_scene :: proc(scene: ^Scene, debug_state: ^Debug_State) { +draw_debug_scene :: proc(scene: ^Scene, solver_config: Solver_Config, debug_state: ^Debug_State) { tracy.Zone() runtime.DEFAULT_TEMP_ALLOCATOR_TEMP_GUARD() @@ -118,7 +118,7 @@ draw_debug_scene :: proc(scene: ^Scene, debug_state: ^Debug_State) { } for _, i in sim_state.bodies { - body := &sim_state.bodies_slice[i] + body := get_interpolated_body(scene, solver_config, Body_Handle(i + 1)) if body.alive { pos := body.x @@ -141,14 +141,14 @@ draw_debug_scene :: proc(scene: ^Scene, debug_state: ^Debug_State) { debug.int_to_color(i32(i + 2)), ) - shape_aabb := body_transform_shape_aabb(body, shape_get_aabb(shape^)) - rl.DrawBoundingBox( - rl.BoundingBox { - min = shape_aabb.center - shape_aabb.extent, - max = shape_aabb.center + shape_aabb.extent, - }, - debug.int_to_color(i32(i + 2)), - ) + // shape_aabb := body_transform_shape_aabb(body, shape_get_aabb(shape^)) + // rl.DrawBoundingBox( + // rl.BoundingBox { + // min = shape_aabb.center - shape_aabb.extent, + // max = shape_aabb.center + shape_aabb.extent, + // }, + // debug.int_to_color(i32(i + 2)), + // ) } } } @@ -169,9 +169,10 @@ draw_debug_scene :: proc(scene: ^Scene, debug_state: ^Debug_State) { // } for _, i in sim_state.suspension_constraints { - wheel := &sim_state.suspension_constraints_slice[i] + wheel_handle := Suspension_Constraint_Handle(i + 1) + wheel := get_interpolated_wheel(scene, solver_config, wheel_handle) if wheel.alive { - body := get_body(sim_state, wheel.body) + body := get_interpolated_body(scene, solver_config, wheel.body) pos := body.x rot := body.q @@ -226,7 +227,7 @@ draw_debug_scene :: proc(scene: ^Scene, debug_state: ^Debug_State) { } } - if true { + if false { for &contact, contact_idx in sim_state.contact_container.contacts { if contact_idx in debug_state.selected_contacts || len(debug_state.selected_contacts) == 0 { @@ -235,13 +236,13 @@ draw_debug_scene :: proc(scene: ^Scene, debug_state: ^Debug_State) { points_a_slice, points_b_slice := points_a[:contact.manifold.points_len], points_b[:contact.manifold.points_len] debug_transform_points_local_to_world( - get_body(sim_state, contact.a), + get_interpolated_body(scene, solver_config, contact.a), points_a_slice, ) b_handle := Body_Handle(contact.b) if contact.type == .Body_vs_Body else INVALID_BODY debug_transform_points_local_to_world( - get_body(sim_state, b_handle), + get_interpolated_body(scene, solver_config, b_handle), points_b_slice, ) debug_draw_manifold_points( diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 09dc777..037b1e7 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -124,12 +124,43 @@ Sim_State :: struct { dynamic_tlas: Dynamic_TLAS, } +Sim_State_Interp_Cache :: struct { + // Mapping from body index in actual sim state to body index in interpolated sim state cache + body_mapping: [dynamic]int, + bodies: #soa[dynamic]Body, + bodies_slice: #soa[]Body, + + // Wheel stuff + wheel_mapping: [dynamic]int, + wheels: #soa[dynamic]Suspension_Constraint, + wheels_slice: #soa[]Suspension_Constraint, +} + +sim_state_interp_cache_clear :: proc(cache: ^Sim_State_Interp_Cache) { + clear(&cache.body_mapping) + clear(&cache.bodies) + cache.bodies_slice = nil + + clear(&cache.wheel_mapping) + clear(&cache.wheels) + cache.wheels_slice = nil +} + +sim_state_interp_cache_destroy :: proc(cache: ^Sim_State_Interp_Cache) { + delete(cache.body_mapping) + delete(cache.bodies) + + delete(cache.wheel_mapping) + delete(cache.wheels_slice) +} + DEV_BUILD :: #config(DEV, false) NUM_SIM_STATES :: 2 Scene :: struct { assetman: ^assets.Asset_Manager, simulation_states: [NUM_SIM_STATES]Sim_State, + interpolation_cache: Sim_State_Interp_Cache, simulation_state_index: i32, solver_state: Solver_State, } @@ -202,6 +233,7 @@ Body :: struct { q: Quat, // Angular vel (omega) w: Vec3, + // TODO: remove these prev_x: Vec3, prev_v: Vec3, prev_q: Quat, @@ -495,9 +527,6 @@ Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint Engine_Ptr :: ^Engine Level_Geom_Ptr :: ^Level_Geom -_invalid_suspension_constraint: #soa[1]Suspension_Constraint -_invalid_suspension_constraint_slice := _invalid_suspension_constraint[:] - get_prev_sim_state_index :: proc(scene: ^Scene) -> i32 { return (scene.simulation_state_index - 1) %% i32(len(scene.simulation_states)) } @@ -540,35 +569,123 @@ get_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) -> Body_Ptr { return &sim_state.bodies_slice[index] } +@(private = "file") +allocate_interpolated_body :: proc( + cache: ^Sim_State_Interp_Cache, + handle: Body_Handle, +) -> ( + idx: int, + existing: bool, +) { + body_idx := int(handle) + if body_idx < len(cache.body_mapping) && cache.body_mapping[body_idx] != 0 { + return cache.body_mapping[body_idx] - 1, true + } + + resize_dynamic_array(&cache.body_mapping, body_idx + 1) + + idx = len(cache.bodies) + append_soa(&cache.bodies, Body{}) + cache.bodies_slice = cache.bodies[:] + cache.body_mapping[body_idx] = idx + 1 + return idx, false +} + get_interpolated_body :: proc( scene: ^Scene, solver_config: Solver_Config, handle: Body_Handle, -) -> Body { +) -> Body_Ptr { + idx, existing := allocate_interpolated_body(&scene.interpolation_cache, handle) + + result := &scene.interpolation_cache.bodies_slice[idx] + + if existing { + return result + } + prev_sim_state := get_prev_sim_state(scene) sim_state := get_sim_state(scene) prev_body := get_body(prev_sim_state, handle) body := get_body(sim_state, handle) - result: Body = Body { + result^ = Body { q = lg.QUATERNIONF32_IDENTITY, } if prev_body.alive && body.alive { // interpolate t := scene.solver_state.accumulated_time / solver_config.timestep - log.debugf("t = {}", t) - result = prev_body^ - result.x = body.x * t + (1.0 - t) * prev_body.x + result^ = body^ + result.x = lg.lerp(prev_body.x, body.x, t) result.q = lg.quaternion_slerp_f32(prev_body.q, body.q, t) result.v = lg.lerp(prev_body.v, body.v, t) // I don't think that's right, but not going to be used anyway probably result.w = lg.lerp(prev_body.w, body.w, t) - } else if prev_body.alive { - result = prev_body^ } else if body.alive { - result = body^ + result^ = body^ + } + + return result +} + +@(private = "file") +allocate_interpolated_wheel :: proc( + cache: ^Sim_State_Interp_Cache, + handle: Suspension_Constraint_Handle, +) -> ( + idx: int, + existing: bool, +) { + wheel_idx := int(handle) + if wheel_idx < len(cache.wheel_mapping) && cache.wheel_mapping[wheel_idx] != 0 { + return cache.wheel_mapping[wheel_idx] - 1, true + } + + resize_dynamic_array(&cache.wheel_mapping, wheel_idx + 1) + + idx = len(cache.wheels) + append_soa(&cache.wheels, Suspension_Constraint{}) + cache.wheels_slice = cache.wheels[:] + cache.wheel_mapping[wheel_idx] = idx + 1 + return idx, false +} + +get_interpolated_wheel :: proc( + scene: ^Scene, + solver_config: Solver_Config, + handle: Suspension_Constraint_Handle, +) -> Suspension_Constraint_Ptr { + idx, existing := allocate_interpolated_wheel(&scene.interpolation_cache, handle) + + result := &scene.interpolation_cache.wheels_slice[idx] + + if existing { + return result + } + + prev_sim_state := get_prev_sim_state(scene) + sim_state := get_sim_state(scene) + + prev_wheel := get_suspension_constraint(prev_sim_state, handle) + wheel := get_suspension_constraint(sim_state, handle) + + if prev_wheel.alive && wheel.alive { + // interpolate + t := scene.solver_state.accumulated_time / solver_config.timestep + result^ = prev_wheel^ + result.hit_t = lg.lerp(prev_wheel.hit_t, wheel.hit_t, t) + result.hit_point = lg.lerp(prev_wheel.hit_point, wheel.hit_point, t) + result.hit_normal = lg.normalize0(lg.lerp(prev_wheel.hit_normal, wheel.hit_normal, t)) + result.turn_angle = lg.lerp(prev_wheel.turn_angle, wheel.turn_angle, t) + result.turn_assist = lg.lerp(prev_wheel.turn_assist, wheel.turn_assist, t) + result.q = lg.lerp(prev_wheel.q, wheel.q, t) + result.w = lg.lerp(prev_wheel.w, wheel.w, t) + } else if prev_wheel.alive { + result^ = prev_wheel^ + } else if wheel.alive { + result^ = wheel^ } return result @@ -903,11 +1020,16 @@ get_suspension_constraint :: proc( sim_state: ^Sim_State, handle: Suspension_Constraint_Handle, ) -> Suspension_Constraint_Ptr { - if !is_handle_valid(handle) { - return &_invalid_suspension_constraint_slice[0] - } + @(static) _invalid_wheel: #soa[1]Suspension_Constraint + @(static) _invalid_wheel_slice: #soa[]Suspension_Constraint index := int(handle) - 1 + if index < 0 || index >= len(sim_state.suspension_constraints_slice) { + _invalid_wheel_slice = _invalid_wheel[:] + _invalid_wheel[0] = {} + return &_invalid_wheel_slice[0] + } + return &sim_state.suspension_constraints_slice[index] } @@ -1190,4 +1312,5 @@ scene_destroy :: proc(scene: ^Scene) { destry_sim_state(&sim_state) } destroy_solver_state(&scene.solver_state) + sim_state_interp_cache_destroy(&scene.interpolation_cache) } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 1f2ef4e..3dc401a 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -706,6 +706,8 @@ simulate :: proc( prune_immediate(scene) + sim_state_interp_cache_clear(&scene.interpolation_cache) + did_copy := false sim_state := get_next_sim_state(scene)