diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp index dd394b7be5a7..7efac12f7820 100644 --- a/Source/Core/Common/Matrix.cpp +++ b/Source/Core/Common/Matrix.cpp @@ -9,21 +9,29 @@ namespace { -void MatrixMul(int n, const float* a, const float* b, float* result) +// Multiply a NxM matrix by a NxP matrix. +template +auto MatrixMultiply(const std::array& a, const std::array& b) + -> std::array { - for (int i = 0; i < n; ++i) + std::array result; + + for (int n = 0; n != N; ++n) { - for (int j = 0; j < n; ++j) + for (int p = 0; p != P; ++p) { - float temp = 0; - for (int k = 0; k < n; ++k) + T temp = {}; + for (int m = 0; m != M; ++m) { - temp += a[i * n + k] * b[k * n + j]; + temp += a[n * M + m] * b[m * P + p]; } - result[i * n + j] = temp; + result[n * P + p] = temp; } } + + return result; } + } // namespace namespace Common @@ -39,8 +47,8 @@ Matrix33 Matrix33::Identity() Matrix33 Matrix33::RotateX(float rad) { - const float s = sin(rad); - const float c = cos(rad); + const float s = std::sin(rad); + const float c = std::cos(rad); Matrix33 mtx = {}; mtx.data[0] = 1; mtx.data[4] = c; @@ -52,8 +60,8 @@ Matrix33 Matrix33::RotateX(float rad) Matrix33 Matrix33::RotateY(float rad) { - const float s = sin(rad); - const float c = cos(rad); + const float s = std::sin(rad); + const float c = std::cos(rad); Matrix33 mtx = {}; mtx.data[0] = c; mtx.data[2] = s; @@ -65,8 +73,8 @@ Matrix33 Matrix33::RotateY(float rad) Matrix33 Matrix33::RotateZ(float rad) { - const float s = sin(rad); - const float c = cos(rad); + const float s = std::sin(rad); + const float c = std::cos(rad); Matrix33 mtx = {}; mtx.data[0] = c; mtx.data[1] = -s; @@ -87,20 +95,12 @@ Matrix33 Matrix33::Scale(const Vec3& vec) void Matrix33::Multiply(const Matrix33& a, const Matrix33& b, Matrix33* result) { - MatrixMul(3, a.data.data(), b.data.data(), result->data.data()); + result->data = MatrixMultiply<3, 3, 3>(a.data, b.data); } void Matrix33::Multiply(const Matrix33& a, const Vec3& vec, Vec3* result) { - for (int i = 0; i < 3; ++i) - { - result->data[i] = 0; - - for (int k = 0; k < 3; ++k) - { - result->data[i] += a.data[i * 3 + k] * vec.data[k]; - } - } + result->data = MatrixMultiply<3, 3, 1>(a.data, vec.data); } Matrix44 Matrix44::Identity() @@ -157,8 +157,32 @@ Matrix44 Matrix44::Shear(const float a, const float b) return mtx; } +Matrix44 Matrix44::Perspective(float fov_y, float aspect_ratio, float z_near, float z_far) +{ + Matrix44 mtx{}; + const float tan_half_fov_y = std::tan(fov_y / 2); + mtx.data[0] = 1 / (aspect_ratio * tan_half_fov_y); + mtx.data[5] = 1 / tan_half_fov_y; + mtx.data[10] = -(z_far + z_near) / (z_far - z_near); + mtx.data[11] = -(2 * z_far * z_near) / (z_far - z_near); + mtx.data[14] = -1; + return mtx; +} + void Matrix44::Multiply(const Matrix44& a, const Matrix44& b, Matrix44* result) { - MatrixMul(4, a.data.data(), b.data.data(), result->data.data()); + result->data = MatrixMultiply<4, 4, 4>(a.data, b.data); +} + +Vec3 Matrix44::Transform(const Vec3& v, float w) const +{ + const auto result = MatrixMultiply<4, 4, 1>(data, {v.x, v.y, v.z, w}); + return Vec3{result[0], result[1], result[2]}; } + +void Matrix44::Multiply(const Matrix44& a, const Vec4& vec, Vec4* result) +{ + result->data = MatrixMultiply<4, 4, 1>(a.data, vec.data); +} + } // namespace Common diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h index ffe241f8d248..f96624fef3e3 100644 --- a/Source/Core/Common/Matrix.h +++ b/Source/Core/Common/Matrix.h @@ -6,18 +6,25 @@ #include #include +#include // Tiny matrix/vector library. // Used for things like Free-Look in the gfx backend. namespace Common { -union Vec3 +template +union TVec3 { - Vec3() = default; - Vec3(float _x, float _y, float _z) : data{_x, _y, _z} {} + TVec3() = default; + TVec3(T _x, T _y, T _z) : data{_x, _y, _z} {} + + T Dot(const TVec3& other) const { return x * other.x + y * other.y + z * other.z; } + T LengthSquared() const { return Dot(*this); } + T Length() const { return std::sqrt(LengthSquared()); } + TVec3 Normalized() const { return *this / Length(); } - Vec3& operator+=(const Vec3& rhs) + TVec3& operator+=(const TVec3& rhs) { x += rhs.x; y += rhs.y; @@ -25,21 +32,135 @@ union Vec3 return *this; } - std::array data = {}; + TVec3& operator-=(const TVec3& rhs) + { + x -= rhs.x; + y -= rhs.y; + z -= rhs.z; + return *this; + } + + TVec3& operator*=(const TVec3& rhs) + { + x *= rhs.x; + y *= rhs.y; + z *= rhs.z; + return *this; + } + + TVec3& operator/=(const TVec3& rhs) + { + x /= rhs.x; + y /= rhs.y; + z /= rhs.z; + return *this; + } + + TVec3 operator-() const { return {-x, -y, -z}; } + + std::array data = {}; struct { - float x; - float y; - float z; + T x; + T y; + T z; }; }; -inline Vec3 operator+(Vec3 lhs, const Vec3& rhs) +template +TVec3 operator+(TVec3 lhs, const TVec3& rhs) { return lhs += rhs; } +template +TVec3 operator-(TVec3 lhs, const TVec3& rhs) +{ + return lhs -= rhs; +} + +template +TVec3 operator*(TVec3 lhs, const TVec3& rhs) +{ + return lhs *= rhs; +} + +template +inline TVec3 operator/(TVec3 lhs, const TVec3& rhs) +{ + return lhs /= rhs; +} + +template +TVec3 operator*(TVec3 lhs, std::common_type_t scalar) +{ + return lhs *= TVec3{scalar, scalar, scalar}; +} + +template +TVec3 operator/(TVec3 lhs, std::common_type_t scalar) +{ + return lhs /= TVec3{scalar, scalar, scalar}; +} + +using Vec3 = TVec3; +using DVec3 = TVec3; + +template +union TVec4 +{ + TVec4() = default; + TVec4(TVec3 _vec, T _w) : TVec4{_vec.x, _vec.y, _vec.z, _w} {} + TVec4(T _x, T _y, T _z, T _w) : data{_x, _y, _z, _w} {} + + TVec4& operator*=(const TVec4& rhs) + { + x *= rhs.x; + y *= rhs.y; + z *= rhs.z; + w *= rhs.w; + return *this; + } + + TVec4& operator/=(const TVec4& rhs) + { + x /= rhs.x; + y /= rhs.y; + z /= rhs.z; + w /= rhs.w; + return *this; + } + + TVec4& operator*=(T scalar) { return *this *= TVec4{scalar, scalar, scalar, scalar}; } + TVec4& operator/=(T scalar) { return *this /= TVec4{scalar, scalar, scalar, scalar}; } + + std::array data = {}; + + struct + { + T x; + T y; + T z; + T w; + }; +}; + +template +TVec4 operator*(TVec4 lhs, std::common_type_t scalar) +{ + return lhs *= scalar; +} + +template +TVec4 operator/(TVec4 lhs, std::common_type_t scalar) +{ + return lhs /= scalar; +} + +using Vec4 = TVec4; +using DVec4 = TVec4; + template union TVec2 { @@ -73,6 +194,13 @@ union TVec2 return *this; } + TVec2& operator/=(T scalar) + { + x /= scalar; + y /= scalar; + return *this; + } + TVec2 operator-() const { return {-x, -y}; } std::array data = {}; @@ -102,6 +230,12 @@ TVec2 operator*(TVec2 lhs, T scalar) return lhs *= scalar; } +template +TVec2 operator/(TVec2 lhs, T scalar) +{ + return lhs /= scalar; +} + using Vec2 = TVec2; using DVec2 = TVec2; @@ -123,10 +257,11 @@ class Matrix33 Matrix33& operator*=(const Matrix33& rhs) { - Multiply(Matrix33(*this), rhs, this); + Multiply(*this, rhs, this); return *this; } + // Note: Row-major storage order. std::array data; }; @@ -135,11 +270,10 @@ inline Matrix33 operator*(Matrix33 lhs, const Matrix33& rhs) return lhs *= rhs; } -inline Vec3 operator*(const Matrix33& lhs, const Vec3& rhs) +inline Vec3 operator*(const Matrix33& lhs, Vec3 rhs) { - Vec3 result; - Matrix33::Multiply(lhs, rhs, &result); - return result; + Matrix33::Multiply(lhs, rhs, &rhs); + return rhs; } class Matrix44 @@ -151,15 +285,21 @@ class Matrix44 static Matrix44 Translate(const Vec3& vec); static Matrix44 Shear(const float a, const float b = 0); + static Matrix44 Perspective(float fov_y, float aspect_ratio, float z_near, float z_far); static void Multiply(const Matrix44& a, const Matrix44& b, Matrix44* result); + static void Multiply(const Matrix44& a, const Vec4& vec, Vec4* result); + + // For when a vec4 isn't needed a multiplication function that takes a Vec3 and w: + Vec3 Transform(const Vec3& point, float w) const; Matrix44& operator*=(const Matrix44& rhs) { - Multiply(Matrix44(*this), rhs, this); + Multiply(*this, rhs, this); return *this; } + // Note: Row-major storage order. std::array data; }; @@ -167,4 +307,11 @@ inline Matrix44 operator*(Matrix44 lhs, const Matrix44& rhs) { return lhs *= rhs; } + +inline Vec4 operator*(const Matrix44& lhs, Vec4 rhs) +{ + Matrix44::Multiply(lhs, rhs, &rhs); + return rhs; +} + } // namespace Common diff --git a/Source/Core/Core/Config/WiimoteInputSettings.cpp b/Source/Core/Core/Config/WiimoteInputSettings.cpp index db8ff55597dd..69a094a3f55d 100644 --- a/Source/Core/Core/Config/WiimoteInputSettings.cpp +++ b/Source/Core/Core/Config/WiimoteInputSettings.cpp @@ -10,24 +10,12 @@ namespace Config // WiimoteInput.Settings -const ConfigInfo WIIMOTE_INPUT_SWING_INTENSITY_FAST{{System::WiiPad, "Swing", "Fast"}, 4.5}; -const ConfigInfo WIIMOTE_INPUT_SWING_INTENSITY_MEDIUM{{System::WiiPad, "Swing", "Medium"}, - 2.5}; -const ConfigInfo WIIMOTE_INPUT_SWING_INTENSITY_SLOW{{System::WiiPad, "Swing", "Slow"}, 1.5}; - const ConfigInfo WIIMOTE_INPUT_SHAKE_INTENSITY_HARD{{System::WiiPad, "Shake", "Hard"}, 5.0}; const ConfigInfo WIIMOTE_INPUT_SHAKE_INTENSITY_MEDIUM{{System::WiiPad, "Shake", "Medium"}, 3.0}; const ConfigInfo WIIMOTE_INPUT_SHAKE_INTENSITY_SOFT{{System::WiiPad, "Shake", "Soft"}, 2.0}; // Dynamic settings -const ConfigInfo WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_HELD_FAST{ - {System::WiiPad, "Dynamic_Swing", "FramesHeldFast"}, 100}; -const ConfigInfo WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_HELD_SLOW{ - {System::WiiPad, "Dynamic_Swing", "FramesHeldSlow"}, 30}; -const ConfigInfo WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_LENGTH{ - {System::WiiPad, "Dynamic_Swing", "FrameCount"}, 30}; - const ConfigInfo WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_HELD_HARD{ {System::WiiPad, "Dynamic_Shake", "FramesHeldHard"}, 45}; const ConfigInfo WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_HELD_SOFT{ @@ -36,18 +24,10 @@ const ConfigInfo WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_LENGTH{ {System::WiiPad, "Dynamic_Shake", "FrameCount"}, 30}; // NunchuckInput.Settings - -const ConfigInfo NUNCHUK_INPUT_SWING_INTENSITY_FAST{ - {System::WiiPad, "Nunchuk_Swing", "Fast"}, 4.5}; -const ConfigInfo NUNCHUK_INPUT_SWING_INTENSITY_MEDIUM{ - {System::WiiPad, "Nunchuk_Swing", "Medium"}, 2.5}; -const ConfigInfo NUNCHUK_INPUT_SWING_INTENSITY_SLOW{ - {System::WiiPad, "Nunchuk_Swing", "Slow"}, 1.5}; - const ConfigInfo NUNCHUK_INPUT_SHAKE_INTENSITY_HARD{ {System::WiiPad, "Nunchuk_Shake", "Hard"}, 5.0}; const ConfigInfo NUNCHUK_INPUT_SHAKE_INTENSITY_MEDIUM{ {System::WiiPad, "Nunchuk_Shake", "Medium"}, 3.0}; const ConfigInfo NUNCHUK_INPUT_SHAKE_INTENSITY_SOFT{ {System::WiiPad, "Nunchuk_Shake", "Soft"}, 2.0}; -} +} // namespace Config diff --git a/Source/Core/Core/Config/WiimoteInputSettings.h b/Source/Core/Core/Config/WiimoteInputSettings.h index de7ffc024117..78c22b8b1b65 100644 --- a/Source/Core/Core/Config/WiimoteInputSettings.h +++ b/Source/Core/Core/Config/WiimoteInputSettings.h @@ -11,24 +11,11 @@ namespace Config // Configuration Information // WiimoteInput.Settings - -extern const ConfigInfo WIIMOTE_INPUT_SWING_INTENSITY_FAST; -extern const ConfigInfo WIIMOTE_INPUT_SWING_INTENSITY_MEDIUM; -extern const ConfigInfo WIIMOTE_INPUT_SWING_INTENSITY_SLOW; - extern const ConfigInfo WIIMOTE_INPUT_SHAKE_INTENSITY_HARD; extern const ConfigInfo WIIMOTE_INPUT_SHAKE_INTENSITY_MEDIUM; extern const ConfigInfo WIIMOTE_INPUT_SHAKE_INTENSITY_SOFT; // Below settings are for dynamic input only (based on how long the user holds a button) - -extern const ConfigInfo - WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_HELD_FAST; // How long button held constitutes a fast swing -extern const ConfigInfo - WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_HELD_SLOW; // How long button held constitutes a slow swing -extern const ConfigInfo - WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_LENGTH; // How long to execute the swing - extern const ConfigInfo WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_HELD_HARD; // How long button held constitutes a hard shake extern const ConfigInfo @@ -37,11 +24,6 @@ extern const ConfigInfo WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_LENGTH; // How long to execute a shake // NunchuckInput.Settings - -extern const ConfigInfo NUNCHUK_INPUT_SWING_INTENSITY_FAST; -extern const ConfigInfo NUNCHUK_INPUT_SWING_INTENSITY_MEDIUM; -extern const ConfigInfo NUNCHUK_INPUT_SWING_INTENSITY_SLOW; - extern const ConfigInfo NUNCHUK_INPUT_SHAKE_INTENSITY_HARD; extern const ConfigInfo NUNCHUK_INPUT_SHAKE_INTENSITY_MEDIUM; extern const ConfigInfo NUNCHUK_INPUT_SHAKE_INTENSITY_SOFT; diff --git a/Source/Core/Core/HW/WiimoteEmu/Camera.cpp b/Source/Core/Core/HW/WiimoteEmu/Camera.cpp index 7dc11622e94d..a462b9186dd3 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Camera.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/Camera.cpp @@ -4,10 +4,12 @@ #include "Core/HW/WiimoteEmu/Camera.h" +#include #include #include "Common/BitUtils.h" #include "Common/ChunkFile.h" +#include "Common/MathUtil.h" #include "Common/Matrix.h" #include "Core/HW/WiimoteCommon/WiimoteReport.h" @@ -49,83 +51,76 @@ int CameraLogic::BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in) return RawWrite(®_data, addr, count, data_in); } -void CameraLogic::Update(const ControllerEmu::Cursor::StateData& cursor, - const NormalizedAccelData& accel, bool sensor_bar_on_top) +void CameraLogic::Update(const Common::Matrix44& transform, bool sensor_bar_on_top) { - double nsin; - - // Ugly code to figure out the wiimote's current angle. - // TODO: Kill this. - double ax = accel.x; - double az = accel.z; - const double len = std::sqrt(ax * ax + az * az); - - if (len) - { - ax /= len; - az /= len; // normalizing the vector - nsin = ax; - } - else + using Common::Matrix33; + using Common::Matrix44; + using Common::Vec3; + using Common::Vec4; + + constexpr int CAMERA_WIDTH = 1024; + constexpr int CAMERA_HEIGHT = 768; + + // Wiibrew claims the camera FOV is about 33 deg by 23 deg. + // Unconfirmed but it seems to work well enough. + constexpr int CAMERA_FOV_X_DEG = 33; + constexpr int CAMERA_FOV_Y_DEG = 23; + + constexpr auto CAMERA_FOV_Y = float(CAMERA_FOV_Y_DEG * MathUtil::TAU / 360); + constexpr auto CAMERA_ASPECT_RATIO = float(CAMERA_FOV_X_DEG) / CAMERA_FOV_Y_DEG; + + // FYI: A real wiimote normally only returns 1 point for each LED cluster (2 total). + // Sending all 4 points can actually cause some stuttering issues. + constexpr int NUM_POINTS = 2; + + // Range from 0-15. Small values (2-4) seem to be very typical. + // This is reduced based on distance from sensor bar. + constexpr int MAX_POINT_SIZE = 15; + + // Sensor bar: + // Values are optimized for default settings in "Super Mario Galaxy 2" + // This seems to be acceptable for a good number of games. + constexpr float SENSOR_BAR_LED_SEPARATION = 0.2f; + const float sensor_bar_height = sensor_bar_on_top ? 0.11 : -0.11; + + const std::array leds{ + Vec3{-SENSOR_BAR_LED_SEPARATION / 2, 0, sensor_bar_height}, + Vec3{SENSOR_BAR_LED_SEPARATION / 2, 0, sensor_bar_height}, + }; + + const auto camera_view = Matrix44::Perspective(CAMERA_FOV_Y, CAMERA_ASPECT_RATIO, 0.001f, 1000) * + Matrix44::FromMatrix33(Matrix33::RotateX(float(MathUtil::TAU / 4))) * + transform; + + struct CameraPoint { - nsin = 0; - } - - static constexpr int camWidth = 1024; - static constexpr int camHeight = 768; - static constexpr double bndleft = 0.78820266; - static constexpr double bndright = -0.78820266; - static constexpr double dist1 = 100.0 / camWidth; // this seems the optimal distance for zelda - static constexpr double dist2 = 1.2 * dist1; - - constexpr int NUM_POINTS = 4; + u16 x; + u16 y; + u8 size; + }; - std::array v; - - for (auto& vtx : v) - { - vtx.x = cursor.x * (bndright - bndleft) / 2 + (bndleft + bndright) / 2; - - static constexpr double bndup = -0.315447; - static constexpr double bnddown = 0.85; - - if (sensor_bar_on_top) - vtx.y = cursor.y * (bndup - bnddown) / 2 + (bndup + bnddown) / 2; - else - vtx.y = cursor.y * (bndup - bnddown) / 2 - (bndup + bnddown) / 2; - - vtx.z = 0; - } + // 0xFFFFs are interpreted as "not visible". + constexpr CameraPoint INVISIBLE_POINT{0xffff, 0xffff, 0xff}; - v[0].x -= (cursor.z * 0.5 + 1) * dist1; - v[1].x += (cursor.z * 0.5 + 1) * dist1; - v[2].x -= (cursor.z * 0.5 + 1) * dist2; - v[3].x += (cursor.z * 0.5 + 1) * dist2; + std::array camera_points; - const auto scale = Common::Matrix33::Scale({1, camWidth / camHeight, 1}); - const auto rot = Common::Matrix33::RotateZ(std::asin(nsin)); - const auto tot = scale * rot; + std::transform(leds.begin(), leds.end(), camera_points.begin(), [&](auto& v) { + const auto point = camera_view * Vec4(v, 1.0); - u16 x[NUM_POINTS], y[NUM_POINTS]; - memset(x, 0xFF, sizeof(x)); - memset(y, 0xFF, sizeof(y)); - - for (std::size_t i = 0; i < v.size(); i++) - { - v[i] = tot * v[i]; - - if ((v[i].x < -1) || (v[i].x > 1) || (v[i].y < -1) || (v[i].y > 1)) - continue; + if (point.z > 0) + { + // FYI: Casting down vs. rounding seems to produce more symmetrical output. + const auto x = s32((1 - point.x / point.w) * CAMERA_WIDTH / 2); + const auto y = s32((1 - point.y / point.w) * CAMERA_HEIGHT / 2); - x[i] = static_cast(lround((v[i].x + 1) / 2 * (camWidth - 1))); - y[i] = static_cast(lround((v[i].y + 1) / 2 * (camHeight - 1))); + const auto point_size = std::lround(MAX_POINT_SIZE / point.w / 2); - if (x[i] >= camWidth || y[i] >= camHeight) - { - x[i] = -1; - y[i] = -1; + if (x >= 0 && y >= 0 && x < CAMERA_WIDTH && y < CAMERA_HEIGHT) + return CameraPoint{u16(x), u16(y), u8(point_size)}; } - } + + return INVISIBLE_POINT; + }); // IR data is read from offset 0x37 on real hardware auto& data = reg_data.camera_data; @@ -139,65 +134,83 @@ void CameraLogic::Update(const ControllerEmu::Cursor::StateData& cursor, switch (reg_data.mode) { case IR_MODE_BASIC: - for (unsigned int i = 0; i < 2; ++i) + for (std::size_t i = 0; i != camera_points.size() / 2; ++i) { IRBasic irdata = {}; - irdata.x1 = static_cast(x[i * 2]); - irdata.x1hi = x[i * 2] >> 8; - irdata.y1 = static_cast(y[i * 2]); - irdata.y1hi = y[i * 2] >> 8; + const auto& p1 = camera_points[i * 2]; + irdata.x1 = p1.x; + irdata.x1hi = p1.x >> 8; + irdata.y1 = p1.y; + irdata.y1hi = p1.y >> 8; - irdata.x2 = static_cast(x[i * 2 + 1]); - irdata.x2hi = x[i * 2 + 1] >> 8; - irdata.y2 = static_cast(y[i * 2 + 1]); - irdata.y2hi = y[i * 2 + 1] >> 8; + const auto& p2 = camera_points[i * 2 + 1]; + irdata.x2 = p2.x; + irdata.x2hi = p2.x >> 8; + irdata.y2 = p2.y; + irdata.y2hi = p2.y >> 8; Common::BitCastPtr(data + i * sizeof(IRBasic)) = irdata; } break; case IR_MODE_EXTENDED: - for (unsigned int i = 0; i < 4; ++i) + for (std::size_t i = 0; i != camera_points.size(); ++i) { - if (x[i] < camWidth) + const auto& p = camera_points[i]; + if (p.x < CAMERA_WIDTH) { IRExtended irdata = {}; - irdata.x = static_cast(x[i]); - irdata.xhi = x[i] >> 8; + // TODO: Move this logic into IRExtended class? + irdata.x = p.x; + irdata.xhi = p.x >> 8; - irdata.y = static_cast(y[i]); - irdata.yhi = y[i] >> 8; + irdata.y = p.y; + irdata.yhi = p.y >> 8; - irdata.size = 10; + irdata.size = p.size; Common::BitCastPtr(data + i * sizeof(IRExtended)) = irdata; } } break; case IR_MODE_FULL: - for (unsigned int i = 0; i < 4; ++i) + for (std::size_t i = 0; i != camera_points.size(); ++i) { - if (x[i] < camWidth) + const auto& p = camera_points[i]; + if (p.x < CAMERA_WIDTH) { IRFull irdata = {}; - irdata.x = static_cast(x[i]); - irdata.xhi = x[i] >> 8; + irdata.x = p.x; + irdata.xhi = p.x >> 8; + + irdata.y = p.y; + irdata.yhi = p.y >> 8; + + irdata.size = p.size; - irdata.y = static_cast(y[i]); - irdata.yhi = y[i] >> 8; + // TODO: does size need to be scaled up? + // E.g. does size 15 cover the entire sensor range? - irdata.size = 10; + irdata.xmin = std::max(p.x - p.size, 0); + irdata.ymin = std::max(p.y - p.size, 0); + irdata.xmax = std::min(p.x + p.size, CAMERA_WIDTH); + irdata.ymax = std::min(p.y + p.size, CAMERA_HEIGHT); - // TODO: implement these sensibly: - // TODO: do high bits of x/y min/max need to be set to zero? - irdata.xmin = 0; - irdata.ymin = 0; - irdata.xmax = 0; - irdata.ymax = 0; + // TODO: Is this maybe MSbs of the "intensity" value? irdata.zero = 0; - irdata.intensity = 0; + + constexpr int SUBPIXEL_RESOLUTION = 8; + constexpr long MAX_INTENSITY = 0xff; + + // This is apparently the number of pixels the point takes up at 128x96 resolution. + // We simulate a circle that shrinks at sensor edges. + const auto intensity = + std::lround((irdata.xmax - irdata.xmin) * (irdata.ymax - irdata.ymin) / + SUBPIXEL_RESOLUTION / SUBPIXEL_RESOLUTION * MathUtil::TAU / 8); + + irdata.intensity = u8(std::min(MAX_INTENSITY, intensity)); Common::BitCastPtr(data + i * sizeof(IRFull)) = irdata; } diff --git a/Source/Core/Core/HW/WiimoteEmu/Camera.h b/Source/Core/Core/HW/WiimoteEmu/Camera.h index bd2e4e2a3c3c..fbc7f6516f93 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Camera.h +++ b/Source/Core/Core/HW/WiimoteEmu/Camera.h @@ -10,6 +10,11 @@ #include "Core/HW/WiimoteEmu/I2CBus.h" #include "InputCommon/ControllerEmu/ControlGroup/Cursor.h" +namespace Common +{ +class Matrix44; +} + namespace WiimoteEmu { // Four bytes for two objects. Filled with 0xFF if empty @@ -66,8 +71,7 @@ class CameraLogic : public I2CSlave void Reset(); void DoState(PointerWrap& p); - void Update(const ControllerEmu::Cursor::StateData& cursor, const NormalizedAccelData& accel, - bool sensor_bar_on_top); + void Update(const Common::Matrix44& transform, bool sensor_bar_on_top); void SetEnabled(bool is_enabled); static constexpr u8 I2C_ADDR = 0x58; diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp index 2d954d36d4cd..2f364171bd0f 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp @@ -11,29 +11,71 @@ #include "Core/HW/Wiimote.h" #include "Core/HW/WiimoteEmu/WiimoteEmu.h" #include "InputCommon/ControllerEmu/ControlGroup/Buttons.h" +#include "InputCommon/ControllerEmu/ControlGroup/Cursor.h" #include "InputCommon/ControllerEmu/ControlGroup/Force.h" #include "InputCommon/ControllerEmu/ControlGroup/Tilt.h" -namespace WiimoteEmu +namespace { constexpr int SHAKE_FREQ = 6; // Frame count of one up/down shake // < 9 no shake detection in "Wario Land: Shake It" constexpr int SHAKE_STEP_MAX = ::Wiimote::UPDATE_FREQ / SHAKE_FREQ; -void EmulateShake(NormalizedAccelData* const accel, ControllerEmu::Buttons* const buttons_group, - const double intensity, u8* const shake_step) +// Given a velocity, acceleration, and maximum jerk value, +// calculate change in position after a stop in the shortest possible time. +// Used to smoothly adjust acceleration and come to complete stops at precise positions. +// Based on equations for motion with constant jerk. +// s = s0 + v0 t + a0 t^2 / 2 + j t^3 / 6 +double CalculateStopDistance(double velocity, double acceleration, double max_jerk) +{ + // Math below expects velocity to be non-negative. + const auto velocity_flip = (velocity < 0 ? -1 : 1); + + const auto v_0 = velocity * velocity_flip; + const auto a_0 = acceleration * velocity_flip; + const auto j = max_jerk; + + // Time to reach zero acceleration. + const auto t_0 = a_0 / j; + + // Distance to reach zero acceleration. + const auto d_0 = std::pow(a_0, 3) / (3 * j * j) + (a_0 * v_0) / j; + + // Velocity at zero acceleration. + const auto v_1 = v_0 + a_0 * std::abs(t_0) - std::copysign(j * t_0 * t_0 / 2, t_0); + + // Distance to complete stop. + const auto d_1 = std::copysign(std::pow(std::abs(v_1), 3.0 / 2), v_1) / std::sqrt(j); + + return (d_0 + d_1) * velocity_flip; +} + +double CalculateStopDistance(double velocity, double max_accel) +{ + return velocity * velocity / (2 * std::copysign(max_accel, velocity)); +} + +} // namespace + +namespace WiimoteEmu +{ +Common::Vec3 EmulateShake(ControllerEmu::Buttons* const buttons_group, const double intensity, + u8* const shake_step) { // shake is a bitfield of X,Y,Z shake button states static const unsigned int btns[] = {0x01, 0x02, 0x04}; unsigned int shake = 0; buttons_group->GetState(&shake, btns); - for (int i = 0; i != 3; ++i) + Common::Vec3 accel; + + for (std::size_t i = 0; i != accel.data.size(); ++i) { if (shake & (1 << i)) { - (&(accel->x))[i] += std::sin(MathUtil::TAU * shake_step[i] / SHAKE_STEP_MAX) * intensity; + accel.data[i] = std::sin(MathUtil::TAU * shake_step[i] / SHAKE_STEP_MAX) * intensity * + GRAVITY_ACCELERATION; shake_step[i] = (shake_step[i] + 1) % SHAKE_STEP_MAX; } else @@ -41,18 +83,22 @@ void EmulateShake(NormalizedAccelData* const accel, ControllerEmu::Buttons* cons shake_step[i] = 0; } } + + return accel; } -void EmulateDynamicShake(NormalizedAccelData* const accel, DynamicData& dynamic_data, - ControllerEmu::Buttons* const buttons_group, - const DynamicConfiguration& config, u8* const shake_step) +Common::Vec3 EmulateDynamicShake(DynamicData& dynamic_data, + ControllerEmu::Buttons* const buttons_group, + const DynamicConfiguration& config, u8* const shake_step) { // shake is a bitfield of X,Y,Z shake button states static const unsigned int btns[] = {0x01, 0x02, 0x04}; unsigned int shake = 0; buttons_group->GetState(&shake, btns); - for (int i = 0; i != 3; ++i) + Common::Vec3 accel; + + for (std::size_t i = 0; i != accel.data.size(); ++i) { if ((shake & (1 << i)) && dynamic_data.executing_frames_left[i] == 0) { @@ -60,8 +106,8 @@ void EmulateDynamicShake(NormalizedAccelData* const accel, DynamicData& dynamic_ } else if (dynamic_data.executing_frames_left[i] > 0) { - (&(accel->x))[i] += - std::sin(MathUtil::TAU * shake_step[i] / SHAKE_STEP_MAX) * dynamic_data.intensity[i]; + accel.data[i] = std::sin(MathUtil::TAU * shake_step[i] / SHAKE_STEP_MAX) * + dynamic_data.intensity[i] * GRAVITY_ACCELERATION; shake_step[i] = (shake_step[i] + 1) % SHAKE_STEP_MAX; dynamic_data.executing_frames_left[i]--; } @@ -87,178 +133,160 @@ void EmulateDynamicShake(NormalizedAccelData* const accel, DynamicData& dynamic_ shake_step[i] = 0; } } + + return accel; +} + +void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* const tilt_group, float time_elapsed) +{ + const auto target = tilt_group->GetState(); + + // 180 degrees is currently the max tilt value. + const ControlState roll = target.x * MathUtil::PI; + const ControlState pitch = target.y * MathUtil::PI; + + // TODO: expose this setting in UI: + constexpr auto MAX_ACCEL = float(MathUtil::TAU * 50); + + ApproachAngleWithAccel(state, Common::Vec3(pitch, -roll, 0), MAX_ACCEL, time_elapsed); +} + +void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed) +{ + const auto target = swing_group->GetState(); + + // Note. Y/Z swapped because X/Y axis to the swing_group is X/Z to the wiimote. + // X is negated because Wiimote X+ is to the left. + ApproachPositionWithJerk(state, {-target.x, -target.z, target.y}, swing_group->GetMaxJerk(), + time_elapsed); + + // Just jump to our target angle scaled by our progress to the target position. + // TODO: If we wanted to be less hacky we could use ApproachAngleWithAccel. + const auto angle = state->position / swing_group->GetMaxDistance() * swing_group->GetTwistAngle(); + + const auto old_angle = state->angle; + state->angle = {-angle.z, 0, angle.x}; + + // Update velocity based on change in angle. + state->angular_velocity = state->angle - old_angle; } -void EmulateTilt(NormalizedAccelData* const accel, ControllerEmu::Tilt* const tilt_group, - const bool sideways, const bool upright) +WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g, + u16 one_g) { - // 180 degrees - const ControllerEmu::Tilt::StateData state = tilt_group->GetState(); - const ControlState roll = state.x * MathUtil::PI; - const ControlState pitch = state.y * MathUtil::PI; - - // Some notes that no one will understand but me :p - // left, forward, up - // lr/ left == negative for all orientations - // ud/ up == negative for upright longways - // fb/ forward == positive for (sideways flat) - - // Determine which axis is which direction - const u32 ud = upright ? (sideways ? 0 : 1) : 2; - const u32 lr = sideways; - const u32 fb = upright ? 2 : (sideways ? 0 : 1); - - // Sign fix - std::array sgn{{-1, 1, 1}}; - if (sideways && !upright) - sgn[fb] *= -1; - if (!sideways && upright) - sgn[ud] *= -1; - - (&accel->x)[ud] = (sin((MathUtil::PI / 2) - std::max(fabs(roll), fabs(pitch)))) * sgn[ud]; - (&accel->x)[lr] = -sin(roll) * sgn[lr]; - (&accel->x)[fb] = sin(pitch) * sgn[fb]; + const auto scaled_accel = accel * (one_g - zero_g) / float(GRAVITY_ACCELERATION); + + // 10-bit integers. + constexpr long MAX_VALUE = (1 << 10) - 1; + + return {u16(MathUtil::Clamp(std::lround(scaled_accel.x + zero_g), 0l, MAX_VALUE)), + u16(MathUtil::Clamp(std::lround(scaled_accel.y + zero_g), 0l, MAX_VALUE)), + u16(MathUtil::Clamp(std::lround(scaled_accel.z + zero_g), 0l, MAX_VALUE))}; } -void EmulateSwing(NormalizedAccelData* const accel, ControllerEmu::Force* const swing_group, - const double intensity, const bool sideways, const bool upright) +Common::Matrix44 EmulateCursorMovement(ControllerEmu::Cursor* ir_group) { - const ControllerEmu::Force::StateData swing = swing_group->GetState(); - - // Determine which axis is which direction - const std::array axis_map{{ - upright ? (sideways ? 0 : 1) : 2, // up/down - sideways, // left/right - upright ? 2 : (sideways ? 0 : 1), // forward/backward - }}; - - // Some orientations have up as positive, some as negative - // same with forward - std::array g_dir{{-1, -1, -1}}; - if (sideways && !upright) - g_dir[axis_map[2]] *= -1; - if (!sideways && upright) - g_dir[axis_map[0]] *= -1; - - for (std::size_t i = 0; i < swing.size(); ++i) - (&accel->x)[axis_map[i]] += swing[i] * g_dir[i] * intensity; + const auto cursor = ir_group->GetState(true); + + using Common::Matrix33; + using Common::Matrix44; + + // Values are optimized for default settings in "Super Mario Galaxy 2" + // This seems to be acceptable for a good number of games. + constexpr float YAW_ANGLE = 0.1472f; + constexpr float PITCH_ANGLE = 0.121f; + + // Nintendo recommends a distance of 1-3 meters. + constexpr float NEUTRAL_DISTANCE = 2.f; + + constexpr float MOVE_DISTANCE = 1.f; + + return Matrix44::Translate({0, MOVE_DISTANCE * float(cursor.z), 0}) * + Matrix44::FromMatrix33(Matrix33::RotateX(PITCH_ANGLE * cursor.y) * + Matrix33::RotateZ(YAW_ANGLE * cursor.x)) * + Matrix44::Translate({0, -NEUTRAL_DISTANCE, 0}); } -void EmulateDynamicSwing(NormalizedAccelData* const accel, DynamicData& dynamic_data, - ControllerEmu::Force* const swing_group, - const DynamicConfiguration& config, const bool sideways, - const bool upright) +void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_target, + float max_accel, float time_elapsed) { - const ControllerEmu::Force::StateData swing = swing_group->GetState(); - - // Determine which axis is which direction - const std::array axis_map{{ - upright ? (sideways ? 0 : 1) : 2, // up/down - sideways, // left/right - upright ? 2 : (sideways ? 0 : 1), // forward/backward - }}; - - // Some orientations have up as positive, some as negative - // same with forward - std::array g_dir{{-1, -1, -1}}; - if (sideways && !upright) - g_dir[axis_map[2]] *= -1; - if (!sideways && upright) - g_dir[axis_map[0]] *= -1; - - for (std::size_t i = 0; i < swing.size(); ++i) + const auto stop_distance = + Common::Vec3(CalculateStopDistance(state->angular_velocity.x, max_accel), + CalculateStopDistance(state->angular_velocity.y, max_accel), + CalculateStopDistance(state->angular_velocity.z, max_accel)); + + const auto offset = angle_target - state->angle; + const auto stop_offset = offset - stop_distance; + + const Common::Vec3 accel{std::copysign(max_accel, stop_offset.x), + std::copysign(max_accel, stop_offset.y), + std::copysign(max_accel, stop_offset.z)}; + + state->angular_velocity += accel * time_elapsed; + + const auto change_in_angle = + state->angular_velocity * time_elapsed + accel * time_elapsed * time_elapsed / 2; + + for (std::size_t i = 0; i != offset.data.size(); ++i) { - if (swing[i] > 0 && dynamic_data.executing_frames_left[i] == 0) + // If new velocity will overshoot assume we would have stopped right on target. + // TODO: Improve check to see if less accel would have caused undershoot. + if ((change_in_angle.data[i] / offset.data[i]) > 1.0) { - dynamic_data.timing[i]++; + state->angular_velocity.data[i] = 0; + state->angle.data[i] = angle_target.data[i]; } - else if (dynamic_data.executing_frames_left[i] > 0) + else { - (&accel->x)[axis_map[i]] += g_dir[i] * dynamic_data.intensity[i]; - dynamic_data.executing_frames_left[i]--; - } - else if (swing[i] == 0 && dynamic_data.timing[i] > 0) - { - if (dynamic_data.timing[i] > config.frames_needed_for_high_intensity) - { - dynamic_data.intensity[i] = config.high_intensity; - } - else if (dynamic_data.timing[i] < config.frames_needed_for_low_intensity) - { - dynamic_data.intensity[i] = config.low_intensity; - } - else - { - dynamic_data.intensity[i] = config.med_intensity; - } - dynamic_data.timing[i] = 0; - dynamic_data.executing_frames_left[i] = config.frames_to_execute; + state->angle.data[i] += change_in_angle.data[i]; } } } -WiimoteCommon::DataReportBuilder::AccelData DenormalizeAccelData(const NormalizedAccelData& accel, - u16 zero_g, u16 one_g) +void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target, + float max_jerk, float time_elapsed) { - const u8 accel_range = one_g - zero_g; + const auto stop_distance = + Common::Vec3(CalculateStopDistance(state->velocity.x, state->acceleration.x, max_jerk), + CalculateStopDistance(state->velocity.y, state->acceleration.y, max_jerk), + CalculateStopDistance(state->velocity.z, state->acceleration.z, max_jerk)); - const s32 unclamped_x = (s32)(accel.x * accel_range + zero_g); - const s32 unclamped_y = (s32)(accel.y * accel_range + zero_g); - const s32 unclamped_z = (s32)(accel.z * accel_range + zero_g); + const auto offset = position_target - state->position; + const auto stop_offset = offset - stop_distance; - WiimoteCommon::DataReportBuilder::AccelData result; + const Common::Vec3 jerk{std::copysign(max_jerk, stop_offset.x), + std::copysign(max_jerk, stop_offset.y), + std::copysign(max_jerk, stop_offset.z)}; - result.x = MathUtil::Clamp(unclamped_x, 0, 0x3ff); - result.y = MathUtil::Clamp(unclamped_y, 0, 0x3ff); - result.z = MathUtil::Clamp(unclamped_z, 0, 0x3ff); + state->acceleration += jerk * time_elapsed; - return result; + state->velocity += state->acceleration * time_elapsed + jerk * time_elapsed * time_elapsed / 2; + + const auto change_in_position = state->velocity * time_elapsed + + state->acceleration * time_elapsed * time_elapsed / 2 + + jerk * time_elapsed * time_elapsed * time_elapsed / 6; + + for (std::size_t i = 0; i != offset.data.size(); ++i) + { + // If new velocity will overshoot assume we would have stopped right on target. + // TODO: Improve check to see if less jerk would have caused undershoot. + if ((change_in_position.data[i] / offset.data[i]) > 1.0) + { + state->acceleration.data[i] = 0; + state->velocity.data[i] = 0; + state->position.data[i] = position_target.data[i]; + } + else + { + state->position.data[i] += change_in_position.data[i]; + } + } } -void Wiimote::GetAccelData(NormalizedAccelData* accel) +Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle) { - const bool is_sideways = IsSideways(); - const bool is_upright = IsUpright(); - - EmulateTilt(accel, m_tilt, is_sideways, is_upright); - - DynamicConfiguration swing_config; - swing_config.low_intensity = Config::Get(Config::WIIMOTE_INPUT_SWING_INTENSITY_SLOW); - swing_config.med_intensity = Config::Get(Config::WIIMOTE_INPUT_SWING_INTENSITY_MEDIUM); - swing_config.high_intensity = Config::Get(Config::WIIMOTE_INPUT_SWING_INTENSITY_FAST); - swing_config.frames_needed_for_high_intensity = - Config::Get(Config::WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_HELD_FAST); - swing_config.frames_needed_for_low_intensity = - Config::Get(Config::WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_HELD_SLOW); - swing_config.frames_to_execute = Config::Get(Config::WIIMOTE_INPUT_SWING_DYNAMIC_FRAMES_LENGTH); - - EmulateSwing(accel, m_swing, Config::Get(Config::WIIMOTE_INPUT_SWING_INTENSITY_MEDIUM), - is_sideways, is_upright); - EmulateSwing(accel, m_swing_slow, Config::Get(Config::WIIMOTE_INPUT_SWING_INTENSITY_SLOW), - is_sideways, is_upright); - EmulateSwing(accel, m_swing_fast, Config::Get(Config::WIIMOTE_INPUT_SWING_INTENSITY_FAST), - is_sideways, is_upright); - EmulateDynamicSwing(accel, m_swing_dynamic_data, m_swing_dynamic, swing_config, is_sideways, - is_upright); - - DynamicConfiguration shake_config; - shake_config.low_intensity = Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_SOFT); - shake_config.med_intensity = Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_MEDIUM); - shake_config.high_intensity = Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_HARD); - shake_config.frames_needed_for_high_intensity = - Config::Get(Config::WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_HELD_HARD); - shake_config.frames_needed_for_low_intensity = - Config::Get(Config::WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_HELD_SOFT); - shake_config.frames_to_execute = Config::Get(Config::WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_LENGTH); - - EmulateShake(accel, m_shake, Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_MEDIUM), - m_shake_step.data()); - EmulateShake(accel, m_shake_soft, Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_SOFT), - m_shake_soft_step.data()); - EmulateShake(accel, m_shake_hard, Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_HARD), - m_shake_hard_step.data()); - EmulateDynamicShake(accel, m_shake_dynamic_data, m_shake_dynamic, shake_config, - m_shake_dynamic_step.data()); + return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) * + Common::Matrix33::RotateX(angle.x); } } // namespace WiimoteEmu diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h index a1258725d1ae..2788c9513113 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h +++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h @@ -6,20 +6,19 @@ #include +#include "Common/Matrix.h" #include "Core/HW/WiimoteCommon/DataReport.h" #include "InputCommon/ControllerEmu/ControlGroup/Buttons.h" +#include "InputCommon/ControllerEmu/ControlGroup/Cursor.h" #include "InputCommon/ControllerEmu/ControlGroup/Force.h" #include "InputCommon/ControllerEmu/ControlGroup/Tilt.h" namespace WiimoteEmu { -struct NormalizedAccelData -{ - // Unit is 1G - double x, y, z; -}; +constexpr double GRAVITY_ACCELERATION = 9.80665; -// Used for a dynamic swing or shake +// Used for dynamic shake +// TODO: kill this. struct DynamicData { std::array timing; // Hold length in frames for each axis @@ -27,8 +26,36 @@ struct DynamicData std::array executing_frames_left; // Number of frames to execute the intensity operation }; -// Used for a dynamic swing or shake. +struct PositionalState +{ + Common::Vec3 position; + Common::Vec3 velocity; + Common::Vec3 acceleration; +}; + +struct RotationalState +{ + Common::Vec3 angle; + Common::Vec3 angular_velocity; +}; + +// Contains both positional and rotational state. +struct MotionState : PositionalState, RotationalState +{ +}; + +// Build a rotational matrix from euler angles. +Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle); + +void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& target, float max_jerk, + float time_elapsed); + +void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& target, float max_accel, + float time_elapsed); + +// Used for dynamic shake. // This is used to pass in data that defines the dynamic action +// TODO: kill this struct DynamicConfiguration { double low_intensity; @@ -43,24 +70,18 @@ struct DynamicConfiguration int frames_to_execute; // How many frames should we execute the action for? }; -void EmulateShake(NormalizedAccelData* accel, ControllerEmu::Buttons* buttons_group, - double intensity, u8* shake_step); - -void EmulateDynamicShake(NormalizedAccelData* accel, DynamicData& dynamic_data, - ControllerEmu::Buttons* buttons_group, const DynamicConfiguration& config, - u8* shake_step); +Common::Vec3 EmulateShake(ControllerEmu::Buttons* buttons_group, double intensity, u8* shake_step); -void EmulateTilt(NormalizedAccelData* accel, ControllerEmu::Tilt* tilt_group, bool sideways = false, - bool upright = false); +Common::Vec3 EmulateDynamicShake(DynamicData& dynamic_data, ControllerEmu::Buttons* buttons_group, + const DynamicConfiguration& config, u8* shake_step); -void EmulateSwing(NormalizedAccelData* accel, ControllerEmu::Force* swing_group, double intensity, - bool sideways = false, bool upright = false); +void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* tilt_group, float time_elapsed); +void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed); -void EmulateDynamicSwing(NormalizedAccelData* accel, DynamicData& dynamic_data, - ControllerEmu::Force* swing_group, const DynamicConfiguration& config, - bool sideways = false, bool upright = false); +// Convert m/s/s acceleration data to the format used by Wiimote/Nunchuk (10-bit unsigned integers). +WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g, + u16 one_g); -WiimoteCommon::DataReportBuilder::AccelData DenormalizeAccelData(const NormalizedAccelData& accel, - u16 zero_g, u16 one_g); +Common::Matrix44 EmulateCursorMovement(ControllerEmu::Cursor* ir_group); } // namespace WiimoteEmu diff --git a/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp b/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp index 07c61b9beefb..8bb39e5106d5 100644 --- a/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp @@ -583,6 +583,9 @@ void Wiimote::DoState(PointerWrap& p) GetActiveExtension()->DoState(p); // Dynamics + p.Do(m_swing_state); + p.Do(m_tilt_state); + // TODO: clean this up: p.Do(m_shake_step); diff --git a/Source/Core/Core/HW/WiimoteEmu/Extension/Extension.h b/Source/Core/Core/HW/WiimoteEmu/Extension/Extension.h index 6392d9d54213..88e3e87a3edc 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Extension/Extension.h +++ b/Source/Core/Core/HW/WiimoteEmu/Extension/Extension.h @@ -98,6 +98,8 @@ class EncryptedExtension : public Extension Register m_reg = {}; + void DoState(PointerWrap& p) override; + private: static constexpr u8 ENCRYPTION_ENABLED = 0xaa; @@ -105,8 +107,6 @@ class EncryptedExtension : public Extension int BusRead(u8 slave_addr, u8 addr, int count, u8* data_out) override; int BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in) override; - - void DoState(PointerWrap& p) override; }; } // namespace WiimoteEmu diff --git a/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp b/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp index 49e4ed0061c9..12aee5c61521 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp @@ -13,6 +13,7 @@ #include "Common/CommonTypes.h" #include "Common/MathUtil.h" #include "Core/Config/WiimoteInputSettings.h" +#include "Core/HW/Wiimote.h" #include "Core/HW/WiimoteEmu/WiimoteEmu.h" #include "InputCommon/ControllerEmu/Control/Input.h" @@ -45,8 +46,6 @@ Nunchuk::Nunchuk() : EncryptedExtension(_trans("Nunchuk")) // swing groups.emplace_back(m_swing = new ControllerEmu::Force(_trans("Swing"))); - groups.emplace_back(m_swing_slow = new ControllerEmu::Force("SwingSlow")); - groups.emplace_back(m_swing_fast = new ControllerEmu::Force("SwingFast")); // tilt groups.emplace_back(m_tilt = new ControllerEmu::Tilt(_trans("Tilt"))); @@ -96,32 +95,32 @@ void Nunchuk::Update() ++nc_data.jy; } - NormalizedAccelData accel; - - // tilt - EmulateTilt(&accel, m_tilt); - - // swing - EmulateSwing(&accel, m_swing, Config::Get(Config::NUNCHUK_INPUT_SWING_INTENSITY_MEDIUM)); - EmulateSwing(&accel, m_swing_slow, Config::Get(Config::NUNCHUK_INPUT_SWING_INTENSITY_SLOW)); - EmulateSwing(&accel, m_swing_fast, Config::Get(Config::NUNCHUK_INPUT_SWING_INTENSITY_FAST)); - - // shake - EmulateShake(&accel, m_shake, Config::Get(Config::NUNCHUK_INPUT_SHAKE_INTENSITY_MEDIUM), - m_shake_step.data()); - EmulateShake(&accel, m_shake_soft, Config::Get(Config::NUNCHUK_INPUT_SHAKE_INTENSITY_SOFT), - m_shake_soft_step.data()); - EmulateShake(&accel, m_shake_hard, Config::Get(Config::NUNCHUK_INPUT_SHAKE_INTENSITY_HARD), - m_shake_hard_step.data()); - // buttons m_buttons->GetState(&nc_data.bt.hex, nunchuk_button_bitmasks.data()); // flip the button bits :/ nc_data.bt.hex ^= 0x03; + // Acceleration data: + EmulateSwing(&m_swing_state, m_swing, 1.f / ::Wiimote::UPDATE_FREQ); + EmulateTilt(&m_tilt_state, m_tilt, 1.f / ::Wiimote::UPDATE_FREQ); + + const auto transformation = + GetRotationalMatrix(-m_tilt_state.angle) * GetRotationalMatrix(-m_swing_state.angle); + + Common::Vec3 accel = transformation * (m_swing_state.acceleration + + Common::Vec3(0, 0, float(GRAVITY_ACCELERATION))); + + // shake + accel += EmulateShake(m_shake, Config::Get(Config::NUNCHUK_INPUT_SHAKE_INTENSITY_MEDIUM), + m_shake_step.data()); + accel += EmulateShake(m_shake_soft, Config::Get(Config::NUNCHUK_INPUT_SHAKE_INTENSITY_SOFT), + m_shake_soft_step.data()); + accel += EmulateShake(m_shake_hard, Config::Get(Config::NUNCHUK_INPUT_SHAKE_INTENSITY_HARD), + m_shake_hard_step.data()); + // Calibration values are 8-bit but we want 10-bit precision, so << 2. - auto acc = DenormalizeAccelData(accel, ACCEL_ZERO_G << 2, ACCEL_ONE_G << 2); + const auto acc = ConvertAccelData(accel, ACCEL_ZERO_G << 2, ACCEL_ONE_G << 2); nc_data.ax = (acc.x >> 2) & 0xFF; nc_data.ay = (acc.y >> 2) & 0xFF; @@ -145,6 +144,9 @@ void Nunchuk::Reset() m_reg = {}; m_reg.identifier = nunchuk_id; + m_swing_state = {}; + m_tilt_state = {}; + // Build calibration data: m_reg.calibration = {{ // Accel Zero X,Y,Z: @@ -195,6 +197,14 @@ ControllerEmu::ControlGroup* Nunchuk::GetGroup(NunchukGroup group) } } +void Nunchuk::DoState(PointerWrap& p) +{ + EncryptedExtension::DoState(p); + + p.Do(m_swing_state); + p.Do(m_tilt_state); +} + void Nunchuk::LoadDefaults(const ControllerInterface& ciface) { // Stick diff --git a/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.h b/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.h index 22c66f526397..a542b7ebc2be 100644 --- a/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.h +++ b/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.h @@ -7,6 +7,7 @@ #include #include "Core/HW/WiimoteCommon/WiimoteReport.h" +#include "Core/HW/WiimoteEmu/Dynamics.h" #include "Core/HW/WiimoteEmu/Extension/Extension.h" namespace ControllerEmu @@ -73,6 +74,7 @@ class Nunchuk : public EncryptedExtension void Update() override; bool IsButtonPressed() const override; void Reset() override; + void DoState(PointerWrap& p) override; ControllerEmu::ControlGroup* GetGroup(NunchukGroup group); @@ -101,8 +103,6 @@ class Nunchuk : public EncryptedExtension ControllerEmu::Tilt* m_tilt; ControllerEmu::Force* m_swing; - ControllerEmu::Force* m_swing_slow; - ControllerEmu::Force* m_swing_fast; ControllerEmu::Buttons* m_shake; ControllerEmu::Buttons* m_shake_soft; @@ -111,6 +111,11 @@ class Nunchuk : public EncryptedExtension ControllerEmu::Buttons* m_buttons; ControllerEmu::AnalogStick* m_stick; + // Dynamics: + MotionState m_swing_state; + RotationalState m_tilt_state; + + // TODO: kill std::array m_shake_step{}; std::array m_shake_soft_step{}; std::array m_shake_hard_step{}; diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp index 6e31340c3afd..0de1cc0b16e3 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp @@ -64,9 +64,6 @@ static const char* const named_buttons[] = { void Wiimote::Reset() { - // TODO: This value should be re-read if SYSCONF gets changed. - m_sensor_bar_on_top = Config::Get(Config::SYSCONF_SENSOR_BAR_POSITION) != 0; - SetRumble(false); // Wiimote starts in non-continuous CORE mode: @@ -133,10 +130,12 @@ void Wiimote::Reset() m_status.extension = m_extension_port.IsDeviceConnected(); // Dynamics: + m_swing_state = {}; + m_tilt_state = {}; + m_shake_step = {}; m_shake_soft_step = {}; m_shake_hard_step = {}; - m_swing_dynamic_data = {}; m_shake_dynamic_data = {}; } @@ -157,9 +156,6 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index) // swing groups.emplace_back(m_swing = new ControllerEmu::Force(_trans("Swing"))); - groups.emplace_back(m_swing_slow = new ControllerEmu::Force("SwingSlow")); - groups.emplace_back(m_swing_fast = new ControllerEmu::Force("SwingFast")); - groups.emplace_back(m_swing_dynamic = new ControllerEmu::Force("Swing Dynamic")); // tilt groups.emplace_back(m_tilt = new ControllerEmu::Tilt(_trans("Tilt"))); @@ -247,6 +243,13 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index) m_hotkeys->AddInput(_trans("Sideways Hold"), false); m_hotkeys->AddInput(_trans("Upright Hold"), false); + auto config_change_callback = [this] { + m_sensor_bar_on_top = Config::Get(Config::SYSCONF_SENSOR_BAR_POSITION) != 0; + }; + Config::AddConfigChangedCallback(config_change_callback); + + config_change_callback(); + Reset(); } @@ -349,11 +352,18 @@ void Wiimote::Update() if (0 == m_reporting_channel) return; + const auto lock = GetStateLock(); + + // Hotkey / settings modifier + // Data is later accessed in IsSideways and IsUpright + m_hotkeys->GetState(); + + StepDynamics(); + // Update buttons in the status struct which is sent in 99% of input reports. // FYI: Movies only sync button updates in data reports. if (!Core::WantsDeterminism()) { - const auto lock = GetStateLock(); UpdateButtonsStatus(); } @@ -405,14 +415,7 @@ void Wiimote::SendDataReport() } else { - const auto lock = GetStateLock(); - - // Hotkey / settings modifier - // Data is later accessed in IsSideways and IsUpright - m_hotkeys->GetState(); - - // CORE - + // Core buttons: if (rpt_builder.HasCore()) { if (Core::WantsDeterminism()) @@ -424,26 +427,19 @@ void Wiimote::SendDataReport() rpt_builder.SetCoreData(m_status.buttons); } - // ACCEL - - // FYI: This data is also used to tilt the IR dots. - NormalizedAccelData norm_accel = {}; - GetAccelData(&norm_accel); - + // Acceleration: if (rpt_builder.HasAccel()) { // Calibration values are 8-bit but we want 10-bit precision, so << 2. DataReportBuilder::AccelData accel = - DenormalizeAccelData(norm_accel, ACCEL_ZERO_G << 2, ACCEL_ONE_G << 2); + ConvertAccelData(GetAcceleration(), ACCEL_ZERO_G << 2, ACCEL_ONE_G << 2); rpt_builder.SetAccelData(accel); } - // IR - + // IR Camera: if (rpt_builder.HasIR()) { - const auto cursor = m_ir->GetState(true); - m_camera_logic.Update(cursor, norm_accel, m_sensor_bar_on_top); + m_camera_logic.Update(GetTransformation(), m_sensor_bar_on_top); // The real wiimote reads camera data from the i2c bus starting at offset 0x37: const u8 camera_data_offset = @@ -453,8 +449,7 @@ void Wiimote::SendDataReport() rpt_builder.GetIRDataPtr()); } - // EXT - + // Extension port: if (rpt_builder.HasExt()) { // Update extension first as motion-plus may read from it. @@ -707,7 +702,67 @@ bool Wiimote::IsUpright() const void Wiimote::SetRumble(bool on) { + const auto lock = GetStateLock(); m_motor->control_ref->State(on); } +void Wiimote::StepDynamics() +{ + EmulateSwing(&m_swing_state, m_swing, 1.f / ::Wiimote::UPDATE_FREQ); + EmulateTilt(&m_tilt_state, m_tilt, 1.f / ::Wiimote::UPDATE_FREQ); + + // TODO: Move cursor state out of ControllerEmu::Cursor + // const auto cursor_mtx = EmulateCursorMovement(m_ir); +} + +Common::Vec3 Wiimote::GetAcceleration() +{ + // Includes effects of: + // IR, Tilt, Swing, Orientation, Shake + + auto orientation = Common::Matrix33::Identity(); + + if (IsSideways()) + orientation *= Common::Matrix33::RotateZ(float(MathUtil::TAU / -4)); + + if (IsUpright()) + orientation *= Common::Matrix33::RotateX(float(MathUtil::TAU / 4)); + + Common::Vec3 accel = + orientation * + GetTransformation().Transform( + m_swing_state.acceleration + Common::Vec3(0, 0, float(GRAVITY_ACCELERATION)), 0); + + DynamicConfiguration shake_config; + shake_config.low_intensity = Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_SOFT); + shake_config.med_intensity = Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_MEDIUM); + shake_config.high_intensity = Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_HARD); + shake_config.frames_needed_for_high_intensity = + Config::Get(Config::WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_HELD_HARD); + shake_config.frames_needed_for_low_intensity = + Config::Get(Config::WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_HELD_SOFT); + shake_config.frames_to_execute = Config::Get(Config::WIIMOTE_INPUT_SHAKE_DYNAMIC_FRAMES_LENGTH); + + accel += EmulateShake(m_shake, Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_MEDIUM), + m_shake_step.data()); + accel += EmulateShake(m_shake_soft, Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_SOFT), + m_shake_soft_step.data()); + accel += EmulateShake(m_shake_hard, Config::Get(Config::WIIMOTE_INPUT_SHAKE_INTENSITY_HARD), + m_shake_hard_step.data()); + accel += EmulateDynamicShake(m_shake_dynamic_data, m_shake_dynamic, shake_config, + m_shake_dynamic_step.data()); + + return accel; +} + +Common::Matrix44 Wiimote::GetTransformation() const +{ + // Includes positional and rotational effects of: + // IR, Swing, Tilt + + return Common::Matrix44::FromMatrix33(GetRotationalMatrix(-m_tilt_state.angle) * + GetRotationalMatrix(-m_swing_state.angle)) * + EmulateCursorMovement(m_ir) * Common::Matrix44::Translate(-m_swing_state.position); +} + } // namespace WiimoteEmu diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h index 09b918ca9dff..f534548d6537 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h @@ -125,6 +125,7 @@ class Wiimote : public ControllerEmu::EmulatedController ControllerEmu::ControlGroup* GetTurntableGroup(TurntableGroup group); void Update(); + void StepDynamics(); void InterruptChannel(u16 channel_id, const void* data, u32 size); void ControlChannel(u16 channel_id, const void* data, u32 size); @@ -146,7 +147,9 @@ class Wiimote : public ControllerEmu::EmulatedController void UpdateButtonsStatus(); - void GetAccelData(NormalizedAccelData* accel); + Common::Vec3 GetAcceleration(); + // Used for simulating camera data. Does not include orientation transformations. + Common::Matrix44 GetTransformation() const; void HIDOutputReport(const void* data, u32 size); @@ -238,9 +241,6 @@ class Wiimote : public ControllerEmu::EmulatedController ControllerEmu::Cursor* m_ir; ControllerEmu::Tilt* m_tilt; ControllerEmu::Force* m_swing; - ControllerEmu::Force* m_swing_slow; - ControllerEmu::Force* m_swing_fast; - ControllerEmu::Force* m_swing_dynamic; ControllerEmu::ControlGroup* m_rumble; ControllerEmu::Output* m_motor; ControllerEmu::Attachments* m_attachments; @@ -281,11 +281,14 @@ class Wiimote : public ControllerEmu::EmulatedController UsableEEPROMData m_eeprom; // Dynamics: + MotionState m_swing_state; + RotationalState m_tilt_state; + + // TODO: kill these: std::array m_shake_step{}; std::array m_shake_soft_step{}; std::array m_shake_hard_step{}; std::array m_shake_dynamic_step{}; - DynamicData m_swing_dynamic_data; DynamicData m_shake_dynamic_data; }; } // namespace WiimoteEmu diff --git a/Source/Core/Core/State.cpp b/Source/Core/Core/State.cpp index 97c3b020db80..c616ee923ba4 100644 --- a/Source/Core/Core/State.cpp +++ b/Source/Core/Core/State.cpp @@ -74,7 +74,7 @@ static Common::Event g_compressAndDumpStateSyncEvent; static std::thread g_save_thread; // Don't forget to increase this after doing changes on the savestate system -static const u32 STATE_VERSION = 103; // Last changed in PR 7674 +static const u32 STATE_VERSION = 104; // Last changed in PR 7806 // Maps savestate versions to Dolphin versions. // Versions after 42 don't need to be added to this list, diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp index 0e03543dc7e5..d9714a6aea29 100644 --- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp +++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp @@ -19,6 +19,7 @@ #include "InputCommon/ControlReference/ControlReference.h" #include "InputCommon/ControllerEmu/Control/Control.h" #include "InputCommon/ControllerEmu/ControlGroup/Cursor.h" +#include "InputCommon/ControllerEmu/ControlGroup/Force.h" #include "InputCommon/ControllerEmu/ControlGroup/MixedTriggers.h" #include "InputCommon/ControllerEmu/Setting/NumericSetting.h" #include "InputCommon/ControllerInterface/Device.h" @@ -45,16 +46,19 @@ const QColor STICK_GATE_COLOR = Qt::lightGray; const QColor C_STICK_GATE_COLOR = Qt::yellow; const QColor CURSOR_TV_COLOR = 0xaed6f1; const QColor TILT_GATE_COLOR = 0xa2d9ce; +const QColor SWING_GATE_COLOR = 0xcea2d9; constexpr int INPUT_DOT_RADIUS = 2; +constexpr int INDICATOR_UPDATE_FREQ = 30; + MappingIndicator::MappingIndicator(ControllerEmu::ControlGroup* group) : m_group(group) { setMinimumHeight(128); const auto timer = new QTimer(this); connect(timer, &QTimer::timeout, this, [this] { repaint(); }); - timer->start(1000 / 30); + timer->start(1000 / INDICATOR_UPDATE_FREQ); } namespace @@ -250,7 +254,19 @@ void MappingIndicator::DrawReshapableInput(ControllerEmu::ReshapableInput& stick // We should probably hold the mutex for UI updates. Settings::Instance().SetControllerStateNeeded(true); const auto raw_coord = stick.GetReshapableState(false); - const auto adj_coord = stick.GetReshapableState(true); + + Common::DVec2 adj_coord; + if (is_tilt) + { + WiimoteEmu::EmulateTilt(&m_motion_state, static_cast(&stick), + 1.f / INDICATOR_UPDATE_FREQ); + adj_coord = Common::DVec2{-m_motion_state.angle.y, m_motion_state.angle.x} / MathUtil::PI; + } + else + { + adj_coord = stick.GetReshapableState(true); + } + Settings::Instance().SetControllerStateNeeded(false); UpdateCalibrationWidget(raw_coord); @@ -408,6 +424,100 @@ void MappingIndicator::DrawMixedTriggers() } } +void MappingIndicator::DrawForce(ControllerEmu::Force& force) +{ + const QColor gate_brush_color = SWING_GATE_COLOR; + const QColor gate_pen_color = gate_brush_color.darker(125); + + // TODO: This SetControllerStateNeeded interface leaks input into the game + // We should probably hold the mutex for UI updates. + Settings::Instance().SetControllerStateNeeded(true); + const auto raw_coord = force.GetState(false); + WiimoteEmu::EmulateSwing(&m_motion_state, &force, 1.f / INDICATOR_UPDATE_FREQ); + const auto& adj_coord = m_motion_state.position; + Settings::Instance().SetControllerStateNeeded(false); + + UpdateCalibrationWidget({raw_coord.x, raw_coord.y}); + + // Bounding box size: + const double scale = height() / 2.5; + + QPainter p(this); + p.translate(width() / 2, height() / 2); + + // Bounding box. + p.setBrush(BBOX_BRUSH_COLOR); + p.setPen(BBOX_PEN_COLOR); + p.drawRect(-scale - 1, -scale - 1, scale * 2 + 1, scale * 2 + 1); + + // UI y-axis is opposite that of stick. + p.scale(1.0, -1.0); + + // Enable AA after drawing bounding box. + p.setRenderHint(QPainter::Antialiasing, true); + p.setRenderHint(QPainter::SmoothPixmapTransform, true); + + if (IsCalibrating()) + { + DrawCalibration(p, {raw_coord.x, raw_coord.y}); + return; + } + + // Deadzone for Z (forward/backward): + const double deadzone = force.numeric_settings[force.SETTING_DEADZONE]->GetValue(); + if (deadzone > 0.0) + { + p.setPen(DEADZONE_COLOR); + p.setBrush(DEADZONE_BRUSH); + p.drawRect(QRectF(-scale, -deadzone * scale, scale * 2, deadzone * scale * 2)); + } + + // Raw Z: + p.setPen(Qt::NoPen); + p.setBrush(RAW_INPUT_COLOR); + p.drawRect( + QRectF(-scale, raw_coord.z * scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS)); + + // Adjusted Z: + if (adj_coord.y) + { + p.setBrush(ADJ_INPUT_COLOR); + p.drawRect( + QRectF(-scale, adj_coord.y * -scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS)); + } + + // Draw "gate" shape. + p.setPen(gate_pen_color); + p.setBrush(gate_brush_color); + p.drawPolygon(GetPolygonFromRadiusGetter( + [&force](double ang) { return force.GetGateRadiusAtAngle(ang); }, scale)); + + // Deadzone. + p.setPen(DEADZONE_COLOR); + p.setBrush(DEADZONE_BRUSH); + p.drawPolygon(GetPolygonFromRadiusGetter( + [&force](double ang) { return force.GetDeadzoneRadiusAtAngle(ang); }, scale)); + + // Input shape. + p.setPen(INPUT_SHAPE_PEN); + p.setBrush(Qt::NoBrush); + p.drawPolygon(GetPolygonFromRadiusGetter( + [&force](double ang) { return force.GetInputRadiusAtAngle(ang); }, scale)); + + // Raw stick position. + p.setPen(Qt::NoPen); + p.setBrush(RAW_INPUT_COLOR); + p.drawEllipse(QPointF{raw_coord.x, raw_coord.y} * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS); + + // Adjusted position: + if (adj_coord.x || adj_coord.z) + { + p.setPen(Qt::NoPen); + p.setBrush(ADJ_INPUT_COLOR); + p.drawEllipse(QPointF{-adj_coord.x, adj_coord.z} * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS); + } +} + void MappingIndicator::paintEvent(QPaintEvent*) { switch (m_group->type) @@ -422,6 +532,9 @@ void MappingIndicator::paintEvent(QPaintEvent*) case ControllerEmu::GroupType::MixedTriggers: DrawMixedTriggers(); break; + case ControllerEmu::GroupType::Force: + DrawForce(*static_cast(m_group)); + break; default: break; } diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h index 67c2e53afdde..4a630eac28dc 100644 --- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h +++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h @@ -7,6 +7,7 @@ #include #include +#include "Core/HW/WiimoteEmu/Dynamics.h" #include "InputCommon/ControllerEmu/StickGate.h" namespace ControllerEmu @@ -14,6 +15,7 @@ namespace ControllerEmu class Control; class ControlGroup; class Cursor; +class Force; class NumericSetting; } // namespace ControllerEmu @@ -34,6 +36,7 @@ class MappingIndicator : public QWidget void DrawCursor(ControllerEmu::Cursor& cursor); void DrawReshapableInput(ControllerEmu::ReshapableInput& stick); void DrawMixedTriggers(); + void DrawForce(ControllerEmu::Force&); void DrawCalibration(QPainter& p, Common::DVec2 point); void paintEvent(QPaintEvent*) override; @@ -43,6 +46,8 @@ class MappingIndicator : public QWidget ControllerEmu::ControlGroup* const m_group; CalibrationWidget* m_calibration_widget{}; + + WiimoteEmu::MotionState m_motion_state{}; }; class CalibrationWidget : public QToolButton diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingWidget.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingWidget.cpp index 3c6036f1593e..0224d4cfed57 100644 --- a/Source/Core/DolphinQt/Config/Mapping/MappingWidget.cpp +++ b/Source/Core/DolphinQt/Config/Mapping/MappingWidget.cpp @@ -77,11 +77,13 @@ QGroupBox* MappingWidget::CreateGroupBox(const QString& name, ControllerEmu::Con const bool need_indicator = group->type == ControllerEmu::GroupType::Cursor || group->type == ControllerEmu::GroupType::Stick || group->type == ControllerEmu::GroupType::Tilt || - group->type == ControllerEmu::GroupType::MixedTriggers; + group->type == ControllerEmu::GroupType::MixedTriggers || + group->type == ControllerEmu::GroupType::Force; const bool need_calibration = group->type == ControllerEmu::GroupType::Cursor || group->type == ControllerEmu::GroupType::Stick || - group->type == ControllerEmu::GroupType::Tilt; + group->type == ControllerEmu::GroupType::Tilt || + group->type == ControllerEmu::GroupType::Force; for (auto& control : group->controls) { diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp index c2178ea370f7..3b3de522a737 100644 --- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp +++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp @@ -4,12 +4,11 @@ #include "InputCommon/ControllerEmu/ControlGroup/Force.h" -#include -#include #include #include "Common/Common.h" -#include "Common/CommonTypes.h" +#include "Common/MathUtil.h" + #include "InputCommon/ControlReference/ControlReference.h" #include "InputCommon/ControllerEmu/Control/Input.h" #include "InputCommon/ControllerEmu/ControllerEmu.h" @@ -17,7 +16,7 @@ namespace ControllerEmu { -Force::Force(const std::string& name_) : ControlGroup(name_, GroupType::Force) +Force::Force(const std::string& name_) : ReshapableInput(name_, name_, GroupType::Force) { controls.emplace_back(std::make_unique(Translate, _trans("Up"))); controls.emplace_back(std::make_unique(Translate, _trans("Down"))); @@ -26,26 +25,69 @@ Force::Force(const std::string& name_) : ControlGroup(name_, GroupType::Force) controls.emplace_back(std::make_unique(Translate, _trans("Forward"))); controls.emplace_back(std::make_unique(Translate, _trans("Backward"))); - numeric_settings.emplace_back(std::make_unique(_trans("Dead Zone"), 0, 0, 50)); + // Maximum swing movement (centimeters). + numeric_settings.emplace_back(std::make_unique(_trans("Distance"), 0.25, 1, 100)); + + // Maximum jerk (m/s^3). + // i18n: "Jerk" as it relates to physics. The time derivative of acceleration. + numeric_settings.emplace_back(std::make_unique(_trans("Jerk"), 5.0, 1, 1000)); + + // Angle of twist applied at the extremities of the swing (degrees). + numeric_settings.emplace_back(std::make_unique(_trans("Angle"), 0.45, 0, 180)); } -Force::StateData Force::GetState() +Force::ReshapeData Force::GetReshapableState(bool adjusted) { - StateData state_data; - const ControlState deadzone = numeric_settings[0]->GetValue(); + const ControlState y = controls[0]->control_ref->State() - controls[1]->control_ref->State(); + const ControlState x = controls[3]->control_ref->State() - controls[2]->control_ref->State(); - for (u32 i = 0; i < 6; i += 2) - { - const ControlState state = - controls[i + 1]->control_ref->State() - controls[i]->control_ref->State(); + // Return raw values. (used in UI) + if (!adjusted) + return {x, y}; + + return Reshape(x, y); +} - ControlState tmpf = 0; - if (fabs(state) > deadzone) - tmpf = ((state - (deadzone * sign(state))) / (1 - deadzone)); +Force::StateData Force::GetState(bool adjusted) +{ + const auto state = GetReshapableState(adjusted); + ControlState z = controls[4]->control_ref->State() - controls[5]->control_ref->State(); - state_data[i / 2] = tmpf; + if (adjusted) + { + // Apply deadzone to z. + const ControlState deadzone = numeric_settings[SETTING_DEADZONE]->GetValue(); + z = std::copysign(std::max(0.0, std::abs(z) - deadzone) / (1.0 - deadzone), z); } - return state_data; + return {float(state.x), float(state.y), float(z)}; +} + +ControlState Force::GetGateRadiusAtAngle(double) const +{ + // Just a circle of the configured distance: + return numeric_settings[SETTING_DISTANCE]->GetValue(); +} + +ControlState Force::GetMaxJerk() const +{ + return numeric_settings[SETTING_JERK]->GetValue() * 100; +} + +ControlState Force::GetTwistAngle() const +{ + return numeric_settings[SETTING_ANGLE]->GetValue() * MathUtil::TAU / 3.60; +} + +ControlState Force::GetMaxDistance() const +{ + return numeric_settings[SETTING_DISTANCE]->GetValue(); +} + +ControlState Force::GetDefaultInputRadiusAtAngle(double) const +{ + // Just a circle of radius 1.0. + return 1.0; } + } // namespace ControllerEmu diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h index 7b55d3535f9d..0b4fdcf63497 100644 --- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h +++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h @@ -6,18 +6,41 @@ #include #include -#include "InputCommon/ControllerEmu/ControlGroup/ControlGroup.h" -#include "InputCommon/ControllerInterface/Device.h" + +#include "Common/Matrix.h" +#include "InputCommon/ControllerEmu/StickGate.h" namespace ControllerEmu { -class Force : public ControlGroup +class Force : public ReshapableInput { public: - using StateData = std::array; + using StateData = Common::Vec3; explicit Force(const std::string& name); - StateData GetState(); + ReshapeData GetReshapableState(bool adjusted) final override; + ControlState GetGateRadiusAtAngle(double ang) const final override; + + ControlState GetDefaultInputRadiusAtAngle(double angle) const final override; + + StateData GetState(bool adjusted = true); + + // Return jerk in m/s^3. + ControlState GetMaxJerk() const; + + // Return twist angle in radians. + ControlState GetTwistAngle() const; + + // Return swing distance in meters. + ControlState GetMaxDistance() const; + +private: + enum + { + SETTING_DISTANCE = ReshapableInput::SETTING_COUNT, + SETTING_JERK, + SETTING_ANGLE, + }; }; } // namespace ControllerEmu diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Tilt.cpp b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Tilt.cpp index bacf158be023..8407aa646ff9 100644 --- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Tilt.cpp +++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Tilt.cpp @@ -4,13 +4,9 @@ #include "InputCommon/ControllerEmu/ControlGroup/Tilt.h" -#include -#include -#include #include #include "Common/Common.h" -#include "Common/MathUtil.h" #include "InputCommon/ControlReference/ControlReference.h" #include "InputCommon/ControllerEmu/Control/Control.h" @@ -19,8 +15,7 @@ namespace ControllerEmu { -Tilt::Tilt(const std::string& name_) - : ReshapableInput(name_, name_, GroupType::Tilt), m_last_update(Clock::now()) +Tilt::Tilt(const std::string& name_) : ReshapableInput(name_, name_, GroupType::Tilt) { controls.emplace_back(std::make_unique(Translate, _trans("Forward"))); controls.emplace_back(std::make_unique(Translate, _trans("Backward"))); @@ -43,33 +38,7 @@ Tilt::ReshapeData Tilt::GetReshapableState(bool adjusted) const ControlState modifier = controls[4]->control_ref->State(); - // Compute desired tilt: - StateData target = Reshape(x, y, modifier); - - // Step the simulation. This is somewhat ugly being here. - // We should be able to GetState without changing state. - // State should be stored outside of this object inside the wiimote, - // and separately inside the UI. - - // We're using system time rather than ticks to step this. - // I don't think that's too horrible as we can consider this part of user input. - // And at least the Mapping UI will behave sanely this way. - // TODO: when state is moved outside of this class have a separate Step() - // function that takes a ms_passed argument - const auto now = Clock::now(); - const auto ms_since_update = - std::chrono::duration_cast(now - m_last_update).count(); - m_last_update = now; - - const double max_step = MAX_DEG_PER_SEC / 180.0 * ms_since_update / 1000; - - // TODO: Allow wrap around from 1.0 to -1.0 - // (take the fastest route to target) - - m_tilt.x += MathUtil::Clamp(target.x - m_tilt.x, -max_step, max_step); - m_tilt.y += MathUtil::Clamp(target.y - m_tilt.y, -max_step, max_step); - - return m_tilt; + return Reshape(x, y, modifier); } Tilt::StateData Tilt::GetState() diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Tilt.h b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Tilt.h index c18f6dc46d09..6a01a575a64a 100644 --- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Tilt.h +++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Tilt.h @@ -4,7 +4,6 @@ #pragma once -#include #include #include "InputCommon/ControllerEmu/StickGate.h" @@ -33,12 +32,5 @@ class Tilt : public ReshapableInput { SETTING_MAX_ANGLE = ReshapableInput::SETTING_COUNT, }; - - static constexpr int MAX_DEG_PER_SEC = 360 * 6; - - StateData m_tilt; - - using Clock = std::chrono::steady_clock; - Clock::time_point m_last_update; }; } // namespace ControllerEmu