Gyro: ROLLGOAL!

src/dusk/gyro.cpp

@@ -0,0 +1,123 @@
+#include "dusk/gyro.h"
+#include "d/actor/d_a_alink.h"
+
+namespace dusk::gyro {
+namespace {
+// TODO: Make deadband and smoothing configurable
+constexpr float kDeadbandRadS           = 0.04f;
+constexpr float kSmoothAlpha            = 0.35f;
+constexpr s32   kRollgoalTableMaxOffset = 12000;
+
+bool  s_sensor_enabled        = false;
+float s_smooth_gx             = 0.0f;
+float s_smooth_gy             = 0.0f;
+float s_smooth_gz             = 0.0f;
+float s_yaw_rad               = 0.0f;
+float s_yaw_rad_pending       = 0.0f;
+float s_pitch_rad             = 0.0f;
+float s_pitch_rad_pending     = 0.0f;
+float s_roll_rad              = 0.0f;
+s32   s_rollgoal_ax           = 0;
+s32   s_rollgoal_az           = 0;
+
+void reset_filter_state() {
+    s_smooth_gx = s_smooth_gy = s_smooth_gz = 0.0f;
+    s_yaw_rad_pending = s_pitch_rad_pending = s_roll_rad = 0.0f;
+    s_rollgoal_ax = s_rollgoal_az = 0;
+}
+
+float apply_deadband(float v) {
+    if (v > -kDeadbandRadS && v < kDeadbandRadS) {
+        return 0.0f;
+    }
+    return v;
+}
+}  // namespace
+
+bool s_sensor_keep_alive = false;
+
+bool get_sensor_keep_alive() { return s_sensor_keep_alive; }
+void set_sensor_keep_alive(bool value) { s_sensor_keep_alive = value; }
+
+bool queryGyroAimItemContext() {
+    if (!static_cast<bool>(dusk::getSettings().game.enableGyroAim)) {
+        return false;
+    }
+
+    daAlink_c* link = daAlink_getAlinkActorClass();
+    if (link == nullptr) {
+        return false;
+    }
+
+    return link->checkGyroAimItemContext() && dComIfGp_checkCameraAttentionStatus(link->field_0x317c, 0x10);
+}
+
+void read(float dt) {
+    if (!s_sensor_keep_alive && !(dusk::getSettings().game.enableGyroAim && queryGyroAimItemContext())) {
+        if (s_sensor_enabled) {
+            PADSetSensorEnabled(PAD_CHAN0, PAD_SENSOR_GYRO, FALSE);
+            s_sensor_enabled = false;
+        }
+        reset_filter_state();
+        return;
+    }
+
+    if (!s_sensor_enabled) {
+        if (!PADHasSensor(PAD_CHAN0, PAD_SENSOR_GYRO)) {
+            return;
+        }
+        if (!PADSetSensorEnabled(PAD_CHAN0, PAD_SENSOR_GYRO, TRUE)) {
+            return;
+        }
+        s_sensor_enabled = true;
+    }
+
+    f32 gyro[3];
+    if (!PADGetSensorData(PAD_CHAN0, PAD_SENSOR_GYRO, gyro, 3)) {
+        return;
+    }
+
+    s_smooth_gx += kSmoothAlpha * (gyro[0] - s_smooth_gx);
+    s_smooth_gy += kSmoothAlpha * (gyro[1] - s_smooth_gy);
+    s_smooth_gz += kSmoothAlpha * (gyro[2] - s_smooth_gz);
+
+    s_pitch_rad = apply_deadband(s_smooth_gx) * dt * dusk::getSettings().game.gyroAimSensitivityX;
+    s_yaw_rad   = apply_deadband(s_smooth_gy) * dt * dusk::getSettings().game.gyroAimSensitivityY;
+    s_roll_rad  = apply_deadband(s_smooth_gz) * dt * dusk::getSettings().game.gyroAimSensitivityX; // GYRO NOTE: Exposing Z sensitivity seems unusual, so I'm just using X
+
+    s_pitch_rad_pending += s_pitch_rad;
+    s_yaw_rad_pending += s_yaw_rad;
+}
+
+void consumeAimDeltas(float& out_yaw_rad, float& out_pitch_rad) {
+    out_yaw_rad = s_yaw_rad_pending;
+    out_pitch_rad = s_pitch_rad_pending;
+    s_yaw_rad_pending = s_pitch_rad_pending = 0.0f;
+}
+
+void rollgoalTick(bool play_active, s16 camera_yaw) {
+    if (!play_active) {
+        reset_filter_state();
+        return;
+    }
+
+    const float pitch_rad = s_pitch_rad * dusk::getSettings().game.gyroRollgoalSensitivity;
+    const float roll_rad = s_roll_rad * dusk::getSettings().game.gyroRollgoalSensitivity;
+
+    s_rollgoal_az += cM_rad2s(roll_rad);
+    cXyz in(roll_rad, 0.0f, pitch_rad);
+    cXyz out;
+    cMtx_YrotS(*calc_mtx, static_cast<s16>(-camera_yaw));
+    MtxPosition(&in, &out);
+
+    s_rollgoal_ax += cM_rad2s(out.z);
+    s_rollgoal_az += cM_rad2s(out.x);
+    s_rollgoal_ax = std::clamp(s_rollgoal_ax, -kRollgoalTableMaxOffset, kRollgoalTableMaxOffset);
+    s_rollgoal_az = std::clamp(s_rollgoal_az, -kRollgoalTableMaxOffset, kRollgoalTableMaxOffset);
+}
+
+void rollgoalTableOffset(s16& out_add_x, s16& out_add_z) {
+    out_add_x = static_cast<s16>(s_rollgoal_ax);
+    out_add_z = static_cast<s16>(s_rollgoal_az);
+}
+}  // namespace dusk::gyro