From 88fac29a6c53cf09bbdc4b6358d9a5b2eeb6f2b8 Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Tue, 29 Jul 2025 01:57:47 +0400 Subject: [PATCH] Fix suspension when driving on other rigid bodies, super solid now The key was to limit the total spring impulse similar to how contact impulse is limited, so that spring always pushes and never pulls --- game/game.odin | 10 +-- game/physics/scene.odin | 7 +- game/physics/simulation.odin | 142 +++++++++++++++++++++++++++-------- 3 files changed, 122 insertions(+), 37 deletions(-) diff --git a/game/game.odin b/game/game.odin index 196ecde..450bbea 100644 --- a/game/game.odin +++ b/game/game.odin @@ -557,7 +557,7 @@ update_world :: proc(world: ^World, dt: f32, config: World_Update_Config) { u32(box_name), physics.Body_Config { name = box_name, - initial_pos = {-5, 0.5 + f32(y) * 1.1, f32(x) * 3 + 10}, + initial_pos = {-5 + f32(y) * 1.01, 1, f32(x) * 1.01 + -11.5}, initial_rot = linalg.QUATERNIONF32_IDENTITY, shapes = { { @@ -598,8 +598,8 @@ update_world :: proc(world: ^World, dt: f32, config: World_Update_Config) { wheel_extent_x_back := f32(2) / 2 wheel_y := f32(0.5) rest := f32(0.7) - natural_frequency := f32(0.4) - damping := f32(0.06) + natural_frequency := f32(1.2) + damping := f32(0.4) radius := f32(0.737649) / 2 wheel_front_z := f32(1.6) wheel_back_z := f32(-1.63) @@ -818,9 +818,9 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { cur_world := runtime_world_current_world(runtime_world) runtime_world.dt = dt - should_single_step := rl.IsKeyPressed(.PERIOD) + should_single_step := g_mem.physics_pause // rl.IsKeyPressed(.PERIOD) runtime_world.rewind_simulation = rl.IsKeyPressed(.COMMA) - runtime_world.commit_simulation = !g_mem.physics_pause || should_single_step + runtime_world.commit_simulation = !g_mem.physics_pause || rl.IsKeyPressed(.PERIOD) runtime_world.single_step_simulation = should_single_step if !runtime_world.rewind_simulation { diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 9c8a819..5269677 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -288,16 +288,21 @@ Suspension_Constraint :: struct { lateral_impulse: f32, longitudinal_impulse: f32, brake_friction_impulse: f32, - + inv_mass: f32, // Inverse inertia along wheel rotation axis inv_inertia: f32, + // Wheel pos along suspension dir + x: f32, // Rotation q: f32, + // Linear velocity along suspension dir + v: f32, // Angular velocity w: f32, hit: bool, hit_normal: Vec3, + hit_body: Body_Handle, hit_point: Vec3, // rel_hit_point = rel_pos + rel_dir * hit_t hit_t: f32, diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 281a94b..2f4f6ce 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -247,6 +247,7 @@ raycast_bodies :: proc( ) -> ( t: f32, normal: Vec3, + body_handle: Body_Handle, hit: bool, ) { temp := runtime.default_temp_allocator_temp_begin() @@ -263,8 +264,9 @@ raycast_bodies :: proc( for leaf_node in bvh.iterator_intersect_leaf_next(&it) { for j in 0 ..< leaf_node.prim_len { body_idx := tlas.bvh_tree.primitives[leaf_node.child_or_prim_start + j] + tmp_body_handle := index_to_body_handle(int(body_idx)) - body := get_body(sim_state, index_to_body_handle(int(body_idx))) + body := get_body(sim_state, tmp_body_handle) it := shapes_iterator(sim_state, body.shape) for shape in shapes_iterator_next(&it) { @@ -285,12 +287,17 @@ raycast_bodies :: proc( if ok && (!hit || hit_t < t) { t = hit_t normal = tmp_normal + body_handle = tmp_body_handle hit = true } } } } + if hit { + normal = lg.normalize0(normal) + } + return } @@ -304,12 +311,13 @@ raycast :: proc( ) -> ( t: f32, normal: Vec3, + hit_body: Body_Handle, hit: bool, ) { t = distance t1, normal1, hit1 := raycasts_level(sim_state, sim_cache, static_tlas, origin, dir, t) - t2, normal2, hit2 := raycast_bodies(sim_state, dyn_tlas, origin, dir, t) + t2, normal2, body, hit2 := raycast_bodies(sim_state, dyn_tlas, origin, dir, t) hit = hit1 || hit2 @@ -320,6 +328,7 @@ raycast :: proc( } else { t = t2 normal = normal2 + hit_body = body } } else if hit1 { t = t1 @@ -327,11 +336,9 @@ raycast :: proc( } else if hit2 { t = t2 normal = normal2 + hit_body = body } - // TODO: raycast_level and raycast_bodies should return a normalized vec - normal = lg.normalize0(normal) - return } @@ -1267,18 +1274,31 @@ pgs_solve_engines :: proc(sim_state: ^Sim_State, config: Solver_Config, dt: f32, calculate_ground_vel :: proc( body: Body_Ptr, + hit_body: Body_Ptr, p: Vec3, right, forward: Vec3, ) -> ( body_vel_at_contact_patch: Vec3, ground_vel: Vec2, ) { - body_vel_at_contact_patch = body_velocity_at_point(body, p) + body_vel_at_contact_patch = + body_velocity_at_point(body, p) - body_velocity_at_point(hit_body, p) ground_vel.x = lg.dot(body_vel_at_contact_patch, right) ground_vel.y = lg.dot(body_vel_at_contact_patch, forward) return } +combine_inv_mass :: proc(w1, w2: f32) -> f32 { + if w1 > 0 && w2 > 0 { + return (w1 * w2) / (w1 + w2) + } + if w1 > 0 { + return w1 + } + + return w2 +} + pgs_solve_suspension :: proc( sim_state: ^Sim_State, sim_cache: ^Sim_Cache, @@ -1287,6 +1307,7 @@ pgs_solve_suspension :: proc( config: Solver_Config, dt: f32, inv_dt: f32, + apply_bias: bool, ) { // Solve suspension velocity for _, i in sim_state.suspension_constraints { @@ -1297,7 +1318,8 @@ pgs_solve_suspension :: proc( if body.alive { wheel_world_pos := body_local_to_world(body, v.rel_pos + body.shape_offset) dir := body_local_to_world_vec(body, v.rel_dir) - v.hit_t, v.hit_normal, v.hit = raycast( + prev_hit_body := v.hit_body + v.hit_t, v.hit_normal, v.hit_body, v.hit = raycast( sim_state, sim_cache, static_tlas, @@ -1306,13 +1328,12 @@ pgs_solve_suspension :: proc( dir, v.rest, ) - // log.debugf("hit_t: %v, hit: %v", v.hit_t, v.hit) + hit_body := get_body(sim_state, v.hit_body) v.hit_point = wheel_world_pos + dir * v.hit_t - forward := wheel_get_forward_vec(body, v) - right := wheel_get_right_vec(body, v) - - w_normal := get_body_angular_inverse_mass(body, -v.hit_normal) + w_normal1 := get_body_inverse_mass(body, -dir, wheel_world_pos) + w_normal2 := get_body_inverse_mass(hit_body, dir, v.hit_point) + w_normal := combine_inv_mass(w_normal1, w_normal2) inv_w_normal := 1.0 / w_normal // Drive force @@ -1347,7 +1368,17 @@ pgs_solve_suspension :: proc( } } + if !v.hit || v.hit_body != prev_hit_body { + v.lateral_impulse = 0 + v.spring_impulse = 0 + v.longitudinal_impulse = 0 + } + if v.hit { + forward := wheel_get_forward_vec(body, v) + right := lg.normalize0(lg.cross(forward, v.hit_normal)) + forward = lg.normalize0(lg.cross(v.hit_normal, right)) + // Spring force { @@ -1356,21 +1387,30 @@ pgs_solve_suspension :: proc( f64(v.damping), f64(dt), ) + if !apply_bias { + mass_coef = 1 + bias_coef = 0 + impulse_coef = 0 + } - vel := lg.dot(body_velocity_at_point(body, v.hit_point), -v.hit_normal) + vel := lg.dot( + body_velocity_at_point(body, v.hit_point) - + body_velocity_at_point(hit_body, v.hit_point), + dir, + ) x := v.hit_t separation := v.rest - x + // spring_dot_normal := abs(lg.dot(dir, v.hit_normal)) incremental_impulse := -inv_w_normal * mass_coef * (vel + separation * bias_coef) - impulse_coef * v.spring_impulse - v.spring_impulse += incremental_impulse + new_spring_impulse := min(0, v.spring_impulse + incremental_impulse) + applied_impulse := new_spring_impulse - v.spring_impulse + v.spring_impulse = new_spring_impulse - apply_velocity_correction( - body, - incremental_impulse * v.hit_normal, - v.hit_point, - ) + apply_velocity_correction(body, applied_impulse * dir, wheel_world_pos) + apply_velocity_correction(hit_body, applied_impulse * -dir, v.hit_point) } // Positive means spinning forward @@ -1378,6 +1418,7 @@ pgs_solve_suspension :: proc( body_vel_at_contact_patch, ground_vel := calculate_ground_vel( body, + hit_body, v.hit_point, right, forward, @@ -1432,6 +1473,7 @@ pgs_solve_suspension :: proc( if true { body_vel_at_contact_patch, ground_vel = calculate_ground_vel( body, + hit_body, v.hit_point, right, forward, @@ -1442,9 +1484,11 @@ pgs_solve_suspension :: proc( friction_clamp := abs(v.spring_impulse) * long_friction - w_body := get_body_inverse_mass(body, forward, v.hit_point) + w_body1 := get_body_inverse_mass(body, forward, v.hit_point) + w_body2 := get_body_inverse_mass(hit_body, forward, v.hit_point) + w_body := w_body1 + v.inv_inertia + w_body2 - w_long := w_body + v.inv_inertia + w_long := w_body inv_w_long := 1.0 / w_long incremental_impulse := -inv_w_long * relative_vel @@ -1457,12 +1501,18 @@ pgs_solve_suspension :: proc( v.longitudinal_impulse = new_total_impulse apply_velocity_correction(body, applied_impulse * forward, v.hit_point) + apply_velocity_correction( + hit_body, + -applied_impulse * forward, + v.hit_point, + ) v.w += v.inv_inertia * applied_impulse } // Lateral friction if true { body_vel_at_contact_patch, ground_vel = calculate_ground_vel( body, + hit_body, v.hit_point, right, forward, @@ -1470,7 +1520,14 @@ pgs_solve_suspension :: proc( lateral_vel := ground_vel.x friction_clamp := abs(v.spring_impulse) * lat_friction - incremental_impulse := -inv_w_normal * lateral_vel + w_body1 := get_body_inverse_mass(body, right, v.hit_point) + w_body2 := get_body_inverse_mass(hit_body, right, v.hit_point) + w_body := w_body1 + w_body2 + + w_lat := w_body + inv_w_lat := 1.0 / w_lat + + incremental_impulse := -inv_w_lat * lateral_vel new_total_impulse := clamp( v.lateral_impulse + incremental_impulse, -friction_clamp, @@ -1480,11 +1537,8 @@ pgs_solve_suspension :: proc( v.lateral_impulse = new_total_impulse apply_velocity_correction(body, applied_impulse * right, v.hit_point) + apply_velocity_correction(hit_body, -applied_impulse * right, v.hit_point) } - } else { - v.lateral_impulse = 0 - v.spring_impulse = 0 - v.longitudinal_impulse = 0 } } } @@ -1589,18 +1643,25 @@ pgs_substep :: proc( if s.hit { body := get_body(sim_state, s.body) + hit_body := get_body(sim_state, s.hit_body) p := body_local_to_world(body, s.rel_pos + body.shape_offset) + dir := body_local_to_world_vec(body, s.rel_dir) hit_p := body_local_to_world( body, s.rel_pos + body.shape_offset + s.rel_dir * s.hit_t, ) forward := wheel_get_forward_vec(body, s) - right := wheel_get_right_vec(body, s) + right := lg.normalize0(lg.cross(forward, s.hit_normal)) + forward = lg.normalize0(lg.cross(s.hit_normal, right)) - apply_velocity_correction(body, s.spring_impulse * -s.hit_normal, hit_p) - apply_velocity_correction(body, s.lateral_impulse * right, p) + apply_velocity_correction(body, s.spring_impulse * dir, p) + apply_velocity_correction(hit_body, s.spring_impulse * -dir, hit_p) + + apply_velocity_correction(body, s.lateral_impulse * right, hit_p) + apply_velocity_correction(hit_body, -s.lateral_impulse * right, hit_p) apply_velocity_correction(body, s.longitudinal_impulse * forward, hit_p) + apply_velocity_correction(hit_body, -s.longitudinal_impulse * forward, hit_p) s.w += s.inv_inertia * s.longitudinal_impulse } @@ -1611,7 +1672,16 @@ pgs_substep :: proc( apply_bias := true pgs_solve_contacts(sim_state, config, dt, inv_dt, apply_bias) pgs_solve_engines(sim_state, config, dt, inv_dt) - pgs_solve_suspension(sim_state, sim_cache, static_tlas, dyn_tlas, config, dt, inv_dt) + pgs_solve_suspension( + sim_state, + sim_cache, + static_tlas, + dyn_tlas, + config, + dt, + inv_dt, + apply_bias, + ) for i in 0 ..< len(sim_state.bodies_slice) { body := &sim_state.bodies_slice[i] @@ -1644,12 +1714,22 @@ pgs_substep :: proc( for i in 0 ..< len(sim_state.suspension_constraints_slice) { s := &sim_state.suspension_constraints_slice[i] + s.x += s.v * dt 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) + // pgs_solve_suspension( + // sim_state, + // sim_cache, + // static_tlas, + // dyn_tlas, + // config, + // dt, + // inv_dt, + // apply_bias, + // ) } simulate_step :: proc(