Start implementing rigid body collision

build_debug.sh

@@ -1,3 +1,3 @@
 #!/usr/bin/env bash
 
-odin build main_release -collection:common=./common -collection:game=./game -out:game_debug.bin -strict-style -vet -debug
+odin build main_release -collection:common=./common -collection:game=./game -collection:libs=./libs -out:game_debug.bin -strict-style -vet -debug

game/assets/assets.odin

@@ -86,7 +86,7 @@ get_model_ex :: proc(
 	if ok && existing.modtime == new_modtime {
 		return existing.model,
 			existing.modtime,
-			ref_modtime == 0 ? false : existing.modtime == ref_modtime
+			ref_modtime == 0 ? false : existing.modtime != ref_modtime
 	}
 
 	if ok {
@@ -123,16 +123,6 @@ get_bvh :: proc(assetman: ^Asset_Manager, path: cstring) -> Loaded_BVH {
 
 	should_recreate := reloaded || !ok
 
-	log.debugf(
-		"model %v\nmodtime %v\nreloaded %v\nok %v\nshould_recreate %v\nref_modtime %v",
-		model,
-		modtime,
-		reloaded,
-		ok,
-		should_recreate,
-		loaded_bvh.modtime,
-	)
-
 	if ok && should_recreate {
 		destroy_loaded_bvh(loaded_bvh)
 		delete_key(&assetman.bvhs, path)

game/game.odin

@@ -63,7 +63,7 @@ Car :: struct {
 }
 
 SOLVER_CONFIG :: physics.Solver_Config {
-	timestep            = 1.0 / 104,
+	timestep            = 1.0 / 120,
 	gravity             = rl.Vector3{0, -9.8, 0},
 	substreps_minus_one = 4 - 1,
 }

game/physics/bvh/bvh.odin

@@ -8,6 +8,7 @@ import "core:math"
 import lg "core:math/linalg"
 import "core:mem"
 import "game:debug"
+import "libs:tracy"
 import rl "vendor:raylib"
 
 _ :: log
@@ -62,6 +63,8 @@ is_leaf_node :: #force_inline proc(node: Node) -> bool {
 #assert(size_of(Node) == 32)
 
 build_bvh_from_mesh :: proc(mesh: Mesh, allocator := context.allocator) -> (mesh_bvh: Mesh_BVH) {
+	tracy.Zone()
+
 	vertices, indices := mesh.vertices, mesh.indices
 	assert(len(indices) % 3 == 0)
 
@@ -82,14 +85,25 @@ build_bvh_from_mesh :: proc(mesh: Mesh, allocator := context.allocator) -> (mesh
 	aabbs := make([]AABB, num_triangles, context.temp_allocator)
 
 	// Calculate centroids and aabbs
+	{
+		tracy.ZoneN("calculate_centroids_and_aabbs")
+
 		for i in 0 ..< num_triangles {
 			i1, i2, i3 := indices[i * 3], indices[i * 3 + 1], indices[i * 3 + 2]
 			v1, v2, v3 := vertices[i1], vertices[i2], vertices[i3]
 
 			centroids[i] = (v1 + v2 + v3) * 0.33333333333
 
-		aabbs[i].min = Vec3{min(v1.x, v2.x, v3.x), min(v1.y, v2.y, v3.y), min(v1.z, v2.z, v3.z)}
-		aabbs[i].max = Vec3{max(v1.x, v2.x, v3.x), max(v1.y, v2.y, v3.y), max(v1.z, v2.z, v3.z)}
+			aabbs[i].min = Vec3 {
+				min(v1.x, v2.x, v3.x),
+				min(v1.y, v2.y, v3.y),
+				min(v1.z, v2.z, v3.z),
+			}
+			aabbs[i].max = Vec3 {
+				max(v1.x, v2.x, v3.x),
+				max(v1.y, v2.y, v3.y),
+				max(v1.z, v2.z, v3.z),
+			}
 
 			size := aabbs[i].max - aabbs[i].min
 			assert(size.x >= 0)
@@ -98,6 +112,7 @@ build_bvh_from_mesh :: proc(mesh: Mesh, allocator := context.allocator) -> (mesh
 
 			bvh.primitives[i] = u16(i)
 		}
+	}
 
 	bvh.nodes_used = 1 // root
 
@@ -142,6 +157,8 @@ build_bvh_from_aabbs :: proc(aabbs: []AABB, allocator := context.allocator) -> (
 }
 
 update_node_bounds :: proc(bvh: ^BVH, node_idx: i32, prim_aabbs: []AABB) {
+	tracy.Zone()
+
 	node := &bvh.nodes[node_idx]
 
 	node.aabb.min = math.F32_MAX
@@ -161,6 +178,8 @@ update_node_bounds :: proc(bvh: ^BVH, node_idx: i32, prim_aabbs: []AABB) {
 }
 
 subdivide :: proc(bvh: ^BVH, node_idx: i32, centroids: []Vec3, aabbs: []AABB) {
+	tracy.Zone()
+
 	node := &bvh.nodes[node_idx]
 
 	if node.prim_len <= 2 {
@@ -244,6 +263,8 @@ Collision :: struct {
 }
 
 traverse_bvh_ray_mesh :: proc(bvh: ^BVH, mesh: Mesh, ray: Ray, out_collision: ^Collision) -> bool {
+	tracy.Zone()
+
 	ray := ray
 	ray.dir_inv.x = 1.0 / ray.dir.x
 	ray.dir_inv.y = 1.0 / ray.dir.y

game/physics/collision/collision.odin

@@ -90,6 +90,15 @@ signed_distance_plane :: proc(point: Vec3, plane: Plane) -> f32 {
 	// return (dot(plane.normal, point) - plane.dist) / Ddt(plane.normal, plane.normal);
 }
 
+signed_distance_box_plane :: proc(box: Box, plane: Plane) -> f32 {
+	// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
+	r :=
+		box.rad.x * abs(plane.normal.x) +
+		box.rad.y * abs(plane.normal.y) +
+		box.rad.z * abs(plane.normal.z)
+	return signed_distance_plane(box.pos, plane) - r
+}
+
 squared_distance_aabb :: proc(point: Vec3, aabb: Aabb) -> (dist: f32) {
 	for i in 0 ..< 3 {
 		// For each axis count any excess distance outside box extents
@@ -290,19 +299,15 @@ test_point_vs_halfspace :: proc(pos: Vec3, plane: Plane) -> bool {
 	return signed_distance_plane(pos, plane) <= 0.0
 }
 
-test_sphere_vs_halfspace :: proc(sphere: Sphere, plane: Plane) -> bool {
-	dist := signed_distance_plane(sphere.pos, plane)
-	return dist <= sphere.rad
+test_sphere_vs_halfspace :: proc(sphere: Sphere, plane: Plane) -> (penetration: f32, hit: bool) {
+	dist := signed_distance_plane(sphere.pos, plane) - sphere.rad
+	return -dist, dist <= 0
 }
 
-test_box_vs_plane :: proc(box: Box, plane: Plane) -> bool {
+test_box_vs_halfspace :: proc(box: Box, plane: Plane) -> (penetration: f32, hit: bool) {
 	// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
-	r :=
-		box.rad.x * abs(plane.normal.x) +
-		box.rad.y * abs(plane.normal.y) +
-		box.rad.z * abs(plane.normal.z)
-	s := signed_distance_plane(box.pos, plane)
-	return abs(s) <= r
+	s := signed_distance_box_plane(box, plane)
+	return -s, s <= 0
 }
 
 test_capsule_vs_capsule :: proc(a, b: Capsule) -> bool {

game/physics/debug.odin

@@ -4,6 +4,7 @@ import "core:log"
 import "core:math"
 import lg "core:math/linalg"
 import "game:debug"
+import "libs:tracy"
 import rl "vendor:raylib"
 import "vendor:raylib/rlgl"
 
@@ -32,12 +33,14 @@ draw_debug_shape :: proc(
 		rlgl.LoadIdentity()
 		rlgl.MultMatrixf(cast([^]f32)&mat)
 
-		rl.DrawCubeV(0, s.size, color)
+		rl.DrawCubeWiresV(0, s.size, color)
 	}
 }
 
 draw_debug_scene :: proc(scene: ^Scene) {
-	for &body in scene.bodies {
+	tracy.Zone()
+
+	for &body, i in scene.bodies {
 		if body.alive {
 			pos := body.x
 
@@ -50,7 +53,7 @@ draw_debug_scene :: proc(scene: ^Scene) {
 			rl.DrawLine3D(pos, pos + y, rl.GREEN)
 			rl.DrawLine3D(pos, pos + z, rl.BLUE)
 
-			// draw_debug_shape(body.shape, body.x, body.q, debug.int_to_color(i32(i + 1)))
+			draw_debug_shape(body.shape, body.x, body.q, debug.int_to_color(i32(i + 1)))
 		}
 	}
 

game/physics/halfedge/halfedge.odin

@@ -0,0 +1,89 @@
+package halfedge
+
+import "core:container/small_array"
+
+Vec3 :: [3]f32
+
+Vertex :: struct {
+	pos:  Vec3,
+	edge: i16,
+}
+
+Face :: struct {
+	edge: i16,
+}
+
+Vertex_Index :: distinct u16
+Face_Index :: distinct i16
+Edge_Index :: distinct i16
+
+Half_Edge :: struct {
+	origin: Vertex_Index,
+	twin:   Edge_Index,
+	face:   Face_Index,
+	next:   Edge_Index,
+	prev:   Edge_Index,
+}
+
+Mesh :: struct {
+  vertices: []Vertex,
+  faces: []Face,
+  edges: []Half_Edge,
+}
+
+mesh_from_vertex_index_list :: proc(vertices: []Vec3, indices: []u16, allocator := context.allocator) {
+  vertices: [dynamic]Vertex
+  faces: [dynamic]Face
+  edges: [dynamic]Half_Edge
+
+  temp_edges: map[[2]u16]small_array.Small_Array(2, Edge_Index)
+
+  triangle_num := len(indices) / 3
+  for i in 0..<triangle_num {
+    i1, i2, i3 := indices[i * 3 + 0], indices[i * 3 + 1], indices[i * 3 + 2]
+    v1, v2, v3 := vertices[i1], vertices[i2], vertices[i3]
+
+    e1 := len(edges)
+    e2, e3 := e1 + 1, e1 + 2
+
+    new_edges := [3]Half_Edge{
+      {
+        origin = Vertex_Index(i1),
+        twin = -1,
+        face = Face_Index(i),
+        next = Edge_Index(e2),
+        prev = Edge_Index(e3),
+      },
+      {
+        origin = Vertex_Index(i2),
+        twin = -1,
+        face = Face_Index(i),
+        next = Edge_Index(e3),
+        prev = Edge_Index(e1),
+      },
+      {
+        origin = Vertex_Index(i3),
+        twin = -1,
+        face = Face_Index(i),
+        next = Edge_Index(e1),
+        prev = Edge_Index(e2),
+      },
+    }
+
+    append(&edges, ..new_edges[:])
+
+    temp_indices: [][2]u16 = {
+      {min(i1, i2), max(i1, i2)},
+      {min(i2, i3), max(i2, i3)},
+      {min(i3, i1), max(i3, i1)},
+    }
+
+    for tmp_idx, j in temp_indices {
+      assert(temp_edges[tmp_idx].len < 2)
+
+      if temp_edges[tmp_idx].len > 0 {
+        // small_array.push_back(Edge_Index(e1 + j))
+      }
+    }
+  }
+}

game/physics/simulation.odin

@@ -65,6 +65,22 @@ Body_Sim_State :: struct {
 	prev_q: rl.Quaternion,
 }
 
+GLOBAL_PLANE :: collision.Plane {
+	normal = rl.Vector3{0, 1, 0},
+	dist   = 0,
+}
+
+BOX_CORNERS_NORM :: [8]rl.Vector3 {
+	rl.Vector3{-1, -1, -1},
+	rl.Vector3{-1, -1, 1},
+	rl.Vector3{-1, 1, -1},
+	rl.Vector3{-1, 1, 1},
+	rl.Vector3{1, -1, -1},
+	rl.Vector3{1, -1, 1},
+	rl.Vector3{1, 1, -1},
+	rl.Vector3{1, 1, 1},
+}
+
 simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 	body_states := make([]Body_Sim_State, len(scene.bodies), context.temp_allocator)
 
@@ -100,6 +116,61 @@ 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
+				penetration: f32
+				hit_point: rl.Vector3
+				hit := false
+
+				switch s in body.shape {
+				case 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(
+						collision.Box{pos = 0, rad = extent},
+						local_plane,
+					)
+					if hit {
+						local_hit_point: rl.Vector3
+						min_distance := f32(math.F32_MAX)
+						for corner in 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 {
+					apply_constraint_correction_unilateral(
+						dt,
+						body,
+						0,
+						penetration,
+						normal,
+						hit_point,
+						0,
+					)
+				}
+			}
+		}
+
 		for &v in scene.suspension_constraints {
 			if v.alive {
 				body := get_body(scene, v.body)

libs/tracy/wrapper.odin

@@ -3,7 +3,7 @@ package tracy
 import "core:c"
 
 TRACY_ENABLE :: #config(TRACY_ENABLE, false)
-TRACY_CALLSTACK :: #config(TRACY_CALLSTACK, 5)
+TRACY_CALLSTACK :: #config(TRACY_CALLSTACK, 1)
 TRACY_HAS_CALLSTACK :: #config(TRACY_HAS_CALLSTACK, false)
 TRACY_FIBERS :: #config(TRACY_FIBERS, false)