collision detection progress

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()
+}

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)
+		halfedge.debug_draw_mesh_wires(halfedge.Half_Edge_Mesh(box1_convex), rl.RED)
-		rl.DrawCubeWiresV(box2.pos, box2.rad * 1.999, 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)
 		}
 
 		// {

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)
+	}
+}

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
+}

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 {
+		for edge_b in b.edges {
-			edge_a_dir := halfedge.get_edge_direction_normalized(
+			edge_a_dir := halfedge.get_edge_direction_normalized(a, edge_a)
-				halfedge.Half_Edge_Mesh(a),
+			edge_b_dir := halfedge.get_edge_direction_normalized(b, edge_b)
-				edge_a,
-			)
-			edge_b_dir := halfedge.get_edge_direction_normalized(
-				halfedge.Half_Edge_Mesh(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)
+			distance_a := signed_distance_plane(vert_a.pos, plane_a)
-			if distance > separation {
+			if distance_a > 0 {
-				separation = distance
+				continue
-				a_edge = halfedge.Edge_Index(edge_a_idx)
-				b_edge = halfedge.Edge_Index(edge_b_idx)
 			}
+			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) {
+				for i in 0 ..< vert_count {
-					k := (i + 1) % len(src_polygon)
+					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
 					}
 				}
-
+				vert_count = j
-				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
 		}
 	}
 
+	// 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
 }

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)
+		}
+	}
+}

game/raylib_helpers.odin

@@ -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)
-}

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)
+		debug.rlgl_vertex3v(cur.pos)
-		rlgl_vertex3v(next.pos)
+		debug.rlgl_vertex3v(next.pos)
 
 		rlgl.Color4ub(255, 0, 0, 255)
-		rlgl_vertex3v(cur.pos)
+		debug.rlgl_vertex3v(cur.pos)
-		rlgl_vertex3v(cur.pos + tangent)
+		debug.rlgl_vertex3v(cur.pos + tangent)
 
 		rlgl.Color4ub(0, 255, 0, 255)
-		rlgl_vertex3v(cur.pos)
+		debug.rlgl_vertex3v(cur.pos)
-		rlgl_vertex3v(cur.pos + bitangent * ROAD_WIDTH)
+		debug.rlgl_vertex3v(cur.pos + bitangent * ROAD_WIDTH)
 
 		rlgl.Color4ub(0, 0, 255, 255)
-		rlgl_vertex3v(cur.pos)
+		debug.rlgl_vertex3v(cur.pos)
-		rlgl_vertex3v(cur.pos + normal)
+		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)
+			debug.rlgl_color3v(normal * 0.5 + 0.5)
-			rlgl_vertex3v(p1)
+			debug.rlgl_vertex3v(p1)
-			rlgl_vertex3v(p2)
+			debug.rlgl_vertex3v(p2)
-			rlgl_vertex3v(p3)
+			debug.rlgl_vertex3v(p3)
 
-			rlgl_color3v(next_normal * 0.5 + 0.5)
+			debug.rlgl_color3v(next_normal * 0.5 + 0.5)
-			rlgl_vertex3v(p2)
+			debug.rlgl_vertex3v(p2)
-			rlgl_vertex3v(p4)
+			debug.rlgl_vertex3v(p4)
-			rlgl_vertex3v(p3)
+			debug.rlgl_vertex3v(p3)
 		}
 	}
 }