Merge pull request #8454 from jordan-woyak/motion-input-indicators

DolphinQt: Add accelerometer/gyroscope mapping indicators.

Source/Core/Common/Matrix.cpp

@@ -84,6 +84,23 @@ Matrix33 Matrix33::RotateZ(float rad)
   return mtx;
 }
 
+Matrix33 Matrix33::Rotate(float rad, const Vec3& axis)
+{
+  const float s = std::sin(rad);
+  const float c = std::cos(rad);
+  Matrix33 mtx;
+  mtx.data[0] = axis.x * axis.x * (1 - c) + c;
+  mtx.data[1] = axis.x * axis.y * (1 - c) - axis.z * s;
+  mtx.data[2] = axis.x * axis.z * (1 - c) + axis.y * s;
+  mtx.data[3] = axis.y * axis.x * (1 - c) + axis.z * s;
+  mtx.data[4] = axis.y * axis.y * (1 - c) + c;
+  mtx.data[5] = axis.y * axis.z * (1 - c) - axis.x * s;
+  mtx.data[6] = axis.z * axis.x * (1 - c) - axis.y * s;
+  mtx.data[7] = axis.z * axis.y * (1 - c) + axis.x * s;
+  mtx.data[8] = axis.z * axis.z * (1 - c) + c;
+  return mtx;
+}
+
 Matrix33 Matrix33::Scale(const Vec3& vec)
 {
   Matrix33 mtx = {};

Source/Core/Common/Matrix.h

@@ -20,6 +20,10 @@ union TVec3
   TVec3() = default;
   TVec3(T _x, T _y, T _z) : data{_x, _y, _z} {}
 
+  TVec3 Cross(const TVec3& rhs) const
+  {
+    return {(y * rhs.z) - (rhs.y * z), (z * rhs.x) - (rhs.z * x), (x * rhs.y) - (rhs.x * y)};
+  }
   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()); }
@@ -275,6 +279,8 @@ public:
   static Matrix33 RotateY(float rad);
   static Matrix33 RotateZ(float rad);
 
+  static Matrix33 Rotate(float rad, const Vec3& axis);
+
   static Matrix33 Scale(const Vec3& vec);
 
   // set result = a x b

Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp

@@ -8,6 +8,8 @@
 #include <cmath>
 #include <numeric>
 
+#include <fmt/format.h>
+
 #include <QAction>
 #include <QDateTime>
 #include <QPainter>
@@ -118,6 +120,10 @@ double MappingIndicator::GetScale() const
 
 namespace
 {
+constexpr float SPHERE_SIZE = 0.7f;
+constexpr float SPHERE_INDICATOR_DIST = 0.85f;
+constexpr int SPHERE_POINT_COUNT = 200;
+
 // Constructs a polygon by querying a radius at varying angles:
 template <typename F>
 QPolygonF GetPolygonFromRadiusGetter(F&& radius_getter, double scale,
@@ -184,6 +190,22 @@ bool IsPointOutsideCalibration(Common::DVec2 point, ControllerEmu::ReshapableInp
   return current_radius > input_radius * ALLOWED_ERROR;
 }
 
+template <typename F>
+void GenerateFibonacciSphere(int point_count, F&& callback)
+{
+  const float golden_angle = MathUtil::PI * (3.f - std::sqrt(5.f));
+
+  for (int i = 0; i != point_count; ++i)
+  {
+    const float z = (1.f / point_count - 1.f) + (2.f / point_count) * i;
+    const float r = std::sqrt(1.f - z * z);
+    const float x = std::cos(golden_angle * i) * r;
+    const float y = std::sin(golden_angle * i) * r;
+
+    callback(Common::Vec3{x, y, z});
+  }
+}
+
 }  // namespace
 
 void MappingIndicator::DrawCursor(ControllerEmu::Cursor& cursor)
@@ -681,6 +703,199 @@ void ShakeMappingIndicator::DrawShake()
   }
 }
 
+AccelerometerMappingIndicator::AccelerometerMappingIndicator(ControllerEmu::IMUAccelerometer* group)
+    : MappingIndicator(group), m_accel_group(*group)
+{
+}
+
+void AccelerometerMappingIndicator::paintEvent(QPaintEvent*)
+{
+  const auto accel_state = m_accel_group.GetState();
+  const auto state = accel_state.value_or(Common::Vec3{});
+
+  // Bounding box size:
+  const double scale = GetScale();
+
+  QPainter p(this);
+  p.translate(width() / 2, height() / 2);
+
+  // Bounding box.
+  p.setBrush(GetBBoxBrush());
+  p.setPen(GetBBoxPen());
+  p.drawRect(-scale - 1, -scale - 1, scale * 2 + 1, scale * 2 + 1);
+
+  // UI y-axis is opposite that of acceleration Z.
+  p.scale(1.0, -1.0);
+
+  // Enable AA after drawing bounding box.
+  p.setRenderHint(QPainter::Antialiasing, true);
+  p.setRenderHint(QPainter::SmoothPixmapTransform, true);
+
+  const auto angle = std::acos(state.Normalized().Dot({0, 0, 1}));
+  const auto axis = state.Normalized().Cross({0, 0, 1}).Normalized();
+
+  // Odd checks to handle case of 0g (draw no sphere) and perfect up/down orientation.
+  const auto rotation = (!state.LengthSquared() || axis.LengthSquared() < 2) ?
+                            Common::Matrix33::Rotate(angle, axis) :
+                            Common::Matrix33::Identity();
+
+  // Draw sphere.
+  p.setPen(Qt::NoPen);
+  p.setBrush(GetRawInputColor());
+
+  GenerateFibonacciSphere(SPHERE_POINT_COUNT, [&](const Common::Vec3& point) {
+    const auto pt = rotation * point;
+
+    if (pt.y > 0)
+      p.drawEllipse(QPointF(pt.x, pt.z) * scale * SPHERE_SIZE, 0.5f, 0.5f);
+  });
+
+  // Sphere outline.
+  p.setPen(GetRawInputColor());
+  p.setBrush(Qt::NoBrush);
+  p.drawEllipse(QPointF{}, scale * SPHERE_SIZE, scale * SPHERE_SIZE);
+
+  // Red dot upright target.
+  p.setPen(QPen(GetAdjustedInputColor(), INPUT_DOT_RADIUS / 2));
+  p.drawEllipse(QPointF{0, SPHERE_INDICATOR_DIST} * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
+
+  // Red dot.
+  const auto point = rotation * Common::Vec3{0, 0, SPHERE_INDICATOR_DIST};
+  if (point.y > 0 || Common::Vec2(point.x, point.z).Length() > SPHERE_SIZE)
+  {
+    p.setPen(Qt::NoPen);
+    p.setBrush(GetAdjustedInputColor());
+    p.drawEllipse(QPointF(point.x, point.z) * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
+  }
+
+  // Blue dot target.
+  p.setPen(QPen(Qt::blue, INPUT_DOT_RADIUS / 2));
+  p.setBrush(Qt::NoBrush);
+  p.drawEllipse(QPointF{0, -SPHERE_INDICATOR_DIST} * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
+
+  // Blue dot.
+  const auto point2 = -point;
+  if (point2.y > 0 || Common::Vec2(point2.x, point2.z).Length() > SPHERE_SIZE)
+  {
+    p.setPen(Qt::NoPen);
+    p.setBrush(Qt::blue);
+    p.drawEllipse(QPointF(point2.x, point2.z) * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
+  }
+
+  // Only draw g-force text if acceleration data is present.
+  if (!accel_state.has_value())
+    return;
+
+  // G-force text:
+  p.setPen(GetTextColor());
+  p.scale(1.0, -1.0);
+  p.drawText(QRectF(-2, 0, scale, scale), Qt::AlignBottom | Qt::AlignRight,
+             QString::fromStdString(
+                 // i18n: "g" is the symbol for "gravitational force equivalent" (g-force).
+                 fmt::format("{:.2f} g", state.Length() / WiimoteEmu::GRAVITY_ACCELERATION)));
+}
+
+GyroMappingIndicator::GyroMappingIndicator(ControllerEmu::IMUGyroscope* group)
+    : MappingIndicator(group), m_gyro_group(*group), m_state(Common::Matrix33::Identity())
+{
+}
+
+void GyroMappingIndicator::paintEvent(QPaintEvent*)
+{
+  const auto gyro_state = m_gyro_group.GetState();
+  const auto angular_velocity = gyro_state.value_or(Common::Vec3{});
+
+  m_state *= Common::Matrix33::RotateX(angular_velocity.x / -INDICATOR_UPDATE_FREQ) *
+             Common::Matrix33::RotateY(angular_velocity.y / INDICATOR_UPDATE_FREQ) *
+             Common::Matrix33::RotateZ(angular_velocity.z / -INDICATOR_UPDATE_FREQ);
+
+  // Reset orientation when stable for a bit:
+  constexpr u32 STABLE_RESET_STEPS = INDICATOR_UPDATE_FREQ;
+  // This works well with my DS4 but a potentially noisy device might not behave.
+  const bool is_stable = angular_velocity.Length() < MathUtil::TAU / 30;
+
+  if (!is_stable)
+    m_stable_steps = 0;
+  else if (m_stable_steps != STABLE_RESET_STEPS)
+    ++m_stable_steps;
+
+  if (STABLE_RESET_STEPS == m_stable_steps)
+    m_state = Common::Matrix33::Identity();
+
+  // Use an empty rotation matrix if gyroscope data is not present.
+  const auto rotation = (gyro_state.has_value() ? m_state : Common::Matrix33{});
+
+  // Bounding box size:
+  const double scale = GetScale();
+
+  QPainter p(this);
+  p.translate(width() / 2, height() / 2);
+
+  // Bounding box.
+  p.setBrush(GetBBoxBrush());
+  p.setPen(GetBBoxPen());
+  p.drawRect(-scale - 1, -scale - 1, scale * 2 + 1, scale * 2 + 1);
+
+  // Enable AA after drawing bounding box.
+  p.setRenderHint(QPainter::Antialiasing, true);
+  p.setRenderHint(QPainter::SmoothPixmapTransform, true);
+
+  p.setPen(Qt::NoPen);
+  p.setBrush(GetRawInputColor());
+
+  GenerateFibonacciSphere(SPHERE_POINT_COUNT, [&, this](const Common::Vec3& point) {
+    const auto pt = rotation * point;
+
+    if (pt.y > 0)
+      p.drawEllipse(QPointF(pt.x, pt.z) * scale * SPHERE_SIZE, 0.5f, 0.5f);
+  });
+
+  // Sphere outline.
+  p.setPen(GetRawInputColor());
+  p.setBrush(Qt::NoBrush);
+  p.drawEllipse(QPointF{}, scale * SPHERE_SIZE, scale * SPHERE_SIZE);
+
+  // Red dot upright target.
+  p.setPen(QPen(GetAdjustedInputColor(), INPUT_DOT_RADIUS / 2));
+  p.drawEllipse(QPointF{0, -SPHERE_INDICATOR_DIST} * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
+
+  // Red dot.
+  const auto point = rotation * Common::Vec3{0, 0, -SPHERE_INDICATOR_DIST};
+  if (point.y > 0 || Common::Vec2(point.x, point.z).Length() > SPHERE_SIZE)
+  {
+    p.setPen(Qt::NoPen);
+    p.setBrush(GetAdjustedInputColor());
+    p.drawEllipse(QPointF(point.x, point.z) * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
+  }
+
+  // Blue dot target.
+  p.setPen(QPen(Qt::blue, INPUT_DOT_RADIUS / 2));
+  p.setBrush(Qt::NoBrush);
+  p.drawEllipse(QPointF{}, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
+
+  // Blue dot.
+  const auto point2 = rotation * Common::Vec3{0, SPHERE_INDICATOR_DIST, 0};
+  if (point2.y > 0 || Common::Vec2(point2.x, point2.z).Length() > SPHERE_SIZE)
+  {
+    p.setPen(Qt::NoPen);
+    p.setBrush(Qt::blue);
+    p.drawEllipse(QPointF(point2.x, point2.z) * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
+  }
+
+  // Only draw text if data is present.
+  if (!gyro_state.has_value())
+    return;
+
+  // Angle of red dot from starting position.
+  const auto angle = std::acos(point.Normalized().Dot({0, 0, -1}));
+
+  // Angle text:
+  p.setPen(GetTextColor());
+  p.drawText(QRectF(-2, 0, scale, scale), Qt::AlignBottom | Qt::AlignRight,
+             // i18n: "°" is the symbol for degrees (angular measurement).
+             QString::fromStdString(fmt::format("{:.2f} °", angle / MathUtil::TAU * 360)));
+}
+
 void MappingIndicator::DrawCalibration(QPainter& p, Common::DVec2 point)
 {
   // Bounding box size:

Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h

@@ -82,6 +82,28 @@ private:
   ControllerEmu::Shake& m_shake_group;
 };
 
+class AccelerometerMappingIndicator : public MappingIndicator
+{
+public:
+  explicit AccelerometerMappingIndicator(ControllerEmu::IMUAccelerometer* group);
+  void paintEvent(QPaintEvent*) override;
+
+private:
+  ControllerEmu::IMUAccelerometer& m_accel_group;
+};
+
+class GyroMappingIndicator : public MappingIndicator
+{
+public:
+  explicit GyroMappingIndicator(ControllerEmu::IMUGyroscope* group);
+  void paintEvent(QPaintEvent*) override;
+
+private:
+  ControllerEmu::IMUGyroscope& m_gyro_group;
+  Common::Matrix33 m_state;
+  u32 m_stable_steps = 0;
+};
+
 class CalibrationWidget : public QToolButton
 {
 public:

Source/Core/DolphinQt/Config/Mapping/MappingWidget.cpp

@@ -67,6 +67,8 @@ QGroupBox* MappingWidget::CreateGroupBox(const QString& name, ControllerEmu::Con
                               group->type == ControllerEmu::GroupType::Tilt ||
                               group->type == ControllerEmu::GroupType::MixedTriggers ||
                               group->type == ControllerEmu::GroupType::Force ||
+                              group->type == ControllerEmu::GroupType::IMUAccelerometer ||
+                              group->type == ControllerEmu::GroupType::IMUGyroscope ||
                               group->type == ControllerEmu::GroupType::Shake;
 
   const bool need_calibration = group->type == ControllerEmu::GroupType::Cursor ||
@@ -84,6 +86,15 @@ QGroupBox* MappingWidget::CreateGroupBox(const QString& name, ControllerEmu::Con
       indicator = new ShakeMappingIndicator(static_cast<ControllerEmu::Shake*>(group));
       break;
 
+    case ControllerEmu::GroupType::IMUAccelerometer:
+      indicator =
+          new AccelerometerMappingIndicator(static_cast<ControllerEmu::IMUAccelerometer*>(group));
+      break;
+
+    case ControllerEmu::GroupType::IMUGyroscope:
+      indicator = new GyroMappingIndicator(static_cast<ControllerEmu::IMUGyroscope*>(group));
+      break;
+
     default:
       indicator = new MappingIndicator(group);
       break;