From 196bafb4012c47d4cc59f7874c8ce1c56622826b Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 26 Jan 2025 22:43:23 +0400 Subject: [PATCH] Static and dynamic friction working, it's starting to look pretty convincing Still got a bug when colliding with edges at some specific rotations. Probably fucked relative position conversion somewhere and using a local pos instead --- game/game.odin | 4 +- game/physics/collision/convex.odin | 2 +- game/physics/debug.odin | 33 ++++++++----- game/physics/helpers.odin | 14 +----- game/physics/simulation.odin | 79 +++++++++++++++++++++++++----- 5 files changed, 94 insertions(+), 38 deletions(-) diff --git a/game/game.odin b/game/game.odin index 5a8708c..5306012 100644 --- a/game/game.odin +++ b/game/game.odin @@ -370,8 +370,8 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { drive_wheels := []physics.Suspension_Constraint_Handle{wheel_rl, wheel_rr} turn_wheels := []physics.Suspension_Constraint_Handle{wheel_fl, wheel_fr} - DRIVE_IMPULSE :: 20 - BRAKE_IMPULSE :: 50 + DRIVE_IMPULSE :: 5 + BRAKE_IMPULSE :: 20 TURN_ANGLE :: -f32(30) * math.RAD_PER_DEG for wheel_handle in drive_wheels { diff --git a/game/physics/collision/convex.odin b/game/physics/collision/convex.odin index c797790..6f1e3c2 100644 --- a/game/physics/collision/convex.odin +++ b/game/physics/collision/convex.odin @@ -531,7 +531,7 @@ create_edge_contact_manifold :: proc( _, ps := closest_point_between_segments(a1, a2, b1, b2) - manifold.normal = lg.normalize0(ps[1] - ps[0]) + manifold.normal = separating_plane.normal manifold.separation = separation manifold.points_a[0] = ps[0] manifold.points_b[0] = ps[1] diff --git a/game/physics/debug.odin b/game/physics/debug.odin index b943eb6..97fa0f3 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -99,17 +99,28 @@ draw_debug_scene :: proc(scene: ^Scene) { } } - for &contact, contact_idx in scene.contact_pairs[:scene.contact_pairs_len] { - debug_draw_manifold_points( - -contact.manifold.normal, - contact.manifold.points_a[:contact.manifold.points_len], - color = debug.int_to_color(i32(contact_idx * 2 + 0)), - ) - debug_draw_manifold_points( - contact.manifold.normal, - contact.manifold.points_b[:contact.manifold.points_len], - color = debug.int_to_color(i32(contact_idx * 2 + 1)), - ) + if false { + for &contact, contact_idx in scene.contact_pairs[:scene.contact_pairs_len] { + points_a := contact.manifold.points_a[:contact.manifold.points_len] + points_b := contact.manifold.points_b[:contact.manifold.points_len] + debug_transform_points_local_to_world(get_body(scene, contact.a), points_a) + debug_draw_manifold_points( + -contact.manifold.normal, + contact.manifold.points_a[:contact.manifold.points_len], + color = debug.int_to_color(i32(contact_idx * 2 + 0)), + ) + debug_draw_manifold_points( + contact.manifold.normal, + points_b, + color = debug.int_to_color(i32(contact_idx * 2 + 1)), + ) + } + } +} + +debug_transform_points_local_to_world :: proc(body: Body_Ptr, points: []rl.Vector3) { + for i in 0 ..< len(points) { + points[i] = body_local_to_world(body, points[i]) } } diff --git a/game/physics/helpers.odin b/game/physics/helpers.odin index d28fe2b..7181ac5 100644 --- a/game/physics/helpers.odin +++ b/game/physics/helpers.odin @@ -50,18 +50,8 @@ body_world_to_local_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) - return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec)) } -body_angular_velocity_at_local_point :: #force_inline proc( - body: Body_Ptr, - rel_pos: rl.Vector3, -) -> rl.Vector3 { - return lg.cross(body.w, rel_pos) -} - -body_velocity_at_local_point :: #force_inline proc( - body: Body_Ptr, - rel_pos: rl.Vector3, -) -> rl.Vector3 { - return body.v + body_angular_velocity_at_local_point(body, rel_pos) +body_velocity_at_point :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 { + return body.v + lg.cross(body.w, pos - body.x) } wheel_get_rel_wheel_pos :: #force_inline proc( diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 04f4137..6c64534 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -76,9 +76,10 @@ GLOBAL_PLANE :: collision.Plane { } Contact_Pair :: struct { - a, b: Body_Handle, - manifold: collision.Contact_Manifold, - lambdas: [4]f32, + a, b: Body_Handle, + manifold: collision.Contact_Manifold, + lambda_normal: [4]f32, + lambda_tangent: [4]f32, } simulate_step :: proc(scene: ^Scene, config: Solver_Config) { @@ -147,16 +148,18 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { halfedge.transform_mesh(&box1, mat1) halfedge.transform_mesh(&box2, mat2) - manifold, collision := collision.convex_vs_convex_sat(box1, box2) + // Raw manifold has contact points in world space + raw_manifold, collision := collision.convex_vs_convex_sat(box1, box2) if collision { contact_pair := &scene.contact_pairs[scene.contact_pairs_len] contact_pair^ = Contact_Pair { a = Body_Handle(i + 1), b = Body_Handle(j + 1), - manifold = manifold, + manifold = raw_manifold, } scene.contact_pairs_len += 1 + manifold := &contact_pair.manifold // Convert manifold contact from world to local space for point_idx in 0 ..< manifold.points_len { @@ -189,7 +192,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { p1, p2, ) - contact_pair.lambdas[point_idx] = lambda_norm + contact_pair.lambda_normal[point_idx] = lambda_norm if ok { apply_correction(body, corr1, p1) @@ -234,8 +237,9 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { p1, p2, ) + contact_pair.lambda_tangent[point_idx] = lambda_tangent - STATIC_FRICTION :: 2 + STATIC_FRICTION :: 0.5 if ok_tangent && lambda_tangent < lambda_norm * STATIC_FRICTION { apply_correction(body, corr1_tangent, p1) @@ -281,9 +285,50 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { solve_velocities(scene, body_states, inv_dt) - // for pair in scene.contact_pairs[:scene.contact_pairs_len] { - // pair.a - // } + for &pair in scene.contact_pairs[:scene.contact_pairs_len] { + manifold := &pair.manifold + body1 := get_body(scene, pair.a) + body2 := get_body(scene, pair.b) + + for point_idx in 0 ..< pair.manifold.points_len { + 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.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, + ) + + w1, w2 := + get_body_inverse_mass(body1, manifold.normal, p1), + get_body_inverse_mass(body2, manifold.normal, p2) + + w := w1 + w2 + if w != 0 { + p := delta_v / w + + body1.v += p * body1.inv_mass + body2.v -= p * body2.inv_mass + + body1.w += multiply_inv_mass(body1, lg.cross(r1, p)) + body2.w -= multiply_inv_mass(body2, lg.cross(r2, p)) + } + } + } + } // Solve suspension velocity for _, i in scene.suspension_constraints { @@ -337,8 +382,7 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { // Lateral friction if true { - local_contact_pos := v.hit_point - body.x - vel_contact := body_velocity_at_local_point(body, local_contact_pos) + vel_contact := body_velocity_at_point(body, v.hit_point) right := wheel_get_right_vec(body, v) lateral_vel := lg.dot(right, vel_contact) @@ -479,6 +523,17 @@ apply_constraint_correction_unilateral :: proc( return } +multiply_inv_mass :: proc(body: Body_Ptr, vec: rl.Vector3) -> (result: rl.Vector3) { + q := body.q + inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) + + result = lg.quaternion_mul_vector3(inv_q, vec) + result *= body.inv_inertia_tensor + result = lg.quaternion_mul_vector3(q, result) + + return result +} + apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) { body.x += corr * body.inv_mass