From bb77e8e82181dd6aa668aee68f05056b380d00fe Mon Sep 17 00:00:00 2001 From: sergeypdev Date: Sat, 4 Jan 2025 20:05:24 +0400 Subject: [PATCH] Start of the physics engine with immediate mode api (waaat) --- build_debug.sh | 2 +- build_hot_reload.sh | 2 +- game/game.odin | 176 +++++++++++++++++++++++++++++------ game/physics/debug.odin | 42 +++++++++ game/physics/helpers.odin | 15 +++ game/physics/immediate.odin | 168 +++++++++++++++++++++++++++++++++ game/physics/physics.odin | 1 - game/physics/scene.odin | 172 ++++++++++++++++++++++++++++++++++ game/physics/simulation.odin | 120 ++++++++++++++++++++++++ 9 files changed, 667 insertions(+), 31 deletions(-) create mode 100644 game/physics/debug.odin create mode 100644 game/physics/helpers.odin create mode 100644 game/physics/immediate.odin delete mode 100644 game/physics/physics.odin create mode 100644 game/physics/scene.odin create mode 100644 game/physics/simulation.odin diff --git a/build_debug.sh b/build_debug.sh index 4be0c9f..4488dca 100755 --- a/build_debug.sh +++ b/build_debug.sh @@ -1,3 +1,3 @@ #!/usr/bin/env bash -odin build main_release -out:game_debug.bin -strict-style -vet -debug +odin build main_release -collection:common=./common -collection:game=./game -out:game_debug.bin -strict-style -vet -debug diff --git a/build_hot_reload.sh b/build_hot_reload.sh index 5a64161..e90397d 100755 --- a/build_hot_reload.sh +++ b/build_hot_reload.sh @@ -35,7 +35,7 @@ esac # Build the game. echo "Building game$DLL_EXT" -odin build game -extra-linker-flags:"$EXTRA_LINKER_FLAGS" -define:RAYLIB_SHARED=true -collection:common=./common -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug +odin build game -extra-linker-flags:"$EXTRA_LINKER_FLAGS" -define:RAYLIB_SHARED=true -collection:common=./common -collection:game=./game -build-mode:dll -out:game_tmp$DLL_EXT -strict-style -vet -debug # Need to use a temp file on Linux because it first writes an empty `game.so`, which the game will load before it is actually fully written. mv game_tmp$DLL_EXT game$DLL_EXT diff --git a/game/game.odin b/game/game.odin index 3cfcae9..ce230ac 100644 --- a/game/game.odin +++ b/game/game.odin @@ -20,6 +20,7 @@ import "core:fmt" import "core:log" import "core:math" import "core:math/linalg" +import "game:physics" import rl "vendor:raylib" import "vendor:raylib/rlgl" @@ -30,7 +31,22 @@ Track :: struct { } World :: struct { - track: Track, + track: Track, + physics_scene: physics.Scene, +} + +destroy_world :: proc(world: ^World) { + delete(world.track.points) + physics.destroy_physics_scene(&world.physics_scene) +} + +Car :: struct { + pos: rl.Vector3, +} + +SOLVER_CONFIG :: physics.Solver_Config { + timestep = 1.0 / 120, + gravity = rl.Vector3{0, 0, 0}, } Game_Memory :: struct { @@ -38,6 +54,9 @@ Game_Memory :: struct { 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, } @@ -92,13 +111,17 @@ camera_forward_vec :: proc() -> rl.Vector3 { } game_camera_3d :: proc() -> rl.Camera3D { - return { - position = g_mem.player_pos, - up = {0, 1, 0}, - fovy = 60, - target = g_mem.player_pos + camera_forward_vec(), - projection = .PERSPECTIVE, + if g_mem.editor { + return { + position = g_mem.player_pos, + up = {0, 1, 0}, + fovy = 60, + target = g_mem.player_pos + camera_forward_vec(), + projection = .PERSPECTIVE, + } } + + return g_mem.camera } ui_camera :: proc() -> rl.Camera2D { @@ -314,25 +337,107 @@ update_editor :: proc() { } } } - } update :: proc() { if rl.IsKeyPressed(.TAB) { g_mem.editor = !g_mem.editor - if g_mem.editor { - rl.EnableCursor() - } else { - rl.DisableCursor() - } + // if g_mem.editor { + // rl.EnableCursor() + // } else { + // rl.DisableCursor() + // } } - if g_mem.editor { + dt := rl.GetFrameTime() + + if !g_mem.editor { + car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb") + car_bounds := rl.GetModelBoundingBox(car_model) + + g_mem.car_handle = physics.immediate_body( + &get_world().physics_scene, + &g_mem.solver_state, + #hash("car", "fnv32a"), + physics.Body_Config { + initial_pos = {0, 1, 0}, + initial_rot = linalg.QUATERNIONF32_IDENTITY, + initial_ang_vel = {0, 1, 0}, + mass = 100, + inertia_tensor = physics.inertia_tensor_box(car_bounds.max - car_bounds.min), + }, + ) + + 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{0, 0, 10} + } + + // 1.6 is a good value + wheel_extent_x := f32(2) + rest := f32(1) + + physics.immediate_suspension_constraint( + &get_world().physics_scene, + &g_mem.solver_state, + #hash("FL", "fnv32a"), + { + rel_pos = {-wheel_extent_x, 0, 2.5}, + rel_dir = {0, -1, 0}, + rest = rest, + body = g_mem.car_handle, + }, + ) + physics.immediate_suspension_constraint( + &get_world().physics_scene, + &g_mem.solver_state, + #hash("FR", "fnv32a"), + { + rel_pos = {wheel_extent_x, 0, 2.5}, + rel_dir = {0, -1, 0}, + rest = rest, + body = g_mem.car_handle, + }, + ) + physics.immediate_suspension_constraint( + &get_world().physics_scene, + &g_mem.solver_state, + #hash("RL", "fnv32a"), + { + rel_pos = {-wheel_extent_x, 0, -3}, + rel_dir = {0, -1, 0}, + rest = rest, + body = g_mem.car_handle, + }, + ) + physics.immediate_suspension_constraint( + &get_world().physics_scene, + &g_mem.solver_state, + #hash("RR", "fnv32a"), + { + rel_pos = {wheel_extent_x, 0, -3}, + rel_dir = {0, -1, 0}, + rest = rest, + body = g_mem.car_handle, + }, + ) + + } else { update_free_look_camera() update_editor() } + + physics.simulate( + &g_mem.es.world.physics_scene, + &g_mem.solver_state, + SOLVER_CONFIG, + g_mem.editor ? 0 : dt, + ) } catmull_rom_coefs :: proc( @@ -384,22 +489,32 @@ draw :: proc() { defer rl.EndMode3D() if collision.hit { - tangent, bitangent := get_point_frame(interpolated_points, segment_idx) + // 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 + // 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), - ) - car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb") - car_model.transform = rl.MatrixRotate(axis, angle) - - rl.DrawModel(car_model, collision.point, 1, rl.WHITE) + // angle, axis := linalg.angle_axis_from_quaternion( + // linalg.quaternion_from_matrix3(rot_matrix), + // ) } + rl.DrawGrid(100, 1) + + if !g_mem.editor { + car_model := assets.get_model(&g_mem.assetman, "assets/toyota_corolla_ae86_trueno.glb") + + car_body := physics.get_body(&get_world().physics_scene, g_mem.car_handle) + car_matrix := rl.QuaternionToMatrix(car_body.q) + car_model.transform = car_matrix + + rl.DrawModel(car_model, car_body.x, 1, rl.WHITE) + } + + physics.draw_debug_scene(&get_world().physics_scene) + // road: rl.Mesh // defer rl.UnloadMesh(road) // road_vertices: [dynamic]f32 @@ -479,6 +594,9 @@ draw :: proc() { rl.ORANGE, ) } + } else { + car_pos := physics.get_body(&get_world().physics_scene, g_mem.car_handle).x + rl.DrawText(fmt.ctprintf("Car Pos: %v", car_pos), 5, 32, 8, rl.ORANGE) } } @@ -638,7 +756,7 @@ game_init_window :: proc() { rl.InitWindow(1280, 720, "Odin + Raylib + Hot Reload template!") rl.SetExitKey(.KEY_NULL) rl.SetWindowPosition(200, 200) - rl.SetTargetFPS(500) + rl.SetTargetFPS(60) } @(export) @@ -653,8 +771,10 @@ game_init :: proc() { @(export) game_shutdown :: proc() { assets.shutdown(&g_mem.assetman) - delete(get_world().track.points) + destroy_world(get_world()) delete(g_mem.es.point_selection) + physics.destroy_solver_state(&g_mem.solver_state) + free(g_mem) } diff --git a/game/physics/debug.odin b/game/physics/debug.odin new file mode 100644 index 0000000..e1eb80e --- /dev/null +++ b/game/physics/debug.odin @@ -0,0 +1,42 @@ +package physics + +import "core:log" +import lg "core:math/linalg" +import rl "vendor:raylib" + +_ :: log + +draw_debug_scene :: proc(scene: ^Scene) { + for &body in scene.bodies { + if body.alive { + pos := body.x + + q := body.q + x := lg.quaternion_mul_vector3(q, rl.Vector3{1, 0, 0}) + y := lg.quaternion_mul_vector3(q, rl.Vector3{0, 1, 0}) + z := lg.quaternion_mul_vector3(q, rl.Vector3{0, 0, 1}) + + rl.DrawLine3D(pos, pos + x, rl.RED) + rl.DrawLine3D(pos, pos + y, rl.GREEN) + rl.DrawLine3D(pos, pos + z, rl.BLUE) + } + } + + 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 + + pos := body.x + rot := body.q + pos += lg.quaternion_mul_vector3(rot, constraint.rel_pos) + dir := lg.quaternion_mul_vector3(rot, constraint.rel_dir) + + rl.DrawLine3D(pos, pos + dir * t, rl.ORANGE) + + if constraint.hit { + rl.DrawSphereWires(constraint.hit_point, 0.1, 4, 4, rl.RED) + } + } + } +} diff --git a/game/physics/helpers.odin b/game/physics/helpers.odin new file mode 100644 index 0000000..fef5d84 --- /dev/null +++ b/game/physics/helpers.odin @@ -0,0 +1,15 @@ +package physics + +import rl "vendor:raylib" + +inertia_tensor_box :: proc(size: rl.Vector3) -> (tensor: rl.Vector3) { + CONSTANT :: f32(1.0 / 12.0) + + tensor.x = size.z * size.z + size.y * size.y + tensor.y = size.x * size.x + size.z * size.z + tensor.z = size.x * size.x + size.y * size.y + + tensor *= CONSTANT + + return +} diff --git a/game/physics/immediate.odin b/game/physics/immediate.odin new file mode 100644 index 0000000..ba6ed9c --- /dev/null +++ b/game/physics/immediate.odin @@ -0,0 +1,168 @@ +package physics + +import rl "vendor:raylib" + +// 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, + mass: f32, + // Unit inertia tensor + inertia_tensor: rl.Vector3, +} + +Suspension_Constraint_Config :: struct { + rel_pos: rl.Vector3, + rel_dir: rl.Vector3, + body: Body_Handle, + rest: f32, + compliance: f32, +} + +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_intertia_tensor = 1.0 / (config.inertia_tensor * config.mass) +} + +update_body_from_config :: proc(body: Body_Ptr, config: Body_Config) { + body.inv_mass = 1.0 / config.mass + body.inv_intertia_tensor = 1.0 / (config.inertia_tensor * config.mass) +} + +update_suspension_constraint_from_config :: proc( + constraint: Suspension_Constraint_Ptr, + config: Suspension_Constraint_Config, +) { + constraint.rel_pos = config.rel_pos + constraint.rel_dir = config.rel_dir + constraint.body = config.body + constraint.rest = config.rest + constraint.compliance = config.compliance +} + +immediate_body :: proc( + scene: ^Scene, + state: ^Solver_State, + id: u32, + config: Body_Config, +) -> ( + handle: Body_Handle, +) { + if id in state.immedate_bodies { + body := &state.immedate_bodies[id] + if body.last_ref != state.simulation_frame { + body.last_ref = state.simulation_frame + state.num_referenced_bodies += 1 + } + handle = body.handle + } else { + new_body: Body + state.num_referenced_bodies += 1 + initialize_body_from_config(&new_body, config) + handle = add_body(scene, new_body) + state.immedate_bodies[id] = { + handle = handle, + last_ref = state.simulation_frame, + } + } + + return +} + +immediate_suspension_constraint :: proc( + scene: ^Scene, + state: ^Solver_State, + id: u32, + config: Suspension_Constraint_Config, +) -> ( + handle: Suspension_Constraint_Handle, +) { + if id in state.immediate_suspension_constraints { + constraint := &state.immediate_suspension_constraints[id] + if constraint.last_ref != state.simulation_frame { + constraint.last_ref = state.simulation_frame + state.num_referenced_suspension_constraints += 1 + } + handle = constraint.handle + } else { + state.num_referenced_suspension_constraints += 1 + handle = add_suspension_constraint(scene, {}) + state.immediate_suspension_constraints[id] = { + handle = handle, + last_ref = state.simulation_frame, + } + } + + update_suspension_constraint_from_config(get_suspension_constraint(scene, handle), config) + + return +} + +prune_immediate :: proc(scene: ^Scene, state: ^Solver_State) { + prune_immediate_bodies(scene, state) + prune_immediate_suspension_constraints(scene, state) +} + +// TODO: Generic version +prune_immediate_bodies :: proc(scene: ^Scene, state: ^Solver_State) { + if int(state.num_referenced_bodies) == len(state.immedate_bodies) { + return + } + + num_unreferenced_bodies := len(state.immedate_bodies) - int(state.num_referenced_bodies) + assert(num_unreferenced_bodies >= 0) + + bodies_to_remove := make([]u32, num_unreferenced_bodies, context.temp_allocator) + + i := 0 + for k, &v in state.immedate_bodies { + if v.last_ref != state.simulation_frame { + bodies_to_remove[i] = k + i += 1 + } + } + + assert(i == len(bodies_to_remove)) + + for k in bodies_to_remove { + handle := state.immedate_bodies[k].handle + delete_key(&state.immedate_bodies, k) + remove_body(scene, handle) + } +} + +prune_immediate_suspension_constraints :: proc(scene: ^Scene, state: ^Solver_State) { + if int(state.num_referenced_suspension_constraints) == + len(state.immediate_suspension_constraints) { + return + } + + num_unreferenced_constraints := + len(state.immediate_suspension_constraints) - + int(state.num_referenced_suspension_constraints) + assert(num_unreferenced_constraints >= 0) + + constraints_to_remove := make([]u32, num_unreferenced_constraints, context.temp_allocator) + + i := 0 + for k, &v in state.immediate_suspension_constraints { + if v.last_ref != state.simulation_frame { + constraints_to_remove[i] = k + i += 1 + } + } + + assert(i == len(constraints_to_remove)) + + for k in constraints_to_remove { + handle := state.immediate_suspension_constraints[k].handle + delete_key(&state.immediate_suspension_constraints, k) + remove_suspension_constraint(scene, handle) + } +} diff --git a/game/physics/physics.odin b/game/physics/physics.odin deleted file mode 100644 index e055fc2..0000000 --- a/game/physics/physics.odin +++ /dev/null @@ -1 +0,0 @@ -package physics diff --git a/game/physics/scene.odin b/game/physics/scene.odin new file mode 100644 index 0000000..dfe0357 --- /dev/null +++ b/game/physics/scene.odin @@ -0,0 +1,172 @@ +package physics + +import rl "vendor:raylib" + +Scene :: struct { + bodies: #soa[dynamic]Body, + suspension_constraints: #soa[dynamic]Suspension_Constraint, + first_free_body_plus_one: i32, + first_free_suspension_constraint_plus_one: i32, +} + +Body :: struct { + // Is this body alive (if not it doesn't exist) + alive: bool, + // Pos + x: rl.Vector3, + // Linear vel + v: rl.Vector3, + // Orientation + q: rl.Quaternion, + // Angular vel (omega) + w: rl.Vector3, + // Mass + inv_mass: f32, + // Moment of inertia + inv_intertia_tensor: rl.Vector3, + // + next_plus_one: i32, +} + +Suspension_Constraint :: struct { + alive: bool, + // Pos relative to the body + rel_pos: rl.Vector3, + // Dir relative to the body + rel_dir: rl.Vector3, + // Handle of the rigid body + body: Body_Handle, + // Rest distance + rest: f32, + // Inverse stiffness + compliance: f32, + + // Runtime state + hit: bool, + hit_point: rl.Vector3, + // rel_hit_point = rel_pos + rel_dir * hit_t + hit_t: f32, + + // Free list + next_plus_one: i32, +} + +// Index plus one, so handle 0 maps to invalid body +Body_Handle :: distinct i32 +Suspension_Constraint_Handle :: distinct i32 + +is_body_handle_valid :: proc(handle: Body_Handle) -> bool { + return i32(handle) > 0 +} +is_suspension_constraint_handle_valid :: proc(handle: Suspension_Constraint_Handle) -> bool { + return i32(handle) > 0 +} +is_handle_valid :: proc { + is_body_handle_valid, + is_suspension_constraint_handle_valid, +} + +Body_Ptr :: #soa^#soa[]Body +Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint + +_invalid_body: #soa[1]Body +_invalid_suspension_constraint: #soa[1]Suspension_Constraint + +/// Returns pointer to soa slice. NEVER STORE IT +get_body :: proc(scene: ^Scene, handle: Body_Handle) -> Body_Ptr { + index := int(handle) - 1 + if index < 0 { + slice := _invalid_body[:] + return &slice[0] + } + + bodies_slice := scene.bodies[:] + return &bodies_slice[index] +} + +add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle { + body_copy := body + + body_copy.alive = true + body_copy.next_plus_one = 0 + + if scene.first_free_body_plus_one > 1 { + index := scene.first_free_body_plus_one + new_body := get_body(scene, Body_Handle(index)) + next_plus_one := new_body.next_plus_one + new_body^ = body_copy + scene.first_free_body_plus_one = next_plus_one + return Body_Handle(index) + } + + append_soa(&scene.bodies, body_copy) + index := len(scene.bodies) + return Body_Handle(index) +} + +remove_body :: proc(scene: ^Scene, handle: Body_Handle) { + if int(handle) > 1 { + body := get_body(scene, handle) + + body.alive = false + body.next_plus_one = scene.first_free_body_plus_one + scene.first_free_body_plus_one = i32(handle) + } +} + +/// Returns pointer to soa slice. NEVER STORE IT +get_suspension_constraint :: proc( + scene: ^Scene, + handle: Suspension_Constraint_Handle, +) -> Suspension_Constraint_Ptr { + if !is_handle_valid(handle) { + slice := _invalid_suspension_constraint[:] + return &slice[0] + } + + index := int(handle) - 1 + slice := scene.suspension_constraints[:] + return &slice[index] +} + +add_suspension_constraint :: proc( + scene: ^Scene, + constraint: Suspension_Constraint, +) -> Suspension_Constraint_Handle { + copy := constraint + + copy.alive = true + copy.next_plus_one = 0 + + if scene.first_free_suspension_constraint_plus_one > 0 { + index := scene.first_free_suspension_constraint_plus_one + new_constraint := get_suspension_constraint(scene, Suspension_Constraint_Handle(index)) + next_plus_one := new_constraint.next_plus_one + new_constraint^ = copy + scene.first_free_suspension_constraint_plus_one = next_plus_one + return Suspension_Constraint_Handle(index) + } + + append_soa(&scene.suspension_constraints, copy) + index := len(scene.suspension_constraints) + return Suspension_Constraint_Handle(index) +} + +remove_suspension_constraint :: proc(scene: ^Scene, handle: Suspension_Constraint_Handle) { + if is_handle_valid(handle) { + constraint := get_suspension_constraint(scene, handle) + + constraint.alive = false + constraint.next_plus_one = scene.first_free_suspension_constraint_plus_one + scene.first_free_suspension_constraint_plus_one = i32(handle) + } +} + +_get_first_free_body :: proc(scene: ^Scene) -> i32 { + return scene.first_free_body_plus_one - 1 +} + +destroy_physics_scene :: proc(scene: ^Scene) { + delete_soa(scene.bodies) + delete_soa(scene.suspension_constraints) +} diff --git a/game/physics/simulation.odin b/game/physics/simulation.odin new file mode 100644 index 0000000..083ee6c --- /dev/null +++ b/game/physics/simulation.odin @@ -0,0 +1,120 @@ +package physics + +import "collision" +import lg "core:math/linalg" +import rl "vendor:raylib" + +Solver_Config :: struct { + // Will automatically do fixed timestep + timestep: f32, + gravity: rl.Vector3, +} + +Solver_State :: struct { + accumulated_time: f32, + // Incremented when simulate is called (not simulate_step) + simulation_frame: u32, + + // Number of immediate bodies referenced this frame + num_referenced_bodies: i32, + num_referenced_suspension_constraints: i32, + immedate_bodies: map[u32]Immedate_State(Body_Handle), + immediate_suspension_constraints: map[u32]Immedate_State(Suspension_Constraint_Handle), +} + +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) + + prune_immediate(scene, state) + + state.accumulated_time += dt + + num_steps := 0 + for state.accumulated_time >= config.timestep { + num_steps += 1 + state.accumulated_time -= config.timestep + + simulate_step(scene, config) + } + + state.simulation_frame += 1 + state.num_referenced_bodies = 0 + state.num_referenced_suspension_constraints = 0 +} + +Body_Sim_State :: struct { + prev_x: rl.Vector3, + prev_q: rl.Quaternion, +} + +simulate_step :: proc(scene: ^Scene, config: Solver_Config) { + body_states := make_soa(#soa[]Body_Sim_State, len(scene.bodies), context.temp_allocator) + + dt := config.timestep + 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 + + 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) + + body.q = q + } + } + + for &v in scene.suspension_constraints { + if v.alive { + body := get_body(scene, v.body) + + q := body.q + pos := body.x + pos += lg.quaternion_mul_vector3(q, 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}), + ) + } + } + + // Compute new linear and angular velocities + for &body, i in scene.bodies { + 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 + } + } + } +}