Calculate center of mass for convex meshes

game/assets/assets.odin

@@ -6,11 +6,13 @@ import "core:math"
 import lg "core:math/linalg"
 import "core:os/os2"
 import "core:strconv"
+import "game:debug"
 import "game:halfedge"
 import "game:physics/bvh"
 import "game:physics/collision"
 import "libs:tracy"
 import rl "vendor:raylib"
+import "vendor:raylib/rlgl"
 
 Loaded_Texture :: struct {
 	texture: rl.Texture2D,
@@ -31,7 +33,8 @@ Loaded_BVH :: struct {
 }
 
 Loaded_Convex :: struct {
-	mesh: collision.Convex,
+	mesh:           collision.Convex,
+	center_of_mass: rl.Vector3,
 }
 
 destroy_loaded_bvh :: proc(loaded_bvh: Loaded_BVH) {
@@ -295,26 +298,37 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 					i1, i2, i3 := indices[0], indices[1], indices[2]
 					v1, v2, v3 := vertices[i1].pos, vertices[i2].pos, vertices[i3].pos
 
-					collision.plane_from_point_normal(
+					plane = collision.plane_from_point_normal(
 						v1,
 						lg.normalize0(lg.cross(v2 - v1, v3 - v1)),
 					)
 				}
 				face.normal = plane.normal
 
-				for index in indices[3:] {
+				// for index in indices[3:] {
-					assert(
+				// 	assert(
-						abs(collision.signed_distance_plane(vertices[index].pos, plane)) < 0.00001,
+				// 		abs(collision.signed_distance_plane(vertices[index].pos, plane)) < 0.01,
-						"mesh has non planar faces",
+				// 		"mesh has non planar faces",
-					)
+				// 	)
-				}
+				// }
+
+				// first_vert_pos := vertices[indices[0]].pos
 
 				for i in 0 ..< len(indices) {
 					edge_idx := halfedge.Edge_Index(first_edge_idx + i)
 					prev_edge_relative := i == 0 ? len(indices) - 1 : i - 1
 					next_edge_relative := (i + 1) % len(indices)
 					i1, i2 := indices[i], indices[next_edge_relative]
-					v1 := &vertices[i1]
+					v1, _ := &vertices[i1], &vertices[i2]
+
+					// assert(
+					// 	lg.dot(
+					// 		lg.cross(v1.pos - first_vert_pos, v2.pos - first_vert_pos),
+					// 		plane.normal,
+					// 	) >=
+					// 	0,
+					// 	"non convex face or non ccw winding",
+					// )
 
 					if v1.edge == -1 {
 						v1.edge = edge_idx
@@ -348,7 +362,78 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 
 	center /= f32(len(vertices))
 
-	return {mesh = {vertices = vertices[:], edges = edges[:], faces = faces[:], center = center}}
+	center_of_mass: rl.Vector3
+
+	mesh := halfedge.Half_Edge_Mesh {
+		vertices = vertices[:],
+		edges    = edges[:],
+		faces    = faces[:],
+		center   = center,
+	}
+
+	// Center of mass calculation
+	total_volume := f32(0.0)
+	{
+		rlgl.Begin(rlgl.TRIANGLES)
+		rlgl.End()
+
+		rlgl.EnableWireMode()
+		defer rlgl.DisableWireMode()
+
+		tri_idx := 0
+		for face_idx in 0 ..< len(faces) {
+			face := faces[face_idx]
+			// for all triangles
+			it := halfedge.iterator_face_edges(mesh, halfedge.Face_Index(face_idx))
+			i := 0
+			tri: [3]rl.Vector3
+			for edge in halfedge.iterate_next_edge(&it) {
+				switch i {
+				case 0 ..< 3:
+					tri[i] = mesh.vertices[edge.origin].pos
+				case:
+					tri[1] = tri[2]
+					tri[2] = mesh.vertices[edge.origin].pos
+				}
+
+				if i >= 2 {
+					plane := collision.plane_from_point_normal(tri[0], -face.normal)
+
+					h := max(0, collision.signed_distance_plane(center, plane))
+					tri_area :=
+						lg.dot(lg.cross(tri[1] - tri[0], tri[2] - tri[0]), face.normal) * 0.5
+					tetra_volume := 1.0 / 3.0 * tri_area * h
+					total_volume += tetra_volume
+
+					tetra_centroid := (tri[0] + tri[1] + tri[2] + center) * 0.25
+					center_of_mass += tetra_volume * tetra_centroid
+
+					tri_idx += 1
+				}
+
+				i += 1
+			}
+		}
+	}
+
+	assert(total_volume > 0, "degenerate convex hull")
+	center_of_mass /= total_volume
+
+	return {mesh = mesh, center_of_mass = center_of_mass}
+}
+
+debug_draw_tetrahedron_wires :: proc(tri: [3]rl.Vector3, p: rl.Vector3, color: rl.Color) {
+	rlgl.Begin(rlgl.LINES)
+	defer rlgl.End()
+
+	debug.rlgl_color(color)
+
+	debug.rlgl_vertex3v2(tri[0], tri[1])
+	debug.rlgl_vertex3v2(tri[1], tri[2])
+	debug.rlgl_vertex3v2(tri[2], tri[0])
+	debug.rlgl_vertex3v2(tri[0], p)
+	debug.rlgl_vertex3v2(tri[1], p)
+	debug.rlgl_vertex3v2(tri[2], p)
 }
 
 shutdown :: proc(assetman: ^Asset_Manager) {

game/debug/helpers.odin

@@ -35,6 +35,11 @@ rlgl_vertex3v :: proc(v: rl.Vector3) {
 	rlgl.Vertex3f(v.x, v.y, v.z)
 }
 
+rlgl_vertex3v2 :: proc(v1, v2: rl.Vector3) {
+	rlgl_vertex3v(v1)
+	rlgl_vertex3v(v2)
+}
+
 @(deferred_none = rlgl_transform_scope_end)
 rlgl_transform_scope :: proc(mat: rl.Matrix) {
 	mat := mat

game/game.odin

@@ -555,6 +555,8 @@ draw :: proc() {
 		car_convex := assets.get_convex(&g_mem.assetman, "assets/car_convex.obj")
 
 		halfedge.debug_draw_mesh_wires(car_convex.mesh, rl.RED)
+		rl.DrawSphereWires(car_convex.mesh.center, 0.5, 8, 8, rl.BLUE)
+		rl.DrawSphereWires(car_convex.center_of_mass, 0.5, 8, 8, rl.RED)
 
 		box1_convex := collision.box_to_convex(box1, context.temp_allocator)
 		box2_convex := collision.box_to_convex(box2, context.temp_allocator)

game/physics/simulation.odin

@@ -5,6 +5,7 @@ import "core:fmt"
 import "core:math"
 import lg "core:math/linalg"
 import "game:halfedge"
+import "libs:tracy"
 import rl "vendor:raylib"
 
 _ :: math
@@ -83,6 +84,8 @@ Contact_Pair :: struct {
 }
 
 simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
+	tracy.Zone()
+
 	body_states := make([]Body_Sim_State, len(scene.bodies), context.temp_allocator)
 
 	scene.contact_pairs_len = 0
@@ -124,126 +127,141 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 		}
 		handled_pairs := make_map(map[Body_Pair]bool, context.temp_allocator)
 
-		for _, i in scene.bodies {
+		{
-			body := &scene.bodies_slice[i]
+			tracy.ZoneN("simulate_step::collisions")
-			if body.alive {
-				for _, j in scene.bodies {
-					body2 := &scene.bodies_slice[j]
 
-					if i != j && body2.alive && !handled_pairs[{a = min(i, j), b = max(i, j)}] {
+			for _, i in scene.bodies {
-						s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box)
+				body := &scene.bodies_slice[i]
+				if body.alive {
+					for _, j in scene.bodies {
+						body2 := &scene.bodies_slice[j]
 
-						box1 := collision.box_to_convex(
+						if i != j &&
-							collision.Box{rad = s1.size * 0.5},
+						   body2.alive &&
-							context.temp_allocator,
+						   !handled_pairs[{a = min(i, j), b = max(i, j)}] {
-						)
+							s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box)
-						box2 := collision.box_to_convex(
-							collision.Box{rad = s2.size * 0.5},
-							context.temp_allocator,
-						)
 
-						mat1 := lg.matrix4_translate(body.x) * lg.matrix4_from_quaternion(body.q)
+							box1 := collision.box_to_convex(
-						mat2 := lg.matrix4_translate(body2.x) * lg.matrix4_from_quaternion(body2.q)
+								collision.Box{rad = s1.size * 0.5},
+								context.temp_allocator,
+							)
+							box2 := collision.box_to_convex(
+								collision.Box{rad = s2.size * 0.5},
+								context.temp_allocator,
+							)
 
-						halfedge.transform_mesh(&box1, mat1)
+							mat1 :=
-						halfedge.transform_mesh(&box2, mat2)
+								lg.matrix4_translate(body.x) * lg.matrix4_from_quaternion(body.q)
+							mat2 :=
+								lg.matrix4_translate(body2.x) * lg.matrix4_from_quaternion(body2.q)
 
-						// Raw manifold has contact points in world space
+							halfedge.transform_mesh(&box1, mat1)
-						raw_manifold, collision := collision.convex_vs_convex_sat(box1, box2)
+							halfedge.transform_mesh(&box2, mat2)
 
-						if collision {
+							// Raw manifold has contact points in world space
-							contact_pair := &scene.contact_pairs[scene.contact_pairs_len]
+							raw_manifold, collision := collision.convex_vs_convex_sat(box1, box2)
-							contact_pair^ = Contact_Pair {
-								a        = Body_Handle(i + 1),
-								b        = Body_Handle(j + 1),
-								manifold = raw_manifold,
-							}
-							scene.contact_pairs_len += 1
-							manifold := &contact_pair.manifold
 
-							// Convert manifold contact from world to local space
+							if collision {
-							for point_idx in 0 ..< manifold.points_len {
+								contact_pair := &scene.contact_pairs[scene.contact_pairs_len]
-								manifold.points_a[point_idx] = body_world_to_local(
+								contact_pair^ = Contact_Pair {
-									body,
+									a        = Body_Handle(i + 1),
-									manifold.points_a[point_idx],
+									b        = Body_Handle(j + 1),
-								)
+									manifold = raw_manifold,
-								manifold.points_b[point_idx] = body_world_to_local(
-									body2,
-									manifold.points_b[point_idx],
-								)
-							}
-							for point_idx in 0 ..< manifold.points_len {
-								p1, p2 :=
-									manifold.points_a[point_idx], manifold.points_b[point_idx]
-								p1, p2 =
-									body_local_to_world(body, p1), body_local_to_world(body2, p2)
-
-								separation := min(lg.dot(p2 - p1, manifold.normal), 0)
-
-								handled_pairs[{a = min(i, j), b = max(i, j)}] = true
-
-								lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
-									dt,
-									body,
-									body2,
-									0,
-									separation,
-									-manifold.normal,
-									p1,
-									p2,
-								)
-								contact_pair.lambda_normal[point_idx] = lambda_norm
-
-								if ok {
-									apply_correction(body, corr1, p1)
-									apply_correction(body2, corr2, p2)
 								}
+								scene.contact_pairs_len += 1
+								manifold := &contact_pair.manifold
 
-								// Static friction
+								// Convert manifold contact from world to local space
-								if ok {
+								for point_idx in 0 ..< manifold.points_len {
+									manifold.points_a[point_idx] = body_world_to_local(
+										body,
+										manifold.points_a[point_idx],
+									)
+									manifold.points_b[point_idx] = body_world_to_local(
+										body2,
+										manifold.points_b[point_idx],
+									)
+								}
+								for point_idx in 0 ..< manifold.points_len {
+									p1, p2 :=
+										manifold.points_a[point_idx], manifold.points_b[point_idx]
 									p1, p2 =
-										body_local_to_world(body, manifold.points_a[point_idx]),
+										body_local_to_world(body, p1),
-										body_local_to_world(body2, manifold.points_b[point_idx])
+										body_local_to_world(body2, p2)
-									prev_p1 :=
-										body_states[i].prev_x +
-										lg.quaternion_mul_vector3(
-											body_states[i].prev_q,
-											manifold.points_a[point_idx],
-										)
-									prev_p2 :=
-										body_states[j].prev_x +
-										lg.quaternion_mul_vector3(
-											body_states[j].prev_q,
-											manifold.points_b[point_idx],
-										)
 
-									delta_p := (p2 - prev_p2) - (p1 - prev_p1)
+									separation := min(lg.dot(p2 - p1, manifold.normal), 0)
-									delta_p_tangent_norm :=
-										delta_p -
-										lg.dot(manifold.normal, delta_p) * manifold.normal
-									delta_p_tangent_len := lg.length(delta_p_tangent_norm)
 
-									if delta_p_tangent_len > 0 {
+									handled_pairs[{a = min(i, j), b = max(i, j)}] = true
-										delta_p_tangent_norm /= delta_p_tangent_len
 
-										lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent :=
+									lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
-											calculate_constraint_params2(
+										dt,
-												dt,
+										body,
+										body2,
+										0,
+										separation,
+										-manifold.normal,
+										p1,
+										p2,
+									)
+									contact_pair.lambda_normal[point_idx] = lambda_norm
+
+									if ok {
+										apply_correction(body, corr1, p1)
+										apply_correction(body2, corr2, p2)
+									}
+
+									// Static friction
+									if ok {
+										p1, p2 =
+											body_local_to_world(
 												body,
+												manifold.points_a[point_idx],
+											),
+											body_local_to_world(
 												body2,
-												0,
+												manifold.points_b[point_idx],
-												-delta_p_tangent_len,
+											)
-												delta_p_tangent_norm,
+										prev_p1 :=
-												p1,
+											body_states[i].prev_x +
-												p2,
+											lg.quaternion_mul_vector3(
+												body_states[i].prev_q,
+												manifold.points_a[point_idx],
+											)
+										prev_p2 :=
+											body_states[j].prev_x +
+											lg.quaternion_mul_vector3(
+												body_states[j].prev_q,
+												manifold.points_b[point_idx],
 											)
-										contact_pair.lambda_tangent[point_idx] = lambda_tangent
 
-										STATIC_FRICTION :: 0.3
+										delta_p := (p2 - prev_p2) - (p1 - prev_p1)
-										if ok_tangent &&
+										delta_p_tangent_norm :=
-										   lambda_tangent < lambda_norm * STATIC_FRICTION {
+											delta_p -
-											apply_correction(body, corr1_tangent, p1)
+											lg.dot(manifold.normal, delta_p) * manifold.normal
-											apply_correction(body2, corr2_tangent, p2)
+										delta_p_tangent_len := lg.length(delta_p_tangent_norm)
+
+										if delta_p_tangent_len > 0 {
+											delta_p_tangent_norm /= delta_p_tangent_len
+
+											lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent :=
+												calculate_constraint_params2(
+													dt,
+													body,
+													body2,
+													0,
+													-delta_p_tangent_len,
+													delta_p_tangent_norm,
+													p1,
+													p2,
+												)
+											contact_pair.lambda_tangent[point_idx] = lambda_tangent
+
+											STATIC_FRICTION :: 0.3
+											if ok_tangent &&
+											   lambda_tangent < lambda_norm * STATIC_FRICTION {
+												apply_correction(body, corr1_tangent, p1)
+												apply_correction(body2, corr2_tangent, p2)
+											}
 										}
 									}
 								}
@@ -254,79 +272,89 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 			}
 		}
 
-		for &v in scene.suspension_constraints {
+		{
-			if v.alive {
+			tracy.ZoneN("simulate_step::suspension_constraints")
-				body := get_body(scene, v.body)
 
-				pos := body_local_to_world(body, v.rel_pos)
+			for &v in scene.suspension_constraints {
-				dir := body_local_to_world_vec(body, v.rel_dir)
+				if v.alive {
-				pos2 := pos + dir * v.rest
+					body := get_body(scene, v.body)
-				v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane(
-					{pos, pos2},
-					collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
-				)
-				if v.hit {
-					corr := pos - v.hit_point
-					distance := lg.length(corr)
-					corr = corr / distance if distance > 0 else 0
 
-					_ = apply_constraint_correction_unilateral(
+					pos := body_local_to_world(body, v.rel_pos)
-						dt,
+					dir := body_local_to_world_vec(body, v.rel_dir)
-						body,
+					pos2 := pos + dir * v.rest
-						v.compliance,
+					v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane(
-						error = distance - v.rest,
+						{pos, pos2},
-						error_gradient = corr,
+						collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}),
-						pos = pos,
-						other_combined_inv_mass = 0,
 					)
+					if v.hit {
+						corr := pos - v.hit_point
+						distance := lg.length(corr)
+						corr = corr / distance if distance > 0 else 0
+
+						_ = apply_constraint_correction_unilateral(
+							dt,
+							body,
+							v.compliance,
+							error = distance - v.rest,
+							error_gradient = corr,
+							pos = pos,
+							other_combined_inv_mass = 0,
+						)
+					}
 				}
 			}
 		}
 
 		solve_velocities(scene, body_states, inv_dt)
 
-		for &pair in scene.contact_pairs[:scene.contact_pairs_len] {
+		{
-			manifold := &pair.manifold
+			tracy.ZoneN("simulate_step::collision_velocities")
-			body1 := get_body(scene, pair.a)
-			body2 := get_body(scene, pair.b)
 
-			for point_idx in 0 ..< pair.manifold.points_len {
+			for &pair in scene.contact_pairs[:scene.contact_pairs_len] {
-				p1, p2 :=
+				manifold := &pair.manifold
-					body_local_to_world(body1, manifold.points_a[point_idx]),
+				body1 := get_body(scene, pair.a)
-					body_local_to_world(body2, manifold.points_b[point_idx])
+				body2 := get_body(scene, pair.b)
-				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
+				for point_idx in 0 ..< pair.manifold.points_len {
-				v_normal := lg.dot(manifold.normal, v) * manifold.normal
+					p1, p2 :=
-				v_tangent := v - v_normal
+						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)
 
-				DYNAMIC_FRICTION :: 0.1
+					v := v1 - v2
-				v_tangent_len := lg.length(v_tangent)
+					v_normal := lg.dot(manifold.normal, v) * manifold.normal
-				if v_tangent_len > 0 {
+					v_tangent := v - v_normal
-					delta_v :=
-						-(v_tangent / v_tangent_len) *
-						min(
-							dt * DYNAMIC_FRICTION * abs(pair.lambda_normal[point_idx] / (dt * dt)),
-							v_tangent_len,
-						)
 
-					delta_v_norm := lg.normalize0(delta_v)
+					DYNAMIC_FRICTION :: 0.1
+					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 :=
+						delta_v_norm := lg.normalize0(delta_v)
-						get_body_inverse_mass(body1, delta_v_norm, p1),
-						get_body_inverse_mass(body2, delta_v_norm, p2)
 
-					w := w1 + w2
+						w1, w2 :=
-					if w != 0 {
+							get_body_inverse_mass(body1, delta_v_norm, p1),
-						p := delta_v / w
+							get_body_inverse_mass(body2, delta_v_norm, p2)
 
-						body1.v += p * body1.inv_mass
+						w := w1 + w2
-						body2.v -= p * body2.inv_mass
+						if w != 0 {
+							p := delta_v / w
 
-						body1.w += multiply_inv_intertia(body1, lg.cross(r1, p))
+							body1.v += p * body1.inv_mass
-						body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p))
+							body2.v -= p * body2.inv_mass
+
+							body1.w += multiply_inv_intertia(body1, lg.cross(r1, p))
+							body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p))
+						}
 					}
 				}
 			}