From 9f6d57b5ea092336c085500703e36b581bc1139a Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 2 Feb 2025 21:45:25 +0400 Subject: [PATCH] Fix dynamic friction (mistake in paper actually), improve friction --- assets/car_convex.obj | 114 ++++++++----------- game/game.odin | 6 +- game/physics/simulation.odin | 215 ++++++++++++++++++++++------------- src_assets/car_convex.blend | 4 +- src_assets/car_convex.blend1 | 4 +- 5 files changed, 189 insertions(+), 154 deletions(-) diff --git a/assets/car_convex.obj b/assets/car_convex.obj index e6b6f67..0733656 100644 --- a/assets/car_convex.obj +++ b/assets/car_convex.obj @@ -1,80 +1,60 @@ # Blender 4.3.2 # www.blender.org o Object_5 -v -1.841497 0.441915 2.882739 -v -1.528260 -0.439969 4.246670 +v -1.859343 0.407766 2.882677 +v -1.496358 -0.439898 4.246497 v -1.532271 -0.014560 4.306011 -v -1.458986 0.585761 4.084804 -v -1.169056 1.845618 -0.044352 -v -1.794738 0.759021 -2.991723 -v -1.751396 -0.067717 -3.937637 -v 1.598544 -0.437865 4.228289 -v -1.355442 0.205843 4.443853 -v -1.495447 -0.513379 -3.072723 -v -1.081991 -0.669625 2.507290 -v 1.152275 -0.667619 2.498550 -v 0.035142 0.587956 4.202047 -v 0.035142 1.889431 -0.088813 -v 0.035142 1.953676 -2.216545 -v -1.179023 1.913049 -2.281286 -v -1.418515 1.165435 -4.711776 -v -1.589515 0.055308 -5.153280 -v 1.558805 0.203133 4.425279 -v 1.565731 -0.511086 -3.071773 -v 1.821680 -0.067717 -3.937637 -v 1.911781 0.441916 2.882739 -v 0.035142 -0.194318 -4.868722 -v 1.529270 0.585761 4.084804 -v 1.239340 1.845618 -0.044352 -v 1.249308 1.913049 -2.281285 -v 1.488799 1.165434 -4.711775 -v 0.035142 1.168489 -4.782736 -v 0.035142 0.107657 -5.212766 -v 1.659799 0.055307 -5.153279 -v 1.865023 0.759021 -2.991723 +v -1.458925 0.588598 4.098169 +v -1.169193 1.844997 -0.020979 +v -1.767808 0.808841 -2.967643 +v -1.755688 0.019238 -3.954955 +v 1.566465 -0.437912 4.228478 +v -1.355442 0.201413 4.442485 +v -1.527377 -0.513372 -3.070427 +v -1.081991 -0.669592 2.507292 +v 1.152275 -0.667651 2.498548 +v -1.177522 1.915790 -2.273865 +v -1.440784 1.165512 -4.710642 +v -1.567565 0.052245 -5.155589 +v 1.558806 0.207560 4.426651 +v 1.597780 -0.511147 -3.072741 +v 1.825968 0.019292 -3.954931 +v 1.929686 0.407656 2.882679 +v 1.529256 0.593033 4.083607 +v 1.239481 1.844996 -0.020995 +v 1.247809 1.915789 -2.273863 +v 1.511068 1.165512 -4.710640 +v 1.637697 0.053622 -5.155048 +v 1.838094 0.808837 -2.967645 s 0 f 1 2 3 f 1 3 4 f 1 4 5 -f 31 26 25 22 +f 25 22 21 19 f 1 6 7 -f 1 7 2 +f 18 19 8 17 f 2 9 3 -f 2 7 10 f 2 10 11 -f 20 12 11 10 +f 17 12 11 10 f 3 9 4 -f 4 9 13 -f 24 25 14 13 -f 25 26 15 14 -f 6 16 17 -f 6 17 18 -f 6 18 7 -f 7 18 10 -f 8 12 20 -f 8 20 21 -f 8 21 22 -f 8 22 19 -f 9 19 13 -f 10 18 23 -f 10 23 20 -f 8 19 9 2 -f 13 19 24 -f 15 16 5 14 -f 27 28 15 26 -f 17 16 15 28 -f 29 18 17 28 -f 29 28 27 30 -f 18 29 30 -f 18 30 23 -f 19 22 24 -f 20 23 30 -f 20 30 21 -f 21 31 22 -f 21 30 31 -f 22 25 24 -f 26 31 27 +f 9 16 20 4 +f 6 13 14 +f 23 25 18 24 +f 7 15 10 +f 8 12 17 +f 8 19 16 +f 8 16 9 2 +f 14 23 24 15 +f 22 13 5 21 +f 17 10 15 24 +f 16 19 20 +f 17 24 18 +f 18 25 19 +f 7 10 2 1 +f 19 21 20 +f 22 25 23 f 12 8 2 11 -f 27 31 30 -f 14 5 4 13 -f 5 16 6 1 +f 5 13 6 1 +f 5 4 20 21 +f 15 7 6 14 +f 14 13 22 23 diff --git a/game/game.odin b/game/game.odin index 9faf5af..26fc750 100644 --- a/game/game.odin +++ b/game/game.odin @@ -69,7 +69,7 @@ Car :: struct { SOLVER_CONFIG :: physics.Solver_Config { timestep = 1.0 / 120, gravity = rl.Vector3{0, -9.8, 0}, - substreps_minus_one = 1 - 1, + substreps_minus_one = 2 - 1, } Game_Memory :: struct { @@ -277,9 +277,9 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { &runtime_world.solver_state, hash.fnv32a(slice.to_bytes([]int{(x | y << 8)})), physics.Body_Config { - initial_pos = {f32(x) * 2, 5, f32(y) * 2}, + initial_pos = {f32(x) * 3 + 20, 5 + f32(y) * 2, 0}, initial_rot = linalg.QUATERNIONF32_IDENTITY, - shape = physics.Shape_Box{size = 1.5}, + shape = physics.Shape_Box{size = 1}, mass = 10, }, ) diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index ea21402..a1683c5 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -4,6 +4,7 @@ import "collision" import "core:fmt" import "core:math" import lg "core:math/linalg" +import "core:slice" import "libs:tracy" import rl "vendor:raylib" @@ -76,10 +77,13 @@ GLOBAL_PLANE :: collision.Plane { } Contact_Pair :: struct { - a, b: Body_Handle, - manifold: collision.Contact_Manifold, - lambda_normal: [4]f32, - lambda_tangent: [4]f32, + a, b: Body_Handle, + prev_x_a, prev_x_b: rl.Vector3, + prev_q_a, prev_q_b: rl.Quaternion, + manifold: collision.Contact_Manifold, + applied_corrections: int, + lambda_normal: [4]f32, + lambda_tangent: [4]f32, } simulate_step :: proc(scene: ^Scene, config: Solver_Config) { @@ -150,6 +154,10 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { 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, } scene.contact_pairs_len += 1 @@ -173,7 +181,8 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { body_local_to_world(body, p1), body_local_to_world(body2, p2) - separation := min(lg.dot(p2 - p1, manifold.normal), 0) + 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 @@ -187,67 +196,13 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { p1, p2, ) - contact_pair.lambda_normal[point_idx] = lambda_norm - if ok { + contact_pair.applied_corrections += 1 + contact_pair.lambda_normal[point_idx] = lambda_norm + apply_correction(body, corr1, p1) apply_correction(body2, corr2, p2) } - - // Static friction - if ok { - p1, p2 = - body_local_to_world( - body, - manifold.points_a[point_idx], - ), - 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], - ) - prev_p2 := - body_states[j].prev_x + - lg.quaternion_mul_vector3( - body_states[j].prev_q, - manifold.points_b[point_idx], - ) - - delta_p := (p2 - prev_p2) - (p1 - prev_p1) - delta_p_tangent_norm := - delta_p - - lg.dot(manifold.normal, delta_p) * manifold.normal - delta_p_tangent_len := lg.length(delta_p_tangent_norm) - - if delta_p_tangent_len > 0 { - delta_p_tangent_norm /= delta_p_tangent_len - - lambda_tangent, corr1_tangent, corr2_tangent, ok_tangent := - calculate_constraint_params2( - dt, - body, - body2, - 0, - -delta_p_tangent_len, - delta_p_tangent_norm, - p1, - p2, - ) - contact_pair.lambda_tangent[point_idx] = lambda_tangent - - STATIC_FRICTION :: 0.7 - if ok_tangent && - lambda_tangent < lambda_norm * STATIC_FRICTION { - apply_correction(body, corr1_tangent, p1) - apply_correction(body2, corr2_tangent, p2) - } - } - } } } } @@ -256,6 +211,103 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { } } + { + tracy.ZoneN("simulate_step::static_friction") + + if true { + + context = context + context.user_ptr = scene + slice.sort_by( + scene.contact_pairs[:scene.contact_pairs_len], + proc(c1, c2: Contact_Pair) -> bool { + scene := cast(^Scene)context.user_ptr + + find_min_contact_y :: proc( + scene: ^Scene, + 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(scene, c1) + min_y2 := find_min_contact_y(scene, c2) + + return min_y1 > min_y2 + }, + ) + } + + for &contact_pair in scene.contact_pairs[:scene.contact_pairs_len] { + manifold := contact_pair.manifold + body, body2 := get_body(scene, contact_pair.a), get_body(scene, contact_pair.b) + i, j := int(contact_pair.a) - 1, int(contact_pair.b) - 1 + + lambda_tangent: f32 + 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_states[i].prev_x + + lg.quaternion_mul_vector3( + body_states[i].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], + ) + + 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 / f32(contact_pair.applied_corrections), + -tangent_diff_normalized, + p1, + p2, + ) + contact_pair.lambda_tangent[point_idx] = lambda_tangent + + new_lambda_tangent := lambda_tangent + delta_lambda_tangent + + STATIC_FRICTION :: 0.7 + if ok_tangent && delta_lambda_tangent < STATIC_FRICTION * lambda_norm { + lambda_tangent = new_lambda_tangent + apply_correction(body, corr1_tangent, p1) + apply_correction(body2, corr2_tangent, p2) + } + } + } + } + } + } + { tracy.ZoneN("simulate_step::suspension_constraints") @@ -291,8 +343,8 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { solve_velocities(scene, body_states, inv_dt) - { - tracy.ZoneN("simulate_step::collision_velocities") + if true { + tracy.ZoneN("simulate_step::dynamic_friction") for &pair in scene.contact_pairs[:scene.contact_pairs_len] { manifold := &pair.manifold @@ -311,27 +363,30 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { v_normal := lg.dot(manifold.normal, v) * manifold.normal v_tangent := v - v_normal - DYNAMIC_FRICTION :: 0.2 + DYNAMIC_FRICTION :: 0.4 v_tangent_len := lg.length(v_tangent) if v_tangent_len > 0 { - delta_v := - -(v_tangent / v_tangent_len) * - min( - dt * - DYNAMIC_FRICTION * - abs(pair.lambda_normal[point_idx] / (dt * dt)), - v_tangent_len, - ) - - delta_v_norm := lg.normalize0(delta_v) + v_tangent_norm := v_tangent / v_tangent_len w1, w2 := - get_body_inverse_mass(body1, delta_v_norm, p1), - get_body_inverse_mass(body2, delta_v_norm, p2) + get_body_inverse_mass(body1, v_tangent_norm, p1), + get_body_inverse_mass(body2, v_tangent_norm, p2) w := w1 + w2 - if w != 0 { - p := delta_v / w + + 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 @@ -489,7 +544,7 @@ calculate_constraint_params2 :: proc( return } - w := get_body_inverse_mass(body1, error_gradient, pos1) + w := get_body_inverse_mass(body1, -error_gradient, pos1) w += get_body_inverse_mass(body2, error_gradient, pos2) if w == 0 { diff --git a/src_assets/car_convex.blend b/src_assets/car_convex.blend index a6e5701..a5d668f 100644 --- a/src_assets/car_convex.blend +++ b/src_assets/car_convex.blend @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3763a97f1176bcbe7b2ef9b3db200ead0fe4b27ae314d7866c67d9d616968186 -size 510084 +oid sha256:737f4939732c16701be956ba58f54dd632b31ebedd1555f17e3e0e70c00d81da +size 502668 diff --git a/src_assets/car_convex.blend1 b/src_assets/car_convex.blend1 index 9609249..395fa06 100644 --- a/src_assets/car_convex.blend1 +++ b/src_assets/car_convex.blend1 @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4762a0818fea68dbfe4d5a83fb8ca81481f113eafc8fc033d0066cff3525dbbe -size 1377343 +oid sha256:4ee7b9b556b122424340ce70aafc0904eea5afcc424df0afb60fc726b32d2b30 +size 502880