package physics

import lg "core:math/linalg"
import rl "vendor:raylib"

inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) {
	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 *= CONSTANT

	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_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 {
	// TODO: maybe store that
	inv_q := lg.quaternion_inverse(body.q)
	return lg.quaternion_mul_vector3(inv_q, pos - body.x)
}