diff --git a/src/common/vector_math.h b/src/common/vector_math.h index 6624b01e7e..f99fdcb01e 100644 --- a/src/common/vector_math.h +++ b/src/common/vector_math.h @@ -7,258 +7,100 @@ #pragma once -#ifdef __ARM_NEON -#include -#endif - #include #include namespace Common { -template -class Vec2; -template -class Vec3; -template -class Quaternion; - -template -class Vec2 { +template +class Vec { public: - T x{}; - T y{}; + std::array elems{}; - constexpr Vec2() = default; - constexpr Vec2(const T& x_, const T& y_) noexcept : x(x_), y(y_) {} + constexpr Vec() = default; + constexpr Vec(T e0) noexcept : elems{e0} {} + constexpr Vec(T e0, T e1) noexcept : elems{e0, e1} {} + constexpr Vec(T e0, T e1, T e2) noexcept : elems{e0, e1, e2} {} + constexpr Vec(T e0, T e1, T e2, T e4) noexcept : elems{e0, e1, e2, e4} {} + //explicit constexpr Vec(const std::initializer_list elems_) noexcept : elems{elems_} {} - [[nodiscard]] constexpr Vec2 operator+(const Vec2& other) const noexcept { - return {x + other.x, y + other.y}; + [[nodiscard]] constexpr Vec operator+(const Vec o) const noexcept { + Vec r{}; + for (size_t i = 0; i < N; ++i) + r.elems[i] = elems[i] + o.elems[i]; + return r; } - constexpr Vec2& operator+=(const Vec2& other) noexcept { - x += other.x; - y += other.y; - return *this; - } - [[nodiscard]] constexpr Vec2 operator-(const Vec2& other) const noexcept { - return {x - other.x, y - other.y}; - } - constexpr Vec2& operator-=(const Vec2& other) noexcept { - x -= other.x; - y -= other.y; - return *this; + constexpr Vec operator+=(const Vec o) noexcept { return *this = *this + o; } + + [[nodiscard]] constexpr Vec operator-(const Vec o) const noexcept { + Vec r{}; + for (size_t i = 0; i < N; ++i) + r.elems[i] = elems[i] - o.elems[i]; + return r; } + constexpr Vec operator-=(const Vec o) noexcept { return *this = *this - o; } template - [[nodiscard]] constexpr Vec2, U>> operator-() const noexcept { - return {-x, -y}; - } - [[nodiscard]] constexpr Vec2 operator*(const Vec2& other) const noexcept { - return {x * other.x, y * other.y}; + [[nodiscard]] constexpr Vec, U>, N> operator-() const noexcept { + Vec r{}; + for (size_t i = 0; i < N; ++i) + r.elems[i] = -elems[i]; + return r; } + [[nodiscard]] constexpr Vec operator*(const Vec o) const noexcept { + Vec r{}; + for (size_t i = 0; i < N; ++i) + r.elems[i] = elems[i] * o.elems[i]; + return r; + } template - [[nodiscard]] constexpr Vec2 operator*(const V& f) const noexcept { + [[nodiscard]] constexpr Vec operator*(const V f) const noexcept { using TV = decltype(T{} * V{}); using C = std::common_type_t; - return { - TV(C(x) * C(f)), - TV(C(y) * C(f)), - }; + Vec r{}; + for (size_t i = 0; i < N; ++i) + r.elems[i] = TV(C(elems[i]) * C(f)); + return r; } + template + constexpr Vec operator*=(const V f) noexcept { return *this = *this * f; } template - constexpr Vec2& operator*=(const V& f) noexcept { - return *this = *this * f; - } - - template - [[nodiscard]] constexpr Vec2 operator/(const V& f) const noexcept { + [[nodiscard]] constexpr Vec operator/(const V f) const noexcept { using TV = decltype(T{} / V{}); using C = std::common_type_t; - return { - TV(C(x) / C(f)), - TV(C(y) / C(f)), - }; + Vec r{}; + for (size_t i = 0; i < N; ++i) + r.elems[i] = TV(C(elems[i]) / C(f)); + return r; } - template - constexpr Vec2& operator/=(const V& f) noexcept { - return *this = *this / f; - } + constexpr Vec operator/=(const V f) noexcept { return *this = *this / f; } [[nodiscard]] constexpr T Length2() const noexcept { - return x * x + y * y; + T r{}; + for (size_t i = 0; i < N; ++i) + r += elems[i] * elems[i]; + return r; } - // Only implemented for T=float - [[nodiscard]] float Length() const; - - [[nodiscard]] constexpr T& operator[](std::size_t i) noexcept { - return *((&x) + i); - } - [[nodiscard]] constexpr const T& operator[](std::size_t i) const noexcept { - return *((&x) + i); - } -}; - -template -[[nodiscard]] constexpr Vec2 operator*(const V& f, const Vec2& vec) noexcept { - using C = std::common_type_t; - return Vec2(T(C(f) * C(vec.x)), T(C(f) * C(vec.y))); -} - -using Vec2f = Vec2; - -template <> -inline float Vec2::Length() const { - return std::sqrt(x * x + y * y); -} - -template -class Vec3 { -public: - T x{}; - T y{}; - T z{}; - - constexpr Vec3() = default; - constexpr Vec3(const T& x_, const T& y_, const T& z_) noexcept : x(x_), y(y_), z(z_) {} - - [[nodiscard]] constexpr Vec3 operator+(const Vec3& other) const noexcept { - return {x + other.x, y + other.y, z + other.z}; - } - - constexpr Vec3 operator+=(const Vec3& other) noexcept { - x += other.x; - y += other.y; - z += other.z; - return *this; - } - - [[nodiscard]] constexpr Vec3 operator-(const Vec3& other) const noexcept { - return {x - other.x, y - other.y, z - other.z}; - } - - constexpr Vec3 operator-=(const Vec3& other) noexcept { - x -= other.x; - y -= other.y; - z -= other.z; - return *this; - } - - template - [[nodiscard]] constexpr Vec3, U>> operator-() const noexcept { - return {-x, -y, -z}; - } - - [[nodiscard]] constexpr Vec3 operator*(const Vec3& other) const noexcept { - return {x * other.x, y * other.y, z * other.z}; - } - - template - [[nodiscard]] constexpr Vec3 operator*(const V& f) const noexcept { - using TV = decltype(T{} * V{}); - using C = std::common_type_t; - - return { - TV(C(x) * C(f)), - TV(C(y) * C(f)), - TV(C(z) * C(f)), - }; - } - - template - constexpr Vec3 operator*=(const V& f) noexcept { - return *this = *this * f; - } - template - [[nodiscard]] constexpr Vec3 operator/(const V& f) const noexcept { - using TV = decltype(T{} / V{}); - using C = std::common_type_t; - - return { - TV(C(x) / C(f)), - TV(C(y) / C(f)), - TV(C(z) / C(f)), - }; - } - - template - constexpr Vec3& operator/=(const V& f) noexcept { - return *this = *this / f; - } - - [[nodiscard]] constexpr T Length2() const noexcept { - return x * x + y * y + z * z; - } - - // Only implemented for T=float - [[nodiscard]] float Length() const; - [[nodiscard]] Vec3 Normalized() const; - - [[nodiscard]] constexpr T& operator[](std::size_t i) noexcept { - return *((&x) + i); - } - - [[nodiscard]] constexpr const T& operator[](std::size_t i) const noexcept { - return *((&x) + i); - } -}; - -template -[[nodiscard]] constexpr Vec3 operator*(const V& f, const Vec3& vec) noexcept { - using C = std::common_type_t; - return Vec3(T(C(f) * C(vec.x)), T(C(f) * C(vec.y)), T(C(f) * C(vec.z))); -} - -template <> -inline float Vec3::Length() const { - return std::sqrt(x * x + y * y + z * z); -} - -template <> -inline Vec3 Vec3::Normalized() const { - return *this / Length(); -} - -using Vec3f = Vec3; - -template -class Quaternion { -public: - Vec3 xyz; - T w{}; - - [[nodiscard]] Quaternion operator+(const Quaternion& other) const noexcept { - return {xyz + other.xyz, w + other.w}; - } - - [[nodiscard]] Quaternion operator-(const Quaternion& other) const noexcept { - return {xyz - other.xyz, w - other.w}; - } - - [[nodiscard]] Quaternion operator*(const Quaternion& other) const noexcept { - return {xyz * other.w + other.xyz * w + Cross(xyz, other.xyz), w * other.w - Dot(xyz, other.xyz)}; - } - - [[nodiscard]] Quaternion Normalized() const noexcept { - T length = std::sqrt(xyz.Length2() + w * w); - return {xyz / length, w / length}; - } + [[nodiscard]] T Length() const { return T(std::sqrt(float(Length2()))); } + [[nodiscard]] Vec Normalized() const { return *this / Length(); } + [[nodiscard]] constexpr T& operator[](std::size_t i) noexcept { return elems[i]; } + [[nodiscard]] constexpr const T& operator[](std::size_t i) const noexcept { return elems[i]; } [[nodiscard]] std::array ToMatrix() const { - const T x2 = xyz[0] * xyz[0]; - const T y2 = xyz[1] * xyz[1]; - const T z2 = xyz[2] * xyz[2]; - - const T xy = xyz[0] * xyz[1]; - const T wz = w * xyz[2]; - const T xz = xyz[0] * xyz[2]; - const T wy = w * xyz[1]; - const T yz = xyz[1] * xyz[2]; - const T wx = w * xyz[0]; + const T x2 = elems[0] * elems[0]; + const T y2 = elems[1] * elems[1]; + const T z2 = elems[2] * elems[2]; + const T xy = elems[0] * elems[1]; + const T wz = elems[3] * elems[2]; + const T xz = elems[0] * elems[2]; + const T wy = elems[3] * elems[1]; + const T yz = elems[1] * elems[2]; + const T wx = elems[3] * elems[0]; return { 1.0f - 2.0f * (y2 + z2), 2.0f * (xy + wz), @@ -280,4 +122,13 @@ public: } }; +template +[[nodiscard]] constexpr Vec operator*(const V f, const Vec v) noexcept { + using C = std::common_type_t; + Vec r{}; + for (size_t i = 0; i < N; ++i) + r.elems[i] = T(C(f) * C(v.elems[i])); + return r; +} + } // namespace Common diff --git a/src/core/hle/service/set/setting_formats/device_settings.h b/src/core/hle/service/set/setting_formats/device_settings.h index 2827756f6a..d0eecb917b 100644 --- a/src/core/hle/service/set/setting_formats/device_settings.h +++ b/src/core/hle/service/set/setting_formats/device_settings.h @@ -30,15 +30,15 @@ struct DeviceSettings { INSERT_PADDING_BYTES(0x20); // Reserved // nn::settings::system::ConsoleSixAxisSensorAccelerationBias - Common::Vec3 console_six_axis_sensor_acceleration_bias; + Common::Vec console_six_axis_sensor_acceleration_bias; // nn::settings::system::ConsoleSixAxisSensorAngularVelocityBias - Common::Vec3 console_six_axis_sensor_angular_velocity_bias; + Common::Vec console_six_axis_sensor_angular_velocity_bias; // nn::settings::system::ConsoleSixAxisSensorAccelerationGain std::array console_six_axis_sensor_acceleration_gain; // nn::settings::system::ConsoleSixAxisSensorAngularVelocityGain std::array console_six_axis_sensor_angular_velocity_gain; // nn::settings::system::ConsoleSixAxisSensorAngularVelocityTimeBias - Common::Vec3 console_six_axis_sensor_angular_velocity_time_bias; + Common::Vec console_six_axis_sensor_angular_velocity_time_bias; // nn::settings::system::ConsoleSixAxisSensorAngularAcceleration std::array console_six_axis_sensor_angular_acceleration; }; diff --git a/src/core/hle/service/set/setting_formats/system_settings.h b/src/core/hle/service/set/setting_formats/system_settings.h index 75deb1a93f..0817369e54 100644 --- a/src/core/hle/service/set/setting_formats/system_settings.h +++ b/src/core/hle/service/set/setting_formats/system_settings.h @@ -153,15 +153,15 @@ struct SystemSettings { INSERT_PADDING_BYTES(0x7FF8); // Reserved // nn::settings::system::ConsoleSixAxisSensorAccelerationBias - Common::Vec3 console_six_axis_sensor_acceleration_bias; + Common::Vec console_six_axis_sensor_acceleration_bias; // nn::settings::system::ConsoleSixAxisSensorAngularVelocityBias - Common::Vec3 console_six_axis_sensor_angular_velocity_bias; + Common::Vec console_six_axis_sensor_angular_velocity_bias; // nn::settings::system::ConsoleSixAxisSensorAccelerationGain std::array console_six_axis_sensor_acceleration_gain; // nn::settings::system::ConsoleSixAxisSensorAngularVelocityGain std::array console_six_axis_sensor_angular_velocity_gain; // nn::settings::system::ConsoleSixAxisSensorAngularVelocityTimeBias - Common::Vec3 console_six_axis_sensor_angular_velocity_time_bias; + Common::Vec console_six_axis_sensor_angular_velocity_time_bias; // nn::settings::system::ConsoleSixAxisSensorAngularAcceleration std::array console_six_axis_sensor_angular_velocity_acceleration; INSERT_PADDING_BYTES(0x70); // Reserved diff --git a/src/hid_core/frontend/emulated_console.cpp b/src/hid_core/frontend/emulated_console.cpp index 92e99a1f54..fd34233122 100644 --- a/src/hid_core/frontend/emulated_console.cpp +++ b/src/hid_core/frontend/emulated_console.cpp @@ -170,12 +170,12 @@ void EmulatedConsole::SetMotion(const Common::Input::CallbackStatus& callback) { auto& emulated = console.motion_values.emulated; raw_status = TransformToMotion(callback); - emulated.SetAcceleration(Common::Vec3f{ + emulated.SetAcceleration(Common::Vec{ raw_status.accel.x.value, raw_status.accel.y.value, raw_status.accel.z.value, }); - emulated.SetGyroscope(Common::Vec3f{ + emulated.SetGyroscope(Common::Vec{ raw_status.gyro.x.value, raw_status.gyro.y.value, raw_status.gyro.z.value, diff --git a/src/hid_core/frontend/emulated_console.h b/src/hid_core/frontend/emulated_console.h index 1b3ec8a87b..9e2f7e2c1b 100644 --- a/src/hid_core/frontend/emulated_console.h +++ b/src/hid_core/frontend/emulated_console.h @@ -42,12 +42,12 @@ using TouchValues = std::array; // Contains all motion related data that is used on the services struct ConsoleMotion { - Common::Vec3f accel{}; - Common::Vec3f gyro{}; - Common::Vec3f rotation{}; - std::array orientation{}; - Common::Quaternion quaternion{}; - Common::Vec3f gyro_bias{}; + Common::Vec accel{}; + Common::Vec gyro{}; + Common::Vec rotation{}; + std::array, 3> orientation{}; + Common::Vec quaternion{}; + Common::Vec gyro_bias{}; f32 verticalization_error{}; bool is_at_rest{}; }; diff --git a/src/hid_core/frontend/emulated_controller.cpp b/src/hid_core/frontend/emulated_controller.cpp index 36c9a0f02f..1092970ee8 100644 --- a/src/hid_core/frontend/emulated_controller.cpp +++ b/src/hid_core/frontend/emulated_controller.cpp @@ -1051,12 +1051,12 @@ void EmulatedController::SetMotion(const Common::Input::CallbackStatus& callback auto& emulated = controller.motion_values[index].emulated; raw_status = TransformToMotion(callback); - emulated.SetAcceleration(Common::Vec3f{ + emulated.SetAcceleration(Common::Vec{ raw_status.accel.x.value, raw_status.accel.y.value, raw_status.accel.z.value, }); - emulated.SetGyroscope(Common::Vec3f{ + emulated.SetGyroscope(Common::Vec{ raw_status.gyro.x.value, raw_status.gyro.y.value, raw_status.gyro.z.value, diff --git a/src/hid_core/frontend/emulated_controller.h b/src/hid_core/frontend/emulated_controller.h index d98ed3df3c..7a2291b8a4 100644 --- a/src/hid_core/frontend/emulated_controller.h +++ b/src/hid_core/frontend/emulated_controller.h @@ -107,11 +107,11 @@ struct RingSensorForce { using NfcState = Common::Input::NfcStatus; struct ControllerMotion { - Common::Vec3f accel{}; - Common::Vec3f gyro{}; - Common::Vec3f rotation{}; - Common::Vec3f euler{}; - std::array orientation{}; + Common::Vec accel{}; + Common::Vec gyro{}; + Common::Vec rotation{}; + Common::Vec euler{}; + std::array, 3> orientation{}; bool is_at_rest{}; }; diff --git a/src/hid_core/frontend/motion_input.cpp b/src/hid_core/frontend/motion_input.cpp index a1e09e3ce9..1dcb910a50 100644 --- a/src/hid_core/frontend/motion_input.cpp +++ b/src/hid_core/frontend/motion_input.cpp @@ -26,20 +26,19 @@ void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) { kd = new_kd; } -void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) { +void MotionInput::SetAcceleration(const Common::Vec& acceleration) { accel = acceleration; - - accel.x = std::clamp(accel.x, -AccelMaxValue, AccelMaxValue); - accel.y = std::clamp(accel.y, -AccelMaxValue, AccelMaxValue); - accel.z = std::clamp(accel.z, -AccelMaxValue, AccelMaxValue); + accel[0] = std::clamp(accel[0], -AccelMaxValue, AccelMaxValue); + accel[1] = std::clamp(accel[1], -AccelMaxValue, AccelMaxValue); + accel[2] = std::clamp(accel[2], -AccelMaxValue, AccelMaxValue); } -void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) { +void MotionInput::SetGyroscope(const Common::Vec& gyroscope) { gyro = gyroscope - gyro_bias; - gyro.x = std::clamp(gyro.x, -GyroMaxValue, GyroMaxValue); - gyro.y = std::clamp(gyro.y, -GyroMaxValue, GyroMaxValue); - gyro.z = std::clamp(gyro.z, -GyroMaxValue, GyroMaxValue); + gyro[0] = std::clamp(gyro[0], -GyroMaxValue, GyroMaxValue); + gyro[1] = std::clamp(gyro[1], -GyroMaxValue, GyroMaxValue); + gyro[2] = std::clamp(gyro[2], -GyroMaxValue, GyroMaxValue); // Auto adjust gyro_bias to minimize drift if (!IsMoving(IsAtRestRelaxed)) { @@ -59,25 +58,25 @@ void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) { } } -void MotionInput::SetQuaternion(const Common::Quaternion& quaternion) { +void MotionInput::SetQuaternion(const Common::Vec& quaternion) { quat = quaternion; } -void MotionInput::SetEulerAngles(const Common::Vec3f& euler_angles) { - const float cr = std::cos(euler_angles.x * 0.5f); - const float sr = std::sin(euler_angles.x * 0.5f); - const float cp = std::cos(euler_angles.y * 0.5f); - const float sp = std::sin(euler_angles.y * 0.5f); - const float cy = std::cos(euler_angles.z * 0.5f); - const float sy = std::sin(euler_angles.z * 0.5f); +void MotionInput::SetEulerAngles(const Common::Vec& euler_angles) { + const float cr = std::cos(euler_angles[0] * 0.5f); + const float sr = std::sin(euler_angles[0] * 0.5f); + const float cp = std::cos(euler_angles[1] * 0.5f); + const float sp = std::sin(euler_angles[1] * 0.5f); + const float cy = std::cos(euler_angles[2] * 0.5f); + const float sy = std::sin(euler_angles[2] * 0.5f); - quat.w = cr * cp * cy + sr * sp * sy; - quat.xyz.x = sr * cp * cy - cr * sp * sy; - quat.xyz.y = cr * sp * cy + sr * cp * sy; - quat.xyz.z = cr * cp * sy - sr * sp * cy; + quat[3] = cr * cp * cy + sr * sp * sy; + quat[0] = sr * cp * cy - cr * sp * sy; + quat[1] = cr * sp * cy + sr * cp * sy; + quat[2] = cr * cp * sy - sr * sp * cy; } -void MotionInput::SetGyroBias(const Common::Vec3f& bias) { +void MotionInput::SetGyroBias(const Common::Vec& bias) { gyro_bias = bias; } @@ -98,7 +97,7 @@ void MotionInput::ResetRotations() { } void MotionInput::ResetQuaternion() { - quat = {{0.0f, 0.0f, -1.0f}, 0.0f}; + quat = Common::Vec{0.0f, 0.0f, -1.0f, 0.0f}; } bool MotionInput::IsMoving(f32 sensitivity) const { @@ -137,10 +136,10 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { ResetOrientation(); } // Short name local variable for readability - f32 q1 = quat.w; - f32 q2 = quat.xyz[0]; - f32 q3 = quat.xyz[1]; - f32 q4 = quat.xyz[2]; + f32 q1 = quat[3]; + f32 q2 = quat[0]; + f32 q3 = quat[1]; + f32 q4 = quat[2]; const auto sample_period = static_cast(elapsed_time) / 1000000.0f; // Ignore invalid elapsed time @@ -150,23 +149,23 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { const auto normal_accel = accel.Normalized(); auto rad_gyro = gyro * std::numbers::pi_v * 2.f; - const f32 swap = rad_gyro.x; - rad_gyro.x = rad_gyro.y; - rad_gyro.y = -swap; - rad_gyro.z = -rad_gyro.z; + const f32 swap = rad_gyro[0]; + rad_gyro[0] = rad_gyro[1]; + rad_gyro[1] = -swap; + rad_gyro[2] = -rad_gyro[2]; // Clear gyro values if there is no gyro present if (only_accelerometer) { - rad_gyro.x = 0; - rad_gyro.y = 0; - rad_gyro.z = 0; + rad_gyro[0] = 0; + rad_gyro[1] = 0; + rad_gyro[2] = 0; } // Ignore drift correction if acceleration is not reliable if (accel.Length() >= 0.75f && accel.Length() <= 1.25f) { - const f32 ax = -normal_accel.x; - const f32 ay = normal_accel.y; - const f32 az = -normal_accel.z; + const f32 ax = -normal_accel[0]; + const f32 ay = normal_accel[1]; + const f32 az = -normal_accel[2]; // Estimated direction of gravity const f32 vx = 2.0f * (q2 * q4 - q1 * q3); @@ -174,7 +173,7 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { const f32 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; // Error is cross product between estimated direction and measured direction of gravity - const Common::Vec3f new_real_error = { + const Common::Vec new_real_error{ az * vx - ax * vz, ay * vz - az * vy, ax * vy - ay * vx, @@ -202,16 +201,16 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { rad_gyro += 10.0f * kd * derivative_error; // Emulate gyro values for games that need them - gyro.x = -rad_gyro.y; - gyro.y = rad_gyro.x; - gyro.z = -rad_gyro.z; + gyro[0] = -rad_gyro[1]; + gyro[1] = rad_gyro[0]; + gyro[2] = -rad_gyro[2]; UpdateRotation(elapsed_time); } } - const f32 gx = rad_gyro.y; - const f32 gy = rad_gyro.x; - const f32 gz = rad_gyro.z; + const f32 gx = rad_gyro[1]; + const f32 gy = rad_gyro[0]; + const f32 gz = rad_gyro[2]; // Integrate rate of change of quaternion const f32 pa = q2; @@ -222,57 +221,58 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period); q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period); - quat.w = q1; - quat.xyz[0] = q2; - quat.xyz[1] = q3; - quat.xyz[2] = q4; + quat[3] = q1; + quat[0] = q2; + quat[1] = q3; + quat[2] = q4; quat = quat.Normalized(); } -std::array MotionInput::GetOrientation() const { - const Common::Quaternion quad{ - .xyz = {-quat.xyz[1], -quat.xyz[0], -quat.w}, - .w = -quat.xyz[2], +std::array, 3> MotionInput::GetOrientation() const { + const Common::Vec quad{ + -quat[1], + -quat[0], + -quat[3], + -quat[2], }; - const std::array matrix4x4 = quad.ToMatrix(); - - return {Common::Vec3f(matrix4x4[0], matrix4x4[1], -matrix4x4[2]), - Common::Vec3f(matrix4x4[4], matrix4x4[5], -matrix4x4[6]), - Common::Vec3f(-matrix4x4[8], -matrix4x4[9], matrix4x4[10])}; + const std::array matrix4x4 = quad.ToMatrix(); + return {Common::Vec(matrix4x4[0], matrix4x4[1], -matrix4x4[2]), + Common::Vec(matrix4x4[4], matrix4x4[5], -matrix4x4[6]), + Common::Vec(-matrix4x4[8], -matrix4x4[9], matrix4x4[10])}; } -Common::Vec3f MotionInput::GetAcceleration() const { +Common::Vec MotionInput::GetAcceleration() const { return accel; } -Common::Vec3f MotionInput::GetGyroscope() const { +Common::Vec MotionInput::GetGyroscope() const { return gyro; } -Common::Vec3f MotionInput::GetGyroBias() const { +Common::Vec MotionInput::GetGyroBias() const { return gyro_bias; } -Common::Quaternion MotionInput::GetQuaternion() const { +Common::Vec MotionInput::GetQuaternion() const { return quat; } -Common::Vec3f MotionInput::GetRotations() const { +Common::Vec MotionInput::GetRotations() const { return rotations; } -Common::Vec3f MotionInput::GetEulerAngles() const { +Common::Vec MotionInput::GetEulerAngles() const { // roll (x-axis rotation) - const float sinr_cosp = 2 * (quat.w * quat.xyz.x + quat.xyz.y * quat.xyz.z); - const float cosr_cosp = 1 - 2 * (quat.xyz.x * quat.xyz.x + quat.xyz.y * quat.xyz.y); + const float sinr_cosp = 2 * (quat[3] * quat[0] + quat[1] * quat[2]); + const float cosr_cosp = 1 - 2 * (quat[0] * quat[0] + quat[1] * quat[1]); // pitch (y-axis rotation) - const float sinp = std::sqrt(1 + 2 * (quat.w * quat.xyz.y - quat.xyz.x * quat.xyz.z)); - const float cosp = std::sqrt(1 - 2 * (quat.w * quat.xyz.y - quat.xyz.x * quat.xyz.z)); + const float sinp = std::sqrt(1 + 2 * (quat[3] * quat[1] - quat[0] * quat[2])); + const float cosp = std::sqrt(1 - 2 * (quat[3] * quat[1] - quat[0] * quat[2])); // yaw (z-axis rotation) - const float siny_cosp = 2 * (quat.w * quat.xyz.z + quat.xyz.x * quat.xyz.y); - const float cosy_cosp = 1 - 2 * (quat.xyz.y * quat.xyz.y + quat.xyz.z * quat.xyz.z); + const float siny_cosp = 2 * (quat[3] * quat[2] + quat[0] * quat[1]); + const float cosy_cosp = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]); return { std::atan2(sinr_cosp, cosr_cosp), @@ -285,13 +285,13 @@ void MotionInput::ResetOrientation() { if (!reset_enabled || only_accelerometer) { return; } - if (!IsMoving(IsAtRestRelaxed) && accel.z <= -0.9f) { + if (!IsMoving(IsAtRestRelaxed) && accel[2] <= -0.9f) { ++reset_counter; if (reset_counter > 900) { - quat.w = 0; - quat.xyz[0] = 0; - quat.xyz[1] = 0; - quat.xyz[2] = -1; + quat[3] = 0; + quat[0] = 0; + quat[1] = 0; + quat[2] = -1; SetOrientationFromAccelerometer(); integral_error = {}; reset_counter = 0; @@ -309,15 +309,15 @@ void MotionInput::SetOrientationFromAccelerometer() { while (!IsCalibrated(0.01f) && ++iterations < 100) { // Short name local variable for readability - f32 q1 = quat.w; - f32 q2 = quat.xyz[0]; - f32 q3 = quat.xyz[1]; - f32 q4 = quat.xyz[2]; + f32 q1 = quat[3]; + f32 q2 = quat[0]; + f32 q3 = quat[1]; + f32 q4 = quat[2]; - Common::Vec3f rad_gyro; - const f32 ax = -normal_accel.x; - const f32 ay = normal_accel.y; - const f32 az = -normal_accel.z; + Common::Vec rad_gyro; + const f32 ax = -normal_accel[0]; + const f32 ay = normal_accel[1]; + const f32 az = -normal_accel[2]; // Estimated direction of gravity const f32 vx = 2.0f * (q2 * q4 - q1 * q3); @@ -325,7 +325,7 @@ void MotionInput::SetOrientationFromAccelerometer() { const f32 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; // Error is cross product between estimated direction and measured direction of gravity - const Common::Vec3f new_real_error = { + const Common::Vec new_real_error = { az * vx - ax * vz, ay * vz - az * vy, ax * vy - ay * vx, @@ -338,9 +338,9 @@ void MotionInput::SetOrientationFromAccelerometer() { rad_gyro += 5.0f * ki * integral_error; rad_gyro += 10.0f * kd * derivative_error; - const f32 gx = rad_gyro.y; - const f32 gy = rad_gyro.x; - const f32 gz = rad_gyro.z; + const f32 gx = rad_gyro[1]; + const f32 gy = rad_gyro[0]; + const f32 gz = rad_gyro[2]; // Integrate rate of change of quaternion const f32 pa = q2; @@ -351,10 +351,10 @@ void MotionInput::SetOrientationFromAccelerometer() { q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period); q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period); - quat.w = q1; - quat.xyz[0] = q2; - quat.xyz[1] = q3; - quat.xyz[2] = q4; + quat[3] = q1; + quat[0] = q2; + quat[1] = q3; + quat[2] = q4; quat = quat.Normalized(); } } diff --git a/src/hid_core/frontend/motion_input.h b/src/hid_core/frontend/motion_input.h index 212f51c83d..09dbb706d7 100644 --- a/src/hid_core/frontend/motion_input.h +++ b/src/hid_core/frontend/motion_input.h @@ -36,11 +36,11 @@ public: MotionInput& operator=(MotionInput&&) = default; void SetPID(f32 new_kp, f32 new_ki, f32 new_kd); - void SetAcceleration(const Common::Vec3f& acceleration); - void SetGyroscope(const Common::Vec3f& gyroscope); - void SetQuaternion(const Common::Quaternion& quaternion); - void SetEulerAngles(const Common::Vec3f& euler_angles); - void SetGyroBias(const Common::Vec3f& bias); + void SetAcceleration(const Common::Vec& acceleration); + void SetGyroscope(const Common::Vec& gyroscope); + void SetQuaternion(const Common::Vec& quaternion); + void SetEulerAngles(const Common::Vec& euler_angles); + void SetGyroBias(const Common::Vec& bias); void SetGyroThreshold(f32 threshold); /// Applies a modifier on top of the normal gyro threshold @@ -55,13 +55,13 @@ public: void Calibrate(); - [[nodiscard]] std::array GetOrientation() const; - [[nodiscard]] Common::Vec3f GetAcceleration() const; - [[nodiscard]] Common::Vec3f GetGyroscope() const; - [[nodiscard]] Common::Vec3f GetGyroBias() const; - [[nodiscard]] Common::Vec3f GetRotations() const; - [[nodiscard]] Common::Quaternion GetQuaternion() const; - [[nodiscard]] Common::Vec3f GetEulerAngles() const; + [[nodiscard]] std::array, 3> GetOrientation() const; + [[nodiscard]] Common::Vec GetAcceleration() const; + [[nodiscard]] Common::Vec GetGyroscope() const; + [[nodiscard]] Common::Vec GetGyroBias() const; + [[nodiscard]] Common::Vec GetRotations() const; + [[nodiscard]] Common::Vec GetQuaternion() const; + [[nodiscard]] Common::Vec GetEulerAngles() const; [[nodiscard]] bool IsMoving(f32 sensitivity) const; [[nodiscard]] bool IsCalibrated(f32 sensitivity) const; @@ -77,24 +77,24 @@ private: f32 kd; // PID errors - Common::Vec3f real_error; - Common::Vec3f integral_error; - Common::Vec3f derivative_error; + Common::Vec real_error; + Common::Vec integral_error; + Common::Vec derivative_error; // Quaternion containing the device orientation - Common::Quaternion quat; + Common::Vec quat; // Number of full rotations in each axis - Common::Vec3f rotations; + Common::Vec rotations; // Acceleration vector measurement in G force - Common::Vec3f accel; + Common::Vec accel; // Gyroscope vector measurement in radians/s. - Common::Vec3f gyro; + Common::Vec gyro; // Vector to be subtracted from gyro measurements - Common::Vec3f gyro_bias; + Common::Vec gyro_bias; // Minimum gyro amplitude to detect if the device is moving f32 gyro_threshold = 0.0f; diff --git a/src/hid_core/hid_types.h b/src/hid_core/hid_types.h index 40a90c2bc3..4e82679d63 100644 --- a/src/hid_core/hid_types.h +++ b/src/hid_core/hid_types.h @@ -602,10 +602,10 @@ static_assert(sizeof(SixAxisSensorAttribute) == 4, "SixAxisSensorAttribute is an struct SixAxisSensorState { s64 delta_time{}; s64 sampling_number{}; - Common::Vec3f accel{}; - Common::Vec3f gyro{}; - Common::Vec3f rotation{}; - std::array orientation{}; + Common::Vec accel{}; + Common::Vec gyro{}; + Common::Vec rotation{}; + std::array, 3> orientation{}; SixAxisSensorAttribute attribute{}; INSERT_PADDING_BYTES(4); // Reserved }; diff --git a/src/hid_core/resources/shared_memory_format.h b/src/hid_core/resources/shared_memory_format.h index b9deeec302..05877438bb 100644 --- a/src/hid_core/resources/shared_memory_format.h +++ b/src/hid_core/resources/shared_memory_format.h @@ -196,7 +196,7 @@ struct ConsoleSixAxisSensorSharedMemoryFormat { bool is_seven_six_axis_sensor_at_rest{}; INSERT_PADDING_BYTES(3); // padding f32 verticalization_error{}; - Common::Vec3f gyro_bias{}; + Common::Vec gyro_bias{}; INSERT_PADDING_BYTES(4); // padding }; static_assert(sizeof(ConsoleSixAxisSensorSharedMemoryFormat) == 0x20, diff --git a/src/hid_core/resources/six_axis/seven_six_axis.cpp b/src/hid_core/resources/six_axis/seven_six_axis.cpp index 584e6d6505..13dd5f943a 100644 --- a/src/hid_core/resources/six_axis/seven_six_axis.cpp +++ b/src/hid_core/resources/six_axis/seven_six_axis.cpp @@ -46,14 +46,11 @@ void SevenSixAxis::OnUpdate(const Core::Timing::CoreTiming& core_timing) { next_seven_sixaxis_state.accel = motion_status.accel; next_seven_sixaxis_state.gyro = motion_status.gyro; next_seven_sixaxis_state.quaternion = { - { - motion_status.quaternion.xyz.y, - motion_status.quaternion.xyz.x, - -motion_status.quaternion.w, - }, - -motion_status.quaternion.xyz.z, + motion_status.quaternion[1], + motion_status.quaternion[0], + -motion_status.quaternion[3], + -motion_status.quaternion[2], }; - seven_sixaxis_lifo.WriteNextEntry(next_seven_sixaxis_state); transfer_memory_owner->GetMemory().WriteBlock(transfer_memory, &seven_sixaxis_lifo, sizeof(seven_sixaxis_lifo)); diff --git a/src/hid_core/resources/six_axis/seven_six_axis.h b/src/hid_core/resources/six_axis/seven_six_axis.h index d106f0bfc0..a59d697356 100644 --- a/src/hid_core/resources/six_axis/seven_six_axis.h +++ b/src/hid_core/resources/six_axis/seven_six_axis.h @@ -51,9 +51,9 @@ private: u64 timestamp{}; u64 sampling_number{}; u64 unknown{}; - Common::Vec3f accel{}; - Common::Vec3f gyro{}; - Common::Quaternion quaternion{}; + Common::Vec accel{}; + Common::Vec gyro{}; + Common::Vec quaternion{}; }; static_assert(sizeof(SevenSixAxisState) == 0x48, "SevenSixAxisState is an invalid size"); diff --git a/src/hid_core/resources/six_axis/six_axis.cpp b/src/hid_core/resources/six_axis/six_axis.cpp index b407a5c76e..28359901e3 100644 --- a/src/hid_core/resources/six_axis/six_axis.cpp +++ b/src/hid_core/resources/six_axis/six_axis.cpp @@ -93,9 +93,9 @@ void SixAxis::OnUpdate(const Core::Timing::CoreTiming& core_timing) { .accel = {0, 0, -1.0f}, .orientation = { - Common::Vec3f{1.0f, 0, 0}, - Common::Vec3f{0, 1.0f, 0}, - Common::Vec3f{0, 0, 1.0f}, + Common::Vec{1.0f, 0, 0}, + Common::Vec{0, 1.0f, 0}, + Common::Vec{0, 0, 1.0f}, }, .attribute = {1}, }; diff --git a/src/input_common/drivers/mouse.cpp b/src/input_common/drivers/mouse.cpp index d306ae7e29..6aa09ffc9e 100644 --- a/src/input_common/drivers/mouse.cpp +++ b/src/input_common/drivers/mouse.cpp @@ -135,8 +135,8 @@ void Mouse::UpdateMotionInput() { void Mouse::Move(int x, int y, int center_x, int center_y) { if (IsMousePanningEnabled()) { - auto const mouse_change_int = Common::Vec2(x, y) - Common::Vec2(center_x, center_y); - auto const mouse_change = Common::Vec2(float(mouse_change_int.x), float(mouse_change_int.y)); + auto const mouse_change_int = Common::Vec(x, y) - Common::Vec(center_x, center_y); + auto const mouse_change = Common::Vec(float(mouse_change_int.x), float(mouse_change_int.y)); auto const x_sensitivity = Settings::values.mouse_panning_x_sensitivity.GetValue() * default_panning_sensitivity; auto const y_sensitivity = Settings::values.mouse_panning_y_sensitivity.GetValue() * default_panning_sensitivity; auto const deadzone_cw = Settings::values.mouse_panning_deadzone_counterweight.GetValue() * default_deadzone_counterweight; @@ -153,7 +153,7 @@ void Mouse::Move(int x, int y, int center_x, int center_y) { } if (button_pressed) { - const auto mouse_move = Common::Vec2(x, y) - mouse_origin; + const auto mouse_move = Common::Vec(x, y) - mouse_origin; const float x_sensitivity = Settings::values.mouse_panning_x_sensitivity.GetValue() * default_stick_sensitivity; const float y_sensitivity = diff --git a/src/input_common/drivers/mouse.h b/src/input_common/drivers/mouse.h index 166168c824..25f75109b9 100644 --- a/src/input_common/drivers/mouse.h +++ b/src/input_common/drivers/mouse.h @@ -107,11 +107,11 @@ private: Common::Input::ButtonNames GetUIButtonName(const Common::ParamPackage& params) const; - Common::Vec2 mouse_origin; - Common::Vec2 last_mouse_position; - Common::Vec2 last_mouse_change; - Common::Vec3 last_motion_change; - Common::Vec2 wheel_position; + Common::Vec mouse_origin; + Common::Vec last_mouse_position; + Common::Vec last_mouse_change; + Common::Vec last_motion_change; + Common::Vec wheel_position; bool button_pressed = false; }; diff --git a/src/yuzu/configuration/configure_input_player_widget.cpp b/src/yuzu/configuration/configure_input_player_widget.cpp index 4891536322..2c1c424bdc 100644 --- a/src/yuzu/configuration/configure_input_player_widget.cpp +++ b/src/yuzu/configuration/configure_input_player_widget.cpp @@ -2936,10 +2936,10 @@ void PlayerControlPreview::DrawArrow(QPainter& p, const QPointF center, const Di } // Draw motion functions -void PlayerControlPreview::Draw3dCube(QPainter& p, QPointF center, const Common::Vec3f& euler, +void PlayerControlPreview::Draw3dCube(QPainter& p, QPointF center, const Common::Vec& euler, float size) { - std::array cube{ - Common::Vec3f{-0.7f, -1, -0.5f}, + std::array, 8> cube{ + Common::Vec{-0.7f, -1, -0.5f}, {-0.7f, 1, -0.5f}, {0.7f, 1, -0.5f}, {0.7f, -1, -0.5f}, @@ -2949,7 +2949,7 @@ void PlayerControlPreview::Draw3dCube(QPainter& p, QPointF center, const Common: {0.7f, -1, 0.5f}, }; - for (Common::Vec3f& point : cube) { + for (Common::Vec& point : cube) { float temp = point.y; point.y = std::cos(euler.x) * point.y - std::sin(euler.x) * point.z; point.z = std::sin(euler.x) * temp + std::cos(euler.x) * point.z; diff --git a/src/yuzu/configuration/configure_input_player_widget.h b/src/yuzu/configuration/configure_input_player_widget.h index d7ec943240..1c5f9eaf49 100644 --- a/src/yuzu/configuration/configure_input_player_widget.h +++ b/src/yuzu/configuration/configure_input_player_widget.h @@ -198,7 +198,7 @@ private: void DrawArrow(QPainter& p, QPointF center, Direction direction, float size); // Draw motion functions - void Draw3dCube(QPainter& p, QPointF center, const Common::Vec3f& euler, float size); + void Draw3dCube(QPainter& p, QPointF center, const Common::Vec& euler, float size); // Draw primitive types template