gutter_runner/game/physics/bvh/debug.odin

package bvh

import "base:runtime"
import "core:container/queue"
import "core:fmt"
import "core:log"
import lg "core:math/linalg"
import "game:debug"
import "game:ui"
import rl "libs:raylib"
import "libs:raylib/rlgl"
import "libs:tracy"

_ :: fmt
_ :: log

@(private = "file")
Traversal :: struct {
	node_idx:    i32,
	should_draw: bool,
}

// Assuming rl.BeginMode3D was called before this
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)

	temp := runtime.default_temp_allocator_temp_begin()
	defer runtime.default_temp_allocator_temp_end(temp)

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

Iterator :: struct {
	bvh:              ^BVH,
	nodes_to_process: queue.Queue(i32),
}

iterator :: proc(bvh: ^BVH) -> (it: Iterator) {
	it.bvh = bvh
	queue.init(&it.nodes_to_process, queue.DEFAULT_CAPACITY, context.temp_allocator)

	if len(bvh.nodes) > 0 {
		queue.push_back(&it.nodes_to_process, 0)
	}

	return
}

iterator_next :: proc(it: ^Iterator) -> (node: Node, node_index: i32, ok: bool) {
	if queue.len(it.nodes_to_process) == 0 {
		return {}, -1, false
	}

	node_index = queue.pop_front(&it.nodes_to_process)
	node = it.bvh.nodes[node_index]
	if !is_leaf_node(node) {
		left_child := node.child_or_prim_start
		queue.push_back_elems(&it.nodes_to_process, left_child, left_child + 1)
	}
	ok = true
	return
}

debug_draw_bvh_bounds :: proc(bvh: ^BVH, primitive_bounds: []AABB, node_index: int) {
	tracy.Zone()

	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)

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

debug_ui_bvh_node_recursive :: proc(
	ctx: ^ui.Context,
	bvh: BVH,
	primitive_bounds: []AABB,
	node_idx: i32,
) {
	if .ACTIVE in ui.treenode(ctx, fmt.tprintf("Node %v", node_idx)) {
		node := bvh.nodes[node_idx]
		if !is_leaf_node(node) {
			debug_ui_bvh_node_recursive(ctx, bvh, primitive_bounds, node.child_or_prim_start)
			debug_ui_bvh_node_recursive(ctx, bvh, primitive_bounds, node.child_or_prim_start + 1)
		} else {
			for i in node.child_or_prim_start ..< node.child_or_prim_start + node.prim_len {
				prim := bvh.primitives[i]
				aabb := primitive_bounds[prim]

				ui.layout_row(ctx, {-1})
				ui.layout_next(ctx)
				ui.label(ctx, fmt.tprintf("Child: %v", aabb))
			}
		}
	}
}

debug_ui_bvh_tree :: proc(ctx: ^ui.Context, bvh: BVH, primitive_bounds: []AABB) {
	if .ACTIVE in ui.treenode(ctx, "BVH") {
		debug_ui_bvh_node_recursive(ctx, bvh, primitive_bounds, 0)
	}
}

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],
	}
}