From c2284c962b720743424090b0bb3de15948feb252 Mon Sep 17 00:00:00 2001 From: Megamouse Date: Thu, 11 Dec 2025 16:22:27 +0100 Subject: [PATCH] move ps_move_data to own file --- rpcs3/Emu/CMakeLists.txt | 1 + rpcs3/Emu/Io/pad_types.cpp | 128 ------------------------------- rpcs3/Emu/Io/pad_types.h | 75 +------------------ rpcs3/Emu/Io/ps_move_data.cpp | 137 ++++++++++++++++++++++++++++++++++ rpcs3/Emu/Io/ps_move_data.h | 75 +++++++++++++++++++ rpcs3/emucore.vcxproj | 2 + rpcs3/emucore.vcxproj.filters | 6 ++ 7 files changed, 223 insertions(+), 201 deletions(-) create mode 100644 rpcs3/Emu/Io/ps_move_data.cpp create mode 100644 rpcs3/Emu/Io/ps_move_data.h diff --git a/rpcs3/Emu/CMakeLists.txt b/rpcs3/Emu/CMakeLists.txt index 48674612c7..83e8a28398 100644 --- a/rpcs3/Emu/CMakeLists.txt +++ b/rpcs3/Emu/CMakeLists.txt @@ -411,6 +411,7 @@ target_sources(rpcs3_emu PRIVATE Io/pad_config_types.cpp Io/pad_types.cpp Io/PadHandler.cpp + Io/ps_move_data.cpp Io/rb3drums_config.cpp Io/RB3MidiDrums.cpp Io/RB3MidiGuitar.cpp diff --git a/rpcs3/Emu/Io/pad_types.cpp b/rpcs3/Emu/Io/pad_types.cpp index 2396a2ff74..ad3369a7a7 100644 --- a/rpcs3/Emu/Io/pad_types.cpp +++ b/rpcs3/Emu/Io/pad_types.cpp @@ -159,134 +159,6 @@ u32 get_axis_keycode(u32 offset, u16 value) } } -void ps_move_data::reset_sensors() -{ - quaternion = default_quaternion; - 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) { if (m_pressure_intensity_button_index < 0) diff --git a/rpcs3/Emu/Io/pad_types.h b/rpcs3/Emu/Io/pad_types.h index f8f86dfd54..5fd9c8973a 100644 --- a/rpcs3/Emu/Io/pad_types.h +++ b/rpcs3/Emu/Io/pad_types.h @@ -2,7 +2,8 @@ #include "util/types.hpp" #include "util/endian.hpp" -#include "Emu/Io/pad_config_types.h" +#include "pad_config_types.h" +#include "ps_move_data.h" #include #include @@ -469,78 +470,6 @@ 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 - std::array external_device_data{}; - bool external_device_connected = false; - bool external_device_read_requested = false; - bool external_device_write_requested = false; - - bool calibration_requested = false; - bool calibration_succeeded = false; - - bool magnetometer_enabled = false; - bool orientation_enabled = false; - - // 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 { const pad_handler m_pad_handler; diff --git a/rpcs3/Emu/Io/ps_move_data.cpp b/rpcs3/Emu/Io/ps_move_data.cpp new file mode 100644 index 0000000000..0a167eed39 --- /dev/null +++ b/rpcs3/Emu/Io/ps_move_data.cpp @@ -0,0 +1,137 @@ +#include "stdafx.h" +#include "ps_move_data.h" + +const ps_move_data::vect<4> ps_move_data::default_quaternion = ps_move_data::vect<4>({ 0.0f, 0.0f, 0.0f, 1.0f }); + +ps_move_data::ps_move_data() + : quaternion(default_quaternion) +{ +} + +void ps_move_data::reset_sensors() +{ + quaternion = default_quaternion; + 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; +} diff --git a/rpcs3/Emu/Io/ps_move_data.h b/rpcs3/Emu/Io/ps_move_data.h new file mode 100644 index 0000000000..1ae30f5c66 --- /dev/null +++ b/rpcs3/Emu/Io/ps_move_data.h @@ -0,0 +1,75 @@ +#pragma once + +struct ps_move_data +{ + template + struct vect + { + public: + constexpr 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 {}; + }; + + ps_move_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 + std::array external_device_data{}; + bool external_device_connected = false; + bool external_device_read_requested = false; + bool external_device_write_requested = false; + + bool calibration_requested = false; + bool calibration_succeeded = false; + + bool magnetometer_enabled = false; + bool orientation_enabled = false; + + // 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 const vect<4> default_quaternion; + vect<4> 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]); +}; diff --git a/rpcs3/emucore.vcxproj b/rpcs3/emucore.vcxproj index 77deb6088e..437d79df8a 100644 --- a/rpcs3/emucore.vcxproj +++ b/rpcs3/emucore.vcxproj @@ -91,6 +91,7 @@ + @@ -604,6 +605,7 @@ + diff --git a/rpcs3/emucore.vcxproj.filters b/rpcs3/emucore.vcxproj.filters index 6b15f662e5..17ccca3792 100644 --- a/rpcs3/emucore.vcxproj.filters +++ b/rpcs3/emucore.vcxproj.filters @@ -1393,6 +1393,9 @@ Emu\GPU\RSX\Program\Assembler + + Emu\Io + @@ -2800,6 +2803,9 @@ Emu\GPU\RSX\Program\Assembler + + Emu\Io +