sergeypdev
999a7a4631
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
75 lines
2.1 KiB
Odin
75 lines
2.1 KiB
Odin
package physics
|
|
|
|
import lg "core:math/linalg"
|
|
import rl "vendor:raylib"
|
|
|
|
inertia_tensor_sphere :: proc(radius: f32) -> (tensor: Matrix3) {
|
|
tensor = radius * radius * (2.0 / 3.0)
|
|
|
|
return
|
|
}
|
|
|
|
inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: Matrix3) {
|
|
CONSTANT :: f32(1.0 / 12.0)
|
|
|
|
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 = tensor * Matrix3(CONSTANT)
|
|
|
|
return
|
|
}
|
|
|
|
inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: Matrix3) {
|
|
switch s in shape {
|
|
case Shape_Sphere:
|
|
tensor = inertia_tensor_sphere(s.radius)
|
|
case Shape_Box:
|
|
tensor = inertia_tensor_box(s.size)
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
|
|
return body.x + lg.quaternion_mul_vector3(body.q, pos)
|
|
}
|
|
|
|
body_local_to_world_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
|
|
return lg.normalize0(lg.quaternion_mul_vector3(body.q, vec))
|
|
}
|
|
|
|
body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
|
|
inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(body.q))
|
|
return lg.quaternion_mul_vector3(inv_q, pos - body.x)
|
|
}
|
|
|
|
body_world_to_local_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 {
|
|
inv_q := lg.quaternion_inverse(body.q)
|
|
return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec))
|
|
}
|
|
|
|
body_velocity_at_point :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
|
|
return body.v + lg.cross(body.w, pos - body.x)
|
|
}
|
|
|
|
wheel_get_rel_wheel_pos :: #force_inline proc(
|
|
body: Body_Ptr,
|
|
wheel: Suspension_Constraint_Ptr,
|
|
) -> rl.Vector3 {
|
|
t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest
|
|
return wheel.rel_pos + wheel.rel_dir * (t - wheel.radius)
|
|
}
|
|
|
|
wheel_get_right_vec :: #force_inline proc(
|
|
body: Body_Ptr,
|
|
wheel: Suspension_Constraint_Ptr,
|
|
) -> rl.Vector3 {
|
|
local_right := lg.quaternion_mul_vector3(
|
|
lg.quaternion_angle_axis(wheel.turn_angle, rl.Vector3{0, 1, 0}),
|
|
rl.Vector3{1, 0, 0},
|
|
)
|
|
return body_local_to_world_vec(body, local_right)
|
|
}
|