From f2f23ee2e0472153c8945b94f3f4406b05cba8aa Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Thu, 23 Jan 2025 02:16:12 +0400 Subject: [PATCH] Proper stable collisions Turns out when you get a collision pair YOU SHOULD HANDLE CONTACT RESPONSE FOR BOTH BODIES, NOT JUST ONE OF THEM Otherwise they move and their next collision (if a->b, b->a are found and resolved separately) they might penetrate badly or worse and everything blows up This is probably not as important in sequential impulse type solvers because the actual position update is deferred, but in a position based one it's pretty important as it turns out. It's also better for performance because I don't have to run the collision query for the same two bodies two times --- game/game.odin | 15 ++++++----- game/physics/collision/convex.odin | 41 ++++++++++++++++++++++++------ game/physics/debug.odin | 8 +++--- game/physics/simulation.odin | 35 +++++++++++++++---------- 4 files changed, 66 insertions(+), 33 deletions(-) diff --git a/game/game.odin b/game/game.odin index 0690c95..5d672a4 100644 --- a/game/game.odin +++ b/game/game.odin @@ -79,7 +79,7 @@ Game_Memory :: struct { editor: bool, preview_bvh: int, preview_node: int, - draw_car: bool, + physics_pause: bool, } Track_Edit_State :: enum { @@ -398,7 +398,9 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { } - physics.simulate(&world.physics_scene, &runtime_world.solver_state, SOLVER_CONFIG, dt) + if !g_mem.physics_pause || rl.IsKeyPressed(.PERIOD) { + physics.simulate(&world.physics_scene, &runtime_world.solver_state, SOLVER_CONFIG, dt) + } } } @@ -441,12 +443,13 @@ update :: proc() { } if rl.IsKeyPressed(.SPACE) { - g_mem.draw_car = !g_mem.draw_car + g_mem.physics_pause = !g_mem.physics_pause } if g_mem.editor { update_editor(get_editor_state()) } else { + // update_free_look_camera(get_editor_state()) update_runtime_world(get_runtime_world(), dt) } } @@ -557,7 +560,7 @@ draw :: proc() { // rlgl_transform_scope(auto_cast linalg.matrix4_from_quaternion(rot2)) // rl.DrawCubeWiresV(box2.pos, box2.rad * 2, rl.RED) // } - for p in manifold.points[:manifold.points_len] { + for p in manifold.points_a[:manifold.points_len] { rl.DrawSphereWires(p, 0.05, 8, 8, rl.BLUE) } @@ -604,9 +607,7 @@ draw :: proc() { rl.WHITE, ) } else { - if g_mem.draw_car { - rl.DrawModel(car_model, 0, 1, rl.WHITE) - } + // rl.DrawModel(car_model, 0, 1, rl.WHITE) } { diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index b8634b9..9d0355e 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -42,7 +42,8 @@ box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Conv Contact_Manifold :: struct { normal: Vec3, separation: f32, - points: [4]Vec3, + points_a: [4]Vec3, + points_b: [4]Vec3, points_len: int, } @@ -421,7 +422,7 @@ create_face_contact_manifold :: proc( for i in 0 ..< vert_count { d := signed_distance_plane(src_polygon[i], ref_plane) - if d <= 0 { + if d <= 0.01 { clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal j += 1 } @@ -433,6 +434,8 @@ create_face_contact_manifold :: proc( // Resulting polygon before contact optimization full_clipped_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)][:vert_count] + ref_points: [4]Vec3 + // Contact optimization if len(full_clipped_polygon) > 4 { start_dir := lg.cross(ref_plane.normal, ref_face_vert) @@ -473,17 +476,33 @@ create_face_contact_manifold :: proc( } } - manifold.points[0] = first_point - manifold.points[1] = second_point - manifold.points[2] = third_point - manifold.points[3] = fourth_point + ref_points[0] = first_point + ref_points[1] = second_point + ref_points[2] = third_point + ref_points[3] = fourth_point manifold.points_len = 4 } else { - copy(manifold.points[:], full_clipped_polygon) + copy(ref_points[:], full_clipped_polygon) manifold.points_len = len(full_clipped_polygon) assert(len(full_clipped_polygon) <= 4) } + inc_face_vert := ref_convex.vertices[inc_convex.edges[inc_face.edge].origin].pos + inc_plane := plane_from_point_normal(inc_face_vert, inc_face.normal) + + inc_points: [4]Vec3 + for p, i in ref_points[:manifold.points_len] { + inc_points[i] = project_point_on_plane(p, inc_plane) + } + + if is_ref_a { + manifold.points_a = ref_points + manifold.points_b = inc_points + } else { + manifold.points_b = ref_points + manifold.points_a = inc_points + } + manifold.separation = ref_face_query.separation // Normal is always pointing from a to b manifold.normal = is_ref_a ? ref_face.normal : -ref_face.normal @@ -491,6 +510,11 @@ create_face_contact_manifold :: proc( return } +project_point_on_plane :: proc(point: Vec3, plane: Plane) -> Vec3 { + dist := signed_distance_plane(point, plane) + return point + plane.normal * -dist +} + create_edge_contact_manifold :: proc( a, b: Convex, separating_plane: Plane, @@ -506,7 +530,8 @@ create_edge_contact_manifold :: proc( manifold.normal = separating_plane.normal manifold.separation = lg.dot(ps[1] - ps[0], manifold.normal) - manifold.points[0] = (ps[0] + ps[1]) * 0.5 + manifold.points_a[0] = ps[0] + manifold.points_b[1] = ps[1] manifold.points_len = 1 return diff --git a/game/physics/debug.odin b/game/physics/debug.odin index 0be17a1..7efd889 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -103,19 +103,19 @@ draw_debug_scene :: proc(scene: ^Scene) { color := debug.int_to_color(i32(contact_idx)) if contact.manifold.points_len >= 3 { // Triangle or quad - v1 := contact.manifold.points[0] + v1 := contact.manifold.points_a[0] for i in 2 ..< contact.manifold.points_len { - v2, v3 := contact.manifold.points[i - 1], contact.manifold.points[i] + v2, v3 := contact.manifold.points_a[i - 1], contact.manifold.points_a[i] rl.DrawTriangle3D(v1, v2, v3, color) } } else if contact.manifold.points_len == 2 { // Line - rl.DrawLine3D(contact.manifold.points[0], contact.manifold.points[1], color) + rl.DrawLine3D(contact.manifold.points_a[0], contact.manifold.points_a[1], color) } - for p in contact.manifold.points[:contact.manifold.points_len] { + for p in contact.manifold.points_a[:contact.manifold.points_len] { rl.DrawSphereWires(p, 0.1, 4, 4, color) } } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 1a3051a..e6f15bc 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -116,13 +116,19 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { } } + + Body_Pair :: struct { + a, b: int, + } + handled_pairs := make_map(map[Body_Pair]bool, context.temp_allocator) + for _, i in scene.bodies { body := &scene.bodies_slice[i] if body.alive { for _, j in scene.bodies { body2 := &scene.bodies_slice[j] - if i != j && body2.alive { + if i != j && body2.alive && !handled_pairs[{a = min(i, j), b = max(i, j)}] { s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box) box1 := collision.box_to_convex( @@ -149,30 +155,31 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { manifold = manifold, } scene.contact_pairs_len += 1 - factor := 1.0 / f32(manifold.points_len) - for p in manifold.points[:manifold.points_len] { - // body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p) + for p in manifold.points_a[:manifold.points_len] { + body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p) body2_inv_mass := get_body_inverse_mass(body2, manifold.normal, p) + handled_pairs[{a = min(i, j), b = max(i, j)}] = true + apply_constraint_correction_unilateral( dt, body, 0, - -manifold.separation * factor, + -manifold.separation, -manifold.normal, p, body2_inv_mass, ) - // apply_constraint_correction_unilateral( - // dt, - // body2, - // 0, - // -manifold.separation, - // manifold.normal, - // p, - // body1_inv_mass, - // ) + apply_constraint_correction_unilateral( + dt, + body2, + 0, + -manifold.separation, + manifold.normal, + p, + body1_inv_mass, + ) } } }