From bf7b7e6ae39fdb5eb048f895a9852f2b65803bb9 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 16 Mar 2025 14:53:13 +0400 Subject: [PATCH] Implement wheel rotation and friction --- game/game.odin | 5 ++++ game/physics/debug.odin | 20 +++++++++++--- game/physics/scene.odin | 53 ++++++++++++++++++++++-------------- game/physics/simulation.odin | 45 ++++++++++++++++++++++++++---- 4 files changed, 93 insertions(+), 30 deletions(-) diff --git a/game/game.odin b/game/game.odin index 9b6418f..95e43ac 100644 --- a/game/game.odin +++ b/game/game.odin @@ -325,6 +325,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { radius := f32(0.6) wheel_front_z := f32(3.05) wheel_back_z := f32(-2.45) + wheel_mass := f32(12) wheel_fl := physics.immediate_suspension_constraint( &world.physics_scene, @@ -338,6 +339,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { natural_frequency = natural_frequency, damping = damping, body = runtime_world.car_handle, + mass = wheel_mass, }, ) wheel_fr := physics.immediate_suspension_constraint( @@ -352,6 +354,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { natural_frequency = natural_frequency, damping = damping, body = runtime_world.car_handle, + mass = wheel_mass, }, ) wheel_rl := physics.immediate_suspension_constraint( @@ -366,6 +369,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { natural_frequency = natural_frequency, damping = damping, body = runtime_world.car_handle, + mass = wheel_mass, }, ) wheel_rr := physics.immediate_suspension_constraint( @@ -380,6 +384,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { natural_frequency = natural_frequency, damping = damping, body = runtime_world.car_handle, + mass = wheel_mass, }, ) diff --git a/game/physics/debug.odin b/game/physics/debug.odin index 28cca69..b4ff3a6 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -70,19 +70,31 @@ draw_debug_scene :: proc(scene: ^Scene) { wheel := &sim_state.suspension_constraints_slice[i] if wheel.alive { body := get_body(sim_state, wheel.body) - t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest pos := body.x rot := body.q pos += lg.quaternion_mul_vector3(rot, wheel.rel_pos) - dir := lg.quaternion_mul_vector3(rot, wheel.rel_dir) - - rl.DrawLine3D(pos, pos + dir * t, rl.ORANGE) rel_wheel_pos := wheel_get_rel_wheel_pos(body, wheel) wheel_pos := body_local_to_world(body, rel_wheel_pos) right := wheel_get_right_vec(body, wheel) + rotation_dir := body_local_to_world_vec( + body, + lg.matrix3_rotate(wheel.q, Vec3{-1, 0, 0}) * Vec3{0, 1, 0}, + ) + + rl.DrawLine3D( + wheel_pos - right * 0.1, + wheel_pos - right * 0.1 + rotation_dir * wheel.radius, + rl.ORANGE, + ) + rl.DrawLine3D( + wheel_pos + right * 0.1, + wheel_pos + right * 0.1 + rotation_dir * wheel.radius, + rl.ORANGE, + ) + rl.DrawCylinderWiresEx( wheel_pos - right * 0.1, wheel_pos + right * 0.1, diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 6265ebf..bf1fba0 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -120,38 +120,49 @@ Collision_Shape :: union { } Suspension_Constraint :: struct { - alive: bool, + alive: bool, // Pos relative to the body - rel_pos: Vec3, + rel_pos: Vec3, // Dir relative to the body - rel_dir: Vec3, + rel_dir: Vec3, // Handle of the rigid body - body: Body_Handle, + body: Body_Handle, // Wheel radius - radius: f32, + radius: f32, + // Wheel mass + mass: f32, // Rest distance - rest: f32, + rest: f32, // Frequency of the spring, affects stiffness - natural_frequency: f32, + natural_frequency: f32, // How much to damp velocity of the spring - damping: f32, - - // Accumulated impulse - spring_impulse: f32, - lateral_impulse: f32, + damping: f32, // Runtime state - hit: bool, - hit_point: Vec3, + + // Accumulated impulse + spring_impulse: f32, + lateral_impulse: f32, + longitudinal_impulse: f32, + + // Inverse inertia along wheel rotation axis + inv_inertia: f32, + + // Rotation + q: f32, + // Angular velocity + w: f32, + hit: bool, + hit_point: Vec3, // rel_hit_point = rel_pos + rel_dir * hit_t - hit_t: f32, - turn_angle: f32, - drive_impulse: f32, - brake_impulse: f32, - applied_impulse: Vec3, + hit_t: f32, + turn_angle: f32, + drive_impulse: f32, + brake_impulse: f32, + applied_impulse: Vec3, // Free list - next_plus_one: i32, + next_plus_one: i32, } // Index plus one, so handle 0 maps to invalid body @@ -248,6 +259,7 @@ Suspension_Constraint_Config :: struct { natural_frequency: f32, damping: f32, radius: f32, + mass: f32, } calculate_body_params_from_config :: proc( @@ -318,6 +330,7 @@ update_suspension_constraint_from_config :: proc( constraint.natural_frequency = config.natural_frequency constraint.damping = config.damping constraint.radius = config.radius + constraint.inv_inertia = 1.0 / (0.5 * config.mass * config.radius * config.radius) } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 59bdbf6..e7577bb 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -363,10 +363,10 @@ pgs_solve_contacts :: proc( 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(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) { @@ -488,6 +488,11 @@ pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f {wheel_world_pos, pos2}, collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}), ) + forward := body_local_to_world_vec(body, Vec3{0, 0, 1}) + + contact_patch_linear_vel := + body_velocity_at_point(body, v.hit_point) + (v.radius * v.w * forward) + w := get_body_angular_inverse_mass(body, dir) inv_w := 1.0 / w if v.hit { @@ -510,10 +515,30 @@ pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f apply_velocity_correction(body, incremental_impulse * dir, wheel_world_pos) } + right := wheel_get_right_vec(body, v) + + // Longitudinal friction + if true { + vel_long := lg.dot(contact_patch_linear_vel, forward) + + friction := f32(1) + friction_clamp := abs(v.spring_impulse) * friction + + incremental_impulse := -v.inv_inertia * vel_long + new_total_impulse := clamp( + v.longitudinal_impulse + incremental_impulse, + -friction_clamp, + friction_clamp, + ) + applied_impulse := new_total_impulse - v.longitudinal_impulse + v.longitudinal_impulse = new_total_impulse + + v.w += v.inv_inertia * applied_impulse + } + // Drive forces if true { total_impulse := v.drive_impulse - v.brake_impulse - forward := body_local_to_world_vec(body, Vec3{0, 0, 1}) apply_velocity_correction( body, @@ -525,7 +550,6 @@ pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f // Lateral friction if true { vel_contact := body_velocity_at_point(body, wheel_world_pos) - right := wheel_get_right_vec(body, v) lateral_vel := lg.dot(right, vel_contact) @@ -546,6 +570,7 @@ pgs_solve_suspension :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f } else { v.lateral_impulse = 0 v.spring_impulse = 0 + v.longitudinal_impulse = 0 } } } @@ -601,6 +626,8 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d p, ) apply_velocity_correction(body, s.lateral_impulse * right, p) + + s.w += s.inv_inertia * s.longitudinal_impulse } } } @@ -631,6 +658,12 @@ pgs_substep :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, inv_d } } + for i in 0 ..< len(sim_state.suspension_constraints_slice) { + s := &sim_state.suspension_constraints_slice[i] + + s.q = math.mod_f32(s.q + 0.5 * s.w * dt, math.PI * 2) + } + apply_bias = false pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias) // pgs_solve_suspension(sim_state, config, dt, inv_dt, apply_bias)