From 0687ff485873f2e4cc08e518d5ad64e1ef255ead Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sun, 5 Jan 2025 03:16:44 +0400 Subject: [PATCH] Wheel friction working --- game/editor.odin | 153 +++++++++++++ game/game.odin | 426 +++++++++++++---------------------- game/physics/debug.odin | 38 +++- game/physics/helpers.odin | 43 +++- game/physics/immediate.odin | 5 + game/physics/scene.odin | 30 ++- game/physics/simulation.odin | 192 +++++++++++----- 7 files changed, 539 insertions(+), 348 deletions(-) create mode 100644 game/editor.odin diff --git a/game/editor.odin b/game/editor.odin new file mode 100644 index 0000000..5493d1d --- /dev/null +++ b/game/editor.odin @@ -0,0 +1,153 @@ +package game + +import lg "core:math/linalg" +import rl "vendor:raylib" + +update_free_look_camera :: proc(es: ^Editor_State) { + input: rl.Vector2 + + if rl.IsKeyDown(.UP) || rl.IsKeyDown(.W) { + input.y -= 1 + } + if rl.IsKeyDown(.DOWN) || rl.IsKeyDown(.S) { + input.y += 1 + } + if rl.IsKeyDown(.LEFT) || rl.IsKeyDown(.A) { + input.x -= 1 + } + if rl.IsKeyDown(.RIGHT) || rl.IsKeyDown(.D) { + input.x += 1 + } + + should_capture_mouse := rl.IsMouseButtonDown(.RIGHT) + if es.mouse_captured != should_capture_mouse { + if should_capture_mouse { + rl.DisableCursor() + } else { + rl.EnableCursor() + } + } + es.mouse_captured = should_capture_mouse + + if es.mouse_captured { + get_runtime_world().camera_yaw_pitch += rl.GetMouseDelta().yx * -1 * 0.001 + } + + get_runtime_world().camera_speed += rl.GetMouseWheelMove() * 0.01 + get_runtime_world().camera_speed = lg.clamp(get_runtime_world().camera_speed, 0.01, 10) + + rotation_matrix := camera_rotation_matrix() + forward := -rotation_matrix[2] + right := lg.cross(rl.Vector3{0, 1, 0}, forward) + + input = lg.normalize0(input) + get_world().player_pos += + (input.x * right + input.y * forward) * get_runtime_world().camera_speed +} + +update_editor :: proc(es: ^Editor_State) { + update_free_look_camera(es) + + switch es.track_edit_state { + case .Select: + { + if rl.IsKeyPressed(.F) { + add_track_spline_point() + } + + if is_point_selected() { + if rl.IsKeyPressed(.X) { + + if len(es.point_selection) <= 1 { + for i in es.point_selection { + ordered_remove(&get_world().track.points, i) + } + } else { + #reverse for _, i in get_world().track.points { + if i in es.point_selection { + ordered_remove(&get_world().track.points, i) + } + } + } + + clear(&es.point_selection) + } + if rl.IsKeyPressed(.G) { + es.track_edit_state = .Move + es.move_axis = .None + es.total_movement_world = {} + // es.initial_point_pos = g_mem.track.points[es.selected_track_point] + } + } + } + case .Move: + { + if rl.IsKeyPressed(.ESCAPE) { + es.track_edit_state = .Select + // g_mem.track.points[es.selected_track_point] = es.initial_point_pos + break + } + + if (rl.IsMouseButtonPressed(.LEFT)) { + es.track_edit_state = .Select + break + } + + if !es.mouse_captured { + // Blender style movement + if rl.IsKeyDown(.LEFT_SHIFT) { + if rl.IsKeyPressed(.X) { + es.move_axis = .YZ + } + if rl.IsKeyPressed(.Y) { + es.move_axis = .XZ + } + if rl.IsKeyPressed(.Z) { + es.move_axis = .XY + } + } else { + if rl.IsKeyPressed(.X) { + es.move_axis = .X + } + if rl.IsKeyPressed(.Y) { + es.move_axis = .Y + } + if rl.IsKeyPressed(.Z) { + es.move_axis = .Z + } + } + + // log.debugf("Move axis %v", es.move_axis) + + camera := game_camera_3d() + + mouse_delta := rl.GetMouseDelta() * 0.05 + + view_rotation := lg.transpose(rl.GetCameraMatrix(camera)) + view_rotation[3].xyz = 0 + view_proj := view_rotation * rl.MatrixOrtho(-1, 1, 1, -1, -1, 1) + + axes_buf: [2]rl.Vector3 + colors_buf: [2]rl.Color + axes, _ := get_movement_axes(es.move_axis, &axes_buf, &colors_buf) + + movement_world: rl.Vector3 + for axis in axes { + axis_screen := (rl.Vector4{axis.x, axis.y, axis.z, 1} * view_proj).xy + axis_screen = lg.normalize0(axis_screen) + + movement_screen := lg.dot(axis_screen, mouse_delta) * axis_screen + movement_world += + (rl.Vector4{movement_screen.x, movement_screen.y, 0, 1} * rl.MatrixInvert(view_proj)).xyz + } + + for k in es.point_selection { + get_world().track.points[k] += movement_world + } + + es.total_movement_world += movement_world + } + } + } +} + diff --git a/game/game.odin b/game/game.odin index b7d63fa..f121689 100644 --- a/game/game.odin +++ b/game/game.odin @@ -31,34 +31,46 @@ Track :: struct { } World :: struct { + player_pos: rl.Vector3, track: Track, physics_scene: physics.Scene, } - destroy_world :: proc(world: ^World) { delete(world.track.points) physics.destroy_physics_scene(&world.physics_scene) } + +Runtime_World :: struct { + world: World, + pause: bool, + solver_state: physics.Solver_State, + car_com: rl.Vector3, + car_handle: physics.Body_Handle, + camera_yaw_pitch: rl.Vector2, + camera_speed: f32, + camera: rl.Camera3D, +} +destroy_runtime_world :: proc(runtime_world: ^Runtime_World) { + destroy_world(&runtime_world.world) + physics.destroy_solver_state(&runtime_world.solver_state) +} + Car :: struct { pos: rl.Vector3, } SOLVER_CONFIG :: physics.Solver_Config { - timestep = 1.0 / 120, - gravity = rl.Vector3{0, -9.8, 0}, + timestep = 1.0 / 120, + gravity = rl.Vector3{0, -9.8, 0}, + substreps_minus_one = 8 - 1, } Game_Memory :: struct { - assetman: assets.Asset_Manager, - player_pos: rl.Vector3, - camera_yaw_pitch: rl.Vector2, - camera_speed: f32, - camera: rl.Camera3D, - solver_state: physics.Solver_State, - car_handle: physics.Body_Handle, - es: Editor_State, - editor: bool, + assetman: assets.Asset_Manager, + runtime_world: Runtime_World, + es: Editor_State, + editor: bool, } Track_Edit_State :: enum { @@ -90,19 +102,34 @@ Editor_State :: struct { g_mem: ^Game_Memory +get_runtime_world :: proc() -> ^Runtime_World { + return &g_mem.runtime_world +} + get_world :: proc() -> ^World { - return &g_mem.es.world + return g_mem.editor ? &g_mem.es.world : &g_mem.runtime_world.world +} + +get_editor_state :: proc() -> ^Editor_State { + return &g_mem.es } game_camera :: proc() -> rl.Camera2D { w := f32(rl.GetScreenWidth()) h := f32(rl.GetScreenHeight()) - return {zoom = h / PIXEL_WINDOW_HEIGHT, target = g_mem.player_pos.xy, offset = {w / 2, h / 2}} + return { + zoom = h / PIXEL_WINDOW_HEIGHT, + target = get_world().player_pos.xy, + offset = {w / 2, h / 2}, + } } camera_rotation_matrix :: proc() -> matrix[3, 3]f32 { - return linalg.matrix3_from_euler_angles_xy(g_mem.camera_yaw_pitch.x, g_mem.camera_yaw_pitch.y) + return linalg.matrix3_from_euler_angles_xy( + get_runtime_world().camera_yaw_pitch.x, + get_runtime_world().camera_yaw_pitch.y, + ) } camera_forward_vec :: proc() -> rl.Vector3 { @@ -113,64 +140,21 @@ camera_forward_vec :: proc() -> rl.Vector3 { game_camera_3d :: proc() -> rl.Camera3D { if g_mem.editor { return { - position = g_mem.player_pos, + position = get_world().player_pos, up = {0, 1, 0}, fovy = 60, - target = g_mem.player_pos + camera_forward_vec(), + target = get_world().player_pos + camera_forward_vec(), projection = .PERSPECTIVE, } } - return g_mem.camera + return get_runtime_world().camera } ui_camera :: proc() -> rl.Camera2D { return {zoom = f32(rl.GetScreenHeight()) / PIXEL_WINDOW_HEIGHT} } -update_free_look_camera :: proc() { - es := &g_mem.es - - input: rl.Vector2 - - if rl.IsKeyDown(.UP) || rl.IsKeyDown(.W) { - input.y -= 1 - } - if rl.IsKeyDown(.DOWN) || rl.IsKeyDown(.S) { - input.y += 1 - } - if rl.IsKeyDown(.LEFT) || rl.IsKeyDown(.A) { - input.x -= 1 - } - if rl.IsKeyDown(.RIGHT) || rl.IsKeyDown(.D) { - input.x += 1 - } - - should_capture_mouse := rl.IsMouseButtonDown(.RIGHT) - if es.mouse_captured != should_capture_mouse { - if should_capture_mouse { - rl.DisableCursor() - } else { - rl.EnableCursor() - } - } - es.mouse_captured = should_capture_mouse - - if es.mouse_captured { - g_mem.camera_yaw_pitch += rl.GetMouseDelta().yx * -1 * 0.001 - } - - g_mem.camera_speed += rl.GetMouseWheelMove() * 0.01 - g_mem.camera_speed = linalg.clamp(g_mem.camera_speed, 0.01, 10) - - rotation_matrix := camera_rotation_matrix() - forward := -rotation_matrix[2] - right := linalg.cross(rl.Vector3{0, 1, 0}, forward) - - input = linalg.normalize0(input) - g_mem.player_pos += (input.x * right + input.y * forward) * g_mem.camera_speed -} - select_track_point :: proc(index: int) { clear(&g_mem.es.point_selection) g_mem.es.point_selection[index] = true @@ -183,7 +167,7 @@ is_point_selected :: proc() -> bool { add_track_spline_point :: proc() { forward := camera_rotation_matrix()[2] - append(&get_world().track.points, g_mem.player_pos + forward) + append(&get_world().track.points, get_world().player_pos + forward) select_track_point(len(&get_world().track.points) - 1) } @@ -233,135 +217,20 @@ get_movement_axes :: proc( return out_axes[0:0], out_colors[0:0] } -update_editor :: proc() { - es := &g_mem.es +update_runtime_world :: proc(runtime_world: ^Runtime_World, dt: f32) { + world := &runtime_world.world - switch es.track_edit_state { - case .Select: - { - if rl.IsKeyPressed(.F) { - add_track_spline_point() - } - - if is_point_selected() { - if rl.IsKeyPressed(.X) { - - if len(es.point_selection) <= 1 { - for i in es.point_selection { - ordered_remove(&get_world().track.points, i) - } - } else { - #reverse for _, i in get_world().track.points { - if i in es.point_selection { - ordered_remove(&get_world().track.points, i) - } - } - } - - clear(&es.point_selection) - } - if rl.IsKeyPressed(.G) { - es.track_edit_state = .Move - es.move_axis = .None - es.total_movement_world = {} - // es.initial_point_pos = g_mem.track.points[es.selected_track_point] - } - } - } - case .Move: - { - if rl.IsKeyPressed(.ESCAPE) { - es.track_edit_state = .Select - // g_mem.track.points[es.selected_track_point] = es.initial_point_pos - break - } - - if (rl.IsMouseButtonPressed(.LEFT)) { - es.track_edit_state = .Select - break - } - - if !es.mouse_captured { - // Blender style movement - if rl.IsKeyDown(.LEFT_SHIFT) { - if rl.IsKeyPressed(.X) { - es.move_axis = .YZ - } - if rl.IsKeyPressed(.Y) { - es.move_axis = .XZ - } - if rl.IsKeyPressed(.Z) { - es.move_axis = .XY - } - } else { - if rl.IsKeyPressed(.X) { - es.move_axis = .X - } - if rl.IsKeyPressed(.Y) { - es.move_axis = .Y - } - if rl.IsKeyPressed(.Z) { - es.move_axis = .Z - } - } - - // log.debugf("Move axis %v", es.move_axis) - - camera := game_camera_3d() - - mouse_delta := rl.GetMouseDelta() * 0.05 - - view_rotation := linalg.transpose(rl.GetCameraMatrix(camera)) - view_rotation[3].xyz = 0 - view_proj := view_rotation * rl.MatrixOrtho(-1, 1, 1, -1, -1, 1) - - axes_buf: [2]rl.Vector3 - colors_buf: [2]rl.Color - axes, _ := get_movement_axes(es.move_axis, &axes_buf, &colors_buf) - - movement_world: rl.Vector3 - for axis in axes { - axis_screen := (rl.Vector4{axis.x, axis.y, axis.z, 1} * view_proj).xy - axis_screen = linalg.normalize0(axis_screen) - - movement_screen := linalg.dot(axis_screen, mouse_delta) * axis_screen - movement_world += - (rl.Vector4{movement_screen.x, movement_screen.y, 0, 1} * rl.MatrixInvert(view_proj)).xyz - } - - for k in es.point_selection { - get_world().track.points[k] += movement_world - } - - es.total_movement_world += movement_world - } - } - } -} - -update :: proc() { - if rl.IsKeyPressed(.TAB) { - g_mem.editor = !g_mem.editor - - // if g_mem.editor { - // rl.EnableCursor() - // } else { - // rl.DisableCursor() - // } - } - - dt := rl.GetFrameTime() - - if !g_mem.editor { + if !runtime_world.pause { car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb") car_bounds := rl.GetModelBoundingBox(car_model) + runtime_world.car_com = (car_bounds.min + car_bounds.max) / 2 - g_mem.car_handle = physics.immediate_body( - &get_world().physics_scene, - &g_mem.solver_state, + runtime_world.car_handle = physics.immediate_body( + &world.physics_scene, + &runtime_world.solver_state, #hash("car", "fnv32a"), physics.Body_Config { - initial_pos = {0, 1, 0}, + initial_pos = {0, 2, 0}, initial_rot = linalg.QUATERNIONF32_IDENTITY, initial_ang_vel = {0, 0, 0}, mass = 100, @@ -369,81 +238,133 @@ update :: proc() { }, ) - g_mem.camera.up = rl.Vector3{0, 1, 0} - g_mem.camera.fovy = 60 - g_mem.camera.projection = .PERSPECTIVE - g_mem.camera.target = physics.get_body(&get_world().physics_scene, g_mem.car_handle).x - if g_mem.camera.position == {} { - g_mem.camera.position = g_mem.camera.target - rl.Vector3{10, 0, 10} + 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 + if runtime_world.camera.position == {} { + runtime_world.camera.position = runtime_world.camera.target - rl.Vector3{10, 0, 10} } // 1.6 is a good value - wheel_extent_x := f32(2.0) - rest := f32(0.9) - suspension_stiffness := f32(10000) + wheel_extent_x := f32(2) + wheel_y := f32(-0.5) + rest := f32(1) + suspension_stiffness := f32(2000) compliance := 1.0 / suspension_stiffness + damping := f32(0.1) + radius := f32(0.6) - physics.immediate_suspension_constraint( - &get_world().physics_scene, - &g_mem.solver_state, + wheel_fl := physics.immediate_suspension_constraint( + &world.physics_scene, + &runtime_world.solver_state, #hash("FL", "fnv32a"), { - rel_pos = {-wheel_extent_x, 0, 2.5}, + rel_pos = {-wheel_extent_x, wheel_y, 2.9}, rel_dir = {0, -1, 0}, + radius = radius, rest = rest, compliance = compliance, - body = g_mem.car_handle, + damping = damping, + body = runtime_world.car_handle, }, ) - physics.immediate_suspension_constraint( - &get_world().physics_scene, - &g_mem.solver_state, + wheel_fr := physics.immediate_suspension_constraint( + &world.physics_scene, + &runtime_world.solver_state, #hash("FR", "fnv32a"), { - rel_pos = {wheel_extent_x, 0, 2.5}, + rel_pos = {wheel_extent_x, wheel_y, 2.9}, rel_dir = {0, -1, 0}, + radius = radius, rest = rest, compliance = compliance, - body = g_mem.car_handle, + damping = damping, + body = runtime_world.car_handle, }, ) - physics.immediate_suspension_constraint( - &get_world().physics_scene, - &g_mem.solver_state, + wheel_rl := physics.immediate_suspension_constraint( + &world.physics_scene, + &runtime_world.solver_state, #hash("RL", "fnv32a"), { - rel_pos = {-wheel_extent_x, 0, -3}, + rel_pos = {-wheel_extent_x, wheel_y, -2.6}, rel_dir = {0, -1, 0}, + radius = radius, rest = rest, compliance = compliance, - body = g_mem.car_handle, + damping = damping, + body = runtime_world.car_handle, }, ) - physics.immediate_suspension_constraint( - &get_world().physics_scene, - &g_mem.solver_state, + wheel_rr := physics.immediate_suspension_constraint( + &world.physics_scene, + &runtime_world.solver_state, #hash("RR", "fnv32a"), { - rel_pos = {wheel_extent_x, 0, -3}, + rel_pos = {wheel_extent_x, wheel_y, -2.6}, rel_dir = {0, -1, 0}, + radius = radius, rest = rest, compliance = compliance, - body = g_mem.car_handle, + damping = damping, + body = runtime_world.car_handle, }, ) - } else { - update_free_look_camera() + drive_wheels := []physics.Suspension_Constraint_Handle{wheel_rl, wheel_rr} + turn_wheels := []physics.Suspension_Constraint_Handle{wheel_fl, wheel_fr} - update_editor() + DRIVE_IMPULSE :: 1 + BRAKE_IMPULSE :: 2 + TURN_ANGLE :: -f32(10) * math.RAD_PER_DEG + + for wheel_handle in drive_wheels { + wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle) + + wheel.drive_impulse = 0 + wheel.brake_impulse = 0 + + if rl.IsKeyDown(.W) { + wheel.drive_impulse = DRIVE_IMPULSE + } + + if rl.IsKeyDown(.S) { + wheel.brake_impulse = BRAKE_IMPULSE + } + } + + for wheel_handle in turn_wheels { + wheel := physics.get_suspension_constraint(&world.physics_scene, wheel_handle) + wheel.turn_angle = 0 + + if (rl.IsKeyDown(.A)) { + wheel.turn_angle += -TURN_ANGLE + } + + if (rl.IsKeyDown(.D)) { + wheel.turn_angle += TURN_ANGLE + } + + } + + physics.simulate(&world.physics_scene, &runtime_world.solver_state, SOLVER_CONFIG, dt) + } +} + +update :: proc() { + if rl.IsKeyPressed(.TAB) { + g_mem.editor = !g_mem.editor } - physics.simulate( - &g_mem.es.world.physics_scene, - &g_mem.solver_state, - SOLVER_CONFIG, - g_mem.editor ? 0 : dt, - ) + dt := rl.GetFrameTime() + + if g_mem.editor { + update_editor(get_editor_state()) + } else { + update_runtime_world(get_runtime_world(), dt) + } } catmull_rom_coefs :: proc( @@ -479,9 +400,12 @@ draw :: proc() { defer rl.EndDrawing() rl.ClearBackground(rl.BLACK) + runtime_world := get_runtime_world() + world := get_world() + camera := game_camera_3d() - points := &get_world().track.points + points := &world.track.points interpolated_points := calculate_spline_interpolated_points(points[:], context.temp_allocator) @@ -496,43 +420,17 @@ draw :: proc() { rl.BeginMode3D(camera) defer rl.EndMode3D() - if collision.hit { - // tangent, bitangent := get_point_frame(interpolated_points, segment_idx) - - // rot_matrix: linalg.Matrix3f32 - // rot_matrix[0] = bitangent - // rot_matrix[1] = interpolated_points[segment_idx].normal - // rot_matrix[2] = -tangent - - // angle, axis := linalg.angle_axis_from_quaternion( - // linalg.quaternion_from_matrix3(rot_matrix), - // ) - } - rl.DrawGrid(100, 1) if !g_mem.editor { - - car_body := physics.get_body(&get_world().physics_scene, g_mem.car_handle) + car_body := physics.get_body(&world.physics_scene, runtime_world.car_handle) car_matrix := rl.QuaternionToMatrix(car_body.q) car_model.transform = car_matrix - rl.DrawModel(car_model, car_body.x, 1, rl.WHITE) + rl.DrawModel(car_model, car_body.x - runtime_world.car_com, 1, rl.WHITE) } - physics.draw_debug_scene(&get_world().physics_scene) - - // road: rl.Mesh - // defer rl.UnloadMesh(road) - // road_vertices: [dynamic]f32 - // road_normals: [dynamic]f32 - // road_uvs: [dynamic]f32 - // road_indices: [dynamic]u16 - // road_vertices.allocator = context.temp_allocator - // road_normals.allocator = context.temp_allocator - // road_uvs.allocator = context.temp_allocator - // road_indices.allocator = context.temp_allocator - + physics.draw_debug_scene(&world.physics_scene) { // Debug draw spline road @@ -602,14 +500,8 @@ draw :: proc() { ) } } else { - car_pos := physics.get_body(&get_world().physics_scene, g_mem.car_handle).x - rl.DrawText( - fmt.ctprintf("Car Pos: %v. Mesh count: %v", car_pos, car_model.meshCount), - 5, - 32, - 8, - rl.ORANGE, - ) + 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) } } @@ -637,7 +529,7 @@ draw :: proc() { false, rl.GuiIconName.ICON_TARGET_POINT, )) { - inject_at(&get_world().track.points, 0, new_point_pos) + inject_at(&world.track.points, 0, new_point_pos) log.debugf("add point before 0") } } @@ -660,7 +552,7 @@ draw :: proc() { false, rl.GuiIconName.ICON_TARGET_POINT, )) { - inject_at(&get_world().track.points, points_len - 1 + 1, new_point_pos) + inject_at(&world.track.points, points_len - 1 + 1, new_point_pos) log.debugf("add point before 0") } } @@ -674,12 +566,12 @@ draw :: proc() { middle_pos := sample_spline(points[:], t) if (spline_handle(middle_pos, camera, false, rl.GuiIconName.ICON_TARGET_POINT)) { - inject_at(&get_world().track.points, i + 1, middle_pos) + inject_at(&world.track.points, i + 1, middle_pos) log.debugf("add point after %d", i) } } - if spline_handle(get_world().track.points[i], camera, es.point_selection[i]) { + if spline_handle(world.track.points[i], camera, es.point_selection[i]) { if !rl.IsKeyDown(.LEFT_CONTROL) { clear(&g_mem.es.point_selection) } @@ -784,9 +676,9 @@ game_init :: proc() { @(export) game_shutdown :: proc() { assets.shutdown(&g_mem.assetman) - destroy_world(get_world()) + destroy_world(&g_mem.es.world) delete(g_mem.es.point_selection) - physics.destroy_solver_state(&g_mem.solver_state) + destroy_runtime_world(&g_mem.runtime_world) free(g_mem) } diff --git a/game/physics/debug.odin b/game/physics/debug.odin index e1eb80e..a002f31 100644 --- a/game/physics/debug.odin +++ b/game/physics/debug.odin @@ -1,10 +1,12 @@ package physics import "core:log" +import "core:math" import lg "core:math/linalg" import rl "vendor:raylib" _ :: log +_ :: math draw_debug_scene :: proc(scene: ^Scene) { for &body in scene.bodies { @@ -22,20 +24,40 @@ draw_debug_scene :: proc(scene: ^Scene) { } } - for &constraint in scene.suspension_constraints { - if constraint.alive { - body := get_body(scene, constraint.body) - t := constraint.hit_t > 0 ? constraint.hit_t : constraint.rest + for _, i in scene.suspension_constraints { + wheel := &scene.suspension_constraints_slice[i] + if wheel.alive { + body := get_body(scene, wheel.body) + t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest pos := body.x rot := body.q - pos += lg.quaternion_mul_vector3(rot, constraint.rel_pos) - dir := lg.quaternion_mul_vector3(rot, constraint.rel_dir) + 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) - if constraint.hit { - rl.DrawSphereWires(constraint.hit_point, 0.1, 4, 4, rl.RED) + 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) + + rl.DrawCylinderWiresEx( + wheel_pos - right * 0.1, + wheel_pos + right * 0.1, + wheel.radius, + wheel.radius, + 16, + rl.RED, + ) + + 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 6998651..cf6e06c 100644 --- a/game/physics/helpers.odin +++ b/game/physics/helpers.odin @@ -19,8 +19,49 @@ body_local_to_world :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl 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) +} + body_world_to_local :: #force_inline proc(body: Body_Ptr, pos: rl.Vector3) -> rl.Vector3 { - // TODO: maybe store that inv_q := 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) +} + +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) +} + +wheel_get_rel_wheel_pos :: #force_inline proc( + body: Body_Ptr, + wheel: Suspension_Constraint_Ptr, +) -> rl.Vector3 { + t := wheel.hit_t > 0 ? wheel.hit_t : wheel.rest + return wheel.rel_pos + wheel.rel_dir * (t - wheel.radius) +} + +wheel_get_right_vec :: #force_inline proc( + body: Body_Ptr, + wheel: Suspension_Constraint_Ptr, +) -> rl.Vector3 { + local_right := lg.quaternion_mul_vector3( + lg.quaternion_angle_axis(wheel.turn_angle, rl.Vector3{0, 1, 0}), + rl.Vector3{1, 0, 0}, + ) + return body_local_to_world_vec(body, local_right) +} diff --git a/game/physics/immediate.odin b/game/physics/immediate.odin index 3058b60..f451ce0 100644 --- a/game/physics/immediate.odin +++ b/game/physics/immediate.odin @@ -13,12 +13,15 @@ Body_Config :: struct { inertia_tensor: rl.Vector3, } +// TODO: rename to wheel Suspension_Constraint_Config :: struct { rel_pos: rl.Vector3, rel_dir: rl.Vector3, body: Body_Handle, rest: f32, compliance: f32, + damping: f32, + radius: f32, } initialize_body_from_config :: proc(body: ^Body, config: Body_Config) { @@ -44,6 +47,8 @@ update_suspension_constraint_from_config :: proc( constraint.body = config.body constraint.rest = config.rest constraint.compliance = config.compliance + constraint.damping = config.damping + constraint.radius = config.radius } immediate_body :: proc( diff --git a/game/physics/scene.odin b/game/physics/scene.odin index 84a9a00..8598a9c 100644 --- a/game/physics/scene.odin +++ b/game/physics/scene.odin @@ -33,26 +33,34 @@ Body :: struct { } Suspension_Constraint :: struct { - alive: bool, + alive: bool, // Pos relative to the body - rel_pos: rl.Vector3, + rel_pos: rl.Vector3, // Dir relative to the body - rel_dir: rl.Vector3, + rel_dir: rl.Vector3, // Handle of the rigid body - body: Body_Handle, + body: Body_Handle, + // Wheel radius + radius: f32, // Rest distance - rest: f32, + rest: f32, // Inverse stiffness - compliance: f32, + compliance: f32, + // How much to damp velocity of the spring + damping: f32, // Runtime state - hit: bool, - hit_point: rl.Vector3, + hit: bool, + hit_point: rl.Vector3, // rel_hit_point = rel_pos + rel_dir * hit_t - hit_t: f32, + hit_t: f32, + turn_angle: f32, + drive_impulse: f32, + brake_impulse: f32, + applied_impulse: rl.Vector3, // Free list - next_plus_one: i32, + next_plus_one: i32, } // Index plus one, so handle 0 maps to invalid body @@ -98,7 +106,7 @@ add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle { body_copy.alive = true body_copy.next_plus_one = 0 - if scene.first_free_body_plus_one > 1 { + if scene.first_free_body_plus_one > 0 { index := scene.first_free_body_plus_one new_body := get_body(scene, Body_Handle(index)) next_plus_one := new_body.next_plus_one diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin index df26169..d1a68ef 100644 --- a/game/physics/simulation.odin +++ b/game/physics/simulation.odin @@ -1,16 +1,19 @@ package physics import "collision" +import "core:fmt" import "core:math" import lg "core:math/linalg" import rl "vendor:raylib" _ :: math +_ :: fmt Solver_Config :: struct { // Will automatically do fixed timestep - timestep: f32, - gravity: rl.Vector3, + timestep: f32, + gravity: rl.Vector3, + substreps_minus_one: i32, } Solver_State :: struct { @@ -25,16 +28,17 @@ Solver_State :: struct { immediate_suspension_constraints: map[u32]Immedate_State(Suspension_Constraint_Handle), } +destroy_solver_state :: proc(state: ^Solver_State) { + delete(state.immedate_bodies) + delete(state.immediate_suspension_constraints) +} + Immedate_State :: struct($T: typeid) { handle: T, // When was this referenced last time (frame number) last_ref: u32, } -destroy_solver_state :: proc(state: ^Solver_State) { - delete(state.immedate_bodies) -} - // Outer simulation loop for fixed timestepping simulate :: proc(scene: ^Scene, state: ^Solver_State, config: Solver_Config, dt: f32) { assert(config.timestep > 0) @@ -62,81 +66,147 @@ Body_Sim_State :: struct { } simulate_step :: proc(scene: ^Scene, config: Solver_Config) { - body_states := make_soa(#soa[]Body_Sim_State, len(scene.bodies), context.temp_allocator) + body_states := make([]Body_Sim_State, len(scene.bodies), context.temp_allocator) - dt := config.timestep + substeps := config.substreps_minus_one + 1 + + dt := config.timestep / f32(substeps) inv_dt := 1.0 / dt - // Integrate positions and rotations - 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 + for _ in 0 ..< substeps { + // Integrate positions and rotations + 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_states[i].prev_q = body.q + 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) - delta_rot = delta_rot * q - q.x += 0.5 * dt * delta_rot.x - q.y += 0.5 * dt * delta_rot.y - q.z += 0.5 * dt * delta_rot.z - q.w += 0.5 * dt * delta_rot.w - q = lg.normalize0(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) + delta_rot = delta_rot * q + q.x += 0.5 * dt * delta_rot.x + q.y += 0.5 * dt * delta_rot.y + q.z += 0.5 * dt * delta_rot.z + q.w += 0.5 * dt * delta_rot.w + q = lg.normalize0(q) - body.q = q + body.q = q + } } - } - for &v in scene.suspension_constraints { - if v.alive { - body := get_body(scene, v.body) + for &v in scene.suspension_constraints { + 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) - 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) - corr = corr / distance if distance > 0 else 0 - - apply_constraint_correction_unilateral( - dt, - body, - v.compliance, - error = distance - v.rest, - error_gradient = corr, - pos = pos, - other_combined_inv_mass = 0, + q := body.q + pos := body_local_to_world(body, v.rel_pos) + dir := lg.quaternion_mul_vector3(q, 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) + corr = corr / distance if distance > 0 else 0 + + apply_constraint_correction_unilateral( + dt, + body, + v.compliance, + error = distance - v.rest, + error_gradient = corr, + pos = pos, + other_combined_inv_mass = 0, + ) + } + } + } + + solve_velocities(scene, body_states, inv_dt) + + // Solve suspension velocity + for _, i in scene.suspension_constraints { + v := &scene.suspension_constraints_slice[i] + if v.alive { + body := get_body(scene, v.body) + + if body.alive && v.hit { + wheel_world_pos := body_local_to_world(body, v.rel_pos) + 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) + + vel := lg.dot(vel_3d, dir) + damp_delta := -vel * v.damping * dt * dir + + apply_correction(body, damp_delta, wheel_world_pos) + body_solve_velocity(body, body_state, inv_dt) + } + + // Drive forces + { + 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_correction(body, corr, wheel_world_pos) + body_solve_velocity(body, body_state, inv_dt) + } + + // Lateral friction + { + 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) + impulse := -lateral_vel * friction + corr := right * impulse * dt + v.applied_impulse.x = impulse + + apply_correction(body, corr, v.hit_point) + body_solve_velocity(body, body_state, inv_dt) + } + } } } } +} +solve_velocities :: proc(scene: ^Scene, body_states: []Body_Sim_State, inv_dt: f32) { // Compute new linear and angular velocities - for &body, i in scene.bodies { + for _, i in scene.bodies_slice { + body := &scene.bodies_slice[i] if body.alive { - body.v = (body.x - body_states[i].prev_x) * inv_dt - - delta_q := body.q * lg.quaternion_inverse(body_states[i].prev_q) - body.w = rl.Vector3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt - - if delta_q.w < 0 { - body.w = -body.w - } + body_solve_velocity(body, body_states[i], inv_dt) } } } +body_solve_velocity :: #force_inline proc(body: Body_Ptr, state: Body_Sim_State, inv_dt: f32) { + body.v = (body.x - state.prev_x) * inv_dt + + delta_q := body.q * lg.quaternion_inverse(state.prev_q) + body.w = rl.Vector3{delta_q.x, delta_q.y, delta_q.z} * 2.0 * inv_dt + + if delta_q.w < 0 { + body.w = -body.w + } +} + apply_constraint_correction_unilateral :: proc( dt: f32, body: Body_Ptr,