From aca890bcb77b82abdf52e1ff79c4522569f4f604 Mon Sep 17 00:00:00 2001
From: sergeypdev <sergeypoznyak@protonmail.com>
Date: Sun, 26 Jan 2025 17:31:28 +0400
Subject: [PATCH] Super stable collision resolution

- Store collision manifold points as local to body before applying them, this way penetration can be calculated exactly for each contact point
- Fix implementation error in closest_point_between_segments
- Calculate separation for each contact point separately
---
 game/physics/collision/collision.odin |  8 +++-----
 game/physics/collision/convex.odin    |  4 ++--
 game/physics/simulation.odin          | 17 ++++++++---------
 3 files changed, 13 insertions(+), 16 deletions(-)

diff --git a/game/physics/collision/collision.odin b/game/physics/collision/collision.odin
index 315ae15..88001bd 100644
--- a/game/physics/collision/collision.odin
+++ b/game/physics/collision/collision.odin
@@ -201,19 +201,17 @@ closest_point_between_segments :: proc(p1, q1, p2, q2: Vec3) -> (t: [2]f32, poin
 			}
 			// Compute point on L2 closest to S1(s) using
 			// t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
-			tnom := (b * t[0] + f)
+			t[1] = (b * t[0] + f) / e
 
 			// If t in [0,1] done. Else clamp t, recompute s for the new value
 			// of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
 			// and clamp s to [0, 1]
-			if tnom < 0 {
+			if t[1] < 0 {
 				t[1] = 0
 				t[0] = clamp(-c / a, 0, 1)
-			} else if tnom > 1 {
+			} else if t[1] > 1 {
 				t[1] = 1
 				t[0] = clamp((b - c) / a, 0, 1)
-			} else {
-				t[1] = tnom / e
 			}
 		}
 	}
diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin
index 08c0a61..c797790 100644
--- a/game/physics/collision/convex.odin
+++ b/game/physics/collision/convex.odin
@@ -531,10 +531,10 @@ create_edge_contact_manifold :: proc(
 
 	_, ps := closest_point_between_segments(a1, a2, b1, b2)
 
-	manifold.normal = 0
+	manifold.normal = lg.normalize0(ps[1] - ps[0])
 	manifold.separation = separation
 	manifold.points_a[0] = ps[0]
-	manifold.points_b[0] = ps[0]
+	manifold.points_b[0] = ps[1]
 	manifold.points_len = 1
 
 	return
diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin
index ed26937..83dd182 100644
--- a/game/physics/simulation.odin
+++ b/game/physics/simulation.odin
@@ -158,20 +158,19 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 
 							points_a_local, points_b_local: [4]rl.Vector3
 							for point_idx in 0 ..< manifold.points_len {
-								points_a_local = body_world_to_local(
+								points_a_local[point_idx] = body_world_to_local(
 									body,
 									manifold.points_a[point_idx],
 								)
-								points_b_local = body_world_to_local(
+								points_b_local[point_idx] = 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)
+								p1, p2 := points_a_local[point_idx], points_b_local[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)
 
@@ -180,7 +179,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 								if length != 0 {
 									diff /= length
 								}
-								// separation := lg.dot(p2 - p1, manifold.normal)
+								separation := min(lg.dot(p2 - p1, manifold.normal), 0)
 
 								handled_pairs[{a = min(i, j), b = max(i, j)}] = true
 
@@ -188,7 +187,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 									dt,
 									body,
 									0,
-									-manifold.separation,
+									-separation,
 									-manifold.normal,
 									p1,
 									body2_inv_mass,
@@ -198,7 +197,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 									dt,
 									body2,
 									0,
-									-manifold.separation,
+									-separation,
 									manifold.normal,
 									p2,
 									body1_inv_mass,