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,
@@ -32,6 +34,7 @@ Loaded_BVH :: struct {
 
 Loaded_Convex :: struct {
 	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,13 +127,18 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 		}
 		handled_pairs := make_map(map[Body_Pair]bool, context.temp_allocator)
 
+		{
+			tracy.ZoneN("simulate_step::collisions")
+
 			for _, i in scene.bodies {
 				body := &scene.bodies_slice[i]
 				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)}] {
+						if i != j &&
+						   body2.alive &&
+						   !handled_pairs[{a = min(i, j), b = max(i, j)}] {
 							s1, s2 := body.shape.(Shape_Box), body2.shape.(Shape_Box)
 
 							box1 := collision.box_to_convex(
@@ -142,8 +150,10 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 								context.temp_allocator,
 							)
 
-						mat1 := lg.matrix4_translate(body.x) * lg.matrix4_from_quaternion(body.q)
+							mat1 :=
-						mat2 := lg.matrix4_translate(body2.x) * lg.matrix4_from_quaternion(body2.q)
+								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)
@@ -176,7 +186,8 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 									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)
+										body_local_to_world(body, p1),
+										body_local_to_world(body2, p2)
 
 									separation := min(lg.dot(p2 - p1, manifold.normal), 0)
 
@@ -202,8 +213,14 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 									// Static friction
 									if ok {
 										p1, p2 =
-										body_local_to_world(body, manifold.points_a[point_idx]),
+											body_local_to_world(
-										body_local_to_world(body2, manifold.points_b[point_idx])
+												body,
+												manifold.points_a[point_idx],
+											),
+											body_local_to_world(
+												body2,
+												manifold.points_b[point_idx],
+											)
 										prev_p1 :=
 											body_states[i].prev_x +
 											lg.quaternion_mul_vector3(
@@ -253,6 +270,10 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 					}
 				}
 			}
+		}
+
+		{
+			tracy.ZoneN("simulate_step::suspension_constraints")
 
 			for &v in scene.suspension_constraints {
 				if v.alive {
@@ -282,9 +303,13 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 					}
 				}
 			}
+		}
 
 		solve_velocities(scene, body_states, inv_dt)
 
+		{
+			tracy.ZoneN("simulate_step::collision_velocities")
+
 			for &pair in scene.contact_pairs[:scene.contact_pairs_len] {
 				manifold := &pair.manifold
 				body1 := get_body(scene, pair.a)
@@ -308,7 +333,9 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 						delta_v :=
 							-(v_tangent / v_tangent_len) *
 							min(
-							dt * DYNAMIC_FRICTION * abs(pair.lambda_normal[point_idx] / (dt * dt)),
+								dt *
+								DYNAMIC_FRICTION *
+								abs(pair.lambda_normal[point_idx] / (dt * dt)),
 								v_tangent_len,
 							)
 
@@ -331,6 +358,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
 					}
 				}
 			}
+		}
 
 		// Solve suspension velocity
 		for _, i in scene.suspension_constraints {