From 82470ca3c735d46445224cdbaa441eb2d8fef6e2 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 19 Jan 2025 18:16:52 +0400 Subject: [PATCH] Edge contacts and fix bug with face contacts --- game/game.odin | 6 ++- game/halfedge/halfedge.odin | 71 +++++++++++++++++++++++++++--- game/physics/collision/convex.odin | 65 ++++++++++++++++++++------- 3 files changed, 117 insertions(+), 25 deletions(-) diff --git a/game/game.odin b/game/game.odin index 389d8b3..9020cb3 100644 --- a/game/game.odin +++ b/game/game.odin @@ -495,9 +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_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(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 + box2_mat = linalg.matrix4_rotate(f32(rl.GetTime()), rl.Vector3{0, 1, 0}) * box2_mat box1, box2 := collision.Box { diff --git a/game/halfedge/halfedge.odin b/game/halfedge/halfedge.odin index 82a1708..44748bc 100644 --- a/game/halfedge/halfedge.odin +++ b/game/halfedge/halfedge.odin @@ -82,8 +82,11 @@ mesh_from_vertex_index_list :: proc( assert(abs(dist - point_dist) < 0.00001) } - if i == 0 { + if verts[index].edge == -1 { verts[index].edge = Edge_Index(e) + } + + if i == 0 { faces[f].edge = Edge_Index(e) } @@ -165,16 +168,23 @@ iterator_reset_edges :: proc(it: ^Edge_Iterator) { it.past_first = false } -iterate_next_edge :: proc(it: ^Edge_Iterator) -> (edge: Half_Edge, ok: bool) { +iterate_next_edge :: proc( + it: ^Edge_Iterator, +) -> ( + edge: Half_Edge, + edge_idx: Edge_Index, + ok: bool, +) { if it.current_edge == it.first_edge { if !it.past_first { it.past_first = true } else { - return {}, false + return {}, -1, false } } edge = it.mesh.edges[it.current_edge] + edge_idx = it.current_edge ok = true it.current_edge = edge.next @@ -195,9 +205,8 @@ transform_mesh :: proc(mesh: ^Half_Edge_Mesh, mat: lg.Matrix4f32) { 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) + n := &mesh.faces[i].normal + mesh.faces[i].normal = lg.normalize0((mat * Vec4{n.x, n.y, n.z, 0}).xyz) } } @@ -213,3 +222,53 @@ get_face_centroid :: proc(mesh: Half_Edge_Mesh, face_idx: Face_Index) -> Vec3 { return center } + +Vertex_Edge_Iterator :: struct { + mesh: Half_Edge_Mesh, + vert: Vertex_Index, + first_edge_idx: Edge_Index, + edge_idx: Edge_Index, + iteration: i32, +} + +// Iterates over all edges that have vert_idx as the origin +iterator_vertex_edges :: proc( + mesh: Half_Edge_Mesh, + vert_idx: Vertex_Index, +) -> ( + it: Vertex_Edge_Iterator, +) { + it.mesh = mesh + it.vert = vert_idx + it.first_edge_idx = mesh.vertices[vert_idx].edge + it.edge_idx = it.first_edge_idx + + return +} + +iterate_next_vertex_edge :: proc( + it: ^Vertex_Edge_Iterator, +) -> ( + edge: Half_Edge, + edge_idx: Edge_Index, + ok: bool, +) { + if it.edge_idx != -1 && (it.edge_idx != it.first_edge_idx || it.iteration == 0) { + edge = it.mesh.edges[it.edge_idx] + edge_idx = it.edge_idx + ok = true + + twin := edge.twin + if twin >= 0 { + it.edge_idx = it.mesh.edges[twin].next + } else { + it.edge_idx = -1 + } + + it.iteration += 1 + } else { + edge_idx = -1 + } + + return +} diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index a8b597f..f926608 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -59,9 +59,11 @@ 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 - is_face_a_contact := face_query_a.separation > edge_separation - is_face_b_contact := face_query_b.separation > edge_separation + 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", @@ -93,7 +95,7 @@ query_separation_face_directions :: proc(a, b: Convex) -> (result: Face_Query) { pos := a.vertices[index].pos normal := face.normal - support_point := find_support_point(b, -normal) + support_point, _, _ := find_support_point(b, -normal) plane := plane_from_point_normal(pos, normal) @@ -121,18 +123,26 @@ find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> Vec3 { return p } -find_support_point :: proc(convex: Convex, normal: Vec3) -> halfedge.Vertex { - p: halfedge.Vertex +find_support_point :: proc( + convex: Convex, + normal: Vec3, +) -> ( + vert: halfedge.Vertex, + idx: halfedge.Vertex_Index, + ok: bool, +) { max_proj := min(f32) - for vert in convex.vertices { - proj := lg.dot(vert.pos, normal) + for v, i in convex.vertices { + proj := lg.dot(v.pos, normal) if proj > max_proj { max_proj = proj - p = vert + vert = v + idx = halfedge.Vertex_Index(i) + ok = true } } - return p + return } query_separation_edges :: proc( @@ -169,8 +179,30 @@ query_separation_edges :: proc( 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) + 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) + + 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 + } + } + + assert(vert_b_edge_idx >= 0, "couldn't find the edge on convex B") + } + distance_a := signed_distance_plane(vert_a.pos, plane_a) if distance_a > 0 { continue @@ -182,7 +214,7 @@ query_separation_edges :: proc( // 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) + 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) @@ -203,7 +235,7 @@ query_separation_edges :: proc( if distance_b > separation { separation = distance_b a_edge = halfedge.Edge_Index(edge_a_idx) - b_edge = vert_b.edge + b_edge = vert_b_edge_idx separating_plane = plane_a separating_plane_p = edge_a_origin success_step = step @@ -231,7 +263,7 @@ create_face_contact_manifold :: proc( ref_convex := is_ref_a ? a : b inc_convex := is_ref_a ? b : a - ref_face := a.faces[ref_face_query.face] + ref_face := ref_convex.faces[ref_face_query.face] // incident face inc_face: halfedge.Face @@ -403,9 +435,8 @@ create_edge_contact_manifold :: proc( _, 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] + manifold.points = make([]Vec3, 1, context.temp_allocator) + manifold.points[0] = (ps[0] + ps[1]) * 0.5 return }