Edge contacts and fix bug with face contacts

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_rotate(f32(rl.GetTime()), rl.Vector3{0, 1, 0}) * box2_mat
-		box2_mat = linalg.matrix4_translate(rl.Vector3{1.35, -0.5, 0}) * box2_mat
 
 
 		box1, box2 := collision.Box {

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 := &mesh.faces[i].normal
-		n := face.normal
+		mesh.faces[i].normal = lg.normalize0((mat * Vec4{n.x, n.y, n.z, 0}).xyz)
-		face.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
+}

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_a_contact := face_query_a.separation >= edge_separation
-	is_face_b_contact := face_query_b.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 {
+find_support_point :: proc(
-	p: halfedge.Vertex
+	convex: Convex,
+	normal: Vec3,
+) -> (
+	vert: halfedge.Vertex,
+	idx: halfedge.Vertex_Index,
+	ok: bool,
+) {
 	max_proj := min(f32)
-	for vert in convex.vertices {
+	for v, i in convex.vertices {
-		proj := lg.dot(vert.pos, normal)
+		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_a, _, _ := find_support_point(a, plane_a.normal)
-			vert_b := find_support_point(b, -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 = make([]Vec3, 1, context.temp_allocator)
-	manifold.points[0] = ps[0]
+	manifold.points[0] = (ps[0] + ps[1]) * 0.5
-	manifold.points[1] = ps[1]
 
 	return
 }