Skip to content
Permalink
Browse files

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

DolphinQt: Add accelerometer/gyroscope mapping indicators.
  • Loading branch information
delroth committed Nov 10, 2019
2 parents 499f065 + 8ef25dd commit 066012b80d7a149779b4e910dcda3be8ec97f8e3
@@ -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 = {};
@@ -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 @@ class Matrix33
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
@@ -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:
@@ -82,6 +82,28 @@ class ShakeMappingIndicator : public MappingIndicator
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:
@@ -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;

0 comments on commit 066012b

Please sign in to comment.
You can’t perform that action at this time.