Implement restitution, better hack for static friction

assets/cube.obj

@@ -0,0 +1,16 @@
+# Blender 3.5.0 Alpha
+# www.blender.org
+v 1 1 -1
+v 1 -1 -1
+v 1 1 1
+v 1 -1 1
+v -1 1 -1
+v -1 -1 -1
+v -1 1 1
+v -1 -1 1
+f 1 5 7 3
+f 4 3 7 8
+f 8 7 5 6
+f 6 2 4 8
+f 2 1 3 4
+f 6 5 1 2

game/game.odin

@@ -67,9 +67,9 @@ Car :: struct {
 }
 
 SOLVER_CONFIG :: physics.Solver_Config {
-	timestep            = 1.0 / 120,
+	timestep            = 1.0 / 60,
 	gravity             = rl.Vector3{0, -9.8, 0},
-	substreps_minus_one = 2 - 1,
+	substreps_minus_one = 4 - 1,
 }
 
 Game_Memory :: struct {
@@ -270,14 +270,14 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 
 		if true {
 
-			for x in -3 ..< 3 {
+			for x in 0 ..< 1 {
-				for y in -3 ..< 3 {
+				for y in -3 ..< 4 {
 					physics.immediate_body(
 						&world.physics_scene,
 						&runtime_world.solver_state,
 						hash.fnv32a(slice.to_bytes([]int{(x | y << 8)})),
 						physics.Body_Config {
-							initial_pos = {f32(x) * 3 + 20, 5 + f32(y) * 2, 0},
+							initial_pos = {f32(x) * 3 - 10, 5 + f32(y) * 3, 0},
 							initial_rot = linalg.QUATERNIONF32_IDENTITY,
 							shape = physics.Shape_Box{size = 1},
 							mass = 10,

game/physics/collision/convex.odin

@@ -444,7 +444,7 @@ create_face_contact_manifold :: proc(
 		for i in 0 ..< vert_count {
 			d := signed_distance_plane(src_polygon[i], ref_plane)
 
-			if d <= EPS {
+			if d <= 1e-03 {
 				clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal
 				j += 1
 			}

game/physics/simulation.odin

@@ -68,6 +68,8 @@ simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt:
 
 Body_Sim_State :: struct {
 	prev_x: rl.Vector3,
+	prev_v: rl.Vector3,
+	prev_w: rl.Vector3,
 	prev_q: rl.Quaternion,
 }
 
@@ -84,6 +86,8 @@ Contact_Pair :: struct {
 	applied_corrections:       int,
 	lambda_normal:             [4]f32,
 	lambda_tangent:            [4]f32,
+	applied_static_friction:   [4]bool,
+	applied_normal_correction: [4]f32,
 }
 
 simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
@@ -103,11 +107,13 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 		for &body, i in scene.bodies {
 			if body.alive {
 				body_states[i].prev_x = body.x
+				body_states[i].prev_v = body.v
+				body_states[i].prev_w = body.w
+				body_states[i].prev_q = body.q
+
 				body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO
 				body.x += body.v * dt
 
-				body_states[i].prev_q = body.q
-
 				// NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
 				q := body.q
 
@@ -197,6 +203,8 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 										p2,
 									)
 									if ok {
+										contact_pair.applied_normal_correction[point_idx] =
+										-separation
 										contact_pair.applied_corrections += 1
 										contact_pair.lambda_normal[point_idx] = lambda_norm
 
@@ -214,8 +222,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 		{
 			tracy.ZoneN("simulate_step::static_friction")
 
-			if true {
+			if false {
-
 				context = context
 				context.user_ptr = scene
 				slice.sort_by(
@@ -253,9 +260,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 				body, body2 := get_body(scene, contact_pair.a), get_body(scene, contact_pair.b)
 				i, j := int(contact_pair.a) - 1, int(contact_pair.b) - 1
 
-				lambda_tangent: f32
 				for point_idx in 0 ..< manifold.points_len {
-
 					lambda_norm := contact_pair.lambda_normal[point_idx]
 					if lambda_norm != 0 {
 						p1 := body_local_to_world(body, manifold.points_a[point_idx])
@@ -287,18 +292,17 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 									body,
 									body2,
 									0,
-									-tangent_diff_len / f32(contact_pair.applied_corrections),
+									-tangent_diff_len / max(f32(contact_pair.applied_corrections) * 0.5, 1),
 									-tangent_diff_normalized,
 									p1,
 									p2,
 								)
-							contact_pair.lambda_tangent[point_idx] = lambda_tangent
 
-							new_lambda_tangent := lambda_tangent + delta_lambda_tangent
+							STATIC_FRICTION :: 0.6
-
-							STATIC_FRICTION :: 0.7
 							if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm {
-								lambda_tangent = new_lambda_tangent
+								contact_pair.applied_static_friction[point_idx] = true
+								contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent
+
 								apply_correction(body, corr1_tangent, p1)
 								apply_correction(body2, corr2_tangent, p2)
 							}
@@ -343,6 +347,69 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 
 		solve_velocities(scene, body_states, inv_dt)
 
+		// Restituion
+		{
+			tracy.ZoneN("simulate_step::restitution")
+
+			for &pair in scene.contact_pairs[:scene.contact_pairs_len] {
+				i, j := int(pair.a) - 1, int(pair.b) - 1
+				manifold := &pair.manifold
+
+				body, body2 := get_body(scene, pair.a), get_body(scene, pair.b)
+				s1, s2 := body_states[i], body_states[j]
+				prev_q1, prev_q2 := s1.prev_q, s2.prev_q
+
+				for point_idx in 0..<manifold.points_len {
+					if pair.lambda_normal == 0 {
+						continue
+					}
+					prev_r1 :=
+						lg.quaternion_mul_vector3(
+							prev_q1,
+							manifold.points_a[point_idx],
+						)
+					prev_r2 :=
+						lg.quaternion_mul_vector3(
+							prev_q2,
+							manifold.points_b[point_idx],
+						)
+
+					r1 := lg.quaternion_mul_vector3(body.q, manifold.points_a[point_idx])
+					r2 := lg.quaternion_mul_vector3(body2.q, manifold.points_b[point_idx])
+
+					prev_v := (s1.prev_v + lg.cross(s1.prev_w, prev_r1)) - (s2.prev_v + lg.cross(s2.prev_w, prev_r2))
+					v := (body.v + lg.cross(body.w, r1)) - (body2.v + lg.cross(body2.w, r2))
+
+					prev_v_normal := lg.dot(prev_v, manifold.normal)
+					v_normal := lg.dot(v, manifold.normal)
+
+					RESTITUTION :: 0.5
+
+					restitution := f32(RESTITUTION)
+
+					if abs(v_normal) <= 2 * abs(lg.dot(manifold.normal, -config.gravity) * dt) {
+						restitution = 0
+					}
+
+					delta_v := manifold.normal * (-v_normal + min(-RESTITUTION * prev_v_normal, 0))
+
+					w1 := get_body_inverse_mass(body, manifold.normal, r1 + body.x)
+					w2 := get_body_inverse_mass(body2, manifold.normal, r2 + body.x)
+					w := w1 + w2
+
+					if w != 0 {
+						p := delta_v / w
+
+						body.v += p * body.inv_mass
+						body2.v -= p * body2.inv_mass
+
+						body.w += multiply_inv_intertia(body, lg.cross(r1, p))
+						body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p))
+					}
+				}
+			}
+		}
+
 		if true {
 			tracy.ZoneN("simulate_step::dynamic_friction")
 
@@ -352,6 +419,9 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 				body2 := get_body(scene, pair.b)
 
 				for point_idx in 0 ..< pair.manifold.points_len {
+					if pair.applied_static_friction[point_idx] {
+						continue
+					}
 					p1, p2 :=
 						body_local_to_world(body1, manifold.points_a[point_idx]),
 						body_local_to_world(body2, manifold.points_b[point_idx])