From 493f311ad067289ab27dad2eb1d6faafac1838c4 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Mon, 6 Jan 2025 03:40:40 +0400 Subject: [PATCH] Almost working bvh --- build_debug.sh | 2 +- build_release.sh | 2 +- game/assets/assets.odin | 101 ++- game/game.odin | 82 ++- game/physics/bvh/bvh.odin | 382 +++++++++++ game/physics/bvh/debug.odin | 115 ++++ game/physics/collision/collision.odin | 952 +++++++++++++------------- game/physics/scene.odin | 4 +- 8 files changed, 1158 insertions(+), 482 deletions(-) create mode 100644 game/physics/bvh/bvh.odin create mode 100644 game/physics/bvh/debug.odin diff --git a/build_debug.sh b/build_debug.sh index 4488dca..87b88b2 100755 --- a/build_debug.sh +++ b/build_debug.sh @@ -1,3 +1,3 @@ #!/usr/bin/env bash -odin build main_release -collection:common=./common -collection:game=./game -out:game_debug.bin -strict-style -vet -debug +odin build main_release -collection:common=./common -collection:game=./game -out:game_debug.bin -strict-style -vet -debug diff --git a/build_release.sh b/build_release.sh index 447a8c1..cf5debe 100755 --- a/build_release.sh +++ b/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 diff --git a/game/assets/assets.odin b/game/assets/assets.odin index 42ab74a..6a572e3 100644 --- a/game/assets/assets.odin +++ b/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 { - modtime := rl.GetFileModTime(path) +get_model_ex :: proc( + 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 { - return existing.model + if ok && existing.modtime == new_modtime { + 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) } diff --git a/game/game.odin b/game/game.odin index f121689..ac6f35c 100644 --- a/game/game.odin +++ b/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( - interpolated_points, - rl.GetScreenToWorldRay(rl.GetMousePosition(), camera), - ) + // collision, segment_idx := raycast_spline_tube( + // interpolated_points, + // 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(fmt.ctprintf("Segment: %v", segment_idx), 5, 32, 8, rl.ORANGE) - } + rl.DrawText( + 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: diff --git a/game/physics/bvh/bvh.odin b/game/physics/bvh/bvh.odin new file mode 100644 index 0000000..853a391 --- /dev/null +++ b/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 + //} +} diff --git a/game/physics/bvh/debug.odin b/game/physics/bvh/debug.odin new file mode 100644 index 0000000..31c3dab --- /dev/null +++ b/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], + } +} diff --git a/game/physics/collision/collision.odin b/game/physics/collision/collision.odin index c5baa69..f89bd73 100644 --- a/game/physics/collision/collision.odin +++ b/game/physics/collision/collision.odin @@ -21,36 +21,36 @@ cross :: linalg.cross length2 :: linalg.vector_length2 Aabb :: struct { - min: Vec3, - max: Vec3, + min: Vec3, + max: Vec3, } // Infinitely small AABB_INVALID :: Aabb { - min = 1e20, - max = -1e20, + min = 1e20, + max = -1e20, } Sphere :: struct { - pos: Vec3, - rad: f32, + pos: Vec3, + rad: f32, } // Radius is the half size Box :: struct { - pos: Vec3, - rad: Vec3, + pos: Vec3, + rad: Vec3, } Plane :: struct { - normal: Vec3, - dist: f32, + normal: Vec3, + dist: f32, } Capsule :: struct { - a: Vec3, - b: Vec3, - rad: f32, + a: Vec3, + b: Vec3, + rad: f32, } // Same layout, slightly different meaning @@ -58,95 +58,93 @@ Cylinder :: distinct Capsule aabb_center :: proc(a: Aabb) -> Vec3 { - return (a.min + a.max) * 0.5 + return (a.min + a.max) * 0.5 } aabb_half_size :: proc(a: Aabb) -> Vec3 { - return (a.max - a.min) * 0.5 + return (a.max - a.min) * 0.5 } aabb_to_box :: proc(a: Aabb) -> Box { - center := aabb_center(a) - return {pos = center, rad = a.max - center} + center := aabb_center(a) + return {pos = center, rad = a.max - center} } box_to_aabb :: proc(a: Box) -> Aabb { - return {min = a.pos - a.rad, max = a.pos + a.rad} + return {min = a.pos - a.rad, max = a.pos + a.rad} } plane_from_point_normal :: proc(point: Vec3, normal: Vec3) -> Plane { - return {normal = normal, dist = dot(point, normal)} + return {normal = normal, dist = dot(point, normal)} } - ////////////////////////////////////////////////////////////////////////////////// // Distance to closest point // signed_distance_plane :: proc(point: Vec3, plane: Plane) -> f32 { - // If plane equation normalized (||p.n||==1) - return dot(point, plane.normal) - plane.dist - // If not normalized - // return (dot(plane.normal, point) - plane.dist) / Ddt(plane.normal, plane.normal); + // If plane equation normalized (||p.n||==1) + return dot(point, plane.normal) - plane.dist + // If not normalized + // return (dot(plane.normal, point) - plane.dist) / Ddt(plane.normal, plane.normal); } squared_distance_aabb :: proc(point: Vec3, aabb: Aabb) -> (dist: f32) { - for i in 0 ..< 3 { - // For each axis count any excess distance outside box extents - if point[i] < aabb.min[i] do dist += (aabb.min[i] - point[i]) * (aabb.min[i] - point[i]) - if point[i] > aabb.max[i] do dist += (point[i] - aabb.max[i]) * (point[i] - aabb.max[i]) - } - return dist + for i in 0 ..< 3 { + // For each axis count any excess distance outside box extents + if point[i] < aabb.min[i] do dist += (aabb.min[i] - point[i]) * (aabb.min[i] - point[i]) + if point[i] > aabb.max[i] do dist += (point[i] - aabb.max[i]) * (point[i] - aabb.max[i]) + } + return dist } // Returns the squared distance between point and segment ab squared_distance_segment :: proc(point, a, b: Vec3) -> f32 { - ab := b - a - ac := point - a - bc := point - b - e := dot(ac, ab) - // Handle cases where c projects outside ab - if e <= 0.0 { - return dot(ac, ac) - } - f := dot(ab, ab) - if e >= f { - return dot(bc, bc) - } - // Handle cases where c projects onto ab - return dot(ac, ac) - e * e / f + ab := b - a + ac := point - a + bc := point - b + e := dot(ac, ab) + // Handle cases where c projects outside ab + if e <= 0.0 { + return dot(ac, ac) + } + f := dot(ab, ab) + if e >= f { + return dot(bc, bc) + } + // Handle cases where c projects onto ab + return dot(ac, ac) - e * e / f } - ////////////////////////////////////////////////////////////////////////////////// // Closest point // closest_point_plane :: proc(point: Vec3, plane: Plane) -> Vec3 { - t := dot(plane.normal, point) - plane.dist - return point - t * plane.normal + t := dot(plane.normal, point) - plane.dist + return point - t * plane.normal } closest_point_aabb :: proc(point: Vec3, aabb: Aabb) -> Vec3 { - return { - clamp(point.x, aabb.min.x, aabb.max.x), - clamp(point.y, aabb.min.y, aabb.max.y), - clamp(point.z, aabb.min.z, aabb.max.z), - } + return { + clamp(point.x, aabb.min.x, aabb.max.x), + clamp(point.y, aabb.min.y, aabb.max.y), + clamp(point.z, aabb.min.z, aabb.max.z), + } } // Given segment ab and point c, computes closest point d on ab. // Also returns t for the position of d, d(t)=a+ t*(b - a) closest_point_segment :: proc(pos, a, b: Vec3) -> (t: f32, point: Vec3) { - ab := b - a - // Project pos onto ab, computing parameterized position d(t)=a+ t*(b – a) - t = dot(pos - a, ab) / dot(ab, ab) - t = clamp(t, 0, 1) - // Compute projected position from the clamped t - point = a + t * ab - return t, point + ab := b - a + // Project pos onto ab, computing parameterized position d(t)=a+ t*(b – a) + t = dot(pos - a, ab) / dot(ab, ab) + t = clamp(t, 0, 1) + // Compute projected position from the clamped t + point = a + t * ab + return t, point } // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and @@ -154,520 +152,556 @@ closest_point_segment :: proc(pos, a, b: Vec3) -> (t: f32, point: Vec3) { // distance between between S1(s) and S2(t) // TODO: [2]Vec3 closest_point_between_segments :: proc(p1, q1, p2, q2: Vec3) -> (t: [2]f32, points: [2]Vec3) { - d1 := q1 - p1 // Direction vector of segment S1 - d2 := q2 - p2 // Direction vector of segment S2 - r := p1 - p2 - a := dot(d1, d1) // Squared length of segment S1, always nonnegative - e := dot(d2, d2) // Squared length of segment S2, always nonnegative - f := dot(d2, r) + d1 := q1 - p1 // Direction vector of segment S1 + d2 := q2 - p2 // Direction vector of segment S2 + r := p1 - p2 + a := dot(d1, d1) // Squared length of segment S1, always nonnegative + e := dot(d2, d2) // Squared length of segment S2, always nonnegative + f := dot(d2, r) - EPS :: 1e-6 + EPS :: 1e-6 - // Check if either or both segments degenerate into points - if a <= EPS && e <= EPS { - // Both segments degenerate into points - t = 0 - points = {p1, p2} - return t, points - } - if a <= EPS { - // First segment degenerates into a point - t[0] = 0 - t[1] = clamp(f / e, 0, 1) // s = 0 => t = (b*s + f) / e = f / e - } else { - c := dot(d1, r) - if e <= EPS { - // Second segment degenerates into a point - t[1] = 0 - t[0] = clamp(-c / a, 0, 1) // t = 0 => s = (b*t - c) / a = -c / a - } else { - // The general nondegenerate case starts here - b := dot(d1, d2) - denom := a * e - b * b // Always nonnegative + // Check if either or both segments degenerate into points + if a <= EPS && e <= EPS { + // Both segments degenerate into points + t = 0 + points = {p1, p2} + return t, points + } + if a <= EPS { + // First segment degenerates into a point + t[0] = 0 + t[1] = clamp(f / e, 0, 1) // s = 0 => t = (b*s + f) / e = f / e + } else { + c := dot(d1, r) + if e <= EPS { + // Second segment degenerates into a point + t[1] = 0 + t[0] = clamp(-c / a, 0, 1) // t = 0 => s = (b*t - c) / a = -c / a + } else { + // The general nondegenerate case starts here + b := dot(d1, d2) + denom := a * e - b * b // Always nonnegative - // If segments not parallel, compute closest point on L1 to L2 and - // clamp to segment S1. Else pick arbitrary s (here 0) - if denom != 0.0 { - t[0] = clamp((b * f - c * e) / denom, 0, 1) - } else { - t[0] = 0 - } - // Compute point on L2 closest to S1(s) using - // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e - tnom := (b * t[0] + f) + // If segments not parallel, compute closest point on L1 to L2 and + // clamp to segment S1. Else pick arbitrary s (here 0) + if denom != 0.0 { + t[0] = clamp((b * f - c * e) / denom, 0, 1) + } else { + t[0] = 0 + } + // Compute point on L2 closest to S1(s) using + // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e + tnom := (b * t[0] + f) - // If t in [0,1] done. Else clamp t, recompute s for the new value - // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a - // and clamp s to [0, 1] - if tnom < 0 { - t[1] = 0 - t[0] = clamp(-c / a, 0, 1) - } else if tnom > 1 { - t[1] = 1 - t[0] = clamp((b - c) / a, 0, 1) - } else { - t[1] = tnom / e - } - } - } + // If t in [0,1] done. Else clamp t, recompute s for the new value + // of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a + // and clamp s to [0, 1] + if tnom < 0 { + t[1] = 0 + t[0] = clamp(-c / a, 0, 1) + } else if tnom > 1 { + t[1] = 1 + t[0] = clamp((b - c) / a, 0, 1) + } else { + t[1] = tnom / e + } + } + } - points[0] = p1 + d1 * t[0] - points[1] = p2 + d2 * t[1] - return t, points + points[0] = p1 + d1 * t[0] + points[1] = p2 + d2 * t[1] + return t, points } closest_point_triangle :: proc(point, a, b, c: Vec3) -> Vec3 { - ab := b - a - ac := c - a - ap := point - a - d1 := dot(ab, ap) - d2 := dot(ac, ap) - if d1 <= 0 && d2 <= 0 do return a // barycentric coordinates (1,0,0) + ab := b - a + ac := c - a + ap := point - a + d1 := dot(ab, ap) + d2 := dot(ac, ap) + if d1 <= 0 && d2 <= 0 do return a // barycentric coordinates (1,0,0) - // Check if P in vertex region outside B - bp := point - b - d3 := dot(ab, bp) - d4 := dot(ac, bp) - if d3 >= 0 && d4 <= d3 do return b // barycentric coordinates (0,1,0) + // Check if P in vertex region outside B + bp := point - b + d3 := dot(ab, bp) + d4 := dot(ac, bp) + if d3 >= 0 && d4 <= d3 do return b // barycentric coordinates (0,1,0) - // Check if P in edge region of AB, if so return projection of P onto AB - vc := d1 * d4 - d3 * d2 - if vc < 0 && d1 >= 0 && d3 <= 0 { - v := d1 / (d1 - d3) - return a + v * ab // barycentric coordinates (1-v,v,0) - } + // Check if P in edge region of AB, if so return projection of P onto AB + vc := d1 * d4 - d3 * d2 + if vc < 0 && d1 >= 0 && d3 <= 0 { + v := d1 / (d1 - d3) + return a + v * ab // barycentric coordinates (1-v,v,0) + } - // Check if P in vertex region outside C - cp := point - c - d5 := dot(ab, cp) - d6 := dot(ac, cp) - if d6 >= 0 && d5 <= d6 do return c // barycentric coordinates (0,0,1) + // Check if P in vertex region outside C + cp := point - c + d5 := dot(ab, cp) + d6 := dot(ac, cp) + if d6 >= 0 && d5 <= d6 do return c // barycentric coordinates (0,0,1) - // Check if P in edge region of AC, if so return projection of P onto AC - vb := d5 * d2 - d1 * d6 - if vb <= 0 && d2 >= 0 && d6 <= 0 { - w := d2 / (d2 - d6) - return a + w * ac // barycentric coordinates (1-w,0,w) - } + // Check if P in edge region of AC, if so return projection of P onto AC + vb := d5 * d2 - d1 * d6 + if vb <= 0 && d2 >= 0 && d6 <= 0 { + w := d2 / (d2 - d6) + return a + w * ac // barycentric coordinates (1-w,0,w) + } - // Check if P in edge region of BC, if so return projection of P onto BC - va := d3 * d6 - d5 * d4 - if va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0 { - w := (d4 - d3) / ((d4 - d3) + (d5 - d6)) - return b + w * (c - b) // barycentric coordinates (0,1-w,w) - } + // Check if P in edge region of BC, if so return projection of P onto BC + va := d3 * d6 - d5 * d4 + if va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0 { + w := (d4 - d3) / ((d4 - d3) + (d5 - d6)) + return b + w * (c - b) // barycentric coordinates (0,1-w,w) + } - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - denom := 1.0 / (va + vb + vc) - v := vb * denom - w := vc * denom - return a + ab * v + ac * w // = u*a + v*b + w*c, u = va * denom = 1.0f-v-w + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + denom := 1.0 / (va + vb + vc) + v := vb * denom + w := vc * denom + return a + ab * v + ac * w // = u*a + v*b + w*c, u = va * denom = 1.0f-v-w } - ////////////////////////////////////////////////////////////////////////////////// // Tests // test_aabb_vs_aabb :: proc(a, b: Aabb) -> bool { - // Exit with no intersection if separated along an axis - if a.max[0] < b.min[0] || a.min[0] > b.max[0] do return false - if a.max[1] < b.min[1] || a.min[1] > b.max[1] do return false - if a.max[2] < b.min[2] || a.min[2] > b.max[2] do return false - // Overlapping on all axes means AABBs are intersecting - return true + // Exit with no intersection if separated along an axis + if a.max[0] < b.min[0] || a.min[0] > b.max[0] do return false + if a.max[1] < b.min[1] || a.min[1] > b.max[1] do return false + if a.max[2] < b.min[2] || a.min[2] > b.max[2] do return false + // Overlapping on all axes means AABBs are intersecting + return true } test_sphere_vs_aabb :: proc(sphere: Sphere, aabb: Aabb) -> bool { - s := squared_distance_aabb(sphere.pos, aabb) - return s <= sphere.rad * sphere.rad + s := squared_distance_aabb(sphere.pos, aabb) + return s <= sphere.rad * sphere.rad } test_sphere_vs_plane :: proc(sphere: Sphere, plane: Plane) -> bool { - dist := signed_distance_plane(sphere.pos, plane) - return abs(dist) <= sphere.rad + dist := signed_distance_plane(sphere.pos, plane) + return abs(dist) <= sphere.rad } test_point_vs_halfspace :: proc(pos: Vec3, plane: Plane) -> bool { - return signed_distance_plane(pos, plane) <= 0.0 + return signed_distance_plane(pos, plane) <= 0.0 } test_sphere_vs_halfspace :: proc(sphere: Sphere, plane: Plane) -> bool { - dist := signed_distance_plane(sphere.pos, plane) - return dist <= sphere.rad + dist := signed_distance_plane(sphere.pos, plane) + return dist <= sphere.rad } 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) - s := signed_distance_plane(box.pos, plane) - return abs(s) <= r + // 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) + s := signed_distance_plane(box.pos, plane) + return abs(s) <= r } test_capsule_vs_capsule :: proc(a, b: Capsule) -> bool { - // Compute (squared) distance between the inner structures of the capsules - _, points := closest_point_between_segments(a.a, a.b, b.a, b.b) - squared_dist := length2(points[1] - points[0]) - // If (squared) distance smaller than (squared) sum of radii, they collide - rad := a.rad + b.rad - return squared_dist <= rad * rad + // Compute (squared) distance between the inner structures of the capsules + _, points := closest_point_between_segments(a.a, a.b, b.a, b.b) + squared_dist := length2(points[1] - points[0]) + // If (squared) distance smaller than (squared) sum of radii, they collide + rad := a.rad + b.rad + return squared_dist <= rad * rad } test_sphere_vs_capsule :: proc(sphere: Sphere, capsule: Capsule) -> bool { - // Compute (squared) distance between sphere center and capsule line segment - dist2 := squared_distance_segment(point = sphere.pos, a = capsule.a, b = capsule.b) - // If (squared) distance smaller than (squared) sum of radii, they collide - rad := sphere.rad + capsule.rad - return dist2 <= rad * rad + // Compute (squared) distance between sphere center and capsule line segment + dist2 := squared_distance_segment(point = sphere.pos, a = capsule.a, b = capsule.b) + // If (squared) distance smaller than (squared) sum of radii, they collide + rad := sphere.rad + capsule.rad + return dist2 <= rad * rad } test_capsule_vs_plane :: proc(capsule: Capsule, plane: Plane) -> bool { - adist := dot(capsule.a, plane.normal) - plane.dist - bdist := dot(capsule.b, plane.normal) - plane.dist - // Intersects if on different sides of plane (distances have different signs) - if adist * bdist < 0.0 do return true - // Intersects if start or end position within radius from plane - if abs(adist) <= capsule.rad || abs(bdist) <= capsule.rad do return true - return false + adist := dot(capsule.a, plane.normal) - plane.dist + bdist := dot(capsule.b, plane.normal) - plane.dist + // Intersects if on different sides of plane (distances have different signs) + if adist * bdist < 0.0 do return true + // Intersects if start or end position within radius from plane + if abs(adist) <= capsule.rad || abs(bdist) <= capsule.rad do return true + return false } test_capsule_vs_halfspace :: proc(capsule: Capsule, plane: Plane) -> bool { - adist := dot(capsule.a, plane.normal) - plane.dist - bdist := dot(capsule.b, plane.normal) - plane.dist - return min(adist, bdist) <= capsule.rad + adist := dot(capsule.a, plane.normal) - plane.dist + bdist := dot(capsule.b, plane.normal) - plane.dist + return min(adist, bdist) <= capsule.rad } test_ray_sphere :: proc(pos, dir: Vec3, sphere: Sphere) -> bool { - m := pos - sphere.pos - c := dot(m, m) - sphere.rad * sphere.rad - // If there is definitely at least one real root, there must be an intersection - if c <= 0 do return true - b := dot(m, dir) - // Early exit if ray origin outside sphere and ray pointing away from sphere - if b > 0 do return false - discr := b * b - c - // A negative discriminant corresponds to ray missing sphere - return discr >= 0 + m := pos - sphere.pos + c := dot(m, m) - sphere.rad * sphere.rad + // If there is definitely at least one real root, there must be an intersection + if c <= 0 do return true + b := dot(m, dir) + // Early exit if ray origin outside sphere and ray pointing away from sphere + if b > 0 do return false + discr := b * b - c + // A negative discriminant corresponds to ray missing sphere + return discr >= 0 } test_point_polyhedron :: proc(pos: Vec3, planes: []Plane) -> bool { - for plane in planes { - if signed_distance_plane(pos, plane) > 0.0 { - return false - } - } - return true + for plane in planes { + if signed_distance_plane(pos, plane) > 0.0 { + return false + } + } + return true } - ////////////////////////////////////////////////////////////////////////////////// // Intersections // // Given planes a and b, compute line L = p+t*d of their intersection. intersect_planes :: proc(a, b: Plane) -> (point, dir: Vec3, ok: bool) { - // Compute direction of intersection line - dir = cross(a.normal, b.normal) - // If d is (near) zero, the planes are parallel (and separated) - // or coincident, so they’re not considered intersecting - denom := dot(dir, dir) - EPS :: 1e-6 - if denom < EPS do return {}, dir, false - // Compute point on intersection line - point = cross(a.dist * b.normal - b.dist * a.normal, dir) / denom - return point, dir, true + // Compute direction of intersection line + dir = cross(a.normal, b.normal) + // If d is (near) zero, the planes are parallel (and separated) + // or coincident, so they’re not considered intersecting + denom := dot(dir, dir) + EPS :: 1e-6 + if denom < EPS do return {}, dir, false + // Compute point on intersection line + point = cross(a.dist * b.normal - b.dist * a.normal, dir) / denom + return point, dir, true } // TODO: moving vs static intersect_moving_spheres :: proc(a, b: Sphere, vel_a, vel_b: Vec3) -> (t: f32, ok: bool) { - s := b.pos - a.pos - v := vel_b - vel_a // Relative motion of s1 with respect to stationary s0 - r := a.rad + b.rad - c := dot(s, s) - r * r - if c < 0 { - // Spheres initially overlapping so exit directly - return 0, true - } - a := dot(v, v) - EPS :: 1e-6 - if a < EPS { - return 1, false // Spheres not moving relative each other - } - b := dot(v, s) - if b >= 0 { - return 1, false // Spheres not moving towards each other - } - d := b * b - a * c - if d < 0 { - return 1, false // No real-valued root, spheres do not intersect - } - t = (-b - sqrt(d)) / a - return t, true + s := b.pos - a.pos + v := vel_b - vel_a // Relative motion of s1 with respect to stationary s0 + r := a.rad + b.rad + c := dot(s, s) - r * r + if c < 0 { + // Spheres initially overlapping so exit directly + return 0, true + } + a := dot(v, v) + EPS :: 1e-6 + if a < EPS { + return 1, false // Spheres not moving relative each other + } + b := dot(v, s) + if b >= 0 { + return 1, false // Spheres not moving towards each other + } + d := b * b - a * c + if d < 0 { + return 1, false // No real-valued root, spheres do not intersect + } + t = (-b - sqrt(d)) / a + return t, true } intersect_moving_aabbs :: proc(a, b: Aabb, vel_a, vel_b: Vec3) -> (t: [2]f32, ok: bool) { - // Use relative velocity; effectively treating ’a’ as stationary - return intersect_static_aabb_vs_moving_aabb(a, b, vel_relative = vel_b - vel_a) + // Use relative velocity; effectively treating ’a’ as stationary + return intersect_static_aabb_vs_moving_aabb(a, b, vel_relative = vel_b - vel_a) } // 'a' is static, 'b' is moving -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 - } +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 + } - // Initialize ts of first and last contact - t = {0, 1} + // Initialize ts of first and last contact + t = {0, 1} - // For each axis, determine ts of first and last contact, if any - for i in 0 ..< 3 { - if vel_relative[i] < 0.0 { - if b.max[i] < a.min[i] do return 1, false // Nonintersecting and moving apart - if a.max[i] < b.min[i] do t[0] = max(t[0], (a.max[i] - b.min[i]) / vel_relative[i]) - if b.max[i] > a.min[i] do t[1] = min(t[1], (a.min[i] - b.max[i]) / vel_relative[i]) - } + // For each axis, determine ts of first and last contact, if any + for i in 0 ..< 3 { + if vel_relative[i] < 0.0 { + if b.max[i] < a.min[i] do return 1, false // Nonintersecting and moving apart + if a.max[i] < b.min[i] do t[0] = max(t[0], (a.max[i] - b.min[i]) / vel_relative[i]) + if b.max[i] > a.min[i] do t[1] = min(t[1], (a.min[i] - b.max[i]) / vel_relative[i]) + } - if vel_relative[i] > 0.0 { - if b.min[i] > a.max[i] do return 1, false // Nonintersecting and moving apart - if b.max[i] < a.min[i] do t[0] = max(t[0], (a.min[i] - b.max[i]) / vel_relative[i]) - if a.max[i] > b.min[i] do t[1] = min(t[1], (a.max[i] - b.min[i]) / vel_relative[i]) - } + if vel_relative[i] > 0.0 { + if b.min[i] > a.max[i] do return 1, false // Nonintersecting and moving apart + if b.max[i] < a.min[i] do t[0] = max(t[0], (a.min[i] - b.max[i]) / vel_relative[i]) + if a.max[i] > b.min[i] do t[1] = min(t[1], (a.max[i] - b.min[i]) / vel_relative[i]) + } - // No overlap possible if t of first contact occurs after t of last contact - if t[0] > t[1] do return 1, false - } + // No overlap possible if t of first contact occurs after t of last contact + if t[0] > t[1] do return 1, false + } - return t, true + return t, true } // 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) { - // Compute distance of sphere center to plane - dist := dot(plane.normal, sphere.pos) - plane.dist - if abs(dist) <= sphere.rad { - // The sphere is already overlapping the plane. Set t of - // intersection to zero and q to sphere center - return 0.0, sphere.pos, true - } +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 { + // The sphere is already overlapping the plane. Set t of + // intersection to zero and q to sphere center + return 0.0, sphere.pos, true + } - denom := dot(plane.normal, vel) - if (denom * dist >= 0.0) { - // No intersection as sphere moving parallel to or away from plane - return 1.0, sphere.pos, false - } + denom := dot(plane.normal, vel) + if (denom * dist >= 0.0) { + // No intersection as sphere moving parallel to or away from plane + return 1.0, sphere.pos, false + } - // Sphere is moving towards the plane + // Sphere is moving towards the plane - // Use +r in computations if sphere in front of plane, else -r - r := dist > 0.0 ? sphere.rad : -sphere.rad - t = (r - dist) / denom - point = sphere.pos + vel * t - r * plane.normal - return t, point, t <= 1.0 + // Use +r in computations if sphere in front of plane, else -r + r := dist > 0.0 ? sphere.rad : -sphere.rad + t = (r - dist) / denom + point = sphere.pos + vel * t - r * plane.normal + return t, point, t <= 1.0 } intersect_ray_sphere :: proc(pos: Vec3, dir: Vec3, sphere: Sphere) -> (t: f32, ok: bool) { - m := pos - sphere.pos - b := dot(m, dir) - c := dot(m, m) - sphere.rad * sphere.rad - // Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) - if c > 0 && b > 0 { - return 0, false - } - discr := b * b - c - // A negative discriminant corresponds to ray missing sphere - if discr < 0 do return 0, false - // Ray now found to intersect sphere, compute smallest t value of intersection - t = -b - sqrt(discr) - // If t is negative, ray started inside sphere so clamp t to zero - t = max(0, t) - return t, true + m := pos - sphere.pos + b := dot(m, dir) + c := dot(m, m) - sphere.rad * sphere.rad + // Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) + if c > 0 && b > 0 { + return 0, false + } + discr := b * b - c + // A negative discriminant corresponds to ray missing sphere + if discr < 0 do return 0, false + // Ray now found to intersect sphere, compute smallest t value of intersection + t = -b - sqrt(discr) + // If t is negative, ray started inside sphere so clamp t to zero + t = max(0, t) + return t, true } -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 +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 - // (dir.{x,y,z} == 0). It works because the infinities that result from - // dividing by zero will still behave correctly in the comparisons. Rays - // which are parallel to an axis and outside the box will have tmin == inf - // or tmax == -inf, while rays inside the box will have tmin and tmax - // unchanged. + // This is actually correct, even though it appears not to handle edge cases + // (dir.{x,y,z} == 0). It works because the infinities that result from + // dividing by zero will still behave correctly in the comparisons. Rays + // which are parallel to an axis and outside the box will have tmin == inf + // or tmax == -inf, while rays inside the box will have tmin and tmax + // unchanged. - inv_dir := 1.0 / dir + inv_dir := 1.0 / dir - t1 := (aabb.min - pos) * inv_dir - t2 := (aabb.max - pos) * inv_dir + 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 + 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) { - t = segment - for plane in planes { - denom := dot(plane.normal, dir) - dist := plane.dist - dot(plane.normal, pos) - // Test if segment runs parallel to the plane - if denom == 0.0 { - // If so, return “no intersection” if segment lies outside plane - if dist > 0.0 { - return 0, false - } - } else { - // Compute parameterized t value for intersection with current plane - tplane := dist / denom - if denom < 0.0 { - // When entering halfspace, update tfirst if t is larger - t[0] = max(t[0], tplane) - } else { - // When exiting halfspace, update tlast if t is smaller - t[1] = min(t[1], tplane) - } - if t[0] > t[1] { - return 0, false - } - } - } - return t, true +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) + dist := plane.dist - dot(plane.normal, pos) + // Test if segment runs parallel to the plane + if denom == 0.0 { + // If so, return “no intersection” if segment lies outside plane + if dist > 0.0 { + return 0, false + } + } else { + // Compute parameterized t value for intersection with current plane + tplane := dist / denom + if denom < 0.0 { + // When entering halfspace, update tfirst if t is larger + t[0] = max(t[0], tplane) + } else { + // When exiting halfspace, update tlast if t is smaller + t[1] = min(t[1], tplane) + } + if t[0] > t[1] { + return 0, false + } + } + } + return t, true } intersect_segment_triangle :: proc( - segment: [2]Vec3, - triangle: [3]Vec3, + segment: [2]Vec3, + triangle: [3]Vec3, ) -> ( - t: f32, - normal: Vec3, - barycentric: [3]f32, - ok: bool, + t: f32, + normal: Vec3, + barycentric: [3]f32, + ok: bool, ) { - ab := triangle[1] - triangle[0] - ac := triangle[2] - triangle[0] - qp := segment[0] - segment[1] + ab := triangle[1] - triangle[0] + ac := triangle[2] - triangle[0] + qp := segment[0] - segment[1] - normal = cross(ab, ac) + normal = cross(ab, ac) - denom := dot(qp, normal) - // If denom <= 0, segment is parallel to or points away from triangle - if denom <= 0 { - return 0, normal, 0, false - } + denom := dot(qp, normal) + // If denom <= 0, segment is parallel to or points away from triangle + if denom <= 0 { + return 0, normal, 0, false + } - // Compute intersection t value of pq with plane of triangle. A ray - // intersects if 0 <= t. Segment intersects iff 0 <= t <= 1. Delay - // dividing by d until intersection has been found to pierce triangle - ap := segment[0] - triangle[0] - t = dot(ap, normal) - if t < 0 { - return - } - if t > denom { - // For segment; exclude this code line for a ray test - return - } + // Compute intersection t value of pq with plane of triangle. A ray + // intersects if 0 <= t. Segment intersects iff 0 <= t <= 1. Delay + // dividing by d until intersection has been found to pierce triangle + ap := segment[0] - triangle[0] + t = dot(ap, normal) + if t < 0 { + return + } - // Compute barycentric coordinate components and test if within bounds - e := cross(qp, ap) - barycentric.y = dot(ac, e) - if barycentric.y < 0 || barycentric.y > denom { - return - } - barycentric.z = -dot(ab, e) - if barycentric.z < 0 || barycentric.y + barycentric.z > denom { - return - } + // Compute barycentric coordinate components and test if within bounds + e := cross(qp, ap) + barycentric.y = dot(ac, e) + if barycentric.y < 0 || barycentric.y > denom { + return + } + barycentric.z = -dot(ab, e) + if barycentric.z < 0 || barycentric.y + barycentric.z > denom { + return + } - // Segment/ray intersects triangle. Perform delayed division and - // compute the last barycentric coordinate component - ood := 1.0 / denom - t *= ood - barycentric.yz *= ood - barycentric.x = 1.0 - barycentric.y - barycentric.z - return t, normal, barycentric, true + // Segment/ray intersects triangle. Perform delayed division and + // compute the last barycentric coordinate component + ood := 1.0 / denom + t *= ood + barycentric.yz *= ood + barycentric.x = 1.0 - barycentric.y - barycentric.z + return t, normal, barycentric, true } -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) +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) - if t >= 0 && t <= 1 { - point = segment[0] + t * ab - return t, point, true - } + if t >= 0 && t <= 1 { + point = segment[0] + t * ab + return t, point, true + } - return t, segment[0], false + return t, segment[0], false } // TODO: alternative with capsule endcaps intersect_segment_cylinder :: proc(segment: [2]Vec3, cylinder: Cylinder) -> (t: f32, ok: bool) { - d := cylinder.b - cylinder.a - m := segment[0] - cylinder.a - n := segment[1] - segment[0] - md := dot(m, d) - nd := dot(n, d) - dd := dot(d, d) - // Test if segment fully outside either endcap of cylinder - if md < 0 && md + nd < 0 { - return 0, false // Segment outside ’a’ side of cylinder - } - if md > dd && md + nd > dd { - return 0, false // Segment outside ’b’ side of cylinder - } - nn := dot(n, n) - mn := dot(m, n) - a := dd * nn - nd * nd - k := dot(m, m) - cylinder.rad * cylinder.rad - c := dd * k - md * md - EPS :: 1e-6 - if abs(a) < EPS { - // Segment runs parallel to cylinder axis - if c > 0 { - return 0, false - } - // Now known that segment intersects cylinder; figure out how it intersects - if md < 0 { - // Intersect segment against ’a’ endcap - t = -mn / nn - } else if md > dd { - // Intersect segment against ’b’ endcap - t = (nd - mn) / nn - } else { - // ’a’ lies inside cylinder - t = 0 - } - return t, true - } - b := dd * mn - nd * md - discr := b * b - a * c - if discr < 0 { - return 0, false // no real roots - } - t = (-b - sqrt(discr)) / a - if t < 0 || t > 1 { - return 0, false // intersection outside segment - } - if md + t * nd < 0 { - // Intersection outside cylinder on ’a’ side - if nd <= 0 { - // Segment pointing away from endcap - return 0, false - } - t = -md / nd - ok = k + 2 * t * (mn + t * nn) <= 0 - return t, ok - } else if md + t * nd > dd { - // Intersection outside cylinder on ’b’ side - if nd >= 0 { - // Segment pointing away from endcap - return 0, false - } - t = (dd - md) / nd - ok = k + dd - 2 * md + t * (2 * (mn - nd) + t * nn) <= 0 - return t, ok - } - return t, true + d := cylinder.b - cylinder.a + m := segment[0] - cylinder.a + n := segment[1] - segment[0] + md := dot(m, d) + nd := dot(n, d) + dd := dot(d, d) + // Test if segment fully outside either endcap of cylinder + if md < 0 && md + nd < 0 { + return 0, false // Segment outside ’a’ side of cylinder + } + if md > dd && md + nd > dd { + return 0, false // Segment outside ’b’ side of cylinder + } + nn := dot(n, n) + mn := dot(m, n) + a := dd * nn - nd * nd + k := dot(m, m) - cylinder.rad * cylinder.rad + c := dd * k - md * md + EPS :: 1e-6 + if abs(a) < EPS { + // Segment runs parallel to cylinder axis + if c > 0 { + return 0, false + } + // Now known that segment intersects cylinder; figure out how it intersects + if md < 0 { + // Intersect segment against ’a’ endcap + t = -mn / nn + } else if md > dd { + // Intersect segment against ’b’ endcap + t = (nd - mn) / nn + } else { + // ’a’ lies inside cylinder + t = 0 + } + return t, true + } + b := dd * mn - nd * md + discr := b * b - a * c + if discr < 0 { + return 0, false // no real roots + } + t = (-b - sqrt(discr)) / a + if t < 0 || t > 1 { + return 0, false // intersection outside segment + } + if md + t * nd < 0 { + // Intersection outside cylinder on ’a’ side + if nd <= 0 { + // Segment pointing away from endcap + return 0, false + } + t = -md / nd + ok = k + 2 * t * (mn + t * nn) <= 0 + return t, ok + } else if md + t * nd > dd { + // Intersection outside cylinder on ’b’ side + if nd >= 0 { + // Segment pointing away from endcap + return 0, false + } + t = (dd - md) / nd + ok = k + dd - 2 * md + t * (2 * (mn - nd) + t * nn) <= 0 + return t, ok + } + return t, true } diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 8598a9c..687ac18 100644 --- a/game/physics/scene.odin +++ b/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] }