Static and dynamic friction working, it's starting to look pretty convincing

Still got a bug when colliding with edges at some specific rotations. Probably fucked relative position conversion somewhere and using a local pos instead

game/game.odin

@@ -370,8 +370,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 		drive_wheels := []physics.Suspension_Constraint_Handle{wheel_rl, wheel_rr}
 		turn_wheels := []physics.Suspension_Constraint_Handle{wheel_fl, wheel_fr}
 
-		DRIVE_IMPULSE :: 20
-		BRAKE_IMPULSE :: 50
+		DRIVE_IMPULSE :: 5
+		BRAKE_IMPULSE :: 20
 		TURN_ANGLE :: -f32(30) * math.RAD_PER_DEG
 
 		for wheel_handle in drive_wheels {

game/physics/collision/convex.odin

@@ -531,7 +531,7 @@ create_edge_contact_manifold :: proc(
 
 	_, ps := closest_point_between_segments(a1, a2, b1, b2)
 
-	manifold.normal = lg.normalize0(ps[1] - ps[0])
+	manifold.normal = separating_plane.normal
 	manifold.separation = separation
 	manifold.points_a[0] = ps[0]
 	manifold.points_b[0] = ps[1]

game/physics/debug.odin

@@ -99,7 +99,11 @@ draw_debug_scene :: proc(scene: ^Scene) {
 		}
 	}
 
+	if false {
 		for &contact, contact_idx in scene.contact_pairs[:scene.contact_pairs_len] {
+			points_a := contact.manifold.points_a[:contact.manifold.points_len]
+			points_b := contact.manifold.points_b[:contact.manifold.points_len]
+			debug_transform_points_local_to_world(get_body(scene, contact.a), points_a)
 			debug_draw_manifold_points(
 				-contact.manifold.normal,
 				contact.manifold.points_a[:contact.manifold.points_len],
@@ -107,11 +111,18 @@ draw_debug_scene :: proc(scene: ^Scene) {
 			)
 			debug_draw_manifold_points(
 				contact.manifold.normal,
-			contact.manifold.points_b[:contact.manifold.points_len],
+				points_b,
 				color = debug.int_to_color(i32(contact_idx * 2 + 1)),
 			)
 		}
 	}
+}
+
+debug_transform_points_local_to_world :: proc(body: Body_Ptr, points: []rl.Vector3) {
+	for i in 0 ..< len(points) {
+		points[i] = body_local_to_world(body, points[i])
+	}
+}
 
 debug_draw_manifold_points :: proc(normal: rl.Vector3, points: []rl.Vector3, color: rl.Color) {
 	if len(points) >= 3 {

game/physics/helpers.odin

@@ -50,18 +50,8 @@ body_world_to_local_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -
 	return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec))
 }
 
-body_angular_velocity_at_local_point :: #force_inline proc(
-	body: Body_Ptr,
-	rel_pos: rl.Vector3,
-) -> rl.Vector3 {
-	return lg.cross(body.w, rel_pos)
-}
-
-body_velocity_at_local_point :: #force_inline proc(
-	body: Body_Ptr,
-	rel_pos: rl.Vector3,
-) -> rl.Vector3 {
-	return body.v + body_angular_velocity_at_local_point(body, rel_pos)
+body_velocity_at_point :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
+	return body.v + lg.cross(body.w, pos - body.x)
 }
 
 wheel_get_rel_wheel_pos :: #force_inline proc(

game/physics/simulation.odin

@@ -78,7 +78,8 @@ GLOBAL_PLANE :: collision.Plane {
 Contact_Pair :: struct {
 	a, b:           Body_Handle,
 	manifold:       collision.Contact_Manifold,
-	lambdas:  [4]f32,
+	lambda_normal:  [4]f32,
+	lambda_tangent: [4]f32,
 }
 
 simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
@@ -147,16 +148,18 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 						halfedge.transform_mesh(&box1, mat1)
 						halfedge.transform_mesh(&box2, mat2)
 
-						manifold, collision := collision.convex_vs_convex_sat(box1, box2)
+						// Raw manifold has contact points in world space
+						raw_manifold, collision := collision.convex_vs_convex_sat(box1, box2)
 
 						if collision {
 							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,
+								manifold = raw_manifold,
 							}
 							scene.contact_pairs_len += 1
+							manifold := &contact_pair.manifold
 
 							// Convert manifold contact from world to local space
 							for point_idx in 0 ..< manifold.points_len {
@@ -189,7 +192,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 									p1,
 									p2,
 								)
-								contact_pair.lambdas[point_idx] = lambda_norm
+								contact_pair.lambda_normal[point_idx] = lambda_norm
 
 								if ok {
 									apply_correction(body, corr1, p1)
@@ -234,8 +237,9 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 												p1,
 												p2,
 											)
+										contact_pair.lambda_tangent[point_idx] = lambda_tangent
 
-										STATIC_FRICTION :: 2
+										STATIC_FRICTION :: 0.5
 										if ok_tangent &&
 										   lambda_tangent < lambda_norm * STATIC_FRICTION {
 											apply_correction(body, corr1_tangent, p1)
@@ -281,9 +285,50 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 
 		solve_velocities(scene, body_states, inv_dt)
 
-		// for pair in scene.contact_pairs[:scene.contact_pairs_len] {
-		//   pair.a
-		// }
+		for &pair in scene.contact_pairs[:scene.contact_pairs_len] {
+			manifold := &pair.manifold
+			body1 := get_body(scene, pair.a)
+			body2 := get_body(scene, pair.b)
+
+			for point_idx in 0 ..< pair.manifold.points_len {
+				p1, p2 :=
+					body_local_to_world(body1, manifold.points_a[point_idx]),
+					body_local_to_world(body2, manifold.points_b[point_idx])
+				r1, r2 := p1 - body1.x, p2 - body2.x
+				v1 := body_velocity_at_point(body1, p1)
+				v2 := body_velocity_at_point(body2, p2)
+
+				v := v1 - v2
+				v_normal := lg.dot(manifold.normal, v) * manifold.normal
+				v_tangent := v - v_normal
+
+				DYNAMIC_FRICTION :: 0.4
+				v_tangent_len := lg.length(v_tangent)
+				if v_tangent_len > 0 {
+					delta_v :=
+						-(v_tangent / v_tangent_len) *
+						min(
+							dt * DYNAMIC_FRICTION * abs(pair.lambda_normal[point_idx] / (dt * dt)),
+							v_tangent_len,
+						)
+
+					w1, w2 :=
+						get_body_inverse_mass(body1, manifold.normal, p1),
+						get_body_inverse_mass(body2, manifold.normal, p2)
+
+					w := w1 + w2
+					if w != 0 {
+						p := delta_v / w
+
+						body1.v += p * body1.inv_mass
+						body2.v -= p * body2.inv_mass
+
+						body1.w += multiply_inv_mass(body1, lg.cross(r1, p))
+						body2.w -= multiply_inv_mass(body2, lg.cross(r2, p))
+					}
+				}
+			}
+		}
 
 		// Solve suspension velocity
 		for _, i in scene.suspension_constraints {
@@ -337,8 +382,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 
 					// Lateral friction
 					if true {
-						local_contact_pos := v.hit_point - body.x
-						vel_contact := body_velocity_at_local_point(body, local_contact_pos)
+						vel_contact := body_velocity_at_point(body, v.hit_point)
 						right := wheel_get_right_vec(body, v)
 
 						lateral_vel := lg.dot(right, vel_contact)
@@ -479,6 +523,17 @@ apply_constraint_correction_unilateral :: proc(
 	return
 }
 
+multiply_inv_mass :: proc(body: Body_Ptr, vec: rl.Vector3) -> (result: rl.Vector3) {
+	q := body.q
+	inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q))
+
+	result = lg.quaternion_mul_vector3(inv_q, vec)
+	result *= body.inv_inertia_tensor
+	result = lg.quaternion_mul_vector3(q, result)
+
+	return result
+}
+
 apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
 	body.x += corr * body.inv_mass