Interpolate wheels too, use interpolated bodies and wheels in debug visualization

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

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^))
+				// shape_aabb := body_transform_shape_aabb(body, shape_get_aabb(shape^))
-				rl.DrawBoundingBox(
+				// rl.DrawBoundingBox(
-					rl.BoundingBox {
+				// 	rl.BoundingBox {
-						min = shape_aabb.center - shape_aabb.extent,
+				// 		min = shape_aabb.center - shape_aabb.extent,
-						max = shape_aabb.center + shape_aabb.extent,
+				// 		max = shape_aabb.center + shape_aabb.extent,
-					},
+				// 	},
-					debug.int_to_color(i32(i + 2)),
+				// 	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(

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^ = body^
-		result = prev_body^
+		result.x = lg.lerp(prev_body.x, body.x, t)
-		result.x = body.x * t + (1.0 - t) * prev_body.x
 		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) {
+	@(static) _invalid_wheel: #soa[1]Suspension_Constraint
-		return &_invalid_suspension_constraint_slice[0]
+	@(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)
 }

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)