Start implementing optimized substepping (collision detection only once per frame)

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 x in 0 ..< 40 {
-				for y in 0 ..< 10 {
+				for y in 0 ..< 5 {
 					physics.immediate_body(
 						&world.physics_scene,
 						&runtime_world.solver_state,

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_a_separation := face_query_a.separation + 0.3
-	biased_face_b_separation := face_query_b.separation
+	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
 			}

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 :=

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

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)
+	convex_container_reconcile(&src.convex_container)
-	next_state := get_next_sim_state(scene)
 
-	convex_container_reconcile(&current_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
+	resize(&dst.bodies, len(src.bodies))
-	next_state.first_free_body_plus_one = current_state.first_free_body_plus_one
+	resize(&dst.suspension_constraints, len(src.suspension_constraints))
-	next_state.first_free_suspension_constraint_plus_one =
-		current_state.first_free_suspension_constraint_plus_one
 
-	resize(&next_state.bodies, len(current_state.bodies))
+	dst.bodies_slice = dst.bodies[:]
-	resize(&next_state.suspension_constraints, len(current_state.suspension_constraints))
+	dst.suspension_constraints_slice = dst.suspension_constraints[:]
 
-	next_state.bodies_slice = next_state.bodies[:]
+	for i in 0 ..< len(dst.bodies) {
-	next_state.suspension_constraints_slice = next_state.suspension_constraints[:]
+		dst.bodies[i] = src.bodies[i]
-
-	for i in 0 ..< len(next_state.bodies) {
-		next_state.bodies[i] = current_state.bodies[i]
 	}
-	for i in 0 ..< len(next_state.suspension_constraints) {
+	for i in 0 ..< len(dst.suspension_constraints) {
-		next_state.suspension_constraints[i] = current_state.suspension_constraints[i]
+		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
+		predict_positions(sim_state, config, dt)
-		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
-			}
-		}
-
 
 		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 {
+				contact_pair^ = Contact_Pair {
-				i, j := int(pair[0]), int(pair[1])
+					a        = contact_pair.a,
-
+					b        = contact_pair.b,
-				body, body2 := &sim_state.bodies_slice[i], &sim_state.bodies_slice[j]
+					prev_x_a = body.x,
-
+					prev_x_b = body2.x,
-				assert(body.alive)
+					prev_q_a = body.q,
-				assert(body2.alive)
+					prev_q_b = body2.q,
-
+					manifold = contact_pair.manifold,
-				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 :=
+				manifold := &contact_pair.manifold
-					body_get_convex_shape_world(sim_state, body),
-					body_get_convex_shape_world(sim_state, body2)
 
-				// 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)
+					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 {
+					p_diff_normal := lg.dot(p2 - p1, manifold.normal)
-					new_contact_idx := len(sim_state.contact_pairs)
+					separation := min(p_diff_normal, 0)
-					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
 
-					// Convert manifold contact from world to local space
+					lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
-					for point_idx in 0 ..< manifold.points_len {
+						dt,
-						manifold.points_a[point_idx] = body_world_to_local(
+						body,
-							body,
+						body2,
-							manifold.points_a[point_idx],
+						0,
-						)
+						separation,
-						manifold.points_b[point_idx] = body_world_to_local(
+						-manifold.normal,
-							body2,
+						p1,
-							manifold.points_b[point_idx],
+						p2,
-						)
+					)
-					}
+					if ok {
-					for point_idx in 0 ..< manifold.points_len {
+						contact_pair.applied_normal_correction[point_idx] = -separation
-						p1, p2 := manifold.points_a[point_idx], manifold.points_b[point_idx]
+						contact_pair.applied_corrections += 1
-						p1, p2 = body_local_to_world(body, p1), body_local_to_world(body2, p2)
+						contact_pair.lambda_normal[point_idx] = lambda_norm
 
-						p_diff_normal := lg.dot(p2 - p1, manifold.normal)
+						apply_correction(body, corr1, p1)
-						separation := min(p_diff_normal, 0)
+						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)
-						}
 					}
 				}
 			}
@@ -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 +
+							body.prev_x +
-							lg.quaternion_mul_vector3(
+							lg.quaternion_mul_vector3(body.prev_q, manifold.points_a[point_idx])
-								body_states[i].prev_q,
-								manifold.points_a[point_idx],
-							)
 						prev_p2 :=
-							body_states[j].prev_x +
+							body2.prev_x +
-							lg.quaternion_mul_vector3(
+							lg.quaternion_mul_vector3(body2.prev_q, manifold.points_b[point_idx])
-								body_states[j].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 := body.prev_q, body2.prev_q
-				prev_q1, prev_q2 := s1.prev_q, s2.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)) -
+						(body.prev_v + lg.cross(body.prev_w, prev_r1)) -
-						(s2.prev_v + lg.cross(s2.prev_w, prev_r2))
+						(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)
 					}
 				}
 			}