From 503d6170c2771da5ced0444393a2862b031d03d5 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sat, 1 Mar 2025 19:04:01 +0400 Subject: [PATCH] Fix restitution and start implementing BVH acceleration for collision --- game/assets/assets.odin | 15 +++++--- game/game.odin | 24 +++++++------ game/halfedge/debug.odin | 21 +++++++++-- game/halfedge/halfedge.odin | 24 ++++++++++--- game/physics/bvh/bvh.odin | 28 ++++++++------- game/physics/bvh/debug.odin | 56 +++++++++++++++++++++++++++++- game/physics/collision/convex.odin | 7 ++-- game/physics/debug.odin | 2 +- game/physics/helpers.odin | 27 ++++++++++++++ game/physics/scene.odin | 14 ++++++++ game/physics/simulation.odin | 44 ++++++++++++++++++++--- 11 files changed, 216 insertions(+), 46 deletions(-) diff --git a/game/assets/assets.odin b/game/assets/assets.odin index 5ff4f4d..a93a611 100644 --- a/game/assets/assets.odin +++ b/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, - max = -math.F32_MAX, + min = max(f32), + 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 diff --git a/game/game.odin b/game/game.odin index a226e92..b4a5172 100644 --- a/game/game.odin +++ b/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( - &world.physics_scene, - &runtime_world.solver_state, - SOLVER_CONFIG, - dt, - commit = runtime_world.step_simulation, - step_mode = g_mem.physics_pause ? physics.Step_Mode.Single : physics.Step_Mode.Accumulated_Time, - ) + physics.simulate( + &world.physics_scene, + &runtime_world.solver_state, + SOLVER_CONFIG, + dt, + commit = runtime_world.step_simulation, + step_mode = g_mem.physics_pause ? physics.Step_Mode.Single : physics.Step_Mode.Accumulated_Time, + ) } } diff --git a/game/halfedge/debug.odin b/game/halfedge/debug.odin index 135c7a0..91ae37f 100644 --- a/game/halfedge/debug.odin +++ b/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 { - a, b := get_edge_points(mesh, edge) + center: Vec3 + 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))) } } diff --git a/game/halfedge/halfedge.odin b/game/halfedge/halfedge.odin index ca71fb3..f2a0692 100644 --- a/game/halfedge/halfedge.odin +++ b/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)) diff --git a/game/physics/bvh/bvh.odin b/game/physics/bvh/bvh.odin index 8ec1d10..51d426c 100644 --- a/game/physics/bvh/bvh.odin +++ b/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.max = -math.F32_MAX + node.aabb.min = max(f32) + 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.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) + node.aabb.min = lg.min(node.aabb.min, prim_aabb.min) + node.aabb.max = lg.max(node.aabb.max, prim_aabb.max) } } @@ -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 diff --git a/game/physics/bvh/debug.odin b/game/physics/bvh/debug.odin index aece4d1..40b4122 100644 --- a/game/physics/bvh/debug.odin +++ b/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], diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index 7df7246..7b1d1ad 100644 --- a/game/physics/collision/convex.odin +++ b/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_b_separation := face_query_b.separation + 0.1 + biased_face_a_separation := face_query_a.separation + biased_face_b_separation := face_query_b.separation biased_edge_separation := edge_separation is_face_a_contact := biased_face_a_separation >= biased_edge_separation diff --git a/game/physics/debug.odin b/game/physics/debug.odin index 9bed3da..2aada19 100644 --- a/game/physics/debug.odin +++ b/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 diff --git a/game/physics/helpers.odin b/game/physics/helpers.odin index 655d424..0183aa4 100644 --- a/game/physics/helpers.odin +++ 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 +} diff --git a/game/physics/scene.odin b/game/physics/scene.odin index b166e16..9c9d950 100644 --- a/game/physics/scene.odin +++ b/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 } } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index a8df5b5..3178048 100644 --- a/game/physics/simulation.odin +++ b/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(¤t_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