From 4f7a494fffa1fc9e7e40bd01a08a0b311b6931c0 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 19 Jan 2025 22:15:22 +0400 Subject: [PATCH] Interesting... collision response --- game/game.odin | 10 ++++ game/physics/collision/convex.odin | 32 +++++------ game/physics/simulation.odin | 89 +++++++++++++++--------------- 3 files changed, 71 insertions(+), 60 deletions(-) diff --git a/game/game.odin b/game/game.odin index 328d579..03d3cb6 100644 --- a/game/game.odin +++ b/game/game.odin @@ -232,6 +232,16 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { car_bounds := rl.GetModelBoundingBox(car_model) runtime_world.car_com = (car_bounds.min + car_bounds.max) / 2 + physics.immediate_body( + &world.physics_scene, + &runtime_world.solver_state, + #hash("floor", "fnv32a"), + physics.Body_Config { + initial_pos = {0, -0.5, 0}, + shape = physics.Shape_Box{size = {100, 1, 100}}, + }, + ) + runtime_world.car_handle = physics.immediate_body( &world.physics_scene, &runtime_world.solver_state, diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index e10c659..b2b7e74 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -41,6 +41,7 @@ box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Conv Contact_Manifold :: struct { normal: Vec3, + separation: f32, points: [4]Vec3, points_len: int, } @@ -60,18 +61,12 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli if edge_separation > 0 { return } - face_query_a.separation += 0.1 - edge_separation -= 0.1 + biased_face_a_separation := face_query_a.separation + 0.1 + biased_face_b_separation := face_query_b.separation + biased_edge_separation := edge_separation - 0.1 - is_face_a_contact := face_query_a.separation >= edge_separation - is_face_b_contact := face_query_b.separation >= edge_separation - - log.infof( - "face_a_sep: %v, face_b_sep: %v, edge_sep: %v", - face_query_a.separation, - face_query_b.separation, - edge_separation, - ) + is_face_a_contact := biased_face_a_separation >= biased_edge_separation + is_face_b_contact := biased_face_b_separation >= biased_edge_separation collision = true if is_face_a_contact && is_face_b_contact { @@ -212,7 +207,9 @@ query_separation_edges :: proc( } } - assert(vert_b_edge_idx >= 0, "couldn't find the edge on convex B") + if vert_b_edge_idx < 0 { + continue + } } distance_a := signed_distance_plane(vert_a.pos, plane_a) @@ -304,7 +301,7 @@ create_face_contact_manifold :: proc( original_vert_count += 1 } - inc_polygon = make([]Vec3, original_vert_count * 2, context.temp_allocator) + inc_polygon = make([]Vec3, original_vert_count * 4, context.temp_allocator) halfedge.iterator_reset_edges(&it) @@ -480,7 +477,9 @@ create_face_contact_manifold :: proc( assert(len(full_clipped_polygon) <= 4) } - manifold.normal = ref_face.normal + 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 return } @@ -495,11 +494,10 @@ create_edge_contact_manifold :: proc( a1, a2 := halfedge.get_edge_points(a, a.edges[edge_a]) b1, b2 := halfedge.get_edge_points(b, b.edges[edge_b]) - rl.DrawLine3D(a1 + 0.1, a2 + 0.1, rl.ORANGE) - rl.DrawLine3D(b1 + 0.1, b2 + 0.1, rl.BLUE) - _, ps := closest_point_between_segments(a1, a2, b1, b2) + manifold.normal = lg.normalize0(ps[1] - ps[0]) + manifold.separation = separation manifold.points[0] = (ps[0] + ps[1]) * 0.5 manifold.points_len = 1 diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index ccad7f6..532d826 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -4,6 +4,7 @@ import "collision" import "core:fmt" import "core:math" import lg "core:math/linalg" +import "game:halfedge" import rl "vendor:raylib" _ :: math @@ -108,54 +109,56 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { for _, i in scene.bodies { body := &scene.bodies_slice[i] if body.alive { - normal := GLOBAL_PLANE.normal - penetration: f32 - hit_point: rl.Vector3 - hit := false + for _, j in scene.bodies { + body2 := &scene.bodies_slice[j] - switch s in body.shape { - case Shape_Box: - extent := s.size * 0.5 - local_plane := collision.Plane { - normal = body_world_to_local_vec(body, GLOBAL_PLANE.normal), - dist = -lg.dot(GLOBAL_PLANE.normal, body.x) - GLOBAL_PLANE.dist, - } + if body2.alive { + s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box) - penetration, hit = collision.test_box_vs_halfspace( - collision.Box{pos = 0, rad = extent}, - local_plane, - ) - if hit { - local_hit_point: rl.Vector3 - min_distance := f32(math.F32_MAX) - for corner in collision.BOX_CORNERS_NORM { - local_pos := extent * corner - dist := collision.signed_distance_plane(local_pos, local_plane) - if dist < min_distance { - min_distance = dist - local_hit_point = local_pos + box1 := collision.box_to_convex( + collision.Box{rad = s1.size * 0.5}, + context.temp_allocator, + ) + box2 := collision.box_to_convex( + collision.Box{rad = s2.size * 0.5}, + context.temp_allocator, + ) + + mat1 := lg.matrix4_translate(body.x) * lg.matrix4_from_quaternion(body.q) + mat2 := lg.matrix4_translate(body2.x) * lg.matrix4_from_quaternion(body2.q) + + halfedge.transform_mesh(&box1, mat1) + halfedge.transform_mesh(&box2, mat2) + + manifold, collision := collision.convex_vs_convex_sat(box1, box2) + + if collision { + for p in manifold.points[: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) + + apply_constraint_correction_unilateral( + dt, + body, + 0, + -manifold.separation, + manifold.normal, + p, + body2_inv_mass, + ) + + apply_constraint_correction_unilateral( + dt, + body2, + 0, + -manifold.separation, + -manifold.normal, + p, + body1_inv_mass, + ) } } - hit_point = body_local_to_world(body, local_hit_point) - penetration = -min(min_distance, 0) } - case Shape_Sphere: - penetration, hit = collision.test_sphere_vs_halfspace( - collision.Sphere{pos = body.x, rad = s.radius}, - GLOBAL_PLANE, - ) - } - - if hit { - apply_constraint_correction_unilateral( - dt, - body, - 0, - penetration, - normal, - hit_point, - 0, - ) } } }