From 757e9a04939555c10054550688bd4cbff32f4f94 Mon Sep 17 00:00:00 2001 From: Megamouse Date: Thu, 11 Dec 2025 04:50:21 +0100 Subject: [PATCH] cellGem: implement world coordinate orientation in cellGemGetState --- rpcs3/Emu/Cell/Modules/cellGem.cpp | 44 +++++++--- rpcs3/Emu/Io/PadHandler.cpp | 31 +++---- rpcs3/Emu/Io/pad_types.cpp | 132 +++++++++++++++++++++++++++-- rpcs3/Emu/Io/pad_types.h | 62 +++++++++++--- rpcs3/Input/ps_move_handler.cpp | 18 ++-- 5 files changed, 232 insertions(+), 55 deletions(-) diff --git a/rpcs3/Emu/Cell/Modules/cellGem.cpp b/rpcs3/Emu/Cell/Modules/cellGem.cpp index b4902835f0..fccf164b94 100644 --- a/rpcs3/Emu/Cell/Modules/cellGem.cpp +++ b/rpcs3/Emu/Cell/Modules/cellGem.cpp @@ -276,6 +276,8 @@ public: u64 start_timestamp_us = 0; + std::array mouse_move_data {}; // No need to be in savestate + atomic_t m_wake_up = 0; atomic_t m_done = 0; @@ -1657,7 +1659,7 @@ static inline void pos_to_gem_image_state(u32 gem_num, gem_config::gem_controlle } } -static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& controller, vm::ptr& gem_state, s32 x_pos, s32 y_pos, s32 x_max, s32 y_max, const ps_move_data& move_data) +static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& controller, vm::ptr& gem_state, s32 x_pos, s32 y_pos, s32 x_max, s32 y_max, ps_move_data& move_data) { const auto& shared_data = g_fxo->get(); @@ -1733,6 +1735,17 @@ static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& con gem_state->quat[3] = q_w; } + if constexpr (!ps_move_data::use_imu_for_velocity) + { + move_data.update_velocity(shared_data.frame_timestamp_us, gem_state->pos); + + for (u32 i = 0; i < 3; i++) + { + gem_state->vel[i] = move_data.vel_world[i]; + gem_state->accel[i] = move_data.accel_world[i]; + } + } + // Update visibility for fake handlers if (g_cfg.io.move != move_handler::real) { @@ -1906,9 +1919,17 @@ static void ps_move_pos_to_gem_state(u32 gem_num, gem_config::gem_controller& co if constexpr (std::is_same_v>) { gem_state->temperature = pad->move_data.temperature; - gem_state->accel[0] = pad->move_data.accelerometer_x * 1000; // linear velocity in mm/s² - gem_state->accel[1] = pad->move_data.accelerometer_y * 1000; // linear velocity in mm/s² - gem_state->accel[2] = pad->move_data.accelerometer_z * 1000; // linear velocity in mm/s² + + for (u32 i = 0; i < 3; i++) + { + if constexpr (ps_move_data::use_imu_for_velocity) + { + gem_state->vel[i] = pad->move_data.vel_world[i]; + gem_state->accel[i] = pad->move_data.accel_world[i]; + } + gem_state->angvel[i] = pad->move_data.angvel_world[i]; + gem_state->angaccel[i] = pad->move_data.angaccel_world[i]; + } pos_to_gem_state(gem_num, controller, gem_state, info.x_pos, info.y_pos, info.x_max, info.y_max, pad->move_data); } @@ -2147,7 +2168,8 @@ static void mouse_pos_to_gem_state(u32 mouse_no, gem_config::gem_controller& con if constexpr (std::is_same_v>) { - pos_to_gem_state(mouse_no, controller, gem_state, mouse.x_pos, mouse.y_pos, mouse.x_max, mouse.y_max, {}); + ps_move_data& move_data = ::at32(g_fxo->get().mouse_move_data, mouse_no); + pos_to_gem_state(mouse_no, controller, gem_state, mouse.x_pos, mouse.y_pos, mouse.x_max, mouse.y_max, move_data); } else if constexpr (std::is_same_v>) { @@ -2786,12 +2808,12 @@ error_code cellGemGetInertialState(u32 gem_num, u32 state_flag, u64 timestamp, v if (pad && pad->is_connected() && !pad->is_copilot()) { inertial_state->temperature = pad->move_data.temperature; - inertial_state->accelerometer[0] = pad->move_data.accelerometer_x; - inertial_state->accelerometer[1] = pad->move_data.accelerometer_y; - inertial_state->accelerometer[2] = pad->move_data.accelerometer_z; - inertial_state->gyro[0] = pad->move_data.gyro_x; - inertial_state->gyro[1] = pad->move_data.gyro_y; - inertial_state->gyro[2] = pad->move_data.gyro_z; + + for (u32 i = 0; i < 3; i++) + { + inertial_state->accelerometer[i] = pad->move_data.accelerometer[i]; + inertial_state->gyro[i] = pad->move_data.gyro[i]; + } } } diff --git a/rpcs3/Emu/Io/PadHandler.cpp b/rpcs3/Emu/Io/PadHandler.cpp index 175cd3e90a..d25ba1379c 100644 --- a/rpcs3/Emu/Io/PadHandler.cpp +++ b/rpcs3/Emu/Io/PadHandler.cpp @@ -874,12 +874,12 @@ void PadHandlerBase::set_raw_orientation(ps_move_data& move_data, f32 accel_x, f // The default position is flat on the ground, pointing forward. // The accelerometers constantly measure G forces. // The gyros measure changes in orientation and will reset when the device isn't moved anymore. - move_data.accelerometer_x = -accel_x; // move_data: Increases if the device is rolled to the left - move_data.accelerometer_y = accel_z; // move_data: Increases if the device is pitched upwards - move_data.accelerometer_z = accel_y; // move_data: Increases if the device is moved upwards - move_data.gyro_x = degree_to_rad(-gyro_x); // move_data: Increases if the device is pitched upwards - move_data.gyro_y = degree_to_rad(gyro_z); // move_data: Increases if the device is rolled to the right - move_data.gyro_z = degree_to_rad(-gyro_y); // move_data: Increases if the device is yawed to the left + move_data.accelerometer.x() = -accel_x; // move_data: Increases if the device is rolled to the left + move_data.accelerometer.y() = accel_z; // move_data: Increases if the device is pitched upwards + move_data.accelerometer.z() = accel_y; // move_data: Increases if the device is moved upwards + move_data.gyro.x() = degree_to_rad(-gyro_x); // move_data: Increases if the device is pitched upwards + move_data.gyro.y() = degree_to_rad(gyro_z); // move_data: Increases if the device is rolled to the right + move_data.gyro.z() = degree_to_rad(-gyro_y); // move_data: Increases if the device is yawed to the left } void PadHandlerBase::set_raw_orientation(Pad& pad) @@ -959,17 +959,17 @@ void PadDevice::update_orientation(ps_move_data& move_data) const FusionVector accelerometer{ .axis { - .x = -move_data.accelerometer_x, - .y = +move_data.accelerometer_y, - .z = +move_data.accelerometer_z + .x = -move_data.accelerometer.x(), + .y = +move_data.accelerometer.y(), + .z = +move_data.accelerometer.z() } }; const FusionVector gyroscope{ .axis { - .x = +PadHandlerBase::rad_to_degree(move_data.gyro_x), - .y = +PadHandlerBase::rad_to_degree(move_data.gyro_z), - .z = -PadHandlerBase::rad_to_degree(move_data.gyro_y) + .x = +PadHandlerBase::rad_to_degree(move_data.gyro.x()), + .y = +PadHandlerBase::rad_to_degree(move_data.gyro.z()), + .z = -PadHandlerBase::rad_to_degree(move_data.gyro.y()) } }; @@ -979,9 +979,9 @@ void PadDevice::update_orientation(ps_move_data& move_data) { magnetometer = FusionVector{ .axis { - .x = move_data.magnetometer_x, - .y = move_data.magnetometer_y, - .z = move_data.magnetometer_z + .x = move_data.magnetometer.x(), + .y = move_data.magnetometer.y(), + .z = move_data.magnetometer.z() } }; } @@ -995,4 +995,5 @@ void PadDevice::update_orientation(ps_move_data& move_data) move_data.quaternion[1] = quaternion.array[2]; move_data.quaternion[2] = quaternion.array[3]; move_data.quaternion[3] = quaternion.array[0]; + move_data.update_orientation(elapsed_sec); } diff --git a/rpcs3/Emu/Io/pad_types.cpp b/rpcs3/Emu/Io/pad_types.cpp index 9005bc7fe5..2396a2ff74 100644 --- a/rpcs3/Emu/Io/pad_types.cpp +++ b/rpcs3/Emu/Io/pad_types.cpp @@ -162,15 +162,129 @@ u32 get_axis_keycode(u32 offset, u16 value) void ps_move_data::reset_sensors() { quaternion = default_quaternion; - accelerometer_x = 0.0f; - accelerometer_y = 0.0f; - accelerometer_z = 0.0f; - gyro_x = 0.0f; - gyro_y = 0.0f; - gyro_z = 0.0f; - magnetometer_x = 0.0f; - magnetometer_y = 0.0f; - magnetometer_z = 0.0f; + accelerometer = {}; + gyro = {}; + prev_gyro = {}; + angular_acceleration = {}; + magnetometer = {}; + //prev_pos_world = {}; // probably no reset needed ? + vel_world = {}; + prev_vel_world = {}; + accel_world = {}; + angvel_world = {}; + angaccel_world = {}; +} + +void ps_move_data::update_orientation(f32 delta_time) +{ + if (!delta_time) + return; + + // Rotate vector v by quaternion q + const auto rotate_vector = [](const vect<4>& q, const vect<3>& v) + { + const vect<4> qv({0.0f, v.x(), v.y(), v.z()}); + const vect<4> q_inv({q.w(), -q.x(), -q.y(), -q.z()}); + + // t = q * v + vect<4> t; + t.w() = -q.x() * qv.x() - q.y() * qv.y() - q.z() * qv.z(); + t.x() = q.w() * qv.x() + q.y() * qv.z() - q.z() * qv.y(); + t.y() = q.w() * qv.y() - q.x() * qv.z() + q.z() * qv.x(); + t.z() = q.w() * qv.z() + q.x() * qv.y() - q.y() * qv.x(); + + // r = t * q_inv + vect<4> r; + r.w() = -t.x() * q_inv.x() - t.y() * q_inv.y() - t.z() * q_inv.z(); + r.x() = t.w() * q_inv.x() + t.y() * q_inv.z() - t.z() * q_inv.y(); + r.y() = t.w() * q_inv.y() - t.x() * q_inv.z() + t.z() * q_inv.x(); + r.z() = t.w() * q_inv.z() + t.x() * q_inv.y() - t.y() * q_inv.x(); + + return vect<3>({r.x(), r.y(), r.z()}); + }; + + if constexpr (use_imu_for_velocity) + { + // Gravity in world frame + constexpr f32 gravity = 9.81f; + constexpr vect<3> g({0.0f, 0.0f, -gravity}); + + // Rotate gravity into sensor frame + const vect<3> g_sensor = rotate_vector(quaternion, g); + + // Remove gravity + vect<3> linear_local; + for (u32 i = 0; i < 3; i++) + { + linear_local[i] = (accelerometer[i] * gravity) - g_sensor[i]; + } + + // Linear acceleration (rotate to world coordinates) + accel_world = rotate_vector(quaternion, linear_local); + + // convert to mm/s² + for (u32 i = 0; i < 3; i++) + { + accel_world[i] *= 1000.0f; + } + + // Linear velocity (integrate acceleration) + for (u32 i = 0; i < 3; i++) + { + vel_world[i] = prev_vel_world[i] + accel_world[i] * delta_time; + } + + prev_vel_world = vel_world; + } + + // Compute raw angular acceleration + for (u32 i = 0; i < 3; i++) + { + const f32 alpha = (gyro[i] - prev_gyro[i]) / delta_time; + + // Filtering + constexpr f32 weight = 0.8f; + constexpr f32 weight_inv = 1.0f - weight; + angular_acceleration[i] = weight * angular_acceleration[i] + weight_inv * alpha; + } + + // Angular velocity (rotate to world coordinates) + angvel_world = rotate_vector(quaternion, gyro); + + // Angular acceleration (rotate to world coordinates) + angaccel_world = rotate_vector(quaternion, angular_acceleration); + + prev_gyro = gyro; +} + +void ps_move_data::update_velocity(u64 timestamp, be_t pos_world[4]) +{ + if constexpr (use_imu_for_velocity) + return; + + if (last_velocity_update_time_us == timestamp) + return; + + // Get elapsed time since last update + const f32 delta_time = (last_velocity_update_time_us == 0) ? 0.0f : ((timestamp - last_velocity_update_time_us) / 1'000'000.0f); + last_velocity_update_time_us = timestamp; + + if (!delta_time) + return; + + for (u32 i = 0; i < 3; i++) + { + // Linear velocity + constexpr f32 weight = 0.8f; + constexpr f32 weight_inv = 1.0f - weight; + vel_world[i] = weight * ((pos_world[i] - prev_pos_world[i]) / delta_time) + weight_inv * prev_vel_world[i]; + prev_pos_world[i] = pos_world[i]; + + // Linear acceleration + accel_world[i] = (vel_world[i] - prev_vel_world[i]) / delta_time; + } + + prev_vel_world = vel_world; } bool Pad::get_pressure_intensity_button_active(bool is_toggle_mode, u32 player_id) diff --git a/rpcs3/Emu/Io/pad_types.h b/rpcs3/Emu/Io/pad_types.h index eeeec3c658..f8f86dfd54 100644 --- a/rpcs3/Emu/Io/pad_types.h +++ b/rpcs3/Emu/Io/pad_types.h @@ -471,6 +471,33 @@ struct VibrateMotor struct ps_move_data { + template + struct vect + { + public: + vect() = default; + constexpr vect(const std::array& vec) : data(vec) {}; + + template + T& operator[](I i) { return data[i]; } + + template + const T& operator[](I i) const { return data[i]; } + + T x() const requires (Size >= 1) { return data[0]; } + T y() const requires (Size >= 2) { return data[1]; } + T z() const requires (Size >= 3) { return data[2]; } + T w() const requires (Size >= 4) { return data[3]; } + + T& x() requires (Size >= 1) { return data[0]; } + T& y() requires (Size >= 2) { return data[1]; } + T& z() requires (Size >= 3) { return data[2]; } + T& w() requires (Size >= 4) { return data[3]; } + + private: + std::array data {}; + }; + u32 external_device_id = 0; std::array external_device_read{}; // CELL_GEM_EXTERNAL_PORT_DEVICE_INFO_SIZE std::array external_device_write{}; // CELL_GEM_EXTERNAL_PORT_OUTPUT_SIZE @@ -485,20 +512,33 @@ struct ps_move_data bool magnetometer_enabled = false; bool orientation_enabled = false; - static constexpr std::array default_quaternion { 0.0f, 0.0f, 0.0f, 1.0f }; - std::array quaternion = default_quaternion; // quaternion orientation (x,y,z,w) of controller relative to default (facing the camera with buttons up) - f32 accelerometer_x = 0.0f; // linear velocity in m/s² - f32 accelerometer_y = 0.0f; // linear velocity in m/s² - f32 accelerometer_z = 0.0f; // linear velocity in m/s² - f32 gyro_x = 0.0f; // angular velocity in rad/s - f32 gyro_y = 0.0f; // angular velocity in rad/s - f32 gyro_z = 0.0f; // angular velocity in rad/s - f32 magnetometer_x = 0.0f; - f32 magnetometer_y = 0.0f; - f32 magnetometer_z = 0.0f; + // Disable IMU tracking of velocity and acceleration (massive drift) + static constexpr bool use_imu_for_velocity = false; + u64 last_velocity_update_time_us = 0; + + static constexpr vect<4> default_quaternion = vect<4>({ 0.0f, 0.0f, 0.0f, 1.0f }); + vect<4> quaternion = default_quaternion; // quaternion orientation (x,y,z,w) of controller relative to default (facing the camera with buttons up) + + // Raw values (local) + vect<3> accelerometer {}; // linear acceleration in G units (9.81 = 1 unit) + vect<3> gyro {}; // angular velocity in rad/s + vect<3> prev_gyro {}; // previous angular velocity in rad/s + vect<3> angular_acceleration {}; // angular acceleration in rad/s² + vect<3> magnetometer {}; + + // In world coordinates + vect<3> prev_pos_world {}; + vect<3> vel_world {}; // velocity of sphere in world coordinates (mm/s) + vect<3> prev_vel_world {}; // previous velocity of sphere in world coordinates (mm/s) + vect<3> accel_world {}; // acceleration of sphere in world coordinates (mm/s²) + vect<3> angvel_world {}; // angular velocity of controller in world coordinates (radians/s) + vect<3> angaccel_world {}; // angular acceleration of controller in world coordinates (radians/s²) + s16 temperature = 0; void reset_sensors(); + void update_orientation(f32 delta_time); + void update_velocity(u64 timestamp, be_t pos_world[4]); }; struct Pad diff --git a/rpcs3/Input/ps_move_handler.cpp b/rpcs3/Input/ps_move_handler.cpp index acf5fac3b7..f37fc8c68f 100644 --- a/rpcs3/Input/ps_move_handler.cpp +++ b/rpcs3/Input/ps_move_handler.cpp @@ -711,21 +711,21 @@ void ps_move_handler::get_extended_info(const pad_ensemble& binding) gyro_z /= MOVE_ONE_G; } - pad->move_data.accelerometer_x = accel_x; - pad->move_data.accelerometer_y = accel_y; - pad->move_data.accelerometer_z = accel_z; - pad->move_data.gyro_x = gyro_x; - pad->move_data.gyro_y = gyro_y; - pad->move_data.gyro_z = gyro_z; + pad->move_data.accelerometer.x() = accel_x; + pad->move_data.accelerometer.y() = accel_y; + pad->move_data.accelerometer.z() = accel_z; + pad->move_data.gyro.x() = gyro_x; + pad->move_data.gyro.y() = gyro_y; + pad->move_data.gyro.z() = gyro_z; if (dev->model == ps_move_model::ZCM1) { const ps_move_input_report_ZCM1& input_zcm1 = dev->input_report_ZCM1; #define TWELVE_BIT_SIGNED(x) (((x) & 0x800) ? (-(((~(x)) & 0xFFF) + 1)) : (x)) - pad->move_data.magnetometer_x = static_cast(TWELVE_BIT_SIGNED(((input.magnetometer_x & 0x0F) << 8) | input_zcm1.magnetometer_x2)); - pad->move_data.magnetometer_y = static_cast(TWELVE_BIT_SIGNED((input_zcm1.magnetometer_y << 4) | (input_zcm1.magnetometer_yz & 0xF0) >> 4)); - pad->move_data.magnetometer_z = static_cast(TWELVE_BIT_SIGNED(((input_zcm1.magnetometer_yz & 0x0F) << 8) | input_zcm1.magnetometer_z)); + pad->move_data.magnetometer.x() = static_cast(TWELVE_BIT_SIGNED(((input.magnetometer_x & 0x0F) << 8) | input_zcm1.magnetometer_x2)); + pad->move_data.magnetometer.y() = static_cast(TWELVE_BIT_SIGNED((input_zcm1.magnetometer_y << 4) | (input_zcm1.magnetometer_yz & 0xF0) >> 4)); + pad->move_data.magnetometer.z() = static_cast(TWELVE_BIT_SIGNED(((input_zcm1.magnetometer_yz & 0x0F) << 8) | input_zcm1.magnetometer_z)); } }