From 44a401344f6f451c795f723132f0e3182ce13365 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 27 Jul 2025 15:33:00 +0400 Subject: [PATCH] Optimize edge collision checking by sorting so they always follow in pairs Fix suspension constraint force being weird --- game/assets/assets.odin | 3 -- game/assets/parsers.odin | 3 +- game/halfedge/halfedge.odin | 46 +++++++++++++++++++++++-- game/physics/collision/convex.odin | 38 ++++++++++++--------- game/physics/simulation.odin | 55 +++++++++++++++++++----------- 5 files changed, 102 insertions(+), 43 deletions(-) diff --git a/game/assets/assets.odin b/game/assets/assets.odin index d5a1d4e..f03b078 100644 --- a/game/assets/assets.odin +++ b/game/assets/assets.odin @@ -565,8 +565,6 @@ assetman_fetch_or_load_internal :: proc( modtime: physfs.sint64, result: Asset_Cache_Result, ) { - tracy.Zone() - existing, has_existing := assetman.assets[key] if has_existing { @@ -636,7 +634,6 @@ assetman_fetch_or_load :: proc( modtime: physfs.sint64, result: Asset_Cache_Result, ) { - tracy.Zone() value, modtime, result = assetman_fetch_or_load_internal( assetman, key, diff --git a/game/assets/parsers.odin b/game/assets/parsers.odin index 7bbac22..e6b767b 100644 --- a/game/assets/parsers.odin +++ b/game/assets/parsers.odin @@ -338,7 +338,6 @@ parse_convex :: proc(bytes: []byte, allocator := context.allocator) -> (Loaded_C final_edges := make([]halfedge.Half_Edge, len(edges), allocator) final_faces := make([]halfedge.Face, len(faces), allocator) copy(final_vertices, vertices[:]) - copy(final_edges, edges[:]) copy(final_faces, faces[:]) mesh := halfedge.Half_Edge_Mesh { @@ -349,6 +348,8 @@ parse_convex :: proc(bytes: []byte, allocator := context.allocator) -> (Loaded_C extent = extent, } + halfedge.sort_edges(mesh, edges[:]) + // Center of mass calculation total_volume := f32(0.0) { diff --git a/game/halfedge/halfedge.odin b/game/halfedge/halfedge.odin index 9834c5f..9a4c26e 100644 --- a/game/halfedge/halfedge.odin +++ b/game/halfedge/halfedge.odin @@ -51,11 +51,10 @@ mesh_from_vertex_index_list :: proc( mesh: Half_Edge_Mesh verts := make([]Vertex, len(vertices), allocator) faces := make([]Face, num_faces, allocator) - edges := make([]Half_Edge, len(indices), allocator) + edges := make([]Half_Edge, len(indices), context.temp_allocator) mesh.vertices = verts mesh.faces = faces - mesh.edges = edges min_pos, max_pos: Vec3 = max(f32), min(f32) @@ -128,9 +127,52 @@ mesh_from_vertex_index_list :: proc( } } + mesh.edges = make([]Half_Edge, len(edges), allocator) + + sort_edges(mesh, edges) + return mesh } +sort_edges :: proc(dst_mesh: Half_Edge_Mesh, unsorted_edges: []Half_Edge) { + assert(len(dst_mesh.edges) == len(unsorted_edges)) + + keys := make([]u32, len(unsorted_edges), context.temp_allocator) + + for &e, i in unsorted_edges { + v0 := e.origin + v1 := unsorted_edges[e.next].origin + + min_v := min(v0, v1) + max_v := max(v0, v1) + + keys[i] = (u32(max_v) << 16) | u32(min_v) + } + + sorted_edges_indices := slice.sort_with_indices(keys, context.temp_allocator) + + unsorted_to_sorted_lookup := make([]Edge_Index, len(unsorted_edges), context.temp_allocator) + for i in 0 ..< len(unsorted_edges) { + unsorted_to_sorted_lookup[sorted_edges_indices[i]] = Edge_Index(i) + } + + for i in 0 ..< len(unsorted_edges) { + sorted := &dst_mesh.edges[i] + sorted^ = unsorted_edges[sorted_edges_indices[i]] + sorted.twin = unsorted_to_sorted_lookup[sorted.twin] + sorted.next = unsorted_to_sorted_lookup[sorted.next] + sorted.prev = unsorted_to_sorted_lookup[sorted.prev] + } + + for &v in dst_mesh.vertices { + v.edge = unsorted_to_sorted_lookup[v.edge] + } + + for &f in dst_mesh.faces { + f.edge = unsorted_to_sorted_lookup[f.edge] + } +} + get_edge_points :: #force_inline proc( mesh: Half_Edge_Mesh, edge: Half_Edge, diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index f7d1ce2..c92819b 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -1,7 +1,6 @@ package collision import "base:runtime" -import "core:container/bit_array" import "core:log" import "core:math" import lg "core:math/linalg" @@ -274,29 +273,34 @@ query_separation_edges :: proc( runtime.DEFAULT_TEMP_ALLOCATOR_TEMP_GUARD() - checked_pairs: bit_array.Bit_Array - bit_array.init(&checked_pairs, len(a.edges) * len(b.edges), 0, context.temp_allocator) - a_len := len(a.edges) + // checked_pairs: bit_array.Bit_Array + // bit_array.init(&checked_pairs, len(a.edges) * len(b.edges), 0, context.temp_allocator) + // a_len := len(a.edges) calc_pair_index :: #force_inline proc(a, b, a_len: int) -> int { return (b * a_len) + a } - for edge_a, edge_a_idx in a.edges { - for edge_b, edge_b_idx in b.edges { - pair_idx := calc_pair_index(edge_a_idx, edge_b_idx, a_len) - if bit_array.get(&checked_pairs, pair_idx) { - continue - } + assert(len(a.edges) % 2 == 0) + assert(len(b.edges) % 2 == 0) + + for edge_a_idx := 0; edge_a_idx < len(a.edges); edge_a_idx += 2 { + edge_a := a.edges[edge_a_idx] + for edge_b_idx := 0; edge_b_idx < len(b.edges); edge_b_idx += 2 { + edge_b := b.edges[edge_b_idx] + // pair_idx := calc_pair_index(edge_a_idx, edge_b_idx, a_len) + // if bit_array.get(&checked_pairs, pair_idx) { + // continue + // } // TODO: sort edges so twins are next to each other, then can just iterate with step = 2 and skip this bitfield - bit_array.set(&checked_pairs, pair_idx) - bit_array.set(&checked_pairs, calc_pair_index(int(edge_a.twin), edge_b_idx, a_len)) - bit_array.set(&checked_pairs, calc_pair_index(edge_a_idx, int(edge_b.twin), a_len)) - bit_array.set( - &checked_pairs, - calc_pair_index(int(edge_a.twin), int(edge_b.twin), a_len), - ) + // bit_array.set(&checked_pairs, pair_idx) + // bit_array.set(&checked_pairs, calc_pair_index(int(edge_a.twin), edge_b_idx, a_len)) + // bit_array.set(&checked_pairs, calc_pair_index(edge_a_idx, int(edge_b.twin), a_len)) + // bit_array.set( + // &checked_pairs, + // calc_pair_index(int(edge_a.twin), int(edge_b.twin), a_len), + // ) if build_minkowski_face(a, b, edge_a, edge_b) { edge_a1, edge_a2 := halfedge.get_edge_points(a, edge_a) diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index b569c93..281a94b 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -148,7 +148,7 @@ build_dynamic_tlas :: proc( phys_aabb := body_get_aabb(body) - EXPAND_K :: 2 + EXPAND_K :: 10 expand := lg.abs(EXPAND_K * config.timestep * body.v) + 0.1 phys_aabb.extent += expand * 0.5 @@ -329,6 +329,9 @@ raycast :: proc( normal = normal2 } + // TODO: raycast_level and raycast_bodies should return a normalized vec + normal = lg.normalize0(normal) + return } @@ -472,6 +475,7 @@ find_new_contacts :: proc( sim_cache: ^Sim_Cache, static_tlas: ^Static_TLAS, dyn_tlas: ^Dynamic_TLAS, + config: Solver_Config, ) { tracy.Zone() @@ -503,6 +507,9 @@ find_new_contacts :: proc( shape_a_aabb := shape_get_aabb(shape_a^) shape_a_aabb = body_transform_shape_aabb(body, shape_a_aabb) + EXPAND_K :: 2 + expand := lg.abs(EXPAND_K * config.timestep * body.v) + 0.1 + shape_a_aabb.extent += expand * 0.5 shapes_b_it := shapes_iterator(sim_state, other_body.shape) @@ -598,16 +605,18 @@ find_new_contacts :: proc( ) tri := get_triangle(vertices, indices, tri_idx) prim_aabb := get_triangle_aabb(tri) + prim_aabb.min -= 0.1 + prim_aabb.max += 0.1 if bvh.test_aabb_vs_aabb( body_aabb_in_Level_geom_space, prim_aabb, ) { - tracy.ZoneN("body_vs_level_geom") + // tracy.ZoneN("body_vs_triangle", false) shapes_it := shapes_iterator(sim_state, body.shape) for shape in shapes_iterator_next(&shapes_it) { - tracy.ZoneN("body_shape_vs_level_geom") + // tracy.ZoneN("body_shape_vs_triangle") shape_idx := shapes_it.counter - 1 shape_aabb := shape_get_aabb(shape^) @@ -920,7 +929,7 @@ update_contacts :: proc(sim_state: ^Sim_State, sim_cache: ^Sim_Cache, static_tla } calculate_soft_constraint_params :: proc( - natural_freq, damping_ratio, dt: f32, + natural_freq, damping_ratio, dt: f64, ) -> ( bias_rate: f32, mass_coef: f32, @@ -930,9 +939,9 @@ calculate_soft_constraint_params :: proc( a1 := 2.0 * damping_ratio + omega * dt a2 := dt * omega * a1 a3 := 1.0 / (1.0 + a2) - bias_rate = omega / a1 - mass_coef = a2 * a3 - impulse_coef = a3 + bias_rate = f32(omega / a1) + mass_coef = f32(a2 * a3) + impulse_coef = f32(a3) return } @@ -944,7 +953,7 @@ pgs_solve_contacts :: proc( inv_dt: f32, apply_bias: bool, ) { - bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(40, 1.0, dt) + bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(30, 0.8, f64(dt)) if !apply_bias { mass_coef = 1 bias_rate = 0 @@ -1303,7 +1312,7 @@ pgs_solve_suspension :: proc( forward := wheel_get_forward_vec(body, v) right := wheel_get_right_vec(body, v) - w_normal := get_body_angular_inverse_mass(body, dir) + w_normal := get_body_angular_inverse_mass(body, -v.hit_normal) inv_w_normal := 1.0 / w_normal // Drive force @@ -1343,12 +1352,12 @@ pgs_solve_suspension :: proc( // Spring force { bias_coef, mass_coef, impulse_coef := calculate_soft_constraint_params( - v.natural_frequency, - v.damping, - dt, + f64(v.natural_frequency), + f64(v.damping), + f64(dt), ) - vel := lg.dot(body_velocity_at_point(body, wheel_world_pos), dir) + vel := lg.dot(body_velocity_at_point(body, v.hit_point), -v.hit_normal) x := v.hit_t separation := v.rest - x @@ -1357,7 +1366,11 @@ pgs_solve_suspension :: proc( impulse_coef * v.spring_impulse v.spring_impulse += incremental_impulse - apply_velocity_correction(body, incremental_impulse * dir, wheel_world_pos) + apply_velocity_correction( + body, + incremental_impulse * v.hit_normal, + v.hit_point, + ) } // Positive means spinning forward @@ -1584,11 +1597,7 @@ pgs_substep :: proc( forward := wheel_get_forward_vec(body, s) right := wheel_get_right_vec(body, s) - apply_velocity_correction( - body, - s.spring_impulse * body_local_to_world_vec(body, s.rel_dir), - p, - ) + apply_velocity_correction(body, s.spring_impulse * -s.hit_normal, hit_p) apply_velocity_correction(body, s.lateral_impulse * right, p) apply_velocity_correction(body, s.longitudinal_impulse * forward, hit_p) @@ -1663,7 +1672,13 @@ simulate_step :: proc( build_dynamic_tlas(sim_state, config, &sim_state.dynamic_tlas) remove_invalid_contacts(sim_state, sim_cache, sim_state.static_tlas, sim_state.dynamic_tlas) - find_new_contacts(sim_state, sim_cache, &sim_state.static_tlas, &sim_state.dynamic_tlas) + find_new_contacts( + sim_state, + sim_cache, + &sim_state.static_tlas, + &sim_state.dynamic_tlas, + config, + ) update_contacts(sim_state, sim_cache, &sim_state.static_tlas) Solver :: enum {