Almost working bvh

build_release.sh

@@ -1,3 +1,3 @@
 #!/usr/bin/env bash
 
-odin build main_release -out:game_release.bin -strict-style -vet -no-bounds-check -o:speed
+odin build main_release -collection:common=./common -collection:game=./game -out:game_release.bin -strict-style -vet -no-bounds-check -o:speed -debug

game/assets/assets.odin

@@ -2,6 +2,9 @@ package assets
 
 import "core:c"
 import "core:log"
+import "core:math"
+import lg "core:math/linalg"
+import "game:physics/bvh"
 import rl "vendor:raylib"
 
 Loaded_Texture :: struct {
@@ -14,9 +17,25 @@ Loaded_Model :: struct {
 	modtime: c.long,
 }
 
+Loaded_BVH :: struct {
+	// AABB of all bvhs
+	aabb:    bvh.AABB,
+	// BVH for each mesh in a model
+	bvhs:    []bvh.BVH,
+	modtime: c.long,
+}
+
+destroy_loaded_bvh :: proc(loaded_bvh: Loaded_BVH) {
+	for &mesh_bvh in loaded_bvh.bvhs {
+		bvh.destroy_bvh(&mesh_bvh)
+	}
+	delete(loaded_bvh.bvhs)
+}
+
 Asset_Manager :: struct {
 	textures: map[cstring]Loaded_Texture,
 	models:   map[cstring]Loaded_Model,
+	bvhs:     map[cstring]Loaded_BVH,
 }
 
 get_texture :: proc(assetman: ^Asset_Manager, path: cstring) -> rl.Texture2D {
@@ -45,12 +64,22 @@ get_texture :: proc(assetman: ^Asset_Manager, path: cstring) -> rl.Texture2D {
 	}
 }
 
-get_model :: proc(assetman: ^Asset_Manager, path: cstring) -> rl.Model {
+get_model_ex :: proc(
-	modtime := rl.GetFileModTime(path)
+	assetman: ^Asset_Manager,
+	path: cstring,
+	ref_modtime: c.long = 0, // will check reload status using reference load time. When 0 reloaded will be true only if this call triggered reload
+) -> (
+	model: rl.Model,
+	modtime: c.long,
+	reloaded: bool,
+) {
+	new_modtime := rl.GetFileModTime(path)
 
 	existing, ok := assetman.models[path]
-	if ok && existing.modtime == modtime {
+	if ok && existing.modtime == new_modtime {
-		return existing.model
+		return existing.model,
+			existing.modtime,
+			ref_modtime == 0 ? false : existing.modtime == ref_modtime
 	}
 
 	if ok {
@@ -63,12 +92,66 @@ get_model :: proc(assetman: ^Asset_Manager, path: cstring) -> rl.Model {
 	if rl.IsModelValid(loaded) {
 		assetman.models[path] = {
 			model   = loaded,
+			modtime = new_modtime,
+		}
+		return loaded, new_modtime, true
+	} else {
+		return rl.Model{}, 0, true
+	}
+}
+
+get_model :: proc(assetman: ^Asset_Manager, path: cstring) -> rl.Model {
+	model, _, _ := get_model_ex(assetman, path)
+
+	return model
+}
+
+null_bvhs: []bvh.BVH
+
+get_bvh :: proc(assetman: ^Asset_Manager, path: cstring) -> Loaded_BVH {
+	loaded_bvh, ok := assetman.bvhs[path]
+	model, modtime, reloaded := get_model_ex(assetman, path, loaded_bvh.modtime)
+
+	should_recreate := reloaded || !ok || true
+
+	if ok && should_recreate {
+		destroy_loaded_bvh(loaded_bvh)
+		delete_key(&assetman.bvhs, path)
+	}
+
+	if should_recreate {
+		new_bvhs := make([]bvh.BVH, model.meshCount)
+
+		outer_aabb := bvh.AABB {
+			min = math.F32_MAX,
+			max = math.F32_MIN,
+		}
+
+		for i in 0 ..< model.meshCount {
+			mesh := model.meshes[i]
+			vertices := (cast([^]rl.Vector3)mesh.vertices)[:mesh.vertexCount]
+			indices := mesh.indices[:mesh.triangleCount * 3]
+
+			mesh_bvh := bvh.build_bvh_from_mesh(
+				{vertices = vertices, indices = indices},
+				context.allocator,
+			)
+
+			root_aabb := mesh_bvh.bvh.nodes[0].aabb
+			outer_aabb.min = lg.min(outer_aabb.min, root_aabb.min)
+			outer_aabb.max = lg.max(outer_aabb.max, root_aabb.max)
+
+			new_bvhs[i] = mesh_bvh.bvh
+		}
+
+		assetman.bvhs[path] = Loaded_BVH {
+			aabb    = outer_aabb,
+			bvhs    = new_bvhs,
 			modtime = modtime,
 		}
-		return loaded
-	} else {
-		return rl.Model{}
 	}
+
+	return assetman.bvhs[path]
 }
 
 shutdown :: proc(assetman: ^Asset_Manager) {
@@ -78,6 +161,10 @@ shutdown :: proc(assetman: ^Asset_Manager) {
 	for _, model in assetman.models {
 		rl.UnloadModel(model.model)
 	}
+	for _, loaded_bvh in assetman.bvhs {
+		destroy_loaded_bvh(loaded_bvh)
+	}
 	delete(assetman.textures)
 	delete(assetman.models)
+	delete(assetman.bvhs)
 }

game/game.odin

@@ -21,6 +21,7 @@ import "core:log"
 import "core:math"
 import "core:math/linalg"
 import "game:physics"
+import "game:physics/bvh"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
 
@@ -71,6 +72,7 @@ Game_Memory :: struct {
 	runtime_world: Runtime_World,
 	es:            Editor_State,
 	editor:        bool,
+	preview_bvh:   int,
 }
 
 Track_Edit_State :: enum {
@@ -360,6 +362,13 @@ update :: proc() {
 
 	dt := rl.GetFrameTime()
 
+	if rl.IsKeyReleased(.LEFT_BRACKET) {
+		g_mem.preview_bvh -= 1
+	}
+	if rl.IsKeyReleased(.RIGHT_BRACKET) {
+		g_mem.preview_bvh += 1
+	}
+
 	if g_mem.editor {
 		update_editor(get_editor_state())
 	} else {
@@ -409,25 +418,68 @@ draw :: proc() {
 
 	interpolated_points := calculate_spline_interpolated_points(points[:], context.temp_allocator)
 
-	collision, segment_idx := raycast_spline_tube(
+	// collision, segment_idx := raycast_spline_tube(
-		interpolated_points,
+	// 	interpolated_points,
-		rl.GetScreenToWorldRay(rl.GetMousePosition(), camera),
+	// 	rl.GetScreenToWorldRay(rl.GetMousePosition(), camera),
-	)
+	// )
 
+	car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
 	car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
 
+	mesh_col: bvh.Collision
+	hit_mesh_idx := -1
+
+	rl_ray := rl.GetScreenToWorldRay(rl.GetMousePosition(), camera)
+	ray := bvh.Ray {
+		origin = rl_ray.position,
+		dir    = rl_ray.direction,
+	}
+
 	{
 		rl.BeginMode3D(camera)
 		defer rl.EndMode3D()
 
 		rl.DrawGrid(100, 1)
 
+		{
+			mesh_bvh := assets.get_bvh(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
+
+			for &blas, i in mesh_bvh.bvhs {
+				mesh := car_model.meshes[i]
+
+				if i == -1 {
+					bvh.debug_draw_bvh_bounds(
+						&blas,
+						bvh.bvh_mesh_from_rl_mesh(mesh),
+						0,
+						g_mem.preview_bvh,
+					)
+				}
+
+				vertices := (cast([^]rl.Vector3)mesh.vertices)[:mesh.vertexCount]
+				indices := mesh.indices[:mesh.triangleCount * 3]
+				if bvh.traverse_bvh_ray_mesh(
+					&blas,
+					bvh.Mesh{vertices = vertices, indices = indices},
+					ray,
+					&mesh_col,
+				) {
+					hit_mesh_idx = i
+				}
+			}
+
+			if mesh_col.hit {
+				rl.DrawSphereWires(ray.origin + ray.dir * mesh_col.t, 1, 8, 8, rl.RED)
+			}
+		}
+
 		if !g_mem.editor {
-			car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
 			car_matrix := rl.QuaternionToMatrix(car_body.q)
 			car_model.transform = car_matrix
 
 			rl.DrawModel(car_model, car_body.x - runtime_world.car_com, 1, rl.WHITE)
+		} else {
+			// rl.DrawModel(car_model, 0, 1, rl.WHITE)
 		}
 
 		physics.draw_debug_scene(&world.physics_scene)
@@ -471,10 +523,6 @@ draw :: proc() {
 				}
 			}
 		}
-
-		if collision.hit {
-			rl.DrawSphereWires(collision.point, 1, 8, 8, rl.RED)
-		}
 	}
 
 	{
@@ -484,9 +532,19 @@ draw :: proc() {
 		if g_mem.editor {
 			rl.DrawText("Editor", 5, 5, 8, rl.ORANGE)
 
-			if collision.hit {
+			rl.DrawText(
-				rl.DrawText(fmt.ctprintf("Segment: %v", segment_idx), 5, 32, 8, rl.ORANGE)
+				fmt.ctprintf(
-			}
+					"mesh: %v, tri: %v, bary: %v, idx: %v",
+					hit_mesh_idx,
+					mesh_col.prim,
+					mesh_col.bary,
+					g_mem.preview_bvh,
+				),
+				5,
+				32,
+				8,
+				rl.ORANGE,
+			)
 
 			switch g_mem.es.track_edit_state {
 			case .Select:

game/physics/bvh/bvh.odin

@@ -0,0 +1,382 @@
+package bvh
+
+import "../collision"
+import "base:runtime"
+import "core:container/queue"
+import "core:log"
+import "core:math"
+import lg "core:math/linalg"
+import "core:mem"
+import rl "vendor:raylib"
+
+_ :: log
+_ :: rl
+_ :: lg
+
+Vec3 :: [3]f32
+
+AABB :: struct {
+	min, max: Vec3,
+}
+
+// Helper struct to avoid passing verts/indices separately
+Mesh :: struct {
+	vertices: []Vec3,
+	indices:  []u16,
+}
+
+BVH :: struct {
+	nodes:      []Node,
+	// Triangle IDs. first_index = indices[primitive * 3]
+	primitives: []u16,
+	nodes_used: i32,
+}
+
+destroy_bvh :: proc(bvh: ^BVH) {
+	delete(bvh.nodes)
+	delete(bvh.primitives)
+}
+
+// Helper struct to store mesh data together with its bvh for convenience
+// You don't have to use it
+Mesh_BVH :: struct {
+	bvh:  BVH,
+	mesh: Mesh,
+}
+
+Node :: struct {
+	aabb:                AABB,
+	// Index of the left child, right child is left_child + 1
+	child_or_prim_start: i32,
+	prim_len:            i32,
+}
+
+// uvw
+Bary :: [3]f32
+
+is_leaf_node :: #force_inline proc(node: Node) -> bool {
+	return node.prim_len > 0
+}
+
+#assert(size_of(Node) == 32)
+
+build_bvh_from_mesh :: proc(mesh: Mesh, allocator := context.allocator) -> (mesh_bvh: Mesh_BVH) {
+	vertices, indices := mesh.vertices, mesh.indices
+	assert(len(indices) % 3 == 0)
+
+	bvh := &mesh_bvh.bvh
+
+	num_triangles := len(indices) / 3
+
+	// Caller owned, allocator might be temp_allocator so do this before checkpoint below, otherwise we the result accidentally
+	bvh.nodes, _ = mem.make_aligned([]Node, num_triangles * 2 - 1, size_of(Node), allocator)
+	bvh.primitives = make([]u16, num_triangles, allocator)
+
+	// Clean up after ourselves
+	temp := runtime.default_temp_allocator_temp_begin()
+	defer runtime.default_temp_allocator_temp_end(temp)
+
+	// Temp stuff
+	centroids := make([]Vec3, num_triangles, context.temp_allocator)
+	aabbs := make([]AABB, num_triangles, context.temp_allocator)
+
+	// Calculate centroids and aabbs
+	for i in 0 ..< num_triangles {
+		i1, i2, i3 := indices[i * 3], indices[i * 3 + 1], indices[i * 3 + 2]
+		v1, v2, v3 := vertices[i1], vertices[i2], vertices[i3]
+
+		centroids[i] = (v1 + v2 + v3) * 0.33333333333
+
+		aabbs[i].min = Vec3{min(v1.x, v2.x, v3.x), min(v1.y, v2.y, v3.y), min(v1.z, v2.z, v3.z)}
+		aabbs[i].max = Vec3{max(v1.x, v2.x, v3.x), max(v1.y, v2.y, v3.y), max(v1.z, v2.z, v3.z)}
+
+		size := aabbs[i].max - aabbs[i].min
+		assert(size.x >= 0)
+		assert(size.y >= 0)
+		assert(size.z >= 0)
+
+		bvh.primitives[i] = u16(i)
+	}
+
+	bvh.nodes_used = 1 // root
+
+	root := &bvh.nodes[0]
+	root.child_or_prim_start = 0
+	root.prim_len = i32(num_triangles)
+
+	update_node_bounds(bvh, 0, aabbs)
+	subdivide(bvh, 0, centroids, aabbs)
+
+	return
+}
+
+/// Useful for a top level accel structure
+build_bvh_from_aabbs :: proc(aabbs: []AABB, 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)
+
+	temp := runtime.default_temp_allocator_temp_begin()
+	defer runtime.default_temp_allocator_temp_end(temp)
+
+	// Temp stuff
+	centroids := make([]Vec3, len(aabbs), context.temp_allocator)
+
+	// Calculate centroids
+	for i in 0 ..< len(aabbs) {
+		centroids[i] = (aabbs[i].max + aabbs[i].min) * 0.5
+
+		bvh.primitives[i] = u16(i)
+	}
+
+	bvh.nodes_used = 1
+
+	root := &bvh.nodes[0]
+	root.prim_len = i32(len(aabbs))
+
+	update_node_bounds(&bvh, 0, aabbs)
+
+	subdivide(&bvh, 0, centroids, aabbs)
+
+	return
+}
+
+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.max = math.F32_MIN
+
+	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.y = min(node.aabb.min.y, prim_aabb.min.y)
+		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)
+	}
+
+	size := node.aabb.max - node.aabb.min
+	assert(size.x >= 0)
+	assert(size.y >= 0)
+	assert(size.z >= 0)
+}
+
+subdivide :: proc(bvh: ^BVH, node_idx: i32, centroids: []Vec3, aabbs: []AABB) {
+	node := &bvh.nodes[node_idx]
+
+	if node.prim_len <= 2 {
+		return
+	}
+
+	size := node.aabb.max - node.aabb.min
+
+	// Split along longest axis
+	largest_side := size.x
+	split_axis := 0
+	if size.y > largest_side {
+		split_axis = 1
+		largest_side = size.y
+	}
+	if size.z > largest_side {
+		split_axis = 2
+	}
+
+	split_pos := node.aabb.min[split_axis] + size[split_axis] * 0.5
+
+	// Partition
+	i := node.child_or_prim_start
+	j := i + node.prim_len - 1
+
+	for i <= j {
+		prim_i := bvh.primitives[i]
+		prim_j := bvh.primitives[j]
+		if centroids[prim_i][split_axis] < split_pos {
+			i += 1
+		} else {
+			bvh.primitives[i] = prim_j
+			bvh.primitives[j] = prim_i
+			j -= 1
+		}
+	}
+
+	left_count := i - node.child_or_prim_start
+	if left_count == 0 || left_count == node.prim_len {
+		return
+	}
+
+	left_child := bvh.nodes_used
+	right_child := bvh.nodes_used + 1
+	bvh.nodes_used += 2
+
+	prim_start := node.child_or_prim_start
+	node.child_or_prim_start = left_child
+
+	bvh.nodes[left_child] = {}
+	bvh.nodes[right_child] = {}
+
+	bvh.nodes[left_child].child_or_prim_start = prim_start
+	bvh.nodes[left_child].prim_len = left_count
+	bvh.nodes[right_child].child_or_prim_start = i
+	bvh.nodes[right_child].prim_len = node.prim_len - left_count
+	node.prim_len = 0
+
+	update_node_bounds(bvh, left_child, aabbs)
+	update_node_bounds(bvh, right_child, aabbs)
+
+	subdivide(bvh, left_child, centroids, aabbs)
+	subdivide(bvh, right_child, centroids, aabbs)
+}
+
+Ray :: struct {
+	origin, dir: Vec3,
+	dir_inv:     Vec3,
+}
+
+Collision :: struct {
+	hit:  bool,
+	t:    f32,
+	// which primitive we hit
+	prim: u16,
+	// Barycentric coords of the hit triangle
+	bary: Bary,
+}
+
+traverse_bvh_ray_mesh :: proc(bvh: ^BVH, mesh: Mesh, ray: Ray, out_collision: ^Collision) -> bool {
+	ray := ray
+	ray.dir_inv.x = 1.0 / ray.dir.x
+	ray.dir_inv.y = 1.0 / ray.dir.y
+	ray.dir_inv.z = 1.0 / ray.dir.z
+
+	if !out_collision.hit {
+		out_collision.t = math.F32_MAX
+	}
+
+	prev_t := out_collision.t
+
+	internal_traverse_bvh_ray_triangles(bvh, mesh, ray, out_collision)
+
+	return out_collision.hit && out_collision.t < prev_t
+}
+
+internal_traverse_bvh_ray_triangles :: proc(
+	bvh: ^BVH,
+	mesh: Mesh,
+	ray: Ray,
+	out_collision: ^Collision,
+) {
+	temp := runtime.default_temp_allocator_temp_begin()
+	defer runtime.default_temp_allocator_temp_end(temp)
+
+	nodes_to_process: queue.Queue(i32)
+	queue.init(&nodes_to_process, queue.DEFAULT_CAPACITY, context.temp_allocator)
+	queue.push_back(&nodes_to_process, 0)
+
+	for queue.len(nodes_to_process) > 0 {
+		node_idx := queue.pop_front(&nodes_to_process)
+		assert(node_idx < bvh.nodes_used)
+
+		node := &bvh.nodes[node_idx]
+
+		if !internal_ray_aabb_test(ray, node.aabb, out_collision.t) {
+			return
+		}
+
+		rl.DrawBoundingBox(
+			{min = node.aabb.min, max = node.aabb.max},
+			debug_int_to_color(node_idx),
+		)
+
+		if is_leaf_node(node^) {
+			for i in node.child_or_prim_start ..< node.child_or_prim_start + node.prim_len {
+				internal_ray_tri_test(ray, mesh, bvh.primitives[i], out_collision)
+			}
+		} else {
+			left_node := node.child_or_prim_start
+
+			queue.push_back_elems(&nodes_to_process, left_node, left_node + 1)
+		}
+	}
+}
+
+// https://tavianator.com/2022/ray_box_boundary.html
+internal_ray_aabb_test :: proc(ray: Ray, box: AABB, min_t: f32) -> bool {
+	_, ok := collision.intersect_ray_aabb(ray.origin, ray.dir, collision.Aabb{box.min, box.max})
+	return ok
+
+	// t1 := (box.min[0] - ray.origin[0]) * ray.dir_inv[0]
+	// t2 := (box.max[0] - ray.origin[0]) * ray.dir_inv[0]
+
+	// tmin := min(t1, t2)
+	// tmax := max(t1, t2)
+
+	// for i in 1 ..< 3 {
+	// 	t1 = (box.min[i] - ray.origin[i]) * ray.dir_inv[i]
+	// 	t2 = (box.max[i] - ray.origin[i]) * ray.dir_inv[i]
+
+	// 	tmin = max(tmin, min(t1, t2))
+	// 	tmax = min(tmax, max(t1, t2))
+	// }
+
+	// return tmax > max(tmin, 0.0)
+}
+
+// Möller–Trumbore intersection algorithm
+// https://jacco.ompf2.com/2022/04/13/how-to-build-a-bvh-part-1-basics/
+internal_ray_tri_test :: proc(ray: Ray, mesh: Mesh, tri: u16, col: ^Collision) {
+	i1, i2, i3 := mesh.indices[tri * 3], mesh.indices[tri * 3 + 1], mesh.indices[tri * 3 + 2]
+	v1, v2, v3 := mesh.vertices[i1], mesh.vertices[i2], mesh.vertices[i3]
+
+	t, _, barycentric, ok := collision.intersect_segment_triangle(
+		{ray.origin, ray.origin + ray.dir},
+		{v1, v2, v3},
+	)
+
+	if ok && t < col.t {
+		col.hit = true
+		col.t = t
+		col.prim = tri
+		col.bary = barycentric
+	}
+	// rl.DrawTriangle3D(v1, v2, v3, debug_int_to_color(i32(tri)))
+
+	// rl_col := rl.GetRayCollisionTriangle(rl.Ray{ray.origin, ray.dir}, v1, v2, v3)
+
+	// if rl_col.hit && rl_col.distance < col.t {
+	// 	col.hit = true
+	// 	col.t = lg.distance(ray.origin, rl_col.point)
+	// }
+
+	return
+
+	//e1, e2 := v2 - v1, v3 - v1
+
+	//h := lg.cross(ray.dir, e2)
+	//a := lg.dot(e1, h)
+
+	//// ray parallel to triangle
+	//if a > -0.0001 || a < 0.0001 {
+	//	return
+	//}
+
+	//f: f32 = 1.0 / a
+	//s := ray.origin - v1
+	//u := f * lg.dot(s, h)
+	//if u < 0 || u > 1 {
+	//	return
+	//}
+	//q := lg.cross(s, e1)
+	//v := f * lg.dot(ray.dir, q)
+	//if v < 0 || u + v > 1 {
+	//	return
+	//}
+
+	//t := f * lg.dot(e2, q)
+	//if t > 0.0001 && t < col.t {
+	//	col.hit = true
+	//	col.t = t
+	//	col.prim = tri
+	//	col.bary = Vec3{u, v, 0} // TODO: calc W
+	//}
+}

game/physics/bvh/debug.odin

@@ -0,0 +1,115 @@
+package bvh
+
+import "base:runtime"
+import "core:container/queue"
+import "core:fmt"
+import "core:log"
+import lg "core:math/linalg"
+import rl "vendor:raylib"
+import "vendor:raylib/rlgl"
+
+_ :: fmt
+_ :: log
+
+// Assuming rl.BeginMode3D was called before this
+debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, 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 + pos, node.aabb.max + pos},
+				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 {
+				tri := bvh.primitives[i]
+				i1, i2, i3 :=
+					mesh.indices[tri * 3], mesh.indices[tri * 3 + 1], mesh.indices[tri * 3 + 2]
+				v1, v2, v3 := mesh.vertices[i1], mesh.vertices[i2], mesh.vertices[i3]
+
+				centroid := (v1 + v2 + v3) * 0.33333333
+
+				aabb: AABB
+				aabb.min = Vec3 {
+					min(v1.x, v2.x, v3.x),
+					min(v1.y, v2.y, v3.y),
+					min(v1.z, v2.z, v3.z),
+				}
+				aabb.max = Vec3 {
+					max(v1.x, v2.x, v3.x),
+					max(v1.y, v2.y, v3.y),
+					max(v1.z, v2.z, v3.z),
+				}
+
+				size := lg.length(aabb.max - aabb.min)
+				rl.DrawTriangle3D(v1, v2, v3, debug_int_to_color(i32(tri) + 1))
+				rl.DrawBoundingBox(
+					rl.BoundingBox{aabb.min, aabb.max},
+					debug_int_to_color(i32(tri) + 2),
+				)
+
+				if size < 1 {
+					rl.DrawCubeWiresV(centroid, 0.05, debug_int_to_color(i32(tri) + 3))
+				}
+			}
+		}
+	}
+}
+
+debug_int_to_color :: proc(num: i32) -> (color: rl.Color) {
+	x := debug_hash(num)
+
+	color.r = u8(x % 256)
+	color.g = u8((x / 256) % 256)
+	color.b = u8((x / 256 / 256) % 256)
+	color.a = 255
+	return
+}
+
+debug_hash :: proc(num: i32) -> u32 {
+	x := cast(u32)num
+
+	x = ((x >> 16) ~ x) * 0x45d9f3b
+	x = ((x >> 16) ~ x) * 0x45d9f3b
+	x = (x >> 16) ~ x
+
+	return x
+}
+
+bvh_mesh_from_rl_mesh :: proc(mesh: rl.Mesh) -> Mesh {
+	return Mesh {
+		vertices = (cast([^]Vec3)mesh.vertices)[:mesh.vertexCount],
+		indices = mesh.indices[:mesh.triangleCount * 3],
+	}
+}

game/physics/collision/collision.odin

@@ -79,7 +79,6 @@ plane_from_point_normal :: proc(point: Vec3, normal: Vec3) -> Plane {
 }
 
 
-
 //////////////////////////////////////////////////////////////////////////////////
 // Distance to closest point
 //
@@ -119,7 +118,6 @@ squared_distance_segment :: proc(point, a, b: Vec3) -> f32 {
 }
 
 
-
 //////////////////////////////////////////////////////////////////////////////////
 // Closest point
 //
@@ -265,7 +263,6 @@ closest_point_triangle :: proc(point, a, b, c: Vec3) -> Vec3 {
 }
 
 
-
 //////////////////////////////////////////////////////////////////////////////////
 // Tests
 //
@@ -300,7 +297,10 @@ test_sphere_vs_halfspace :: proc(sphere: Sphere, plane: Plane) -> bool {
 
 test_box_vs_plane :: proc(box: Box, plane: Plane) -> bool {
 	// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
-    r := box.rad.x * abs(plane.normal.x) + box.rad.y * abs(plane.normal.y) + box.rad.z * abs(plane.normal.z)
+	r :=
+		box.rad.x * abs(plane.normal.x) +
+		box.rad.y * abs(plane.normal.y) +
+		box.rad.z * abs(plane.normal.z)
 	s := signed_distance_plane(box.pos, plane)
 	return abs(s) <= r
 }
@@ -361,7 +361,6 @@ test_point_polyhedron :: proc(pos: Vec3, planes: []Plane) -> bool {
 }
 
 
-
 //////////////////////////////////////////////////////////////////////////////////
 // Intersections
 //
@@ -413,7 +412,13 @@ intersect_moving_aabbs :: proc(a, b: Aabb, vel_a, vel_b: Vec3) -> (t: [2]f32, ok
 }
 
 // 'a' is static, 'b' is moving
-intersect_static_aabb_vs_moving_aabb :: proc(a, b: Aabb, vel_relative: Vec3) -> (t: [2]f32, ok: bool) {
+intersect_static_aabb_vs_moving_aabb :: proc(
+	a, b: Aabb,
+	vel_relative: Vec3,
+) -> (
+	t: [2]f32,
+	ok: bool,
+) {
 	// Exit early if ‘a’ and ‘b’ initially overlapping
 	if test_aabb_vs_aabb(a, b) {
 		return 0, true
@@ -445,7 +450,15 @@ intersect_static_aabb_vs_moving_aabb :: proc(a, b: Aabb, vel_relative: Vec3) ->
 
 // Intersect sphere s with movement vector v with plane p. If intersecting
 // return t t of collision and point at which sphere hits plane
-intersect_moving_sphere_vs_plane :: proc(sphere: Sphere, vel: Vec3, plane: Plane) -> (t: f32, point: Vec3, ok: bool) {
+intersect_moving_sphere_vs_plane :: proc(
+	sphere: Sphere,
+	vel: Vec3,
+	plane: Plane,
+) -> (
+	t: f32,
+	point: Vec3,
+	ok: bool,
+) {
 	// Compute distance of sphere center to plane
 	dist := dot(plane.normal, sphere.pos) - plane.dist
 	if abs(dist) <= sphere.rad {
@@ -487,7 +500,15 @@ intersect_ray_sphere :: proc(pos: Vec3, dir: Vec3, sphere: Sphere) -> (t: f32, o
 	return t, true
 }
 
-intersect_ray_aabb :: proc(pos: Vec3, dir: Vec3, aabb: Aabb, range: f32 = max(f32)) -> (t: [2]f32, ok: bool) {
+intersect_ray_aabb :: proc(
+	pos: Vec3,
+	dir: Vec3,
+	aabb: Aabb,
+	range: f32 = max(f32),
+) -> (
+	t: [2]f32,
+	ok: bool,
+) {
 	// https://tavianator.com/cgit/dimension.git/tree/libdimension/bvh/bvh.c#n196
 
 	// This is actually correct, even though it appears not to handle edge cases
@@ -502,12 +523,22 @@ intersect_ray_aabb :: proc(pos: Vec3, dir: Vec3, aabb: Aabb, range: f32 = max(f3
 	t1 := (aabb.min - pos) * inv_dir
 	t2 := (aabb.max - pos) * inv_dir
 
-    t = {max(min(t1.x, t2.x), min(t1.y, t2.y), min(t1.z, t2.z)), min(max(t1.x, t2.x), max(t1.y, t2.y), max(t1.z, t2.z))}
+	t = {
+		max(min(t1.x, t2.x), min(t1.y, t2.y), min(t1.z, t2.z)),
+		min(max(t1.x, t2.x), max(t1.y, t2.y), max(t1.z, t2.z)),
+	}
 
 	return t, t[1] >= max(0.0, t[0]) && t[0] < range
 }
 
-intersect_ray_polyhedron :: proc(pos, dir: Vec3, planes: []Plane, segment: [2]f32 = {0.0, max(f32)}) -> (t: [2]f32, ok: bool) {
+intersect_ray_polyhedron :: proc(
+	pos, dir: Vec3,
+	planes: []Plane,
+	segment: [2]f32 = {0.0, max(f32)},
+) -> (
+	t: [2]f32,
+	ok: bool,
+) {
 	t = segment
 	for plane in planes {
 		denom := dot(plane.normal, dir)
@@ -565,10 +596,6 @@ intersect_segment_triangle :: proc(
 	if t < 0 {
 		return
 	}
-    if t > denom {
-        // For segment; exclude this code line for a ray test
-        return
-    }
 
 	// Compute barycentric coordinate components and test if within bounds
 	e := cross(qp, ap)
@@ -590,7 +617,14 @@ intersect_segment_triangle :: proc(
 	return t, normal, barycentric, true
 }
 
-intersect_segment_plane :: proc(segment: [2]Vec3, plane: Plane) -> (t: f32, point: Vec3, ok: bool) {
+intersect_segment_plane :: proc(
+	segment: [2]Vec3,
+	plane: Plane,
+) -> (
+	t: f32,
+	point: Vec3,
+	ok: bool,
+) {
 	ab := segment[1] - segment[0]
 	t = (plane.dist - dot(plane.normal, segment[0])) / dot(plane.normal, ab)
 

game/physics/scene.odin

@@ -85,7 +85,7 @@ Body_Ptr :: #soa^#soa[]Body
 Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint
 
 _invalid_body: #soa[1]Body
-_invalid_body_slice: #soa[]Body
+_invalid_body_slice := _invalid_body[:]
 
 _invalid_suspension_constraint: #soa[1]Suspension_Constraint
 _invalid_suspension_constraint_slice := _invalid_suspension_constraint[:]
@@ -93,7 +93,7 @@ _invalid_suspension_constraint_slice := _invalid_suspension_constraint[:]
 /// Returns pointer to soa slice. NEVER STORE IT
 get_body :: proc(scene: ^Scene, handle: Body_Handle) -> Body_Ptr {
 	index := int(handle) - 1
-	if index < 0 {
+	if index < 0 || index >= len(scene.bodies_slice) {
 		return &_invalid_body_slice[0]
 	}