From 0961b7551ac71d666ade875e690dbd5d3d749d6e Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 19 Jan 2025 21:44:42 +0400 Subject: [PATCH] Manifold contact optimization --- game/game.odin | 11 ++-- game/physics/collision/convex.odin | 89 ++++++++++++++++++++++++++---- 2 files changed, 82 insertions(+), 18 deletions(-) diff --git a/game/game.odin b/game/game.odin index 9020cb3..328d579 100644 --- a/game/game.odin +++ b/game/game.odin @@ -495,12 +495,11 @@ draw :: proc() { box1_mat := linalg.Matrix4f32(1) box1_mat = linalg.matrix4_rotate(45 * math.RAD_PER_DEG, rl.Vector3{0, 1, 0}) * box1_mat box2_mat := linalg.Matrix4f32(1) - box2_mat = linalg.matrix4_translate(rl.Vector3{0.5, 0.2, 0}) * box2_mat - // box2_mat = linalg.matrix4_rotate(45 * math.RAD_PER_DEG, rl.Vector3{0, 0, 1}) * box2_mat + box2_mat = linalg.matrix4_translate(rl.Vector3{0.0, 0.2, 0}) * box2_mat + box2_mat = linalg.matrix4_rotate(45 * math.RAD_PER_DEG, rl.Vector3{0, 0, 1}) * box2_mat // box2_mat = linalg.matrix4_rotate(f32(rl.GetTime()), rl.Vector3{0, -1, 0}) * box2_mat - box2_mat = linalg.matrix4_translate(rl.Vector3{0.5, 0.5, 0}) * box2_mat - box2_mat = linalg.matrix4_rotate(f32(rl.GetTime()), rl.Vector3{0, 1, 0}) * box2_mat - + box2_mat = linalg.matrix4_translate(rl.Vector3{0.0, 0, 0}) * box2_mat + box2_mat = linalg.matrix4_rotate(f32(rl.GetTime()) * 0.1, rl.Vector3{0, 1, 0}) * box2_mat box1, box2 := collision.Box { pos = 0, @@ -529,7 +528,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 { + for p in manifold.points[:manifold.points_len] { rl.DrawSphereWires(p, 0.05, 8, 8, rl.BLUE) } diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index f926608..e10c659 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -40,8 +40,9 @@ box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Conv } Contact_Manifold :: struct { - points: []Vec3, - normal: Vec3, + normal: Vec3, + points: [4]Vec3, + points_len: int, } convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, collision: bool) { @@ -109,8 +110,7 @@ query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) { return } -find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> Vec3 { - p: Vec3 +find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> (p: Vec3) { max_proj := min(f32) for vert in points { proj := lg.dot(vert, normal) @@ -120,7 +120,7 @@ find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> Vec3 { } } - return p + return } find_support_point :: proc( @@ -145,6 +145,18 @@ find_support_point :: proc( return } +find_furthest_point_from_point :: proc(points: []Vec3, ref: Vec3) -> (p: Vec3) { + max_dist_sq := min(f32) + for v in points { + dist_sq := lg.length2(v - ref) + if dist_sq > max_dist_sq { + p = v + max_dist_sq = dist_sq + } + } + return +} + query_separation_edges :: proc( a, b: Convex, ) -> ( @@ -393,20 +405,20 @@ create_face_contact_manifold :: proc( } } - // Final clipping, remove verts above ref face + ref_face_vert := ref_convex.vertices[ref_convex.edges[ref_face.edge].origin].pos + ref_plane := plane_from_point_normal(ref_face_vert, ref_face.normal) + + // Final clipping, remove verts above ref face and project those left onto the reference plane { src_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)] clipped_polygon := clip_bufs[clip_buf_idx] - ref_face_vert := ref_convex.vertices[ref_convex.edges[ref_face.edge].origin].pos - ref_plane := plane_from_point_normal(ref_face_vert, ref_face.normal) - j := 0 for i in 0 ..< vert_count { d := signed_distance_plane(src_polygon[i], ref_plane) if d <= 0 { - clipped_polygon[j] = src_polygon[i] + clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal j += 1 } } @@ -414,8 +426,61 @@ create_face_contact_manifold :: proc( clip_buf_idx = get_other_clip_buf(clip_buf_idx) } + // Resulting polygon before contact optimization + full_clipped_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)][:vert_count] + + // Contact optimization + if len(full_clipped_polygon) > 4 { + start_dir := lg.cross(ref_plane.normal, ref_face_vert) + + first_point := find_support_point_from_slice(full_clipped_polygon, start_dir) + second_point := find_furthest_point_from_point(full_clipped_polygon, first_point) + + third_point: Vec3 + { + max_area := min(f32) + for v in full_clipped_polygon { + area := 0.5 * lg.dot(ref_plane.normal, lg.cross(first_point - v, second_point - v)) + if area > max_area { + max_area = area + third_point = v + } + } + } + + existing_edges := [][2]Vec3 { + {first_point, second_point}, + {second_point, third_point}, + {third_point, first_point}, + } + + fourth_point: Vec3 + { + // We're looking for max negative area actually + min_area := max(f32) + for v in full_clipped_polygon { + for edge in existing_edges { + area := 0.5 * lg.dot(ref_plane.normal, lg.cross(edge[0] - v, edge[1] - v)) + if area < min_area { + min_area = area + fourth_point = v + } + } + } + } + + manifold.points[0] = first_point + manifold.points[1] = second_point + manifold.points[2] = third_point + manifold.points[3] = fourth_point + manifold.points_len = 4 + } else { + copy(manifold.points[:], full_clipped_polygon) + manifold.points_len = len(full_clipped_polygon) + assert(len(full_clipped_polygon) <= 4) + } + manifold.normal = ref_face.normal - manifold.points = clip_bufs[get_other_clip_buf(clip_buf_idx)][:vert_count] return } @@ -435,8 +500,8 @@ create_edge_contact_manifold :: proc( _, ps := closest_point_between_segments(a1, a2, b1, b2) - manifold.points = make([]Vec3, 1, context.temp_allocator) manifold.points[0] = (ps[0] + ps[1]) * 0.5 + manifold.points_len = 1 return }