Implement friction in PGS

build_hot_reload.sh

@@ -35,7 +35,7 @@ esac
 
 # Build the game.
 echo "Building game$DLL_EXT"
-odin build game -extra-linker-flags:"$EXTRA_LINKER_FLAGS" -define:RAYLIB_SHARED=true -define:TRACY_ENABLE=true -collection:libs=./libs -collection:common=./common -collection:game=./game -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug
+odin build game -extra-linker-flags:"$EXTRA_LINKER_FLAGS" -define:RAYLIB_SHARED=true -define:TRACY_ENABLE=true -collection:libs=./libs -collection:common=./common -collection:game=./game -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug -o:speed
 
 # Need to use a temp file on Linux because it first writes an empty `game.so`, which the game will load before it is actually fully written.
 mv game_tmp$DLL_EXT game$DLL_EXT

game/physics/collision/convex.odin

@@ -59,6 +59,8 @@ Contact_Type :: union {
 Contact_Manifold :: struct {
 	type:       Contact_Type,
 	normal:     Vec3,
+	tangent:    Vec3,
+	bitangent:  Vec3,
 	separation: f32,
 	points_a:   [4]Vec3,
 	points_b:   [4]Vec3,
@@ -89,7 +91,6 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
 	is_face_a_contact := biased_face_a_separation >= biased_edge_separation
 	is_face_b_contact := biased_face_b_separation >= biased_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 {
@@ -102,6 +103,10 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
 			edge_b,
 		)
 	}
+	collision = manifold.points_len > 0
+	if collision {
+		manifold.bitangent = lg.normalize0(lg.cross(manifold.tangent, manifold.normal))
+	}
 
 	return
 }
@@ -423,8 +428,12 @@ create_face_contact_manifold :: proc(
 		}
 	}
 
-	ref_face_vert := ref_convex.vertices[ref_convex.edges[ref_face.edge].origin].pos
+	ref_face_vert, ref_face_vert2 := halfedge.get_edge_points(
+		ref_convex,
+		ref_convex.edges[ref_face.edge],
+	)
 	ref_plane := plane_from_point_normal(ref_face_vert, ref_face.normal)
+	ref_tangent := lg.normalize0(ref_face_vert2 - ref_face_vert)
 
 	// Final clipping, remove verts above ref face and project those left onto the reference plane
 	{
@@ -527,6 +536,7 @@ create_face_contact_manifold :: proc(
 	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
+	manifold.tangent = ref_tangent
 
 	return
 }
@@ -560,6 +570,7 @@ create_edge_contact_manifold :: proc(
 	manifold.points_a[0] = ps[0]
 	manifold.points_b[0] = ps[1]
 	manifold.points_len = 1
+	manifold.tangent = lg.normalize0(lg.cross(a2 - a1, manifold.normal))
 
 	return
 }

game/physics/debug.odin

@@ -117,12 +117,14 @@ draw_debug_scene :: proc(scene: ^Scene) {
 			debug_transform_points_local_to_world(get_body(sim_state, contact.a), points_a_slice)
 			debug_transform_points_local_to_world(get_body(sim_state, contact.b), points_b_slice)
 			debug_draw_manifold_points(
-				-contact.manifold.normal,
+				contact,
+				-1,
 				points_a_slice,
 				color = debug.int_to_color(i32(contact_idx * 2 + 0)),
 			)
 			debug_draw_manifold_points(
-				contact.manifold.normal,
+				contact,
+				1,
 				points_b_slice,
 				color = debug.int_to_color(i32(contact_idx * 2 + 1)),
 			)
@@ -136,7 +138,12 @@ debug_transform_points_local_to_world :: proc(body: Body_Ptr, points: []Vec3) {
 	}
 }
 
-debug_draw_manifold_points :: proc(normal: Vec3, points: []Vec3, color: rl.Color) {
+debug_draw_manifold_points :: proc(
+	contact: Contact,
+	impulse_sign: f32,
+	points: []Vec3,
+	color: rl.Color,
+) {
 	if len(points) >= 3 {
 		// Triangle or quad
 		v1 := points[0]
@@ -151,9 +158,15 @@ debug_draw_manifold_points :: proc(normal: Vec3, points: []Vec3, color: rl.Color
 		rl.DrawLine3D(points[0], points[1], color)
 	}
 
-	for p in points {
-		rl.DrawLine3D(p, p + normal, color)
 
-		rl.DrawSphereWires(p, 0.1, 4, 4, color)
+	for point_idx in 0 ..< len(points) {
+		p := points[point_idx]
+		total_impulse :=
+			contact.total_normal_impulse[point_idx] * contact.manifold.normal +
+			contact.total_friction_impulse[point_idx].x * contact.manifold.tangent +
+			contact.total_friction_impulse[point_idx].y * contact.manifold.bitangent
+		rl.DrawLine3D(p, p + total_impulse * impulse_sign, color)
+
+		// rl.DrawSphereWires(p, 0.1, 4, 4, color)
 	}
 }

game/physics/scene.odin

@@ -6,6 +6,7 @@ import lg "core:math/linalg"
 MAX_CONTACTS :: 1024 * 16
 
 Vec3 :: [3]f32
+Vec2 :: [2]f32
 Quat :: quaternion128
 Matrix3 :: # row_major matrix[3, 3]f32
 AABB :: struct {

game/physics/simulation.odin

@@ -219,7 +219,8 @@ Contact :: struct {
 
 	// For PGS
 	total_normal_impulse:      [4]f32,
-	total_tangent_impulse:     [4]f32,
+	// xy - tangent and bitangent (linear friction), z - twist friction around normal (rotational impulse only)
+	total_friction_impulse:    [4]Vec2,
 
 	// XPBD stuff
 	lambda_normal:             [4]f32,
@@ -253,7 +254,7 @@ update_contacts :: proc(sim_state: ^Sim_State) {
 
 		old_manifold := contact.manifold
 		old_total_normal_impulse := contact.total_normal_impulse
-		old_total_tangent_impulse := contact.total_tangent_impulse
+		old_total_friction_impulse := contact.total_friction_impulse
 
 		contact.prev_x_a = body.x
 		contact.prev_x_b = body2.x
@@ -261,7 +262,7 @@ update_contacts :: proc(sim_state: ^Sim_State) {
 		contact.prev_q_b = body2.q
 		contact.manifold = {}
 		contact.total_normal_impulse = 0
-		contact.total_tangent_impulse = 0
+		contact.total_friction_impulse = 0
 		contact.lambda_normal = 0
 		contact.lambda_tangent = 0
 		contact.applied_static_friction = false
@@ -285,18 +286,11 @@ update_contacts :: proc(sim_state: ^Sim_State) {
 		raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
 
 		if collision {
+			assert(raw_manifold.points_len > 0)
+
 			manifold := &contact.manifold
 			manifold^ = raw_manifold
 
-			preserve_impulse :=
-				old_manifold.points_len == raw_manifold.points_len &&
-				old_manifold.type == raw_manifold.type
-
-			if preserve_impulse {
-				contact.total_normal_impulse = old_total_normal_impulse
-				contact.total_tangent_impulse = old_total_tangent_impulse
-			}
-
 			// Convert manifold contact from world to local space
 			for point_idx in 0 ..< manifold.points_len {
 				manifold.points_a[point_idx] = body_world_to_local(
@@ -308,6 +302,25 @@ update_contacts :: proc(sim_state: ^Sim_State) {
 					manifold.points_b[point_idx],
 				)
 			}
+
+			preserve_impulse :=
+				old_manifold.points_len == raw_manifold.points_len &&
+				old_manifold.type == raw_manifold.type
+
+			if preserve_impulse {
+				contact.total_normal_impulse = old_total_normal_impulse
+
+				for point_idx in 0 ..< manifold.points_len {
+					contact.total_friction_impulse[point_idx].x = lg.dot(
+						old_total_friction_impulse[point_idx].x * old_manifold.tangent,
+						manifold.tangent,
+					)
+					contact.total_friction_impulse[point_idx].y = lg.dot(
+						old_total_friction_impulse[point_idx].y * old_manifold.bitangent,
+						manifold.bitangent,
+					)
+				}
+			}
 		}
 	}
 }
@@ -822,13 +835,27 @@ pgs_solve_contacts :: proc(
 	inv_dt: f32,
 	apply_bias: bool,
 ) {
-	bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(100, 1, dt)
+	bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(30, 0.8, dt)
 	if !apply_bias {
+		mass_coef = 1
 		bias_rate = 0
+		impulse_coef = 0
+	}
+
+	random_order := make([]i32, len(sim_state.contact_container.contacts))
+	{
+		for i in 0 ..< len(random_order) {
+			random_order[i] = i32(i)
+		}
+
+		for i in 0 ..< len(random_order) - 1 {
+			j := rand.int_max(len(random_order))
+			slice.swap(random_order, i, j)
+		}
 	}
 
 	for i in 0 ..< len(sim_state.contact_container.contacts) {
-		contact := &sim_state.contact_container.contacts[i]
+		contact := &sim_state.contact_container.contacts[random_order[i]]
 		manifold := &contact.manifold
 
 		body1, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b)
@@ -857,29 +884,71 @@ pgs_solve_contacts :: proc(
 
 				inv_w := 1.0 / w
 
-				delta_v := lg.dot(v2 - v1, manifold.normal)
+				delta_v := v2 - v1
+				{
+					delta_v_norm := lg.dot(delta_v, manifold.normal)
 
-				incremental_impulse :=
+					bias := f32(0.0)
-					-inv_w * mass_coef * (delta_v + bias_rate * separation) -
-					impulse_coef * contact.total_normal_impulse[point_idx]
 
-				new_total_impulse := max(
+					MAX_BAUMGARTE_VELOCITY :: 4.0
-					contact.total_normal_impulse[point_idx] + incremental_impulse,
-					0,
-				)
-				applied_impulse := new_total_impulse - contact.total_normal_impulse[point_idx]
-				contact.total_normal_impulse[point_idx] = new_total_impulse
 
-				applied_impulse_vec := applied_impulse * manifold.normal
+					if separation > 0 {
+						bias = separation * inv_dt
+					} else if apply_bias {
+						bias = lg.max(bias_rate * separation, -MAX_BAUMGARTE_VELOCITY)
+					}
 
-				apply_velocity_correction(body1, -applied_impulse_vec, p1)
+					incremental_impulse :=
-				apply_velocity_correction(body2, applied_impulse_vec, p2)
+						-inv_w * mass_coef * (delta_v_norm + bias) -
+						impulse_coef * contact.total_normal_impulse[point_idx]
+
+					new_total_impulse := max(
+						contact.total_normal_impulse[point_idx] + incremental_impulse,
+						0,
+					)
+					applied_impulse := new_total_impulse - contact.total_normal_impulse[point_idx]
+					contact.total_normal_impulse[point_idx] = new_total_impulse
+
+					applied_impulse_vec := applied_impulse * manifold.normal
+
+					apply_velocity_correction(body1, -applied_impulse_vec, p1)
+					apply_velocity_correction(body2, applied_impulse_vec, p2)
+				}
+
+				{
+					FRICTION :: 0.6
+					friction_clamp := contact.total_normal_impulse[point_idx] * FRICTION
+
+					delta_v_tang := Vec2 {
+						lg.dot(delta_v, manifold.tangent),
+						lg.dot(delta_v, manifold.bitangent),
+					}
+
+					incremental_impulse := -inv_w * delta_v_tang
+
+					new_total_impulse: Vec2 = lg.clamp(
+						contact.total_friction_impulse[point_idx] + incremental_impulse,
+						Vec2(-friction_clamp),
+						Vec2(friction_clamp),
+					)
+
+					applied_impulse :=
+						new_total_impulse - contact.total_friction_impulse[point_idx]
+					contact.total_friction_impulse[point_idx] = new_total_impulse
+
+					applied_impulse_vec :=
+						applied_impulse.x * manifold.tangent +
+						applied_impulse.y * manifold.bitangent
+
+					apply_velocity_correction(body1, -applied_impulse_vec, p1)
+					apply_velocity_correction(body2, applied_impulse_vec, p2)
+				}
 			} else {
 				contact.total_normal_impulse[point_idx] = 0
+				contact.total_friction_impulse[point_idx] = 0
 			}
 		}
 	}
-
 }
 
 pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) {
@@ -907,6 +976,13 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d
 
 				apply_velocity_correction(body1, -total_normal_impulse, p1)
 				apply_velocity_correction(body2, total_normal_impulse, p2)
+
+				total_tangent_impulse :=
+					contact.total_friction_impulse[point_idx].x * manifold.tangent +
+					contact.total_friction_impulse[point_idx].y * manifold.bitangent
+
+				apply_velocity_correction(body1, -total_tangent_impulse, p1)
+				apply_velocity_correction(body2, total_tangent_impulse, p2)
 			}
 		}
 	}
@@ -950,7 +1026,6 @@ simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Confi
 
 
 	tlas := build_tlas(sim_state, config)
-	find_new_contacts(sim_state, &tlas)
 
 	{
 		tracy.ZoneN("simulate_step::remove_invalid_contacts")
@@ -981,6 +1056,8 @@ simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Confi
 		}
 	}
 
+	find_new_contacts(sim_state, &tlas)
+
 	update_contacts(sim_state)
 
 	Solver :: enum {
@@ -1131,14 +1208,32 @@ multiply_inv_intertia :: proc(body: Body_Ptr, vec: Vec3) -> (result: Vec3) {
 	return result
 }
 
-apply_velocity_correction :: proc(body: Body_Ptr, impulse: Vec3, pos: Vec3) {
+apply_velocity_correction :: #force_inline proc "contextless" (
-	body.v += impulse * body.inv_mass
+	body: Body_Ptr,
+	impulse: Vec3,
+	pos: Vec3,
+) {
+	apply_velocity_correction_linear(body, impulse)
 
+	angular_impulse := lg.cross(pos - body.x, impulse)
+
+	apply_velocity_correction_angular(body, angular_impulse)
+}
+
+apply_velocity_correction_linear :: #force_inline proc "contextless" (
+	body: Body_Ptr,
+	impulse: Vec3,
+) {
+	body.v += impulse * body.inv_mass
+}
+
+apply_velocity_correction_angular :: #force_inline proc "contextless" (
+	body: Body_Ptr,
+	angular_impulse: Vec3,
+) {
 	q := body.q
 	inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
-	delta_omega := pos - body.x
+	delta_omega := lg.quaternion_mul_vector3(inv_q, angular_impulse)
-	delta_omega = lg.cross(delta_omega, impulse)
-	delta_omega = lg.quaternion_mul_vector3(inv_q, delta_omega)
 	delta_omega *= body.inv_inertia_tensor
 	delta_omega = lg.quaternion_mul_vector3(q, delta_omega)
 
@@ -1167,7 +1262,20 @@ apply_position_correction :: proc(body: Body_Ptr, corr: Vec3, pos: Vec3) {
 	body.q = q
 }
 
+// Total inverse mass (linear + angular)
 get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: Vec3) -> f32 {
+	linear, angular := get_body_inverse_mass_separate(body, normal, pos)
+
+	return linear + angular
+}
+
+get_body_inverse_mass_separate :: proc(
+	body: Body_Ptr,
+	normal, pos: Vec3,
+) -> (
+	linear: f32,
+	angular: f32,
+) {
 	q := body.q
 	inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
 
@@ -1175,10 +1283,10 @@ get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: Vec3) -> f32 {
 	rn = lg.cross(rn, normal)
 	rn = lg.quaternion_mul_vector3(inv_q, rn)
 
-	w := lg.dot(rn, rn * body.inv_inertia_tensor)
+	linear = body.inv_mass
-	w += body.inv_mass
+	angular = lg.dot(rn, rn * body.inv_inertia_tensor)
 
-	return w
+	return
 }
 
 get_body_angular_inverse_mass :: proc(body: Body_Ptr, normal: Vec3) -> f32 {