From 860f9f59b6cb1378e2b20118a6b538de4cc0d6b5 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 2 Mar 2025 21:14:12 +0400 Subject: [PATCH] Optimize edge separation test, use SOA structs in more places --- game/game.odin | 15 ++- game/halfedge/debug.odin | 14 +-- game/halfedge/halfedge.odin | 20 +++- game/physics/collision/convex.odin | 159 +++++++++++------------------ game/physics/debug.odin | 4 +- game/physics/scene.odin | 4 +- game/physics/simulation.odin | 68 +++++++----- 7 files changed, 133 insertions(+), 151 deletions(-) diff --git a/game/game.odin b/game/game.odin index a6ea559..9a91788 100644 --- a/game/game.odin +++ b/game/game.odin @@ -73,7 +73,7 @@ Car :: struct { } SOLVER_CONFIG :: physics.Solver_Config { - timestep = 1.0 / 60, + timestep = 1.0 / 120, gravity = rl.Vector3{0, -9.8, 0}, substreps_minus_one = 2 - 1, } @@ -259,7 +259,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { &runtime_world.solver_state, #hash("car", "fnv32a"), physics.Body_Config { - initial_pos = {0, 4, 0}, + initial_pos = {0, 4, -10}, initial_rot = linalg.QUATERNIONF32_IDENTITY, // initial_rot = linalg.quaternion_angle_axis( // math.RAD_PER_DEG * 180, @@ -277,18 +277,17 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { ) if true { - - for x in 0 ..< 10 { - for y in -3 ..< 100 { + for x in 0 ..< 20 { + for y in 0 ..< 10 { physics.immediate_body( &world.physics_scene, &runtime_world.solver_state, hash.fnv32a(slice.to_bytes([]int{(x | y << 8)})), physics.Body_Config { - initial_pos = {f32(x) * 3 - 10, 5 + f32(y) * 3, 0}, + initial_pos = {0, 0.5 + f32(y) * 1.1, f32(x) * 3 + 10}, initial_rot = linalg.QUATERNIONF32_IDENTITY, shape = physics.Shape_Box{size = 1}, - mass = 10, + mass = 1, }, ) } @@ -648,7 +647,7 @@ draw :: proc() { } } - // rl.DrawModel(car_model, physics.body_get_shape_pos(car_body), 1, rl.WHITE) + rl.DrawModel(car_model, physics.body_get_shape_pos(car_body), 1, rl.WHITE) } { diff --git a/game/halfedge/debug.odin b/game/halfedge/debug.odin index 91ae37f..c0fcce7 100644 --- a/game/halfedge/debug.odin +++ b/game/halfedge/debug.odin @@ -1,27 +1,15 @@ 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 { + for _, f in mesh.faces { it := iterator_face_edges(mesh, Face_Index(f)) - center: Vec3 - num_points := 0 - 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 f2a0692..9e43884 100644 --- a/game/halfedge/halfedge.odin +++ b/game/halfedge/halfedge.odin @@ -129,13 +129,29 @@ mesh_from_vertex_index_list :: proc( return mesh } -get_edge_points :: proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (a: Vec3, b: Vec3) { +get_edge_points :: #force_inline proc( + mesh: Half_Edge_Mesh, + edge: Half_Edge, +) -> ( + a: Vec3, + b: Vec3, +) { a = mesh.vertices[edge.origin].pos b = mesh.vertices[mesh.edges[edge.next].origin].pos return } -get_edge_direction_normalized :: proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (dir: Vec3) { +get_edge_direction :: #force_inline proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (dir: Vec3) { + a, b := get_edge_points(mesh, edge) + return b - a +} + +get_edge_direction_normalized :: #force_inline proc( + mesh: Half_Edge_Mesh, + edge: Half_Edge, +) -> ( + dir: Vec3, +) { a, b := get_edge_points(mesh, edge) return lg.normalize0(b - a) } diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index 3431d84..8327d85 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -1,5 +1,6 @@ package collision +import "core:container/bit_array" import "core:log" import "core:math" import lg "core:math/linalg" @@ -164,6 +165,33 @@ find_furthest_point_from_point :: proc(points: []Vec3, ref: Vec3) -> (p: Vec3) { return } +// See: Physics for Game Programmers: The Separating Axis Test between Convex Polyhedra +// https://gdcvault.com/play/1017646/Physics-for-Game-Programmers-The +is_minkowski_face :: #force_inline proc(bxa, dxc, a, b, c, d: Vec3) -> bool { + cba := lg.dot(c, bxa) + dba := lg.dot(d, bxa) + adc := lg.dot(a, dxc) + bdc := lg.dot(b, dxc) + + return cba * dba < 0 && adc * bdc < 0 && cba * bdc > 0 +} + +build_minkowski_face :: #force_inline proc( + mesh_a, mesh_b: Convex, + edge_a, edge_b: halfedge.Half_Edge, +) -> bool { + a := mesh_a.faces[edge_a.face].normal + b := mesh_a.faces[mesh_a.edges[edge_a.twin].face].normal + c := mesh_b.faces[edge_b.face].normal + d := mesh_b.faces[mesh_b.edges[edge_b.twin].face].normal + + bxa := halfedge.get_edge_direction(mesh_a, edge_a) + dxc := halfedge.get_edge_direction(mesh_b, edge_b) + + // Negate normals c and d because we're interested in minkowski difference instead of sum + return is_minkowski_face(bxa, dxc, a, b, -c, -d) +} + query_separation_edges :: proc( a, b: Convex, ) -> ( @@ -178,123 +206,56 @@ query_separation_edges :: proc( a_edge = -1 b_edge = -1 - step := 0 + 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) - if false { + calc_pair_index :: proc(a, b, a_len: int) -> int { + return (b * a_len) + a + } - separating_plane_p: Vec3 - success_step: int + 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 + } - Edge_Pair :: [2]halfedge.Edge_Index - checked_pairs := make_map_cap( - map[Edge_Pair]bool, - len(a.edges) * len(b.edges), - context.temp_allocator, - ) - - for edge_a, edge_a_idx in a.edges { - for edge_b, edge_b_idx in b.edges { - pair := Edge_Pair{halfedge.Edge_Index(edge_a_idx), halfedge.Edge_Index(edge_b_idx)} - if checked_pairs[pair] { - continue - } - - checked_pairs[pair] = true - if edge_a.twin >= 0 { - checked_pairs[{edge_a.twin, halfedge.Edge_Index(edge_b_idx)}] = true - } - if edge_b.twin >= 0 { - checked_pairs[{halfedge.Edge_Index(edge_a_idx), edge_b.twin}] = true - } - if edge_a.twin >= 0 && edge_b.twin >= 0 { - checked_pairs[{edge_a.twin, edge_b.twin}] = true - } - - edge_a_dir := halfedge.get_edge_direction_normalized(a, edge_a) - edge_b_dir := halfedge.get_edge_direction_normalized(b, edge_b) + // 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), + ) + if build_minkowski_face(a, b, edge_a, edge_b) { + edge_a1, edge_a2 := halfedge.get_edge_points(a, edge_a) + edge_b1, edge_b2 := halfedge.get_edge_points(b, edge_b) + edge_a_dir := edge_a2 - edge_a1 + edge_b_dir := edge_b2 - edge_b1 axis := lg.normalize0(lg.cross(edge_a_dir, edge_b_dir)) - if axis == 0 { continue } - - edge_a_origin, _ := halfedge.get_edge_points(a, edge_a) - if lg.dot(axis, edge_a_origin - a.center) < 0 { + if lg.dot(axis, edge_a1 - a.center) < 0 { axis = -axis } - plane_a := plane_from_point_normal(edge_a_origin, axis) - vert_a, _, _ := find_support_point(a, plane_a.normal) - vert_b, vert_b_idx, _ := find_support_point(b, -plane_a.normal) - // We found the support vert on mesh b, but now we need to find the - // best edge that includes that point - vert_b_edge: halfedge.Half_Edge - vert_b_edge_idx: halfedge.Edge_Index = -1 - { - min_b2_distance := max(f32) - it := halfedge.iterator_vertex_edges(b, vert_b_idx) - for edge, edge_idx in halfedge.iterate_next_vertex_edge(&it) { - _, vert_b2 := halfedge.get_edge_points(b, edge) + plane_a := plane_from_point_normal(edge_a1, axis) - distance_b2 := signed_distance_plane(vert_b2, plane_a) - if distance_b2 < min_b2_distance { - min_b2_distance = distance_b2 - vert_b_edge = edge - vert_b_edge_idx = edge_idx - } - } - - if vert_b_edge_idx < 0 { - continue - } - } - - distance_a := signed_distance_plane(vert_a.pos, plane_a) - if distance_a > 0 { - continue - } - distance_b := signed_distance_plane(vert_b.pos, plane_a) - vert_b_projected := vert_b.pos + plane_a.normal * -distance_b - - if step == -1 { - // a1, a2 := halfedge.get_edge_points(a, edge_a) - // edge_a_center := (a1 + a2) * 0.5 - a1, a2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(a), edge_a) - b1, b2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(b), vert_b_edge) - - rl.DrawLine3D(edge_a_origin, edge_a_origin + plane_a.normal, rl.BLUE) - rl.DrawLine3D(a1 + 0.1, a2 + 0.1, rl.ORANGE) - rl.DrawLine3D(b1 + 0.1, b2 + 0.1, rl.PURPLE) - - rl.DrawSphereWires(edge_a_origin, 0.1, 4, 4, rl.ORANGE) - rl.DrawSphereWires(vert_b.pos, 0.05, 4, 4, rl.BLUE) - rl.DrawSphereWires(vert_b_projected, 0.05, 4, 4, rl.BLUE) - rl.DrawLine3D(vert_b.pos, vert_b_projected, rl.VIOLET) - log.debugf("dist: %v", distance_b) - - { - // rl.BeginBlendMode(.ALPHA) - // defer rl.EndBlendMode() - debug_draw_plane(edge_a_origin, plane_a, rl.Color{0, 228, 48, 100}) - } - } - - if distance_b > separation { - separation = distance_b + distance := signed_distance_plane(edge_b1, plane_a) + if distance > separation { + separation = distance a_edge = halfedge.Edge_Index(edge_a_idx) - b_edge = vert_b_edge_idx + b_edge = halfedge.Edge_Index(edge_b_idx) separating_plane = plane_a - separating_plane_p = edge_a_origin - success_step = step } - - step += 1 } } - // log.debugf("step: %v", success_step) - // debug_draw_plane(separating_plane_p, separating_plane, rl.Color{228, 0, 48, 100}) } + return } diff --git a/game/physics/debug.odin b/game/physics/debug.odin index 2aada19..d640b92 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -108,8 +108,8 @@ draw_debug_scene :: proc(scene: ^Scene) { } } - if true { - for &contact, contact_idx in sim_state.contact_pairs[:sim_state.contact_pairs_len] { + if false { + for &contact, contact_idx in sim_state.contact_pairs { points_a := contact.manifold.points_a points_b := contact.manifold.points_b points_a_slice, points_b_slice := diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 714e481..43e1ddd 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -26,8 +26,7 @@ Sim_State :: struct { first_free_suspension_constraint_plus_one: i32, // Persistent stuff for simulation - contact_pairs: [MAX_CONTACTS]Contact_Pair, - contact_pairs_len: int, + contact_pairs: #soa[dynamic]Contact_Pair, convex_container: Convex_Container, } @@ -381,6 +380,7 @@ _get_first_free_body :: proc(sim_state: ^Sim_State) -> i32 { destry_sim_state :: proc(sim_state: ^Sim_State) { delete_soa(sim_state.bodies) delete_soa(sim_state.suspension_constraints) + delete_soa(sim_state.contact_pairs) convex_container_destroy(&sim_state.convex_container) } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 4be125e..fb7e7d1 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -10,6 +10,7 @@ import "libs:tracy" _ :: math _ :: fmt +_ :: slice Solver_Config :: struct { // Will automatically do fixed timestep @@ -229,9 +230,9 @@ simulate_step :: proc( ) { tracy.Zone() - body_states := make([]Body_Sim_State, len(sim_state.bodies), context.temp_allocator) + body_states := make_soa(#soa[]Body_Sim_State, len(sim_state.bodies), context.temp_allocator) - sim_state.contact_pairs_len = 0 + resize_soa(&sim_state.contact_pairs, 0) substeps := config.substreps_minus_one + 1 @@ -283,6 +284,16 @@ simulate_step :: proc( assert(body.alive) assert(body2.alive) + aabb1, aabb2 := body_get_aabb(body), body_get_aabb(body2) + + // TODO: extract common math functions into a sane place + if !collision.test_aabb_vs_aabb( + {min = aabb1.center - aabb1.extent, max = aabb1.center + aabb1.extent}, + {min = aabb2.center - aabb2.extent, max = aabb2.center + aabb2.extent}, + ) { + continue + } + m1, m2 := body_get_convex_shape_world(sim_state, body), body_get_convex_shape_world(sim_state, body2) @@ -291,7 +302,9 @@ simulate_step :: proc( raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2) if collision { - contact_pair := &sim_state.contact_pairs[sim_state.contact_pairs_len] + new_contact_idx := len(sim_state.contact_pairs) + resize_soa(&sim_state.contact_pairs, new_contact_idx + 1) + contact_pair := &sim_state.contact_pairs[new_contact_idx] contact_pair^ = Contact_Pair { a = Body_Handle(i + 1), b = Body_Handle(j + 1), @@ -301,7 +314,6 @@ simulate_step :: proc( prev_q_b = body2.q, manifold = raw_manifold, } - sim_state.contact_pairs_len += 1 manifold := &contact_pair.manifold // Convert manifold contact from world to local space @@ -350,7 +362,7 @@ simulate_step :: proc( { tracy.ZoneN("simulate_step::static_friction") - if false { + when false { context = context context.user_ptr = sim_state slice.sort_by( @@ -383,7 +395,7 @@ simulate_step :: proc( ) } - for &contact_pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] { + for &contact_pair in sim_state.contact_pairs { manifold := contact_pair.manifold body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b) @@ -428,7 +440,7 @@ simulate_step :: proc( p2, ) - STATIC_FRICTION :: 0.8 + STATIC_FRICTION :: 0.6 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 @@ -475,13 +487,21 @@ simulate_step :: proc( } } - solve_velocities(sim_state, body_states, inv_dt) + { + // Compute new linear and angular velocities + for _, i in sim_state.bodies_slice { + body := &sim_state.bodies_slice[i] + if body.alive { + body_solve_velocity(body, body_states[i].prev_x, body_states[i].prev_q, inv_dt) + } + } + } // Restituion { tracy.ZoneN("simulate_step::restitution") - for &pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] { + for &pair in sim_state.contact_pairs { i, j := int(pair.a) - 1, int(pair.b) - 1 manifold := &pair.manifold @@ -507,7 +527,7 @@ simulate_step :: proc( prev_v_normal := lg.dot(prev_v, manifold.normal) v_normal := lg.dot(v, manifold.normal) - RESTITUTION :: 1 + RESTITUTION :: 0 restitution := f32(RESTITUTION) @@ -537,7 +557,7 @@ simulate_step :: proc( if true { tracy.ZoneN("simulate_step::dynamic_friction") - for &pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] { + for &pair in sim_state.contact_pairs { manifold := &pair.manifold body1 := get_body(sim_state, pair.a) body2 := get_body(sim_state, pair.b) @@ -557,7 +577,7 @@ simulate_step :: proc( v_normal := lg.dot(manifold.normal, v) * manifold.normal v_tangent := v - v_normal - DYNAMIC_FRICTION :: 0.6 + DYNAMIC_FRICTION :: 0.3 v_tangent_len := lg.length(v_tangent) if v_tangent_len > 0 { v_tangent_norm := v_tangent / v_tangent_len @@ -624,7 +644,7 @@ simulate_step :: proc( pos = wheel_world_pos, ) - body_solve_velocity(body, body_state, inv_dt) + body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt) } // Drive forces @@ -640,7 +660,7 @@ simulate_step :: proc( -forward, wheel_world_pos, ) - body_solve_velocity(body, body_state, inv_dt) + body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt) } // Lateral friction @@ -656,7 +676,7 @@ simulate_step :: proc( v.applied_impulse.x = impulse apply_correction(body, corr, v.hit_point) - body_solve_velocity(body, body_state, inv_dt) + body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt) } } } @@ -665,19 +685,17 @@ simulate_step :: proc( } solve_velocities :: proc(scene: ^Sim_State, body_states: []Body_Sim_State, inv_dt: f32) { - // Compute new linear and angular velocities - for _, i in scene.bodies_slice { - body := &scene.bodies_slice[i] - if body.alive { - body_solve_velocity(body, body_states[i], inv_dt) - } - } } -body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State, inv_dt: f32) { - body.v = (body.x - state.prev_x) * inv_dt +body_solve_velocity :: #force_inline proc( + body: Body_Ptr, + prev_x: Vec3, + prev_q: Quat, + inv_dt: f32, +) { + body.v = (body.x - prev_x) * inv_dt - delta_q := body.q * lg.quaternion_inverse(state.prev_q) + delta_q := body.q * lg.quaternion_inverse(prev_q) body.w = Vec3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt if delta_q.w < 0 {