Fix restitution and start implementing BVH acceleration for collision

game/assets/assets.odin

@@ -14,6 +14,8 @@ import "libs:tracy"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
 
+_ :: math
+
 Loaded_Texture :: struct {
 	texture: rl.Texture2D,
 	modtime: c.long,
@@ -144,8 +146,8 @@ get_bvh :: proc(assetman: ^Asset_Manager, path: cstring) -> Loaded_BVH {
 		new_bvhs := make([]bvh.BVH, model.meshCount)
 
 		outer_aabb := bvh.AABB {
-			min = math.F32_MAX,
+			min = max(f32),
-			max = -math.F32_MAX,
+			max = min(f32),
 		}
 
 		for i in 0 ..< model.meshCount {
@@ -223,7 +225,7 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 	edges := make_dynamic_array([dynamic]halfedge.Half_Edge, context.temp_allocator)
 	vertices := make_dynamic_array([dynamic]halfedge.Vertex, context.temp_allocator)
 	faces := make_dynamic_array([dynamic]halfedge.Face, context.temp_allocator)
-	center: rl.Vector3
+	min_pos, max_pos: rl.Vector3 = max(f32), min(f32)
 
 	// Parse obj file directly into halfedge data structure
 	{
@@ -256,7 +258,8 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 					coord_idx += 1
 				}
 				append(&vertices, halfedge.Vertex{pos = vertex, edge = -1})
-				center += vertex
+				min_pos = lg.min(vertex, min_pos)
+				max_pos = lg.max(vertex, max_pos)
 
 				advance(&ctx)
 				ctx.line += 1
@@ -361,7 +364,8 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 		}
 	}
 
-	center /= f32(len(vertices))
+	center := (max_pos + min_pos) * 0.5
+	extent := (max_pos - min_pos) * 0.5
 
 	center_of_mass: rl.Vector3
 
@@ -370,6 +374,7 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 		edges    = edges[:],
 		faces    = faces[:],
 		center   = center,
+		extent   = extent,
 	}
 
 	// Center of mass calculation

game/game.odin

@@ -58,7 +58,7 @@ Runtime_World :: struct {
 	camera:                 rl.Camera3D,
 	orbit_camera:           Orbit_Camera,
 	dt:                     f32,
-	rewind_simulation: bool,
+	rewind_simulation:      bool,
 	step_simulation:        bool,
 	single_step_simulation: bool,
 }
@@ -275,7 +275,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 			},
 		)
 
-		if false {
+		if true {
 
 			for x in 0 ..< 1 {
 				for y in -3 ..< 4 {
@@ -632,16 +632,18 @@ draw :: proc() {
 
 			if !runtime_world.pause {
 				if runtime_world.rewind_simulation {
-					world.physics_scene.simulation_state_index = physics.get_prev_sim_state_index(&world.physics_scene)
+					world.physics_scene.simulation_state_index = physics.get_prev_sim_state_index(
+						&world.physics_scene,
+					)
 				} else {
-				physics.simulate(
+					physics.simulate(
-					&world.physics_scene,
+						&world.physics_scene,
-					&runtime_world.solver_state,
+						&runtime_world.solver_state,
-					SOLVER_CONFIG,
+						SOLVER_CONFIG,
-					dt,
+						dt,
-					commit = runtime_world.step_simulation,
+						commit = runtime_world.step_simulation,
-					step_mode = g_mem.physics_pause ? physics.Step_Mode.Single : physics.Step_Mode.Accumulated_Time,
+						step_mode = g_mem.physics_pause ? physics.Step_Mode.Single : physics.Step_Mode.Accumulated_Time,
-				)
+					)
 				}
 			}
 

game/halfedge/debug.odin

@@ -1,12 +1,27 @@
 package halfedge
 
+import "game:debug"
 import rl "vendor:raylib"
 
 debug_draw_mesh_wires :: proc(mesh: Half_Edge_Mesh, color: rl.Color) {
+	for face, f in mesh.faces {
+		it := iterator_face_edges(mesh, Face_Index(f))
 
-	for edge in mesh.edges {
+		center: Vec3
-		a, b := get_edge_points(mesh, edge)
+		num_points := 0
 
-		rl.DrawLine3D(a, b, color)
+		for edge in iterate_next_edge(&it) {
+			a, b := get_edge_points(mesh, edge)
+
+			center += a
+			num_points += 1
+
+			rl.DrawLine3D(a, b, color)
+		}
+
+		center /= f32(num_points)
+
+		rl.DrawSphereWires(center, 0.1, 4, 4, rl.RED)
+		rl.DrawLine3D(center, center + face.normal, debug.int_to_color(i32(f + 1)))
 	}
 }

game/halfedge/halfedge.odin

@@ -1,9 +1,9 @@
 package halfedge
 
-import lg "core:math/linalg"
 import "core:hash/xxhash"
-import "core:slice"
+import lg "core:math/linalg"
 import "core:mem"
+import "core:slice"
 
 Vec3 :: [3]f32
 
@@ -31,6 +31,7 @@ Half_Edge :: struct {
 
 Half_Edge_Mesh :: struct {
 	center:   Vec3,
+	extent:   Vec3,
 	vertices: []Vertex,
 	faces:    []Face,
 	edges:    []Half_Edge,
@@ -54,14 +55,22 @@ mesh_from_vertex_index_list :: proc(
 	mesh.faces = faces
 	mesh.edges = edges
 
-	mesh_center_avg_factor := 1.0 / f32(len(vertices))
+	min_pos, max_pos: Vec3 = max(f32), min(f32)
 
 	for pos, i in vertices {
 		verts[i].pos = pos
 		verts[i].edge = -1
-		mesh.center += pos * mesh_center_avg_factor
+		min_pos = lg.min(pos, min_pos)
+		max_pos = lg.max(pos, max_pos)
 	}
 
+	if len(vertices) == 0 {
+		min_pos, max_pos = 0, 0
+	}
+
+	mesh.center = (max_pos + min_pos) * 0.5
+	mesh.extent = (max_pos - min_pos) * 0.5
+
 	temp_edges: map[[2]u16]Edge_Index = make_map(map[[2]u16]Edge_Index, context.temp_allocator)
 
 	for f in 0 ..< num_faces {
@@ -214,6 +223,7 @@ copy_mesh :: proc(
 	return
 }
 
+
 transform_mesh :: proc(mesh: ^Half_Edge_Mesh, mat: lg.Matrix4f32) {
 	mesh_center_avg_factor := 1.0 / f32(len(mesh.vertices))
 	new_center: Vec3
@@ -224,6 +234,10 @@ transform_mesh :: proc(mesh: ^Half_Edge_Mesh, mat: lg.Matrix4f32) {
 		new_center += vert.pos * mesh_center_avg_factor
 	}
 	mesh.center = new_center
+	original_extent := mesh.extent
+	mesh.extent = lg.abs(original_extent.xxx * mat[0].xyz)
+	mesh.extent += lg.abs(original_extent.yyy * mat[1].xyz)
+	mesh.extent += lg.abs(original_extent.zzz * mat[2].xyz)
 
 	for i in 0 ..< len(mesh.faces) {
 		n := &mesh.faces[i].normal
@@ -297,7 +311,7 @@ iterate_next_vertex_edge :: proc(
 hash :: proc(mesh: Half_Edge_Mesh) -> u32 {
 	state: xxhash.XXH32_state
 	xxhash.XXH32_reset_state(&state)
-	
+
 	_ = xxhash.XXH32_update(&state, mem.any_to_bytes(mesh.center))
 	_ = xxhash.XXH32_update(&state, slice.to_bytes(mesh.vertices))
 	_ = xxhash.XXH32_update(&state, slice.to_bytes(mesh.faces))

game/physics/bvh/bvh.odin

@@ -14,6 +14,7 @@ import rl "vendor:raylib"
 _ :: log
 _ :: rl
 _ :: lg
+_ :: math
 
 Vec3 :: [3]f32
 
@@ -127,7 +128,13 @@ build_bvh_from_mesh :: proc(mesh: Mesh, allocator := context.allocator) -> (mesh
 }
 
 /// Useful for a top level accel structure
-build_bvh_from_aabbs :: proc(aabbs: []AABB, allocator := context.allocator) -> (bvh: BVH) {
+build_bvh_from_aabbs :: proc(
+	aabbs: []AABB,
+	primitives: []u16,
+	allocator := context.allocator,
+) -> (
+	bvh: BVH,
+) {
 	bvh.nodes, _ = mem.make_aligned([]Node, len(aabbs) * 2 - 1, size_of(Node), allocator)
 	bvh.primitives = make([]u16, len(aabbs), allocator)
 
@@ -141,13 +148,13 @@ build_bvh_from_aabbs :: proc(aabbs: []AABB, allocator := context.allocator) -> (
 	for i in 0 ..< len(aabbs) {
 		centroids[i] = (aabbs[i].max + aabbs[i].min) * 0.5
 
-		bvh.primitives[i] = u16(i)
+		bvh.primitives[i] = primitives[i]
 	}
 
 	bvh.nodes_used = 1
 
 	root := &bvh.nodes[0]
-	root.prim_len = i32(len(aabbs))
+	root.prim_len = i32(len(primitives))
 
 	update_node_bounds(&bvh, 0, aabbs)
 
@@ -161,19 +168,14 @@ update_node_bounds :: proc(bvh: ^BVH, node_idx: i32, prim_aabbs: []AABB) {
 
 	node := &bvh.nodes[node_idx]
 
-	node.aabb.min = math.F32_MAX
+	node.aabb.min = max(f32)
-	node.aabb.max = -math.F32_MAX
+	node.aabb.max = min(f32)
 
 	for i in node.child_or_prim_start ..< node.child_or_prim_start + node.prim_len {
 		prim_aabb := prim_aabbs[bvh.primitives[i]]
 
-		node.aabb.min.x = min(node.aabb.min.x, prim_aabb.min.x)
+		node.aabb.min = lg.min(node.aabb.min, prim_aabb.min)
-		node.aabb.min.y = min(node.aabb.min.y, prim_aabb.min.y)
+		node.aabb.max = lg.max(node.aabb.max, prim_aabb.max)
-		node.aabb.min.z = min(node.aabb.min.z, prim_aabb.min.z)
-
-		node.aabb.max.x = max(node.aabb.max.x, prim_aabb.max.x)
-		node.aabb.max.y = max(node.aabb.max.y, prim_aabb.max.y)
-		node.aabb.max.z = max(node.aabb.max.z, prim_aabb.max.z)
 	}
 }
 
@@ -271,7 +273,7 @@ traverse_bvh_ray_mesh :: proc(bvh: ^BVH, mesh: Mesh, ray: Ray, out_collision: ^C
 	ray.dir_inv.z = 1.0 / ray.dir.z
 
 	if !out_collision.hit {
-		out_collision.t = math.F32_MAX
+		out_collision.t = max(f32)
 	}
 
 	prev_t := out_collision.t

game/physics/bvh/debug.odin

@@ -13,7 +13,7 @@ _ :: fmt
 _ :: log
 
 // Assuming rl.BeginMode3D was called before this
-debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index: int) {
+debug_draw_bvh_bounds_mesh :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index: int) {
 	old_width := rlgl.GetLineWidth()
 	rlgl.SetLineWidth(4)
 	defer rlgl.SetLineWidth(old_width)
@@ -88,6 +88,60 @@ debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index
 	}
 }
 
+debug_draw_bvh_bounds :: proc(bvh: ^BVH, primitive_bounds: []AABB, node_index: int) {
+	old_width := rlgl.GetLineWidth()
+	rlgl.SetLineWidth(4)
+	defer rlgl.SetLineWidth(old_width)
+
+	temp := runtime.default_temp_allocator_temp_begin()
+	defer runtime.default_temp_allocator_temp_end(temp)
+
+	Traversal :: struct {
+		node_idx:    i32,
+		should_draw: bool,
+	}
+	nodes_to_process: queue.Queue(Traversal)
+	queue.init(&nodes_to_process, queue.DEFAULT_CAPACITY, context.temp_allocator)
+
+	queue.push_back(&nodes_to_process, Traversal{0, node_index == 0})
+
+	for queue.len(nodes_to_process) > 0 {
+		traversal := queue.pop_front(&nodes_to_process)
+
+		node_idx := traversal.node_idx
+		should_draw := traversal.should_draw || node_index == int(node_idx)
+
+		node := &bvh.nodes[node_idx]
+
+		if should_draw {
+			rl.DrawBoundingBox(
+				rl.BoundingBox{node.aabb.min, node.aabb.max},
+				debug.int_to_color(node_idx + 1),
+			)
+		}
+
+		if !is_leaf_node(node^) {
+			left_child := node.child_or_prim_start
+			queue.push_back_elems(
+				&nodes_to_process,
+				Traversal{left_child, should_draw},
+				Traversal{left_child + 1, should_draw},
+			)
+		} else if should_draw {
+			for i in node.child_or_prim_start ..< node.child_or_prim_start + node.prim_len {
+				prim := bvh.primitives[i]
+
+				aabb := primitive_bounds[prim]
+
+				rl.DrawBoundingBox(
+					rl.BoundingBox{aabb.min, aabb.max},
+					debug.int_to_color(i32(prim) + 2),
+				)
+			}
+		}
+	}
+}
+
 bvh_mesh_from_rl_mesh :: proc(mesh: rl.Mesh) -> Mesh {
 	return Mesh {
 		vertices = (cast([^]Vec3)mesh.vertices)[:mesh.vertexCount],

game/physics/collision/convex.odin

@@ -50,20 +50,23 @@ Contact_Manifold :: struct {
 convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, collision: bool) {
 	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 + 0.2
+	biased_face_a_separation := face_query_a.separation
-	biased_face_b_separation := face_query_b.separation + 0.1
+	biased_face_b_separation := face_query_b.separation
 	biased_edge_separation := edge_separation
 
 	is_face_a_contact := biased_face_a_separation >= biased_edge_separation

game/physics/debug.odin

@@ -108,7 +108,7 @@ draw_debug_scene :: proc(scene: ^Scene) {
 		}
 	}
 
-	if false {
+	if true {
 		for &contact, contact_idx in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
 			points_a := contact.manifold.points_a
 			points_b := contact.manifold.points_b

game/physics/helpers.odin

@@ -112,3 +112,30 @@ body_get_convex_shape_world :: proc(
 
 	return
 }
+
+shape_get_aabb :: proc(shape: Collision_Shape) -> (aabb: AABB) {
+	switch s in shape {
+	case Shape_Box:
+		aabb.center = 0
+		aabb.extent = s.size * 0.5
+	case Internal_Shape_Convex:
+		aabb.center = s.center
+		aabb.extent = s.extent
+	}
+
+	return aabb
+}
+
+body_get_aabb :: proc(body: Body_Ptr) -> (aabb: AABB) {
+	local_aabb := shape_get_aabb(body.shape)
+
+	aabb_center_local_to_body := body_get_shape_offset_local(body) + local_aabb.center
+	aabb.center = body_local_to_world(body, aabb_center_local_to_body)
+
+	rotation_mat := lg.matrix3_from_quaternion(body.q)
+	aabb.extent = lg.abs(local_aabb.extent.xxx * rotation_mat[0])
+	aabb.extent += lg.abs(local_aabb.extent.yyy * rotation_mat[1])
+	aabb.extent += lg.abs(local_aabb.extent.zzz * rotation_mat[2])
+
+	return aabb
+}

game/physics/scene.odin

@@ -8,10 +8,16 @@ MAX_CONTACTS :: 1024
 Vec3 :: [3]f32
 Quat :: quaternion128
 Matrix3 :: # row_major matrix[3, 3]f32
+AABB :: struct {
+	center: Vec3,
+	extent: Vec3,
+}
 
 Sim_State :: struct {
 	bodies:                                    #soa[dynamic]Body,
 	suspension_constraints:                    #soa[dynamic]Suspension_Constraint,
+	// Number of alive bodies
+	num_bodies:                                i32,
 
 	// Slices. When you call get_body or get_suspension_constraint you will get a pointer to an element in this slice
 	bodies_slice:                              #soa[]Body,
@@ -66,6 +72,8 @@ Internal_Shape_Convex :: struct {
 	mesh:           Convex_Handle,
 	center_of_mass: Vec3,
 	inertia_tensor: Matrix3,
+	center:         Vec3,
+	extent:         Vec3,
 }
 
 Shape_Convex :: struct {
@@ -234,6 +242,8 @@ add_shape :: proc(sim_state: ^Sim_State, shape: Input_Shape) -> (result: Collisi
 	case Shape_Convex:
 		convex: Internal_Shape_Convex
 		convex.mesh = convex_container_add(&sim_state.convex_container, s.mesh)
+		convex.center = s.mesh.center
+		convex.extent = s.mesh.extent
 		convex.center_of_mass = s.center_of_mass
 		convex.inertia_tensor = s.inertia_tensor
 		result = convex
@@ -278,6 +288,8 @@ update_suspension_constraint_from_config :: proc(
 add_body :: proc(sim_state: ^Sim_State, config: Body_Config) -> Body_Handle {
 	body: Body
 
+	sim_state.num_bodies += 1
+
 	initialize_body_from_config(sim_state, &body, config)
 
 	body.alive = true
@@ -310,6 +322,8 @@ remove_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) {
 
 		body.next_plus_one = sim_state.first_free_body_plus_one
 		sim_state.first_free_body_plus_one = i32(handle)
+
+		sim_state.num_bodies -= 1
 	}
 }
 

game/physics/simulation.odin

@@ -1,5 +1,6 @@
 package physics
 
+import "bvh"
 import "collision"
 import "core:fmt"
 import "core:math"
@@ -42,6 +43,7 @@ Immedate_State :: struct($T: typeid) {
 
 MAX_STEPS :: 10
 
+// TODO: move into scene.odin
 // Copy current state to next
 prepare_next_sim_state :: proc(scene: ^Scene) {
 	current_state := get_sim_state(scene)
@@ -49,6 +51,7 @@ prepare_next_sim_state :: proc(scene: ^Scene) {
 
 	convex_container_reconcile(&current_state.convex_container)
 
+	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
@@ -89,6 +92,37 @@ simulate :: proc(
 
 	prepare_next_sim_state(scene)
 
+	sim_state := get_next_sim_state(scene)
+
+	body_aabbs := make([]bvh.AABB, sim_state.num_bodies, context.temp_allocator)
+	body_indices := make([]u16, sim_state.num_bodies, context.temp_allocator)
+
+	{
+		aabb_index := 0
+		for i in 0 ..< len(sim_state.bodies_slice) {
+			body := &sim_state.bodies_slice[i]
+
+			if body.alive {
+				aabb := &body_aabbs[aabb_index]
+				body_indices[aabb_index] = u16(i)
+				aabb_index += 1
+
+				phys_aabb := body_get_aabb(body)
+
+				EXPAND_K :: 2
+				expand := lg.abs(EXPAND_K * config.timestep * body.v)
+				phys_aabb.extent += expand * 0.5
+
+				aabb.min = phys_aabb.center - phys_aabb.extent
+				aabb.max = phys_aabb.center + phys_aabb.extent
+			}
+		}
+	}
+
+	sim_state_bvh := bvh.build_bvh_from_aabbs(body_aabbs, body_indices, context.temp_allocator)
+
+	bvh.debug_draw_bvh_bounds(&sim_state_bvh, body_aabbs, 0)
+
 	switch step_mode {
 	case .Accumulated_Time:
 		state.accumulated_time += dt
@@ -99,7 +133,7 @@ simulate :: proc(
 			state.accumulated_time -= config.timestep
 
 			if num_steps < MAX_STEPS {
-				simulate_step(get_next_sim_state(scene), config)
+				simulate_step(sim_state, config)
 			}
 		}
 	case .Single:
@@ -349,7 +383,7 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
 									p2,
 								)
 
-							STATIC_FRICTION :: 0.5
+							STATIC_FRICTION :: 0.8
 							if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm {
 								contact_pair.applied_static_friction[point_idx] = true
 								contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent
@@ -428,7 +462,7 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
 					prev_v_normal := lg.dot(prev_v, manifold.normal)
 					v_normal := lg.dot(v, manifold.normal)
 
-					RESTITUTION :: 0
+					RESTITUTION :: 0.3
 
 					restitution := f32(RESTITUTION)
 
@@ -439,7 +473,7 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
 					delta_v := manifold.normal * (-v_normal + min(-RESTITUTION * prev_v_normal, 0))
 
 					w1 := get_body_inverse_mass(body, manifold.normal, r1 + body.x)
-					w2 := get_body_inverse_mass(body2, manifold.normal, r2 + body.x)
+					w2 := get_body_inverse_mass(body2, manifold.normal, r2 + body2.x)
 					w := w1 + w2
 
 					if w != 0 {
@@ -478,7 +512,7 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
 					v_normal := lg.dot(manifold.normal, v) * manifold.normal
 					v_tangent := v - v_normal
 
-					DYNAMIC_FRICTION :: 0.4
+					DYNAMIC_FRICTION :: 0.6
 					v_tangent_len := lg.length(v_tangent)
 					if v_tangent_len > 0 {
 						v_tangent_norm := v_tangent / v_tangent_len