Calculate inertia tensor for the convex hull, convert simulation to use full inertia tensor matrix

I couldn't figure out how to diagonalize the inertia tensor matrix so I will go the simle route and just use the full 3x3 matrix

game/assets/assets.odin

@@ -35,7 +35,7 @@ Loaded_BVH :: struct {
 Loaded_Convex :: struct {
 	mesh:           collision.Convex,
 	center_of_mass: rl.Vector3,
-	center_of_mass_tris: rl.Vector3,
+	inertia_tensor: lg.Matrix3f32,
 }
 
 destroy_loaded_bvh :: proc(loaded_bvh: Loaded_BVH) {
@@ -364,7 +364,6 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 	center /= f32(len(vertices))
 
 	center_of_mass: rl.Vector3
-	center_of_mass_tris: rl.Vector3
 
 	mesh := halfedge.Half_Edge_Mesh {
 		vertices = vertices[:],
@@ -375,7 +374,6 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 
 	// Center of mass calculation
 	total_volume := f32(0.0)
-	total_area_tris := f32(0.0)
 	{
 		rlgl.Begin(rlgl.TRIANGLES)
 		rlgl.End()
@@ -410,8 +408,6 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 
 					tetra_centroid := (tri[0] + tri[1] + tri[2] + center) * 0.25
 					center_of_mass += tetra_volume * tetra_centroid
-					center_of_mass_tris += 1.0 / 3.0 * (tri[0] + tri[1] + tri[2]) * tri_area
-					total_area_tris += tri_area
 
 					tri_idx += 1
 				}
@@ -423,13 +419,122 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
 
 	assert(total_volume > 0, "degenerate convex hull")
 	center_of_mass /= total_volume
-	center_of_mass_tris /= total_area_tris
 
-	return {
+	inertia_tensor: lg.Matrix3f32
-		mesh = mesh,
+	// Find inertia tensor
-		center_of_mass = center_of_mass,
+	{
-		center_of_mass_tris = center_of_mass_tris,
+		tri_idx := 0
+		for face_idx in 0 ..< len(faces) {
+			// 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 {
+					tet := Tetrahedron {
+						p = {tri[0], tri[1], tri[2], center_of_mass},
+					}
+
+					inertia_tensor += tetrahedron_inertia_tensor(tet, center_of_mass)
+
+					tri_idx += 1
+				}
+
+				i += 1
+			}
+		}
+	}
+	log.infof("inertia tensor: %v", inertia_tensor)
+
+	return {mesh = mesh, center_of_mass = center_of_mass, inertia_tensor = inertia_tensor}
+}
+
+Tetrahedron :: struct {
+	p: [4]rl.Vector3,
+}
+
+tetrahedron_volume :: #force_inline proc(tet: Tetrahedron) -> f32 {
+	return(
+		1.0 /
+		6.0 *
+		abs(lg.dot(lg.cross(tet.p[1] - tet.p[0], tet.p[2] - tet.p[0]), tet.p[3] - tet.p[0])) \
+	)
+}
+
+square :: #force_inline proc(val: f32) -> f32 {
+	return val * val
+}
+
+tetrahedron_inertia_tensor :: proc(tet: Tetrahedron, o: rl.Vector3) -> lg.Matrix3f32 {
+	p1, p2, p3, p4 := tet.p[0] - o, tet.p[1] - o, tet.p[2] - o, tet.p[3] - o
+	// Jacobian determinant is 6*Volume
+	det_j := abs(6.0 * tetrahedron_volume(tet))
+
+	moment_of_inertia_term :: proc(p1, p2, p3, p4: rl.Vector3, axis: int) -> f32 {
+		return(
+			square(p1[axis]) +
+			p1[axis] * p2[axis] +
+			square(p2[axis]) +
+			p1[axis] * p3[axis] +
+			p2[axis] * p3[axis] +
+			square(p3[axis]) +
+			p1[axis] * p4[axis] +
+			p2[axis] * p4[axis] +
+			p3[axis] * p4[axis] +
+			square(p4[axis]) \
+		)
+	}
+
+	product_of_inertia_term :: proc(p1, p2, p3, p4: rl.Vector3, axis1, axis2: int) -> f32 {
+		return(
+			2.0 * p1[axis1] * p1[axis2] +
+			p2[axis1] * p1[axis2] +
+			p3[axis1] * p1[axis2] +
+			p4[axis1] * p1[axis2] +
+			p1[axis1] * p2[axis2] +
+			2.0 * p2[axis1] * p2[axis2] +
+			p3[axis1] * p2[axis2] +
+			p4[axis1] * p2[axis2] +
+			p1[axis1] * p3[axis2] +
+			p2[axis1] * p3[axis2] +
+			2.0 * p3[axis1] * p3[axis2] +
+			p4[axis1] * p3[axis2] +
+			p1[axis1] * p4[axis2] +
+			p2[axis1] * p4[axis2] +
+			p3[axis1] * p4[axis2] +
+			2.0 * p4[axis1] * p4[axis2] \
+		)
+	}
+
+	MOMENT_OF_INERTIA_DENOM :: 1.0 / 60.0
+	PRODUCT_OF_INERTIA_DENOM :: 1.0 / 120.0
+
+	x_term := moment_of_inertia_term(p1, p2, p3, p4, 0)
+	y_term := moment_of_inertia_term(p1, p2, p3, p4, 1)
+	z_term := moment_of_inertia_term(p1, p2, p3, p4, 2)
+
+	// Moments of intertia with respect to XYZ
+	// Integral(y^2 + z^2)
+	a := det_j * (y_term + z_term) * MOMENT_OF_INERTIA_DENOM
+	// Integral(x^2 + z^2)
+	b := det_j * (x_term + z_term) * MOMENT_OF_INERTIA_DENOM
+	// Integral(x^2 + y^2)
+	c := det_j * (x_term + y_term) * MOMENT_OF_INERTIA_DENOM
+
+	// Products of inertia
+	a_ := product_of_inertia_term(p1, p2, p3, p4, axis1 = 1, axis2 = 2) * PRODUCT_OF_INERTIA_DENOM
+	b_ := product_of_inertia_term(p1, p2, p3, p4, axis1 = 0, axis2 = 2) * PRODUCT_OF_INERTIA_DENOM
+	c_ := product_of_inertia_term(p1, p2, p3, p4, axis1 = 0, axis2 = 1) * PRODUCT_OF_INERTIA_DENOM
+
+	return {a, -b_, -c_, -b_, b, -a_, -c_, -a_, c}
 }
 
 debug_draw_tetrahedron_wires :: proc(tri: [3]rl.Vector3, p: rl.Vector3, color: rl.Color) {

game/game.odin

@@ -557,7 +557,6 @@ draw :: proc() {
 		halfedge.debug_draw_mesh_wires(car_convex.mesh, rl.RED)
 		rl.DrawSphereWires(car_convex.mesh.center, 0.0, 8, 8, rl.BLUE)
 		rl.DrawSphereWires(car_convex.center_of_mass, 0.5, 8, 8, rl.RED)
-		rl.DrawSphereWires(car_convex.center_of_mass_tris, 0.5, 8, 8, rl.GREEN)
 
 		box1_convex := collision.box_to_convex(box1, context.temp_allocator)
 		box2_convex := collision.box_to_convex(box2, context.temp_allocator)

game/physics/helpers.odin

@@ -3,25 +3,25 @@ package physics
 import lg "core:math/linalg"
 import rl "vendor:raylib"
 
-inertia_tensor_sphere :: proc(radius: f32) -> (tensor: rl.Vector3) {
+inertia_tensor_sphere :: proc(radius: f32) -> (tensor: Matrix3) {
 	tensor = radius * radius * (2.0 / 3.0)
 
 	return
 }
 
-inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
+inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: Matrix3) {
 	CONSTANT :: f32(1.0 / 12.0)
 
-	tensor.x = size.z * size.z + size.y * size.y
+	tensor[0][0] = size.z * size.z + size.y * size.y
-	tensor.y = size.x * size.x + size.z * size.z
+	tensor[1][1] = size.x * size.x + size.z * size.z
-	tensor.z = size.x * size.x + size.y * size.y
+	tensor[2][2] = size.x * size.x + size.y * size.y
 
-	tensor *= CONSTANT
+	tensor = tensor * Matrix3(CONSTANT)
 
 	return
 }
 
-inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: rl.Vector3) {
+inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: Matrix3) {
 	switch s in shape {
 	case Shape_Sphere:
 		tensor = inertia_tensor_sphere(s.radius)

game/physics/immediate.odin

@@ -1,8 +1,11 @@
 package physics
 
+import "core:log"
 import lg "core:math/linalg"
 import rl "vendor:raylib"
 
+_ :: log
+
 Body_Config_Inertia_Mode :: enum {
 	From_Shape,
 	Explicit,
@@ -18,7 +21,7 @@ Body_Config :: struct {
 	mass:            f32,
 	inertia_mode:    Body_Config_Inertia_Mode,
 	// Unit inertia tensor
-	inertia_tensor:  rl.Vector3,
+	inertia_tensor:  Matrix3,
 }
 
 // TODO: rename to wheel
@@ -36,19 +39,19 @@ calculate_body_params_from_config :: proc(
 	config: Body_Config,
 ) -> (
 	inv_mass: f32,
-	inv_inertia_tensor: rl.Vector3,
+	inv_inertia_tensor: Matrix3,
 ) {
 	inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass
 
-	inertia_tensor: rl.Vector3
+	inertia_tensor: Matrix3
 	if config.inertia_mode == .Explicit {
 		inertia_tensor = config.inertia_tensor
 	} else {
 		inertia_tensor = inertia_tensor_collision_shape(config.shape)
 	}
-	inertia_tensor *= config.mass
+	inertia_tensor = inertia_tensor * Matrix3(config.mass)
 
-	inv_inertia_tensor = lg.all(lg.equal(inertia_tensor, rl.Vector3(0))) ? 0 : 1.0 / inertia_tensor
+	inv_inertia_tensor = lg.determinant(inertia_tensor) == 0 ? 0 : lg.inverse(inertia_tensor)
 
 	return
 }

game/physics/scene.odin

@@ -4,6 +4,8 @@ import rl "vendor:raylib"
 
 MAX_CONTACTS :: 1024
 
+Matrix3 :: # row_major matrix[3, 3]f32
+
 Scene :: struct {
 	bodies:                                    #soa[dynamic]Body,
 	suspension_constraints:                    #soa[dynamic]Suspension_Constraint,
@@ -33,7 +35,7 @@ Body :: struct {
 	// Mass
 	inv_mass:           f32,
 	// Moment of inertia
-	inv_inertia_tensor: rl.Vector3,
+	inv_inertia_tensor: Matrix3,
 	shape:              Collision_Shape,
 	// 
 	next_plus_one:      i32,

game/physics/simulation.odin

@@ -594,7 +594,7 @@ get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 {
 	rn = lg.cross(rn, normal)
 	rn = lg.quaternion_mul_vector3(inv_q, rn)
 
-	w := lg.dot(rn * rn, body.inv_inertia_tensor)
+	w := lg.dot(rn, rn * body.inv_inertia_tensor)
 	w += body.inv_mass
 
 	return w