Refactor convex shape handling
This will allow me to implement rewind easily
This commit is contained in:
parent
bf995882e6
commit
d816e96c3e
@ -1,5 +1,6 @@
|
||||
package spanpool
|
||||
|
||||
import "base:builtin"
|
||||
import "core:fmt"
|
||||
import "core:slice"
|
||||
|
||||
@ -133,3 +134,13 @@ reconcile :: proc(s: ^$T/Span_Pool($E)) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
copy :: proc(dst: ^Span_Pool($E), src: Span_Pool(E)) {
|
||||
resize(&dst.elems, len(src.elems))
|
||||
resize(&dst.generations, len(src.generations))
|
||||
resize(&dst.free_spans, len(src.free_spans))
|
||||
|
||||
builtin.copy(dst.elems[:], src.elems[:])
|
||||
builtin.copy(dst.generations[:], src.generations[:])
|
||||
builtin.copy(dst.free_spans[:], src.free_spans[:])
|
||||
}
|
||||
|
||||
@ -72,7 +72,7 @@ Car :: struct {
|
||||
}
|
||||
|
||||
SOLVER_CONFIG :: physics.Solver_Config {
|
||||
timestep = 1.0 / 120,
|
||||
timestep = 1.0 / 60,
|
||||
gravity = rl.Vector3{0, -9.8, 0},
|
||||
substreps_minus_one = 2 - 1,
|
||||
}
|
||||
|
||||
@ -14,6 +14,7 @@ _ :: math
|
||||
_ :: debug
|
||||
|
||||
draw_debug_shape :: proc(
|
||||
sim_state: ^Sim_State,
|
||||
shape: Collision_Shape,
|
||||
pos: rl.Vector3,
|
||||
rot: rl.Quaternion,
|
||||
@ -30,8 +31,9 @@ draw_debug_shape :: proc(
|
||||
switch s in shape {
|
||||
case Shape_Box:
|
||||
rl.DrawCubeV(0, s.size, color)
|
||||
case Shape_Convex:
|
||||
halfedge.debug_draw_mesh_wires(s.mesh, color)
|
||||
case Internal_Shape_Convex:
|
||||
mesh := convex_container_get_mesh(&sim_state.convex_container, s.mesh)
|
||||
halfedge.debug_draw_mesh_wires(mesh, color)
|
||||
}
|
||||
}
|
||||
|
||||
@ -55,6 +57,7 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
||||
rl.DrawLine3D(pos, pos + z, rl.BLUE)
|
||||
|
||||
draw_debug_shape(
|
||||
sim_state,
|
||||
body.shape,
|
||||
body_get_shape_pos(body),
|
||||
body.q,
|
||||
|
||||
@ -23,7 +23,7 @@ inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: Matrix3) {
|
||||
return
|
||||
}
|
||||
|
||||
inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: Matrix3) {
|
||||
inertia_tensor_collision_shape :: proc(shape: Input_Shape) -> (tensor: Matrix3) {
|
||||
switch s in shape {
|
||||
case Shape_Box:
|
||||
tensor = inertia_tensor_box(s.size)
|
||||
@ -78,7 +78,7 @@ wheel_get_right_vec :: #force_inline proc(
|
||||
|
||||
body_get_shape_offset_local :: proc(body: Body_Ptr) -> (offset: rl.Vector3) {
|
||||
#partial switch s in body.shape {
|
||||
case Shape_Convex:
|
||||
case Internal_Shape_Convex:
|
||||
offset = -s.center_of_mass
|
||||
}
|
||||
return
|
||||
@ -92,6 +92,7 @@ body_get_shape_pos :: proc(body: Body_Ptr) -> rl.Vector3 {
|
||||
}
|
||||
|
||||
body_get_convex_shape_world :: proc(
|
||||
sim_state: ^Sim_State,
|
||||
body: Body_Ptr,
|
||||
allocator := context.temp_allocator,
|
||||
) -> (
|
||||
@ -100,8 +101,10 @@ body_get_convex_shape_world :: proc(
|
||||
switch s in body.shape {
|
||||
case Shape_Box:
|
||||
mesh = collision.box_to_convex(collision.Box{rad = s.size * 0.5}, allocator)
|
||||
case Shape_Convex:
|
||||
mesh = halfedge.copy_mesh(s.mesh, context.temp_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 :=
|
||||
|
||||
@ -1,8 +1,6 @@
|
||||
package physics
|
||||
|
||||
import "core:log"
|
||||
import lg "core:math/linalg"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
_ :: log
|
||||
|
||||
@ -11,79 +9,6 @@ Body_Config_Inertia_Mode :: enum {
|
||||
Explicit,
|
||||
}
|
||||
|
||||
// Immediate mode stuff for testing
|
||||
Body_Config :: struct {
|
||||
initial_pos: rl.Vector3,
|
||||
initial_rot: rl.Quaternion,
|
||||
initial_vel: rl.Vector3,
|
||||
initial_ang_vel: rl.Vector3,
|
||||
shape: Collision_Shape,
|
||||
mass: f32,
|
||||
inertia_mode: Body_Config_Inertia_Mode,
|
||||
// Unit inertia tensor
|
||||
inertia_tensor: Matrix3,
|
||||
}
|
||||
|
||||
// TODO: rename to wheel
|
||||
Suspension_Constraint_Config :: struct {
|
||||
rel_pos: rl.Vector3,
|
||||
rel_dir: rl.Vector3,
|
||||
body: Body_Handle,
|
||||
rest: f32,
|
||||
compliance: f32,
|
||||
damping: f32,
|
||||
radius: f32,
|
||||
}
|
||||
|
||||
calculate_body_params_from_config :: proc(
|
||||
config: Body_Config,
|
||||
) -> (
|
||||
inv_mass: f32,
|
||||
inv_inertia_tensor: Matrix3,
|
||||
) {
|
||||
inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass
|
||||
|
||||
inertia_tensor: Matrix3
|
||||
if config.inertia_mode == .Explicit {
|
||||
inertia_tensor = config.inertia_tensor
|
||||
} else {
|
||||
inertia_tensor = inertia_tensor_collision_shape(config.shape)
|
||||
}
|
||||
inertia_tensor = inertia_tensor * Matrix3(config.mass)
|
||||
|
||||
inv_inertia_tensor = lg.determinant(inertia_tensor) == 0 ? 0 : lg.inverse(inertia_tensor)
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
initialize_body_from_config :: proc(body: ^Body, config: Body_Config) {
|
||||
body.x = config.initial_pos
|
||||
body.q = config.initial_rot
|
||||
body.v = config.initial_vel
|
||||
body.w = config.initial_ang_vel
|
||||
body.shape = config.shape
|
||||
|
||||
body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
|
||||
}
|
||||
|
||||
update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) {
|
||||
body.shape = config.shape
|
||||
body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
|
||||
}
|
||||
|
||||
update_suspension_constraint_from_config :: proc(
|
||||
constraint: Suspension_Constraint_Ptr,
|
||||
config: Suspension_Constraint_Config,
|
||||
) {
|
||||
constraint.rel_pos = config.rel_pos
|
||||
constraint.rel_dir = config.rel_dir
|
||||
constraint.body = config.body
|
||||
constraint.rest = config.rest
|
||||
constraint.compliance = config.compliance
|
||||
constraint.damping = config.damping
|
||||
constraint.radius = config.radius
|
||||
}
|
||||
|
||||
immediate_body :: proc(
|
||||
scene: ^Scene,
|
||||
state: ^Solver_State,
|
||||
@ -101,10 +26,8 @@ immediate_body :: proc(
|
||||
handle = body.handle
|
||||
update_body_from_config(get_body(get_sim_state(scene), handle), config)
|
||||
} else {
|
||||
new_body: Body
|
||||
state.num_referenced_bodies += 1
|
||||
initialize_body_from_config(&new_body, config)
|
||||
handle = add_body(get_sim_state(scene), new_body)
|
||||
handle = add_body(get_sim_state(scene), config)
|
||||
state.immedate_bodies[id] = {
|
||||
handle = handle,
|
||||
last_ref = state.simulation_frame,
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
package physics
|
||||
|
||||
import "collision"
|
||||
import "game:halfedge"
|
||||
import lg "core:math/linalg"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
MAX_CONTACTS :: 1024
|
||||
@ -21,6 +21,7 @@ Sim_State :: struct {
|
||||
// Persistent stuff for simulation
|
||||
contact_pairs: [MAX_CONTACTS]Contact_Pair,
|
||||
contact_pairs_len: int,
|
||||
convex_container: Convex_Container,
|
||||
}
|
||||
|
||||
Scene :: struct {
|
||||
@ -56,7 +57,12 @@ Shape_Box :: struct {
|
||||
size: rl.Vector3,
|
||||
}
|
||||
|
||||
// TODO: Assuming mesh is generated and reinserted every frame for now, make it persistent yada yada
|
||||
Internal_Shape_Convex :: struct {
|
||||
mesh: Convex_Handle,
|
||||
center_of_mass: rl.Vector3,
|
||||
inertia_tensor: Matrix3,
|
||||
}
|
||||
|
||||
Shape_Convex :: struct {
|
||||
mesh: collision.Convex,
|
||||
center_of_mass: rl.Vector3,
|
||||
@ -65,7 +71,7 @@ Shape_Convex :: struct {
|
||||
|
||||
Collision_Shape :: union {
|
||||
Shape_Box,
|
||||
Shape_Convex,
|
||||
Internal_Shape_Convex,
|
||||
}
|
||||
|
||||
Suspension_Constraint :: struct {
|
||||
@ -151,59 +157,127 @@ get_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) -> Body_Ptr {
|
||||
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) {
|
||||
remove_shape :: proc(sim_state: ^Sim_State, shape: ^Collision_Shape) {
|
||||
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 = {}
|
||||
case Internal_Shape_Convex:
|
||||
convex_container_remove(&sim_state.convex_container, 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
|
||||
Input_Shape :: union {
|
||||
Shape_Box,
|
||||
Shape_Convex,
|
||||
}
|
||||
|
||||
Body_Config :: struct {
|
||||
initial_pos: rl.Vector3,
|
||||
initial_rot: rl.Quaternion,
|
||||
initial_vel: rl.Vector3,
|
||||
initial_ang_vel: rl.Vector3,
|
||||
shape: Input_Shape,
|
||||
mass: f32,
|
||||
inertia_mode: Body_Config_Inertia_Mode,
|
||||
// Unit inertia tensor
|
||||
inertia_tensor: Matrix3,
|
||||
}
|
||||
|
||||
// TODO: rename to wheel
|
||||
Suspension_Constraint_Config :: struct {
|
||||
rel_pos: rl.Vector3,
|
||||
rel_dir: rl.Vector3,
|
||||
body: Body_Handle,
|
||||
rest: f32,
|
||||
compliance: f32,
|
||||
damping: f32,
|
||||
radius: f32,
|
||||
}
|
||||
|
||||
calculate_body_params_from_config :: proc(
|
||||
config: Body_Config,
|
||||
) -> (
|
||||
inv_mass: f32,
|
||||
inv_inertia_tensor: Matrix3,
|
||||
) {
|
||||
inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass
|
||||
|
||||
inertia_tensor: Matrix3
|
||||
if config.inertia_mode == .Explicit {
|
||||
inertia_tensor = config.inertia_tensor
|
||||
} else {
|
||||
inertia_tensor = inertia_tensor_collision_shape(config.shape)
|
||||
}
|
||||
inertia_tensor = inertia_tensor * Matrix3(config.mass)
|
||||
|
||||
inv_inertia_tensor = lg.determinant(inertia_tensor) == 0 ? 0 : lg.inverse(inertia_tensor)
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
add_body :: proc(sim_state: ^Sim_State, body: Body) -> Body_Handle {
|
||||
body_copy := copy_body(body)
|
||||
add_shape :: proc(sim_state: ^Sim_State, shape: Input_Shape) -> (result: Collision_Shape) {
|
||||
switch s in shape {
|
||||
case Shape_Box:
|
||||
result = s
|
||||
case Shape_Convex:
|
||||
convex: Internal_Shape_Convex
|
||||
convex.mesh = convex_container_add(&sim_state.convex_container, s.mesh)
|
||||
convex.center_of_mass = s.center_of_mass
|
||||
convex.inertia_tensor = s.inertia_tensor
|
||||
result = convex
|
||||
}
|
||||
|
||||
body_copy.alive = true
|
||||
body_copy.next_plus_one = 0
|
||||
return
|
||||
}
|
||||
|
||||
initialize_body_from_config :: proc(sim_state: ^Sim_State, body: ^Body, config: Body_Config) {
|
||||
body.x = config.initial_pos
|
||||
body.q = config.initial_rot
|
||||
body.v = config.initial_vel
|
||||
body.w = config.initial_ang_vel
|
||||
|
||||
body.shape = add_shape(sim_state, config.shape)
|
||||
|
||||
body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
|
||||
}
|
||||
|
||||
update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) {
|
||||
// TODO: Figure out how to update shape
|
||||
// body.shape = config.shape
|
||||
body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
|
||||
}
|
||||
|
||||
update_suspension_constraint_from_config :: proc(
|
||||
constraint: Suspension_Constraint_Ptr,
|
||||
config: Suspension_Constraint_Config,
|
||||
) {
|
||||
constraint.rel_pos = config.rel_pos
|
||||
constraint.rel_dir = config.rel_dir
|
||||
constraint.body = config.body
|
||||
constraint.rest = config.rest
|
||||
constraint.compliance = config.compliance
|
||||
constraint.damping = config.damping
|
||||
constraint.radius = config.radius
|
||||
}
|
||||
|
||||
|
||||
add_body :: proc(sim_state: ^Sim_State, config: Body_Config) -> Body_Handle {
|
||||
body: Body
|
||||
|
||||
initialize_body_from_config(sim_state, &body, config)
|
||||
|
||||
body.alive = true
|
||||
|
||||
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
|
||||
new_body^ = body
|
||||
sim_state.first_free_body_plus_one = next_plus_one
|
||||
|
||||
return Body_Handle(index)
|
||||
}
|
||||
|
||||
append_soa(&sim_state.bodies, body_copy)
|
||||
append_soa(&sim_state.bodies, body)
|
||||
index := len(sim_state.bodies)
|
||||
|
||||
sim_state.bodies_slice = sim_state.bodies[:]
|
||||
@ -217,7 +291,7 @@ remove_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) {
|
||||
|
||||
body.alive = false
|
||||
|
||||
destroy_shape(&body.shape)
|
||||
remove_shape(sim_state, &body.shape)
|
||||
|
||||
body.next_plus_one = sim_state.first_free_body_plus_one
|
||||
sim_state.first_free_body_plus_one = i32(handle)
|
||||
@ -278,6 +352,7 @@ _get_first_free_body :: proc(sim_state: ^Sim_State) -> i32 {
|
||||
destry_sim_state :: proc(sim_state: ^Sim_State) {
|
||||
delete_soa(sim_state.bodies)
|
||||
delete_soa(sim_state.suspension_constraints)
|
||||
convex_container_destroy(&sim_state.convex_container)
|
||||
}
|
||||
|
||||
destroy_physics_scene :: proc(scene: ^Scene) {
|
||||
|
||||
@ -1,4 +1,66 @@
|
||||
package physics
|
||||
|
||||
import he "game:halfedge"
|
||||
import "game:container/spanpool"
|
||||
|
||||
Convex_Container :: struct {
|
||||
vertices: spanpool.Span_Pool(he.Vertex),
|
||||
faces: spanpool.Span_Pool(he.Face),
|
||||
edges: spanpool.Span_Pool(he.Half_Edge),
|
||||
}
|
||||
|
||||
Convex_Handle :: struct {
|
||||
vertices: spanpool.Handle,
|
||||
faces: spanpool.Handle,
|
||||
edges: spanpool.Handle,
|
||||
}
|
||||
|
||||
convex_container_add :: proc(container: ^Convex_Container, mesh: he.Half_Edge_Mesh) -> (handle: Convex_Handle) {
|
||||
handle.vertices = spanpool.allocate_elems(&container.vertices, ..mesh.vertices)
|
||||
handle.faces = spanpool.allocate_elems(&container.faces, ..mesh.faces)
|
||||
handle.edges = spanpool.allocate_elems(&container.edges, ..mesh.edges)
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
convex_container_get_mesh :: proc(container: ^Convex_Container, handle: Convex_Handle) -> (mesh: he.Half_Edge_Mesh) {
|
||||
mesh.vertices = spanpool.resolve_slice(&container.vertices, handle.vertices)
|
||||
mesh.faces = spanpool.resolve_slice(&container.faces, handle.faces)
|
||||
mesh.edges = spanpool.resolve_slice(&container.edges, handle.edges)
|
||||
|
||||
// TODO: save and return mesh center
|
||||
avg_scale := 1.0 / f32(len(mesh.vertices))
|
||||
for v in mesh.vertices {
|
||||
mesh.center += v.pos * avg_scale
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
convex_container_remove :: proc(container: ^Convex_Container, handle: Convex_Handle) {
|
||||
spanpool.free(&container.vertices, handle.vertices)
|
||||
spanpool.free(&container.faces, handle.faces)
|
||||
spanpool.free(&container.edges, handle.edges)
|
||||
}
|
||||
|
||||
convex_container_reconcile :: proc(container: ^Convex_Container) {
|
||||
spanpool.reconcile(&container.vertices)
|
||||
spanpool.reconcile(&container.faces)
|
||||
spanpool.reconcile(&container.edges)
|
||||
}
|
||||
|
||||
convex_container_copy :: proc(dst: ^Convex_Container, src: Convex_Container) {
|
||||
spanpool.copy(&dst.vertices, src.vertices)
|
||||
spanpool.copy(&dst.faces, src.faces)
|
||||
spanpool.copy(&dst.edges, src.edges)
|
||||
}
|
||||
|
||||
convex_container_destroy :: proc(container: ^Convex_Container) {
|
||||
spanpool.destroy_spanpool(&container.vertices)
|
||||
spanpool.destroy_spanpool(&container.faces)
|
||||
spanpool.destroy_spanpool(&container.edges)
|
||||
}
|
||||
|
||||
//
|
||||
// import "core:container/intrusive/list"
|
||||
// import "game:container/spanpool"
|
||||
|
||||
@ -64,6 +64,8 @@ prepare_next_sim_state :: proc(scene: ^Scene) {
|
||||
for i in 0 ..< len(next_state.suspension_constraints) {
|
||||
next_state.suspension_constraints[i] = current_state.suspension_constraints[i]
|
||||
}
|
||||
|
||||
convex_container_copy(&next_state.convex_container, current_state.convex_container)
|
||||
}
|
||||
|
||||
Step_Mode :: enum {
|
||||
@ -195,8 +197,8 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
|
||||
body2.alive &&
|
||||
!handled_pairs[{a = min(i, j), b = max(i, j)}] {
|
||||
m1, m2 :=
|
||||
body_get_convex_shape_world(body),
|
||||
body_get_convex_shape_world(body2)
|
||||
body_get_convex_shape_world(sim_state, body),
|
||||
body_get_convex_shape_world(sim_state, body2)
|
||||
|
||||
// Raw manifold has contact points in world space
|
||||
raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user