From e22e667e27a77d22f9b2091df7e7f16e47808a9a Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Tue, 7 Jan 2025 04:23:27 +0400 Subject: [PATCH] Fixed unstable suspension (feel like an idiot) --- game/debug/helpers.odin | 24 +++++++++++ game/game.odin | 55 +++++++++++++++++++------- game/physics/bvh/bvh.odin | 7 ++-- game/physics/bvh/debug.odin | 29 +++----------- game/physics/collision/collision.odin | 4 +- game/physics/debug.odin | 44 ++++++++++++++++++--- game/physics/helpers.odin | 23 +++++++++-- game/physics/immediate.odin | 39 ++++++++++++++++-- game/physics/scene.odin | 14 +++++++ game/physics/simulation.odin | 57 ++++++++++++++++++--------- 10 files changed, 221 insertions(+), 75 deletions(-) create mode 100644 game/debug/helpers.odin diff --git a/game/debug/helpers.odin b/game/debug/helpers.odin new file mode 100644 index 0000000..6cbcd86 --- /dev/null +++ b/game/debug/helpers.odin @@ -0,0 +1,24 @@ +package debug + +import rl "vendor:raylib" + +int_to_color :: proc(num: i32) -> (color: rl.Color) { + x := int_hash(num) + + color.r = u8(x % 256) + color.g = u8((x / 256) % 256) + color.b = u8((x / 256 / 256) % 256) + color.a = 255 + return +} + +int_hash :: proc(num: i32) -> u32 { + x := cast(u32)num + + x = ((x >> 16) ~ x) * 0x45d9f3b + x = ((x >> 16) ~ x) * 0x45d9f3b + x = (x >> 16) ~ x + + return x +} + diff --git a/game/game.odin b/game/game.odin index 87d72b6..bad2b1e 100644 --- a/game/game.odin +++ b/game/game.odin @@ -64,7 +64,7 @@ Car :: struct { SOLVER_CONFIG :: physics.Solver_Config { timestep = 1.0 / 120, gravity = rl.Vector3{0, -9.8, 0}, - substreps_minus_one = 8 - 1, + substreps_minus_one = 4 - 1, } Game_Memory :: struct { @@ -235,18 +235,31 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { #hash("car", "fnv32a"), physics.Body_Config { initial_pos = {0, 2, 0}, - initial_rot = linalg.QUATERNIONF32_IDENTITY, + initial_rot = linalg.quaternion_angle_axis( + math.RAD_PER_DEG * 100, + rl.Vector3{0, 1, 0}, + ), initial_ang_vel = {0, 0, 0}, + shape = physics.Shape_Box{size = car_bounds.max - car_bounds.min}, mass = 100, - inertia_tensor = physics.inertia_tensor_box(car_bounds.max - car_bounds.min), }, ) - runtime_world.camera.up = rl.Vector3{0, 1, 0} - runtime_world.camera.fovy = 60 - runtime_world.camera.projection = .PERSPECTIVE - runtime_world.camera.target = - physics.get_body(&world.physics_scene, runtime_world.car_handle).x + car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle) + + camera := &runtime_world.camera + + camera.up = rl.Vector3{0, 1, 0} + camera.fovy = 60 + camera.projection = .PERSPECTIVE + camera.position = physics.body_local_to_world( + car_body, + physics.body_world_to_local( + car_body, + physics.body_local_to_world(car_body, rl.Vector3{1, 0, -2}), + ), + ) + camera.target = physics.get_body(&world.physics_scene, runtime_world.car_handle).x if runtime_world.camera.position == {} { runtime_world.camera.position = runtime_world.camera.target - rl.Vector3{10, 0, 10} } @@ -257,7 +270,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { rest := f32(1) suspension_stiffness := f32(2000) compliance := 1.0 / suspension_stiffness - damping := f32(0.1) + damping := f32(0.01) radius := f32(0.6) wheel_fl := physics.immediate_suspension_constraint( @@ -322,7 +335,7 @@ update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { DRIVE_IMPULSE :: 1 BRAKE_IMPULSE :: 2 - TURN_ANGLE :: -f32(10) * math.RAD_PER_DEG + TURN_ANGLE :: -f32(30) * math.RAD_PER_DEG for wheel_handle in drive_wheels { wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle) @@ -469,6 +482,8 @@ draw :: proc() { rl.DrawGrid(100, 1) + physics.draw_debug_scene(&world.physics_scene) + { mesh_bvh := assets.get_bvh(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb") @@ -505,15 +520,13 @@ draw :: proc() { car_matrix := rl.QuaternionToMatrix(car_body.q) car_model.transform = car_matrix - rl.DrawModel(car_model, car_body.x - runtime_world.car_com, 1, rl.WHITE) + rl.DrawModel(car_model, car_body.x + runtime_world.car_com, 1, rl.WHITE) } else { if g_mem.draw_car { rl.DrawModel(car_model, 0, 1, rl.WHITE) } } - physics.draw_debug_scene(&world.physics_scene) - { // Debug draw spline road { @@ -596,8 +609,20 @@ draw :: proc() { ) } } else { - car_pos := physics.get_body(&world.physics_scene, runtime_world.car_handle).x - rl.DrawText(fmt.ctprintf("Car Pos: %v", car_pos), 5, 32, 8, rl.ORANGE) + car := physics.get_body(&world.physics_scene, runtime_world.car_handle) + rl.DrawText( + fmt.ctprintf( + "p: %v\nv: %v\nw: %v\ng: %v", + car.x, + car.v, + car.w, + SOLVER_CONFIG.gravity, + ), + 5, + 32, + 8, + rl.ORANGE, + ) } } diff --git a/game/physics/bvh/bvh.odin b/game/physics/bvh/bvh.odin index 297d149..490af8c 100644 --- a/game/physics/bvh/bvh.odin +++ b/game/physics/bvh/bvh.odin @@ -7,6 +7,7 @@ import "core:log" import "core:math" import lg "core:math/linalg" import "core:mem" +import "game:debug" import rl "vendor:raylib" _ :: log @@ -285,7 +286,7 @@ internal_traverse_bvh_ray_triangles :: proc( rl.DrawBoundingBox( {min = node.aabb.min, max = node.aabb.max}, - debug_int_to_color(node_idx), + debug.int_to_color(node_idx), ) if is_leaf_node(node^) { @@ -312,9 +313,9 @@ internal_ray_tri_test :: proc(ray: Ray, mesh: Mesh, tri: u16, col: ^Collision) { v1, v2, v3 := mesh.vertices[i1], mesh.vertices[i2], mesh.vertices[i3] col.triangle_tests += 1 - rl.DrawTriangle3D(v1, v2, v3, debug_int_to_color(i32(tri) + 1)) + rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(tri) + 1)) - t, _, barycentric, ok := collision.intersect_segment_triangle( + t, _, barycentric, ok := collision.intersect_ray_triangle( {ray.origin, ray.origin + ray.dir}, {v1, v2, v3}, ) diff --git a/game/physics/bvh/debug.odin b/game/physics/bvh/debug.odin index 31c3dab..aece4d1 100644 --- a/game/physics/bvh/debug.odin +++ b/game/physics/bvh/debug.odin @@ -5,6 +5,7 @@ import "core:container/queue" import "core:fmt" import "core:log" import lg "core:math/linalg" +import "game:debug" import rl "vendor:raylib" import "vendor:raylib/rlgl" @@ -40,7 +41,7 @@ debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index if should_draw { rl.DrawBoundingBox( rl.BoundingBox{node.aabb.min + pos, node.aabb.max + pos}, - debug_int_to_color(node_idx + 1), + debug.int_to_color(node_idx + 1), ) } @@ -73,40 +74,20 @@ debug_draw_bvh_bounds :: proc(bvh: ^BVH, mesh: Mesh, pos: rl.Vector3, node_index } size := lg.length(aabb.max - aabb.min) - rl.DrawTriangle3D(v1, v2, v3, debug_int_to_color(i32(tri) + 1)) + rl.DrawTriangle3D(v1, v2, v3, debug.int_to_color(i32(tri) + 1)) rl.DrawBoundingBox( rl.BoundingBox{aabb.min, aabb.max}, - debug_int_to_color(i32(tri) + 2), + debug.int_to_color(i32(tri) + 2), ) if size < 1 { - rl.DrawCubeWiresV(centroid, 0.05, debug_int_to_color(i32(tri) + 3)) + rl.DrawCubeWiresV(centroid, 0.05, debug.int_to_color(i32(tri) + 3)) } } } } } -debug_int_to_color :: proc(num: i32) -> (color: rl.Color) { - x := debug_hash(num) - - color.r = u8(x % 256) - color.g = u8((x / 256) % 256) - color.b = u8((x / 256 / 256) % 256) - color.a = 255 - return -} - -debug_hash :: proc(num: i32) -> u32 { - x := cast(u32)num - - x = ((x >> 16) ~ x) * 0x45d9f3b - x = ((x >> 16) ~ x) * 0x45d9f3b - x = (x >> 16) ~ x - - return x -} - bvh_mesh_from_rl_mesh :: proc(mesh: rl.Mesh) -> Mesh { return Mesh { vertices = (cast([^]Vec3)mesh.vertices)[:mesh.vertexCount], diff --git a/game/physics/collision/collision.odin b/game/physics/collision/collision.odin index f89bd73..d14a126 100644 --- a/game/physics/collision/collision.odin +++ b/game/physics/collision/collision.odin @@ -567,7 +567,7 @@ intersect_ray_polyhedron :: proc( return t, true } -intersect_segment_triangle :: proc( +intersect_ray_triangle :: proc( segment: [2]Vec3, triangle: [3]Vec3, ) -> ( @@ -633,7 +633,7 @@ intersect_segment_plane :: proc( return t, point, true } - return t, segment[0], false + return linalg.length(ab), segment[1], false } // TODO: alternative with capsule endcaps diff --git a/game/physics/debug.odin b/game/physics/debug.odin index a002f31..b62089f 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -3,10 +3,38 @@ package physics import "core:log" import "core:math" import lg "core:math/linalg" +import "game:debug" import rl "vendor:raylib" +import "vendor:raylib/rlgl" _ :: log _ :: math +_ :: debug + +draw_debug_shape :: proc( + shape: Collision_Shape, + pos: rl.Vector3, + rot: rl.Quaternion, + color: rl.Color, +) { + rlgl.PushMatrix() + defer rlgl.PopMatrix() + + rlgl.Begin(rlgl.LINES) + defer rlgl.End() + + switch s in shape { + case Shape_Sphere: + rl.DrawSphere(pos, s.radius, color) + case Shape_Box: + mat := lg.matrix4_from_trs(pos, rot, 1) + + rlgl.LoadIdentity() + rlgl.MultMatrixf(cast([^]f32)&mat) + + rl.DrawCubeV(0, s.size, color) + } +} draw_debug_scene :: proc(scene: ^Scene) { for &body in scene.bodies { @@ -21,6 +49,8 @@ draw_debug_scene :: proc(scene: ^Scene) { rl.DrawLine3D(pos, pos + x, rl.RED) rl.DrawLine3D(pos, pos + y, rl.GREEN) rl.DrawLine3D(pos, pos + z, rl.BLUE) + + // draw_debug_shape(body.shape, body.x, body.q, debug.int_to_color(i32(i + 1))) } } @@ -50,11 +80,15 @@ draw_debug_scene :: proc(scene: ^Scene) { rl.RED, ) - rl.DrawLine3D( - pos + t * dir, - pos + t * dir + wheel.applied_impulse.x * right * 10, - rl.RED, - ) + rl.DrawLine3D(wheel_pos, wheel_pos + right * 10, rl.RED) + + if wheel.hit { + // rl.DrawLine3D( + // pos + t * dir, + // pos + t * dir + wheel.applied_impulse.x * right * 10, + // rl.RED, + // ) + } if wheel.hit { rl.DrawSphereWires(wheel.hit_point, 0.1, 4, 4, rl.RED) diff --git a/game/physics/helpers.odin b/game/physics/helpers.odin index cf6e06c..d28fe2b 100644 --- a/game/physics/helpers.odin +++ b/game/physics/helpers.odin @@ -3,6 +3,12 @@ package physics import lg "core:math/linalg" import rl "vendor:raylib" +inertia_tensor_sphere :: proc(radius: f32) -> (tensor: rl.Vector3) { + tensor = radius * radius * (2.0 / 3.0) + + return +} + inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) { CONSTANT :: f32(1.0 / 12.0) @@ -15,22 +21,33 @@ inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) { return } +inertia_tensor_collision_shape :: proc(shape: Collision_Shape) -> (tensor: rl.Vector3) { + switch s in shape { + case Shape_Sphere: + tensor = inertia_tensor_sphere(s.radius) + case Shape_Box: + tensor = inertia_tensor_box(s.size) + } + + return +} + body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 { return body.x + lg.quaternion_mul_vector3(body.q, pos) } body_local_to_world_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 { - return lg.quaternion_mul_vector3(body.q, vec) + return lg.normalize0(lg.quaternion_mul_vector3(body.q, vec)) } body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 { - inv_q := lg.quaternion_inverse(body.q) + inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(body.q)) return lg.quaternion_mul_vector3(inv_q, pos - body.x) } body_world_to_local_vec :: #force_inline proc(body: Body_Ptr, vec: rl.Vector3) -> rl.Vector3 { inv_q := lg.quaternion_inverse(body.q) - return lg.quaternion_mul_vector3(inv_q, vec) + return lg.normalize0(lg.quaternion_mul_vector3(inv_q, vec)) } body_angular_velocity_at_local_point :: #force_inline proc( diff --git a/game/physics/immediate.odin b/game/physics/immediate.odin index f451ce0..3851353 100644 --- a/game/physics/immediate.odin +++ b/game/physics/immediate.odin @@ -1,14 +1,22 @@ package physics +import lg "core:math/linalg" import rl "vendor:raylib" +Body_Config_Inertia_Mode :: enum { + From_Shape, + Explicit, +} + // Immediate mode stuff for testing Body_Config :: struct { initial_pos: rl.Vector3, initial_rot: rl.Quaternion, initial_vel: rl.Vector3, initial_ang_vel: rl.Vector3, + shape: Collision_Shape, mass: f32, + inertia_mode: Body_Config_Inertia_Mode, // Unit inertia tensor inertia_tensor: rl.Vector3, } @@ -24,18 +32,40 @@ Suspension_Constraint_Config :: struct { radius: f32, } +calculate_body_params_from_config :: proc( + config: Body_Config, +) -> ( + inv_mass: f32, + inv_inertia_tensor: rl.Vector3, +) { + inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass + + inertia_tensor: rl.Vector3 + if config.inertia_mode == .Explicit { + inertia_tensor = config.inertia_tensor + } else { + inertia_tensor = inertia_tensor_collision_shape(config.shape) + } + inertia_tensor *= config.mass + + inv_inertia_tensor = lg.all(lg.equal(inertia_tensor, rl.Vector3(0))) ? 0 : 1.0 / inertia_tensor + + return +} + initialize_body_from_config :: proc(body: ^Body, config: Body_Config) { body.x = config.initial_pos body.q = config.initial_rot body.v = config.initial_vel body.w = config.initial_ang_vel - body.inv_mass = 1.0 / config.mass - body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass) + body.shape = config.shape + + body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config) } update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) { - body.inv_mass = 1.0 / config.mass - body.inv_inertia_tensor = 1.0 / (config.inertia_tensor * config.mass) + body.shape = config.shape + body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config) } update_suspension_constraint_from_config :: proc( @@ -66,6 +96,7 @@ immediate_body :: proc( state.num_referenced_bodies += 1 } handle = body.handle + update_body_from_config(get_body(scene, handle), config) } else { new_body: Body state.num_referenced_bodies += 1 diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 687ac18..51a78a0 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -28,10 +28,24 @@ Body :: struct { inv_mass: f32, // Moment of inertia inv_inertia_tensor: rl.Vector3, + shape: Collision_Shape, // next_plus_one: i32, } +Shape_Sphere :: struct { + radius: f32, +} + +Shape_Box :: struct { + size: rl.Vector3, +} + +Collision_Shape :: union { + Shape_Sphere, + Shape_Box, +} + Suspension_Constraint :: struct { alive: bool, // Pos relative to the body diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index d1a68ef..69e2d4f 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -48,7 +48,7 @@ simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt: state.accumulated_time += dt num_steps := 0 - for state.accumulated_time >= config.timestep { + for state.accumulated_time > config.timestep { num_steps += 1 state.accumulated_time -= config.timestep @@ -78,15 +78,17 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { for &body, i in scene.bodies { if body.alive { body_states[i].prev_x = body.x - body.v += dt * config.gravity - body.x += dt * body.v + body.v += config.gravity * dt + body.x += body.v * dt body_states[i].prev_q = body.q // TODO: Probably can do it using built in quaternion math but I have no idea how it works // NOTE: figure out how this works https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ q := body.q - delta_rot := quaternion(x = body.w.x, y = body.w.y, z = body.w.z, w = 0) + + w := body.w + delta_rot := quaternion(x = w.x, y = w.y, z = w.z, w = 0) delta_rot = delta_rot * q q.x += 0.5 * dt * delta_rot.x q.y += 0.5 * dt * delta_rot.y @@ -102,15 +104,13 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { if v.alive { body := get_body(scene, v.body) - q := body.q pos := body_local_to_world(body, v.rel_pos) - dir := lg.quaternion_mul_vector3(q, v.rel_dir) + dir := body_local_to_world_vec(body, v.rel_dir) pos2 := pos + dir * v.rest v.hit_t, v.hit_point, v.hit = collision.intersect_segment_plane( {pos, pos2}, collision.plane_from_point_normal({}, collision.Vec3{0, 1, 0}), ) - if v.hit { corr := v.hit_point - pos distance := lg.length(corr) @@ -139,40 +139,56 @@ simulate_step :: proc(scene: ^Scene, config: Solver_Config) { if body.alive && v.hit { wheel_world_pos := body_local_to_world(body, v.rel_pos) + dir := body_local_to_world_vec(body, v.rel_dir) body_state := body_states[i32(v.body) - 1] // Spring damping - { - dir := body_local_to_world_vec(body, v.rel_dir) - vel_3d := body_velocity_at_local_point(body, v.rel_pos) + if true { + vel_3d := body_velocity_at_local_point(body, wheel_world_pos - body.x) vel := lg.dot(vel_3d, dir) - damp_delta := -vel * v.damping * dt * dir + damping := -vel * min(v.damping * dt, 1) + + apply_constraint_correction_unilateral( + dt, + body, + 0, + error = damping, + error_gradient = dir, + pos = wheel_world_pos, + ) - apply_correction(body, damp_delta, wheel_world_pos) body_solve_velocity(body, body_state, inv_dt) } // Drive forces - { + if true { total_impulse := v.drive_impulse - v.brake_impulse forward := body_local_to_world_vec(body, rl.Vector3{0, 0, 1}) corr := total_impulse * forward * dt + apply_constraint_correction_unilateral( + dt, + body, + 0, + total_impulse * dt * dt, + forward, + wheel_world_pos, + ) apply_correction(body, corr, wheel_world_pos) body_solve_velocity(body, body_state, inv_dt) } // Lateral friction - { + if true { local_contact_pos := v.hit_point - body.x vel_contact := body_velocity_at_local_point(body, local_contact_pos) right := wheel_get_right_vec(body, v) lateral_vel := lg.dot(right, vel_contact) - friction := f32(0.7) + friction := f32(0.5) impulse := -lateral_vel * friction corr := right * impulse * dt v.applied_impulse.x = impulse @@ -239,7 +255,7 @@ apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) { body.x += corr * body.inv_mass q := body.q - inv_q := lg.quaternion_inverse(q) + inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) delta_omega := pos - body.x delta_omega = lg.cross(delta_omega, corr) delta_omega = lg.quaternion_mul_vector3(inv_q, delta_omega) @@ -259,14 +275,17 @@ apply_correction :: proc(body: Body_Ptr, corr: rl.Vector3, pos: rl.Vector3) { get_body_inverse_mass :: proc(body: Body_Ptr, normal, pos: rl.Vector3) -> f32 { q := body.q - inv_q := lg.quaternion_inverse(q) + inv_q := lg.quaternion_normalize0(lg.quaternion_inverse(q)) rn := pos - body.x rn = lg.cross(rn, normal) rn = lg.quaternion_mul_vector3(inv_q, rn) - rn *= rn - w := lg.dot(rn, body.inv_inertia_tensor) + 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 += body.inv_mass return w