diff --git a/build_hot_reload.sh b/build_hot_reload.sh index 64d2beb..1dabf5d 100755 --- a/build_hot_reload.sh +++ b/build_hot_reload.sh @@ -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 +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 # 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 diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index 1492b49..af591de 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -59,6 +59,8 @@ Contact_Type :: union { Contact_Manifold :: struct { type: Contact_Type, normal: Vec3, + tangent: Vec3, + bitangent: Vec3, separation: f32, points_a: [4]Vec3, points_b: [4]Vec3, @@ -89,7 +91,6 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli is_face_a_contact := biased_face_a_separation >= biased_edge_separation is_face_b_contact := biased_face_b_separation >= biased_edge_separation - collision = true if is_face_a_contact || is_face_b_contact { manifold = create_face_contact_manifold(face_query_a, a, face_query_b, b) } else { @@ -102,6 +103,10 @@ convex_vs_convex_sat :: proc(a, b: Convex) -> (manifold: Contact_Manifold, colli edge_b, ) } + collision = manifold.points_len > 0 + if collision { + manifold.bitangent = lg.normalize0(lg.cross(manifold.tangent, manifold.normal)) + } return } @@ -423,8 +428,12 @@ create_face_contact_manifold :: proc( } } - ref_face_vert := ref_convex.vertices[ref_convex.edges[ref_face.edge].origin].pos + ref_face_vert, ref_face_vert2 := halfedge.get_edge_points( + ref_convex, + ref_convex.edges[ref_face.edge], + ) ref_plane := plane_from_point_normal(ref_face_vert, ref_face.normal) + ref_tangent := lg.normalize0(ref_face_vert2 - ref_face_vert) // Final clipping, remove verts above ref face and project those left onto the reference plane { @@ -527,6 +536,7 @@ create_face_contact_manifold :: proc( manifold.separation = ref_face_query.separation // Normal is always pointing from a to b manifold.normal = is_ref_a ? ref_face.normal : -ref_face.normal + manifold.tangent = ref_tangent return } @@ -560,6 +570,7 @@ create_edge_contact_manifold :: proc( manifold.points_a[0] = ps[0] manifold.points_b[0] = ps[1] manifold.points_len = 1 + manifold.tangent = lg.normalize0(lg.cross(a2 - a1, manifold.normal)) return } diff --git a/game/physics/debug.odin b/game/physics/debug.odin index aa0970c..28cca69 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -117,12 +117,14 @@ draw_debug_scene :: proc(scene: ^Scene) { 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, + contact, + -1, points_a_slice, color = debug.int_to_color(i32(contact_idx * 2 + 0)), ) debug_draw_manifold_points( - contact.manifold.normal, + contact, + 1, points_b_slice, color = debug.int_to_color(i32(contact_idx * 2 + 1)), ) @@ -136,7 +138,12 @@ debug_transform_points_local_to_world :: proc(body: Body_Ptr, points: []Vec3) { } } -debug_draw_manifold_points :: proc(normal: Vec3, points: []Vec3, color: rl.Color) { +debug_draw_manifold_points :: proc( + contact: Contact, + impulse_sign: f32, + points: []Vec3, + color: rl.Color, +) { if len(points) >= 3 { // Triangle or quad v1 := points[0] @@ -151,9 +158,15 @@ debug_draw_manifold_points :: proc(normal: Vec3, points: []Vec3, color: rl.Color rl.DrawLine3D(points[0], points[1], color) } - for p in points { - rl.DrawLine3D(p, p + normal, color) - rl.DrawSphereWires(p, 0.1, 4, 4, color) + for point_idx in 0 ..< len(points) { + p := points[point_idx] + total_impulse := + contact.total_normal_impulse[point_idx] * contact.manifold.normal + + contact.total_friction_impulse[point_idx].x * contact.manifold.tangent + + contact.total_friction_impulse[point_idx].y * contact.manifold.bitangent + rl.DrawLine3D(p, p + total_impulse * impulse_sign, color) + + // rl.DrawSphereWires(p, 0.1, 4, 4, color) } } diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 28bb396..68143b3 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -6,6 +6,7 @@ import lg "core:math/linalg" MAX_CONTACTS :: 1024 * 16 Vec3 :: [3]f32 +Vec2 :: [2]f32 Quat :: quaternion128 Matrix3 :: # row_major matrix[3, 3]f32 AABB :: struct { diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 2c9361a..7d63824 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -219,7 +219,8 @@ Contact :: struct { // For PGS total_normal_impulse: [4]f32, - total_tangent_impulse: [4]f32, + // xy - tangent and bitangent (linear friction), z - twist friction around normal (rotational impulse only) + total_friction_impulse: [4]Vec2, // XPBD stuff lambda_normal: [4]f32, @@ -253,7 +254,7 @@ update_contacts :: proc(sim_state: ^Sim_State) { old_manifold := contact.manifold old_total_normal_impulse := contact.total_normal_impulse - old_total_tangent_impulse := contact.total_tangent_impulse + old_total_friction_impulse := contact.total_friction_impulse contact.prev_x_a = body.x contact.prev_x_b = body2.x @@ -261,7 +262,7 @@ update_contacts :: proc(sim_state: ^Sim_State) { contact.prev_q_b = body2.q contact.manifold = {} contact.total_normal_impulse = 0 - contact.total_tangent_impulse = 0 + contact.total_friction_impulse = 0 contact.lambda_normal = 0 contact.lambda_tangent = 0 contact.applied_static_friction = false @@ -285,18 +286,11 @@ update_contacts :: proc(sim_state: ^Sim_State) { raw_manifold, collision := collision.convex_vs_convex_sat(m1, m2) if collision { + assert(raw_manifold.points_len > 0) + manifold := &contact.manifold manifold^ = raw_manifold - preserve_impulse := - old_manifold.points_len == raw_manifold.points_len && - old_manifold.type == raw_manifold.type - - if preserve_impulse { - contact.total_normal_impulse = old_total_normal_impulse - contact.total_tangent_impulse = old_total_tangent_impulse - } - // Convert manifold contact from world to local space for point_idx in 0 ..< manifold.points_len { manifold.points_a[point_idx] = body_world_to_local( @@ -308,6 +302,25 @@ update_contacts :: proc(sim_state: ^Sim_State) { manifold.points_b[point_idx], ) } + + preserve_impulse := + old_manifold.points_len == raw_manifold.points_len && + old_manifold.type == raw_manifold.type + + if preserve_impulse { + contact.total_normal_impulse = old_total_normal_impulse + + for point_idx in 0 ..< manifold.points_len { + contact.total_friction_impulse[point_idx].x = lg.dot( + old_total_friction_impulse[point_idx].x * old_manifold.tangent, + manifold.tangent, + ) + contact.total_friction_impulse[point_idx].y = lg.dot( + old_total_friction_impulse[point_idx].y * old_manifold.bitangent, + manifold.bitangent, + ) + } + } } } } @@ -822,13 +835,27 @@ pgs_solve_contacts :: proc( inv_dt: f32, apply_bias: bool, ) { - bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(100, 1, dt) + bias_rate, mass_coef, impulse_coef := calculate_soft_constraint_params(30, 0.8, dt) if !apply_bias { + mass_coef = 1 bias_rate = 0 + impulse_coef = 0 + } + + random_order := make([]i32, len(sim_state.contact_container.contacts)) + { + for i in 0 ..< len(random_order) { + random_order[i] = i32(i) + } + + for i in 0 ..< len(random_order) - 1 { + j := rand.int_max(len(random_order)) + slice.swap(random_order, i, j) + } } for i in 0 ..< len(sim_state.contact_container.contacts) { - contact := &sim_state.contact_container.contacts[i] + contact := &sim_state.contact_container.contacts[random_order[i]] manifold := &contact.manifold body1, body2 := get_body(sim_state, contact.a), get_body(sim_state, contact.b) @@ -857,29 +884,71 @@ pgs_solve_contacts :: proc( inv_w := 1.0 / w - delta_v := lg.dot(v2 - v1, manifold.normal) + delta_v := v2 - v1 + { + delta_v_norm := lg.dot(delta_v, manifold.normal) - incremental_impulse := - -inv_w * mass_coef * (delta_v + bias_rate * separation) - - impulse_coef * contact.total_normal_impulse[point_idx] + bias := f32(0.0) - new_total_impulse := max( - contact.total_normal_impulse[point_idx] + incremental_impulse, - 0, - ) - applied_impulse := new_total_impulse - contact.total_normal_impulse[point_idx] - contact.total_normal_impulse[point_idx] = new_total_impulse + MAX_BAUMGARTE_VELOCITY :: 4.0 - applied_impulse_vec := applied_impulse * manifold.normal + if separation > 0 { + bias = separation * inv_dt + } else if apply_bias { + bias = lg.max(bias_rate * separation, -MAX_BAUMGARTE_VELOCITY) + } - apply_velocity_correction(body1, -applied_impulse_vec, p1) - apply_velocity_correction(body2, applied_impulse_vec, p2) + incremental_impulse := + -inv_w * mass_coef * (delta_v_norm + bias) - + impulse_coef * contact.total_normal_impulse[point_idx] + + new_total_impulse := max( + contact.total_normal_impulse[point_idx] + incremental_impulse, + 0, + ) + applied_impulse := new_total_impulse - contact.total_normal_impulse[point_idx] + contact.total_normal_impulse[point_idx] = new_total_impulse + + applied_impulse_vec := applied_impulse * manifold.normal + + apply_velocity_correction(body1, -applied_impulse_vec, p1) + apply_velocity_correction(body2, applied_impulse_vec, p2) + } + + { + FRICTION :: 0.6 + friction_clamp := contact.total_normal_impulse[point_idx] * FRICTION + + delta_v_tang := Vec2 { + lg.dot(delta_v, manifold.tangent), + lg.dot(delta_v, manifold.bitangent), + } + + incremental_impulse := -inv_w * delta_v_tang + + new_total_impulse: Vec2 = lg.clamp( + contact.total_friction_impulse[point_idx] + incremental_impulse, + Vec2(-friction_clamp), + Vec2(friction_clamp), + ) + + applied_impulse := + new_total_impulse - contact.total_friction_impulse[point_idx] + contact.total_friction_impulse[point_idx] = new_total_impulse + + applied_impulse_vec := + applied_impulse.x * manifold.tangent + + applied_impulse.y * manifold.bitangent + + apply_velocity_correction(body1, -applied_impulse_vec, p1) + apply_velocity_correction(body2, applied_impulse_vec, p2) + } } else { contact.total_normal_impulse[point_idx] = 0 + contact.total_friction_impulse[point_idx] = 0 } } } - } pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) { @@ -907,6 +976,13 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d apply_velocity_correction(body1, -total_normal_impulse, p1) apply_velocity_correction(body2, total_normal_impulse, p2) + + total_tangent_impulse := + contact.total_friction_impulse[point_idx].x * manifold.tangent + + contact.total_friction_impulse[point_idx].y * manifold.bitangent + + apply_velocity_correction(body1, -total_tangent_impulse, p1) + apply_velocity_correction(body2, total_tangent_impulse, p2) } } } @@ -950,7 +1026,6 @@ simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Confi tlas := build_tlas(sim_state, config) - find_new_contacts(sim_state, &tlas) { tracy.ZoneN("simulate_step::remove_invalid_contacts") @@ -981,6 +1056,8 @@ simulate_step :: proc(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Confi } } + find_new_contacts(sim_state, &tlas) + update_contacts(sim_state) Solver :: enum { @@ -1131,14 +1208,32 @@ multiply_inv_intertia :: proc(body: Body_Ptr, vec: Vec3) -> (result: Vec3) { return result } -apply_velocity_correction :: proc(body: Body_Ptr, impulse: Vec3, pos: Vec3) { - body.v += impulse * body.inv_mass +apply_velocity_correction :: #force_inline proc "contextless" ( + body: Body_Ptr, + impulse: Vec3, + pos: Vec3, +) { + apply_velocity_correction_linear(body, impulse) + angular_impulse := lg.cross(pos - body.x, impulse) + + apply_velocity_correction_angular(body, angular_impulse) +} + +apply_velocity_correction_linear :: #force_inline proc "contextless" ( + body: Body_Ptr, + impulse: Vec3, +) { + body.v += impulse * body.inv_mass +} + +apply_velocity_correction_angular :: #force_inline proc "contextless" ( + body: Body_Ptr, + angular_impulse: Vec3, +) { q := body.q inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) - delta_omega := pos - body.x - delta_omega = lg.cross(delta_omega, impulse) - delta_omega = lg.quaternion_mul_vector3(inv_q, delta_omega) + delta_omega := lg.quaternion_mul_vector3(inv_q, angular_impulse) delta_omega *= body.inv_inertia_tensor delta_omega = lg.quaternion_mul_vector3(q, delta_omega) @@ -1167,7 +1262,20 @@ apply_position_correction :: proc(body: Body_Ptr, corr: Vec3, pos: Vec3) { body.q = q } +// Total inverse mass (linear + angular) get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: Vec3) -> f32 { + linear, angular := get_body_inverse_mass_separate(body, normal, pos) + + return linear + angular +} + +get_body_inverse_mass_separate :: proc( + body: Body_Ptr, + normal, pos: Vec3, +) -> ( + linear: f32, + angular: f32, +) { q := body.q inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) @@ -1175,10 +1283,10 @@ get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: Vec3) -> f32 { rn = lg.cross(rn, normal) rn = lg.quaternion_mul_vector3(inv_q, rn) - w := lg.dot(rn, rn * body.inv_inertia_tensor) - w += body.inv_mass + linear = body.inv_mass + angular = lg.dot(rn, rn * body.inv_inertia_tensor) - return w + return } get_body_angular_inverse_mass :: proc(body: Body_Ptr, normal: Vec3) -> f32 {