Static friction and refactor constraint params

game/game.odin

@@ -69,7 +69,7 @@ Car :: struct {
 SOLVER_CONFIG :: physics.Solver_Config {
 	timestep            = 1.0 / 120,
 	gravity             = rl.Vector3{0, -9.8, 0},
-	substreps_minus_one = 1 - 1,
+	substreps_minus_one = 2 - 1,
 }
 
 Game_Memory :: struct {
@@ -303,7 +303,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 
 		// 1.6 is a good value
 		wheel_extent_x := f32(2)
-		wheel_y := f32(-0.5)
+		wheel_y := f32(-1)
 		rest := f32(1)
 		suspension_stiffness := f32(2000)
 		compliance := 1.0 / suspension_stiffness

game/physics/debug.odin

@@ -131,6 +131,6 @@ debug_draw_manifold_points :: proc(normal: rl.Vector3, points: []rl.Vector3, col
 	for p in points {
 		rl.DrawLine3D(p, p + normal, color)
 
-		rl.DrawSphereWires(p, len(points) == 1 ? 0.5 : 0.1, 4, 4, color)
+		rl.DrawSphereWires(p, 0.1, 4, 4, color)
 	}
 }

game/physics/simulation.odin

@@ -78,6 +78,7 @@ GLOBAL_PLANE :: collision.Plane {
 Contact_Pair :: struct {
 	a, b:     Body_Handle,
 	manifold: collision.Contact_Manifold,
+	lambdas:  [4]f32,
 }
 
 simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
@@ -149,59 +150,99 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 						manifold, collision := collision.convex_vs_convex_sat(box1, box2)
 
 						if collision {
-							scene.contact_pairs[scene.contact_pairs_len] = Contact_Pair {
+							contact_pair := &scene.contact_pairs[scene.contact_pairs_len]
+							contact_pair^ = Contact_Pair {
 								a        = Body_Handle(i + 1),
 								b        = Body_Handle(j + 1),
 								manifold = manifold,
 							}
 							scene.contact_pairs_len += 1
 
-							points_a_local, points_b_local: [4]rl.Vector3
+							// Convert manifold contact from world to local space
 							for point_idx in 0 ..< manifold.points_len {
-								points_a_local[point_idx] = body_world_to_local(
+								manifold.points_a[point_idx] = body_world_to_local(
 									body,
 									manifold.points_a[point_idx],
 								)
-								points_b_local[point_idx] = body_world_to_local(
+								manifold.points_b[point_idx] = body_world_to_local(
 									body2,
 									manifold.points_b[point_idx],
 								)
 							}
 							for point_idx in 0 ..< manifold.points_len {
-								p1, p2 := points_a_local[point_idx], points_b_local[point_idx]
+								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 := min(lg.dot(p2 - p1, manifold.normal), 0)
 
 								handled_pairs[{a = min(i, j), b = max(i, j)}] = true
 
-								apply_constraint_correction_unilateral(
+								lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
 									dt,
 									body,
-									0,
-									-separation,
-									-manifold.normal,
-									p1,
-									body2_inv_mass,
-								)
-
-								apply_constraint_correction_unilateral(
-									dt,
 									body2,
 									0,
-									-separation,
+									separation,
-									manifold.normal,
+									-manifold.normal,
+									p1,
 									p2,
-									body1_inv_mass,
 								)
+								contact_pair.lambdas[point_idx] = lambda_norm
+
+								if ok {
+									apply_correction(body, corr1, p1)
+									apply_correction(body2, corr2, p2)
+								}
+
+								// Static friction
+								if ok {
+									p1, p2 =
+										body_local_to_world(body, manifold.points_a[point_idx]),
+										body_local_to_world(body2, manifold.points_b[point_idx])
+									prev_p1 :=
+										body_states[i].prev_x +
+										lg.quaternion_mul_vector3(
+											body_states[i].prev_q,
+											manifold.points_a[point_idx],
+										)
+									prev_p2 :=
+										body_states[j].prev_x +
+										lg.quaternion_mul_vector3(
+											body_states[j].prev_q,
+											manifold.points_b[point_idx],
+										)
+
+									delta_p := (p2 - prev_p2) - (p1 - prev_p1)
+									delta_p_tangent_norm :=
+										delta_p -
+										lg.dot(manifold.normal, delta_p) * manifold.normal
+									delta_p_tangent_len := lg.length(delta_p_tangent_norm)
+
+									if delta_p_tangent_len > 0 {
+										delta_p_tangent_norm /= delta_p_tangent_len
+
+										lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent :=
+											calculate_constraint_params2(
+												dt,
+												body,
+												body2,
+												0,
+												-delta_p_tangent_len,
+												delta_p_tangent_norm,
+												p1,
+												p2,
+											)
+
+										STATIC_FRICTION :: 2
+										if ok_tangent &&
+										   lambda_tangent < lambda_norm * STATIC_FRICTION {
+											apply_correction(body, corr1_tangent, p1)
+											apply_correction(body2, corr2_tangent, p2)
+										}
+									}
+								}
 							}
 						}
 					}
@@ -221,11 +262,11 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 					collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
 				)
 				if v.hit {
-					corr := v.hit_point - pos
+					corr := pos - v.hit_point
 					distance := lg.length(corr)
 					corr = corr / distance if distance > 0 else 0
 
-					apply_constraint_correction_unilateral(
+					_ = apply_constraint_correction_unilateral(
 						dt,
 						body,
 						v.compliance,
@@ -266,12 +307,12 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 						vel := lg.dot(vel_3d, dir)
 						damping := -vel * min(v.damping * dt, 1)
 
-						apply_constraint_correction_unilateral(
+						_ = apply_constraint_correction_unilateral(
 							dt,
 							body,
 							0,
 							error = damping,
-							error_gradient = dir,
+							error_gradient = -dir,
 							pos = wheel_world_pos,
 						)
 
@@ -283,12 +324,12 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 						total_impulse := v.drive_impulse - v.brake_impulse
 						forward := body_local_to_world_vec(body, rl.Vector3{0, 0, 1})
 
-						apply_constraint_correction_unilateral(
+						_ = apply_constraint_correction_unilateral(
 							dt,
 							body,
 							0,
 							total_impulse * dt * dt,
-							forward,
+							-forward,
 							wheel_world_pos,
 						)
 						body_solve_velocity(body, body_state, inv_dt)
@@ -337,6 +378,76 @@ body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State,
 	}
 }
 
+calculate_constraint_params1 :: proc(
+	dt: f32,
+	body: Body_Ptr,
+	compliance: f32,
+	error: f32,
+	error_gradient: rl.Vector3,
+	pos: rl.Vector3,
+	other_combined_inv_mass: f32,
+) -> (
+	lambda: f32,
+	w: f32,
+	correction: rl.Vector3,
+	ok: bool,
+) {
+	if error == 0 {
+		return
+	}
+
+	w = get_body_inverse_mass(body, error_gradient, pos)
+	w += other_combined_inv_mass
+
+	if w == 0 {
+		return
+	}
+
+	ok = true
+
+	alpha := compliance / dt / dt
+	lambda = -error / (w + alpha)
+	correction = -error_gradient * -lambda
+
+	return
+}
+
+calculate_constraint_params2 :: proc(
+	dt: f32,
+	body1: Body_Ptr,
+	body2: Body_Ptr,
+	compliance: f32,
+	error: f32,
+	error_gradient: rl.Vector3,
+	pos1: rl.Vector3,
+	pos2: rl.Vector3,
+) -> (
+	lambda: f32,
+	correction1: rl.Vector3,
+	correction2: rl.Vector3,
+	ok: bool,
+) {
+	if error == 0 {
+		return
+	}
+
+	w := get_body_inverse_mass(body1, error_gradient, pos1)
+	w += get_body_inverse_mass(body2, error_gradient, pos2)
+
+	if w == 0 {
+		return
+	}
+
+	ok = true
+
+	alpha := compliance / dt / dt
+	lambda = -error / (w + alpha)
+	correction1 = -error_gradient * -lambda
+	correction2 = error_gradient * -lambda
+
+	return
+}
+
 apply_constraint_correction_unilateral :: proc(
 	dt: f32,
 	body: Body_Ptr,
@@ -345,24 +456,27 @@ apply_constraint_correction_unilateral :: proc(
 	error_gradient: rl.Vector3,
 	pos: rl.Vector3,
 	other_combined_inv_mass: f32 = 0,
+) -> (
+	lambda: f32,
 ) {
-	if error == 0 {
+	w: f32
-		return
+	correction: rl.Vector3
+	ok: bool
+	lambda, w, correction, ok = calculate_constraint_params1(
+		dt,
+		body,
+		compliance,
+		error,
+		error_gradient,
+		pos,
+		other_combined_inv_mass,
+	)
+
+	if ok {
+		apply_correction(body, correction, pos)
 	}
 
-	w := get_body_inverse_mass(body, error_gradient, pos)
+	return
-	w += other_combined_inv_mass
-
-	if w == 0 {
-		return
-	}
-
-	alpha := compliance / dt / dt
-	lambda := -error / (w + alpha)
-
-	delta_pos := error_gradient * -lambda
-
-	apply_correction(body, delta_pos, pos)
 }
 
 apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
@@ -395,11 +509,7 @@ get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
 	rn = lg.cross(rn, normal)
 	rn = lg.quaternion_mul_vector3(inv_q, rn)
 
-	w :=
+	w := lg.dot(rn * rn, body.inv_inertia_tensor)
-		rn.x * rn.x * body.inv_inertia_tensor.x +
-		rn.y * rn.y * body.inv_inertia_tensor.y +
-		rn.z * rn.z * body.inv_inertia_tensor.z
-
 	w += body.inv_mass
 
 	return w