Interesting... collision response

game/game.odin

@@ -232,6 +232,16 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
 		car_bounds := rl.GetModelBoundingBox(car_model)
 		runtime_world.car_com = (car_bounds.min + car_bounds.max) / 2
 
+		physics.immediate_body(
+			&world.physics_scene,
+			&runtime_world.solver_state,
+			#hash("floor", "fnv32a"),
+			physics.Body_Config {
+				initial_pos = {0, -0.5, 0},
+				shape = physics.Shape_Box{size = {100, 1, 100}},
+			},
+		)
+
 		runtime_world.car_handle = physics.immediate_body(
 			&world.physics_scene,
 			&runtime_world.solver_state,

game/physics/collision/convex.odin

@@ -41,6 +41,7 @@ box_to_convex :: proc(box: Box, allocator := context.allocator) -> (convex: Conv
 
 Contact_Manifold :: struct {
 	normal:     Vec3,
+	separation: f32,
 	points:     [4]Vec3,
 	points_len: int,
 }
@@ -60,18 +61,12 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
 	if edge_separation > 0 {
 		return
 	}
-	face_query_a.separation += 0.1
+	biased_face_a_separation := face_query_a.separation + 0.1
-	edge_separation -= 0.1
+	biased_face_b_separation := face_query_b.separation
+	biased_edge_separation := edge_separation - 0.1
 
-	is_face_a_contact := face_query_a.separation >= edge_separation
+	is_face_a_contact := biased_face_a_separation >= biased_edge_separation
-	is_face_b_contact := face_query_b.separation >= edge_separation
+	is_face_b_contact := biased_face_b_separation >= biased_edge_separation
-
-	log.infof(
-		"face_a_sep: %v, face_b_sep: %v, edge_sep: %v",
-		face_query_a.separation,
-		face_query_b.separation,
-		edge_separation,
-	)
 
 	collision = true
 	if is_face_a_contact && is_face_b_contact {
@@ -212,7 +207,9 @@ query_separation_edges :: proc(
 					}
 				}
 
-				assert(vert_b_edge_idx >= 0, "couldn't find the edge on convex B")
+				if vert_b_edge_idx < 0 {
+					continue
+				}
 			}
 
 			distance_a := signed_distance_plane(vert_a.pos, plane_a)
@@ -304,7 +301,7 @@ create_face_contact_manifold :: proc(
 			original_vert_count += 1
 		}
 
-		inc_polygon = make([]Vec3, original_vert_count * 2, context.temp_allocator)
+		inc_polygon = make([]Vec3, original_vert_count * 4, context.temp_allocator)
 
 		halfedge.iterator_reset_edges(&it)
 
@@ -480,7 +477,9 @@ create_face_contact_manifold :: proc(
 		assert(len(full_clipped_polygon) <= 4)
 	}
 
-	manifold.normal = ref_face.normal
+	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
 
 	return
 }
@@ -495,11 +494,10 @@ create_edge_contact_manifold :: proc(
 	a1, a2 := halfedge.get_edge_points(a, a.edges[edge_a])
 	b1, b2 := halfedge.get_edge_points(b, b.edges[edge_b])
 
-	rl.DrawLine3D(a1 + 0.1, a2 + 0.1, rl.ORANGE)
-	rl.DrawLine3D(b1 + 0.1, b2 + 0.1, rl.BLUE)
-
 	_, ps := closest_point_between_segments(a1, a2, b1, b2)
 
+	manifold.normal = lg.normalize0(ps[1] - ps[0])
+	manifold.separation = separation
 	manifold.points[0] = (ps[0] + ps[1]) * 0.5
 	manifold.points_len = 1
 

game/physics/simulation.odin

@@ -4,6 +4,7 @@ import "collision"
 import "core:fmt"
 import "core:math"
 import lg "core:math/linalg"
+import "game:halfedge"
 import rl "vendor:raylib"
 
 _ :: math
@@ -108,54 +109,56 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 		for _, i in scene.bodies {
 			body := &scene.bodies_slice[i]
 			if body.alive {
-				normal := GLOBAL_PLANE.normal
+				for _, j in scene.bodies {
-				penetration: f32
+					body2 := &scene.bodies_slice[j]
-				hit_point: rl.Vector3
-				hit := false
 
-				switch s in body.shape {
+					if body2.alive {
-				case Shape_Box:
+						s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box)
-					extent := s.size * 0.5
-					local_plane := collision.Plane {
-						normal = body_world_to_local_vec(body, GLOBAL_PLANE.normal),
-						dist   = -lg.dot(GLOBAL_PLANE.normal, body.x) - GLOBAL_PLANE.dist,
-					}
 
-					penetration, hit = collision.test_box_vs_halfspace(
+						box1 := collision.box_to_convex(
-						collision.Box{pos = 0, rad = extent},
+							collision.Box{rad = s1.size * 0.5},
-						local_plane,
+							context.temp_allocator,
 						)
-					if hit {
+						box2 := collision.box_to_convex(
-						local_hit_point: rl.Vector3
+							collision.Box{rad = s2.size * 0.5},
-						min_distance := f32(math.F32_MAX)
+							context.temp_allocator,
-						for corner in collision.BOX_CORNERS_NORM {
-							local_pos := extent * corner
-							dist := collision.signed_distance_plane(local_pos, local_plane)
-							if dist < min_distance {
-								min_distance = dist
-								local_hit_point = local_pos
-							}
-						}
-						hit_point = body_local_to_world(body, local_hit_point)
-						penetration = -min(min_distance, 0)
-					}
-				case Shape_Sphere:
-					penetration, hit = collision.test_sphere_vs_halfspace(
-						collision.Sphere{pos = body.x, rad = s.radius},
-						GLOBAL_PLANE,
 						)
-				}
 
-				if hit {
+						mat1 := lg.matrix4_translate(body.x) * lg.matrix4_from_quaternion(body.q)
+						mat2 := lg.matrix4_translate(body2.x) * lg.matrix4_from_quaternion(body2.q)
+
+						halfedge.transform_mesh(&box1, mat1)
+						halfedge.transform_mesh(&box2, mat2)
+
+						manifold, collision := collision.convex_vs_convex_sat(box1, box2)
+
+						if collision {
+							for p in manifold.points[:manifold.points_len] {
+								body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p)
+								body2_inv_mass := get_body_inverse_mass(body2, manifold.normal, p)
+
 								apply_constraint_correction_unilateral(
 									dt,
 									body,
 									0,
-						penetration,
+									-manifold.separation,
-						normal,
+									manifold.normal,
-						hit_point,
+									p,
-						0,
+									body2_inv_mass,
 								)
+
+								apply_constraint_correction_unilateral(
+									dt,
+									body2,
+									0,
+									-manifold.separation,
+									-manifold.normal,
+									p,
+									body1_inv_mass,
+								)
+							}
+						}
+					}
 				}
 			}
 		}