Double buffer physics state to allow easy debugging and interpolation
Fix a bug in restitution
This commit is contained in:
parent
66f42c3cee
commit
bf995882e6
@ -35,7 +35,7 @@ esac
|
||||
|
||||
# Build the game.
|
||||
echo "Building game$DLL_EXT"
|
||||
odin build game -extra-linker-flags:"$EXTRA_LINKER_FLAGS" -define:RAYLIB_SHARED=true -define:TRACY_ENABLE=true -collection:libs=./libs -collection:common=./common -collection:game=./game -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug -o:speed
|
||||
odin build game -extra-linker-flags:"$EXTRA_LINKER_FLAGS" -define:RAYLIB_SHARED=true -define:TRACY_ENABLE=true -collection:libs=./libs -collection:common=./common -collection:game=./game -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug
|
||||
|
||||
# Need to use a temp file on Linux because it first writes an empty `game.so`, which the game will load before it is actually fully written.
|
||||
mv game_tmp$DLL_EXT game$DLL_EXT
|
||||
|
||||
@ -452,7 +452,6 @@ get_convex :: proc(assetman: ^Asset_Manager, path: cstring) -> (result: Loaded_C
|
||||
}
|
||||
}
|
||||
}
|
||||
log.infof("inertia tensor: %v", inertia_tensor)
|
||||
inertia_tensor = inertia_tensor * lg.Matrix3f32(1.0 / total_volume)
|
||||
|
||||
return {mesh = mesh, center_of_mass = center_of_mass, inertia_tensor = inertia_tensor}
|
||||
|
||||
188
game/game.odin
188
game/game.odin
@ -48,15 +48,20 @@ destroy_world :: proc(world: ^World) {
|
||||
|
||||
|
||||
Runtime_World :: struct {
|
||||
world: World,
|
||||
pause: bool,
|
||||
solver_state: physics.Solver_State,
|
||||
car_com: rl.Vector3,
|
||||
car_handle: physics.Body_Handle,
|
||||
camera_yaw_pitch: rl.Vector2,
|
||||
camera_speed: f32,
|
||||
camera: rl.Camera3D,
|
||||
world: World,
|
||||
pause: bool,
|
||||
solver_state: physics.Solver_State,
|
||||
car_com: rl.Vector3,
|
||||
car_handle: physics.Body_Handle,
|
||||
camera_yaw_pitch: rl.Vector2,
|
||||
camera_speed: f32,
|
||||
camera: rl.Camera3D,
|
||||
orbit_camera: Orbit_Camera,
|
||||
dt: f32,
|
||||
step_simulation: bool,
|
||||
single_step_simulation: bool,
|
||||
}
|
||||
|
||||
destroy_runtime_world :: proc(runtime_world: ^Runtime_World) {
|
||||
destroy_world(&runtime_world.world)
|
||||
physics.destroy_solver_state(&runtime_world.solver_state)
|
||||
@ -67,9 +72,9 @@ Car :: struct {
|
||||
}
|
||||
|
||||
SOLVER_CONFIG :: physics.Solver_Config {
|
||||
timestep = 1.0 / 60,
|
||||
timestep = 1.0 / 120,
|
||||
gravity = rl.Vector3{0, -9.8, 0},
|
||||
substreps_minus_one = 4 - 1,
|
||||
substreps_minus_one = 2 - 1,
|
||||
}
|
||||
|
||||
Game_Memory :: struct {
|
||||
@ -253,12 +258,13 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
&runtime_world.solver_state,
|
||||
#hash("car", "fnv32a"),
|
||||
physics.Body_Config {
|
||||
initial_pos = {0, 2, 0},
|
||||
initial_pos = {0, 4, 0},
|
||||
initial_rot = linalg.quaternion_angle_axis(
|
||||
math.RAD_PER_DEG * 100,
|
||||
rl.Vector3{0, 1, 0},
|
||||
),
|
||||
initial_ang_vel = {0, 0, 0},
|
||||
math.RAD_PER_DEG * 180,
|
||||
rl.Vector3{0, 0, 1},
|
||||
) *
|
||||
linalg.quaternion_angle_axis(math.RAD_PER_DEG * 30, rl.Vector3{1, 0, 0}),
|
||||
initial_ang_vel = {0, 0, 20},
|
||||
shape = physics.Shape_Convex {
|
||||
mesh = car_convex.mesh,
|
||||
center_of_mass = car_convex.center_of_mass,
|
||||
@ -268,7 +274,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
},
|
||||
)
|
||||
|
||||
if true {
|
||||
if false {
|
||||
|
||||
for x in 0 ..< 1 {
|
||||
for y in -3 ..< 4 {
|
||||
@ -290,11 +296,11 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
|
||||
// car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
|
||||
|
||||
camera := &runtime_world.camera
|
||||
// camera := &runtime_world.camera
|
||||
|
||||
camera.up = rl.Vector3{0, 1, 0}
|
||||
camera.fovy = 60
|
||||
camera.projection = .PERSPECTIVE
|
||||
// camera.up = rl.Vector3{0, 1, 0}
|
||||
// camera.fovy = 60
|
||||
// camera.projection = .PERSPECTIVE
|
||||
// camera.position = physics.body_local_to_world(
|
||||
// car_body,
|
||||
// physics.body_world_to_local(
|
||||
@ -302,10 +308,12 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
// physics.body_local_to_world(car_body, rl.Vector3{1, 0, -2}),
|
||||
// ),
|
||||
// )
|
||||
camera.target = physics.get_body(&world.physics_scene, runtime_world.car_handle).x
|
||||
if runtime_world.camera.position == {} {
|
||||
runtime_world.camera.position = runtime_world.camera.target - rl.Vector3{10, 0, 10}
|
||||
}
|
||||
// camera.target = physics.get_body(&world.physics_scene, runtime_world.car_handle).x
|
||||
// if runtime_world.camera.position == {} {
|
||||
// runtime_world.camera.position = runtime_world.camera.target - rl.Vector3{10, 0, 10}
|
||||
// }
|
||||
|
||||
sim_state := physics.get_sim_state(&world.physics_scene)
|
||||
|
||||
// 1.6 is a good value
|
||||
wheel_extent_x := f32(1.7)
|
||||
@ -383,7 +391,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
TURN_ANGLE :: -f32(30) * math.RAD_PER_DEG
|
||||
|
||||
for wheel_handle in drive_wheels {
|
||||
wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle)
|
||||
wheel := physics.get_suspension_constraint(sim_state, wheel_handle)
|
||||
|
||||
wheel.drive_impulse = 0
|
||||
wheel.brake_impulse = 0
|
||||
@ -398,7 +406,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
}
|
||||
|
||||
for wheel_handle in turn_wheels {
|
||||
wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle)
|
||||
wheel := physics.get_suspension_constraint(sim_state, wheel_handle)
|
||||
wheel.turn_angle = 0
|
||||
|
||||
if rl.IsKeyDown(.A) && !g_mem.free_cam {
|
||||
@ -408,15 +416,57 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
if rl.IsKeyDown(.D) && !g_mem.free_cam {
|
||||
wheel.turn_angle += TURN_ANGLE
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if !g_mem.physics_pause || rl.IsKeyPressed(.PERIOD) {
|
||||
physics.simulate(&world.physics_scene, &runtime_world.solver_state, SOLVER_CONFIG, dt)
|
||||
}
|
||||
runtime_world.dt = dt
|
||||
should_single_step := rl.IsKeyPressed(.PERIOD)
|
||||
runtime_world.step_simulation = !g_mem.physics_pause || should_single_step
|
||||
runtime_world.single_step_simulation = should_single_step
|
||||
}
|
||||
}
|
||||
|
||||
Orbit_Camera :: struct {
|
||||
target: rl.Vector3,
|
||||
yaw, pitch: f32,
|
||||
distance: f32,
|
||||
}
|
||||
|
||||
orbit_camera_update :: proc(camera: ^Orbit_Camera) {
|
||||
camera.target =
|
||||
physics.get_body(physics.get_sim_state(&get_runtime_world().world.physics_scene), get_runtime_world().car_handle).x
|
||||
|
||||
mouse_delta := rl.GetMouseDelta()
|
||||
|
||||
SENSE :: 0.01
|
||||
|
||||
rl.HideCursor()
|
||||
|
||||
camera.yaw += mouse_delta.x * SENSE
|
||||
camera.pitch += mouse_delta.y * SENSE
|
||||
camera.pitch = math.clamp(camera.pitch, -math.PI / 2.0 + 0.0001, math.PI / 2.0 - 0.0001)
|
||||
}
|
||||
|
||||
orbit_camera_to_rl :: proc(camera: Orbit_Camera) -> rl.Camera3D {
|
||||
result: rl.Camera3D
|
||||
|
||||
result.target = camera.target
|
||||
|
||||
rotation :=
|
||||
linalg.matrix3_rotate(-camera.yaw, rl.Vector3{0, 1, 0}) *
|
||||
linalg.matrix3_rotate(-camera.pitch, rl.Vector3{1, 0, 0})
|
||||
|
||||
// rotation = linalg.transpose(rotation)
|
||||
|
||||
position := rotation * rl.Vector3{0, 0, 1}
|
||||
position *= camera.distance
|
||||
|
||||
result.position = result.target + position
|
||||
result.up = rl.Vector3{0, 1, 0}
|
||||
result.fovy = 60
|
||||
|
||||
return result
|
||||
}
|
||||
|
||||
update :: proc() {
|
||||
tracy.Zone()
|
||||
|
||||
@ -426,6 +476,8 @@ update :: proc() {
|
||||
|
||||
if rl.IsKeyPressed(.F1) {
|
||||
g_mem.free_cam = !g_mem.free_cam
|
||||
|
||||
g_mem.es.world.player_pos = g_mem.runtime_world.camera.position
|
||||
}
|
||||
|
||||
dt := rl.GetFrameTime()
|
||||
@ -468,6 +520,9 @@ update :: proc() {
|
||||
} else {
|
||||
if g_mem.free_cam {
|
||||
update_free_look_camera(get_editor_state())
|
||||
} else {
|
||||
orbit_camera_update(&get_runtime_world().orbit_camera)
|
||||
get_runtime_world().camera = orbit_camera_to_rl(get_runtime_world().orbit_camera)
|
||||
}
|
||||
update_runtime_world(get_runtime_world(), dt)
|
||||
}
|
||||
@ -511,6 +566,7 @@ draw :: proc() {
|
||||
runtime_world := get_runtime_world()
|
||||
world := get_world()
|
||||
|
||||
dt := runtime_world.dt
|
||||
camera := game_camera_3d()
|
||||
|
||||
points := &world.track.points
|
||||
@ -522,7 +578,9 @@ draw :: proc() {
|
||||
// rl.GetScreenToWorldRay(rl.GetMousePosition(), camera),
|
||||
// )
|
||||
|
||||
car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle)
|
||||
sim_state := physics.get_sim_state(&world.physics_scene)
|
||||
|
||||
car_body := physics.get_body(sim_state, runtime_world.car_handle)
|
||||
car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
|
||||
|
||||
mesh_col: bvh.Collision
|
||||
@ -566,62 +624,22 @@ draw :: proc() {
|
||||
halfedge.transform_mesh(&box1_convex, box1_mat)
|
||||
halfedge.transform_mesh(&box2_convex, box2_mat)
|
||||
|
||||
// manifold, _ := collision.convex_vs_convex_sat(box1_convex, box2_convex)
|
||||
|
||||
// halfedge.debug_draw_mesh_wires(halfedge.Half_Edge_Mesh(box1_convex), rl.RED)
|
||||
// halfedge.debug_draw_mesh_wires(halfedge.Half_Edge_Mesh(box2_convex), rl.RED)
|
||||
|
||||
// {
|
||||
// rlgl_transform_scope(auto_cast linalg.matrix4_from_quaternion(rot1))
|
||||
// rl.DrawCubeWiresV(box1.pos, box1.rad * 2, rl.RED)
|
||||
// }
|
||||
// {
|
||||
// rlgl_transform_scope(auto_cast linalg.matrix4_from_quaternion(rot2))
|
||||
// rl.DrawCubeWiresV(box2.pos, box2.rad * 2, rl.RED)
|
||||
// }
|
||||
// for p in manifold.points_a[:manifold.points_len] {
|
||||
// rl.DrawSphereWires(p, 0.05, 8, 8, rl.BLUE)
|
||||
// }
|
||||
|
||||
// {
|
||||
// mesh_bvh := assets.get_bvh(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb")
|
||||
|
||||
// for &blas, i in mesh_bvh.bvhs {
|
||||
// mesh := car_model.meshes[i]
|
||||
|
||||
// if i == g_mem.preview_bvh {
|
||||
// bvh.debug_draw_bvh_bounds(
|
||||
// &blas,
|
||||
// bvh.bvh_mesh_from_rl_mesh(mesh),
|
||||
// 0,
|
||||
// g_mem.preview_node,
|
||||
// )
|
||||
// }
|
||||
|
||||
// vertices := (cast([^]rl.Vector3)mesh.vertices)[:mesh.vertexCount]
|
||||
// indices := mesh.indices[:mesh.triangleCount * 3]
|
||||
// if bvh.traverse_bvh_ray_mesh(
|
||||
// &blas,
|
||||
// bvh.Mesh{vertices = vertices, indices = indices},
|
||||
// ray,
|
||||
// &mesh_col,
|
||||
// ) {
|
||||
// hit_mesh_idx = i
|
||||
// }
|
||||
// }
|
||||
|
||||
// if mesh_col.hit {
|
||||
// rl.DrawSphereWires(ray.origin + ray.dir * mesh_col.t, 0.1, 8, 8, rl.RED)
|
||||
// }
|
||||
// }
|
||||
|
||||
if !g_mem.editor {
|
||||
car_matrix := rl.QuaternionToMatrix(car_body.q)
|
||||
car_model.transform = car_matrix
|
||||
|
||||
rl.DrawModel(car_model, physics.body_get_shape_pos(car_body), 1, rl.WHITE)
|
||||
} else {
|
||||
// rl.DrawModel(car_model, 0, 1, rl.WHITE)
|
||||
if !runtime_world.pause {
|
||||
physics.simulate(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
SOLVER_CONFIG,
|
||||
dt,
|
||||
commit = runtime_world.step_simulation,
|
||||
step_mode = g_mem.physics_pause ? physics.Step_Mode.Single : physics.Step_Mode.Accumulated_Time,
|
||||
)
|
||||
}
|
||||
|
||||
// rl.DrawModel(car_model, physics.body_get_shape_pos(car_body), 1, rl.WHITE)
|
||||
}
|
||||
|
||||
{
|
||||
@ -739,7 +757,7 @@ draw :: proc() {
|
||||
)
|
||||
}
|
||||
} else {
|
||||
car := physics.get_body(&world.physics_scene, runtime_world.car_handle)
|
||||
car := physics.get_body(sim_state, runtime_world.car_handle)
|
||||
rl.DrawText(
|
||||
fmt.ctprintf(
|
||||
"p: %v\nv: %v\nw: %v\ng: %v",
|
||||
@ -957,6 +975,8 @@ game_memory_size :: proc() -> int {
|
||||
@(export)
|
||||
game_hot_reloaded :: proc(mem: rawptr) {
|
||||
g_mem = (^Game_Memory)(mem)
|
||||
|
||||
g_mem.runtime_world.orbit_camera.distance = 10
|
||||
}
|
||||
|
||||
@(export)
|
||||
|
||||
@ -38,8 +38,10 @@ draw_debug_shape :: proc(
|
||||
draw_debug_scene :: proc(scene: ^Scene) {
|
||||
tracy.Zone()
|
||||
|
||||
for _, i in scene.bodies {
|
||||
body := &scene.bodies_slice[i]
|
||||
sim_state := get_next_sim_state(scene)
|
||||
|
||||
for _, i in sim_state.bodies {
|
||||
body := &sim_state.bodies_slice[i]
|
||||
if body.alive {
|
||||
pos := body.x
|
||||
|
||||
@ -61,10 +63,10 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
||||
}
|
||||
}
|
||||
|
||||
for _, i in scene.suspension_constraints {
|
||||
wheel := &scene.suspension_constraints_slice[i]
|
||||
for _, i in sim_state.suspension_constraints {
|
||||
wheel := &sim_state.suspension_constraints_slice[i]
|
||||
if wheel.alive {
|
||||
body := get_body(scene, wheel.body)
|
||||
body := get_body(sim_state, wheel.body)
|
||||
t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest
|
||||
|
||||
pos := body.x
|
||||
@ -103,14 +105,14 @@ draw_debug_scene :: proc(scene: ^Scene) {
|
||||
}
|
||||
}
|
||||
|
||||
if true {
|
||||
for &contact, contact_idx in scene.contact_pairs[:scene.contact_pairs_len] {
|
||||
if false {
|
||||
for &contact, contact_idx in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
|
||||
points_a := contact.manifold.points_a
|
||||
points_b := contact.manifold.points_b
|
||||
points_a_slice, points_b_slice :=
|
||||
points_a[:contact.manifold.points_len], points_b[:contact.manifold.points_len]
|
||||
debug_transform_points_local_to_world(get_body(scene, contact.a), points_a_slice)
|
||||
debug_transform_points_local_to_world(get_body(scene, contact.b), points_b_slice)
|
||||
debug_transform_points_local_to_world(get_body(sim_state, contact.a), points_a_slice)
|
||||
debug_transform_points_local_to_world(get_body(sim_state, contact.b), points_b_slice)
|
||||
debug_draw_manifold_points(
|
||||
-contact.manifold.normal,
|
||||
points_a_slice,
|
||||
|
||||
@ -99,12 +99,12 @@ immediate_body :: proc(
|
||||
state.num_referenced_bodies += 1
|
||||
}
|
||||
handle = body.handle
|
||||
update_body_from_config(get_body(scene, handle), config)
|
||||
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(scene, new_body)
|
||||
handle = add_body(get_sim_state(scene), new_body)
|
||||
state.immedate_bodies[id] = {
|
||||
handle = handle,
|
||||
last_ref = state.simulation_frame,
|
||||
@ -131,14 +131,17 @@ immediate_suspension_constraint :: proc(
|
||||
handle = constraint.handle
|
||||
} else {
|
||||
state.num_referenced_suspension_constraints += 1
|
||||
handle = add_suspension_constraint(scene, {})
|
||||
handle = add_suspension_constraint(get_sim_state(scene), {})
|
||||
state.immediate_suspension_constraints[id] = {
|
||||
handle = handle,
|
||||
last_ref = state.simulation_frame,
|
||||
}
|
||||
}
|
||||
|
||||
update_suspension_constraint_from_config(get_suspension_constraint(scene, handle), config)
|
||||
update_suspension_constraint_from_config(
|
||||
get_suspension_constraint(get_sim_state(scene), handle),
|
||||
config,
|
||||
)
|
||||
|
||||
return
|
||||
}
|
||||
@ -172,7 +175,7 @@ prune_immediate_bodies :: proc(scene: ^Scene, state: ^Solver_State) {
|
||||
for k in bodies_to_remove {
|
||||
handle := state.immedate_bodies[k].handle
|
||||
delete_key(&state.immedate_bodies, k)
|
||||
remove_body(scene, handle)
|
||||
remove_body(get_sim_state(scene), handle)
|
||||
}
|
||||
}
|
||||
|
||||
@ -202,6 +205,6 @@ prune_immediate_suspension_constraints :: proc(scene: ^Scene, state: ^Solver_Sta
|
||||
for k in constraints_to_remove {
|
||||
handle := state.immediate_suspension_constraints[k].handle
|
||||
delete_key(&state.immediate_suspension_constraints, k)
|
||||
remove_suspension_constraint(scene, handle)
|
||||
remove_suspension_constraint(get_sim_state(scene), handle)
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,27 +1,33 @@
|
||||
package physics
|
||||
|
||||
import "collision"
|
||||
import "game:halfedge"
|
||||
import rl "vendor:raylib"
|
||||
|
||||
MAX_CONTACTS :: 1024
|
||||
|
||||
Matrix3 :: # row_major matrix[3, 3]f32
|
||||
|
||||
Scene :: struct {
|
||||
Sim_State :: struct {
|
||||
bodies: #soa[dynamic]Body,
|
||||
suspension_constraints: #soa[dynamic]Suspension_Constraint,
|
||||
first_free_body_plus_one: i32,
|
||||
first_free_suspension_constraint_plus_one: i32,
|
||||
|
||||
// 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,
|
||||
@ -120,52 +126,107 @@ _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(scene: ^Scene, handle: Body_Handle) -> Body_Ptr {
|
||||
get_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) -> Body_Ptr {
|
||||
index := int(handle) - 1
|
||||
if index < 0 || index >= len(scene.bodies_slice) {
|
||||
if index < 0 || index >= len(sim_state.bodies_slice) {
|
||||
return &_invalid_body_slice[0]
|
||||
}
|
||||
|
||||
return &scene.bodies_slice[index]
|
||||
return &sim_state.bodies_slice[index]
|
||||
}
|
||||
|
||||
add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle {
|
||||
body_copy := body
|
||||
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 scene.first_free_body_plus_one > 0 {
|
||||
index := scene.first_free_body_plus_one
|
||||
new_body := get_body(scene, Body_Handle(index))
|
||||
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
|
||||
scene.first_free_body_plus_one = next_plus_one
|
||||
sim_state.first_free_body_plus_one = next_plus_one
|
||||
|
||||
return Body_Handle(index)
|
||||
}
|
||||
|
||||
append_soa(&scene.bodies, body_copy)
|
||||
index := len(scene.bodies)
|
||||
append_soa(&sim_state.bodies, body_copy)
|
||||
index := len(sim_state.bodies)
|
||||
|
||||
scene.bodies_slice = scene.bodies[:]
|
||||
sim_state.bodies_slice = sim_state.bodies[:]
|
||||
|
||||
return Body_Handle(index)
|
||||
}
|
||||
|
||||
remove_body :: proc(scene: ^Scene, handle: Body_Handle) {
|
||||
remove_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) {
|
||||
if int(handle) > 1 {
|
||||
body := get_body(scene, handle)
|
||||
body := get_body(sim_state, handle)
|
||||
|
||||
body.alive = false
|
||||
body.next_plus_one = scene.first_free_body_plus_one
|
||||
scene.first_free_body_plus_one = i32(handle)
|
||||
|
||||
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(
|
||||
scene: ^Scene,
|
||||
sim_state: ^Sim_State,
|
||||
handle: Suspension_Constraint_Handle,
|
||||
) -> Suspension_Constraint_Ptr {
|
||||
if !is_handle_valid(handle) {
|
||||
@ -173,11 +234,11 @@ get_suspension_constraint :: proc(
|
||||
}
|
||||
|
||||
index := int(handle) - 1
|
||||
return &scene.suspension_constraints_slice[index]
|
||||
return &sim_state.suspension_constraints_slice[index]
|
||||
}
|
||||
|
||||
add_suspension_constraint :: proc(
|
||||
scene: ^Scene,
|
||||
sim_state: ^Sim_State,
|
||||
constraint: Suspension_Constraint,
|
||||
) -> Suspension_Constraint_Handle {
|
||||
copy := constraint
|
||||
@ -185,36 +246,42 @@ add_suspension_constraint :: proc(
|
||||
copy.alive = true
|
||||
copy.next_plus_one = 0
|
||||
|
||||
if scene.first_free_suspension_constraint_plus_one > 0 {
|
||||
index := scene.first_free_suspension_constraint_plus_one
|
||||
new_constraint := get_suspension_constraint(scene, Suspension_Constraint_Handle(index))
|
||||
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
|
||||
scene.first_free_suspension_constraint_plus_one = next_plus_one
|
||||
sim_state.first_free_suspension_constraint_plus_one = next_plus_one
|
||||
return Suspension_Constraint_Handle(index)
|
||||
}
|
||||
|
||||
append_soa(&scene.suspension_constraints, copy)
|
||||
scene.suspension_constraints_slice = scene.suspension_constraints[:]
|
||||
index := len(scene.suspension_constraints)
|
||||
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(scene: ^Scene, handle: Suspension_Constraint_Handle) {
|
||||
remove_suspension_constraint :: proc(sim_state: ^Sim_State, handle: Suspension_Constraint_Handle) {
|
||||
if is_handle_valid(handle) {
|
||||
constraint := get_suspension_constraint(scene, handle)
|
||||
constraint := get_suspension_constraint(sim_state, handle)
|
||||
|
||||
constraint.alive = false
|
||||
constraint.next_plus_one = scene.first_free_suspension_constraint_plus_one
|
||||
scene.first_free_suspension_constraint_plus_one = i32(handle)
|
||||
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(scene: ^Scene) -> i32 {
|
||||
return scene.first_free_body_plus_one - 1
|
||||
_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) {
|
||||
delete_soa(scene.bodies)
|
||||
delete_soa(scene.suspension_constraints)
|
||||
for &sim_state in scene.simulation_states {
|
||||
destry_sim_state(&sim_state)
|
||||
}
|
||||
}
|
||||
|
||||
@ -43,22 +43,68 @@ Immedate_State :: struct($T: typeid) {
|
||||
|
||||
MAX_STEPS :: 10
|
||||
|
||||
// Copy current state to next
|
||||
prepare_next_sim_state :: proc(scene: ^Scene) {
|
||||
current_state := get_sim_state(scene)
|
||||
next_state := get_next_sim_state(scene)
|
||||
|
||||
next_state.first_free_body_plus_one = current_state.first_free_body_plus_one
|
||||
next_state.first_free_suspension_constraint_plus_one =
|
||||
current_state.first_free_suspension_constraint_plus_one
|
||||
|
||||
resize(&next_state.bodies, len(current_state.bodies))
|
||||
resize(&next_state.suspension_constraints, len(current_state.suspension_constraints))
|
||||
|
||||
next_state.bodies_slice = next_state.bodies[:]
|
||||
next_state.suspension_constraints_slice = next_state.suspension_constraints[:]
|
||||
|
||||
for i in 0 ..< len(next_state.bodies) {
|
||||
next_state.bodies[i] = current_state.bodies[i]
|
||||
}
|
||||
for i in 0 ..< len(next_state.suspension_constraints) {
|
||||
next_state.suspension_constraints[i] = current_state.suspension_constraints[i]
|
||||
}
|
||||
}
|
||||
|
||||
Step_Mode :: enum {
|
||||
Accumulated_Time,
|
||||
Single,
|
||||
}
|
||||
|
||||
// Outer simulation loop for fixed timestepping
|
||||
simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt: f32) {
|
||||
simulate :: proc(
|
||||
scene: ^Scene,
|
||||
state: ^Solver_State,
|
||||
config: Solver_Config,
|
||||
dt: f32,
|
||||
commit := true, // commit = false is a special mode for debugging physics stepping to allow rerunning the same step each frame
|
||||
step_mode := Step_Mode.Accumulated_Time,
|
||||
) {
|
||||
assert(config.timestep > 0)
|
||||
|
||||
prune_immediate(scene, state)
|
||||
|
||||
state.accumulated_time += dt
|
||||
prepare_next_sim_state(scene)
|
||||
|
||||
num_steps := 0
|
||||
for state.accumulated_time >= config.timestep {
|
||||
num_steps += 1
|
||||
state.accumulated_time -= config.timestep
|
||||
switch step_mode {
|
||||
case .Accumulated_Time:
|
||||
state.accumulated_time += dt
|
||||
|
||||
if num_steps < MAX_STEPS {
|
||||
simulate_step(scene, config)
|
||||
num_steps := 0
|
||||
for state.accumulated_time >= config.timestep {
|
||||
num_steps += 1
|
||||
state.accumulated_time -= config.timestep
|
||||
|
||||
if num_steps < MAX_STEPS {
|
||||
simulate_step(get_next_sim_state(scene), config)
|
||||
}
|
||||
}
|
||||
case .Single:
|
||||
simulate_step(get_next_sim_state(scene), config)
|
||||
}
|
||||
|
||||
if commit {
|
||||
flip_sim_state(scene)
|
||||
}
|
||||
|
||||
state.simulation_frame += 1
|
||||
@ -90,12 +136,12 @@ Contact_Pair :: struct {
|
||||
applied_normal_correction: [4]f32,
|
||||
}
|
||||
|
||||
simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
|
||||
tracy.Zone()
|
||||
|
||||
body_states := make([]Body_Sim_State, len(scene.bodies), context.temp_allocator)
|
||||
body_states := make([]Body_Sim_State, len(sim_state.bodies), context.temp_allocator)
|
||||
|
||||
scene.contact_pairs_len = 0
|
||||
sim_state.contact_pairs_len = 0
|
||||
|
||||
substeps := config.substreps_minus_one + 1
|
||||
|
||||
@ -104,7 +150,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
|
||||
for _ in 0 ..< substeps {
|
||||
// Integrate positions and rotations
|
||||
for &body, i in scene.bodies {
|
||||
for &body, i in sim_state.bodies {
|
||||
if body.alive {
|
||||
body_states[i].prev_x = body.x
|
||||
body_states[i].prev_v = body.v
|
||||
@ -139,11 +185,11 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
{
|
||||
tracy.ZoneN("simulate_step::collisions")
|
||||
|
||||
for _, i in scene.bodies {
|
||||
body := &scene.bodies_slice[i]
|
||||
for _, i in sim_state.bodies {
|
||||
body := &sim_state.bodies_slice[i]
|
||||
if body.alive {
|
||||
for _, j in scene.bodies {
|
||||
body2 := &scene.bodies_slice[j]
|
||||
for _, j in sim_state.bodies {
|
||||
body2 := &sim_state.bodies_slice[j]
|
||||
|
||||
if i != j &&
|
||||
body2.alive &&
|
||||
@ -156,7 +202,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
|
||||
|
||||
if collision {
|
||||
contact_pair := &scene.contact_pairs[scene.contact_pairs_len]
|
||||
contact_pair := &sim_state.contact_pairs[sim_state.contact_pairs_len]
|
||||
contact_pair^ = Contact_Pair {
|
||||
a = Body_Handle(i + 1),
|
||||
b = Body_Handle(j + 1),
|
||||
@ -166,7 +212,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
prev_q_b = body2.q,
|
||||
manifold = raw_manifold,
|
||||
}
|
||||
scene.contact_pairs_len += 1
|
||||
sim_state.contact_pairs_len += 1
|
||||
manifold := &contact_pair.manifold
|
||||
|
||||
// Convert manifold contact from world to local space
|
||||
@ -224,14 +270,14 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
|
||||
if false {
|
||||
context = context
|
||||
context.user_ptr = scene
|
||||
context.user_ptr = sim_state
|
||||
slice.sort_by(
|
||||
scene.contact_pairs[:scene.contact_pairs_len],
|
||||
sim_state.contact_pairs[:sim_state.contact_pairs_len],
|
||||
proc(c1, c2: Contact_Pair) -> bool {
|
||||
scene := cast(^Scene)context.user_ptr
|
||||
sim_state := cast(^Sim_State)context.user_ptr
|
||||
|
||||
find_min_contact_y :: proc(
|
||||
scene: ^Scene,
|
||||
scene: ^Sim_State,
|
||||
c: Contact_Pair,
|
||||
) -> (
|
||||
min_contact_y: f32,
|
||||
@ -247,17 +293,18 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
return
|
||||
}
|
||||
|
||||
min_y1 := find_min_contact_y(scene, c1)
|
||||
min_y2 := find_min_contact_y(scene, c2)
|
||||
min_y1 := find_min_contact_y(sim_state, c1)
|
||||
min_y2 := find_min_contact_y(sim_state, c2)
|
||||
|
||||
return min_y1 > min_y2
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
for &contact_pair in scene.contact_pairs[:scene.contact_pairs_len] {
|
||||
for &contact_pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
|
||||
manifold := contact_pair.manifold
|
||||
body, body2 := get_body(scene, contact_pair.a), get_body(scene, contact_pair.b)
|
||||
body, body2 :=
|
||||
get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
|
||||
i, j := int(contact_pair.a) - 1, int(contact_pair.b) - 1
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
@ -292,13 +339,14 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
body,
|
||||
body2,
|
||||
0,
|
||||
-tangent_diff_len / max(f32(contact_pair.applied_corrections) * 0.5, 1),
|
||||
-tangent_diff_len /
|
||||
max(f32(contact_pair.applied_corrections) * 0.5, 1),
|
||||
-tangent_diff_normalized,
|
||||
p1,
|
||||
p2,
|
||||
)
|
||||
|
||||
STATIC_FRICTION :: 0.6
|
||||
STATIC_FRICTION :: 0.5
|
||||
if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm {
|
||||
contact_pair.applied_static_friction[point_idx] = true
|
||||
contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent
|
||||
@ -315,9 +363,9 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
{
|
||||
tracy.ZoneN("simulate_step::suspension_constraints")
|
||||
|
||||
for &v in scene.suspension_constraints {
|
||||
for &v in sim_state.suspension_constraints {
|
||||
if v.alive {
|
||||
body := get_body(scene, v.body)
|
||||
body := get_body(sim_state, v.body)
|
||||
|
||||
pos := body_local_to_world(body, v.rel_pos)
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
@ -345,45 +393,39 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
}
|
||||
}
|
||||
|
||||
solve_velocities(scene, body_states, inv_dt)
|
||||
solve_velocities(sim_state, body_states, inv_dt)
|
||||
|
||||
// Restituion
|
||||
{
|
||||
tracy.ZoneN("simulate_step::restitution")
|
||||
|
||||
for &pair in scene.contact_pairs[:scene.contact_pairs_len] {
|
||||
for &pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
|
||||
i, j := int(pair.a) - 1, int(pair.b) - 1
|
||||
manifold := &pair.manifold
|
||||
|
||||
body, body2 := get_body(scene, pair.a), get_body(scene, pair.b)
|
||||
body, body2 := get_body(sim_state, pair.a), get_body(sim_state, pair.b)
|
||||
s1, s2 := body_states[i], body_states[j]
|
||||
prev_q1, prev_q2 := s1.prev_q, s2.prev_q
|
||||
|
||||
for point_idx in 0..<manifold.points_len {
|
||||
if pair.lambda_normal == 0 {
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
if pair.lambda_normal[point_idx] == 0 {
|
||||
continue
|
||||
}
|
||||
prev_r1 :=
|
||||
lg.quaternion_mul_vector3(
|
||||
prev_q1,
|
||||
manifold.points_a[point_idx],
|
||||
)
|
||||
prev_r2 :=
|
||||
lg.quaternion_mul_vector3(
|
||||
prev_q2,
|
||||
manifold.points_b[point_idx],
|
||||
)
|
||||
prev_r1 := lg.quaternion_mul_vector3(prev_q1, manifold.points_a[point_idx])
|
||||
prev_r2 := lg.quaternion_mul_vector3(prev_q2, manifold.points_b[point_idx])
|
||||
|
||||
r1 := lg.quaternion_mul_vector3(body.q, manifold.points_a[point_idx])
|
||||
r2 := lg.quaternion_mul_vector3(body2.q, manifold.points_b[point_idx])
|
||||
|
||||
prev_v := (s1.prev_v + lg.cross(s1.prev_w, prev_r1)) - (s2.prev_v + lg.cross(s2.prev_w, prev_r2))
|
||||
prev_v :=
|
||||
(s1.prev_v + lg.cross(s1.prev_w, prev_r1)) -
|
||||
(s2.prev_v + lg.cross(s2.prev_w, prev_r2))
|
||||
v := (body.v + lg.cross(body.w, r1)) - (body2.v + lg.cross(body2.w, r2))
|
||||
|
||||
prev_v_normal := lg.dot(prev_v, manifold.normal)
|
||||
v_normal := lg.dot(v, manifold.normal)
|
||||
|
||||
RESTITUTION :: 0.5
|
||||
RESTITUTION :: 0
|
||||
|
||||
restitution := f32(RESTITUTION)
|
||||
|
||||
@ -413,10 +455,10 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
if true {
|
||||
tracy.ZoneN("simulate_step::dynamic_friction")
|
||||
|
||||
for &pair in scene.contact_pairs[:scene.contact_pairs_len] {
|
||||
for &pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] {
|
||||
manifold := &pair.manifold
|
||||
body1 := get_body(scene, pair.a)
|
||||
body2 := get_body(scene, pair.b)
|
||||
body1 := get_body(sim_state, pair.a)
|
||||
body2 := get_body(sim_state, pair.b)
|
||||
|
||||
for point_idx in 0 ..< pair.manifold.points_len {
|
||||
if pair.applied_static_friction[point_idx] {
|
||||
@ -470,11 +512,11 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
}
|
||||
|
||||
// Solve suspension velocity
|
||||
for _, i in scene.suspension_constraints {
|
||||
v := &scene.suspension_constraints_slice[i]
|
||||
for _, i in sim_state.suspension_constraints {
|
||||
v := &sim_state.suspension_constraints_slice[i]
|
||||
if v.alive {
|
||||
body_idx := int(v.body) - 1
|
||||
body := get_body(scene, v.body)
|
||||
body := get_body(sim_state, v.body)
|
||||
|
||||
if body.alive && v.hit {
|
||||
prev_x, prev_q := body_states[body_idx].prev_x, body_states[body_idx].prev_q
|
||||
@ -540,7 +582,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) {
|
||||
}
|
||||
}
|
||||
|
||||
solve_velocities :: proc(scene: ^Scene, body_states: []Body_Sim_State, inv_dt: f32) {
|
||||
solve_velocities :: proc(scene: ^Sim_State, body_states: []Body_Sim_State, inv_dt: f32) {
|
||||
// Compute new linear and angular velocities
|
||||
for _, i in scene.bodies_slice {
|
||||
body := &scene.bodies_slice[i]
|
||||
@ -674,6 +716,9 @@ multiply_inv_intertia :: proc(body: Body_Ptr, vec: rl.Vector3) -> (result: rl.Ve
|
||||
}
|
||||
|
||||
apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) {
|
||||
// rl.DrawSphereWires(pos, 0.5, 4, 4, rl.BLUE)
|
||||
// rl.DrawLine3D(pos, pos + corr, rl.BLUE)
|
||||
|
||||
body.x += corr * body.inv_mass
|
||||
|
||||
q := body.q
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user