From 4af30979d5b1190b6c816c5a33bd6053e13c2661 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 19 Jan 2025 04:32:36 +0400 Subject: [PATCH] collision detection progress --- game/debug/helpers.odin | 24 ++++ game/game.odin | 31 ++++- game/halfedge/debug.odin | 10 ++ game/halfedge/halfedge.odin | 33 +++++ game/physics/collision/convex.odin | 202 ++++++++++++++++++++--------- game/physics/collision/debug.odin | 56 ++++++++ game/raylib_helpers.odin | 12 -- game/track.odin | 33 ++--- 8 files changed, 304 insertions(+), 97 deletions(-) create mode 100644 game/physics/collision/debug.odin delete mode 100644 game/raylib_helpers.odin diff --git a/game/debug/helpers.odin b/game/debug/helpers.odin index 6cbcd86..93b475a 100644 --- a/game/debug/helpers.odin +++ b/game/debug/helpers.odin @@ -1,6 +1,7 @@ package debug import rl "vendor:raylib" +import "vendor:raylib/rlgl" int_to_color :: proc(num: i32) -> (color: rl.Color) { x := int_hash(num) @@ -22,3 +23,26 @@ int_hash :: proc(num: i32) -> u32 { return x } +rlgl_color :: proc(color: rl.Color) { + rlgl.Color4ub(color.r, color.g, color.b, color.a) +} + +rlgl_color3v :: proc(v: rl.Vector3) { + rlgl.Color3f(v.r, v.g, v.b) +} + +rlgl_vertex3v :: proc(v: rl.Vector3) { + rlgl.Vertex3f(v.x, v.y, v.z) +} + +@(deferred_none = rlgl_transform_scope_end) +rlgl_transform_scope :: proc(mat: rl.Matrix) { + mat := mat + rlgl.PushMatrix() + + rlgl.MultMatrixf(cast([^]f32)&mat) +} + +rlgl_transform_scope_end :: proc() { + rlgl.PopMatrix() +} diff --git a/game/game.odin b/game/game.odin index 8c0a3a5..389d8b3 100644 --- a/game/game.odin +++ b/game/game.odin @@ -20,6 +20,7 @@ import "core:fmt" import "core:log" import "core:math" import "core:math/linalg" +import "game:halfedge" import "game:physics" import "game:physics/bvh" import "game:physics/collision" @@ -491,23 +492,43 @@ draw :: proc() { physics.draw_debug_scene(&world.physics_scene) + 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.5, 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{1.35, -0.5, 0}) * box2_mat + + box1, box2 := collision.Box { - pos = {0, 0.5, 0}, + pos = 0, rad = 0.5, }, collision.Box { - pos = {0.5, 1.5, 0.5}, + pos = 0, rad = 0.5, } box1_convex := collision.box_to_convex(box1, context.temp_allocator) box2_convex := collision.box_to_convex(box2, context.temp_allocator) + halfedge.transform_mesh(&box1_convex, box1_mat) + halfedge.transform_mesh(&box2_convex, box2_mat) + manifold, _ := collision.convex_vs_convex_sat(box1_convex, box2_convex) - rl.DrawCubeWiresV(box1.pos, box1.rad * 1.999, rl.RED) - rl.DrawCubeWiresV(box2.pos, box2.rad * 1.999, rl.RED) + halfedge.debug_draw_mesh_wires(halfedge.Half_Edge_Mesh(box1_convex), rl.RED) + halfedge.debug_draw_mesh_wires(halfedge.Half_Edge_Mesh(box2_convex), rl.RED) + + // { + // rlgl_transform_scope(auto_cast linalg.matrix4_from_quaternion(rot1)) + // rl.DrawCubeWiresV(box1.pos, box1.rad * 2, rl.RED) + // } + // { + // rlgl_transform_scope(auto_cast linalg.matrix4_from_quaternion(rot2)) + // rl.DrawCubeWiresV(box2.pos, box2.rad * 2, rl.RED) + // } for p in manifold.points { - rl.DrawSphereWires(p, 0.1, 8, 8, rl.BLUE) + rl.DrawSphereWires(p, 0.05, 8, 8, rl.BLUE) } // { diff --git a/game/halfedge/debug.odin b/game/halfedge/debug.odin index 084bcb1..135c7a0 100644 --- a/game/halfedge/debug.odin +++ b/game/halfedge/debug.odin @@ -1,2 +1,12 @@ package halfedge +import rl "vendor:raylib" + +debug_draw_mesh_wires :: proc(mesh: Half_Edge_Mesh, color: rl.Color) { + + for edge in mesh.edges { + a, b := get_edge_points(mesh, edge) + + rl.DrawLine3D(a, b, color) + } +} diff --git a/game/halfedge/halfedge.odin b/game/halfedge/halfedge.odin index 31e3087..82a1708 100644 --- a/game/halfedge/halfedge.odin +++ b/game/halfedge/halfedge.odin @@ -180,3 +180,36 @@ iterate_next_edge :: proc(it: ^Edge_Iterator) -> (edge: Half_Edge, ok: bool) { return } + +Vec4 :: [4]f32 + +transform_mesh :: proc(mesh: ^Half_Edge_Mesh, mat: lg.Matrix4f32) { + mesh_center_avg_factor := 1.0 / f32(len(mesh.vertices)) + new_center: Vec3 + for i in 0 ..< len(mesh.vertices) { + vert := &mesh.vertices[i] + p := vert.pos + vert.pos = (mat * Vec4{p.x, p.y, p.z, 1}).xyz + new_center += vert.pos * mesh_center_avg_factor + } + mesh.center = new_center + + for i in 0 ..< len(mesh.faces) { + face := &mesh.faces[i] + n := face.normal + face.normal = lg.normalize0((mat * Vec4{n.x, n.y, n.z, 0}).xyz) + } +} + +get_face_centroid :: proc(mesh: Half_Edge_Mesh, face_idx: Face_Index) -> Vec3 { + center: Vec3 + it := iterator_face_edges(mesh, face_idx) + num_verts := 0 + for edge in iterate_next_edge(&it) { + num_verts += 1 + center += mesh.vertices[edge.origin].pos + } + center /= f32(num_verts) + + return center +} diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index ca99cf0..a8b597f 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -1,5 +1,6 @@ package collision +import "core:log" import "core:math" import lg "core:math/linalg" import "game:halfedge" @@ -7,8 +8,11 @@ import rl "vendor:raylib" import "vendor:raylib/rlgl" _ :: math +_ :: rl +_ :: rlgl +_ :: log -Convex :: distinct halfedge.Half_Edge_Mesh +Convex :: halfedge.Half_Edge_Mesh BOX_CORNERS_NORM :: [8]Vec3 { {-1, 1, 1}, @@ -55,15 +59,22 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli if edge_separation > 0 { return } - edge_separation = -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, + ) + collision = true if is_face_a_contact && is_face_b_contact { manifold = create_face_contact_manifold(face_query_a, a, face_query_b, b) } else { + manifold = create_edge_contact_manifold(a, b, edge_separation, edge_a, edge_b) } return @@ -86,7 +97,7 @@ query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) { plane := plane_from_point_normal(pos, normal) - distance := signed_distance_plane(support_point, plane) + distance := signed_distance_plane(support_point.pos, plane) if distance > result.separation { result.face = halfedge.Face_Index(f) result.separation = distance @@ -96,14 +107,28 @@ query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) { return } -find_support_point :: proc(convex: Convex, normal: Vec3) -> Vec3 { +find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> Vec3 { p: Vec3 max_proj := min(f32) + for vert in points { + proj := lg.dot(vert, normal) + if proj > max_proj { + max_proj = proj + p = vert + } + } + + return p +} + +find_support_point :: proc(convex: Convex, normal: Vec3) -> halfedge.Vertex { + p: halfedge.Vertex + max_proj := min(f32) for vert in convex.vertices { proj := lg.dot(vert.pos, normal) if proj > max_proj { max_proj = proj - p = vert.pos + p = vert } } @@ -121,34 +146,74 @@ query_separation_edges :: proc( a_edge = -1 b_edge = -1 + step := 0 + + separating_plane: Plane + 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 { - edge_a_dir := halfedge.get_edge_direction_normalized( - halfedge.Half_Edge_Mesh(a), - edge_a, - ) - edge_b_dir := halfedge.get_edge_direction_normalized( - halfedge.Half_Edge_Mesh(b), - edge_b, - ) + for edge_b in b.edges { + edge_a_dir := halfedge.get_edge_direction_normalized(a, edge_a) + edge_b_dir := halfedge.get_edge_direction_normalized(b, edge_b) - axis := lg.cross(edge_a_dir, edge_b_dir) + axis := lg.normalize0(lg.cross(edge_a_dir, edge_b_dir)) - edge_a_origin := a.vertices[edge_a.origin].pos + if axis == 0 { + continue + } + + + edge_a_origin, _ := halfedge.get_edge_points(a, edge_a) if lg.dot(axis, edge_a_origin - 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 := find_support_point(b, -plane_a.normal) - distance := signed_distance_plane(vert_b, plane_a) - if distance > separation { - separation = distance - a_edge = halfedge.Edge_Index(edge_a_idx) - b_edge = halfedge.Edge_Index(edge_b_idx) + 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), edge_b) + + 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 + a_edge = halfedge.Edge_Index(edge_a_idx) + b_edge = vert_b.edge + 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 } @@ -185,17 +250,17 @@ create_face_contact_manifold :: proc( } inc_polygon: []Vec3 + original_vert_count := 0 // Get incident face vertices { it := halfedge.iterator_face_edges(halfedge.Half_Edge_Mesh(inc_convex), inc_face_idx) - vert_count := 0 for _ in halfedge.iterate_next_edge(&it) { - vert_count += 1 + original_vert_count += 1 } - inc_polygon = make([]Vec3, vert_count, context.temp_allocator) + inc_polygon = make([]Vec3, original_vert_count * 2, context.temp_allocator) halfedge.iterator_reset_edges(&it) @@ -220,6 +285,7 @@ create_face_contact_manifold :: proc( } step := 0 + vert_count := original_vert_count { it := halfedge.iterator_face_edges( @@ -260,8 +326,8 @@ create_face_contact_manifold :: proc( // Actual clipping { j := 0 - for i in 0 ..< len(src_polygon) { - k := (i + 1) % len(src_polygon) + for i in 0 ..< vert_count { + k := (i + 1) % vert_count d1 := signed_distance_plane(src_polygon[i], clipping_plane) d2 := signed_distance_plane(src_polygon[k], clipping_plane) @@ -287,51 +353,59 @@ create_face_contact_manifold :: proc( j += 1 } } - - clipped_polygon = clipped_polygon[:j] - - if step == 3 { - for p in clipped_polygon { - rl.DrawSphereWires(p, 0.05, 4, 4, rl.GREEN) - } - - { - rl.BeginBlendMode(.ALPHA) - defer rl.EndBlendMode() - - rlgl.PushMatrix() - defer rlgl.PopMatrix() - - f := clipping_face.normal - r := lg.normalize0(lg.cross(f, rl.Vector3{0, 1, 0})) - u := lg.cross(r, f) - - // ps: [4]Vec3 = {{-1, -1, 0}, {1, -1, 0}, {1, 1, 0}, {-1, 1, 0}} - - rl.DrawLine3D(clipping_plane_center, clipping_plane_center + f, rl.BLUE) - rl.DrawLine3D(clipping_plane_center, clipping_plane_center + r, rl.RED) - rl.DrawLine3D(clipping_plane_center, clipping_plane_center + u, rl.GREEN) - // mat: rlgl.Matrix = auto_cast lg.matrix4_look_at_from_fru(0, f, r, u, false) - - // rlgl.LoadIdentity() - // rlgl.MultMatrixf(cast([^]f32)&mat) - - // col := rl.Color{0, 228, 48, 50} - // rl.DrawTriangle3D(ps[0], ps[1], ps[2], col) - // rl.DrawTriangle3D(ps[2], ps[3], ps[0], col) - } - - } + vert_count = j } - clip_bufs[clip_buf_idx] = clipped_polygon clip_buf_idx = get_other_clip_buf(clip_buf_idx) step += 1 } } + // Final clipping, remove verts above ref face + { + 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] + j += 1 + } + } + vert_count = j + clip_buf_idx = get_other_clip_buf(clip_buf_idx) + } + manifold.normal = ref_face.normal - manifold.points = clip_bufs[get_other_clip_buf(clip_buf_idx)] + manifold.points = clip_bufs[get_other_clip_buf(clip_buf_idx)][:vert_count] + + return +} + +create_edge_contact_manifold :: proc( + a, b: Convex, + separation: f32, + edge_a, edge_b: halfedge.Edge_Index, +) -> ( + manifold: Contact_Manifold, +) { + 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.points = make([]Vec3, 2, context.temp_allocator) + manifold.points[0] = ps[0] + manifold.points[1] = ps[1] return } diff --git a/game/physics/collision/debug.odin b/game/physics/collision/debug.odin new file mode 100644 index 0000000..4db0f15 --- /dev/null +++ b/game/physics/collision/debug.odin @@ -0,0 +1,56 @@ +package collision + +import lg "core:math/linalg" +import "game:debug" +import rl "vendor:raylib" +import "vendor:raylib/rlgl" + +debug_plane_verts := []Vec3{{-1, -1, 0}, {1, -1, 0}, {1, 1, 0}, {-1, 1, 0}} +debug_plane_indices := []u16{0, 1, 2, 2, 3, 0} + +debug_plane_mesh := rl.Mesh { + vertices = cast([^]f32)&debug_plane_verts[0], + indices = &debug_plane_indices[0], + vertexCount = i32(len(debug_plane_verts)), + triangleCount = i32(len(debug_plane_indices)) / 3, +} + +debug_draw_plane :: proc(center: Vec3, plane: Plane, color: rl.Color) { + assert(abs(signed_distance_plane(center, plane)) < 0.001, "point should be on a plane") + + { + rlgl.Begin(rlgl.TRIANGLES) + defer rlgl.End() + + debug.rlgl_color(color) + + up := Vec3{0, 1, 0} + if lg.dot(up, plane.normal) > 1.0 - 0.0001 { + up = Vec3{1, 0, 0} + } + + mat := rl.MatrixLookAt(0, plane.normal, up) + trans := rl.Matrix(1) + trans[3][0] = center.x + trans[3][1] = center.y + trans[3][2] = center.z + mat = mat * trans + + debug.rlgl_transform_scope(mat) + + for i in 0 ..< len(debug_plane_indices) / 3 { + i1, i2, i3 := + debug_plane_indices[i * 3 + 0], + debug_plane_indices[i * 3 + 1], + debug_plane_indices[i * 3 + 2] + v1, v2, v3 := debug_plane_verts[i1], debug_plane_verts[i2], debug_plane_verts[i3] + + debug.rlgl_vertex3v(v1) + debug.rlgl_vertex3v(v2) + debug.rlgl_vertex3v(v3) + debug.rlgl_vertex3v(v3) + debug.rlgl_vertex3v(v2) + debug.rlgl_vertex3v(v1) + } + } +} diff --git a/game/raylib_helpers.odin b/game/raylib_helpers.odin deleted file mode 100644 index 0e28725..0000000 --- a/game/raylib_helpers.odin +++ /dev/null @@ -1,12 +0,0 @@ -package game - -import rl "vendor:raylib" -import "vendor:raylib/rlgl" - -rlgl_color3v :: proc(v: rl.Vector3) { - rlgl.Color3f(v.r, v.g, v.b) -} - -rlgl_vertex3v :: proc(v: rl.Vector3) { - rlgl.Vertex3f(v.x, v.y, v.z) -} diff --git a/game/track.odin b/game/track.odin index 2c446be..07f5c0c 100644 --- a/game/track.odin +++ b/game/track.odin @@ -3,6 +3,7 @@ package game import "base:builtin" import "core:math" import lg "core:math/linalg" +import "game:debug" import rl "vendor:raylib" import "vendor:raylib/rlgl" @@ -268,20 +269,20 @@ debug_draw_spline :: proc(interpolated_points: #soa[]Interpolated_Point) { bitangent := lg.cross(tangent, normal) rlgl.Color4ub(255, 255, 255, 255) - rlgl_vertex3v(cur.pos) - rlgl_vertex3v(next.pos) + debug.rlgl_vertex3v(cur.pos) + debug.rlgl_vertex3v(next.pos) rlgl.Color4ub(255, 0, 0, 255) - rlgl_vertex3v(cur.pos) - rlgl_vertex3v(cur.pos + tangent) + debug.rlgl_vertex3v(cur.pos) + debug.rlgl_vertex3v(cur.pos + tangent) rlgl.Color4ub(0, 255, 0, 255) - rlgl_vertex3v(cur.pos) - rlgl_vertex3v(cur.pos + bitangent * ROAD_WIDTH) + debug.rlgl_vertex3v(cur.pos) + debug.rlgl_vertex3v(cur.pos + bitangent * ROAD_WIDTH) rlgl.Color4ub(0, 0, 255, 255) - rlgl_vertex3v(cur.pos) - rlgl_vertex3v(cur.pos + normal) + debug.rlgl_vertex3v(cur.pos) + debug.rlgl_vertex3v(cur.pos + normal) } } @@ -323,15 +324,15 @@ debug_draw_spline_mesh :: proc(interpolated_points: #soa[]Interpolated_Point) { p3 := next.pos + next_bitangent * u_t p4 := next.pos + next_bitangent * u_t2 - rlgl_color3v(normal * 0.5 + 0.5) - rlgl_vertex3v(p1) - rlgl_vertex3v(p2) - rlgl_vertex3v(p3) + debug.rlgl_color3v(normal * 0.5 + 0.5) + debug.rlgl_vertex3v(p1) + debug.rlgl_vertex3v(p2) + debug.rlgl_vertex3v(p3) - rlgl_color3v(next_normal * 0.5 + 0.5) - rlgl_vertex3v(p2) - rlgl_vertex3v(p4) - rlgl_vertex3v(p3) + debug.rlgl_color3v(next_normal * 0.5 + 0.5) + debug.rlgl_vertex3v(p2) + debug.rlgl_vertex3v(p4) + debug.rlgl_vertex3v(p3) } } }