diff --git a/game/game.odin b/game/game.odin index b074a9f..5a8708c 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 { @@ -303,7 +303,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { // 1.6 is a good value wheel_extent_x := f32(2) - wheel_y := f32(-0.5) + wheel_y := f32(-1) rest := f32(1) suspension_stiffness := f32(2000) compliance := 1.0 / suspension_stiffness diff --git a/game/physics/debug.odin b/game/physics/debug.odin index 0f9d4c1..b943eb6 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -131,6 +131,6 @@ debug_draw_manifold_points :: proc(normal: rl.Vector3, points: []rl.Vector3, col for p in points { rl.DrawLine3D(p, p + normal, color) - rl.DrawSphereWires(p, len(points) == 1 ? 0.5 : 0.1, 4, 4, color) + rl.DrawSphereWires(p, 0.1, 4, 4, color) } } diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index 83dd182..04f4137 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -78,6 +78,7 @@ GLOBAL_PLANE :: collision.Plane { Contact_Pair :: struct { a, b: Body_Handle, manifold: collision.Contact_Manifold, + lambdas: [4]f32, } simulate_step :: proc(scene: ^Scene, config: Solver_Config) { @@ -149,59 +150,99 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { manifold, collision := collision.convex_vs_convex_sat(box1, box2) if collision { - scene.contact_pairs[scene.contact_pairs_len] = Contact_Pair { + 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, } scene.contact_pairs_len += 1 - points_a_local, points_b_local: [4]rl.Vector3 + // Convert manifold contact from world to local space for point_idx in 0 ..< manifold.points_len { - points_a_local[point_idx] = body_world_to_local( + manifold.points_a[point_idx] = body_world_to_local( body, manifold.points_a[point_idx], ) - points_b_local[point_idx] = body_world_to_local( + manifold.points_b[point_idx] = body_world_to_local( body2, manifold.points_b[point_idx], ) } for point_idx in 0 ..< manifold.points_len { - p1, p2 := points_a_local[point_idx], points_b_local[point_idx] + 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) - body1_inv_mass := get_body_inverse_mass(body, manifold.normal, p1) - body2_inv_mass := get_body_inverse_mass(body2, manifold.normal, p2) - diff := p2 - p1 - length := lg.length(diff) - if length != 0 { - diff /= length - } separation := min(lg.dot(p2 - p1, manifold.normal), 0) handled_pairs[{a = min(i, j), b = max(i, j)}] = true - apply_constraint_correction_unilateral( + lambda_norm, corr1, corr2, ok := calculate_constraint_params2( dt, body, - 0, - -separation, - -manifold.normal, - p1, - body2_inv_mass, - ) - - apply_constraint_correction_unilateral( - dt, body2, 0, - -separation, - manifold.normal, + separation, + -manifold.normal, + p1, p2, - body1_inv_mass, ) + contact_pair.lambdas[point_idx] = lambda_norm + + if ok { + 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, + ) + + STATIC_FRICTION :: 2 + if ok_tangent && + lambda_tangent < lambda_norm * STATIC_FRICTION { + apply_correction(body, corr1_tangent, p1) + apply_correction(body2, corr2_tangent, p2) + } + } + } } } } @@ -221,11 +262,11 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}), ) if v.hit { - corr := v.hit_point - pos + corr := pos - v.hit_point distance := lg.length(corr) corr = corr / distance if distance > 0 else 0 - apply_constraint_correction_unilateral( + _ = apply_constraint_correction_unilateral( dt, body, v.compliance, @@ -266,12 +307,12 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { vel := lg.dot(vel_3d, dir) damping := -vel * min(v.damping * dt, 1) - apply_constraint_correction_unilateral( + _ = apply_constraint_correction_unilateral( dt, body, 0, error = damping, - error_gradient = dir, + error_gradient = -dir, pos = wheel_world_pos, ) @@ -283,12 +324,12 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { total_impulse := v.drive_impulse - v.brake_impulse forward := body_local_to_world_vec(body, rl.Vector3{0, 0, 1}) - apply_constraint_correction_unilateral( + _ = apply_constraint_correction_unilateral( dt, body, 0, total_impulse * dt * dt, - forward, + -forward, wheel_world_pos, ) body_solve_velocity(body, body_state, inv_dt) @@ -337,6 +378,76 @@ body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State, } } +calculate_constraint_params1 :: proc( + dt: f32, + body: Body_Ptr, + compliance: f32, + error: f32, + error_gradient: rl.Vector3, + pos: rl.Vector3, + other_combined_inv_mass: f32, +) -> ( + lambda: f32, + w: f32, + correction: rl.Vector3, + ok: bool, +) { + if error == 0 { + return + } + + w = get_body_inverse_mass(body, error_gradient, pos) + w += other_combined_inv_mass + + if w == 0 { + return + } + + ok = true + + alpha := compliance / dt / dt + lambda = -error / (w + alpha) + correction = -error_gradient * -lambda + + return +} + +calculate_constraint_params2 :: proc( + dt: f32, + body1: Body_Ptr, + body2: Body_Ptr, + compliance: f32, + error: f32, + error_gradient: rl.Vector3, + pos1: rl.Vector3, + pos2: rl.Vector3, +) -> ( + lambda: f32, + correction1: rl.Vector3, + correction2: rl.Vector3, + ok: bool, +) { + if error == 0 { + return + } + + w := get_body_inverse_mass(body1, error_gradient, pos1) + w += get_body_inverse_mass(body2, error_gradient, pos2) + + if w == 0 { + return + } + + ok = true + + alpha := compliance / dt / dt + lambda = -error / (w + alpha) + correction1 = -error_gradient * -lambda + correction2 = error_gradient * -lambda + + return +} + apply_constraint_correction_unilateral :: proc( dt: f32, body: Body_Ptr, @@ -345,24 +456,27 @@ apply_constraint_correction_unilateral :: proc( error_gradient: rl.Vector3, pos: rl.Vector3, other_combined_inv_mass: f32 = 0, +) -> ( + lambda: f32, ) { - if error == 0 { - return + w: f32 + correction: rl.Vector3 + ok: bool + lambda, w, correction, ok = calculate_constraint_params1( + dt, + body, + compliance, + error, + error_gradient, + pos, + other_combined_inv_mass, + ) + + if ok { + apply_correction(body, correction, pos) } - w := get_body_inverse_mass(body, error_gradient, pos) - w += other_combined_inv_mass - - if w == 0 { - return - } - - alpha := compliance / dt / dt - lambda := -error / (w + alpha) - - delta_pos := error_gradient * -lambda - - apply_correction(body, delta_pos, pos) + return } apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) { @@ -395,11 +509,7 @@ get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 { rn = lg.cross(rn, normal) rn = lg.quaternion_mul_vector3(inv_q, rn) - w := - rn.x * rn.x * body.inv_inertia_tensor.x + - rn.y * rn.y * body.inv_inertia_tensor.y + - rn.z * rn.z * body.inv_inertia_tensor.z - + w := lg.dot(rn * rn, body.inv_inertia_tensor) w += body.inv_mass return w