Proper stable collisions

Turns out when you get a collision pair YOU SHOULD HANDLE CONTACT RESPONSE FOR BOTH BODIES, NOT JUST ONE OF THEM
Otherwise they move and their next collision (if a->b, b->a are found and resolved separately) they might penetrate badly or worse and everything blows up

This is probably not as important in sequential impulse type solvers because the actual position update is deferred, but in a position based one it's pretty important as it turns out.

It's also better for performance because I don't have to run the collision query for the same two bodies two times

game/game.odin

@@ -79,7 +79,7 @@ Game_Memory :: struct {
 	editor:        bool,
 	preview_bvh:   int,
 	preview_node:  int,
-	draw_car:      bool,
+	physics_pause: bool,
 }
 
 Track_Edit_State :: enum {
@@ -398,7 +398,9 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 
 		}
 
-		physics.simulate(&world.physics_scene, &runtime_world.solver_state, SOLVER_CONFIG, dt)
+		if !g_mem.physics_pause || rl.IsKeyPressed(.PERIOD) {
+			physics.simulate(&world.physics_scene, &runtime_world.solver_state, SOLVER_CONFIG, dt)
+		}
 	}
 }
 
@@ -441,12 +443,13 @@ update :: proc() {
 	}
 
 	if rl.IsKeyPressed(.SPACE) {
-		g_mem.draw_car = !g_mem.draw_car
+		g_mem.physics_pause = !g_mem.physics_pause
 	}
 
 	if g_mem.editor {
 		update_editor(get_editor_state())
 	} else {
+		// update_free_look_camera(get_editor_state())
 		update_runtime_world(get_runtime_world(), dt)
 	}
 }
@@ -557,7 +560,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[:manifold.points_len] {
+		for p in manifold.points_a[:manifold.points_len] {
 			rl.DrawSphereWires(p, 0.05, 8, 8, rl.BLUE)
 		}
 
@@ -604,9 +607,7 @@ draw :: proc() {
 				rl.WHITE,
 			)
 		} else {
-			if g_mem.draw_car {
-				rl.DrawModel(car_model, 0, 1, rl.WHITE)
-			}
+			// rl.DrawModel(car_model, 0, 1, rl.WHITE)
 		}
 
 		{

game/physics/collision/convex.odin

@@ -42,7 +42,8 @@ box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Conv
 Contact_Manifold :: struct {
 	normal:     Vec3,
 	separation: f32,
-	points:     [4]Vec3,
+	points_a:   [4]Vec3,
+	points_b:   [4]Vec3,
 	points_len: int,
 }
 
@@ -421,7 +422,7 @@ create_face_contact_manifold :: proc(
 		for i in 0 ..< vert_count {
 			d := signed_distance_plane(src_polygon[i], ref_plane)
 
-			if d <= 0 {
+			if d <= 0.01 {
 				clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal
 				j += 1
 			}
@@ -433,6 +434,8 @@ create_face_contact_manifold :: proc(
 	// Resulting polygon before contact optimization
 	full_clipped_polygon := clip_bufs[get_other_clip_buf(clip_buf_idx)][:vert_count]
 
+	ref_points: [4]Vec3
+
 	// Contact optimization
 	if len(full_clipped_polygon) > 4 {
 		start_dir := lg.cross(ref_plane.normal, ref_face_vert)
@@ -473,17 +476,33 @@ create_face_contact_manifold :: proc(
 			}
 		}
 
-		manifold.points[0] = first_point
-		manifold.points[1] = second_point
-		manifold.points[2] = third_point
-		manifold.points[3] = fourth_point
+		ref_points[0] = first_point
+		ref_points[1] = second_point
+		ref_points[2] = third_point
+		ref_points[3] = fourth_point
 		manifold.points_len = 4
 	} else {
-		copy(manifold.points[:], full_clipped_polygon)
+		copy(ref_points[:], full_clipped_polygon)
 		manifold.points_len = len(full_clipped_polygon)
 		assert(len(full_clipped_polygon) <= 4)
 	}
 
+	inc_face_vert := ref_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
+	for p, i in ref_points[:manifold.points_len] {
+		inc_points[i] = project_point_on_plane(p, inc_plane)
+	}
+
+	if is_ref_a {
+		manifold.points_a = ref_points
+		manifold.points_b = inc_points
+	} else {
+		manifold.points_b = ref_points
+		manifold.points_a = inc_points
+	}
+
 	manifold.separation = ref_face_query.separation
 	// Normal is always pointing from a to b
 	manifold.normal = is_ref_a ? ref_face.normal : -ref_face.normal
@@ -491,6 +510,11 @@ create_face_contact_manifold :: proc(
 	return
 }
 
+project_point_on_plane :: proc(point: Vec3, plane: Plane) -> Vec3 {
+	dist := signed_distance_plane(point, plane)
+	return point + plane.normal * -dist
+}
+
 create_edge_contact_manifold :: proc(
 	a, b: Convex,
 	separating_plane: Plane,
@@ -506,7 +530,8 @@ create_edge_contact_manifold :: proc(
 
 	manifold.normal = separating_plane.normal
 	manifold.separation = lg.dot(ps[1] - ps[0], manifold.normal)
-	manifold.points[0] = (ps[0] + ps[1]) * 0.5
+	manifold.points_a[0] = ps[0]
+	manifold.points_b[1] = ps[1]
 	manifold.points_len = 1
 
 	return

game/physics/debug.odin

@@ -103,19 +103,19 @@ draw_debug_scene :: proc(scene: ^Scene) {
 		color := debug.int_to_color(i32(contact_idx))
 		if contact.manifold.points_len >= 3 {
 			// Triangle or quad
-			v1 := contact.manifold.points[0]
+			v1 := contact.manifold.points_a[0]
 
 			for i in 2 ..< contact.manifold.points_len {
-				v2, v3 := contact.manifold.points[i - 1], contact.manifold.points[i]
+				v2, v3 := contact.manifold.points_a[i - 1], contact.manifold.points_a[i]
 
 				rl.DrawTriangle3D(v1, v2, v3, color)
 			}
 		} else if contact.manifold.points_len == 2 {
 			// Line
-			rl.DrawLine3D(contact.manifold.points[0], contact.manifold.points[1], color)
+			rl.DrawLine3D(contact.manifold.points_a[0], contact.manifold.points_a[1], color)
 		}
 
-		for p in contact.manifold.points[:contact.manifold.points_len] {
+		for p in contact.manifold.points_a[:contact.manifold.points_len] {
 			rl.DrawSphereWires(p, 0.1, 4, 4, color)
 		}
 	}

game/physics/simulation.odin

@@ -116,13 +116,19 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 			}
 		}
 
+
+		Body_Pair :: struct {
+			a, b: int,
+		}
+		handled_pairs := make_map(map[Body_Pair]bool, context.temp_allocator)
+
 		for _, i in scene.bodies {
 			body := &scene.bodies_slice[i]
 			if body.alive {
 				for _, j in scene.bodies {
 					body2 := &scene.bodies_slice[j]
 
-					if i != j && body2.alive {
+					if i != j && body2.alive && !handled_pairs[{a = min(i, j), b = max(i, j)}] {
 						s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box)
 
 						box1 := collision.box_to_convex(
@@ -149,30 +155,31 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 								manifold = manifold,
 							}
 							scene.contact_pairs_len += 1
-							factor := 1.0 / f32(manifold.points_len)
-							for p in manifold.points[:manifold.points_len] {
-								// body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p)
+							for p in manifold.points_a[:manifold.points_len] {
+								body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p)
 								body2_inv_mass := get_body_inverse_mass(body2, manifold.normal, p)
 
+								handled_pairs[{a = min(i, j), b = max(i, j)}] = true
+
 								apply_constraint_correction_unilateral(
 									dt,
 									body,
 									0,
-									-manifold.separation * factor,
+									-manifold.separation,
 									-manifold.normal,
 									p,
 									body2_inv_mass,
 								)
 
-								// apply_constraint_correction_unilateral(
-								// 	dt,
-								// 	body2,
-								// 	0,
-								// 	-manifold.separation,
-								// 	manifold.normal,
-								// 	p,
-								// 	body1_inv_mass,
-								// )
+								apply_constraint_correction_unilateral(
+									dt,
+									body2,
+									0,
+									-manifold.separation,
+									manifold.normal,
+									p,
+									body1_inv_mass,
+								)
 							}
 						}
 					}