Try to fix edge collision issues, more debug stuff

game/game.odin

@@ -80,6 +80,7 @@ Game_Memory :: struct {
 	preview_bvh:   int,
 	preview_node:  int,
 	physics_pause: bool,
+	free_cam:      bool,
 }
 
 Track_Edit_State :: enum {
@@ -147,7 +148,7 @@ camera_forward_vec :: proc() -> rl.Vector3 {
 }
 
 game_camera_3d :: proc() -> rl.Camera3D {
-	if g_mem.editor {
+	if g_mem.editor || g_mem.free_cam {
 		return {
 			position = get_world().player_pos,
 			up = {0, 1, 0},
@@ -260,6 +261,9 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 				mass = 100,
 			},
 		)
+
+		if true {
+
 			for x in -3 ..< 3 {
 				for y in -3 ..< 3 {
 					physics.immediate_body(
@@ -275,6 +279,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 					)
 				}
 			}
+		}
 
 
 		// car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
@@ -375,11 +380,11 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 			wheel.drive_impulse = 0
 			wheel.brake_impulse = 0
 
-			if rl.IsKeyDown(.W) {
+			if rl.IsKeyDown(.W) && !g_mem.free_cam {
 				wheel.drive_impulse = DRIVE_IMPULSE
 			}
 
-			if rl.IsKeyDown(.S) {
+			if rl.IsKeyDown(.S) && !g_mem.free_cam {
 				wheel.brake_impulse = BRAKE_IMPULSE
 			}
 		}
@@ -388,11 +393,11 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 			wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle)
 			wheel.turn_angle = 0
 
-			if (rl.IsKeyDown(.A)) {
+			if rl.IsKeyDown(.A) && !g_mem.free_cam {
 				wheel.turn_angle += -TURN_ANGLE
 			}
 
-			if (rl.IsKeyDown(.D)) {
+			if rl.IsKeyDown(.D) && !g_mem.free_cam {
 				wheel.turn_angle += TURN_ANGLE
 			}
 
@@ -411,6 +416,10 @@ update :: proc() {
 		g_mem.editor = !g_mem.editor
 	}
 
+	if rl.IsKeyPressed(.F1) {
+		g_mem.free_cam = !g_mem.free_cam
+	}
+
 	dt := rl.GetFrameTime()
 
 	// Debug BVH traversal
@@ -449,7 +458,9 @@ update :: proc() {
 	if g_mem.editor {
 		update_editor(get_editor_state())
 	} else {
-		// update_free_look_camera(get_editor_state())
+		if g_mem.free_cam {
+			update_free_look_camera(get_editor_state())
+		}
 		update_runtime_world(get_runtime_world(), dt)
 	}
 }
@@ -487,7 +498,7 @@ draw :: proc() {
 
 	rl.BeginDrawing()
 	defer rl.EndDrawing()
-	rl.ClearBackground(rl.BLACK)
+	rl.ClearBackground(rl.GRAY)
 
 	runtime_world := get_runtime_world()
 	world := get_world()
@@ -547,10 +558,10 @@ draw :: proc() {
 		halfedge.transform_mesh(&box1_convex, box1_mat)
 		halfedge.transform_mesh(&box2_convex, box2_mat)
 
-		manifold, _ := collision.convex_vs_convex_sat(box1_convex, box2_convex)
+		// manifold, _ := collision.convex_vs_convex_sat(box1_convex, box2_convex)
 
-		halfedge.debug_draw_mesh_wires(halfedge.Half_Edge_Mesh(box1_convex), rl.RED)
+		// halfedge.debug_draw_mesh_wires(halfedge.Half_Edge_Mesh(box1_convex), rl.RED)
-		halfedge.debug_draw_mesh_wires(halfedge.Half_Edge_Mesh(box2_convex), 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))
@@ -560,9 +571,9 @@ 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_a[:manifold.points_len] {
+		// for p in manifold.points_a[:manifold.points_len] {
-			rl.DrawSphereWires(p, 0.05, 8, 8, rl.BLUE)
+		// 	rl.DrawSphereWires(p, 0.05, 8, 8, rl.BLUE)
-		}
+		// }
 
 		// {
 		// 	mesh_bvh := assets.get_bvh(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")

game/physics/collision/convex.odin

@@ -62,8 +62,8 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
 	if edge_separation > 0 {
 		return
 	}
-	biased_face_a_separation := face_query_a.separation
+	biased_face_a_separation := face_query_a.separation + 0.2
-	biased_face_b_separation := face_query_b.separation
+	biased_face_b_separation := face_query_b.separation + 0.1
 	biased_edge_separation := edge_separation
 
 	is_face_a_contact := biased_face_a_separation >= biased_edge_separation
@@ -249,6 +249,7 @@ query_separation_edges :: proc(
 					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)
@@ -336,6 +337,8 @@ create_face_contact_manifold :: proc(
 	step := 0
 	vert_count := original_vert_count
 
+	EPS :: 1e-6
+
 	{
 		it := halfedge.iterator_face_edges(
 			halfedge.Half_Edge_Mesh(ref_convex),
@@ -380,11 +383,11 @@ create_face_contact_manifold :: proc(
 					d1 := signed_distance_plane(src_polygon[i], clipping_plane)
 					d2 := signed_distance_plane(src_polygon[k], clipping_plane)
 
-					if d1 < 0 && d2 < 0 {
+					if d1 < -EPS && d2 < -EPS {
 						// Both points inside
 						clipped_polygon[j] = src_polygon[k]
 						j += 1
-					} else if d1 >= 0 && d2 < 0 {
+					} else if d1 > -EPS && d2 < -EPS {
 						// First point is outside
 						_, clipped_polygon[j], _ = intersect_segment_plane(
 							{src_polygon[i], src_polygon[k]},
@@ -393,7 +396,7 @@ create_face_contact_manifold :: proc(
 						j += 1
 						clipped_polygon[j] = src_polygon[k]
 						j += 1
-					} else if d1 < 0 && d2 >= 0 {
+					} else if d1 < -EPS && d2 > -EPS {
 						// Second point outside
 						_, clipped_polygon[j], _ = intersect_segment_plane(
 							{src_polygon[i], src_polygon[k]},
@@ -422,7 +425,7 @@ create_face_contact_manifold :: proc(
 		for i in 0 ..< vert_count {
 			d := signed_distance_plane(src_polygon[i], ref_plane)
 
-			if d <= 0.01 {
+			if d <= EPS {
 				clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal
 				j += 1
 			}
@@ -487,7 +490,7 @@ create_face_contact_manifold :: proc(
 		assert(len(full_clipped_polygon) <= 4)
 	}
 
-	inc_face_vert := ref_convex.vertices[inc_convex.edges[inc_face.edge].origin].pos
+	inc_face_vert := inc_convex.vertices[inc_convex.edges[inc_face.edge].origin].pos
 	inc_plane := plane_from_point_normal(inc_face_vert, inc_face.normal)
 
 	inc_points: [4]Vec3
@@ -528,10 +531,10 @@ create_edge_contact_manifold :: proc(
 
 	_, ps := closest_point_between_segments(a1, a2, b1, b2)
 
-	manifold.normal = separating_plane.normal
+	manifold.normal = 0
-	manifold.separation = lg.dot(ps[1] - ps[0], manifold.normal)
+	manifold.separation = separation
 	manifold.points_a[0] = ps[0]
-	manifold.points_b[1] = ps[1]
+	manifold.points_b[0] = ps[0]
 	manifold.points_len = 1
 
 	return

game/physics/debug.odin

@@ -100,23 +100,37 @@ draw_debug_scene :: proc(scene: ^Scene) {
 	}
 
 	for &contact, contact_idx in scene.contact_pairs[:scene.contact_pairs_len] {
-		color := debug.int_to_color(i32(contact_idx))
+		debug_draw_manifold_points(
-		if contact.manifold.points_len >= 3 {
+			-contact.manifold.normal,
-			// Triangle or quad
+			contact.manifold.points_a[:contact.manifold.points_len],
-			v1 := contact.manifold.points_a[0]
+			color = debug.int_to_color(i32(contact_idx * 2 + 0)),
+		)
+		debug_draw_manifold_points(
+			contact.manifold.normal,
+			contact.manifold.points_b[:contact.manifold.points_len],
+			color = debug.int_to_color(i32(contact_idx * 2 + 1)),
+		)
+	}
+}
 
-			for i in 2 ..< contact.manifold.points_len {
+debug_draw_manifold_points :: proc(normal: rl.Vector3, points: []rl.Vector3, color: rl.Color) {
-				v2, v3 := contact.manifold.points_a[i - 1], contact.manifold.points_a[i]
+	if len(points) >= 3 {
+		// Triangle or quad
+		v1 := points[0]
+
+		for i in 2 ..< len(points) {
+			v2, v3 := points[i - 1], points[i]
 
 			rl.DrawTriangle3D(v1, v2, v3, color)
 		}
-		} else if contact.manifold.points_len == 2 {
+	} else if len(points) == 2 {
 		// Line
-			rl.DrawLine3D(contact.manifold.points_a[0], contact.manifold.points_a[1], color)
+		rl.DrawLine3D(points[0], points[1], color)
 	}
 
-		for p in contact.manifold.points_a[:contact.manifold.points_len] {
+	for p in points {
-			rl.DrawSphereWires(p, 0.1, 4, 4, color)
+		rl.DrawLine3D(p, p + normal, color)
-		}
+
+		rl.DrawSphereWires(p, len(points) == 1 ? 0.5 : 0.1, 4, 4, color)
 	}
 }

game/physics/simulation.odin

@@ -155,9 +155,32 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 								manifold = manifold,
 							}
 							scene.contact_pairs_len += 1
-							for p in manifold.points_a[:manifold.points_len] {
+
-								body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p)
+							points_a_local, points_b_local: [4]rl.Vector3
-								body2_inv_mass := get_body_inverse_mass(body2, manifold.normal, p)
+							for point_idx in 0 ..< manifold.points_len {
+								points_a_local = body_world_to_local(
+									body,
+									manifold.points_a[point_idx],
+								)
+								points_b_local = body_world_to_local(
+									body2,
+									manifold.points_b[point_idx],
+								)
+							}
+							for point_idx in 0 ..< manifold.points_len {
+								p1, p2 :=
+									manifold.points_a[point_idx], manifold.points_b[point_idx]
+								//p1, p2 =
+								// body_local_to_world(body, p1), body_local_to_world(body2, p2)
+								body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p1)
+								body2_inv_mass := get_body_inverse_mass(body2, manifold.normal, p2)
+
+								diff := p2 - p1
+								length := lg.length(diff)
+								if length != 0 {
+									diff /= length
+								}
+								// separation := lg.dot(p2 - p1, manifold.normal)
 
 								handled_pairs[{a = min(i, j), b = max(i, j)}] = true
 
@@ -167,7 +190,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 									0,
 									-manifold.separation,
 									-manifold.normal,
-									p,
+									p1,
 									body2_inv_mass,
 								)
 
@@ -177,7 +200,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 									0,
 									-manifold.separation,
 									manifold.normal,
-									p,
+									p2,
 									body1_inv_mass,
 								)
 							}
@@ -218,6 +241,10 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 
 		solve_velocities(scene, body_states, inv_dt)
 
+		// for pair in scene.contact_pairs[:scene.contact_pairs_len] {
+		//   pair.a
+		// }
+
 		// Solve suspension velocity
 		for _, i in scene.suspension_constraints {
 			v := &scene.suspension_constraints_slice[i]