Manifold contact optimization

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_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(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_translate(rl.Vector3{0.0, 0, 0}) * box2_mat
-		box2_mat = linalg.matrix4_rotate(f32(rl.GetTime()), rl.Vector3{0, 1, 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)
 		}
 

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 {
+find_support_point_from_slice :: proc(points: []Vec3, normal: Vec3) -> (p: 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
 }