diff --git a/gpu/vk/vk_api.h b/gpu/vk/vk_api.h index 5201459cd..5c7503f85 100644 --- a/gpu/vk/vk_api.h +++ b/gpu/vk/vk_api.h @@ -17,11 +17,11 @@ extern "C" { #define MKWVK_API_VERSION 10 enum { HOS_OPT_CHEAT_HEARTS = 32, HOS_OPT_CHEAT_FLIGHT, HOS_OPT_CHEAT_ACTION, HOS_OPT_CHEAT_CAMERA }; -/* (hookshot) the Controls tab's motion rows (host/pad_sdl.c reads them, vk_replay.cpp's option answers them): Motion - controls on or off, the horizontal's and the vertical's sensitivity in percent, and each turned around */ -enum { HOS_OPT_GYRO = 18, HOS_OPT_GYRO_SENS_X, HOS_OPT_GYRO_INV_X, HOS_OPT_GYRO_INV_Y, HOS_OPT_GYRO_SENS_Y }; +/* (hookshot) the Motion page's rows (host/pad_sdl.c reads them, vk_replay.cpp's option answers them): Motion controls on + or off, the horizontal's and the vertical's sensitivity in percent, each turned around, and the deadzone (0 to 2) */ +enum { HOS_OPT_GYRO = 18, HOS_OPT_GYRO_SENS_X, HOS_OPT_GYRO_INV_X, HOS_OPT_GYRO_INV_Y, HOS_OPT_GYRO_SENS_Y, HOS_OPT_GYRO_DEADZONE }; /* (hookshot) NVIDIA DLSS's camera jitter (host/rx_backend.c, gpu/vk/vk_dlss.cpp): the scene's width and height while DLSS - draws, 0 when there is to be no jitter, and the jitter sequence's length. 24 on: 18 to 22 are the motion rows' */ + draws, 0 when there is to be no jitter, and the jitter sequence's length. 24 on: 18 to 23 are the motion rows' */ enum { HOS_OPT_DLSS_JITTER_W = 24, HOS_OPT_DLSS_JITTER_H, HOS_OPT_DLSS_JITTER_PHASES }; /* (hookshot) the controls' names (host/nn_hid.c, host/pad_sdl.c, the pad_ and key_ lines of hookshot_options.ini): @@ -156,8 +156,8 @@ typedef struct MkwVkRenderer { void (*nvn_event_signal)(uint64_t gpu, uint32_t value); int (*option)(int id); /* (hookshot, the Insert menu) 0: game time follows the clock, 1: the menu is open, 2: dynamic resolution, 3: its fixed scale in percent, 4: the controllers count behind other windows, 5: text speed (0 normal to 3 instant), 6: native mouse look (host/mouse_native.c), 7: the mouse's sensitivity in percent, 8: its up and down swapped (host/nn_hid.c), - 18: motion controls from a pad's gyroscope (host/pad_sdl.c), 19 and 22: their horizontal - and vertical sensitivity in percent, 20 and 21: each turned around */ + 18 to 23: the Motion page (HOS_OPT_GYRO*: motion controls from a pad's gyroscope, host/pad_sdl.c; + the horizontal and vertical sensitivity in percent, each turned around, the deadzone) */ /* a submit with its queue */ void (*nvn_submit_q)(const uint64_t *handles, int n, uint64_t queue); /* (hookshot) a fence of one queue: the value its own last submission signals (nvnQueueFenceSync) */ diff --git a/gpu/vk/vk_menu.cpp b/gpu/vk/vk_menu.cpp index ce18bec6b..8b59630f0 100644 --- a/gpu/vk/vk_menu.cpp +++ b/gpu/vk/vk_menu.cpp @@ -49,7 +49,9 @@ enum MenuTab { TAB_GRAPHICS = 0, TAB_GAMEPLAY = 1, TAB_CONTROLS = 2, - TAB_COUNT = 3 + TAB_COUNT = 3, + TAB_MOTION = TAB_COUNT // (hookshot: not a tab of the strip, the Motion page that the Controls tab's Motion row + // opens: one past the strip's tabs, so a tab added before TAB_COUNT moves it along) }; struct Row { @@ -333,6 +335,12 @@ const char* hn_mic(int v, int variant) { return missing ? "Not plugged in: Default stands in until it is back." : "This microphone, by its name. A change applies at once."; } const char* tx_rumble(int v) { return v ? tx_percent(v) : "Off"; } +// (hookshot) the motion's deadzone: the console's three zero-drift modes, the rate under which the pad counts as still +const char* tx_deadzone(int v) { + static const char* t[] = {"Low", "Standard", "High"}; + return t[v < 0 ? 0 : v > 2 ? 2 : v]; +} + const char* tx_stick_dead_zone(int v) { static const char* t[] = {"Default (24%)", "5%", "10%", "15%", "20%", "30%"}; return (unsigned)v < 6 ? t[v] : t[0]; @@ -503,7 +511,7 @@ Row rows[] = { "What the ocarina hears when the game's microphone option is on (playing an instrument, or humming).", hn_mic, false, true}, - // Controls (15 rows) + // Controls (11 rows; the Motion row opens the 6 TAB_MOTION rows) {TAB_CONTROLS, "Controller in background", &opts.pad_background, 0, 1, 1, tx_onoff, "Whether the controller plays the game while another window has the focus.", hn_background}, {TAB_CONTROLS, "Mouse camera", &opts.mouse_native, 0, 1, 1, tx_mousecam, "How the captured mouse turns the camera.", @@ -522,20 +530,25 @@ Row rows[] = { "The game's own dead zone still follows.", nullptr, false, true}, {TAB_CONTROLS, "Rumble strength", &opts.rumble, 0, 150, 25, tx_rumble, "How strongly the controller rumbles, in percent of the game's rumble. Off turns it off.", nullptr, false, true}, - {TAB_CONTROLS, "Motion controls", &opts.gyro, 0, 1, 1, tx_gyro, - "Aiming and looking with the controller's gyroscope where the game uses motion, as on the Switch.", hn_gyro}, - {TAB_CONTROLS, "Motion sensitivity X", &opts.gyro_sens_x, 10, 600, 10, tx_percent, + {TAB_MOTION, "Motion controls", &opts.gyro, 0, 1, 1, tx_gyro, + "Aiming and looking with the controller's gyroscope where the game uses motion, as on the Switch.", hn_gyro, false, true}, + {TAB_MOTION, "Motion sensitivity X", &opts.gyro_sens_x, 10, 600, 10, tx_percent, "How far the aim moves left and right for a given turn of the controller. 100% is the console's own.", nullptr, false, true}, - {TAB_CONTROLS, "Motion sensitivity Y", &opts.gyro_sens_y, 10, 300, 10, tx_percent, + {TAB_MOTION, "Motion sensitivity Y", &opts.gyro_sens_y, 10, 300, 10, tx_percent, "How far the aim moves up and down for a given tilt of the controller. 100% is the console's own.", nullptr, false, true}, - {TAB_CONTROLS, "Invert motion X", &opts.gyro_inv_x, 0, 1, 1, tx_onoff, "Turns the motion aim's left and right around.", + {TAB_MOTION, "Invert motion X", &opts.gyro_inv_x, 0, 1, 1, tx_onoff, "Turns the motion aim's left and right around.", nullptr, false, true}, - {TAB_CONTROLS, "Invert motion Y", &opts.gyro_inv_y, 0, 1, 1, tx_onoff, "Turns the motion aim's up and down around.", + {TAB_MOTION, "Invert motion Y", &opts.gyro_inv_y, 0, 1, 1, tx_onoff, "Turns the motion aim's up and down around.", nullptr, false, true}, + {TAB_MOTION, "Motion deadzone", &opts.gyro_deadzone, 0, 2, 1, tx_deadzone, + "How slow a turn counts as none, the console's three settings: Standard is its own. High steadies a pad that jitters " + "when held still; Low keeps the smallest movements.", nullptr, false, true}, {TAB_CONTROLS, "Layout preset", &opts.pad_layout, 0, 3, 1, tx_pad_layout, "Which face button is which. Binding one yourself on the Controls page makes it Custom.", hn_pad_layout}, + {TAB_CONTROLS, "Motion", nullptr, 0, 0, 0, nullptr, // (hookshot: opens the Motion page, the TAB_MOTION rows) + "Motion controls: aiming with the controller's gyroscope, its sensitivity and its inverts."}, {TAB_CONTROLS, "Controls", nullptr, 0, 0, 0, nullptr, // (hookshot: opens its page) "Which gamepad button and which keys press each of the Switch's buttons."}, // (Restart, Quit and Cheats stay the table's last three rows: kRestartRow, kQuitRow and kCheatsRow count back from its end) @@ -639,7 +652,8 @@ constexpr int kRows = sizeof rows / sizeof rows[0]; constexpr int kMenuWidth = menu_hit::kWidth; // (at a scale of 1: 720 lines) int cur_tab = TAB_GRAPHICS; -int tab_selected[TAB_COUNT] = {0, 0, 0}; +int tab_selected[TAB_MOTION + 1] = {}; // (the strip's tabs and the Motion page's) +bool is_motion_row(const Row& r) { return r.tab == TAB_CONTROLS && !r.value && !strcmp(r.name, "Motion"); } // ---- the rows that take a restart: their values at the start (menu_init, the GPU's at menu_set_gpu_count) ---- namespace mr = menu_restart; @@ -1343,7 +1357,7 @@ void paint_menu(HDC dc, uint32_t w, uint32_t h, int s) { const int center_start = menu_hit::kTabSide * s; const int tab_slot_w = menu_hit::tab_slot_w(s, (int)w); for (int t = 0; t < TAB_COUNT; t++) { - const bool active = (t == cur_tab); + const bool active = t == (cur_tab == TAB_MOTION ? TAB_CONTROLS : cur_tab); // (the Motion page: under Controls) int slot_x = center_start + t * tab_slot_w; SIZE tsz{}; GetTextExtentPoint32A(dc, tab_titles[t], (int)strlen(tab_titles[t]), &tsz); @@ -1448,7 +1462,8 @@ void paint_menu(HDC dc, uint32_t w, uint32_t h, int s) { } SelectObject(dc, body); SetTextColor(dc, RGB(140, 150, 144)); - const char* hint = "Arrows change Q/E or LB/RB: tabs Insert, F1 or Home closes"; // (fits 448 px at 16 px) + const char* hint = cur_tab == TAB_MOTION ? "Arrows change Esc goes back Insert, F1 or Home closes" + : "Arrows change Q/E or LB/RB: tabs Insert, F1 or Home closes"; // (fits 448 px at 16 px) TextOutA(dc, 16 * s, help_top + 4 * s + help_px + 10 * s, hint, (int)strlen(hint)); if (cur_tab == TAB_GRAPHICS && restart_pending()) { // (the rows that take a restart, changed: measure_pending's room) SelectObject(dc, help); @@ -2193,10 +2208,12 @@ void menu_tick() { } } } + if (press[5] && cur_tab == TAB_MOTION) cur_tab = TAB_CONTROLS, press[5] = false, changed = true; // (back) if (press[5] && pending) pending = nullptr, changed = true; // (a Restart or Quit waiting for its second press: cancelled) else if (press[5] && restart_ask()) page = 5, ask_sel = 0, changed = true; // (the restart question first) else if (press[5]) open_now = false; - // (the tabs: LB and RB, Q and E, or Tab) + // (the tabs: LB and RB, Q and E, or Tab; from the Motion page, as from Controls) + if ((tab_left || tab_right) && cur_tab == TAB_MOTION) cur_tab = TAB_CONTROLS; if (tab_left) cur_tab = (cur_tab + TAB_COUNT - 1) % TAB_COUNT, changed = true; else if (tab_right) cur_tab = (cur_tab + 1) % TAB_COUNT, changed = true; const int tab_count = tab_row_count(cur_tab); @@ -2224,6 +2241,7 @@ void menu_tick() { } } } else if (press[7] && &r == &rows[kCheatsRow]) page = 2, changed = true; + else if (press[7] && is_motion_row(r)) cur_tab = TAB_MOTION, tab_selected[TAB_MOTION] = 0, press[7] = false, changed = true; else if (press[7] && !r.value && can_bind()) page = 1, csel = 0, ccol = 0, changed = true; if ((press[3] || press[4] || press[7]) && r.value) { // (Enter, or the bottom face button, as Right) if (press[7]) press[4] = true; diff --git a/gpu/vk/vk_menu.h b/gpu/vk/vk_menu.h index f3cba6805..0f7b7c927 100644 --- a/gpu/vk/vk_menu.h +++ b/gpu/vk/vk_menu.h @@ -40,6 +40,7 @@ struct Options { int gyro_sens_y = 100; // the motion aim's vertical sensitivity in percent int gyro_inv_x = 0; // the motion aim's horizontal turned around int gyro_inv_y = 0; // the motion aim's vertical turned around + int gyro_deadzone = 1; // the motion's deadzone: 0 tight, 1 standard (the console's default), 2 loose int robust = 0; // bounds-checked buffer access: 0 Auto (on), 1 Off, 2 On (gpu/vk/vk_main.cpp; RX_VK_ROBUST) int preset = 0; // quality preset: 0 Custom, 1 Minimum (4GB), 2 Low (6GB), 3 Balanced (8GB), 4 High (12GB), 5 Ultra (16GB+) int vram_mb = 0; // dynamic texture cache budget limit in MB diff --git a/gpu/vk/vk_replay.cpp b/gpu/vk/vk_replay.cpp index 082c8d769..41973ed4c 100644 --- a/gpu/vk/vk_replay.cpp +++ b/gpu/vk/vk_replay.cpp @@ -3799,7 +3799,7 @@ void fill_replay_table(MkwVkRenderer& t) { if (id == HOS_OPT_CHEAT_ACTION) return menu_take_cheat_action(); if (id == HOS_OPT_CHEAT_CAMERA) return menu_free_camera(); return id == 0 ? opts.realtime : id == 1 ? menu_is_open() : id == 2 ? resolution_auto() && !dlss_full_scale() : id == 3 ? (dlss_full_scale() ? 10000 : resolution_scale()) : id == 4 ? opts.pad_background : id == 5 ? opts.text_speed : id == 6 ? opts.mouse_native : - id == 7 ? opts.mouse_sens : id == 8 ? opts.mouse_invert : id == 9 ? opts.rumble : id == 16 ? opts.invert_cam_x : id == 17 ? opts.invert_cam_y : id == HOS_OPT_GYRO ? opts.gyro : id == HOS_OPT_GYRO_SENS_X ? opts.gyro_sens_x : id == HOS_OPT_GYRO_SENS_Y ? opts.gyro_sens_y : id == HOS_OPT_GYRO_INV_X ? opts.gyro_inv_x : id == HOS_OPT_GYRO_INV_Y ? opts.gyro_inv_y : + id == 7 ? opts.mouse_sens : id == 8 ? opts.mouse_invert : id == 9 ? opts.rumble : id == 16 ? opts.invert_cam_x : id == 17 ? opts.invert_cam_y : id == HOS_OPT_GYRO ? opts.gyro : id == HOS_OPT_GYRO_SENS_X ? opts.gyro_sens_x : id == HOS_OPT_GYRO_SENS_Y ? opts.gyro_sens_y : id == HOS_OPT_GYRO_INV_X ? opts.gyro_inv_x : id == HOS_OPT_GYRO_INV_Y ? opts.gyro_inv_y : id == HOS_OPT_GYRO_DEADZONE ? opts.gyro_deadzone : id == HOS_OPT_DLSS_JITTER_W || id == HOS_OPT_DLSS_JITTER_H ? (int)dlss_jitter_size(id - HOS_OPT_DLSS_JITTER_W) : id == HOS_OPT_DLSS_JITTER_PHASES ? (int)dlss_jitter_size(2) : 0; }; diff --git a/host/hos_motion.h b/host/hos_motion.h index 370bb2bd5..385a3b5e2 100644 --- a/host/hos_motion.h +++ b/host/hos_motion.h @@ -1,22 +1,27 @@ -/* (hookshot) A controller's motion sensors as OoT reads its six-axis state (host/pad_sdl.c fills it from SDL, - host/nn_hid.c hands it to the game's GetSixAxisSensorStates). Header-only, no SDL or Windows, so +/* (hookshot) A controller's motion sensors as the Switch reports its six-axis state (host/pad_sdl.c fills it from + SDL, host/nn_hid.c hands it to the game's GetSixAxisSensorStates). Header-only, no SDL or Windows, so tests/test_motion.py runs it as it is. - OoT does not read the six-axis state as a plain sensor. What it aims with was found by spluton (issue 112, "Gyro - aiming from SDL pads", tested on a DualSense) and corrected by Link-22 (the yaw rate's unit, tested on a GameSir T3 - Pro in Switch Pro mode, and the same feel as Eden on that pad): - - the horizontal aim is the yaw rate, in the angular velocity's y slot, in turns a second (SDL's rad/s over 2 pi); - - the vertical aim is the direction matrix, as a pure tilt about x (Rx(pitch)), the pitch rate integrated on its own; - - the game folds the angles, the pitch rate and the acceleration into an aim recenter that cancels the tilt unless - they are neutral: the angles and the pitch rate go out zero, the acceleration at rest (0, 0, -1). - The rates are the pad's own, measured about the world's axes: an up vector from a complementary filter (the gyro's - prediction pulled toward the measured gravity while it reads 0.65 to 1.35 G), the gyro projected onto right - (pitch), up (yaw) and forward (roll). That makes the way the pad is held irrelevant: flat or upright, a turn toward - the ceiling is up. The gyroscope's resting rate (its bias) is learned while the pad is still (both sensors steady, not - merely slow) and taken off, and the vertical aim is pulled gently toward the tilt gravity measures, so it cannot drift. + This is Eden's (and yuzu's) motion, the emulators OoT aims right with: SDL's readings turned into the Switch's frame + and units, and the state built as the console builds it, so the game gets what it gets on hardware and does its own + aiming (it recenters on the camera each time it aims; how the pad is held, in bed or at a desk, does not matter): + - acceleration in G and angular velocity in turns a second, in the Switch's frame (Eden's sdl_driver.cpp: + x = -X, y = Z, z = -Y for the accelerometer, x = X, y = -Z, z = Y for the gyroscope, SDL's +X right, +Y out of the + face buttons, +Z toward the player), so a pad lying face up reads (0, 0, -1) G; + - the rotation: the angular velocity summed (turns); + - the direction: the pad's orientation as a 3x3 matrix, from Mahony's filter (the gyroscope integrated as a + quaternion, pulled toward the measured gravity: kp 0.3, ki 0.005), Eden's motion_input.cpp line for line. + The gyroscope's resting rate (its bias) is learned while the pad is still on a table (both sensors steady) and + taken off, and rates under 0.007 turns a second count as none, as the console's standard zero-drift mode does (the + Motion deadzone row: Low and High are its tight 0.002 and loose 0.01). + Motion sensitivity X/Y and Invert motion X/Y scale the angular velocity before it is used: its part about the + world's up (the turn, X) and the rest (the tilt, Y). At 100% and not inverted the state is Eden's own. Earlier + versions sent a partial state (a yaw rate and a tilt matrix, the rest neutral, spluton's and Link-22's findings in + issue 112), and every way the pad could be held at the start needed its own case; the full state needs none. - Everything is in SDL's gamepad frame (+X right, +Y up out of the face buttons, +Z toward the player; m/s^2 and - rad/s). The defaults are the two testers': the yaw rate as is (turning left aims left), the pitch rate negated. */ + The filter, the frame and the orientation matrix follow Eden's src/hid_core/frontend/motion_input.cpp and + src/input_common/drivers/sdl_driver.cpp (Copyright 2020 yuzu Emulator Project, GPL-2.0-or-later; Copyright 2026 Eden + Emulator Project, GPL-3.0-or-later), as this project is GPL-3.0. */ #pragma once #include #include @@ -26,10 +31,10 @@ #define HOS_MOTION_TAU 6.28318531f typedef struct { - float accel[3]; /* G (at rest, always: the game folds it into its recenter) */ - float gyro[3]; /* turns a second: the yaw rate in y, x and z zero */ - float rotation[3]; /* turns: zero (the game folds them into its recenter) */ - float direction[9]; /* rows: Rx(pitch), the vertical aim */ + float accel[3]; /* G, the Switch's frame */ + float gyro[3]; /* turns a second */ + float rotation[3]; /* turns */ + float direction[9]; /* rows: the orientation */ } HosMotionState; /* one reading's state as GetSixAxisSensorStates hands it out: n counts the readings, dt_ns is the time since the one @@ -41,149 +46,217 @@ typedef struct { #define HOS_MOTION_HISTORY 32 typedef struct { - float bias[3]; /* rad/s, SDL's axes */ - float still_t; /* seconds the pad has been still (bias learning starts after HOS_MOTION_STILL_S) */ - float learned_t; /* seconds of stillness learned from so far (fast at first, then slowly) */ - float gmean[3], gvar; /* the gyro's short-window mean (rad/s) and variance (summed over its axes) */ - float amean, avar; /* the accelerometer's magnitude, the same way (G) */ - int stats_ready; - float last_accel[3], last_gyro[3]; /* (the last reading: a repeat of it is no new reading) */ - float pending_dt; /* (time from repeats, carried into the next new reading) */ - float up[3]; /* the world's up in the pad's frame (unit) */ - int up_ready; - float pitch; /* radians, the vertical aim's tilt */ - float tilt0; /* radians, the pad's tilt about its x axis when the filter started (the aim's level) */ - int tilt_ready; - float rates[3]; /* (for tests and logs) pitch, yaw, roll about the world's axes, rad/s */ + float quat[4]; /* x, y, z, w (Eden's order): the real orientation, Eden's filter on the real rates */ + float quat_out[4]; /* the orientation handed out: integrated from the rates as Motion sensitivity and Invert + motion scale them, with the real filter's drift correction (the same as quat at 100%) */ + float real_error[3], integral_error[3]; + float bias[3]; /* turns a second, the Switch's frame */ + float learned_t; /* seconds of stillness learned from (fast at first, then slowly) */ + float still_t; /* seconds still */ + float gmean[3], gvar, avec[3], avar, amean; /* the short window's means and variances (stillness) */ + int stats_ready, only_accelerometer, reset_counter; + float last_accel[3], last_gyro[3], pending_dt; + float threshold; /* turns a second, the deadzone (0: HOS_MOTION_THRESHOLD) */ + float up[3]; /* the world's up in the Switch's frame (from the accelerometer, smoothed: for the split of + the angular velocity into turn and tilt) */ HosMotionState out; } HosMotion; -/* stillness: the gyro's spread under HOS_MOTION_STILL_GYRO rad/s and the accelerometer's under HOS_MOTION_STILL_ACCEL - G, about 1 G, for HOS_MOTION_STILL_S. A hand never holds that steady (its tremor alone is several times more), so a - slow, steady turn is aim, not bias, however slow; and an offset of any size is learned on a table. */ -#define HOS_MOTION_STILL_GYRO 0.01f -#define HOS_MOTION_STILL_ACCEL 0.01f +#define HOS_MOTION_STILL_GYRO 0.015f /* rad/s, summed over three axes (about 0.009 on each) */ +#define HOS_MOTION_STILL_ACCEL 0.0173f /* G, the same way (0.01 on each) */ #define HOS_MOTION_STILL_S 0.5f -#define HOS_MOTION_WINDOW_S 0.2f /* (the short window's time constant) */ -#define HOS_MOTION_DT_MAX 0.1f /* (a longer gap, a hitch, counts as this long) */ -#define HOS_MOTION_TILT_S 1.0f /* (the vertical aim's pull toward the measured tilt) */ -#define HOS_MOTION_TILT_ROLL 0.25f /* (... only while |up[0]| is under this: about 15 degrees of roll) */ +#define HOS_MOTION_WINDOW_S 0.2f +#define HOS_MOTION_DT_MAX 0.1f /* (Eden ignores a longer gap) */ +#define HOS_MOTION_THRESHOLD 0.007f /* turns a second: less is none (the console's standard zero-drift mode) */ +#define HOS_MOTION_THRESHOLD_TIGHT 0.002f /* (its tight and loose modes: the Motion deadzone row's Low and High) */ +#define HOS_MOTION_THRESHOLD_LOOSE 0.01f -static inline void hos_motion_rest(HosMotionState *o, float pitch) { +static inline void hos_motion_rest(HosMotionState *o) { memset(o, 0, sizeof *o); o->accel[2] = -1; - const float c = cosf(pitch), s = sinf(pitch); - const float r[9] = {1, 0, 0, 0, c, -s, 0, s, c}; - memcpy(o->direction, r, sizeof r); + o->direction[0] = o->direction[4] = o->direction[8] = 1; } static inline void hos_motion_reset(HosMotion *m) { memset(m, 0, sizeof *m); - m->up[1] = 1; - hos_motion_rest(&m->out, 0); + m->quat[2] = -1; /* (Eden's ResetQuaternion: {0, 0, -1, 0}) */ + m->quat_out[2] = -1; + m->only_accelerometer = 1; + m->up[2] = 1; + hos_motion_rest(&m->out); } -static inline float hos_motion_wrap(float a) { - while (a > 3.14159265f) a -= HOS_MOTION_TAU; - while (a < -3.14159265f) a += HOS_MOTION_TAU; - return a; +static inline float hos_motion_len(const float v[3]) { return sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); } + +/* a quaternion (Eden's x, y, z, w) turned at Eden's filter rate rg for period seconds */ +static inline void hos_motion_integrate(float quat[4], const float rg[3], float period) { + float q1 = quat[3], q2 = quat[0], q3 = quat[1], q4 = quat[2]; + const float gx = rg[1], gy = rg[0], gz = rg[2]; + const float pa = q2, pb = q3, pc = q4; + q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * period); + q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * period); + q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * period); + q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * period); + const float n = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); + if (n > 0) quat[3] = q1 / n, quat[0] = q2 / n, quat[1] = q3 / n, quat[2] = q4 / n; +} +static inline void hos_motion_rate(const HosMotion *m, const float gyro[3], float rg[3]) { /* (Eden's swap, rad/s) */ + rg[0] = gyro[1] * HOS_MOTION_TAU, rg[1] = -gyro[0] * HOS_MOTION_TAU, rg[2] = -gyro[2] * HOS_MOTION_TAU; + if (m->only_accelerometer) rg[0] = rg[1] = rg[2] = 0; +} + +/* Mahony's filter on the real quaternion, Eden's UpdateOrientation (period: seconds; gain 10: the settling of + SetOrientationFromAccelerometer). Returns the feedback it added to the rate, in *corr (rad/s, the filter's frame), so + the handed-out orientation can lose the same drift. */ +static inline void hos_motion_mahony(HosMotion *m, const float accel[3], const float gyro[3], float period, float gain, + float corr[3]) { + const float q1 = m->quat[3], q2 = m->quat[0], q3 = m->quat[1], q4 = m->quat[2]; + const float alen = hos_motion_len(accel); + float rg[3]; + hos_motion_rate(m, gyro, rg); + if (gain > 1) rg[0] = rg[1] = rg[2] = 0; + corr[0] = corr[1] = corr[2] = 0; + if ((alen >= 0.75f && alen <= 1.25f) || gain > 1) { + const float ax = -accel[0] / alen, ay = accel[1] / alen, az = -accel[2] / alen; + const float vx = 2.0f * (q2 * q4 - q1 * q3); + const float vy = 2.0f * (q1 * q2 + q3 * q4); + const float vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; + const float e[3] = {az * vx - ax * vz, ay * vz - az * vy, ax * vy - ay * vx}; + memcpy(m->real_error, e, sizeof e); + if (gain <= 1 && hos_motion_len(e) >= 0.05f) + for (int i = 0; i < 3; i++) m->integral_error[i] += e[i]; + else if (gain <= 1) + memset(m->integral_error, 0, sizeof m->integral_error); + const float kp = 0.3f, ki = 0.005f; + const float wp = gain > 1 ? 10 : m->only_accelerometer ? 35 : 1, wi = gain > 1 ? 5 : m->only_accelerometer ? 10 : 1; + for (int i = 0; i < 3; i++) corr[i] = wp * kp * e[i] + wi * ki * m->integral_error[i]; + } + for (int i = 0; i < 3; i++) rg[i] += corr[i]; + hos_motion_integrate(m->quat, rg, period); +} + +/* the orientation matrix, Eden's GetOrientation (its quaternion's matrix, with its sign changes) */ +static inline void hos_motion_direction(const float quat[4], float d[9]) { + const float x = -quat[1], y = -quat[0], z = -quat[3], w = -quat[2]; + const float x2 = x * x, y2 = y * y, z2 = z * z, xy = x * y, wz = w * z, xz = x * z, wy = w * y, yz = y * z, wx = w * x; + const float mm[9] = {1 - 2 * (y2 + z2), 2 * (xy + wz), 2 * (xz - wy), 2 * (xy - wz), 1 - 2 * (x2 + z2), 2 * (yz + wx), + 2 * (xz + wy), 2 * (yz - wx), 1 - 2 * (x2 + y2)}; + d[0] = mm[0], d[1] = mm[1], d[2] = -mm[2]; + d[3] = mm[3], d[4] = mm[4], d[5] = -mm[5]; + d[6] = -mm[6], d[7] = -mm[7], d[8] = mm[8]; } /* One reading: SDL's accelerometer (m/s^2) and gyroscope (rad/s), dt seconds after the last one. sens_h and sens_v - scale the horizontal and the vertical aim (1: as the pad turns); inv_h and inv_v turn them the other way. Returns 0 - for a repeat of the last reading (no new reading: its time goes into the next), 1 otherwise. */ -static inline int hos_motion_update(HosMotion *m, const float accel[3], const float gyro[3], float dt, float sens_h, - float sens_v, int inv_h, int inv_v) { + scale the turn and the tilt (1: as the pad turns); inv_h and inv_v turn them the other way. Returns 0 for a repeat + of the last reading (no new reading: its time goes into the next), 1 otherwise. */ +static inline int hos_motion_update(HosMotion *m, const float accel_sdl[3], const float gyro_sdl[3], float dt, + float sens_h, float sens_v, int inv_h, int inv_v) { if (!(dt >= 0)) dt = 0; - if (m->stats_ready && !memcmp(accel, m->last_accel, sizeof m->last_accel) && - !memcmp(gyro, m->last_gyro, sizeof m->last_gyro)) { + if (m->stats_ready && !memcmp(accel_sdl, m->last_accel, sizeof m->last_accel) && + !memcmp(gyro_sdl, m->last_gyro, sizeof m->last_gyro)) { m->pending_dt += dt; return 0; } - memcpy(m->last_accel, accel, sizeof m->last_accel), memcpy(m->last_gyro, gyro, sizeof m->last_gyro); + memcpy(m->last_accel, accel_sdl, sizeof m->last_accel), memcpy(m->last_gyro, gyro_sdl, sizeof m->last_gyro); dt += m->pending_dt, m->pending_dt = 0; - if (dt > HOS_MOTION_DT_MAX) dt = HOS_MOTION_DT_MAX; /* (a hitch: its time, up to a limit) */ - const float alen = sqrtf(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); - const float ag = alen / HOS_MOTION_G; - /* the bias: learned from the short window's mean while the pad is still (both sensors steady, about 1 G) */ + /* the Switch's frame and units (Eden's sdl_driver.cpp), clamped as Eden clamps them */ + float a[3] = {-accel_sdl[0] / HOS_MOTION_G, accel_sdl[2] / HOS_MOTION_G, -accel_sdl[1] / HOS_MOTION_G}; + float g[3] = {gyro_sdl[0] / HOS_MOTION_TAU, -gyro_sdl[2] / HOS_MOTION_TAU, gyro_sdl[1] / HOS_MOTION_TAU}; + for (int i = 0; i < 3; i++) { + a[i] = a[i] < -7 ? -7 : a[i] > 7 ? 7 : a[i]; + } + const float alen = hos_motion_len(a); + /* stillness: the short window's spread of both sensors (rad/s and G, summed over the axes), about 1 G */ if (!m->stats_ready) { - memcpy(m->gmean, gyro, sizeof m->gmean), m->gvar = 1, m->amean = ag, m->avar = 1; - m->stats_ready = 1; + memcpy(m->gmean, g, sizeof m->gmean), memcpy(m->avec, a, sizeof m->avec); + m->gvar = 0.01f, m->avar = 0.01f, m->amean = alen, m->stats_ready = 1; } else if (dt > 0) { const float k = dt / (HOS_MOTION_WINDOW_S + dt); - float gv = 0; + float gv = 0, av = 0; for (int i = 0; i < 3; i++) { - const float d = gyro[i] - m->gmean[i]; - m->gmean[i] += d * k; - gv += d * d; + const float dg = (g[i] - m->gmean[i]) * HOS_MOTION_TAU, da = a[i] - m->avec[i]; + m->gmean[i] += (g[i] - m->gmean[i]) * k, m->avec[i] += da * k; + gv += dg * dg, av += da * da; } - m->gvar += (gv - m->gvar) * k; - const float da = ag - m->amean; - m->amean += da * k; - m->avar += (da * da - m->avar) * k; + m->gvar += (gv - m->gvar) * k, m->avar += (av - m->avar) * k, m->amean += (alen - m->amean) * k; } const int still = m->gvar < HOS_MOTION_STILL_GYRO * HOS_MOTION_STILL_GYRO && m->avar < HOS_MOTION_STILL_ACCEL * HOS_MOTION_STILL_ACCEL && m->amean > 0.9f && m->amean < 1.1f; m->still_t = still ? m->still_t + dt : 0; if (m->still_t >= HOS_MOTION_STILL_S && dt > 0) { - const float k = dt / ((m->learned_t < 2 ? 0.25f : 3.0f) + dt); /* (fast for the first 2 s still, then slowly) */ + const float k = dt / ((m->learned_t < 2 ? 0.25f : 3.0f) + dt); for (int i = 0; i < 3; i++) m->bias[i] += (m->gmean[i] - m->bias[i]) * k; m->learned_t += dt; } - const float w[3] = {gyro[0] - m->bias[0], gyro[1] - m->bias[1], gyro[2] - m->bias[2]}; - /* up: the gyro's prediction (a world vector seen from a frame turning at w turns the other way), pulled toward - the measured gravity while it is about 1 G */ - const int valid = alen > 0.65f * HOS_MOTION_G && alen < 1.35f * HOS_MOTION_G; - float *up = m->up; - if (!m->up_ready) { - if (valid) { - for (int i = 0; i < 3; i++) up[i] = accel[i] / alen; - m->up_ready = 1; - } - } else { - float p[3] = {up[0] + (-w[1] * up[2] + w[2] * up[1]) * dt, up[1] + (-w[2] * up[0] + w[0] * up[2]) * dt, - up[2] + (-w[0] * up[1] + w[1] * up[0]) * dt}; - float n = sqrtf(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]); - if (n > 0.001f) { - for (int i = 0; i < 3; i++) p[i] /= n; - if (valid) { - const float k = dt / (0.5f + dt); /* (a half-second pull) */ - for (int i = 0; i < 3; i++) p[i] += (accel[i] / alen - p[i]) * k; - n = sqrtf(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]); - if (n > 0.001f) for (int i = 0; i < 3; i++) p[i] /= n; + for (int i = 0; i < 3; i++) { + g[i] -= m->bias[i]; + g[i] = g[i] < -5 ? -5 : g[i] > 5 ? 5 : g[i]; + } + if (hos_motion_len(g) < (m->threshold > 0 ? m->threshold : HOS_MOTION_THRESHOLD)) g[0] = g[1] = g[2] = 0; + else m->only_accelerometer = 0; + /* the turn and the tilt: the angular velocity's part about the world's up, and the rest, scaled and turned around + as the menu says (the up from the accelerometer, smoothed) */ + if (alen > 0.65f && alen < 1.35f) { + const float k = dt / (0.1f + dt); + for (int i = 0; i < 3; i++) m->up[i] += (-a[i] / alen - m->up[i]) * k; + const float n = hos_motion_len(m->up); + if (n > 0.001f) for (int i = 0; i < 3; i++) m->up[i] /= n; + } + float gs[3]; /* (the scaled rates: what the game is handed; g stays real for the filter) */ + { + const float sh = sens_h * (inv_h ? -1.0f : 1.0f), sv = sens_v * (inv_v ? -1.0f : 1.0f); + const float t = g[0] * m->up[0] + g[1] * m->up[1] + g[2] * m->up[2]; + for (int i = 0; i < 3; i++) gs[i] = sh * t * m->up[i] + sv * (g[i] - t * m->up[i]); + } + if (dt > HOS_MOTION_DT_MAX) dt = 0; /* (Eden ignores a stall: no rotation, no orientation step) */ + for (int i = 0; i < 3; i++) m->out.rotation[i] += gs[i] * dt; + /* Eden's ResetOrientation: a filter that lost track (its error over 0.1) is set again from the accelerometer once + the pad has lain face up and still for 900 readings */ + if (hos_motion_len(m->real_error) >= 0.1f && !m->only_accelerometer) { + if (hos_motion_len(g) < 0.05f && alen > 0.9f && alen < 1.1f && a[2] <= -0.9f) { + if (++m->reset_counter > 900) { + m->quat[0] = m->quat[1] = m->quat[3] = 0, m->quat[2] = -1; + memset(m->integral_error, 0, sizeof m->integral_error); + float c[3]; + for (int it = 0; it < 100 && hos_motion_len(m->real_error) >= 0.01f; it++) + hos_motion_mahony(m, a, g, 0.015f, 10, c); + memcpy(m->quat_out, m->quat, sizeof m->quat); /* (set again from gravity: the handed-out one too) */ + m->reset_counter = 0; } - memcpy(up, p, sizeof p); + } else { + m->reset_counter = 0; } } - /* right: the pad's own x, level (gravity taken out); when the pad points x up, its z instead */ - float right[3] = {1 - up[0] * up[0], -up[0] * up[1], -up[0] * up[2]}; - float rl = sqrtf(right[0] * right[0] + right[1] * right[1] + right[2] * right[2]); - if (rl < 0.1f) { - right[0] = -up[0] * up[2], right[1] = -up[1] * up[2], right[2] = 1 - up[2] * up[2]; - rl = sqrtf(right[0] * right[0] + right[1] * right[1] + right[2] * right[2]); - } - for (int i = 0; i < 3; i++) right[i] /= rl; - const float fwd[3] = {right[1] * up[2] - right[2] * up[1], right[2] * up[0] - right[0] * up[2], - right[0] * up[1] - right[1] * up[0]}; - m->rates[0] = w[0] * right[0] + w[1] * right[1] + w[2] * right[2]; - m->rates[1] = w[0] * up[0] + w[1] * up[1] + w[2] * up[2]; - m->rates[2] = w[0] * fwd[0] + w[1] * fwd[1] + w[2] * fwd[2]; - const float sh = sens_h * (inv_h ? -1.0f : 1.0f), sv = -sens_v * (inv_v ? -1.0f : 1.0f); - m->pitch = hos_motion_wrap(m->pitch + m->rates[0] * sv * dt); - /* the vertical aim's drift: pulled gently toward the tilt gravity measures (the pad's tilt about its x axis since - the filter started, scaled the same way), so leftover bias or timing error cannot add up. Only while the pad's - x is about level (rolled under about 15 degrees): rolled, the tilt about the pad's own x moves faster than the - pitch about the levelled x (by 1 / sqrt(1 - up[0]^2)), and the pull would drag the aim past each move - (hckrman101); what builds up while rolled is corrected once it is level again. Not while shaken either. */ - if (m->up_ready && valid && fabsf(up[0]) < HOS_MOTION_TILT_ROLL) { - const float tilt = atan2f(-up[2], up[1]); /* (it grows at the x rate: d/dt = w[0]) */ - /* (anchored so the aim stays where it is: the first time the pad is level may be after it moved while rolled, - or held tilted when the filter started; sv is never 0, the menu's least sensitivity is 10%) */ - if (!m->tilt_ready) m->tilt0 = hos_motion_wrap(tilt - m->pitch / sv), m->tilt_ready = 1; - const float target = hos_motion_wrap(hos_motion_wrap(tilt - m->tilt0) * sv); - m->pitch = hos_motion_wrap(m->pitch + hos_motion_wrap(target - m->pitch) * (dt / (HOS_MOTION_TILT_S + dt))); + if (dt > 0) { + /* the real filter, then the handed-out orientation: the scaled rates plus the real filter's correction, so it + loses the gyro's drift but keeps a scaled or inverted move (the gravity pull acts on the real one only; + hckrman101). While only the accelerometer has been seen the two settle together. */ + const int settling = m->only_accelerometer; + float c[3], rs[3]; + hos_motion_mahony(m, a, g, dt, 1, c); + if (settling) { + memcpy(m->quat_out, m->quat, sizeof m->quat); + } else { + /* the scaling in the world's frame (hckrman101): the real rate taken to the world by the real orientation, + its vertical part (the turn) scaled by sh and the rest (the tilt) by sv, and brought back to the pad by the + handed-out one, so a scaled turn while tilted turns quat_out about its own vertical and does not tip it. + Both matrices map the world to the pad (their third column is the world's up seen from the pad). */ + float dr[9], dq[9], w[3], go[3]; + hos_motion_direction(m->quat, dr); + hos_motion_direction(m->quat_out, dq); + { + const float sh = sens_h * (inv_h ? -1.0f : 1.0f), sv = sens_v * (inv_v ? -1.0f : 1.0f); + for (int i = 0; i < 3; i++) w[i] = dr[i] * g[0] + dr[3 + i] * g[1] + dr[6 + i] * g[2]; /* (dr^T g) */ + w[0] *= sv, w[1] *= sv, w[2] *= sh; + for (int i = 0; i < 3; i++) go[i] = dq[3 * i] * w[0] + dq[3 * i + 1] * w[1] + dq[3 * i + 2] * w[2]; + } + hos_motion_rate(m, go, rs); + for (int i = 0; i < 3; i++) rs[i] += c[i]; + hos_motion_integrate(m->quat_out, rs, dt); + } } - hos_motion_rest(&m->out, m->pitch); - m->out.gyro[1] = m->rates[1] * sh / HOS_MOTION_TAU; + memcpy(m->out.accel, a, sizeof a); + memcpy(m->out.gyro, gs, sizeof gs); + hos_motion_direction(m->quat_out, m->out.direction); return 1; } diff --git a/host/nn_hid.c b/host/nn_hid.c index ec2ae894d..2e8471a6f 100644 --- a/host/nn_hid.c +++ b/host/nn_hid.c @@ -848,7 +848,7 @@ HLE(_ZN2nn3hid23GetSixAxisSensorHandlesEPNS0_19SixAxisSensorHandleEiRKjNS_4util1 /* SixAxisSensorState, 0x60 bytes: {i64 delta time (ns), i64 sampling number, f32 acceleration[3] (G), angular velocity[3] (revolutions a second), angle[3] (revolutions), direction[3][3], u32 attributes (1 connected), pad}. Newest first. (hookshot) With Motion controls on and a pad with a gyroscope (host/pad_sdl.c), the state OoT aims with - (host/hos_motion.h: the yaw rate and a tilt matrix, the rest neutral); else a pad lying still. */ + (host/hos_motion.h: the full state, as the console and Eden build it); else a pad lying still. */ HLE(_ZN2nn3hid22GetSixAxisSensorStatesEPNS0_18SixAxisSensorStateEiRKNS0_19SixAxisSensorHandleE) { /* (the sampling number goes up once per reading, as the hardware's does: while live it is the readings' own count, moved on past the last number handed out; each entry is its own reading, with the time since the one before) */ @@ -860,11 +860,13 @@ HLE(_ZN2nn3hid22GetSixAxisSensorStatesEPNS0_18SixAxisSensorStateEiRKNS0_19SixAxi int hos_sdl_motion_history(HosMotionSample *out, int max); /* pad_sdl.c */ HosMotionSample h[HOS_MOTION_HISTORY]; const int live = count ? hos_sdl_motion_history(h, count) : 0; + /* (each switch between live and at rest moves the numbers a whole history past the last ones given out, so no entry + of one ever has a number an entry of the other had) */ if (live) { - if (!was_live || six_base + h[0].n < six_sampling) six_base = six_sampling + 1 - h[0].n; + if (!was_live || six_base + h[0].n < six_sampling) six_base = six_sampling + HOS_MOTION_HISTORY - h[0].n; six_sampling = six_base + h[0].n; } else { - ++six_sampling; + six_sampling += was_live ? HOS_MOTION_HISTORY : 1; } was_live = live > 0; const int n = live ? live : count; diff --git a/host/pad_sdl.c b/host/pad_sdl.c index 1f8e5cad6..629bbb035 100644 --- a/host/pad_sdl.c +++ b/host/pad_sdl.c @@ -63,7 +63,8 @@ static float hd_values[2][4] = {{0.0f, 160.0f, 0.0f, 320.0f}, {0.0f, 160.0f, 0.0 /* (motion) the pad read and its filter (the polling thread's own), then what the game is handed (under state_lock): the last HOS_MOTION_HISTORY readings, newest at motion_hist[motion_head]; gyro_status for the menu */ static HosMotion motion; -static int64_t motion_ns, motion_reading_ns, motion_log_ns, motion_readings; +static int64_t motion_ns, motion_reading_ns, motion_log_ns, motion_readings, motion_start_ns; +static int motion_warned; static int motion_pad = -1; static uint32_t motion_serial; static HosMotionSample motion_hist[HOS_MOTION_HISTORY]; @@ -224,26 +225,42 @@ static void motion_poll(int64_t now, int count) { memcpy(motion.bias, pads[pick].bias, sizeof motion.bias), motion.learned_t = pads[pick].bias_t; motion_pad = pick, motion_serial = pads[pick].serial, motion_ns = now; fresh = 1; + motion_start_ns = now, motion_warned = 0; hos_log("hid: motion controls from \"%s\"", hos_sdl_pad_name(pads[pick].gc)); } /* the filter runs outside the lock (the game's input calls take it); a repeat of the last reading is no reading */ const int64_t dt_ns = now - motion_ns; motion_ns = now; + { /* (the Motion deadzone row: the console's tight, standard and loose zero-drift thresholds, turns a second) */ + static const float zone[3] = {HOS_MOTION_THRESHOLD_TIGHT, HOS_MOTION_THRESHOLD, HOS_MOTION_THRESHOLD_LOOSE}; + const int z = rx_option(HOS_OPT_GYRO_DEADZONE, 1); + motion.threshold = zone[z < 0 ? 0 : z > 2 ? 2 : z]; + } if (!hos_motion_update(&motion, a, g, (float)(dt_ns / 1e9), rx_option(HOS_OPT_GYRO_SENS_X, 100) / 100.0f, rx_option(HOS_OPT_GYRO_SENS_Y, 100) / 100.0f, rx_option(HOS_OPT_GYRO_INV_X, 0), rx_option(HOS_OPT_GYRO_INV_Y, 0)) && !fresh) return; + if (!motion_warned && motion.learned_t <= 0 && now - motion_start_ns > 30000000000LL) { + motion_warned = 1; /* (a pad noisier than the stillness test allows: its drift is never taken off) */ + hos_log("hid: motion: \"%s\" has not been set down yet, so its gyroscope's drift is not learned: set it on a " + "table for a second (if it is on a table and this stays, its sensor may be noisier than the stillness " + "test allows)", hos_sdl_pad_name(pads[pick].gc)); + } if (debug && now >= motion_log_ns) { /* (twice a second: what the game gets) */ motion_log_ns = now + 500000000LL; - hos_log("hid: motion: up (%+.2f, %+.2f, %+.2f), pitch/yaw/roll %+.2f %+.2f %+.2f rad/s, aim tilt %+.3f, yaw " - "%+.3f turns/s, bias (%+.4f, %+.4f, %+.4f)", motion.up[0], motion.up[1], motion.up[2], motion.rates[0], - motion.rates[1], motion.rates[2], motion.pitch, motion.out.gyro[1], motion.bias[0], motion.bias[1], - motion.bias[2]); + hos_log("hid: motion: accel (%+.2f, %+.2f, %+.2f) G, gyro (%+.3f, %+.3f, %+.3f) turns/s, direction rows " + "(%+.2f %+.2f %+.2f / %+.2f %+.2f %+.2f / %+.2f %+.2f %+.2f), bias (%+.4f, %+.4f, %+.4f)", motion.out.accel[0], + motion.out.accel[1], motion.out.accel[2], motion.out.gyro[0], motion.out.gyro[1], motion.out.gyro[2], + motion.out.direction[0], motion.out.direction[1], motion.out.direction[2], motion.out.direction[3], + motion.out.direction[4], motion.out.direction[5], motion.out.direction[6], motion.out.direction[7], + motion.out.direction[8], motion.bias[0], motion.bias[1], motion.bias[2]); } HosMotionSample r; r.st = motion.out; r.n = ++motion_readings; - r.dt_ns = fresh ? 5000000 : now - motion_reading_ns; /* (the time since the reading before) */ + /* (the time since the reading before, capped as the filter caps it: a stalled pad's next reading is no jump) */ + r.dt_ns = fresh ? 5000000 : now - motion_reading_ns; + if (r.dt_ns > (int64_t)(HOS_MOTION_DT_MAX * 1e9)) r.dt_ns = (int64_t)(HOS_MOTION_DT_MAX * 1e9); motion_reading_ns = now; AcquireSRWLockExclusive(&state_lock); motion_head = (motion_head + 1) % HOS_MOTION_HISTORY; diff --git a/readme.md b/readme.md index 760c6d9fa..ee8aa8751 100644 --- a/readme.md +++ b/readme.md @@ -241,12 +241,8 @@ choices and starts the game again, so progress since your last save is lost. | | Invert camera Y | Off, On | Off | | | Stick dead zone | Default (24%), 5%, 10%, 15%, 20%, 30% | Default (24%) | | | Rumble strength | Off, 25% to 150%, in steps of 25 | 100% | -| | Motion controls | Off, On | On | -| | Motion sensitivity X | 10% to 600% | 100% | -| | Motion sensitivity Y | 10% to 300% | 100% | -| | Invert motion X | Off, On | Off | -| | Invert motion Y | Off, On | Off | | | Layout preset | Nintendo (Def), Xbox / PC, Swap Face, Custom | Nintendo (Def) | +| | Motion | Opens the Motion page: Motion controls (Off, On; On), Motion sensitivity X (10% to 600%) and Y (10% to 300%; 100%), Invert motion X and Y (Off, On; Off), Motion deadzone (Low, Standard, High; Standard) | | | | Controls | Opens the Controls page | | - **Quality preset** sets Render resolution, Window scaling filter, Sharpness, Frame generation, Frame pacing, Game @@ -298,10 +294,11 @@ choices and starts the game again, so progress since your last save is lost. a precise stick, higher one that drifts. The game keeps a dead zone of its own after it. - **Rumble strength** scales the controller's rumble: 100% is the game's own, Off turns it off. The game's HD rumble goes to a controller's heavy and light motors by its frequency. -- **Motion controls:** the controller's gyroscope aims where the game uses motion, as on the Switch. It works with - Switch Pro controllers and pads in Switch mode, DualSense and DualShock 4 (through SDL), held flat or upright. Its row +- **Motion controls** (Controls tab, Motion page): the controller's gyroscope aims where the game uses motion, as on the Switch. It works with + Switch Pro controllers and pads in Switch mode, DualSense and DualShock 4 (through SDL). Its row says whether the connected pad has a gyroscope; Off leaves the game seeing a pad lying still. **Motion sensitivity X/Y** - scale the left-right and up-down aim (100% is the console's), **Invert motion X/Y** turn either direction around. Motion follows the pad used + scale the left-right and up-down aim (100% is the console's), **Invert motion X/Y** turn either direction around. The game gets the motion + state as a Switch (and Eden) builds it, so how you hold the pad when motion starts does not matter. Motion follows the pad used last. The gyroscope's drift is learned while the pad rests on a table, so set it down for a second if the aim creeps. `HOS_MOTION_DEBUG=1` (in `hookshot_env.txt`) logs what the game gets twice a second. - **Layout preset:** Nintendo has A on the right face button, Xbox / PC and Swap Face put A on the bottom one. Changing diff --git a/tests/motion_test.c b/tests/motion_test.c index 17710853f..08e2fd030 100644 --- a/tests/motion_test.c +++ b/tests/motion_test.c @@ -15,6 +15,7 @@ static int failures; static float su[3]; /* the true up, in the pad's frame */ static float offset[3]; /* the gyroscope's bias */ static float tremor; /* the noise's size, rad/s (each axis, uniform) */ +static float anoise; /* the accelerometer's, G (each axis, uniform; 0: a real sensor's least) */ static unsigned rng = 1; static float noise(void) { rng = rng * 1664525u + 1013904223u; @@ -34,8 +35,9 @@ static void run_dt(HosMotion *m, float gx, float gy, float gz, float seconds, fl const int n = (int)lroundf(seconds / dt); for (int i = 0; i < n; i++) { const float w[3] = {gx, gy, gz}; - const float a[3] = {su[0] * HOS_MOTION_G + 1e-5f * noise(), su[1] * HOS_MOTION_G + 1e-5f * noise(), - su[2] * HOS_MOTION_G + 1e-5f * noise()}; + const float an = (anoise > 0 ? anoise * HOS_MOTION_G : 1e-5f); + const float a[3] = {su[0] * HOS_MOTION_G + an * noise(), su[1] * HOS_MOTION_G + an * noise(), + su[2] * HOS_MOTION_G + an * noise()}; float g[3]; for (int k = 0; k < 3; k++) g[k] = w[k] + offset[k] + (tremor > 0 ? tremor : 1e-6f) * noise(); hos_motion_update(m, a, g, dt, sens_h, sens_v, inv_h, inv_v); @@ -56,162 +58,191 @@ static void start(HosMotion *m, float x, float y, float z) { /* a fresh filter hos_motion_reset(m); pose(x, y, z); offset[0] = offset[1] = offset[2] = 0; - tremor = 0; + tremor = 0, anoise = 0; } -static void neutral(const HosMotion *m) { /* what the game folds into its recenter: always at rest */ - NEAR(m->out.accel[0], 0, 1e-6); - NEAR(m->out.accel[1], 0, 1e-6); - NEAR(m->out.accel[2], -1, 1e-6); - NEAR(m->out.gyro[0], 0, 1e-6); - NEAR(m->out.gyro[2], 0, 1e-6); - NEAR(m->out.rotation[0] + m->out.rotation[1] + m->out.rotation[2], 0, 1e-6); - NEAR(m->out.direction[0], 1, 1e-6); /* (a tilt about x only) */ +static float dot3(const float a[3], const float b[3]) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; } +static void same_direction(const HosMotion *a, const HosMotion *b, double tol) { + for (int i = 0; i < 9; i++) NEAR(a->out.direction[i], b->out.direction[i], tol); } int main(void) { - HosMotion m; + HosMotion m, n; - /* before any reading: a pad lying still */ + /* before any reading: a pad lying still, face up */ hos_motion_reset(&m); - neutral(&m); - NEAR(m.out.direction[4] + m.out.direction[8], 2, 1e-6); + NEAR(m.out.accel[2], -1, 1e-6); + NEAR(m.out.direction[0] + m.out.direction[4] + m.out.direction[8], 3, 1e-6); - /* lying still with a gyroscope that reads a small rate: learned and taken off, no aim moves */ + /* lying face up: (0, 0, -1) G in the Switch's frame, no rate, the direction the identity */ start(&m, FLAT); - offset[0] = 0.012f, offset[1] = -0.008f, offset[2] = 0.010f; - run(&m, 0, 0, 0, 4, 1, 1, 0, 0); - NEAR(m.out.gyro[1], 0, 1e-4); - NEAR(m.pitch, 0, 0.02); - neutral(&m); + run(&m, 0, 0, 0, 1, 1, 1, 0, 0); + NEAR(m.out.accel[0], 0, 1e-3); + NEAR(m.out.accel[1], 0, 1e-3); + NEAR(m.out.accel[2], -1, 1e-3); + NEAR(m.out.gyro[0] + m.out.gyro[1] + m.out.gyro[2], 0, 1e-6); + NEAR(m.out.direction[0] + m.out.direction[4] + m.out.direction[8], 3, 0.01); + + /* the Switch's frame and units (Eden's sdl_driver.cpp): a turn about SDL's +Y (the pad flat, turning left) at one + turn a second is +1 in z; about SDL's +X (the front up), +1 in x; held upright, SDL's -Z up, -Z is +y */ + start(&m, FLAT); + run(&m, 0, 0, 0, 0.2f, 1, 1, 0, 0); + run(&m, 0, HOS_MOTION_TAU, 0, 0.05f, 1, 1, 0, 0); + NEAR(m.out.gyro[2], 1, 1e-3); + NEAR(m.out.gyro[0], 0, 1e-3); + start(&m, FLAT); + run(&m, 0, 0, 0, 0.2f, 1, 1, 0, 0); + run(&m, HOS_MOTION_TAU, 0, 0, 0.02f, 1, 1, 0, 0); + NEAR(m.out.gyro[0], 1, 0.02); + start(&m, UPRIGHT); + run(&m, 0, 0, 0, 0.2f, 1, 1, 0, 0); + NEAR(m.out.accel[1], -1, 1e-3); + run(&m, 0, 0, -HOS_MOTION_TAU, 0.05f, 1, 1, 0, 0); + NEAR(m.out.gyro[1], 1, 1e-3); - /* a large offset (0.2 rad/s, past what a rate threshold would call still) is learned too */ + /* the rotation is the angular velocity summed: half a second at a quarter turn a second about +z is 0.125 */ start(&m, FLAT); - offset[1] = 0.2f; - run(&m, 0, 0, 0, 4, 1, 1, 0, 0); - NEAR(m.bias[1], 0.2, 1e-3); + run(&m, 0, 0, 0, 0.2f, 1, 1, 0, 0); + run(&m, 0, HOS_MOTION_TAU / 4, 0, 0.5f, 1, 1, 0, 0); + NEAR(m.out.rotation[2], 0.125, 3e-3); - /* a slow, steady turn in the hand (3 degrees a second, with a hand's tremor) is aim: not learned away */ + /* how the pad is held when the filter starts does not matter: started flat and then tilted 0.7 rad nose up, or + started there, the direction is the same (Link-22's start-pose case: the console's orientation follows gravity) */ start(&m, FLAT); - run(&m, 0, 0, 0, 1, 1, 1, 0, 0); /* (on the table first) */ - tremor = 0.03f; - run(&m, 0, 0.0524f, 0, 2, 1, 1, 0, 0); - NEAR(m.bias[1], 0, 1e-3); - - /* turning left (about SDL's +Y, the pad flat) at one turn a second: the yaw slot reads 1 turn a second */ - start(&m, FLAT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); - run(&m, 0, HOS_MOTION_TAU, 0, 0.1f, 1, 1, 0, 0); - NEAR(m.out.gyro[1], 1, 0.02); - neutral(&m); - - /* tilting the front up (about SDL's +X) at 1 rad/s for half a second: the tilt matrix turns -0.5 rad, the rest - neutral (the pitch rate itself never goes out), and it stays there while held */ - start(&m, FLAT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); - run(&m, 1, 0, 0, 0.5f, 1, 1, 0, 0); - NEAR(m.pitch, -0.5, 0.03); - NEAR(m.out.direction[4], cosf(m.pitch), 1e-5); - NEAR(m.out.direction[5], -sinf(m.pitch), 1e-5); - NEAR(m.out.direction[7], sinf(m.pitch), 1e-5); - NEAR(m.out.gyro[1], 0, 0.01); - neutral(&m); - run(&m, 0, 0, 0, 3, 1, 1, 0, 0); - NEAR(m.pitch, -0.5, 0.03); - - /* rolled 30 degrees, a tilt of the nose up (about the levelled x) moves the aim by the tilt and it stays there: - the pull, which measures about the pad's own x, waits until the pad is level again (hckrman101) */ - start(&m, -0.5f, 0.8660254f, 0); run(&m, 0, 0, 0, 0.5f, 1, 1, 0, 0); + run(&m, 1, 0, 0, 0.7f, 1, 1, 0, 0); + run(&m, 0, 0, 0, 2, 1, 1, 0, 0); + start(&n, su[0], su[1], su[2]); + run(&n, 0, 0, 0, 2.5f, 1, 1, 0, 0); + same_direction(&m, &n, 0.03); + /* ... rolled 40 degrees and pitched 0.6 at the start, then levelled: the same as a level start in that pose */ + start(&m, 0, cosf(0.6f), -sinf(0.6f)); { - float r[3] = {1 - su[0] * su[0], -su[0] * su[1], -su[0] * su[2]}; - const float n = sqrtf(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); - run(&m, r[0] / n, r[1] / n, r[2] / n, 0.5f, 1, 1, 0, 0); + const float c = cosf(0.7f), sn = sinf(0.7f), x = su[0] * c - su[1] * sn, y = su[0] * sn + su[1] * c; + pose(x, y, su[2]); } - NEAR(m.pitch, -0.5, 0.03); - run(&m, 0, 0, 0, 3, 1, 1, 0, 0); - NEAR(m.pitch, -0.5, 0.03); - /* ... then rolled back to level (about the pad's z, which keeps the nose's height): the pull starts there, - anchored to the aim as it is, so the aim neither jumps nor drifts back to level. This filter started rolled, the - case behind Link-22's "wrong after turning Motion controls on while not level" (hckrman101) */ - for (int i = 0; i < 80 && su[0] < 0; i++) run(&m, 0, 0, 0.5236f, 0.02f, 1, 1, 0, 0); - NEAR(su[0], 0, 0.02); + run(&m, 0, 0, 0, 0.5f, 1, 1, 0, 0); + for (int i = 0; i < 80 && fabsf(su[0]) > 0.01f; i++) run(&m, 0, 0, su[0] > 0 ? -0.5236f : 0.5236f, 0.02f, 1, 1, 0, 0); + run(&m, 0, 0, 0, 2, 1, 1, 0, 0); + start(&n, su[0], su[1], su[2]); + run(&n, 0, 0, 0, 2.5f, 1, 1, 0, 0); + /* (the tilt agrees: the gravity each matrix implies; the heading about the vertical is the gyro's own, as on the + console, so compare the matrices' third columns, the world's vertical seen from the pad) */ { - const float p0 = m.pitch; - NEAR(p0, -0.5, 0.08); - run(&m, 0, 0, 0, 3, 1, 1, 0, 0); - NEAR(m.pitch, p0, 0.02); + const float a3[3] = {m.out.direction[2], m.out.direction[5], m.out.direction[8]}; + const float b3[3] = {n.out.direction[2], n.out.direction[5], n.out.direction[8]}; + NEAR(dot3(a3, b3), 1, 2e-3); } - /* a leftover bias the filter cannot learn (the pad in the hand) does not drift the vertical aim: 0.01 rad/s for a - minute would be 0.6 rad of tilt summed open loop */ + /* the gyroscope's resting rate is learned on a table and taken off */ start(&m, FLAT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); - offset[0] = 0.01f, tremor = 0.03f; - run(&m, 0, 0, 0, 60, 1, 1, 0, 0); - NEAR(m.pitch, 0, 0.03); - - /* held upright the same turn left is about the pad's -Z: the yaw slot still reads +1 (how the pad is held does - not matter) */ - start(&m, UPRIGHT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); - run(&m, 0, 0, -HOS_MOTION_TAU, 0.1f, 1, 1, 0, 0); - NEAR(m.out.gyro[1], 1, 0.02); - - /* the sensitivities (each its own axis) and the inverts; the tilt's pull keeps the scaled tilt */ + offset[0] = 0.012f, offset[1] = -0.008f, offset[2] = 0.010f; + run(&m, 0, 0, 0, 4, 1, 1, 0, 0); + NEAR(m.bias[0], 0.012 / HOS_MOTION_TAU, 2e-4); + NEAR(m.bias[2], -0.008 / HOS_MOTION_TAU, 2e-4); /* (SDL's y is the Switch's z) */ + NEAR(m.out.gyro[0] + m.out.gyro[1] + m.out.gyro[2], 0, 1e-6); + /* ... a large one too, and a noisier sensor's (0.0067 rad/s and 5 mG on each axis) */ + start(&m, FLAT); + offset[1] = 0.2f; + run(&m, 0, 0, 0, 4, 1, 1, 0, 0); + NEAR(m.bias[2], 0.2 / HOS_MOTION_TAU, 2e-4); start(&m, FLAT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); - run(&m, 0, HOS_MOTION_TAU, 0, 0.1f, 2, 0.5f, 1, 0); - NEAR(m.out.gyro[1], -2, 0.04); - NEAR(m.pitch, 0, 0.01); + offset[1] = 0.02f, tremor = 0.0116f, anoise = 0.0087f; + run(&m, 0, 0, 0, 6, 1, 1, 0, 0); + NEAR(m.bias[2], 0.02 / HOS_MOTION_TAU, 4e-4); + /* ... but a slow turn in the hand (tremor) is aim, and a steady machine pitch is not stillness: neither is learned */ start(&m, FLAT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); - run(&m, 1, 0, 0, 0.5f, 3, 1, 0, 1); - NEAR(m.pitch, 0.5, 0.03); - NEAR(m.out.gyro[1], 0, 0.01); + run(&m, 0, 0, 0, 1, 1, 1, 0, 0); + tremor = 0.03f; + run(&m, 0, 0.0524f, 0, 2, 1, 1, 0, 0); + NEAR(m.bias[2], 0, 2e-4); start(&m, FLAT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); - run(&m, 1, 0, 0, 0.5f, 1, 2, 0, 0); - NEAR(m.pitch, -1.0, 0.05); - run(&m, 0, 0, 0, 3, 1, 2, 0, 0); - NEAR(m.pitch, -1.0, 0.05); + run(&m, 0.2f, 0, 0, 3, 1, 1, 0, 0); + NEAR(m.bias[0], 0, 2e-4); - /* picked up from flat to upright without the gyro seeing it: up settles on the measured gravity */ + /* under the console's zero-drift threshold (0.007 turns a second) a rate counts as none */ start(&m, FLAT); - run(&m, 0, 0, 0, 0.5f, 1, 1, 0, 0); - pose(UPRIGHT); - run(&m, 0, 0, 0, 5, 1, 1, 0, 0); - NEAR(m.up[2], -1, 0.01); + run(&m, 0, 0, 0, 0.2f, 1, 1, 0, 0); + run(&m, 0, 0.03f, 0, 0.1f, 1, 1, 0, 0); + NEAR(m.out.gyro[2], 0, 1e-6); - /* the real time between readings: polls 1 ms apart (Linux, a 1 kHz pad) and 60 ms apart (a hitch) both add up */ + /* the Motion deadzone row: Low (the console's tight mode) lets a 0.005 turns/s turn through, Standard does not, + High (loose) also drops 0.008 */ start(&m, FLAT); - run_dt(&m, 0, 0, 0, 0.1f, 0.001f, 1, 1, 0, 0); - run_dt(&m, 1, 0, 0, 0.5f, 0.001f, 1, 1, 0, 0); - NEAR(m.pitch, -0.5, 0.03); + run(&m, 0, 0, 0, 0.2f, 1, 1, 0, 0); + m.threshold = HOS_MOTION_THRESHOLD_TIGHT; + run(&m, 0, 0.03f, 0, 0.1f, 1, 1, 0, 0); + NEAR(m.out.gyro[2], 0.03 / HOS_MOTION_TAU, 1e-4); + m.threshold = HOS_MOTION_THRESHOLD_LOOSE; + run(&m, 0, 0.05f, 0, 0.1f, 1, 1, 0, 0); + NEAR(m.out.gyro[2], 0, 1e-6); + m.threshold = 0; + + /* the sensitivities and the inverts: the turn (about the world's up) and the tilt (the rest) each scaled */ start(&m, FLAT); - run_dt(&m, 0, 0, 0, 0.12f, 0.06f, 1, 1, 0, 0); - run_dt(&m, 1, 0, 0, 0.48f, 0.06f, 1, 1, 0, 0); - NEAR(m.pitch, -0.48, 0.03); + run(&m, 0, 0, 0, 0.3f, 1, 1, 0, 0); + run(&m, 0, HOS_MOTION_TAU, 0, 0.05f, 2, 0.5f, 0, 0); + NEAR(m.out.gyro[2], 2, 0.01); + run(&m, 0, HOS_MOTION_TAU, 0, 0.05f, 1, 1, 1, 0); + NEAR(m.out.gyro[2], -1, 0.01); + start(&m, FLAT); + run(&m, 0, 0, 0, 0.3f, 1, 1, 0, 0); + run(&m, HOS_MOTION_TAU, 0, 0, 0.01f, 3, 0.5f, 0, 0); + NEAR(m.out.gyro[0], 0.5, 0.02); + NEAR(m.out.gyro[2], 0, 0.01); + run(&m, HOS_MOTION_TAU, 0, 0, 0.01f, 1, 1, 0, 1); + NEAR(m.out.gyro[0], -1, 0.03); + + /* ... and the orientation keeps a scaled or inverted tilt while held: 0.5 rad nose up at 200% stays at 1.0 for 3 s, + and inverted at -0.5 (the gravity pull acts on the real filter only; hckrman101). Compared against the matrix of + a pad really tilted that far, at 100% */ + for (int c = 0; c < 2; c++) { + const float sens = c ? 1.0f : 2.0f, want = c ? -0.5f : 1.0f; + start(&m, FLAT); + run(&m, 0, 0, 0, 0.5f, 1, sens, 0, c); + run(&m, 1, 0, 0, 0.5f, 1, sens, 0, c); + run(&m, 0, 0, 0, 3, 1, sens, 0, c); + start(&n, FLAT); + run(&n, 0, 0, 0, 0.5f, 1, 1, 0, 0); + run(&n, want > 0 ? 1.0f : -1.0f, 0, 0, fabsf(want), 1, 1, 0, 0); + run(&n, 0, 0, 0, 3, 1, 1, 0, 0); + same_direction(&m, &n, 0.04); + } - /* a stall (a second between readings) counts as HOS_MOTION_DT_MAX, not a jump (shaken: no tilt pull) */ + /* a scaled turn while tilted keeps the handed-out orientation upright (the scaling in the world's frame, + hckrman101): 0.5 rad nose up at 200% (1.0 out), then a quarter turn about the vertical at 200% (a half turn out): + the world's up seen from the pad, its third column, is still a real 1.0 rad tilt's */ start(&m, FLAT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); + run(&m, 0, 0, 0, 0.5f, 2, 2, 0, 0); + run(&m, 1, 0, 0, 0.5f, 2, 2, 0, 0); + { /* (the quarter turn: about the world's up, seen from the pad) */ + const float u[3] = {su[0], su[1], su[2]}; + run(&m, u[0] * 1.5708f, u[1] * 1.5708f, u[2] * 1.5708f, 1, 2, 2, 0, 0); + } + run(&m, 0, 0, 0, 1, 2, 2, 0, 0); + start(&n, FLAT); + run(&n, 0, 0, 0, 0.5f, 1, 1, 0, 0); + run(&n, 1, 0, 0, 1, 1, 1, 0, 0); + run(&n, 0, 0, 0, 1.5f, 1, 1, 0, 0); { - const float a[3] = {0, 2 * HOS_MOTION_G, 0}, g[3] = {1, 0, 0}; - hos_motion_update(&m, a, g, 1.0f, 1, 1, 0, 0); + const float a3[3] = {m.out.direction[2], m.out.direction[5], m.out.direction[8]}; + const float b3[3] = {n.out.direction[2], n.out.direction[5], n.out.direction[8]}; + NEAR(dot3(a3, b3), 1, 2e-3); } - NEAR(m.pitch, -HOS_MOTION_DT_MAX, 2e-3); - /* a repeat of the last reading is no reading: its time goes into the next */ + /* a repeat of the last reading is no reading (its time goes into the next), and a stall is no step */ start(&m, FLAT); - run(&m, 0, 0, 0, 0.1f, 1, 1, 0, 0); + run(&m, 0, 0, 0, 0.2f, 1, 1, 0, 0); { - const float a[3] = {0, 2 * HOS_MOTION_G, 0}, g0[3] = {0, 0, 0}, g1[3] = {1, 0, 0}; - const float p0 = m.pitch; + const float a[3] = {0, HOS_MOTION_G, 0}, g0[3] = {0, 0, 0}, g1[3] = {0, HOS_MOTION_TAU, 0}; NEAR(hos_motion_update(&m, a, g0, 0.004f, 1, 1, 0, 0), 1, 0); NEAR(hos_motion_update(&m, a, g0, 0.004f, 1, 1, 0, 0), 0, 0); + const float r0 = m.out.rotation[2]; NEAR(hos_motion_update(&m, a, g1, 0.004f, 1, 1, 0, 0), 1, 0); - NEAR(m.pitch - p0, -0.008, 2e-4); + NEAR(m.out.rotation[2] - r0, 0.008, 2e-4); + const float r1 = m.out.rotation[2]; + hos_motion_update(&m, a, g0, 1.0f, 1, 1, 0, 0); + NEAR(m.out.rotation[2], r1, 1e-6); } if (failures) return 1;