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

Refactor collision detection to support persistent contacts across frames

Browse Source
This commit is contained in:
sergeypdev 2025-03-09 15:04:13 +04:00
parent cb24365933
commit a2ad9e490a
3 changed files with 129 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
game/physics/debug.odin
View File

@ -109,7 +109,7 @@ draw_debug_scene :: proc(scene: ^Scene) {
}
if false {
for &contact, contact_idx in sim_state.contact_pairs {
for &contact, contact_idx in sim_state.contact_container.contacts {
points_a := contact.manifold.points_a
points_b := contact.manifold.points_b
points_a_slice, points_b_slice :=

31
game/physics/scene.odin
View File

@ -13,6 +13,32 @@ AABB :: struct {
extent: Vec3,
}
Contact_Pair :: [2]i32
make_contact_pair :: proc(body_a: i32, body_b: i32) -> Contact_Pair {
return {min(body_a, body_b), max(body_a, body_b)}
}
Contact_Container :: struct {
// body index pair to contact index
lookup: map[Contact_Pair]i32,
contacts: #soa[dynamic]Contact,
}
contact_container_copy :: proc(dst: ^Contact_Container, src: Contact_Container) {
clear(&dst.lookup)
reserve(&dst.lookup, cap(src.lookup))
resize_soa(&dst.contacts, len(src.contacts))
for k, v in src.lookup {
dst.lookup[k] = v
}
for i in 0 ..< len(src.contacts) {
dst.contacts[i] = src.contacts[i]
}
}
Sim_State :: struct {
bodies: #soa[dynamic]Body,
suspension_constraints: #soa[dynamic]Suspension_Constraint,
@ -26,7 +52,7 @@ Sim_State :: struct {
first_free_suspension_constraint_plus_one: i32,
// Persistent stuff for simulation
contact_pairs: #soa[dynamic]Contact_Pair,
contact_container: Contact_Container,
convex_container: Convex_Container,
}
@ -386,7 +412,8 @@ _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)
delete_soa(sim_state.contact_pairs)
delete_soa(sim_state.contact_container.contacts)
delete_map(sim_state.contact_container.lookup)
convex_container_destroy(&sim_state.convex_container)
}

183
game/physics/simulation.odin
View File

@ -70,6 +70,7 @@ sim_state_copy :: proc(dst: ^Sim_State, src: ^Sim_State) {
dst.suspension_constraints[i] = src.suspension_constraints[i]
}
contact_container_copy(&dst.contact_container, src.contact_container)
convex_container_copy(&dst.convex_container, src.convex_container)
}
@ -78,8 +79,6 @@ Step_Mode :: enum {
Single,
}
Potential_Pair :: [2]u16
// Top Level Acceleration Structure
TLAS :: struct {
bvh_tree: bvh.BVH,
@ -122,9 +121,8 @@ build_tlas :: proc(sim_state: ^Sim_State, config: Solver_Config) -> TLAS {
}
// TODO: free intermediate temp allocs
find_potential_pairs :: proc(sim_state: ^Sim_State, tlas: ^TLAS) -> []Potential_Pair {
find_new_contacts :: proc(sim_state: ^Sim_State, tlas: ^TLAS) {
tracy.Zone()
potential_pairs_map := make(map[Potential_Pair]bool, context.temp_allocator)
for i in 0 ..< len(sim_state.bodies_slice) {
assert(i <= int(max(u16)))
@ -136,34 +134,30 @@ find_potential_pairs :: proc(sim_state: ^Sim_State, tlas: ^TLAS) -> []Potential_
it := bvh.iterator_intersect_leaf(&tlas.bvh_tree, body_aabb)
for leaf_node in bvh.iterator_intersect_leaf_next(&it) {
for i in 0 ..< leaf_node.prim_len {
other_body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + i]
for j in 0 ..< leaf_node.prim_len {
other_body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + j]
prim_aabb := tlas.body_aabbs[other_body_idx]
if body_idx != other_body_idx && bvh.test_aabb_vs_aabb(body_aabb, prim_aabb) {
pair := Potential_Pair {
min(body_idx, other_body_idx),
max(body_idx, other_body_idx),
pair := make_contact_pair(i32(body_idx), i32(other_body_idx))
if body_idx != other_body_idx &&
bvh.test_aabb_vs_aabb(body_aabb, prim_aabb) &&
!(pair in sim_state.contact_container.lookup) {
new_contact_idx := len(sim_state.contact_container.contacts)
resize_soa(&sim_state.contact_container.contacts, new_contact_idx + 1)
contact := &sim_state.contact_container.contacts[new_contact_idx]
contact^ = Contact {
a = Body_Handle(i + 1),
b = Body_Handle(other_body_idx + 1),
}
potential_pairs_map[pair] = true
sim_state.contact_container.lookup[pair] = i32(new_contact_idx)
}
}
}
}
}
potential_pairs := make([]Potential_Pair, len(potential_pairs_map), context.temp_allocator)
{
i := 0
for p in potential_pairs_map {
potential_pairs[i] = p
i += 1
}
}
return potential_pairs
}
// Outer simulation loop for fixed timestepping
@ -216,7 +210,7 @@ GLOBAL_PLANE :: collision.Plane {
dist = 0,
}
Contact_Pair :: struct {
Contact :: struct {
a, b: Body_Handle,
prev_x_a, prev_x_b: Vec3,
prev_q_a, prev_q_b: Quat,
@ -228,11 +222,7 @@ Contact_Pair :: struct {
applied_normal_correction: [4]f32,
}
find_collisions :: proc(
sim_state: ^Sim_State,
contact_pairs: ^#soa[dynamic]Contact_Pair,
potential_pairs: []Potential_Pair,
) {
update_contacts :: proc(sim_state: ^Sim_State) {
tracy.Zone()
graph_color_bitmask: [4]bit_array.Bit_Array
@ -245,14 +235,26 @@ find_collisions :: proc(
)
}
for pair in potential_pairs {
i, j := int(pair[0]), int(pair[1])
for contact_idx in 0 ..< len(sim_state.contact_container.contacts) {
contact := &sim_state.contact_container.contacts[contact_idx]
i, j := i32(contact.a) - 1, i32(contact.b) - 1
body, body2 := &sim_state.bodies_slice[i], &sim_state.bodies_slice[j]
assert(body.alive)
assert(body2.alive)
contact.prev_x_a = body.x
contact.prev_x_b = body2.x
contact.prev_q_a = body.q
contact.prev_q_b = body2.q
contact.manifold = {}
contact.lambda_normal = 0
contact.lambda_tangent = 0
contact.applied_static_friction = false
contact.applied_normal_correction = 0
aabb1, aabb2 := body_get_aabb(body), body_get_aabb(body2)
// TODO: extract common math functions into a sane place
@ -271,19 +273,8 @@ find_collisions :: proc(
raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
if collision {
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),
prev_x_a = body.x,
prev_x_b = body2.x,
prev_q_a = body.q,
prev_q_b = body2.q,
manifold = raw_manifold,
}
manifold := &contact_pair.manifold
manifold := &contact.manifold
manifold^ = raw_manifold
// Convert manifold contact from world to local space
for point_idx in 0 ..< manifold.points_len {
@ -338,21 +329,21 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
{
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)
for i in 0 ..< len(sim_state.contact_container.contacts) {
contact := &sim_state.contact_container.contacts[i]
body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b)
contact_pair^ = Contact_Pair {
a = contact_pair.a,
b = contact_pair.b,
contact^ = Contact {
a = contact.a,
b = contact.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.manifold,
}
manifold := &contact_pair.manifold
manifold := &contact.manifold
for point_idx in 0 ..< manifold.points_len {
p1, p2 := manifold.points_a[point_idx], manifold.points_b[point_idx]
@ -372,9 +363,9 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
p2,
)
if ok {
contact_pair.applied_normal_correction[point_idx] = -separation
contact_pair.applied_corrections += 1
contact_pair.lambda_normal[point_idx] = lambda_norm
contact.applied_normal_correction[point_idx] = -separation
contact.applied_corrections += 1
contact.lambda_normal[point_idx] = lambda_norm
apply_correction(body, corr1, p1)
apply_correction(body2, corr2, p2)
@ -391,12 +382,12 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
context.user_ptr = sim_state
slice.sort_by(
sim_state.contact_pairs[:sim_state.contact_pairs_len],
proc(c1, c2: Contact_Pair) -> bool {
proc(c1, c2: Contact) -> bool {
sim_state := cast(^Sim_State)context.user_ptr
find_min_contact_y :: proc(
scene: ^Sim_State,
c: Contact_Pair,
c: Contact,
) -> (
min_contact_y: f32,
) {
@ -419,12 +410,12 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
)
}
for &contact_pair in sim_state.contact_pairs {
manifold := contact_pair.manifold
body, body2 := get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
for &contact in sim_state.contact_container.contacts {
manifold := contact.manifold
body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b)
for point_idx in 0 ..< manifold.points_len {
lambda_norm := contact_pair.lambda_normal[point_idx]
lambda_norm := contact.lambda_normal[point_idx]
if lambda_norm != 0 {
p1 := body_local_to_world(body, manifold.points_a[point_idx])
p2 := body_local_to_world(body2, manifold.points_b[point_idx])
@ -457,8 +448,8 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
STATIC_FRICTION :: 0
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
contact.applied_static_friction[point_idx] = true
contact.lambda_tangent[point_idx] = delta_lambda_tangent
apply_correction(body, corr1_tangent, p1)
apply_correction(body2, corr2_tangent, p2)
@ -516,15 +507,15 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
if true {
tracy.ZoneN("simulate_step::restitution")
for &pair in sim_state.contact_pairs {
manifold := &pair.manifold
for &contact in sim_state.contact_container.contacts {
manifold := &contact.manifold
body, body2 := get_body(sim_state, pair.a), get_body(sim_state, pair.b)
body, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b)
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 {
if contact.lambda_normal[point_idx] == 0 {
continue
}
prev_r1 := lg.quaternion_mul_vector3(prev_q1, manifold.points_a[point_idx])
@ -571,13 +562,13 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
if true {
tracy.ZoneN("simulate_step::dynamic_friction")
for &pair in sim_state.contact_pairs {
manifold := &pair.manifold
body1 := get_body(sim_state, pair.a)
body2 := get_body(sim_state, pair.b)
for &contact in sim_state.contact_container.contacts {
manifold := &contact.manifold
body1 := get_body(sim_state, contact.a)
body2 := get_body(sim_state, contact.b)
for point_idx in 0 ..< pair.manifold.points_len {
if pair.applied_static_friction[point_idx] || pair.lambda_normal == 0 {
for point_idx in 0 ..< contact.manifold.points_len {
if contact.applied_static_friction[point_idx] || contact.lambda_normal == 0 {
continue
}
p1, p2 :=
@ -608,7 +599,7 @@ xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_
min(
dt *
DYNAMIC_FRICTION *
abs(pair.lambda_normal[point_idx] / (dt * dt)),
abs(contact.lambda_normal[point_idx] / (dt * dt)),
v_tangent_len / w,
)
@ -709,7 +700,7 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d
// for i in 0 ..< len(sim_state.contact_pairs) {
// contact_pair := &sim_state.contact_pairs[i]
//
//
// }
for i in 0 ..< len(sim_state.bodies_slice) {
@ -743,30 +734,54 @@ simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Confi
dt := config.timestep / f32(substeps)
inv_dt := 1.0 / dt
resize_soa(&sim_state.contact_pairs, 0)
tlas := build_tlas(sim_state, config)
potential_pairs := find_potential_pairs(sim_state, &tlas)
find_new_contacts(sim_state, &tlas)
{
tracy.ZoneN("simulate_step::remove_invalid_contacts")
i := 0
for i < len(sim_state.contact_container.contacts) {
contact := sim_state.contact_container.contacts[i]
aabb_a := tlas.body_aabbs[int(contact.a) - 1]
aabb_b := tlas.body_aabbs[int(contact.b) - 1]
if !bvh.test_aabb_vs_aabb(aabb_a, aabb_b) {
removed_pair := make_contact_pair(i32(contact.a) - 1, i32(contact.b) - 1)
delete_key(&sim_state.contact_container.lookup, removed_pair)
unordered_remove_soa(&sim_state.contact_container.contacts, i)
if i < len(sim_state.contact_container.contacts) {
moved_contact := &sim_state.contact_container.contacts[i]
moved_pair := make_contact_pair(
i32(moved_contact.a) - 1,
i32(moved_contact.b) - 1,
)
sim_state.contact_container.lookup[moved_pair] = i32(i)
}
} else {
i += 1
}
}
}
update_contacts(sim_state)
Solver :: enum {
XPBD,
PGS,
}
solver := Solver.PGS
solver := Solver.XPBD
switch solver {
case .XPBD:
sim_state_copy(&scene.scratch_sim_state, get_sim_state(scene))
xpbd_predict_positions(&scene.scratch_sim_state, config, config.timestep)
find_collisions(&scene.scratch_sim_state, &sim_state.contact_pairs, potential_pairs)
for _ in 0 ..< substeps {
xpbd_substep(sim_state, config, dt, inv_dt)
}
case .PGS:
find_collisions(sim_state, &sim_state.contact_pairs, potential_pairs)
for _ in 0 ..< substeps {
pgs_substep(sim_state, config, dt, inv_dt)
}