Proper raycasts against other rigid bodies instead of a fake plane

game/assets/assets.odin

@@ -63,7 +63,7 @@ Asset_Manager :: struct {
 get_texture :: proc(assetman: ^Asset_Manager, path: cstring) -> rl.Texture2D {
 	tracy.Zone()
 
-	modtime := rl.GetFileModTime(path)
+	modtime := physfs.getLastModTime(path)
 
 	existing, ok := assetman.textures[path]
 	if ok && existing.modtime == modtime {
@@ -99,7 +99,7 @@ get_model_ex :: proc(
 ) {
 	tracy.Zone()
 
-	new_modtime := rl.GetFileModTime(path)
+	new_modtime := physfs.getLastModTime(path)
 
 	existing, ok := assetman.models[path]
 	if ok && existing.modtime == new_modtime {

game/halfedge/halfedge.odin

@@ -219,6 +219,42 @@ iterate_next_edge :: proc(
 	return
 }
 
+Face_Triangles_Iterator :: struct {
+	edge_it:    Edge_Iterator,
+	first_vert: Vec3,
+	idx:        int,
+}
+
+iterator_face_triangles :: proc(
+	mesh: Half_Edge_Mesh,
+	face: Face_Index,
+) -> Face_Triangles_Iterator {
+	edge_it := iterator_face_edges(mesh, face)
+	edge, _, ok := iterate_next_edge(&edge_it)
+	assert(ok)
+
+	first_vert := mesh.vertices[edge.origin].pos
+
+	return Face_Triangles_Iterator{edge_it = edge_it, first_vert = first_vert}
+}
+
+Tri :: [3]Vec3
+
+iterate_next_triangle :: proc(it: ^Face_Triangles_Iterator) -> (tri: Tri, tri_idx: int, ok: bool) {
+	edge, _, has_next_edge := iterate_next_edge(&it.edge_it)
+
+	tri_idx = it.idx
+
+	if has_next_edge {
+		p1, p2 := get_edge_points(it.edge_it.mesh, edge)
+		it.idx += 1
+
+		return Tri{it.first_vert, p1, p2}, tri_idx, true
+	} else {
+		return {}, tri_idx, false
+	}
+}
+
 Vec4 :: [4]f32
 
 copy_mesh :: proc(

game/physics/bvh/bvh.odin

@@ -265,13 +265,25 @@ Collision :: struct {
 	triangle_tests: int,
 }
 
-Iterator_Intersect_Leaf :: struct {
+Iterator_Intersect_Type :: enum {
+	AABB,
+	Ray,
+}
+
+Iterator_Intersect_Leaf :: struct($T: Iterator_Intersect_Type) {
 	bvh:              ^BVH,
 	nodes_to_process: queue.Queue(i32),
 	bounds:           AABB,
+	ray:              Ray,
+	min_t:            f32,
 }
 
-iterator_intersect_leaf :: proc(bvh: ^BVH, bounds: AABB) -> (it: Iterator_Intersect_Leaf) {
+iterator_intersect_leaf_aabb :: proc(
+	bvh: ^BVH,
+	bounds: AABB,
+) -> (
+	it: Iterator_Intersect_Leaf(.AABB),
+) {
 	it.bvh = bvh
 	it.bounds = bounds
 	queue.init(&it.nodes_to_process, queue.DEFAULT_CAPACITY, context.temp_allocator)
@@ -280,7 +292,23 @@ iterator_intersect_leaf :: proc(bvh: ^BVH, bounds: AABB) -> (it: Iterator_Inters
 	return it
 }
 
-test_aabb_vs_aabb :: proc(a, b: AABB) -> bool {
+iterator_intersect_leaf_ray :: proc(
+	bvh: ^BVH,
+	ray: Ray,
+	distance := max(f32),
+) -> (
+	it: Iterator_Intersect_Leaf(.Ray),
+) {
+	it.bvh = bvh
+	it.ray = ray
+	it.min_t = distance
+	queue.init(&it.nodes_to_process, queue.DEFAULT_CAPACITY, context.temp_allocator)
+	queue.push_back(&it.nodes_to_process, 0)
+
+	return it
+}
+
+test_aabb_vs_aabb :: #force_inline proc(a, b: AABB) -> bool {
 	// Exit with no intersection if separated along an axis
 	if a.max[0] < b.min[0] || a.min[0] > b.max[0] do return false
 	if a.max[1] < b.min[1] || a.min[1] > b.max[1] do return false
@@ -289,9 +317,8 @@ test_aabb_vs_aabb :: proc(a, b: AABB) -> bool {
 	return true
 }
 
-
 iterator_intersect_leaf_next :: proc(
-	it: ^Iterator_Intersect_Leaf,
+	it: ^$T/Iterator_Intersect_Leaf($Intersect_Type),
 ) -> (
 	node: Node,
 	node_idx: int,
@@ -303,7 +330,11 @@ iterator_intersect_leaf_next :: proc(
 
 		cur_node := &it.bvh.nodes[cur_node_idx]
 
-		if test_aabb_vs_aabb(cur_node.aabb, it.bounds) {
+		passed :=
+			test_aabb_vs_aabb(cur_node.aabb, it.bounds) when Intersect_Type ==
+			.AABB else internal_ray_aabb_test(it.ray, cur_node.aabb, it.min_t)
+
+		if passed {
 			if is_leaf_node(cur_node^) {
 				node = cur_node^
 				node_idx = int(cur_node_idx)

game/physics/collision/convex.odin

@@ -574,3 +574,37 @@ create_edge_contact_manifold :: proc(
 
 	return
 }
+
+ray_vs_convex :: proc(
+	c: Convex,
+	origin, dir: Vec3,
+	min_t: f32,
+) -> (
+	t: f32,
+	face_idx: halfedge.Face_Index,
+	normal: Vec3,
+	bary: [3]f32,
+	hit: bool,
+) {
+	t = min_t
+	for i in 0 ..< len(c.faces) {
+		it := halfedge.iterator_face_triangles(halfedge.Half_Edge_Mesh(c), halfedge.Face_Index(i))
+
+		for tri in halfedge.iterate_next_triangle(&it) {
+			hit_t, tmp_normal, tmp_bary, ok := intersect_ray_triangle(
+				[2]Vec3{origin, origin + dir},
+				tri,
+			)
+
+			if ok && hit_t < t {
+				t = hit_t
+				normal = tmp_normal
+				bary = tmp_bary
+				face_idx = halfedge.Face_Index(i)
+				hit = true
+			}
+		}
+	}
+
+	return
+}

game/physics/scene.odin

@@ -163,6 +163,7 @@ Suspension_Constraint :: struct {
 	// Angular velocity
 	w:                      f32,
 	hit:                    bool,
+	hit_normal:             Vec3,
 	hit_point:              Vec3,
 	// rel_hit_point = rel_pos + rel_dir * hit_t
 	hit_t:                  f32,
@@ -267,6 +268,18 @@ is_handle_valid :: proc {
 	is_engine_handle_valid,
 }
 
+index_to_body_handle :: proc(idx: int) -> Body_Handle {
+	return Body_Handle(idx + 1)
+}
+
+body_handle_to_index :: proc(handle: Body_Handle) -> int {
+	return int(handle) - 1
+}
+
+handle_to_index :: proc {
+	body_handle_to_index,
+}
+
 Body_Ptr :: #soa^#soa[]Body
 Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint
 Engine_Ptr :: ^Engine

game/physics/simulation.odin

@@ -1,5 +1,6 @@
 package physics
 
+import "base:runtime"
 import "bvh"
 import "collision"
 import "core:container/bit_array"
@@ -131,6 +132,52 @@ build_tlas :: proc(sim_state: ^Sim_State, config: Solver_Config) -> TLAS {
 	return TLAS{bvh_tree = sim_state_bvh, body_aabbs = body_aabbs}
 }
 
+raycast_bodies :: proc(
+	sim_state: ^Sim_State,
+	tlas: ^TLAS,
+	origin, dir: Vec3,
+	distance := max(f32),
+) -> (
+	t: f32,
+	normal: Vec3,
+	hit: bool,
+) {
+	temp := runtime.default_temp_allocator_temp_begin()
+	defer runtime.default_temp_allocator_temp_end(temp)
+
+	t = distance
+
+	ray: bvh.Ray
+	ray.origin = origin
+	ray.dir = dir
+	ray.dir_inv = 1.0 / dir
+	it := bvh.iterator_intersect_leaf_ray(&tlas.bvh_tree, ray, distance)
+
+	for leaf_node in bvh.iterator_intersect_leaf_next(&it) {
+		for j in 0 ..< leaf_node.prim_len {
+			body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + j]
+
+			body := get_body(sim_state, index_to_body_handle(int(body_idx)))
+			shape := body_get_convex_shape_world(sim_state, body, context.temp_allocator)
+
+			hit_t, _, tmp_normal, _, ok := collision.ray_vs_convex(
+				shape,
+				ray.origin,
+				ray.dir,
+				distance,
+			)
+
+			if ok && (!hit || hit_t < t) {
+				t = hit_t
+				normal = tmp_normal
+				hit = true
+			}
+		}
+	}
+
+	return
+}
+
 // TODO: free intermediate temp allocs
 find_new_contacts :: proc(sim_state: ^Sim_State, tlas: ^TLAS) {
 	tracy.Zone()
@@ -142,7 +189,7 @@ find_new_contacts :: proc(sim_state: ^Sim_State, tlas: ^TLAS) {
 
 		if body.alive {
 			body_aabb := tlas.body_aabbs[i]
-			it := bvh.iterator_intersect_leaf(&tlas.bvh_tree, body_aabb)
+			it := bvh.iterator_intersect_leaf_aabb(&tlas.bvh_tree, body_aabb)
 
 			for leaf_node in bvh.iterator_intersect_leaf_next(&it) {
 				for j in 0 ..< leaf_node.prim_len {
@@ -550,7 +597,7 @@ pgs_solve_engines :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32,
 					torque = rpm_torque_curve[math.clamp(idx, 0, len(rpm_torque_curve) - 1)][1]
 				}
 
-				log.debugf("torque: %v Nm", torque)
+				// log.debugf("torque: %v Nm", torque)
 				torque *= engine.throttle
 
 				engine.w += (torque / engine.inertia) * dt
@@ -644,7 +691,13 @@ pgs_solve_engines :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32,
 	}
 }
 
-pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) {
+pgs_solve_suspension :: proc(
+	sim_state: ^Sim_State,
+	tlas: ^TLAS,
+	config: Solver_Config,
+	dt: f32,
+	inv_dt: f32,
+) {
 	// Solve suspension velocity
 	for _, i in sim_state.suspension_constraints {
 		v := &sim_state.suspension_constraints_slice[i]
@@ -654,11 +707,16 @@ pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f
 			if body.alive {
 				wheel_world_pos := body_local_to_world(body, v.rel_pos)
 				dir := body_local_to_world_vec(body, v.rel_dir)
-				pos2 := wheel_world_pos + dir * v.rest
-				v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane(
-					{wheel_world_pos, pos2},
-					collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
+				v.hit_t, v.hit_normal, v.hit = raycast_bodies(
+					sim_state,
+					tlas,
+					wheel_world_pos,
+					dir,
+					v.rest,
 				)
+				log.debugf("hit_t: %v, hit: %v", v.hit_t, v.hit)
+				v.hit_point = wheel_world_pos + dir * v.hit_t
+
 				forward := wheel_get_forward_vec(body, v)
 
 				body_vel_at_contact_patch := body_velocity_at_point(body, v.hit_point)
@@ -847,7 +905,13 @@ pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f
 	}
 }
 
-pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) {
+pgs_substep :: proc(
+	sim_state: ^Sim_State,
+	tlas: ^TLAS,
+	config: Solver_Config,
+	dt: f32,
+	inv_dt: f32,
+) {
 	for i in 0 ..< len(sim_state.bodies_slice) {
 		body := &sim_state.bodies_slice[i]
 
@@ -937,7 +1001,7 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d
 	apply_bias := true
 	pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias)
 	pgs_solve_engines(sim_state, config, dt, inv_dt)
-	pgs_solve_suspension(sim_state, config, dt, inv_dt)
+	pgs_solve_suspension(sim_state, tlas, config, dt, inv_dt)
 
 	for i in 0 ..< len(sim_state.bodies_slice) {
 		body := &sim_state.bodies_slice[i]
@@ -1036,7 +1100,7 @@ simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Confi
 		}
 	case .PGS:
 		for _ in 0 ..< substeps {
-			pgs_substep(sim_state, config, dt, inv_dt)
+			pgs_substep(sim_state, &tlas, config, dt, inv_dt)
 		}
 	}