115 lines
3.0 KiB
Odin
115 lines
3.0 KiB
Odin
package physics
|
|
|
|
import "collision"
|
|
import lg "core:math/linalg"
|
|
import "game:halfedge"
|
|
|
|
inertia_tensor_sphere :: proc(radius: f32) -> (tensor: Matrix3) {
|
|
tensor = radius * radius * (2.0 / 3.0)
|
|
|
|
return
|
|
}
|
|
|
|
inertia_tensor_box :: proc(size: Vec3) -> (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: Input_Shape) -> (tensor: Matrix3) {
|
|
switch s in shape {
|
|
case Shape_Box:
|
|
tensor = inertia_tensor_box(s.size)
|
|
case Shape_Convex:
|
|
// TODO: assuming precomputed
|
|
tensor = s.inertia_tensor
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: Vec3) -> Vec3 {
|
|
return body.x + lg.quaternion_mul_vector3(body.q, pos)
|
|
}
|
|
|
|
body_local_to_world_vec :: #force_inline proc(body: Body_Ptr, vec: Vec3) -> Vec3 {
|
|
return lg.normalize0(lg.quaternion_mul_vector3(body.q, vec))
|
|
}
|
|
|
|
body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: Vec3) -> Vec3 {
|
|
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: Vec3) -> Vec3 {
|
|
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: Vec3) -> Vec3 {
|
|
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,
|
|
) -> Vec3 {
|
|
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,
|
|
) -> Vec3 {
|
|
local_right := lg.quaternion_mul_vector3(
|
|
lg.quaternion_angle_axis(wheel.turn_angle, Vec3{0, 1, 0}),
|
|
Vec3{1, 0, 0},
|
|
)
|
|
return body_local_to_world_vec(body, local_right)
|
|
}
|
|
|
|
body_get_shape_offset_local :: proc(body: Body_Ptr) -> (offset: Vec3) {
|
|
#partial switch s in body.shape {
|
|
case Internal_Shape_Convex:
|
|
offset = -s.center_of_mass
|
|
}
|
|
return
|
|
}
|
|
|
|
// Returns the shape's world position
|
|
// Shape can be offset from COM
|
|
body_get_shape_pos :: proc(body: Body_Ptr) -> Vec3 {
|
|
offset := body_get_shape_offset_local(body)
|
|
return body_local_to_world(body, offset)
|
|
}
|
|
|
|
body_get_convex_shape_world :: proc(
|
|
sim_state: ^Sim_State,
|
|
body: Body_Ptr,
|
|
allocator := context.temp_allocator,
|
|
) -> (
|
|
mesh: collision.Convex,
|
|
) {
|
|
switch s in body.shape {
|
|
case Shape_Box:
|
|
mesh = collision.box_to_convex(collision.Box{rad = s.size * 0.5}, allocator)
|
|
case Internal_Shape_Convex:
|
|
mesh = convex_container_get_mesh(&sim_state.convex_container, s.mesh)
|
|
// TODO: make sure this works as intendent
|
|
mesh = halfedge.copy_mesh(mesh, context.temp_allocator)
|
|
}
|
|
|
|
transform :=
|
|
lg.matrix4_translate(body_get_shape_pos(body)) * lg.matrix4_from_quaternion(body.q)
|
|
halfedge.transform_mesh(&mesh, transform)
|
|
|
|
return
|
|
}
|