sergeypdev/gutter_runner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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
This commit is contained in:
sergeypdev 2025-02-02 01:17:35 +04:00
parent 660a535a7c
commit 999a7a4631
6 changed files with 136 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
game/assets/assets.odin
View File

@ -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 {
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) {

1
game/game.odin
View File

@ -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)

14
game/physics/helpers.odin
View File

@ -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)

13
game/physics/immediate.odin
View File

@ -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
}

4
game/physics/scene.odin
View File

@ -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,

2
game/physics/simulation.odin
View File

@ -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