Merge pull request #8352 from rlnilsen/motion-controller-support-via-…

…cemuhook-protocol

Support for motion controllers like the DualShock 4

Source/Core/Common/CommonPaths.h

@@ -85,6 +85,7 @@
 #define GFX_CONFIG "GFX.ini"
 #define DEBUGGER_CONFIG "Debugger.ini"
 #define LOGGER_CONFIG "Logger.ini"
+#define DUALSHOCKUDPCLIENT_CONFIG "DSUClient.ini"
 
 // Files in the directory returned by GetUserPath(D_LOGS_IDX)
 #define MAIN_LOG "dolphin.log"

Source/Core/Common/Config/Config.cpp

@@ -133,9 +133,15 @@ void ClearCurrentRunLayer()
 }
 
 static const std::map<System, std::string> system_to_name = {
-    {System::Main, "Dolphin"},          {System::GCPad, "GCPad"},    {System::WiiPad, "Wiimote"},
-    {System::GCKeyboard, "GCKeyboard"}, {System::GFX, "Graphics"},   {System::Logger, "Logger"},
-    {System::Debugger, "Debugger"},     {System::SYSCONF, "SYSCONF"}};
+    {System::Main, "Dolphin"},
+    {System::GCPad, "GCPad"},
+    {System::WiiPad, "Wiimote"},
+    {System::GCKeyboard, "GCKeyboard"},
+    {System::GFX, "Graphics"},
+    {System::Logger, "Logger"},
+    {System::Debugger, "Debugger"},
+    {System::SYSCONF, "SYSCONF"},
+    {System::DualShockUDPClient, "DualShockUDPClient"}};
 
 const std::string& GetSystemName(System system)
 {

Source/Core/Common/Config/Enums.h

@@ -30,6 +30,7 @@ enum class System
   GFX,
   Logger,
   Debugger,
+  DualShockUDPClient,
 };
 
 constexpr std::array<LayerType, 7> SEARCH_ORDER{{

Source/Core/Common/FileUtil.cpp

@@ -794,6 +794,8 @@ static void RebuildUserDirectories(unsigned int dir_index)
     s_user_paths[F_GFXCONFIG_IDX] = s_user_paths[D_CONFIG_IDX] + GFX_CONFIG;
     s_user_paths[F_DEBUGGERCONFIG_IDX] = s_user_paths[D_CONFIG_IDX] + DEBUGGER_CONFIG;
     s_user_paths[F_LOGGERCONFIG_IDX] = s_user_paths[D_CONFIG_IDX] + LOGGER_CONFIG;
+    s_user_paths[F_DUALSHOCKUDPCLIENTCONFIG_IDX] =
+        s_user_paths[D_CONFIG_IDX] + DUALSHOCKUDPCLIENT_CONFIG;
     s_user_paths[F_MAINLOG_IDX] = s_user_paths[D_LOGS_IDX] + MAIN_LOG;
     s_user_paths[F_MEM1DUMP_IDX] = s_user_paths[D_DUMP_IDX] + MEM1_DUMP;
     s_user_paths[F_MEM2DUMP_IDX] = s_user_paths[D_DUMP_IDX] + MEM2_DUMP;

Source/Core/Common/FileUtil.h

@@ -70,6 +70,7 @@ enum
   F_MEMORYWATCHERLOCATIONS_IDX,
   F_MEMORYWATCHERSOCKET_IDX,
   F_WIISDCARD_IDX,
+  F_DUALSHOCKUDPCLIENTCONFIG_IDX,
   NUM_PATH_INDICES
 };
 

Source/Core/Core/ConfigLoaders/BaseConfigLoader.cpp

@@ -89,6 +89,7 @@ const std::map<Config::System, int> system_to_ini = {
     {Config::System::GFX, F_GFXCONFIG_IDX},
     {Config::System::Logger, F_LOGGERCONFIG_IDX},
     {Config::System::Debugger, F_DEBUGGERCONFIG_IDX},
+    {Config::System::DualShockUDPClient, F_DUALSHOCKUDPCLIENTCONFIG_IDX},
 };
 
 // INI layer configuration loader

Source/Core/Core/ConfigLoaders/IsSettingSaveable.cpp

@@ -16,6 +16,9 @@ namespace ConfigLoaders
 {
 bool IsSettingSaveable(const Config::ConfigLocation& config_location)
 {
+  if (config_location.system == Config::System::DualShockUDPClient)
+    return true;
+
   if (config_location.system == Config::System::Logger)
     return true;
 

Source/Core/Core/Core.vcxproj

@@ -629,4 +629,4 @@
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">
   </ImportGroup>
-</Project>
+</Project>

Source/Core/Core/Core.vcxproj.filters

@@ -1661,8 +1661,11 @@
     <ClInclude Include="HW\AddressSpace.h">
       <Filter>HW %28Flipper/Hollywood%29</Filter>
     </ClInclude>
+    <ClInclude Include="HW\WiimoteEmu\Constants.h">
+      <Filter>HW %28Flipper/Hollywood%29\Wiimote\Emu</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <Text Include="CMakeLists.txt" />
   </ItemGroup>
-</Project>
+</Project>

Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp

@@ -13,6 +13,9 @@
 #include "InputCommon/ControllerEmu/ControlGroup/Buttons.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Cursor.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Force.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUAccelerometer.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUCursor.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Tilt.h"
 
 namespace
@@ -268,6 +271,90 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
   }
 }
 
+static Common::Vec3 NormalizeAngle(Common::Vec3 angle)
+{
+  // TODO: There must be a more elegant way to do this
+  angle.x = fmod(angle.x, float(MathUtil::TAU));
+  angle.y = fmod(angle.y, float(MathUtil::TAU));
+  angle.z = fmod(angle.z, float(MathUtil::TAU));
+  angle.x += angle.x < 0 ? float(MathUtil::TAU) : 0;
+  angle.y += angle.y < 0 ? float(MathUtil::TAU) : 0;
+  angle.z += angle.z < 0 ? float(MathUtil::TAU) : 0;
+  return angle;
+}
+
+static Common::Vec3 ComplementaryFilter(const Common::Vec3& angle,
+                                        const Common::Vec3& accelerometer,
+                                        const Common::Vec3& gyroscope, float time_elapsed)
+{
+  Common::Vec3 gyroangle = angle + gyroscope * time_elapsed;
+  gyroangle = NormalizeAngle(gyroangle);
+
+  // Calculate accelerometer tilt angles
+  Common::Vec3 accangle = gyroangle;
+  if ((accelerometer.x != 0 && accelerometer.y != 0) || accelerometer.z != 0)
+  {
+    float accpitch = -atan2(accelerometer.y, -accelerometer.z) + float(MathUtil::PI);
+    float accroll = atan2(accelerometer.x, -accelerometer.z) + float(MathUtil::PI);
+    accangle = {accpitch, accroll, gyroangle.z};
+  }
+
+  // Massage accelerometer and gyroscope angle values so that averaging them works when they are on
+  // opposite sides of TAU / zero (which both represent the same angle)
+  // TODO: There must be a more elegant way to do this
+  constexpr float DEG360 = float(MathUtil::TAU);
+  constexpr float DEG270 = DEG360 * 0.75f;
+  constexpr float DEG90 = DEG360 * 0.25f;
+  if (accangle.x < DEG90 && gyroangle.x > DEG270)
+    accangle.x += DEG360;
+  else if (gyroangle.x < DEG90 && accangle.x > DEG270)
+    gyroangle.x += DEG360;
+  if (accangle.y < DEG90 && gyroangle.y > DEG270)
+    accangle.y += DEG360;
+  else if (gyroangle.y < DEG90 && accangle.y > DEG270)
+    gyroangle.y += DEG360;
+
+  // Combine accelerometer and gyroscope angles
+  return NormalizeAngle((gyroangle * 0.98f) + (accangle * 0.02f));
+}
+
+void EmulateIMUCursor(std::optional<RotationalState>* state, ControllerEmu::IMUCursor* imu_ir_group,
+                      ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
+                      ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed)
+{
+  // Avoid having to double dereference
+  auto& st = *state;
+
+  auto accel = imu_accelerometer_group->GetState();
+  auto ang_vel = imu_gyroscope_group->GetState();
+
+  // The IMU Cursor requires both an accelerometer and a gyroscope to function correctly.
+  if (!(accel.has_value() && ang_vel.has_value()))
+  {
+    st = std::nullopt;
+    return;
+  }
+
+  if (!st.has_value())
+    st = RotationalState{};
+
+  st->angle = ComplementaryFilter(st->angle, accel.value(), ang_vel.value(), time_elapsed);
+
+  // Reset camera yaw angle
+  constexpr ControlState BUTTON_THRESHOLD = 0.5;
+  if (imu_ir_group->controls[0]->control_ref->State() > BUTTON_THRESHOLD)
+    st->angle.z = 0;
+
+  // Limit camera yaw angle
+  float totalyaw = float(imu_ir_group->GetTotalYaw());
+  float yawmax = totalyaw / 2;
+  float yawmin = float(MathUtil::TAU) - totalyaw / 2;
+  if (st->angle.z > yawmax && st->angle.z <= float(MathUtil::PI))
+    st->angle.z = yawmax;
+  if (st->angle.z < yawmin && st->angle.z > float(MathUtil::PI))
+    st->angle.z = yawmin;
+}
+
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,
                               const Common::Vec3& max_jerk, float time_elapsed)
 {

Source/Core/Core/HW/WiimoteEmu/Dynamics.h

@@ -11,6 +11,9 @@
 #include "InputCommon/ControllerEmu/ControlGroup/Buttons.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Cursor.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Force.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUAccelerometer.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUCursor.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Tilt.h"
 
 namespace WiimoteEmu
@@ -53,6 +56,9 @@ void EmulateShake(PositionalState* state, ControllerEmu::Shake* shake_group, flo
 void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* tilt_group, float time_elapsed);
 void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed);
 void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed);
+void EmulateIMUCursor(std::optional<RotationalState>* state, ControllerEmu::IMUCursor* imu_ir_group,
+                      ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
+                      ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed);
 
 // 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,

Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp

@@ -41,6 +41,9 @@
 #include "InputCommon/ControllerEmu/ControlGroup/ControlGroup.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Cursor.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Force.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUAccelerometer.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUCursor.h"
+#include "InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.h"
 #include "InputCommon/ControllerEmu/ControlGroup/ModifySettingsButton.h"
 #include "InputCommon/ControllerEmu/ControlGroup/Tilt.h"
 
@@ -150,6 +153,7 @@ void Wiimote::Reset()
   m_tilt_state = {};
   m_cursor_state = {};
   m_shake_state = {};
+  m_imu_cursor_state = {};
 }
 
 Wiimote::Wiimote(const unsigned int index) : m_index(index)
@@ -169,6 +173,11 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index)
   groups.emplace_back(m_swing = new ControllerEmu::Force(_trans("Swing")));
   groups.emplace_back(m_tilt = new ControllerEmu::Tilt(_trans("Tilt")));
   groups.emplace_back(m_shake = new ControllerEmu::Shake(_trans("Shake")));
+  groups.emplace_back(m_imu_accelerometer = new ControllerEmu::IMUAccelerometer(
+                          "IMUAccelerometer", _trans("Accelerometer")));
+  groups.emplace_back(m_imu_gyroscope =
+                          new ControllerEmu::IMUGyroscope("IMUGyroscope", _trans("Gyroscope")));
+  groups.emplace_back(m_imu_ir = new ControllerEmu::IMUCursor("IMUIR", _trans("Point")));
 
   // Extension
   groups.emplace_back(m_attachments = new ControllerEmu::Attachments(_trans("Extension")));
@@ -263,6 +272,12 @@ ControllerEmu::ControlGroup* Wiimote::GetWiimoteGroup(WiimoteGroup group)
     return m_options;
   case WiimoteGroup::Hotkeys:
     return m_hotkeys;
+  case WiimoteGroup::IMUAccelerometer:
+    return m_imu_accelerometer;
+  case WiimoteGroup::IMUGyroscope:
+    return m_imu_gyroscope;
+  case WiimoteGroup::IMUPoint:
+    return m_imu_ir;
   default:
     assert(false);
     return nullptr;
@@ -447,7 +462,7 @@ void Wiimote::SendDataReport()
     {
       // Calibration values are 8-bit but we want 10-bit precision, so << 2.
       DataReportBuilder::AccelData accel =
-          ConvertAccelData(GetAcceleration(), ACCEL_ZERO_G << 2, ACCEL_ONE_G << 2);
+          ConvertAccelData(GetTotalAcceleration(), ACCEL_ZERO_G << 2, ACCEL_ONE_G << 2);
       rpt_builder.SetAccelData(accel);
     }
 
@@ -456,7 +471,7 @@ void Wiimote::SendDataReport()
     {
       // Note: Camera logic currently contains no changing state so we can just update it here.
       // If that changes this should be moved to Wiimote::Update();
-      m_camera_logic.Update(GetTransformation());
+      m_camera_logic.Update(GetTotalTransformation());
 
       // The real wiimote reads camera data from the i2c bus starting at offset 0x37:
       const u8 camera_data_offset =
@@ -477,7 +492,7 @@ void Wiimote::SendDataReport()
       if (m_is_motion_plus_attached)
       {
         // TODO: Make input preparation triggered by bus read.
-        m_motion_plus.PrepareInput(GetAngularVelocity());
+        m_motion_plus.PrepareInput(GetTotalAngularVelocity());
       }
 
       u8* ext_data = rpt_builder.GetExtDataPtr();
@@ -670,6 +685,20 @@ void Wiimote::LoadDefaults(const ControllerInterface& ciface)
   m_dpad->SetControlExpression(3, "Right");  // Right
 #endif
 
+  // Motion Source
+  m_imu_accelerometer->SetControlExpression(0, "Accel Left");
+  m_imu_accelerometer->SetControlExpression(1, "Accel Right");
+  m_imu_accelerometer->SetControlExpression(2, "Accel Forward");
+  m_imu_accelerometer->SetControlExpression(3, "Accel Backward");
+  m_imu_accelerometer->SetControlExpression(4, "Accel Up");
+  m_imu_accelerometer->SetControlExpression(5, "Accel Down");
+  m_imu_gyroscope->SetControlExpression(0, "Gyro Pitch Up");
+  m_imu_gyroscope->SetControlExpression(1, "Gyro Pitch Down");
+  m_imu_gyroscope->SetControlExpression(2, "Gyro Roll Left");
+  m_imu_gyroscope->SetControlExpression(3, "Gyro Roll Right");
+  m_imu_gyroscope->SetControlExpression(4, "Gyro Yaw Left");
+  m_imu_gyroscope->SetControlExpression(5, "Gyro Yaw Right");
+
   // Enable Nunchuk:
   constexpr ExtensionNumber DEFAULT_EXT = ExtensionNumber::NUNCHUK;
   m_attachments->SetSelectedAttachment(DEFAULT_EXT);
@@ -720,36 +749,36 @@ void Wiimote::StepDynamics()
   EmulateTilt(&m_tilt_state, m_tilt, 1.f / ::Wiimote::UPDATE_FREQ);
   EmulateCursor(&m_cursor_state, m_ir, 1.f / ::Wiimote::UPDATE_FREQ);
   EmulateShake(&m_shake_state, m_shake, 1.f / ::Wiimote::UPDATE_FREQ);
+  EmulateIMUCursor(&m_imu_cursor_state, m_imu_ir, m_imu_accelerometer, m_imu_gyroscope,
+                   1.f / ::Wiimote::UPDATE_FREQ);
 }
 
-Common::Vec3 Wiimote::GetAcceleration()
+Common::Vec3 Wiimote::GetAcceleration(Common::Vec3 extra_acceleration)
 {
-  Common::Vec3 accel =
-      GetOrientation() *
-      GetTransformation().Transform(
-          m_swing_state.acceleration + Common::Vec3(0, 0, float(GRAVITY_ACCELERATION)), 0);
+  Common::Vec3 accel = GetOrientation() * GetTransformation().Transform(
+                                              m_swing_state.acceleration + extra_acceleration, 0);
 
   // Our shake effects have never been affected by orientation. Should they be?
   accel += m_shake_state.acceleration;
 
   return accel;
 }
 
-Common::Vec3 Wiimote::GetAngularVelocity()
+Common::Vec3 Wiimote::GetAngularVelocity(Common::Vec3 extra_angular_velocity)
 {
   return GetOrientation() * (m_tilt_state.angular_velocity + m_swing_state.angular_velocity +
-                             m_cursor_state.angular_velocity);
+                             m_cursor_state.angular_velocity + extra_angular_velocity);
 }
 
-Common::Matrix44 Wiimote::GetTransformation() const
+Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 extra_rotation) const
 {
   // Includes positional and rotational effects of:
   // Cursor, Swing, Tilt, Shake
 
   // TODO: Think about and clean up matrix order + make nunchuk match.
   return Common::Matrix44::Translate(-m_shake_state.position) *
          Common::Matrix44::FromMatrix33(GetRotationalMatrix(
-             -m_tilt_state.angle - m_swing_state.angle - m_cursor_state.angle)) *
+             -m_tilt_state.angle - m_swing_state.angle - m_cursor_state.angle - extra_rotation)) *
          Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
 }
 
@@ -759,4 +788,31 @@ Common::Matrix33 Wiimote::GetOrientation() const
          Common::Matrix33::RotateX(float(MathUtil::TAU / 4 * IsUpright()));
 }
 
+Common::Vec3 Wiimote::GetTotalAcceleration()
+{
+  auto accel = m_imu_accelerometer->GetState();
+  if (accel.has_value())
+    return GetAcceleration(accel.value());
+  else
+    return GetAcceleration();
+}
+
+Common::Vec3 Wiimote::GetTotalAngularVelocity()
+{
+  auto ang_vel = m_imu_gyroscope->GetState();
+  if (ang_vel.has_value())
+    return GetAngularVelocity(ang_vel.value());
+  else
+    return GetAngularVelocity();
+}
+
+Common::Matrix44 Wiimote::GetTotalTransformation() const
+{
+  auto state = m_imu_cursor_state;
+  if (state.has_value())
+    return GetTransformation(state->angle);
+  else
+    return GetTransformation();
+}
+
 }  // namespace WiimoteEmu