288 lines
7.3 KiB
Odin
288 lines
7.3 KiB
Odin
package physics
|
|
|
|
import "collision"
|
|
import "game:halfedge"
|
|
import rl "vendor:raylib"
|
|
|
|
MAX_CONTACTS :: 1024
|
|
|
|
Matrix3 :: # row_major matrix[3, 3]f32
|
|
|
|
Sim_State :: struct {
|
|
bodies: #soa[dynamic]Body,
|
|
suspension_constraints: #soa[dynamic]Suspension_Constraint,
|
|
|
|
// Slices. When you call get_body or get_suspension_constraint you will get a pointer to an element in this slice
|
|
bodies_slice: #soa[]Body,
|
|
suspension_constraints_slice: #soa[]Suspension_Constraint,
|
|
first_free_body_plus_one: i32,
|
|
first_free_suspension_constraint_plus_one: i32,
|
|
|
|
// Persistent stuff for simulation
|
|
contact_pairs: [MAX_CONTACTS]Contact_Pair,
|
|
contact_pairs_len: int,
|
|
}
|
|
|
|
Scene :: struct {
|
|
simulation_states: [2]Sim_State,
|
|
simulation_state_index: i32,
|
|
}
|
|
|
|
Body :: struct {
|
|
// Is this body alive (if not it doesn't exist)
|
|
alive: bool,
|
|
// Pos
|
|
x: rl.Vector3,
|
|
// Linear vel
|
|
v: rl.Vector3,
|
|
// Orientation
|
|
q: rl.Quaternion,
|
|
// Angular vel (omega)
|
|
w: rl.Vector3,
|
|
// Mass
|
|
inv_mass: f32,
|
|
// Moment of inertia
|
|
inv_inertia_tensor: Matrix3,
|
|
shape: Collision_Shape,
|
|
//
|
|
next_plus_one: i32,
|
|
}
|
|
|
|
Shape_Sphere :: struct {
|
|
radius: f32,
|
|
}
|
|
|
|
Shape_Box :: struct {
|
|
size: rl.Vector3,
|
|
}
|
|
|
|
// TODO: Assuming mesh is generated and reinserted every frame for now, make it persistent yada yada
|
|
Shape_Convex :: struct {
|
|
mesh: collision.Convex,
|
|
center_of_mass: rl.Vector3,
|
|
inertia_tensor: Matrix3,
|
|
}
|
|
|
|
Collision_Shape :: union {
|
|
Shape_Box,
|
|
Shape_Convex,
|
|
}
|
|
|
|
Suspension_Constraint :: struct {
|
|
alive: bool,
|
|
// Pos relative to the body
|
|
rel_pos: rl.Vector3,
|
|
// Dir relative to the body
|
|
rel_dir: rl.Vector3,
|
|
// Handle of the rigid body
|
|
body: Body_Handle,
|
|
// Wheel radius
|
|
radius: f32,
|
|
// Rest distance
|
|
rest: f32,
|
|
// Inverse stiffness
|
|
compliance: f32,
|
|
// How much to damp velocity of the spring
|
|
damping: f32,
|
|
|
|
// Runtime state
|
|
hit: bool,
|
|
hit_point: rl.Vector3,
|
|
// rel_hit_point = rel_pos + rel_dir * hit_t
|
|
hit_t: f32,
|
|
turn_angle: f32,
|
|
drive_impulse: f32,
|
|
brake_impulse: f32,
|
|
applied_impulse: rl.Vector3,
|
|
|
|
// Free list
|
|
next_plus_one: i32,
|
|
}
|
|
|
|
// Index plus one, so handle 0 maps to invalid body
|
|
Body_Handle :: distinct i32
|
|
Suspension_Constraint_Handle :: distinct i32
|
|
|
|
INVALID_BODY :: Body_Handle(0)
|
|
INVALID_SUSPENSION_CONSTRAINT :: Suspension_Constraint_Handle(0)
|
|
|
|
is_body_handle_valid :: proc(handle: Body_Handle) -> bool {
|
|
return i32(handle) > 0
|
|
}
|
|
is_suspension_constraint_handle_valid :: proc(handle: Suspension_Constraint_Handle) -> bool {
|
|
return i32(handle) > 0
|
|
}
|
|
is_handle_valid :: proc {
|
|
is_body_handle_valid,
|
|
is_suspension_constraint_handle_valid,
|
|
}
|
|
|
|
Body_Ptr :: #soa^#soa[]Body
|
|
Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint
|
|
|
|
_invalid_body: #soa[1]Body
|
|
_invalid_body_slice := _invalid_body[:]
|
|
|
|
_invalid_suspension_constraint: #soa[1]Suspension_Constraint
|
|
_invalid_suspension_constraint_slice := _invalid_suspension_constraint[:]
|
|
|
|
get_sim_state :: proc(scene: ^Scene) -> ^Sim_State {
|
|
return &scene.simulation_states[scene.simulation_state_index]
|
|
}
|
|
|
|
get_prev_sim_state :: proc(scene: ^Scene) -> ^Sim_State {
|
|
return &scene.simulation_states[(scene.simulation_state_index + 1) % 2]
|
|
}
|
|
|
|
// lol
|
|
get_next_sim_state :: get_prev_sim_state
|
|
|
|
flip_sim_state :: proc(scene: ^Scene) {
|
|
scene.simulation_state_index = (scene.simulation_state_index + 1) % 2
|
|
}
|
|
|
|
/// Returns pointer to soa slice. NEVER STORE IT
|
|
get_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) -> Body_Ptr {
|
|
index := int(handle) - 1
|
|
if index < 0 || index >= len(sim_state.bodies_slice) {
|
|
return &_invalid_body_slice[0]
|
|
}
|
|
|
|
return &sim_state.bodies_slice[index]
|
|
}
|
|
|
|
copy_shape :: proc(
|
|
src: Collision_Shape,
|
|
allocator := context.allocator,
|
|
) -> (
|
|
dst: Collision_Shape,
|
|
) {
|
|
switch s in src {
|
|
case Shape_Box:
|
|
dst = s
|
|
case Shape_Convex:
|
|
new_convex := s
|
|
new_convex.mesh = halfedge.copy_mesh(s.mesh, allocator)
|
|
dst = new_convex
|
|
}
|
|
return
|
|
}
|
|
|
|
destroy_shape :: proc(shape: ^Collision_Shape, allocator := context.allocator) {
|
|
switch &s in shape {
|
|
case Shape_Box:
|
|
case Shape_Convex:
|
|
delete(s.mesh.faces, allocator)
|
|
delete(s.mesh.edges, allocator)
|
|
delete(s.mesh.vertices, allocator)
|
|
s.mesh = {}
|
|
}
|
|
}
|
|
|
|
copy_body :: proc(src: Body, allocator := context.allocator) -> (dst: Body) {
|
|
dst = src
|
|
dst.shape = copy_shape(src.shape)
|
|
dst.next_plus_one = 0
|
|
|
|
return
|
|
}
|
|
|
|
add_body :: proc(sim_state: ^Sim_State, body: Body) -> Body_Handle {
|
|
body_copy := copy_body(body)
|
|
|
|
body_copy.alive = true
|
|
body_copy.next_plus_one = 0
|
|
|
|
if sim_state.first_free_body_plus_one > 0 {
|
|
index := sim_state.first_free_body_plus_one
|
|
new_body := get_body(sim_state, Body_Handle(index))
|
|
next_plus_one := new_body.next_plus_one
|
|
new_body^ = body_copy
|
|
sim_state.first_free_body_plus_one = next_plus_one
|
|
|
|
return Body_Handle(index)
|
|
}
|
|
|
|
append_soa(&sim_state.bodies, body_copy)
|
|
index := len(sim_state.bodies)
|
|
|
|
sim_state.bodies_slice = sim_state.bodies[:]
|
|
|
|
return Body_Handle(index)
|
|
}
|
|
|
|
remove_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) {
|
|
if int(handle) > 1 {
|
|
body := get_body(sim_state, handle)
|
|
|
|
body.alive = false
|
|
|
|
destroy_shape(&body.shape)
|
|
|
|
body.next_plus_one = sim_state.first_free_body_plus_one
|
|
sim_state.first_free_body_plus_one = i32(handle)
|
|
}
|
|
}
|
|
|
|
/// Returns pointer to soa slice. NEVER STORE IT
|
|
get_suspension_constraint :: proc(
|
|
sim_state: ^Sim_State,
|
|
handle: Suspension_Constraint_Handle,
|
|
) -> Suspension_Constraint_Ptr {
|
|
if !is_handle_valid(handle) {
|
|
return &_invalid_suspension_constraint_slice[0]
|
|
}
|
|
|
|
index := int(handle) - 1
|
|
return &sim_state.suspension_constraints_slice[index]
|
|
}
|
|
|
|
add_suspension_constraint :: proc(
|
|
sim_state: ^Sim_State,
|
|
constraint: Suspension_Constraint,
|
|
) -> Suspension_Constraint_Handle {
|
|
copy := constraint
|
|
|
|
copy.alive = true
|
|
copy.next_plus_one = 0
|
|
|
|
if sim_state.first_free_suspension_constraint_plus_one > 0 {
|
|
index := sim_state.first_free_suspension_constraint_plus_one
|
|
new_constraint := get_suspension_constraint(sim_state, Suspension_Constraint_Handle(index))
|
|
next_plus_one := new_constraint.next_plus_one
|
|
new_constraint^ = copy
|
|
sim_state.first_free_suspension_constraint_plus_one = next_plus_one
|
|
return Suspension_Constraint_Handle(index)
|
|
}
|
|
|
|
append_soa(&sim_state.suspension_constraints, copy)
|
|
sim_state.suspension_constraints_slice = sim_state.suspension_constraints[:]
|
|
index := len(sim_state.suspension_constraints)
|
|
return Suspension_Constraint_Handle(index)
|
|
}
|
|
|
|
remove_suspension_constraint :: proc(sim_state: ^Sim_State, handle: Suspension_Constraint_Handle) {
|
|
if is_handle_valid(handle) {
|
|
constraint := get_suspension_constraint(sim_state, handle)
|
|
|
|
constraint.alive = false
|
|
constraint.next_plus_one = sim_state.first_free_suspension_constraint_plus_one
|
|
sim_state.first_free_suspension_constraint_plus_one = i32(handle)
|
|
}
|
|
}
|
|
|
|
_get_first_free_body :: proc(sim_state: ^Sim_State) -> i32 {
|
|
return sim_state.first_free_body_plus_one - 1
|
|
}
|
|
|
|
destry_sim_state :: proc(sim_state: ^Sim_State) {
|
|
delete_soa(sim_state.bodies)
|
|
delete_soa(sim_state.suspension_constraints)
|
|
}
|
|
|
|
destroy_physics_scene :: proc(scene: ^Scene) {
|
|
for &sim_state in scene.simulation_states {
|
|
destry_sim_state(&sim_state)
|
|
}
|
|
}
|