From cb24365933f58f75f5c1d2f5cfb77e6ed92497b6 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sat, 8 Mar 2025 23:46:14 +0400 Subject: [PATCH] Start implementing a different solver --- game/game.odin | 8 +- game/physics/simulation.odin | 919 ++++++++++++++++++----------------- 2 files changed, 489 insertions(+), 438 deletions(-) diff --git a/game/game.odin b/game/game.odin index 6dbe96a..4bd077a 100644 --- a/game/game.odin +++ b/game/game.odin @@ -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 = 8 - 1, + substreps_minus_one = 4 - 1, } Game_Memory :: struct { @@ -277,8 +277,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { ) if true { - for x in 0 ..< 40 { - for y in 0 ..< 5 { + for x in 0 ..< 20 { + for y in 0 ..< 20 { physics.immediate_body( &world.physics_scene, &runtime_world.solver_state, @@ -287,7 +287,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { initial_pos = {0, 0.5 + f32(y) * 1.1, f32(x) * 3 + 10}, initial_rot = linalg.QUATERNIONF32_IDENTITY, shape = physics.Shape_Box{size = 1}, - mass = 1, + mass = 10, }, ) } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index f79d969..85a91f3 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -10,6 +10,7 @@ import "core:math/rand" import "core:slice" import "libs:tracy" +_ :: rand _ :: math _ :: fmt _ :: slice @@ -79,24 +80,16 @@ Step_Mode :: enum { Potential_Pair :: [2]u16 -// Outer simulation loop for fixed timestepping -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, -) { +// Top Level Acceleration Structure +TLAS :: struct { + bvh_tree: bvh.BVH, + body_aabbs: []bvh.AABB, +} + +// TODO: free intermediate temp allocs +// Creates TLAS using temp allocator +build_tlas :: proc(sim_state: ^Sim_State, config: Solver_Config) -> TLAS { tracy.Zone() - assert(config.timestep > 0) - - prune_immediate(scene, state) - - 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) body_aabbs := make([]bvh.AABB, sim_state.num_bodies, context.temp_allocator) body_indices := make([]u16, sim_state.num_bodies, context.temp_allocator) @@ -125,35 +118,35 @@ simulate :: proc( sim_state_bvh := bvh.build_bvh_from_aabbs(body_aabbs, body_indices, context.temp_allocator) + return TLAS{bvh_tree = sim_state_bvh, body_aabbs = body_aabbs} +} + +// TODO: free intermediate temp allocs +find_potential_pairs :: proc(sim_state: ^Sim_State, tlas: ^TLAS) -> []Potential_Pair { + tracy.Zone() potential_pairs_map := make(map[Potential_Pair]bool, context.temp_allocator) - { - tracy.ZoneN("physics.simulate::find_potential_pairs") + for i in 0 ..< len(sim_state.bodies_slice) { + assert(i <= int(max(u16))) + body_idx := u16(i) + body := &sim_state.bodies_slice[i] - for i in 0 ..< len(sim_state.bodies_slice) { - assert(i <= int(max(u16))) - body_idx := u16(i) - body := &sim_state.bodies_slice[i] + if body.alive { + body_aabb := tlas.body_aabbs[i] + it := bvh.iterator_intersect_leaf(&tlas.bvh_tree, body_aabb) - if body.alive { - body_aabb := body_aabbs[i] - it := bvh.iterator_intersect_leaf(&sim_state_bvh, 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] + prim_aabb := tlas.body_aabbs[other_body_idx] - for leaf_node in bvh.iterator_intersect_leaf_next(&it) { - for i in 0 ..< leaf_node.prim_len { - other_body_idx := - sim_state_bvh.primitives[leaf_node.child_or_prim_start + i] - prim_aabb := 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), - } - - potential_pairs_map[pair] = true + 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), } + + potential_pairs_map[pair] = true } } } @@ -170,11 +163,26 @@ 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) + return potential_pairs +} - // bvh.debug_draw_bvh_bounds(&sim_state_bvh, body_aabbs, 0) +// Outer simulation loop for fixed timestepping +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, +) { + tracy.Zone() + assert(config.timestep > 0) + + prune_immediate(scene, state) + + sim_state_copy(get_next_sim_state(scene), get_sim_state(scene)) + + sim_state := get_next_sim_state(scene) num_steps := 0 switch step_mode { @@ -186,11 +194,11 @@ simulate :: proc( state.accumulated_time -= config.timestep if num_steps < MAX_STEPS { - simulate_step(sim_state, config) + simulate_step(scene, sim_state, config) } } case .Single: - simulate_step(get_next_sim_state(scene), config) + simulate_step(scene, get_next_sim_state(scene), config) num_steps += 1 } @@ -203,13 +211,6 @@ simulate :: proc( state.num_referenced_suspension_constraints = 0 } -Body_Sim_State :: struct { - prev_x: Vec3, - prev_v: Vec3, - prev_w: Vec3, - prev_q: Quat, -} - GLOBAL_PLANE :: collision.Plane { normal = Vec3{0, 1, 0}, dist = 0, @@ -299,7 +300,7 @@ find_collisions :: proc( } } -predict_positions :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32) { +xpbd_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 { @@ -328,7 +329,413 @@ predict_positions :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32) } } -simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) { +xpbd_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) { + xpbd_predict_positions(sim_state, config, dt) + + Body_Pair :: struct { + a, b: int, + } + + { + 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) + + p_diff_normal := lg.dot(p2 - p1, manifold.normal) + separation := min(p_diff_normal, 0) + + lambda_norm, corr1, corr2, ok := calculate_constraint_params2( + dt, + body, + body2, + 0.00002, + separation, + -manifold.normal, + p1, + p2, + ) + if ok { + contact_pair.applied_normal_correction[point_idx] = -separation + contact_pair.applied_corrections += 1 + contact_pair.lambda_normal[point_idx] = lambda_norm + + apply_correction(body, corr1, p1) + apply_correction(body2, corr2, p2) + } + } + } + } + + if false { + tracy.ZoneN("simulate_step::static_friction") + + when false { + context = context + context.user_ptr = sim_state + slice.sort_by( + sim_state.contact_pairs[:sim_state.contact_pairs_len], + proc(c1, c2: Contact_Pair) -> bool { + sim_state := cast(^Sim_State)context.user_ptr + + find_min_contact_y :: proc( + scene: ^Sim_State, + c: Contact_Pair, + ) -> ( + min_contact_y: f32, + ) { + min_contact_y = max(f32) + body_a, body_b := get_body(scene, c.a), get_body(scene, c.b) + for i in 0 ..< c.manifold.points_len { + min_contact_y = min( + body_local_to_world(body_a, c.manifold.points_a[i]).y, + body_local_to_world(body_b, c.manifold.points_b[i]).y, + ) + } + return + } + + 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 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 point_idx in 0 ..< manifold.points_len { + lambda_norm := contact_pair.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]) + prev_p1 := + body.prev_x + + lg.quaternion_mul_vector3(body.prev_q, manifold.points_a[point_idx]) + prev_p2 := + 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 + + tangent_diff_len := lg.length(p_diff_tangent) + + if tangent_diff_len > 0 { + tangent_diff_normalized := p_diff_tangent / tangent_diff_len + + delta_lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent := + calculate_constraint_params2( + dt, + body, + body2, + 0, + -tangent_diff_len, + -tangent_diff_normalized, + p1, + p2, + ) + + 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 + + apply_correction(body, corr1_tangent, p1) + apply_correction(body2, corr2_tangent, p2) + } + } + } + } + } + } + + { + tracy.ZoneN("simulate_step::suspension_constraints") + + for &v in sim_state.suspension_constraints { + if v.alive { + 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) + pos2 := pos + dir * v.rest + v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane( + {pos, pos2}, + collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}), + ) + if v.hit { + corr := pos - v.hit_point + distance := lg.length(corr) + corr = corr / distance if distance > 0 else 0 + + _ = apply_constraint_correction_unilateral( + dt, + body, + v.compliance, + error = distance - v.rest, + error_gradient = corr, + pos = pos, + other_combined_inv_mass = 0, + ) + } + } + } + } + + { + // 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.prev_x, body.prev_q, inv_dt) + } + } + } + + // Restituion + if true { + tracy.ZoneN("simulate_step::restitution") + + for &pair in sim_state.contact_pairs { + manifold := &pair.manifold + + + body, body2 := get_body(sim_state, pair.a), get_body(sim_state, pair.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 { + 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]) + + 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 := + (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) + v_normal := lg.dot(v, manifold.normal) + + RESTITUTION :: 0 + + restitution := f32(RESTITUTION) + + if abs(v_normal) <= 2 * abs(lg.dot(manifold.normal, -config.gravity) * dt) { + restitution = 0 + } + + delta_v := manifold.normal * (-v_normal + min(-RESTITUTION * prev_v_normal, 0)) + + w1 := get_body_inverse_mass(body, manifold.normal, r1 + body.x) + w2 := get_body_inverse_mass(body2, manifold.normal, r2 + body2.x) + w := w1 + w2 + + if w != 0 { + p := delta_v / w + + body.v += p * body.inv_mass + body2.v -= p * body2.inv_mass + + body.w += multiply_inv_intertia(body, lg.cross(r1, p)) + body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p)) + } + } + } + } + + 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 point_idx in 0 ..< pair.manifold.points_len { + if pair.applied_static_friction[point_idx] || pair.lambda_normal == 0 { + continue + } + p1, p2 := + body_local_to_world(body1, manifold.points_a[point_idx]), + body_local_to_world(body2, manifold.points_b[point_idx]) + r1, r2 := p1 - body1.x, p2 - body2.x + v1 := body_velocity_at_point(body1, p1) + v2 := body_velocity_at_point(body2, p2) + + v := v1 - v2 + v_normal := lg.dot(manifold.normal, v) * manifold.normal + v_tangent := v - v_normal + + DYNAMIC_FRICTION :: 1 + v_tangent_len := lg.length(v_tangent) + if v_tangent_len > 0 { + v_tangent_norm := v_tangent / v_tangent_len + + w1, w2 := + get_body_inverse_mass(body1, v_tangent_norm, p1), + get_body_inverse_mass(body2, v_tangent_norm, p2) + + w := w1 + w2 + + if w > 0 { + delta_v := + -v_tangent_norm * + min( + dt * + DYNAMIC_FRICTION * + abs(pair.lambda_normal[point_idx] / (dt * dt)), + v_tangent_len / w, + ) + + // delta_v_norm := lg.normalize0(delta_v) + + p := delta_v + + body1.v += p * body1.inv_mass + body2.v -= p * body2.inv_mass + + body1.w += multiply_inv_intertia(body1, lg.cross(r1, p)) + body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p)) + } + } + } + } + } + + + // Solve suspension velocity + for _, i in sim_state.suspension_constraints { + v := &sim_state.suspension_constraints_slice[i] + if v.alive { + body := get_body(sim_state, v.body) + + if body.alive && v.hit { + 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) + + // Spring damping + if true { + vel := lg.dot(vel_3d, dir) + damping := -vel * min(v.damping * dt, 1) + + _ = apply_constraint_correction_unilateral( + dt, + body, + 0, + error = damping, + error_gradient = -dir, + pos = wheel_world_pos, + ) + + body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) + } + + // Drive forces + if true { + total_impulse := v.drive_impulse - v.brake_impulse + forward := body_local_to_world_vec(body, Vec3{0, 0, 1}) + + _ = apply_constraint_correction_unilateral( + dt, + body, + 0, + total_impulse * dt * dt, + -forward, + wheel_world_pos, + ) + body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) + } + + // Lateral friction + if true { + vel_contact := body_velocity_at_point(body, v.hit_point) + right := wheel_get_right_vec(body, v) + + lateral_vel := lg.dot(right, vel_contact) + + friction := f32(0.5) + impulse := -lateral_vel * friction + corr := right * impulse * dt + v.applied_impulse.x = impulse + + apply_correction(body, corr, v.hit_point) + body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) + } + } + } + } +} + +pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_dt: f32) { + for i in 0 ..< len(sim_state.bodies_slice) { + body := &sim_state.bodies_slice[i] + + if body.alive { + body.v += config.gravity * dt * (body.inv_mass == 0 ? 0 : 1) // special case for gravity, TODO + } + } + + // TODO: apply impulses + // 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) { + body := &sim_state.bodies_slice[i] + + if body.alive { + 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(scene: ^Scene, sim_state: ^Sim_State, config: Solver_Config) { tracy.Zone() substeps := config.substreps_minus_one + 1 @@ -336,391 +743,35 @@ simulate_step :: proc(sim_state: ^Sim_State, config: Solver_Config) { dt := config.timestep / f32(substeps) inv_dt := 1.0 / dt - for _ in 0 ..< substeps { - predict_positions(sim_state, config, dt) + resize_soa(&sim_state.contact_pairs, 0) - Body_Pair :: struct { - a, b: int, + tlas := build_tlas(sim_state, config) + potential_pairs := find_potential_pairs(sim_state, &tlas) + + Solver :: enum { + XPBD, + PGS, + } + + solver := Solver.PGS + + 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) - // 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) - - p_diff_normal := lg.dot(p2 - p1, manifold.normal) - separation := min(p_diff_normal, 0) - - lambda_norm, corr1, corr2, ok := calculate_constraint_params2( - dt, - body, - body2, - 0, - separation, - -manifold.normal, - p1, - p2, - ) - if ok { - contact_pair.applied_normal_correction[point_idx] = -separation - contact_pair.applied_corrections += 1 - contact_pair.lambda_normal[point_idx] = lambda_norm - - apply_correction(body, corr1, p1) - apply_correction(body2, corr2, p2) - } - } - } - } - - { - tracy.ZoneN("simulate_step::static_friction") - - when false { - context = context - context.user_ptr = sim_state - slice.sort_by( - sim_state.contact_pairs[:sim_state.contact_pairs_len], - proc(c1, c2: Contact_Pair) -> bool { - sim_state := cast(^Sim_State)context.user_ptr - - find_min_contact_y :: proc( - scene: ^Sim_State, - c: Contact_Pair, - ) -> ( - min_contact_y: f32, - ) { - min_contact_y = max(f32) - body_a, body_b := get_body(scene, c.a), get_body(scene, c.b) - for i in 0 ..< c.manifold.points_len { - min_contact_y = min( - body_local_to_world(body_a, c.manifold.points_a[i]).y, - body_local_to_world(body_b, c.manifold.points_b[i]).y, - ) - } - return - } - - 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 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 point_idx in 0 ..< manifold.points_len { - lambda_norm := contact_pair.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]) - prev_p1 := - body.prev_x + - lg.quaternion_mul_vector3(body.prev_q, manifold.points_a[point_idx]) - prev_p2 := - 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 - - tangent_diff_len := lg.length(p_diff_tangent) - - if tangent_diff_len > 0 { - tangent_diff_normalized := p_diff_tangent / tangent_diff_len - - delta_lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent := - calculate_constraint_params2( - dt, - body, - body2, - 0, - -tangent_diff_len / - max(f32(contact_pair.applied_corrections) * 0.5, 1), - -tangent_diff_normalized, - p1, - p2, - ) - - STATIC_FRICTION :: 0.6 - 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 - - apply_correction(body, corr1_tangent, p1) - apply_correction(body2, corr2_tangent, p2) - } - } - } - } - } - } - - { - tracy.ZoneN("simulate_step::suspension_constraints") - - for &v in sim_state.suspension_constraints { - if v.alive { - 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) - pos2 := pos + dir * v.rest - v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane( - {pos, pos2}, - collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}), - ) - if v.hit { - corr := pos - v.hit_point - distance := lg.length(corr) - corr = corr / distance if distance > 0 else 0 - - _ = apply_constraint_correction_unilateral( - dt, - body, - v.compliance, - error = distance - v.rest, - error_gradient = corr, - pos = pos, - other_combined_inv_mass = 0, - ) - } - } - } - } - - { - // 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.prev_x, body.prev_q, inv_dt) - } - } - } - - // Restituion - if true { - tracy.ZoneN("simulate_step::restitution") - - for &pair in sim_state.contact_pairs { - manifold := &pair.manifold - - body, body2 := get_body(sim_state, pair.a), get_body(sim_state, pair.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 { - 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]) - - 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 := - (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) - v_normal := lg.dot(v, manifold.normal) - - RESTITUTION :: 0 - - restitution := f32(RESTITUTION) - - if abs(v_normal) <= 2 * abs(lg.dot(manifold.normal, -config.gravity) * dt) { - restitution = 0 - } - - delta_v := manifold.normal * (-v_normal + min(-RESTITUTION * prev_v_normal, 0)) - - w1 := get_body_inverse_mass(body, manifold.normal, r1 + body.x) - w2 := get_body_inverse_mass(body2, manifold.normal, r2 + body2.x) - w := w1 + w2 - - if w != 0 { - p := delta_v / w - - body.v += p * body.inv_mass - body2.v -= p * body2.inv_mass - - body.w += multiply_inv_intertia(body, lg.cross(r1, p)) - body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p)) - } - } - } - } - - 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 point_idx in 0 ..< pair.manifold.points_len { - if pair.applied_static_friction[point_idx] { - continue - } - p1, p2 := - body_local_to_world(body1, manifold.points_a[point_idx]), - body_local_to_world(body2, manifold.points_b[point_idx]) - r1, r2 := p1 - body1.x, p2 - body2.x - v1 := body_velocity_at_point(body1, p1) - v2 := body_velocity_at_point(body2, p2) - - v := v1 - v2 - v_normal := lg.dot(manifold.normal, v) * manifold.normal - v_tangent := v - v_normal - - DYNAMIC_FRICTION :: 0.3 - v_tangent_len := lg.length(v_tangent) - if v_tangent_len > 0 { - v_tangent_norm := v_tangent / v_tangent_len - - w1, w2 := - get_body_inverse_mass(body1, v_tangent_norm, p1), - get_body_inverse_mass(body2, v_tangent_norm, p2) - - w := w1 + w2 - - if w > 0 { - delta_v := - -v_tangent_norm * - min( - dt * - DYNAMIC_FRICTION * - abs(pair.lambda_normal[point_idx] / (dt * dt)), - v_tangent_len / w, - ) - - // delta_v_norm := lg.normalize0(delta_v) - - p := delta_v - - body1.v += p * body1.inv_mass - body2.v -= p * body2.inv_mass - - body1.w += multiply_inv_intertia(body1, lg.cross(r1, p)) - body2.w -= multiply_inv_intertia(body2, lg.cross(r2, p)) - } - } - } - } - } - - - // Solve suspension velocity - for _, i in sim_state.suspension_constraints { - v := &sim_state.suspension_constraints_slice[i] - if v.alive { - body := get_body(sim_state, v.body) - - if body.alive && v.hit { - 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) - - // Spring damping - if true { - vel := lg.dot(vel_3d, dir) - damping := -vel * min(v.damping * dt, 1) - - _ = apply_constraint_correction_unilateral( - dt, - body, - 0, - error = damping, - error_gradient = -dir, - pos = wheel_world_pos, - ) - - body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) - } - - // Drive forces - if true { - total_impulse := v.drive_impulse - v.brake_impulse - forward := body_local_to_world_vec(body, Vec3{0, 0, 1}) - - _ = apply_constraint_correction_unilateral( - dt, - body, - 0, - total_impulse * dt * dt, - -forward, - wheel_world_pos, - ) - body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) - } - - // Lateral friction - if true { - vel_contact := body_velocity_at_point(body, v.hit_point) - right := wheel_get_right_vec(body, v) - - lateral_vel := lg.dot(right, vel_contact) - - friction := f32(0.5) - impulse := -lateral_vel * friction - corr := right * impulse * dt - v.applied_impulse.x = impulse - - apply_correction(body, corr, v.hit_point) - body_solve_velocity(body, body.prev_x, body.prev_q, inv_dt) - } - } - } + for _ in 0 ..< substeps { + pgs_substep(sim_state, config, dt, inv_dt) } } -} -solve_velocities :: proc(scene: ^Sim_State, body_states: []Body_Sim_State, inv_dt: f32) { } body_solve_velocity :: #force_inline proc(