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

Optimize edge separation test, use SOA structs in more places

Browse Source
This commit is contained in:
sergeypdev 2025-03-02 21:14:12 +04:00
parent a1e8d0f231
commit 860f9f59b6
7 changed files with 133 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
game/game.odin
View File

@ -73,7 +73,7 @@ Car :: struct {
} }
SOLVER_CONFIG :: physics.Solver_Config { SOLVER_CONFIG :: physics.Solver_Config {
timestep = 1.0 / 60, timestep = 1.0 / 120,
gravity = rl.Vector3{0, -9.8, 0}, gravity = rl.Vector3{0, -9.8, 0},
substreps_minus_one = 2 - 1, substreps_minus_one = 2 - 1,
} }
@ -259,7 +259,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
&runtime_world.solver_state, &runtime_world.solver_state,
#hash("car", "fnv32a"), #hash("car", "fnv32a"),
physics.Body_Config { physics.Body_Config {
initial_pos = {0, 4, 0}, initial_pos = {0, 4, -10},
initial_rot = linalg.QUATERNIONF32_IDENTITY, initial_rot = linalg.QUATERNIONF32_IDENTITY,
// initial_rot = linalg.quaternion_angle_axis( // initial_rot = linalg.quaternion_angle_axis(
// math.RAD_PER_DEG * 180, // math.RAD_PER_DEG * 180,
@ -277,18 +277,17 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) {
) )
if true { if true {
for x in 0 ..< 20 {
for x in 0 ..< 10 { for y in 0 ..< 10 {
for y in -3 ..< 100 {
physics.immediate_body( physics.immediate_body(
&world.physics_scene, &world.physics_scene,
&runtime_world.solver_state, &runtime_world.solver_state,
hash.fnv32a(slice.to_bytes([]int{(x | y << 8)})), hash.fnv32a(slice.to_bytes([]int{(x | y << 8)})),
physics.Body_Config { physics.Body_Config {
initial_pos = {f32(x) * 3 - 10, 5 + f32(y) * 3, 0}, initial_pos = {0, 0.5 + f32(y) * 1.1, f32(x) * 3 + 10},
initial_rot = linalg.QUATERNIONF32_IDENTITY, initial_rot = linalg.QUATERNIONF32_IDENTITY,
shape = physics.Shape_Box{size = 1}, shape = physics.Shape_Box{size = 1},
mass = 10, mass = 1,
}, },
) )
} }
@ -648,7 +647,7 @@ draw :: proc() {
} }
} }
// rl.DrawModel(car_model, physics.body_get_shape_pos(car_body), 1, rl.WHITE) rl.DrawModel(car_model, physics.body_get_shape_pos(car_body), 1, rl.WHITE)
} }
{ {

14
game/halfedge/debug.odin
View File

@ -1,27 +1,15 @@
package halfedge package halfedge
import "game:debug"
import rl "vendor:raylib" import rl "vendor:raylib"
debug_draw_mesh_wires :: proc(mesh: Half_Edge_Mesh, color: rl.Color) { debug_draw_mesh_wires :: proc(mesh: Half_Edge_Mesh, color: rl.Color) {
for face, f in mesh.faces { for _, f in mesh.faces {
it := iterator_face_edges(mesh, Face_Index(f)) it := iterator_face_edges(mesh, Face_Index(f))
center: Vec3
num_points := 0
for edge in iterate_next_edge(&it) { for edge in iterate_next_edge(&it) {
a, b := get_edge_points(mesh, edge) a, b := get_edge_points(mesh, edge)
center += a
num_points += 1
rl.DrawLine3D(a, b, color) rl.DrawLine3D(a, b, color)
} }
center /= f32(num_points)
rl.DrawSphereWires(center, 0.1, 4, 4, rl.RED)
rl.DrawLine3D(center, center + face.normal, debug.int_to_color(i32(f + 1)))
} }
} }

20
game/halfedge/halfedge.odin
View File

@ -129,13 +129,29 @@ mesh_from_vertex_index_list :: proc(
return mesh return mesh
} }
get_edge_points :: proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (a: Vec3, b: Vec3) { get_edge_points :: #force_inline proc(
mesh: Half_Edge_Mesh,
edge: Half_Edge,
) -> (
a: Vec3,
b: Vec3,
) {
a = mesh.vertices[edge.origin].pos a = mesh.vertices[edge.origin].pos
b = mesh.vertices[mesh.edges[edge.next].origin].pos b = mesh.vertices[mesh.edges[edge.next].origin].pos
return return
} }
get_edge_direction_normalized :: proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (dir: Vec3) { get_edge_direction :: #force_inline proc(mesh: Half_Edge_Mesh, edge: Half_Edge) -> (dir: Vec3) {
a, b := get_edge_points(mesh, edge)
return b - a
}
get_edge_direction_normalized :: #force_inline proc(
mesh: Half_Edge_Mesh,
edge: Half_Edge,
) -> (
dir: Vec3,
) {
a, b := get_edge_points(mesh, edge) a, b := get_edge_points(mesh, edge)
return lg.normalize0(b - a) return lg.normalize0(b - a)
} }

159
game/physics/collision/convex.odin
View File

@ -1,5 +1,6 @@
package collision package collision
import "core:container/bit_array"
import "core:log" import "core:log"
import "core:math" import "core:math"
import lg "core:math/linalg" import lg "core:math/linalg"
@ -164,6 +165,33 @@ find_furthest_point_from_point :: proc(points: []Vec3, ref: Vec3) -> (p: Vec3) {
return return
} }
// See: Physics for Game Programmers: The Separating Axis Test between Convex Polyhedra
// https://gdcvault.com/play/1017646/Physics-for-Game-Programmers-The
is_minkowski_face :: #force_inline proc(bxa, dxc, a, b, c, d: Vec3) -> bool {
cba := lg.dot(c, bxa)
dba := lg.dot(d, bxa)
adc := lg.dot(a, dxc)
bdc := lg.dot(b, dxc)
return cba * dba < 0 && adc * bdc < 0 && cba * bdc > 0
}
build_minkowski_face :: #force_inline proc(
mesh_a, mesh_b: Convex,
edge_a, edge_b: halfedge.Half_Edge,
) -> bool {
a := mesh_a.faces[edge_a.face].normal
b := mesh_a.faces[mesh_a.edges[edge_a.twin].face].normal
c := mesh_b.faces[edge_b.face].normal
d := mesh_b.faces[mesh_b.edges[edge_b.twin].face].normal
bxa := halfedge.get_edge_direction(mesh_a, edge_a)
dxc := halfedge.get_edge_direction(mesh_b, edge_b)
// Negate normals c and d because we're interested in minkowski difference instead of sum
return is_minkowski_face(bxa, dxc, a, b, -c, -d)
}
query_separation_edges :: proc( query_separation_edges :: proc(
a, b: Convex, a, b: Convex,
) -> ( ) -> (
@ -178,123 +206,56 @@ query_separation_edges :: proc(
a_edge = -1 a_edge = -1
b_edge = -1 b_edge = -1
step := 0 checked_pairs: bit_array.Bit_Array
bit_array.init(&checked_pairs, len(a.edges) * len(b.edges), 0, context.temp_allocator)
a_len := len(a.edges)
if false { calc_pair_index :: proc(a, b, a_len: int) -> int {
return (b * a_len) + a
}
separating_plane_p: Vec3 for edge_a, edge_a_idx in a.edges {
success_step: int for edge_b, edge_b_idx in b.edges {
pair_idx := calc_pair_index(edge_a_idx, edge_b_idx, a_len)
if bit_array.get(&checked_pairs, pair_idx) {
continue
}
Edge_Pair :: [2]halfedge.Edge_Index // TODO: sort edges so twins are next to each other, then can just iterate with step = 2 and skip this bitfield
checked_pairs := make_map_cap( bit_array.set(&checked_pairs, pair_idx)
map[Edge_Pair]bool, bit_array.set(&checked_pairs, calc_pair_index(int(edge_a.twin), edge_b_idx, a_len))
len(a.edges) * len(b.edges), bit_array.set(&checked_pairs, calc_pair_index(edge_a_idx, int(edge_b.twin), a_len))
context.temp_allocator, bit_array.set(
) &checked_pairs,
calc_pair_index(int(edge_a.twin), int(edge_b.twin), a_len),
for edge_a, edge_a_idx in a.edges { )
for edge_b, edge_b_idx in b.edges {
pair := Edge_Pair{halfedge.Edge_Index(edge_a_idx), halfedge.Edge_Index(edge_b_idx)}
if checked_pairs[pair] {
continue
}
checked_pairs[pair] = true
if edge_a.twin >= 0 {
checked_pairs[{edge_a.twin, halfedge.Edge_Index(edge_b_idx)}] = true
}
if edge_b.twin >= 0 {
checked_pairs[{halfedge.Edge_Index(edge_a_idx), edge_b.twin}] = true
}
if edge_a.twin >= 0 && edge_b.twin >= 0 {
checked_pairs[{edge_a.twin, edge_b.twin}] = true
}
edge_a_dir := halfedge.get_edge_direction_normalized(a, edge_a)
edge_b_dir := halfedge.get_edge_direction_normalized(b, edge_b)
if build_minkowski_face(a, b, edge_a, edge_b) {
edge_a1, edge_a2 := halfedge.get_edge_points(a, edge_a)
edge_b1, edge_b2 := halfedge.get_edge_points(b, edge_b)
edge_a_dir := edge_a2 - edge_a1
edge_b_dir := edge_b2 - edge_b1
axis := lg.normalize0(lg.cross(edge_a_dir, edge_b_dir)) axis := lg.normalize0(lg.cross(edge_a_dir, edge_b_dir))
if axis == 0 { if axis == 0 {
continue continue
} }
if lg.dot(axis, edge_a1 - a.center) < 0 {
edge_a_origin, _ := halfedge.get_edge_points(a, edge_a)
if lg.dot(axis, edge_a_origin - a.center) < 0 {
axis = -axis axis = -axis
} }
plane_a := plane_from_point_normal(edge_a_origin, axis)
vert_a, _, _ := find_support_point(a, plane_a.normal)
vert_b, vert_b_idx, _ := find_support_point(b, -plane_a.normal)
// We found the support vert on mesh b, but now we need to find the plane_a := plane_from_point_normal(edge_a1, axis)
// best edge that includes that point
vert_b_edge: halfedge.Half_Edge
vert_b_edge_idx: halfedge.Edge_Index = -1
{
min_b2_distance := max(f32)
it := halfedge.iterator_vertex_edges(b, vert_b_idx)
for edge, edge_idx in halfedge.iterate_next_vertex_edge(&it) {
_, vert_b2 := halfedge.get_edge_points(b, edge)
distance_b2 := signed_distance_plane(vert_b2, plane_a) distance := signed_distance_plane(edge_b1, plane_a)
if distance_b2 < min_b2_distance { if distance > separation {
min_b2_distance = distance_b2 separation = distance
vert_b_edge = edge
vert_b_edge_idx = edge_idx
}
}
if vert_b_edge_idx < 0 {
continue
}
}
distance_a := signed_distance_plane(vert_a.pos, plane_a)
if distance_a > 0 {
continue
}
distance_b := signed_distance_plane(vert_b.pos, plane_a)
vert_b_projected := vert_b.pos + plane_a.normal * -distance_b
if step == -1 {
// a1, a2 := halfedge.get_edge_points(a, edge_a)
// edge_a_center := (a1 + a2) * 0.5
a1, a2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(a), edge_a)
b1, b2 := halfedge.get_edge_points(halfedge.Half_Edge_Mesh(b), vert_b_edge)
rl.DrawLine3D(edge_a_origin, edge_a_origin + plane_a.normal, rl.BLUE)
rl.DrawLine3D(a1 + 0.1, a2 + 0.1, rl.ORANGE)
rl.DrawLine3D(b1 + 0.1, b2 + 0.1, rl.PURPLE)
rl.DrawSphereWires(edge_a_origin, 0.1, 4, 4, rl.ORANGE)
rl.DrawSphereWires(vert_b.pos, 0.05, 4, 4, rl.BLUE)
rl.DrawSphereWires(vert_b_projected, 0.05, 4, 4, rl.BLUE)
rl.DrawLine3D(vert_b.pos, vert_b_projected, rl.VIOLET)
log.debugf("dist: %v", distance_b)
{
// rl.BeginBlendMode(.ALPHA)
// defer rl.EndBlendMode()
debug_draw_plane(edge_a_origin, plane_a, rl.Color{0, 228, 48, 100})
}
}
if distance_b > separation {
separation = distance_b
a_edge = halfedge.Edge_Index(edge_a_idx) a_edge = halfedge.Edge_Index(edge_a_idx)
b_edge = vert_b_edge_idx b_edge = halfedge.Edge_Index(edge_b_idx)
separating_plane = plane_a separating_plane = plane_a
separating_plane_p = edge_a_origin
success_step = step
} }
step += 1
} }
} }
// log.debugf("step: %v", success_step)
// debug_draw_plane(separating_plane_p, separating_plane, rl.Color{228, 0, 48, 100})
} }
return return
} }

4
game/physics/debug.odin
View File

@ -108,8 +108,8 @@ draw_debug_scene :: proc(scene: ^Scene) {
} }
} }
if true { if false {
for &contact, contact_idx in sim_state.contact_pairs[:sim_state.contact_pairs_len] { for &contact, contact_idx in sim_state.contact_pairs {
points_a := contact.manifold.points_a points_a := contact.manifold.points_a
points_b := contact.manifold.points_b points_b := contact.manifold.points_b
points_a_slice, points_b_slice := points_a_slice, points_b_slice :=

4
game/physics/scene.odin
View File

@ -26,8 +26,7 @@ Sim_State :: struct {
first_free_suspension_constraint_plus_one: i32, first_free_suspension_constraint_plus_one: i32,
// Persistent stuff for simulation // Persistent stuff for simulation
contact_pairs: [MAX_CONTACTS]Contact_Pair, contact_pairs: #soa[dynamic]Contact_Pair,
contact_pairs_len: int,
convex_container: Convex_Container, convex_container: Convex_Container,
} }
@ -381,6 +380,7 @@ _get_first_free_body :: proc(sim_state: ^Sim_State) -> i32 {
destry_sim_state :: proc(sim_state: ^Sim_State) { destry_sim_state :: proc(sim_state: ^Sim_State) {
delete_soa(sim_state.bodies) delete_soa(sim_state.bodies)
delete_soa(sim_state.suspension_constraints) delete_soa(sim_state.suspension_constraints)
delete_soa(sim_state.contact_pairs)
convex_container_destroy(&sim_state.convex_container) convex_container_destroy(&sim_state.convex_container)
} }

68
game/physics/simulation.odin
View File

@ -10,6 +10,7 @@ import "libs:tracy"
_ :: math _ :: math
_ :: fmt _ :: fmt
_ :: slice
Solver_Config :: struct { Solver_Config :: struct {
// Will automatically do fixed timestep // Will automatically do fixed timestep
@ -229,9 +230,9 @@ simulate_step :: proc(
) { ) {
tracy.Zone() tracy.Zone()
body_states := make([]Body_Sim_State, len(sim_state.bodies), context.temp_allocator) body_states := make_soa(#soa[]Body_Sim_State, len(sim_state.bodies), context.temp_allocator)
sim_state.contact_pairs_len = 0 resize_soa(&sim_state.contact_pairs, 0)
substeps := config.substreps_minus_one + 1 substeps := config.substreps_minus_one + 1
@ -283,6 +284,16 @@ simulate_step :: proc(
assert(body.alive) assert(body.alive)
assert(body2.alive) assert(body2.alive)
aabb1, aabb2 := body_get_aabb(body), body_get_aabb(body2)
// TODO: extract common math functions into a sane place
if !collision.test_aabb_vs_aabb(
{min = aabb1.center - aabb1.extent, max = aabb1.center + aabb1.extent},
{min = aabb2.center - aabb2.extent, max = aabb2.center + aabb2.extent},
) {
continue
}
m1, m2 := m1, m2 :=
body_get_convex_shape_world(sim_state, body), body_get_convex_shape_world(sim_state, body),
body_get_convex_shape_world(sim_state, body2) body_get_convex_shape_world(sim_state, body2)
@ -291,7 +302,9 @@ simulate_step :: proc(
raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2) raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2)
if collision { if collision {
contact_pair := &sim_state.contact_pairs[sim_state.contact_pairs_len] 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]
contact_pair^ = Contact_Pair { contact_pair^ = Contact_Pair {
a = Body_Handle(i + 1), a = Body_Handle(i + 1),
b = Body_Handle(j + 1), b = Body_Handle(j + 1),
@ -301,7 +314,6 @@ simulate_step :: proc(
prev_q_b = body2.q, prev_q_b = body2.q,
manifold = raw_manifold, manifold = raw_manifold,
} }
sim_state.contact_pairs_len += 1
manifold := &contact_pair.manifold manifold := &contact_pair.manifold
// Convert manifold contact from world to local space // Convert manifold contact from world to local space
@ -350,7 +362,7 @@ simulate_step :: proc(
{ {
tracy.ZoneN("simulate_step::static_friction") tracy.ZoneN("simulate_step::static_friction")
if false { when false {
context = context context = context
context.user_ptr = sim_state context.user_ptr = sim_state
slice.sort_by( slice.sort_by(
@ -383,7 +395,7 @@ simulate_step :: proc(
) )
} }
for &contact_pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] { for &contact_pair in sim_state.contact_pairs {
manifold := contact_pair.manifold manifold := contact_pair.manifold
body, body2 := body, body2 :=
get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b) get_body(sim_state, contact_pair.a), get_body(sim_state, contact_pair.b)
@ -428,7 +440,7 @@ simulate_step :: proc(
p2, p2,
) )
STATIC_FRICTION :: 0.8 STATIC_FRICTION :: 0.6
if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm { if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm {
contact_pair.applied_static_friction[point_idx] = true contact_pair.applied_static_friction[point_idx] = true
contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent contact_pair.lambda_tangent[point_idx] = delta_lambda_tangent
@ -475,13 +487,21 @@ simulate_step :: proc(
} }
} }
solve_velocities(sim_state, body_states, inv_dt) {
// Compute new linear and angular velocities
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)
}
}
}
// Restituion // Restituion
{ {
tracy.ZoneN("simulate_step::restitution") tracy.ZoneN("simulate_step::restitution")
for &pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] { for &pair in sim_state.contact_pairs {
i, j := int(pair.a) - 1, int(pair.b) - 1 i, j := int(pair.a) - 1, int(pair.b) - 1
manifold := &pair.manifold manifold := &pair.manifold
@ -507,7 +527,7 @@ simulate_step :: proc(
prev_v_normal := lg.dot(prev_v, manifold.normal) prev_v_normal := lg.dot(prev_v, manifold.normal)
v_normal := lg.dot(v, manifold.normal) v_normal := lg.dot(v, manifold.normal)
RESTITUTION :: 1 RESTITUTION :: 0
restitution := f32(RESTITUTION) restitution := f32(RESTITUTION)
@ -537,7 +557,7 @@ simulate_step :: proc(
if true { if true {
tracy.ZoneN("simulate_step::dynamic_friction") tracy.ZoneN("simulate_step::dynamic_friction")
for &pair in sim_state.contact_pairs[:sim_state.contact_pairs_len] { for &pair in sim_state.contact_pairs {
manifold := &pair.manifold manifold := &pair.manifold
body1 := get_body(sim_state, pair.a) body1 := get_body(sim_state, pair.a)
body2 := get_body(sim_state, pair.b) body2 := get_body(sim_state, pair.b)
@ -557,7 +577,7 @@ simulate_step :: proc(
v_normal := lg.dot(manifold.normal, v) * manifold.normal v_normal := lg.dot(manifold.normal, v) * manifold.normal
v_tangent := v - v_normal v_tangent := v - v_normal
DYNAMIC_FRICTION :: 0.6 DYNAMIC_FRICTION :: 0.3
v_tangent_len := lg.length(v_tangent) v_tangent_len := lg.length(v_tangent)
if v_tangent_len > 0 { if v_tangent_len > 0 {
v_tangent_norm := v_tangent / v_tangent_len v_tangent_norm := v_tangent / v_tangent_len
@ -624,7 +644,7 @@ simulate_step :: proc(
pos = wheel_world_pos, pos = wheel_world_pos,
) )
body_solve_velocity(body, body_state, inv_dt) body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
} }
// Drive forces // Drive forces
@ -640,7 +660,7 @@ simulate_step :: proc(
-forward, -forward,
wheel_world_pos, wheel_world_pos,
) )
body_solve_velocity(body, body_state, inv_dt) body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
} }
// Lateral friction // Lateral friction
@ -656,7 +676,7 @@ simulate_step :: proc(
v.applied_impulse.x = impulse v.applied_impulse.x = impulse
apply_correction(body, corr, v.hit_point) apply_correction(body, corr, v.hit_point)
body_solve_velocity(body, body_state, inv_dt) body_solve_velocity(body, body_state.prev_x, body_state.prev_q, inv_dt)
} }
} }
} }
@ -665,19 +685,17 @@ simulate_step :: proc(
} }
solve_velocities :: proc(scene: ^Sim_State, 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]
if body.alive {
body_solve_velocity(body, body_states[i], inv_dt)
}
}
} }
body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State, inv_dt: f32) { body_solve_velocity :: #force_inline proc(
body.v = (body.x - state.prev_x) * inv_dt body: Body_Ptr,
prev_x: Vec3,
prev_q: Quat,
inv_dt: f32,
) {
body.v = (body.x - prev_x) * inv_dt
delta_q := body.q * lg.quaternion_inverse(state.prev_q) delta_q := body.q * lg.quaternion_inverse(prev_q)
body.w = Vec3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt body.w = Vec3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt
if delta_q.w < 0 { if delta_q.w < 0 {