sergeypdev/gutter_runner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Double buffer physics state to allow easy debugging and interpolation

Browse Source 
Fix a bug in restitution
This commit is contained in:
sergeypdev 2025-02-09 02:21:34 +04:00
parent 66f42c3cee
commit bf995882e6
7 changed files with 328 additions and 192 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
build_hot_reload.sh
View File

@ -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

1
game/assets/assets.odin
View File

@ -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
View File

@ -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)

20
game/physics/debug.odin
View File

@ -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,

15
game/physics/immediate.odin
View File

@ -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)
}
}

141
game/physics/scene.odin
View File

@ -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)
}
}

153
game/physics/simulation.odin
View File

@ -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