mirror of https://github.com/RPCS3/rpcs3
cellGem: implement world coordinate orientation in cellGemGetState
This commit is contained in:
Megamouse
parent
f7cda4b2b4
commit
757e9a0493
|
|
@ -276,6 +276,8 @@ public:
|
|||
|
||||
u64 start_timestamp_us = 0;
|
||||
|
||||
std::array<ps_move_data, CELL_GEM_MAX_NUM> mouse_move_data {}; // No need to be in savestate
|
||||
|
||||
atomic_t<u32> m_wake_up = 0;
|
||||
atomic_t<u32> 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<CellGemState>& 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<CellGemState>& 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<gem_camera_shared>();
|
||||
|
||||
|
|
@ -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<T, vm::ptr<CellGemState>>)
|
||||
{
|
||||
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<T, vm::ptr<CellGemState>>)
|
||||
{
|
||||
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<gem_config>().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<T, vm::ptr<CellGemImageState>>)
|
||||
{
|
||||
|
|
@ -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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<f32> 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)
|
||||
|
|
|
|||
|
|
@ -471,6 +471,33 @@ struct VibrateMotor
|
|||
|
||||
struct ps_move_data
|
||||
{
|
||||
template <int Size, typename T = f32>
|
||||
struct vect
|
||||
{
|
||||
public:
|
||||
vect() = default;
|
||||
constexpr vect(const std::array<T, Size>& vec) : data(vec) {};
|
||||
|
||||
template <typename I>
|
||||
T& operator[](I i) { return data[i]; }
|
||||
|
||||
template <typename I>
|
||||
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<T, Size> data {};
|
||||
};
|
||||
|
||||
u32 external_device_id = 0;
|
||||
std::array<u8, 38> external_device_read{}; // CELL_GEM_EXTERNAL_PORT_DEVICE_INFO_SIZE
|
||||
std::array<u8, 40> 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<f32, 4> default_quaternion { 0.0f, 0.0f, 0.0f, 1.0f };
|
||||
std::array<f32, 4> 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<f32> pos_world[4]);
|
||||
};
|
||||
|
||||
struct Pad
|
||||
|
|
|
|||
|
|
@ -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<f32>(TWELVE_BIT_SIGNED(((input.magnetometer_x & 0x0F) << 8) | input_zcm1.magnetometer_x2));
|
||||
pad->move_data.magnetometer_y = static_cast<f32>(TWELVE_BIT_SIGNED((input_zcm1.magnetometer_y << 4) | (input_zcm1.magnetometer_yz & 0xF0) >> 4));
|
||||
pad->move_data.magnetometer_z = static_cast<f32>(TWELVE_BIT_SIGNED(((input_zcm1.magnetometer_yz & 0x0F) << 8) | input_zcm1.magnetometer_z));
|
||||
pad->move_data.magnetometer.x() = static_cast<f32>(TWELVE_BIT_SIGNED(((input.magnetometer_x & 0x0F) << 8) | input_zcm1.magnetometer_x2));
|
||||
pad->move_data.magnetometer.y() = static_cast<f32>(TWELVE_BIT_SIGNED((input_zcm1.magnetometer_y << 4) | (input_zcm1.magnetometer_yz & 0xF0) >> 4));
|
||||
pad->move_data.magnetometer.z() = static_cast<f32>(TWELVE_BIT_SIGNED(((input_zcm1.magnetometer_yz & 0x0F) << 8) | input_zcm1.magnetometer_z));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue