Optimize edge collision checking by sorting so they always follow in pairs

Fix suspension constraint force being weird

game/assets/assets.odin

@@ -565,8 +565,6 @@ assetman_fetch_or_load_internal :: proc(
 	modtime: physfs.sint64,
 	result: Asset_Cache_Result,
 ) {
-	tracy.Zone()
-
 	existing, has_existing := assetman.assets[key]
 
 	if has_existing {
@@ -636,7 +634,6 @@ assetman_fetch_or_load :: proc(
 	modtime: physfs.sint64,
 	result: Asset_Cache_Result,
 ) {
-	tracy.Zone()
 	value, modtime, result = assetman_fetch_or_load_internal(
 		assetman,
 		key,

game/assets/parsers.odin

@@ -338,7 +338,6 @@ parse_convex :: proc(bytes: []byte, allocator := context.allocator) -> (Loaded_C
 	final_edges := make([]halfedge.Half_Edge, len(edges), allocator)
 	final_faces := make([]halfedge.Face, len(faces), allocator)
 	copy(final_vertices, vertices[:])
-	copy(final_edges, edges[:])
 	copy(final_faces, faces[:])
 
 	mesh := halfedge.Half_Edge_Mesh {
@@ -349,6 +348,8 @@ parse_convex :: proc(bytes: []byte, allocator := context.allocator) -> (Loaded_C
 		extent   = extent,
 	}
 
+	halfedge.sort_edges(mesh, edges[:])
+
 	// Center of mass calculation
 	total_volume := f32(0.0)
 	{

game/halfedge/halfedge.odin

@@ -51,11 +51,10 @@ mesh_from_vertex_index_list :: proc(
 	mesh: Half_Edge_Mesh
 	verts := make([]Vertex, len(vertices), allocator)
 	faces := make([]Face, num_faces, allocator)
-	edges := make([]Half_Edge, len(indices), allocator)
+	edges := make([]Half_Edge, len(indices), context.temp_allocator)
 
 	mesh.vertices = verts
 	mesh.faces = faces
-	mesh.edges = edges
 
 	min_pos, max_pos: Vec3 = max(f32), min(f32)
 
@@ -128,9 +127,52 @@ mesh_from_vertex_index_list :: proc(
 		}
 	}
 
+	mesh.edges = make([]Half_Edge, len(edges), allocator)
+
+	sort_edges(mesh, edges)
+
 	return mesh
 }
 
+sort_edges :: proc(dst_mesh: Half_Edge_Mesh, unsorted_edges: []Half_Edge) {
+	assert(len(dst_mesh.edges) == len(unsorted_edges))
+
+	keys := make([]u32, len(unsorted_edges), context.temp_allocator)
+
+	for &e, i in unsorted_edges {
+		v0 := e.origin
+		v1 := unsorted_edges[e.next].origin
+
+		min_v := min(v0, v1)
+		max_v := max(v0, v1)
+
+		keys[i] = (u32(max_v) << 16) | u32(min_v)
+	}
+
+	sorted_edges_indices := slice.sort_with_indices(keys, context.temp_allocator)
+
+	unsorted_to_sorted_lookup := make([]Edge_Index, len(unsorted_edges), context.temp_allocator)
+	for i in 0 ..< len(unsorted_edges) {
+		unsorted_to_sorted_lookup[sorted_edges_indices[i]] = Edge_Index(i)
+	}
+
+	for i in 0 ..< len(unsorted_edges) {
+		sorted := &dst_mesh.edges[i]
+		sorted^ = unsorted_edges[sorted_edges_indices[i]]
+		sorted.twin = unsorted_to_sorted_lookup[sorted.twin]
+		sorted.next = unsorted_to_sorted_lookup[sorted.next]
+		sorted.prev = unsorted_to_sorted_lookup[sorted.prev]
+	}
+
+	for &v in dst_mesh.vertices {
+		v.edge = unsorted_to_sorted_lookup[v.edge]
+	}
+
+	for &f in dst_mesh.faces {
+		f.edge = unsorted_to_sorted_lookup[f.edge]
+	}
+}
+
 get_edge_points :: #force_inline proc(
 	mesh: Half_Edge_Mesh,
 	edge: Half_Edge,

game/physics/collision/convex.odin

@@ -1,7 +1,6 @@
 package collision
 
 import "base:runtime"
-import "core:container/bit_array"
 import "core:log"
 import "core:math"
 import lg "core:math/linalg"
@@ -274,29 +273,34 @@ query_separation_edges :: proc(
 
 	runtime.DEFAULT_TEMP_ALLOCATOR_TEMP_GUARD()
 
-	checked_pairs: bit_array.Bit_Array
+	// checked_pairs: bit_array.Bit_Array
-	bit_array.init(&checked_pairs, len(a.edges) * len(b.edges), 0, context.temp_allocator)
+	// bit_array.init(&checked_pairs, len(a.edges) * len(b.edges), 0, context.temp_allocator)
-	a_len := len(a.edges)
+	// a_len := len(a.edges)
 
 	calc_pair_index :: #force_inline proc(a, b, a_len: int) -> int {
 		return (b * a_len) + a
 	}
 
-	for edge_a, edge_a_idx in a.edges {
+	assert(len(a.edges) % 2 == 0)
-		for edge_b, edge_b_idx in b.edges {
+	assert(len(b.edges) % 2 == 0)
-			pair_idx := calc_pair_index(edge_a_idx, edge_b_idx, a_len)
+
-			if bit_array.get(&checked_pairs, pair_idx) {
+	for edge_a_idx := 0; edge_a_idx < len(a.edges); edge_a_idx += 2 {
-				continue
+		edge_a := a.edges[edge_a_idx]
-			}
+		for edge_b_idx := 0; edge_b_idx < len(b.edges); edge_b_idx += 2 {
+			edge_b := b.edges[edge_b_idx]
+			// pair_idx := calc_pair_index(edge_a_idx, edge_b_idx, a_len)
+			// if bit_array.get(&checked_pairs, pair_idx) {
+			// 	continue
+			// }
 
 			// TODO: sort edges so twins are next to each other, then can just iterate with step = 2 and skip this bitfield
-			bit_array.set(&checked_pairs, pair_idx)
+			// bit_array.set(&checked_pairs, pair_idx)
-			bit_array.set(&checked_pairs, calc_pair_index(int(edge_a.twin), edge_b_idx, a_len))
+			// bit_array.set(&checked_pairs, calc_pair_index(int(edge_a.twin), edge_b_idx, a_len))
-			bit_array.set(&checked_pairs, calc_pair_index(edge_a_idx, int(edge_b.twin), a_len))
+			// bit_array.set(&checked_pairs, calc_pair_index(edge_a_idx, int(edge_b.twin), a_len))
-			bit_array.set(
+			// bit_array.set(
-				&checked_pairs,
+			// 	&checked_pairs,
-				calc_pair_index(int(edge_a.twin), int(edge_b.twin), a_len),
+			// 	calc_pair_index(int(edge_a.twin), int(edge_b.twin), a_len),
-			)
+			// )
 
 			if build_minkowski_face(a, b, edge_a, edge_b) {
 				edge_a1, edge_a2 := halfedge.get_edge_points(a, edge_a)

game/physics/simulation.odin

@@ -148,7 +148,7 @@ build_dynamic_tlas :: proc(
 
 				phys_aabb := body_get_aabb(body)
 
-				EXPAND_K :: 2
+				EXPAND_K :: 10
 				expand := lg.abs(EXPAND_K * config.timestep * body.v) + 0.1
 				phys_aabb.extent += expand * 0.5
 
@@ -329,6 +329,9 @@ raycast :: proc(
 		normal = normal2
 	}
 
+	// TODO: raycast_level and raycast_bodies should return a normalized vec
+	normal = lg.normalize0(normal)
+
 	return
 }
 
@@ -472,6 +475,7 @@ find_new_contacts :: proc(
 	sim_cache: ^Sim_Cache,
 	static_tlas: ^Static_TLAS,
 	dyn_tlas: ^Dynamic_TLAS,
+	config: Solver_Config,
 ) {
 	tracy.Zone()
 
@@ -503,6 +507,9 @@ find_new_contacts :: proc(
 
 								shape_a_aabb := shape_get_aabb(shape_a^)
 								shape_a_aabb = body_transform_shape_aabb(body, shape_a_aabb)
+								EXPAND_K :: 2
+								expand := lg.abs(EXPAND_K * config.timestep * body.v) + 0.1
+								shape_a_aabb.extent += expand * 0.5
 
 								shapes_b_it := shapes_iterator(sim_state, other_body.shape)
 
@@ -598,16 +605,18 @@ find_new_contacts :: proc(
 									)
 									tri := get_triangle(vertices, indices, tri_idx)
 									prim_aabb := get_triangle_aabb(tri)
+									prim_aabb.min -= 0.1
+									prim_aabb.max += 0.1
 
 									if bvh.test_aabb_vs_aabb(
 										body_aabb_in_Level_geom_space,
 										prim_aabb,
 									) {
-										tracy.ZoneN("body_vs_level_geom")
+										// tracy.ZoneN("body_vs_triangle", false)
 
 										shapes_it := shapes_iterator(sim_state, body.shape)
 										for shape in shapes_iterator_next(&shapes_it) {
-											tracy.ZoneN("body_shape_vs_level_geom")
+											// tracy.ZoneN("body_shape_vs_triangle")
 
 											shape_idx := shapes_it.counter - 1
 											shape_aabb := shape_get_aabb(shape^)
@@ -920,7 +929,7 @@ update_contacts :: proc(sim_state: ^Sim_State, sim_cache: ^Sim_Cache, static_tla
 }
 
 calculate_soft_constraint_params :: proc(
-	natural_freq, damping_ratio, dt: f32,
+	natural_freq, damping_ratio, dt: f64,
 ) -> (
 	bias_rate: f32,
 	mass_coef: f32,
@@ -930,9 +939,9 @@ calculate_soft_constraint_params :: proc(
 	a1 := 2.0 * damping_ratio + omega * dt
 	a2 := dt * omega * a1
 	a3 := 1.0 / (1.0 + a2)
-	bias_rate = omega / a1
+	bias_rate = f32(omega / a1)
-	mass_coef = a2 * a3
+	mass_coef = f32(a2 * a3)
-	impulse_coef = a3
+	impulse_coef = f32(a3)
 
 	return
 }
@@ -944,7 +953,7 @@ pgs_solve_contacts :: proc(
 	inv_dt: f32,
 	apply_bias: bool,
 ) {
-	bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(40, 1.0, dt)
+	bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(30, 0.8, f64(dt))
 	if !apply_bias {
 		mass_coef = 1
 		bias_rate = 0
@@ -1303,7 +1312,7 @@ pgs_solve_suspension :: proc(
 				forward := wheel_get_forward_vec(body, v)
 				right := wheel_get_right_vec(body, v)
 
-				w_normal := get_body_angular_inverse_mass(body, dir)
+				w_normal := get_body_angular_inverse_mass(body, -v.hit_normal)
 				inv_w_normal := 1.0 / w_normal
 
 				// Drive force
@@ -1343,12 +1352,12 @@ pgs_solve_suspension :: proc(
 					// Spring force
 					{
 						bias_coef, mass_coef, impulse_coef := calculate_soft_constraint_params(
-							v.natural_frequency,
+							f64(v.natural_frequency),
-							v.damping,
+							f64(v.damping),
-							dt,
+							f64(dt),
 						)
 
-						vel := lg.dot(body_velocity_at_point(body, wheel_world_pos), dir)
+						vel := lg.dot(body_velocity_at_point(body, v.hit_point), -v.hit_normal)
 						x := v.hit_t
 						separation := v.rest - x
 
@@ -1357,7 +1366,11 @@ pgs_solve_suspension :: proc(
 							impulse_coef * v.spring_impulse
 						v.spring_impulse += incremental_impulse
 
-						apply_velocity_correction(body, incremental_impulse * dir, wheel_world_pos)
+						apply_velocity_correction(
+							body,
+							incremental_impulse * v.hit_normal,
+							v.hit_point,
+						)
 					}
 
 					// Positive means spinning forward
@@ -1584,11 +1597,7 @@ pgs_substep :: proc(
 				forward := wheel_get_forward_vec(body, s)
 				right := wheel_get_right_vec(body, s)
 
-				apply_velocity_correction(
+				apply_velocity_correction(body, s.spring_impulse * -s.hit_normal, hit_p)
-					body,
-					s.spring_impulse * body_local_to_world_vec(body, s.rel_dir),
-					p,
-				)
 				apply_velocity_correction(body, s.lateral_impulse * right, p)
 
 				apply_velocity_correction(body, s.longitudinal_impulse * forward, hit_p)
@@ -1663,7 +1672,13 @@ simulate_step :: proc(
 	build_dynamic_tlas(sim_state, config, &sim_state.dynamic_tlas)
 
 	remove_invalid_contacts(sim_state, sim_cache, sim_state.static_tlas, sim_state.dynamic_tlas)
-	find_new_contacts(sim_state, sim_cache, &sim_state.static_tlas, &sim_state.dynamic_tlas)
+	find_new_contacts(
+		sim_state,
+		sim_cache,
+		&sim_state.static_tlas,
+		&sim_state.dynamic_tlas,
+		config,
+	)
 	update_contacts(sim_state, sim_cache, &sim_state.static_tlas)
 
 	Solver :: enum {