Convex vs convex face manifolds working

.vscode/launch.json

@@ -1,6 +1,12 @@
 {
     "version": "0.2.0",
     "configurations": [
+        {
+            "type": "lldb",
+            "request": "attach",
+            "name": "Attach Hot Reload (Linux/Max)",
+            "program": "${workspaceFolder}/game_hot_reload.bin"
+        },
         // Windows configs (only difference from linux/mac is "type" and "program")
         {
             "type": "cppvsdbg",
@@ -29,7 +35,6 @@
             "args": [],
             "cwd": "${workspaceFolder}"
         },
-
         // Linux / Mac configs
         {
             "type": "lldb",

.vscode/tasks.json

@@ -3,6 +3,17 @@
     "command": "",
     "args": [],
     "tasks": [
+        {
+            "label": "Test",
+            "type": "shell",
+            "linux": {
+                "command": "${workspaceFolder}/test.sh"
+            },
+            "osx": {
+                "command": "${workspaceFolder}/test.sh"
+            },
+            "group": "test"
+        },
         {
             "label": "Build Debug",
             "type": "shell",
@@ -57,4 +68,4 @@
             },
         }
     ]
-}
+}

game/game.odin

@@ -22,7 +22,7 @@ import "core:math"
 import "core:math/linalg"
 import "game:physics"
 import "game:physics/bvh"
-import "halfedge"
+import "game:physics/collision"
 import "libs:tracy"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
@@ -481,6 +481,7 @@ draw :: proc() {
 		origin = rl_ray.position,
 		dir    = rl_ray.direction,
 	}
+	_ = ray
 
 	{
 		rl.BeginMode3D(camera)
@@ -490,38 +491,57 @@ draw :: proc() {
 
 		physics.draw_debug_scene(&world.physics_scene)
 
-		{
+		box1, box2 := collision.Box {
-			mesh_bvh := assets.get_bvh(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
+				pos = {0, 0.5, 0},
-
+				rad = 0.5,
-			for &blas, i in mesh_bvh.bvhs {
+			}, collision.Box {
-				mesh := car_model.meshes[i]
+				pos = {0.5, 1.5, 0.5},
-
+				rad = 0.5,
-				if i == g_mem.preview_bvh {
-					bvh.debug_draw_bvh_bounds(
-						&blas,
-						bvh.bvh_mesh_from_rl_mesh(mesh),
-						0,
-						g_mem.preview_node,
-					)
-				}
-
-				vertices := (cast([^]rl.Vector3)mesh.vertices)[:mesh.vertexCount]
-				indices := mesh.indices[:mesh.triangleCount * 3]
-				if bvh.traverse_bvh_ray_mesh(
-					&blas,
-					bvh.Mesh{vertices = vertices, indices = indices},
-					ray,
-					&mesh_col,
-				) {
-					hit_mesh_idx = i
-				}
 			}
 
-			if mesh_col.hit {
+		box1_convex := collision.box_to_convex(box1, context.temp_allocator)
-				rl.DrawSphereWires(ray.origin + ray.dir * mesh_col.t, 0.1, 8, 8, rl.RED)
+		box2_convex := collision.box_to_convex(box2, context.temp_allocator)
-			}
+
+		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)
+		for p in manifold.points {
+			rl.DrawSphereWires(p, 0.1, 8, 8, rl.BLUE)
 		}
 
+		// {
+		// 	mesh_bvh := assets.get_bvh(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
+
+		// 	for &blas, i in mesh_bvh.bvhs {
+		// 		mesh := car_model.meshes[i]
+
+		// 		if i == g_mem.preview_bvh {
+		// 			bvh.debug_draw_bvh_bounds(
+		// 				&blas,
+		// 				bvh.bvh_mesh_from_rl_mesh(mesh),
+		// 				0,
+		// 				g_mem.preview_node,
+		// 			)
+		// 		}
+
+		// 		vertices := (cast([^]rl.Vector3)mesh.vertices)[:mesh.vertexCount]
+		// 		indices := mesh.indices[:mesh.triangleCount * 3]
+		// 		if bvh.traverse_bvh_ray_mesh(
+		// 			&blas,
+		// 			bvh.Mesh{vertices = vertices, indices = indices},
+		// 			ray,
+		// 			&mesh_col,
+		// 		) {
+		// 			hit_mesh_idx = i
+		// 		}
+		// 	}
+
+		// 	if mesh_col.hit {
+		// 		rl.DrawSphereWires(ray.origin + ray.dir * mesh_col.t, 0.1, 8, 8, rl.RED)
+		// 	}
+		// }
+
 		if !g_mem.editor {
 			car_matrix := rl.QuaternionToMatrix(car_body.q)
 			car_model.transform = car_matrix
@@ -577,35 +597,35 @@ draw :: proc() {
 				}
 			}
 
-			if mesh_col.hit {
+			// 			if mesh_col.hit {
-				mesh := car_model.meshes[hit_mesh_idx]
+			// 				mesh := car_model.meshes[hit_mesh_idx]
-				vertices := (cast([^]rl.Vector3)mesh.vertices)[:mesh.vertexCount]
+			// 				vertices := (cast([^]rl.Vector3)mesh.vertices)[:mesh.vertexCount]
-				indices := mesh.indices[:mesh.triangleCount * 3]
+			// 				indices := mesh.indices[:mesh.triangleCount * 3]
-				car_halfedge := halfedge.mesh_from_vertex_index_list(vertices, indices, 3, context.temp_allocator)
+			// 				car_halfedge := halfedge.mesh_from_vertex_index_list(vertices, indices, 3, context.temp_allocator)
-
+			// 
-				face_idx := halfedge.Face_Index(mesh_col.prim)
+			// 				face_idx := halfedge.Face_Index(mesh_col.prim)
-				face := car_halfedge.faces[face_idx]
+			// 				face := car_halfedge.faces[face_idx]
-				first_edge_idx := face.edge
+			// 				first_edge_idx := face.edge
-
+			// 
-				first := true
+			// 				first := true
-				cur_edge_idx := first_edge_idx
+			// 				cur_edge_idx := first_edge_idx
-				for first || cur_edge_idx != first_edge_idx {
+			// 				for first || cur_edge_idx != first_edge_idx {
-					first = false
+			// 					first = false
-					edge := car_halfedge.edges[cur_edge_idx]
+			// 					edge := car_halfedge.edges[cur_edge_idx]
-					cur_edge_idx = edge.next
+			// 					cur_edge_idx = edge.next
-
+			// 
-					if edge.twin >= 0 {
+			// 					if edge.twin >= 0 {
-						twin_edge := car_halfedge.edges[edge.twin]
+			// 						twin_edge := car_halfedge.edges[edge.twin]
-						face := twin_edge.face
+			// 						face := twin_edge.face
-
+			// 
-						i1, i2, i3 := indices[face * 3 + 0], indices[face * 3 + 1], indices[face * 3 + 2]
+			// 						i1, i2, i3 := indices[face * 3 + 0], indices[face * 3 + 1], indices[face * 3 + 2]
-						v1, v2, v3 := vertices[i1], vertices[i2], vertices[i3]
+			// 						v1, v2, v3 := vertices[i1], vertices[i2], vertices[i3]
-
+			// 
-						rl.DrawTriangle3D(v1, v2, v3, rl.RED)
+			// 						rl.DrawTriangle3D(v1, v2, v3, rl.RED)
-					}
+			// 					}
-				}
+			// 				}
-			}
+			// 			}
-
+			// 
 		}
 	}
 

game/halfedge/halfedge.odin

@@ -63,7 +63,7 @@ mesh_from_vertex_index_list :: proc(
 
 	for f in 0 ..< num_faces {
 		base_index := f * vertices_per_face
-		i1, i2, i3 := base_index + 0, base_index + 1, base_index + 2
+		i1, i2, i3 := indices[base_index + 0], indices[base_index + 1], indices[base_index + 2]
 		v1, v2, v3 := vertices[i1], vertices[i2], vertices[i3]
 
 		// Assuming ccw winding
@@ -83,6 +83,7 @@ mesh_from_vertex_index_list :: proc(
 			}
 
 			if i == 0 {
+				verts[index].edge = Edge_Index(e)
 				faces[f].edge = Edge_Index(e)
 			}
 
@@ -129,14 +130,16 @@ get_adjacent_face :: proc(
 	edge: Half_Edge,
 ) -> (
 	face: Face,
+	face_idx: Face_Index,
 	ok: bool,
-) #optional_ok {
+) {
 	if edge.twin < 0 {
-		return {}, false
+		return {}, 0, false
 	}
 
 	twin := mesh.edges[edge.twin]
 	face = mesh.faces[twin.face]
+	face_idx = twin.face
 	ok = true
 
 	return
@@ -157,6 +160,11 @@ iterator_face_edges :: proc(mesh: Half_Edge_Mesh, face: Face_Index) -> (it: Edge
 	return
 }
 
+iterator_reset_edges :: proc(it: ^Edge_Iterator) {
+	it.current_edge = it.first_edge
+	it.past_first = false
+}
+
 iterate_next_edge :: proc(it: ^Edge_Iterator) -> (edge: Half_Edge, ok: bool) {
 	if it.current_edge == it.first_edge {
 		if !it.past_first {

game/physics/collision/convex.odin

@@ -3,58 +3,31 @@ package collision
 import "core:math"
 import lg "core:math/linalg"
 import "game:halfedge"
+import rl "vendor:raylib"
+import "vendor:raylib/rlgl"
+
+_ :: math
 
 Convex :: distinct halfedge.Half_Edge_Mesh
 
 BOX_CORNERS_NORM :: [8]Vec3 {
-	Vec3{-1, -1, -1},
+	{-1, 1, 1},
-	Vec3{-1, -1, 1},
+	{-1, -1, 1},
-	Vec3{-1, 1, -1},
+	{-1, 1, -1},
-	Vec3{-1, 1, 1},
+	{-1, -1, -1},
-	Vec3{1, -1, -1},
+	{1, 1, 1},
-	Vec3{1, -1, 1},
+	{1, -1, 1},
-	Vec3{1, 1, -1},
+	{1, 1, -1},
-	Vec3{1, 1, 1},
+	{1, -1, -1},
 }
 
-box_indices := [6 * 4]u16 {
+box_indices := [6 * 4]u16{0, 4, 6, 2, 3, 2, 6, 7, 7, 6, 4, 5, 5, 1, 3, 7, 1, 0, 2, 3, 5, 4, 0, 1}
-	// near
-	0,
-	4,
-	6,
-	2,
-	// right
-	4,
-	5,
-	7,
-	6,
-	// far
-	5,
-	1,
-	3,
-	7,
-	// left
-	1,
-	0,
-	2,
-	3,
-	// top
-	2,
-	6,
-	7,
-	3,
-	// bottom
-	0,
-	4,
-	5,
-	1,
-}
 
 box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Convex) {
 	vertices := make([]Vec3, 8, context.temp_allocator)
 
 	for corner, i in BOX_CORNERS_NORM {
-		vertices[i] = corner * box.rad
+		vertices[i] = box.pos + corner * box.rad
 	}
 
 	convex = Convex(halfedge.mesh_from_vertex_index_list(vertices, box_indices[:], 4, allocator))
@@ -78,9 +51,11 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
 	}
 
 	edge_separation, edge_a, edge_b := query_separation_edges(a, b)
+	_, _ = edge_a, edge_b
 	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
@@ -89,7 +64,6 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
 	if is_face_a_contact && is_face_b_contact {
 		manifold = create_face_contact_manifold(face_query_a, a, face_query_b, b)
 	} else {
-
 	}
 
 	return
@@ -190,15 +164,174 @@ create_face_contact_manifold :: proc(
 	is_ref_a := face_query_a.separation > face_query_b.separation
 	ref_face_query := is_ref_a ? face_query_a : face_query_b
 	ref_convex := is_ref_a ? a : b
-
-	inc_face_query := is_ref_a ? face_query_b : face_query_a
 	inc_convex := is_ref_a ? b : a
 
-	it := halfedge.iterator_face_edges(halfedge.Half_Edge_Mesh(ref_convex), ref_face_query.face)
+	ref_face := a.faces[ref_face_query.face]
 
-	for edge in halfedge.iterate_next_edge(&it) {
+	// incident face
-		clipping_face := halfedge.get_adjacent_face(halfedge.Half_Edge_Mesh(ref_convex), edge)
+	inc_face: halfedge.Face
+	inc_face_idx: halfedge.Face_Index
+	// Find the most anti parallel face
+	{
+		min_dot := f32(1.0)
+		for face, face_idx in inc_convex.faces {
+			dot := lg.dot(ref_face.normal, face.normal)
+			if dot < min_dot {
+				min_dot = dot
+				inc_face = face
+				inc_face_idx = halfedge.Face_Index(face_idx)
+			}
+		}
 	}
 
+	inc_polygon: []Vec3
+
+	// 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
+		}
+
+		inc_polygon = make([]Vec3, vert_count, context.temp_allocator)
+
+		halfedge.iterator_reset_edges(&it)
+
+		i := 0
+		for edge in halfedge.iterate_next_edge(&it) {
+			inc_polygon[i] = inc_convex.vertices[edge.origin].pos
+			i += 1
+		}
+	}
+
+	assert(len(inc_polygon) > 0)
+
+	// Set up ping pong buffers
+	inc_polygon2 := make([]Vec3, len(inc_polygon), context.temp_allocator)
+
+	// Buffer 0 is the original incident polygon, start with index 1
+	clip_buf_idx := 1
+	clip_bufs := [2][]Vec3{inc_polygon, inc_polygon2}
+
+	get_other_clip_buf :: proc(idx: int) -> int {
+		return (idx + 1) % 2
+	}
+
+	step := 0
+
+	{
+		it := halfedge.iterator_face_edges(
+			halfedge.Half_Edge_Mesh(ref_convex),
+			ref_face_query.face,
+		)
+
+		for edge in halfedge.iterate_next_edge(&it) {
+			src_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)]
+			clipped_polygon := clip_bufs[clip_buf_idx]
+
+			clipping_face, clipping_face_idx, _ := halfedge.get_adjacent_face(
+				halfedge.Half_Edge_Mesh(ref_convex),
+				edge,
+			)
+			clipping_face_vert :=
+				ref_convex.vertices[ref_convex.edges[clipping_face.edge].origin].pos
+
+			clipping_plane_center: Vec3
+			{
+				clipping_plane_it := halfedge.iterator_face_edges(
+					halfedge.Half_Edge_Mesh(ref_convex),
+					clipping_face_idx,
+				)
+
+				num := 0
+				for clip_edge in halfedge.iterate_next_edge(&clipping_plane_it) {
+					vert := ref_convex.vertices[clip_edge.origin].pos
+					clipping_plane_center += vert
+					num += 1
+				}
+
+				clipping_plane_center /= f32(num)
+			}
+
+			clipping_plane := plane_from_point_normal(clipping_face_vert, clipping_face.normal)
+
+			// Actual clipping
+			{
+				j := 0
+				for i in 0 ..< len(src_polygon) {
+					k := (i + 1) % len(src_polygon)
+					d1 := signed_distance_plane(src_polygon[i], clipping_plane)
+					d2 := signed_distance_plane(src_polygon[k], clipping_plane)
+
+					if d1 < 0 && d2 < 0 {
+						// Both points inside
+						clipped_polygon[j] = src_polygon[k]
+						j += 1
+					} else if d1 >= 0 && d2 < 0 {
+						// First point is outside
+						_, clipped_polygon[j], _ = intersect_segment_plane(
+							{src_polygon[i], src_polygon[k]},
+							clipping_plane,
+						)
+						j += 1
+						clipped_polygon[j] = src_polygon[k]
+						j += 1
+					} else if d1 < 0 && d2 >= 0 {
+						// Second point outside
+						_, clipped_polygon[j], _ = intersect_segment_plane(
+							{src_polygon[i], src_polygon[k]},
+							clipping_plane,
+						)
+						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)
+					}
+
+				}
+			}
+
+			clip_bufs[clip_buf_idx] = clipped_polygon
+			clip_buf_idx = get_other_clip_buf(clip_buf_idx)
+			step += 1
+		}
+	}
+
+	manifold.normal = ref_face.normal
+	manifold.points = clip_bufs[get_other_clip_buf(clip_buf_idx)]
+
 	return
 }