diff --git a/game/assets/assets.odin b/game/assets/assets.odin index 1deebb7..268c9bb 100644 --- a/game/assets/assets.odin +++ b/game/assets/assets.odin @@ -33,9 +33,9 @@ Loaded_BVH :: struct { } Loaded_Convex :: struct { - mesh: collision.Convex, - center_of_mass: rl.Vector3, - center_of_mass_tris: rl.Vector3, + mesh: collision.Convex, + center_of_mass: 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 { - mesh = mesh, - center_of_mass = center_of_mass, - center_of_mass_tris = center_of_mass_tris, + inertia_tensor: lg.Matrix3f32 + // Find inertia tensor + { + 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) { diff --git a/game/game.odin b/game/game.odin index a3cafc5..b0a40b7 100644 --- a/game/game.odin +++ b/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) diff --git a/game/physics/helpers.odin b/game/physics/helpers.odin index 7181ac5..c03c4c9 100644 --- a/game/physics/helpers.odin +++ b/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.y = size.x * size.x + size.z * size.z - tensor.z = size.x * size.x + size.y * size.y + tensor[0][0] = size.z * size.z + size.y * size.y + tensor[1][1] = size.x * size.x + size.z * size.z + 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) diff --git a/game/physics/immediate.odin b/game/physics/immediate.odin index 3851353..0842af4 100644 --- a/game/physics/immediate.odin +++ b/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 } diff --git a/game/physics/scene.odin b/game/physics/scene.odin index f9e6db7..32e9f29 100644 --- a/game/physics/scene.odin +++ b/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, diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index fb8f736..2fc4901 100644 --- a/game/physics/simulation.odin +++ b/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