Start implementing optimized substepping (collision detection only once per frame)
This commit is contained in:
parent
c3ac6c2db1
commit
ccd987aeae
@ -75,7 +75,7 @@ Car :: struct {
|
||||
SOLVER_CONFIG :: physics.Solver_Config {
|
||||
timestep = 1.0 / 60,
|
||||
gravity = rl.Vector3{0, -9.8, 0},
|
||||
substreps_minus_one = 4 - 1,
|
||||
substreps_minus_one = 8 - 1,
|
||||
}
|
||||
|
||||
Game_Memory :: struct {
|
||||
@ -277,8 +277,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
|
||||
)
|
||||
|
||||
if true {
|
||||
for x in 0 ..< 20 {
|
||||
for y in 0 ..< 10 {
|
||||
for x in 0 ..< 40 {
|
||||
for y in 0 ..< 5 {
|
||||
physics.immediate_body(
|
||||
&world.physics_scene,
|
||||
&runtime_world.solver_state,
|
||||
|
||||
@ -66,8 +66,8 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli
|
||||
if edge_separation > 0 {
|
||||
return
|
||||
}
|
||||
biased_face_a_separation := face_query_a.separation
|
||||
biased_face_b_separation := face_query_b.separation
|
||||
biased_face_a_separation := face_query_a.separation + 0.3
|
||||
biased_face_b_separation := face_query_b.separation + 0.2
|
||||
biased_edge_separation := edge_separation
|
||||
|
||||
is_face_a_contact := biased_face_a_separation >= biased_edge_separation
|
||||
@ -418,7 +418,7 @@ create_face_contact_manifold :: proc(
|
||||
for i in 0 ..< vert_count {
|
||||
d := signed_distance_plane(src_polygon[i], ref_plane)
|
||||
|
||||
if d <= 1e-03 {
|
||||
if d <= 1e-02 {
|
||||
clipped_polygon[j] = src_polygon[i] - d * ref_plane.normal
|
||||
j += 1
|
||||
}
|
||||
|
||||
@ -103,7 +103,7 @@ body_get_convex_shape_world :: proc(
|
||||
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)
|
||||
mesh = halfedge.copy_mesh(mesh, allocator)
|
||||
}
|
||||
|
||||
transform :=
|
||||
|
||||
@ -32,6 +32,8 @@ Sim_State :: struct {
|
||||
|
||||
Scene :: struct {
|
||||
simulation_states: []Sim_State,
|
||||
// Speculative prediction state to find collisions
|
||||
scratch_sim_state: Sim_State,
|
||||
simulation_state_index: i32,
|
||||
}
|
||||
|
||||
@ -50,6 +52,10 @@ Body :: struct {
|
||||
q: Quat,
|
||||
// Angular vel (omega)
|
||||
w: Vec3,
|
||||
prev_x: Vec3,
|
||||
prev_v: Vec3,
|
||||
prev_q: Quat,
|
||||
prev_w: Vec3,
|
||||
// Mass
|
||||
inv_mass: f32,
|
||||
// Moment of inertia
|
||||
@ -388,5 +394,6 @@ destroy_physics_scene :: proc(scene: ^Scene) {
|
||||
for &sim_state in scene.simulation_states {
|
||||
destry_sim_state(&sim_state)
|
||||
}
|
||||
destry_sim_state(&scene.scratch_sim_state)
|
||||
delete(scene.simulation_states)
|
||||
}
|
||||
|
||||
@ -2,9 +2,11 @@ package physics
|
||||
|
||||
import "bvh"
|
||||
import "collision"
|
||||
import "core:container/bit_array"
|
||||
import "core:fmt"
|
||||
import "core:math"
|
||||
import lg "core:math/linalg"
|
||||
import "core:math/rand"
|
||||
import "core:slice"
|
||||
import "libs:tracy"
|
||||
|
||||
@ -46,32 +48,28 @@ MAX_STEPS :: 10
|
||||
|
||||
// TODO: move into scene.odin
|
||||
// Copy current state to next
|
||||
prepare_next_sim_state :: proc(scene: ^Scene) {
|
||||
sim_state_copy :: proc(dst: ^Sim_State, src: ^Sim_State) {
|
||||
tracy.Zone()
|
||||
current_state := get_sim_state(scene)
|
||||
next_state := get_next_sim_state(scene)
|
||||
convex_container_reconcile(&src.convex_container)
|
||||
|
||||
convex_container_reconcile(¤t_state.convex_container)
|
||||
dst.num_bodies = src.num_bodies
|
||||
dst.first_free_body_plus_one = src.first_free_body_plus_one
|
||||
dst.first_free_suspension_constraint_plus_one = src.first_free_suspension_constraint_plus_one
|
||||
|
||||
next_state.num_bodies = current_state.num_bodies
|
||||
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(&dst.bodies, len(src.bodies))
|
||||
resize(&dst.suspension_constraints, len(src.suspension_constraints))
|
||||
|
||||
resize(&next_state.bodies, len(current_state.bodies))
|
||||
resize(&next_state.suspension_constraints, len(current_state.suspension_constraints))
|
||||
dst.bodies_slice = dst.bodies[:]
|
||||
dst.suspension_constraints_slice = dst.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(dst.bodies) {
|
||||
dst.bodies[i] = src.bodies[i]
|
||||
}
|
||||
for i in 0 ..< len(next_state.suspension_constraints) {
|
||||
next_state.suspension_constraints[i] = current_state.suspension_constraints[i]
|
||||
for i in 0 ..< len(dst.suspension_constraints) {
|
||||
dst.suspension_constraints[i] = src.suspension_constraints[i]
|
||||
}
|
||||
|
||||
convex_container_copy(&next_state.convex_container, current_state.convex_container)
|
||||
convex_container_copy(&dst.convex_container, src.convex_container)
|
||||
}
|
||||
|
||||
Step_Mode :: enum {
|
||||
@ -95,7 +93,8 @@ simulate :: proc(
|
||||
|
||||
prune_immediate(scene, state)
|
||||
|
||||
prepare_next_sim_state(scene)
|
||||
sim_state_copy(get_next_sim_state(scene), get_sim_state(scene))
|
||||
sim_state_copy(&scene.scratch_sim_state, get_sim_state(scene))
|
||||
|
||||
sim_state := get_next_sim_state(scene)
|
||||
|
||||
@ -171,6 +170,10 @@ simulate :: proc(
|
||||
}
|
||||
}
|
||||
|
||||
resize_soa(&sim_state.contact_pairs, 0)
|
||||
predict_positions(&scene.scratch_sim_state, config, config.timestep)
|
||||
find_collisions(&scene.scratch_sim_state, &sim_state.contact_pairs, potential_pairs)
|
||||
|
||||
// bvh.debug_draw_bvh_bounds(&sim_state_bvh, body_aabbs, 0)
|
||||
|
||||
num_steps := 0
|
||||
@ -183,11 +186,11 @@ simulate :: proc(
|
||||
state.accumulated_time -= config.timestep
|
||||
|
||||
if num_steps < MAX_STEPS {
|
||||
simulate_step(sim_state, config, potential_pairs)
|
||||
simulate_step(sim_state, config)
|
||||
}
|
||||
}
|
||||
case .Single:
|
||||
simulate_step(get_next_sim_state(scene), config, potential_pairs)
|
||||
simulate_step(get_next_sim_state(scene), config)
|
||||
num_steps += 1
|
||||
}
|
||||
|
||||
@ -224,58 +227,22 @@ Contact_Pair :: struct {
|
||||
applied_normal_correction: [4]f32,
|
||||
}
|
||||
|
||||
simulate_step :: proc(
|
||||
find_collisions :: proc(
|
||||
sim_state: ^Sim_State,
|
||||
config: Solver_Config,
|
||||
contact_pairs: ^#soa[dynamic]Contact_Pair,
|
||||
potential_pairs: []Potential_Pair,
|
||||
) {
|
||||
tracy.Zone()
|
||||
|
||||
body_states := make_soa(#soa[]Body_Sim_State, len(sim_state.bodies), context.temp_allocator)
|
||||
|
||||
resize_soa(&sim_state.contact_pairs, 0)
|
||||
|
||||
substeps := config.substreps_minus_one + 1
|
||||
|
||||
dt := config.timestep / f32(substeps)
|
||||
inv_dt := 1.0 / dt
|
||||
|
||||
for _ in 0 ..< substeps {
|
||||
// Integrate positions and rotations
|
||||
for &body, i in sim_state.bodies {
|
||||
if body.alive {
|
||||
body_states[i].prev_x = body.x
|
||||
body_states[i].prev_v = body.v
|
||||
body_states[i].prev_w = body.w
|
||||
body_states[i].prev_q = body.q
|
||||
|
||||
body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO
|
||||
body.x += body.v * dt
|
||||
|
||||
// NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
|
||||
q := body.q
|
||||
|
||||
w := body.w
|
||||
delta_rot := quaternion(x = w.x, y = w.y, z = w.z, w = 0)
|
||||
delta_rot = delta_rot * q
|
||||
q.x += 0.5 * dt * delta_rot.x
|
||||
q.y += 0.5 * dt * delta_rot.y
|
||||
q.z += 0.5 * dt * delta_rot.z
|
||||
q.w += 0.5 * dt * delta_rot.w
|
||||
q = lg.normalize0(q)
|
||||
|
||||
body.q = q
|
||||
graph_color_bitmask: [4]bit_array.Bit_Array
|
||||
for i in 0 ..< len(graph_color_bitmask) {
|
||||
bit_array.init(
|
||||
&graph_color_bitmask[i],
|
||||
len(sim_state.bodies_slice),
|
||||
0,
|
||||
context.temp_allocator,
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Body_Pair :: struct {
|
||||
a, b: int,
|
||||
}
|
||||
handled_pairs := make_map(map[Body_Pair]bool, context.temp_allocator)
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::collisions")
|
||||
|
||||
for pair in potential_pairs {
|
||||
i, j := int(pair[0]), int(pair[1])
|
||||
@ -303,9 +270,9 @@ simulate_step :: proc(
|
||||
raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
|
||||
|
||||
if collision {
|
||||
new_contact_idx := len(sim_state.contact_pairs)
|
||||
resize_soa(&sim_state.contact_pairs, new_contact_idx + 1)
|
||||
contact_pair := &sim_state.contact_pairs[new_contact_idx]
|
||||
new_contact_idx := len(contact_pairs)
|
||||
resize_soa(contact_pairs, new_contact_idx + 1)
|
||||
contact_pair := &contact_pairs[new_contact_idx]
|
||||
contact_pair^ = Contact_Pair {
|
||||
a = Body_Handle(i + 1),
|
||||
b = Body_Handle(j + 1),
|
||||
@ -328,6 +295,87 @@ simulate_step :: proc(
|
||||
manifold.points_b[point_idx],
|
||||
)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
predict_positions :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32) {
|
||||
// Integrate positions and rotations
|
||||
for &body in sim_state.bodies {
|
||||
if body.alive {
|
||||
body.prev_x = body.x
|
||||
body.prev_v = body.v
|
||||
body.prev_w = body.w
|
||||
body.prev_q = body.q
|
||||
|
||||
body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO
|
||||
body.x += body.v * dt
|
||||
|
||||
// NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/
|
||||
q := body.q
|
||||
|
||||
w := body.w
|
||||
delta_rot := quaternion(x = w.x, y = w.y, z = w.z, w = 0)
|
||||
delta_rot = delta_rot * q
|
||||
q.x += 0.5 * dt * delta_rot.x
|
||||
q.y += 0.5 * dt * delta_rot.y
|
||||
q.z += 0.5 * dt * delta_rot.z
|
||||
q.w += 0.5 * dt * delta_rot.w
|
||||
q = lg.normalize0(q)
|
||||
|
||||
body.q = q
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) {
|
||||
tracy.Zone()
|
||||
|
||||
substeps := config.substreps_minus_one + 1
|
||||
|
||||
dt := config.timestep / f32(substeps)
|
||||
inv_dt := 1.0 / dt
|
||||
|
||||
for _ in 0 ..< substeps {
|
||||
predict_positions(sim_state, config, dt)
|
||||
|
||||
Body_Pair :: struct {
|
||||
a, b: int,
|
||||
}
|
||||
|
||||
// Shuffle contacts
|
||||
{
|
||||
num_contacts := len(sim_state.contact_pairs)
|
||||
for i in 0 ..< num_contacts {
|
||||
j := rand.int_max(num_contacts)
|
||||
|
||||
if i != j {
|
||||
tmp := sim_state.contact_pairs[i]
|
||||
sim_state.contact_pairs[i] = sim_state.contact_pairs[j]
|
||||
sim_state.contact_pairs[j] = tmp
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::solve_collisions")
|
||||
for i in 0 ..< len(sim_state.contact_pairs) {
|
||||
contact_pair := &sim_state.contact_pairs[i]
|
||||
body, body2 :=
|
||||
get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
|
||||
|
||||
contact_pair^ = Contact_Pair {
|
||||
a = contact_pair.a,
|
||||
b = contact_pair.b,
|
||||
prev_x_a = body.x,
|
||||
prev_x_b = body2.x,
|
||||
prev_q_a = body.q,
|
||||
prev_q_b = body2.q,
|
||||
manifold = contact_pair.manifold,
|
||||
}
|
||||
|
||||
manifold := &contact_pair.manifold
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
p1, p2 := manifold.points_a[point_idx], manifold.points_b[point_idx]
|
||||
p1, p2 = body_local_to_world(body, p1), body_local_to_world(body2, p2)
|
||||
@ -335,8 +383,6 @@ simulate_step :: proc(
|
||||
p_diff_normal := lg.dot(p2 - p1, manifold.normal)
|
||||
separation := min(p_diff_normal, 0)
|
||||
|
||||
handled_pairs[{a = min(i, j), b = max(i, j)}] = true
|
||||
|
||||
lambda_norm, corr1, corr2, ok := calculate_constraint_params2(
|
||||
dt,
|
||||
body,
|
||||
@ -358,7 +404,6 @@ simulate_step :: proc(
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
tracy.ZoneN("simulate_step::static_friction")
|
||||
@ -400,7 +445,6 @@ simulate_step :: proc(
|
||||
manifold := contact_pair.manifold
|
||||
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 {
|
||||
lambda_norm := contact_pair.lambda_normal[point_idx]
|
||||
@ -408,17 +452,11 @@ simulate_step :: proc(
|
||||
p1 := body_local_to_world(body, manifold.points_a[point_idx])
|
||||
p2 := body_local_to_world(body2, manifold.points_b[point_idx])
|
||||
prev_p1 :=
|
||||
body_states[i].prev_x +
|
||||
lg.quaternion_mul_vector3(
|
||||
body_states[i].prev_q,
|
||||
manifold.points_a[point_idx],
|
||||
)
|
||||
body.prev_x +
|
||||
lg.quaternion_mul_vector3(body.prev_q, manifold.points_a[point_idx])
|
||||
prev_p2 :=
|
||||
body_states[j].prev_x +
|
||||
lg.quaternion_mul_vector3(
|
||||
body_states[j].prev_q,
|
||||
manifold.points_b[point_idx],
|
||||
)
|
||||
body2.prev_x +
|
||||
lg.quaternion_mul_vector3(body2.prev_q, manifold.points_b[point_idx])
|
||||
|
||||
p_diff_tangent := (p1 - prev_p1) - (p2 - prev_p2)
|
||||
p_diff_tangent -= lg.dot(p_diff_tangent, manifold.normal) * manifold.normal
|
||||
@ -493,22 +531,20 @@ simulate_step :: proc(
|
||||
for _, i in sim_state.bodies_slice {
|
||||
body := &sim_state.bodies_slice[i]
|
||||
if body.alive {
|
||||
body_solve_velocity(body, body_states[i].prev_x, body_states[i].prev_q, inv_dt)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Restituion
|
||||
{
|
||||
if true {
|
||||
tracy.ZoneN("simulate_step::restitution")
|
||||
|
||||
for &pair in sim_state.contact_pairs {
|
||||
i, j := int(pair.a) - 1, int(pair.b) - 1
|
||||
manifold := &pair.manifold
|
||||
|
||||
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
|
||||
prev_q1, prev_q2 := body.prev_q, body2.prev_q
|
||||
|
||||
for point_idx in 0 ..< manifold.points_len {
|
||||
if pair.lambda_normal[point_idx] == 0 {
|
||||
@ -521,8 +557,8 @@ simulate_step :: proc(
|
||||
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))
|
||||
(body.prev_v + lg.cross(body.prev_w, prev_r1)) -
|
||||
(body2.prev_v + lg.cross(body2.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)
|
||||
@ -614,22 +650,21 @@ simulate_step :: proc(
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Solve suspension velocity
|
||||
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(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
|
||||
prev_x, prev_q := body.prev_x, body.prev_q
|
||||
|
||||
wheel_world_pos := body_local_to_world(body, v.rel_pos)
|
||||
prev_wheel_world_pos := prev_x + lg.quaternion_mul_vector3(prev_q, v.rel_pos)
|
||||
|
||||
vel_3d := (wheel_world_pos - prev_wheel_world_pos) * inv_dt
|
||||
dir := body_local_to_world_vec(body, v.rel_dir)
|
||||
body_state := body_states[i32(v.body) - 1]
|
||||
|
||||
// Spring damping
|
||||
if true {
|
||||
@ -645,7 +680,7 @@ simulate_step :: proc(
|
||||
pos = wheel_world_pos,
|
||||
)
|
||||
|
||||
body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
|
||||
// Drive forces
|
||||
@ -661,7 +696,7 @@ simulate_step :: proc(
|
||||
-forward,
|
||||
wheel_world_pos,
|
||||
)
|
||||
body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
|
||||
// Lateral friction
|
||||
@ -677,7 +712,7 @@ simulate_step :: proc(
|
||||
v.applied_impulse.x = impulse
|
||||
|
||||
apply_correction(body, corr, v.hit_point)
|
||||
body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
|
||||
body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
0
game_hot_reload.o
Normal file
0
game_hot_reload.o
Normal file
Loading…
x
Reference in New Issue
Block a user