From 9730882fe405e31cd4042d3556ee5dcc1b025ebb Mon Sep 17 00:00:00 2001 From: minenice55 Date: Thu, 2 Apr 2026 01:32:10 -0400 Subject: [PATCH] gyro calibration, spectator aiming using gyro also moves all of the motion sensor stuff to g_input.c --- src/g_game.c | 251 +++---------------------------------- src/g_game.h | 28 +---- src/g_input.c | 330 +++++++++++++++++++++++++++++++++++++++++++++++++ src/g_input.h | 29 +++++ src/k_hud.c | 1 + src/st_stuff.c | 21 ++-- 6 files changed, 387 insertions(+), 273 deletions(-) diff --git a/src/g_game.c b/src/g_game.c index 679a8d3d4..88672a4a6 100644 --- a/src/g_game.c +++ b/src/g_game.c @@ -51,7 +51,6 @@ #include "m_cond.h" // condition sets #include "r_fps.h" // frame interpolation/uncapped #include "lua_hud.h" -#include "m_easing.h" // SRB2kart #include "k_kart.h" @@ -1404,235 +1403,6 @@ static void G_HandleAxisDeadZone(UINT8 splitnum, joystickvector2_t *joystickvect } } -// copy/pasted from the lua version of these routines -inline static vector3_t *QuaternionMulVec3(vector3_t *out, vector3_t *a, vector4_t *b) -{ - fixed_t ax = a->x, ay = a->y, az = a->z, aw = 0; - fixed_t bx = b->x, by = b->y, bz = b->z, bw = b->a; - - FV3_NormalizeEx(out, FV3_Load(out, - FixedMul(aw, bx) + FixedMul(ax, bw) + FixedMul(ay, bz) - FixedMul(az, by), - FixedMul(aw, by) - FixedMul(ax, bz) + FixedMul(ay, bw) + FixedMul(az, bx), - FixedMul(aw, bz) + FixedMul(ax, by) - FixedMul(ay, bx) + FixedMul(az, bw) - )); - - return out; -} - -inline static fixed_t FV3_LengthSquared(const vector3_t *vec) -{ - return FixedMul(vec->x, vec->x) + FixedMul(vec->y, vec->y) + FixedMul(vec->z, vec->z); -} - -inline static fixed_t FV3_Length(const vector3_t *vec) -{ - return FixedSqrt(FV3_LengthSquared(vec)); -} - -inline static vector4_t AngleAxis(fixed_t angle, fixed_t x, fixed_t y, fixed_t z) -{ - fixed_t sinangle = FINESINE(FixedAngle(angle/2) >> ANGLETOFINESHIFT); - fixed_t cosangle = FINECOSINE(FixedAngle(angle/2) >> ANGLETOFINESHIFT); - vector3_t axis = {x, y, z}; - vector4_t result; - - FV3_Normalize(&axis); - - FV4_Load(&result, - FixedMul(axis.x, sinangle), - FixedMul(axis.y, sinangle), - FixedMul(axis.z, sinangle), - cosangle - ); - - return result; -} - -// math sourced from this article -// http://gyrowiki.jibbsmart.com/blog:finding-gravity-with-sensor-fusion -// state -fixed_t localshakinessfac[MAXSPLITSCREENPLAYERS]; -vector3_t localsmoothedaccel[MAXSPLITSCREENPLAYERS]; -vector3_t localgravityvectors[MAXSPLITSCREENPLAYERS]; - -// the time it takes in our acceleration smoothing for 'A' to get halfway to 'B' -#define SmoothingHalfTime (0.01) -// thresholds of trust for accel shakiness. less shakiness = more trust -#define ShakinessMaxThreshold (50*FRACUNIT/100) -#define ShakinessMinThreshold (1*FRACUNIT/100) -// when we trust the accel a lot (the controller is "still"), how quickly do we correct our gravity vector? -#define CorrectionStillRate (FRACUNIT) -// when we don't trust the accel (the controller is "shaky"), how quickly do we correct our gravity vector? -#define CorrectionShakyRate (10*FRACUNIT/100) -// if our old gravity vector is close enough to our new one, limit further corrections to this proportion of the rotation speed -#define CorrectionGyroFactor (40*FRACUNIT/100) -// thresholds for what's considered "close enough" -#define CorrectionGyroMinThreshold (5*FRACUNIT/100) -#define CorrectionGyroMaxThreshold (FRACUNIT/4) -// no matter what, always apply a minimum of this much correction to our gravity vector -#define CorrectionMinimumSpeed (1*FRACUNIT/100) - -// when holding the controller still (shaking and turning included), corrcet this quickly to resolve error -#define CorrectionInstantRate (80*FRACUNIT/100) -#define CorrectionInstantShake (4*FRACUNIT/100) -#define CorrectionInstantTurn (5*FRACUNIT/100) - -void G_UpdateGamepadGravity(INT32 p, vector3_t gyro, vector3_t accel) -{ - // convert gyro input to reverse rotation - vector3_t invAccel = {-accel.x, -accel.y, -accel.z}; - fixed_t correctionRate = 0; - // scaling is reversed, smaller time scales = larger steps in this code - // (1/timescale)/dt - fixed_t deltaseconds = FixedDiv(FRACUNIT, max(cv_timescale.value, FRACUNIT/20))/TICRATE; - // we don't have exp2 and we actually need it here to take timescale into account :( - fixed_t smoothFactor = FloatToFixed(exp2(-FixedToFloat(deltaseconds) / SmoothingHalfTime)); - fixed_t angleRate; - fixed_t correctionLimit; - vector4_t invRotation = AngleAxis( - FixedMul(FV3_Length(&gyro), deltaseconds), - -gyro.x, - -gyro.y, - -gyro.z - ); - vector3_t gravityDelta = {0}; - vector3_t gravityDeltaDirection = {0}; - vector3_t correction = {0}; - - if (!G_GetGamepadCanUseTilt(p)) return; - CONS_Debug(DBG_IMU, "= Update Gravity Delta Time: %4.3f =\n", FixedToFloat(deltaseconds)); - - // rotate gravity vector - QuaternionMulVec3(&localgravityvectors[p], &localgravityvectors[p], &invRotation); - QuaternionMulVec3(&localsmoothedaccel[p], &localsmoothedaccel[p], &invRotation); - localshakinessfac[p] = FixedMul(localshakinessfac[p], smoothFactor), - FV3_SubEx(&accel, &localsmoothedaccel[p], &correction); - localshakinessfac[p] = max(localshakinessfac[p], FV3_Length(&correction)); - FV3_Load(&localsmoothedaccel[p], - Easing_Linear(smoothFactor, accel.x, localsmoothedaccel[p].x), - Easing_Linear(smoothFactor, accel.y, localsmoothedaccel[p].y), - Easing_Linear(smoothFactor, accel.z, localsmoothedaccel[p].z) - ); - - CONS_Debug(DBG_IMU, "Shakiness: %4.2f\n", FixedToFloat(localshakinessfac[p])); - - FV3_SubEx(&invAccel, &localgravityvectors[p], &gravityDelta); - FV3_NormalizeEx(&gravityDelta, &gravityDeltaDirection); - - if (ShakinessMaxThreshold > ShakinessMinThreshold) - { - fixed_t stillness = CLAMP(FixedDiv((localshakinessfac[p] - ShakinessMinThreshold), (ShakinessMaxThreshold - ShakinessMinThreshold)), 0, FRACUNIT); - correctionRate = CorrectionStillRate + FixedMul((CorrectionShakyRate - CorrectionStillRate), stillness); - } - else if (localshakinessfac[p] > ShakinessMaxThreshold) - { - correctionRate = CorrectionShakyRate; - } - else - { - correctionRate = CorrectionStillRate; - } - - // limit in proportion to rotation rate - angleRate = FixedMul(FV3_Length(&gyro), M_PI_FIXED/180); - correctionLimit = max(FixedMul(FixedMul(angleRate, FV3_Length(&localgravityvectors[p])), CorrectionGyroFactor), CorrectionMinimumSpeed); - - CONS_Debug(DBG_IMU, "Angle Rate: %4.3f\n", FixedToFloat(angleRate)); - CONS_Debug(DBG_IMU, "Correction Limit: %4.3f\n", FixedToFloat(correctionLimit)); - - if (correctionRate > correctionLimit) { - fixed_t closeEnoughFactor; - if (CorrectionGyroMaxThreshold > CorrectionGyroMinThreshold) - { - closeEnoughFactor = CLAMP(FixedDiv((FV3_Length(&gravityDelta) - CorrectionGyroMinThreshold), (CorrectionGyroMaxThreshold - CorrectionGyroMinThreshold)), 0, FRACUNIT); - } - else if (FV3_Length(&gravityDelta) > CorrectionGyroMaxThreshold) - { - closeEnoughFactor = FRACUNIT; - } - else - { - closeEnoughFactor = 0; - } - - CONS_Debug(DBG_IMU, "'Close Enough' Fac: %4.3f\n", FixedToFloat(closeEnoughFactor)); - correctionRate = correctionLimit + FixedMul((correctionRate - correctionLimit), closeEnoughFactor); - } - - if (localshakinessfac[p] < CorrectionInstantShake && angleRate < CorrectionInstantTurn) - { - correctionRate = max(correctionRate, CorrectionInstantRate); - } - - CONS_Debug(DBG_IMU, "Correction Rate: %4.2f\n", FixedToFloat(correctionRate)); - - FV3_Load(&correction, - FixedMul(gravityDelta.x, FixedMul(deltaseconds, correctionRate)), - FixedMul(gravityDelta.y, FixedMul(deltaseconds, correctionRate)), - FixedMul(gravityDelta.z, FixedMul(deltaseconds, correctionRate)) - ); - if ((FV3_LengthSquared(&correction) < FV3_LengthSquared(&gravityDelta))) - { - FV3_Add(&localgravityvectors[p], &correction); - } - else - { - FV3_Load(&localgravityvectors[p], invAccel.x, invAccel.y, invAccel.z); - } -} - -INT32 G_GetGamepadTilt(INT32 p) -{ - fixed_t tilt; - fixed_t curve; - if (!G_GetGamepadCanUseTilt(p)) return 0; - tilt = CLAMP(FixedDiv(G_GetGamepadGravity(p).x + MAXGAMEPADTILT, 2*MAXGAMEPADTILT), 0, FRACUNIT); - CONS_Debug(DBG_IMU, "Tilt: %4.2f\n", FixedToFloat(tilt)); - - curve = FINESINE(FixedAngle(180*(tilt-FRACUNIT/2))>>ANGLETOFINESHIFT); - CONS_Debug(DBG_IMU, "Pinched Tilt: %4.2f\n", FixedToFloat(curve)); - - return (JOYAXISRANGE * curve)/FRACUNIT; -} - -vector3_t G_GetGamepadGravity(INT32 p) -{ - const vector3_t zero = {0, -ACCELEROMETERGRAVITY, 0}; - if (!G_GetGamepadCanUseTilt(p)) return zero; - return localgravityvectors[p]; -} - -vector3_t G_GetGamepadShake(INT32 p) -{ - vector3_t accel = {0}; - if (!G_GetGamepadCanUseTilt(p)) return accel; - accel = G_PlayerInputSensor(p, ACCELEROMETER); - FV3_Add(&accel, &localgravityvectors[p]); - - return accel; -} - -boolean G_GetGamepadCanUseTilt(INT32 p) -{ - if (p >= MAXSPLITSCREENPLAYERS) - { -#ifdef PARANOIA - CONS_Debug(DBG_GAMELOGIC, "G_GetGamepadCanUseTilt: Invalid player ID %d\n", p); -#endif - return false; - } - - return (I_ControllerSupportsSensorAccelerometer(p) && I_ControllerSupportsSensorGyro(p)); -} -#undef ShakinessMaxThreshold -#undef ShakinessMinThreshold -#undef CorrectionStillRate -#undef CorrectionShakyRate -#undef CorrectionGyroFactor -#undef CorrectionGyroMinThreshold -#undef CorrectionGyroMaxThreshold -#undef CorrectionMinimumSpeed - // // G_BuildTiccmd // Builds a ticcmd from all of the available inputs @@ -1699,7 +1469,12 @@ void G_BuildTiccmd(ticcmd_t *cmd, INT32 realtics, UINT8 ssplayer) { vector3_t accel = G_PlayerInputSensor(forplayer, ACCELEROMETER); vector3_t gyro = G_PlayerInputSensor(forplayer, GYROSCOPE); - G_UpdateGamepadGravity(forplayer, gyro, accel); + + G_UpdateGamepadAutoCalibration(forplayer, accel, gyro, + (paused || P_AutoPause() || (menustack[0] == MN_OP_CONTROLSETUP))); + + G_UpdateGamepadGyro(forplayer, gyro); + G_UpdateGamepadGravity(forplayer, G_GetGamepadCalibratedGyro(forplayer), accel); } // why build a ticcmd if we're paused? @@ -1783,9 +1558,15 @@ void G_BuildTiccmd(ticcmd_t *cmd, INT32 realtics, UINT8 ssplayer) side += (accelerometertilt * 4) / JOYAXISRANGE; //todo: control spectator camera using the gyro - // if (spectating) - // { - // } + if (spectating) + { + fixed_t deltaseconds = FixedDiv(FRACUNIT, max(cv_timescale.value, FRACUNIT/20))/TICRATE; + vector3_t gyro = G_GetGamepadCalibratedGyro(forplayer); + cmd->turning -= (FixedMul(FixedMul(gyro.y, M_PI_FIXED/180), deltaseconds) * 1) * (encoremode ? -1 : 1); + cmd->angle -= (FixedMul(FixedMul(gyro.y, M_PI_FIXED/180), deltaseconds) * 1) * (encoremode ? -1 : 1); + + cmd->aiming -= (FixedMul(FixedMul(gyro.x, M_PI_FIXED/180), deltaseconds) * 1); + } } else if (joystickvector.xaxis != 0) { @@ -2001,8 +1782,6 @@ void G_BuildTiccmd(ticcmd_t *cmd, INT32 realtics, UINT8 ssplayer) if (abs(tilt) > MAXGAMEPADTILT) cmd->flags |= TICCMD_EXCESSTILT; } - - CONS_Debug(DBG_IMU, "Shake: %4.2f\n", FixedToFloat(FV3_Length(&shake))); //cmd->shake = CLAMP((GAMEPADSHAKETHRESHOLD*FV3_Length(&shake))/FRACUNIT, 0, UINT8_MAX); //CONS_Debug(DBG_IMU, "CMD Tilt: %d, Shake: %d\n", cmd->tilt, cmd->shake); diff --git a/src/g_game.h b/src/g_game.h index 2b282b72a..dffc38507 100644 --- a/src/g_game.h +++ b/src/g_game.h @@ -121,6 +121,9 @@ mapnum_t G_LevelTitleToMapNum(const char * leveltitle); mapnum_t G_KartMapToNative(mapnum_t mapnum); mapnum_t G_NativeMapToKart(mapnum_t mapnum); +#define GAMEPADSHAKETHRESHOLD (UINT8_MAX/2) +#define TILTTOSTICKEASE 6 + void G_ResetAnglePrediction(player_t *player); void G_BuildTiccmd(ticcmd_t *cmd, INT32 realtics, UINT8 ssplayer); @@ -137,31 +140,6 @@ void G_FinalClipAimingPitch(INT32 *aiming, player_t *player, boolean skybox); extern angle_t localangle[MAXSPLITSCREENPLAYERS]; extern INT32 localaiming[MAXSPLITSCREENPLAYERS]; // should be an angle_t but signed -typedef enum -{ - DEADZONE_X, - DEADZONE_Y, - DEADZONE_BUTTON, -} analogdeadzone_e; - -typedef enum -{ - ACCELEROMETER, - GYROSCOPE, -} motionsensortype_e; - -#define MAXGAMEPADTILT (50*FRACUNIT/100) -// #define ACCELEROMETERGRAVITY ((fixed_t)(9.80665f * ((float)FRACUNIT))) -#define ACCELEROMETERGRAVITY 642688 -#define GAMEPADSHAKETHRESHOLD (UINT8_MAX/2) -#define TILTTOSTICKEASE 6 -boolean G_GetGamepadCanUseTilt(INT32 p); -void G_UpdateGamepadGravity(INT32 p, vector3_t gyro, vector3_t accel); -INT32 G_GetGamepadTilt(INT32 p); -vector3_t G_GetGamepadShake(INT32 p); -vector3_t G_GetGamepadGravity(INT32 p); -vector3_t G_PlayerInputSensor(UINT8 p, motionsensortype_e sensor); - fixed_t G_GetDeadZoneType(INT32 p, SINT8 type); INT32 G_PlayerInputAnalog(UINT8 p, INT32 gc, boolean digital, SINT8 type); boolean G_PlayerInputDown(UINT8 p, INT32 gc, boolean digital, SINT8 type); diff --git a/src/g_input.c b/src/g_input.c index 993679ed0..4e01a3bfa 100644 --- a/src/g_input.c +++ b/src/g_input.c @@ -26,6 +26,8 @@ #include "v_video.h" #include "p_local.h" #include "k_kart.h" +#include "m_fixed.h" +#include "m_easing.h" #define MAXMOUSESENSITIVITY 100 // sensitivity steps @@ -1226,3 +1228,331 @@ void Command_Setcontrol4_f(void) setcontrol(3); } + +// accelerometer and gyro stuff + +// when holding the controller still (shaking and turning included), correct this quickly to resolve error +#define GyroCalibrationTrust (1*FRACUNIT/100) + +#define CorrectionInstantRate (80*FRACUNIT/100) +#define CorrectionInstantShake (4*FRACUNIT/100) +#define CorrectionInstantTurn (5*FRACUNIT/100) + +boolean localgyrocalibrating[MAXSPLITSCREENPLAYERS]; +vector3_t localgyrovectors[MAXSPLITSCREENPLAYERS]; + +vector3_t localgyrocalibrationoffset[MAXSPLITSCREENPLAYERS]; + +// assume the accelerometer doesn't need calibration, use this to determine if gyro can be calibrated +vector3_t localaccelcalibrationoffset[MAXSPLITSCREENPLAYERS]; + +fixed_t localshakinessfac[MAXSPLITSCREENPLAYERS]; +vector3_t localsmoothedaccel[MAXSPLITSCREENPLAYERS]; +vector3_t localgravityvectors[MAXSPLITSCREENPLAYERS]; + +// copy/pasted from the lua version of these routines +inline static vector3_t *QuaternionMulVec3(vector3_t *out, vector3_t *a, vector4_t *b) +{ + fixed_t ax = a->x, ay = a->y, az = a->z, aw = 0; + fixed_t bx = b->x, by = b->y, bz = b->z, bw = b->a; + + FV3_NormalizeEx(out, FV3_Load(out, + FixedMul(aw, bx) + FixedMul(ax, bw) + FixedMul(ay, bz) - FixedMul(az, by), + FixedMul(aw, by) - FixedMul(ax, bz) + FixedMul(ay, bw) + FixedMul(az, bx), + FixedMul(aw, bz) + FixedMul(ax, by) - FixedMul(ay, bx) + FixedMul(az, bw) + )); + + return out; +} + +inline static fixed_t FV3_LengthSquared(const vector3_t *vec) +{ + return FixedMul(vec->x, vec->x) + FixedMul(vec->y, vec->y) + FixedMul(vec->z, vec->z); +} + +inline static fixed_t FV3_Length(const vector3_t *vec) +{ + return FixedSqrt(FV3_LengthSquared(vec)); +} + +inline static vector4_t AngleAxis(fixed_t angle, fixed_t x, fixed_t y, fixed_t z) +{ + fixed_t sinangle = FINESINE(FixedAngle(angle/2) >> ANGLETOFINESHIFT); + fixed_t cosangle = FINECOSINE(FixedAngle(angle/2) >> ANGLETOFINESHIFT); + vector3_t axis = {x, y, z}; + vector4_t result; + + FV3_Normalize(&axis); + + FV4_Load(&result, + FixedMul(axis.x, sinangle), + FixedMul(axis.y, sinangle), + FixedMul(axis.z, sinangle), + cosangle + ); + + return result; +} + +vector3_t G_GetCalibratedGyroOffset(INT32 p) +{ + return localgyrocalibrationoffset[p]; +} + +void G_UpdateGamepadAutoCalibration(INT32 p, vector3_t accel, vector3_t gyro, boolean allowautocalibration) +{ + fixed_t trust = FV3_Distance(&localaccelcalibrationoffset[p], &accel); + FV3_Load( + &localaccelcalibrationoffset[p], + (localaccelcalibrationoffset[p].x + accel.x)/2, + (localaccelcalibrationoffset[p].y + accel.y)/2, + (localaccelcalibrationoffset[p].z + accel.z)/2 + ); + + CONS_Debug(DBG_IMU, "== Gyro Update ==\n"); + CONS_Debug(DBG_IMU, "Gyro calibration safety: %4.3f\n", FixedToFloat(trust)); + if (allowautocalibration && (trust < GyroCalibrationTrust)) + { + if (!localgyrocalibrating[p]) + { + FV3_Load( + &localgyrocalibrationoffset[p], + 0, 0, 0 + ); + localgyrocalibrating[p] = true; + } + } + else + { + localgyrocalibrating[p] = false; + } +} + +void G_UpdateGamepadGyro(INT32 p, vector3_t gyro) +{ + vector3_t offset = {0}; + + if (p >= MAXSPLITSCREENPLAYERS) + { +#ifdef PARANOIA + CONS_Debug(DBG_GAMELOGIC, "G_UpdateGamepadGyro: Invalid player ID %d\n", p); +#endif + return; + } + if (!I_ControllerSupportsSensorGyro(p)) return; + + if (localgyrocalibrating[p]) + { + FV3_Load( + &localgyrocalibrationoffset[p], + ((3*localgyrocalibrationoffset[p].x) + gyro.x)/4, + ((3*localgyrocalibrationoffset[p].y) + gyro.y)/4, + ((3*localgyrocalibrationoffset[p].z) + gyro.z)/4 + ); + } + offset = G_GetCalibratedGyroOffset(p); + FV3_SubEx(&gyro, &offset, &localgyrovectors[p]); +} + +// math sourced from this article +// http://gyrowiki.jibbsmart.com/blog:finding-gravity-with-sensor-fusion + +// the time it takes in our acceleration smoothing for 'A' to get halfway to 'B' +#define SmoothingHalfTime (0.01) +// thresholds of trust for accel shakiness. less shakiness = more trust +#define ShakinessMaxThreshold (50*FRACUNIT/100) +#define ShakinessMinThreshold (1*FRACUNIT/100) +// when we trust the accel a lot (the controller is "still"), how quickly do we correct our gravity vector? +#define CorrectionStillRate (FRACUNIT) +// when we don't trust the accel (the controller is "shaky"), how quickly do we correct our gravity vector? +#define CorrectionShakyRate (10*FRACUNIT/100) +// if our old gravity vector is close enough to our new one, limit further corrections to this proportion of the rotation speed +#define CorrectionGyroFactor (40*FRACUNIT/100) +// thresholds for what's considered "close enough" +#define CorrectionGyroMinThreshold (5*FRACUNIT/100) +#define CorrectionGyroMaxThreshold (FRACUNIT/4) +// no matter what, always apply a minimum of this much correction to our gravity vector +#define CorrectionMinimumSpeed (1*FRACUNIT/100) + +void G_UpdateGamepadGravity(INT32 p, vector3_t gyro, vector3_t accel) +{ + // convert gyro input to reverse rotation + vector3_t invAccel = {-accel.x, -accel.y, -accel.z}; + fixed_t correctionRate = 0; + // scaling is reversed, smaller time scales = larger steps in this code + // (1/timescale)/dt + fixed_t deltaseconds = FixedDiv(FRACUNIT, max(cv_timescale.value, FRACUNIT/20))/TICRATE; + // we don't have exp2 and we actually need it here to take timescale into account :( + fixed_t smoothFactor = FloatToFixed(exp2(-FixedToFloat(deltaseconds) / SmoothingHalfTime)); + fixed_t angleRate; + fixed_t correctionLimit; + vector4_t invRotation = AngleAxis( + FixedMul(FV3_Length(&gyro), deltaseconds), + -gyro.x, + -gyro.y, + -gyro.z + ); + vector3_t gravityDelta = {0}; + vector3_t gravityDeltaDirection = {0}; + vector3_t correction = {0}; + + if (!G_GetGamepadCanUseTilt(p)) return; + CONS_Debug(DBG_IMU, "= Update Gravity Delta Time: %4.3f =\n", FixedToFloat(deltaseconds)); + + // rotate gravity vector + QuaternionMulVec3(&localgravityvectors[p], &localgravityvectors[p], &invRotation); + QuaternionMulVec3(&localsmoothedaccel[p], &localsmoothedaccel[p], &invRotation); + localshakinessfac[p] = FixedMul(localshakinessfac[p], smoothFactor), + FV3_SubEx(&accel, &localsmoothedaccel[p], &correction); + localshakinessfac[p] = max(localshakinessfac[p], FV3_Length(&correction)); + FV3_Load(&localsmoothedaccel[p], + Easing_Linear(smoothFactor, accel.x, localsmoothedaccel[p].x), + Easing_Linear(smoothFactor, accel.y, localsmoothedaccel[p].y), + Easing_Linear(smoothFactor, accel.z, localsmoothedaccel[p].z) + ); + + CONS_Debug(DBG_IMU, "Shakiness: %4.2f\n", FixedToFloat(localshakinessfac[p])); + + FV3_SubEx(&invAccel, &localgravityvectors[p], &gravityDelta); + FV3_NormalizeEx(&gravityDelta, &gravityDeltaDirection); + + if (ShakinessMaxThreshold > ShakinessMinThreshold) + { + fixed_t stillness = CLAMP(FixedDiv((localshakinessfac[p] - ShakinessMinThreshold), (ShakinessMaxThreshold - ShakinessMinThreshold)), 0, FRACUNIT); + correctionRate = CorrectionStillRate + FixedMul((CorrectionShakyRate - CorrectionStillRate), stillness); + } + else if (localshakinessfac[p] > ShakinessMaxThreshold) + { + correctionRate = CorrectionShakyRate; + } + else + { + correctionRate = CorrectionStillRate; + } + + // limit in proportion to rotation rate + angleRate = FixedMul(FV3_Length(&gyro), M_PI_FIXED/180); + correctionLimit = max(FixedMul(FixedMul(angleRate, FV3_Length(&localgravityvectors[p])), CorrectionGyroFactor), CorrectionMinimumSpeed); + + CONS_Debug(DBG_IMU, "Angle Rate: %4.3f\n", FixedToFloat(angleRate)); + CONS_Debug(DBG_IMU, "Correction Limit: %4.3f\n", FixedToFloat(correctionLimit)); + + if (correctionRate > correctionLimit) { + fixed_t closeEnoughFactor; + if (CorrectionGyroMaxThreshold > CorrectionGyroMinThreshold) + { + closeEnoughFactor = CLAMP(FixedDiv((FV3_Length(&gravityDelta) - CorrectionGyroMinThreshold), (CorrectionGyroMaxThreshold - CorrectionGyroMinThreshold)), 0, FRACUNIT); + } + else if (FV3_Length(&gravityDelta) > CorrectionGyroMaxThreshold) + { + closeEnoughFactor = FRACUNIT; + } + else + { + closeEnoughFactor = 0; + } + + CONS_Debug(DBG_IMU, "'Close Enough' Fac: %4.3f\n", FixedToFloat(closeEnoughFactor)); + correctionRate = correctionLimit + FixedMul((correctionRate - correctionLimit), closeEnoughFactor); + } + + if (localshakinessfac[p] < CorrectionInstantShake && angleRate < CorrectionInstantTurn) + { + correctionRate = max(correctionRate, CorrectionInstantRate); + } + + CONS_Debug(DBG_IMU, "Correction Rate: %4.2f\n", FixedToFloat(correctionRate)); + + FV3_Load(&correction, + FixedMul(gravityDelta.x, FixedMul(deltaseconds, correctionRate)), + FixedMul(gravityDelta.y, FixedMul(deltaseconds, correctionRate)), + FixedMul(gravityDelta.z, FixedMul(deltaseconds, correctionRate)) + ); + if ((FV3_LengthSquared(&correction) < FV3_LengthSquared(&gravityDelta))) + { + FV3_Add(&localgravityvectors[p], &correction); + } + else + { + FV3_Load(&localgravityvectors[p], invAccel.x, invAccel.y, invAccel.z); + } +} + +INT32 G_GetGamepadTilt(INT32 p) +{ + fixed_t tilt; + fixed_t curve; + if (!G_GetGamepadCanUseTilt(p)) return 0; + tilt = CLAMP(FixedDiv(G_GetGamepadGravity(p).x + MAXGAMEPADTILT, 2*MAXGAMEPADTILT), 0, FRACUNIT); + CONS_Debug(DBG_IMU, "Tilt: %4.2f\n", FixedToFloat(tilt)); + + curve = FINESINE(FixedAngle(180*(tilt-FRACUNIT/2))>>ANGLETOFINESHIFT); + CONS_Debug(DBG_IMU, "Pinched Tilt: %4.2f\n", FixedToFloat(curve)); + + return (JOYAXISRANGE * curve)/FRACUNIT; +} + +vector3_t G_GetGamepadGravity(INT32 p) +{ + const vector3_t zero = {0, -ACCELEROMETERGRAVITY, 0}; + if (!G_GetGamepadCanUseTilt(p)) return zero; + return localgravityvectors[p]; +} + +vector3_t G_GetGamepadShake(INT32 p) +{ + vector3_t accel = {0}; + if (!G_GetGamepadCanUseTilt(p)) return accel; + accel = G_PlayerInputSensor(p, ACCELEROMETER); + FV3_Add(&accel, &localgravityvectors[p]); + + return accel; +} + +fixed_t G_GetGamepadShakinessFactor(INT32 p) +{ + if (!G_GetGamepadCanUseTilt(p)) return 0; + return localshakinessfac[p]; +} + +vector3_t G_GetGamepadCalibratedGyro(INT32 p) +{ + vector3_t zero = {0}; + if (p >= MAXSPLITSCREENPLAYERS) + { +#ifdef PARANOIA + CONS_Debug(DBG_GAMELOGIC, "G_GetGamepadGyro: Invalid player ID %d\n", p); +#endif + return zero; + } + + if (!I_ControllerSupportsSensorGyro(p)) return zero; + + return localgyrovectors[p]; +} + +boolean G_GetGamepadCanUseTilt(INT32 p) +{ + if (p >= MAXSPLITSCREENPLAYERS) + { +#ifdef PARANOIA + CONS_Debug(DBG_GAMELOGIC, "G_GetGamepadCanUseTilt: Invalid player ID %d\n", p); +#endif + return false; + } + + return (I_ControllerSupportsSensorAccelerometer(p) && I_ControllerSupportsSensorGyro(p)); +} +#undef ShakinessMaxThreshold +#undef ShakinessMinThreshold +#undef CorrectionStillRate +#undef CorrectionShakyRate +#undef CorrectionGyroFactor +#undef CorrectionGyroMinThreshold +#undef CorrectionGyroMaxThreshold +#undef CorrectionMinimumSpeed + +#undef CorrectionInstantRate +#undef CorrectionInstantShake +#undef CorrectionInstantTurn +#undef GyroCalibrationTrust \ No newline at end of file diff --git a/src/g_input.h b/src/g_input.h index 02bd76b7e..09fc49f5c 100644 --- a/src/g_input.h +++ b/src/g_input.h @@ -98,6 +98,19 @@ typedef enum num_gamecontrols } gamecontrols_e; +typedef enum +{ + DEADZONE_X, + DEADZONE_Y, + DEADZONE_BUTTON, +} analogdeadzone_e; + +typedef enum +{ + ACCELEROMETER, + GYROSCOPE, +} motionsensortype_e; + // mouse values are used once extern consvar_t cv_mousesens, cv_mouseysens; extern consvar_t cv_mousesens2, cv_mouseysens2; @@ -178,6 +191,22 @@ void G_ResetControls(UINT8 p); void G_SaveKeySetting(FILE *f, INT32 (*fromcontrolsa)[MAXINPUTMAPPING], INT32 (*fromcontrolsb)[MAXINPUTMAPPING], INT32 (*fromcontrolsc)[MAXINPUTMAPPING], INT32 (*fromcontrolsd)[MAXINPUTMAPPING]); INT32 G_CheckDoubleUsage(INT32 keynum, INT32 playernum, boolean modify); +#define MAXGAMEPADTILT (50*FRACUNIT/100) +// #define ACCELEROMETERGRAVITY ((fixed_t)(9.80665f * ((float)FRACUNIT))) +#define ACCELEROMETERGRAVITY 642688 +boolean G_GetGamepadCanUseTilt(INT32 p); +void G_ResetGyroCalibration(INT32 p); +void G_UpdateGamepadAutoCalibration(INT32 p, vector3_t accel, vector3_t gyro, boolean allowautocalibration); +void G_UpdateGamepadGyro(INT32 p, vector3_t gyro); +void G_UpdateGamepadGravity(INT32 p, vector3_t gyro, vector3_t accel); +fixed_t G_GetGamepadShakinessFactor(INT32 p); +vector3_t G_GetGamepadShake(INT32 p); +vector3_t G_GetGamepadGravity(INT32 p); +vector3_t G_GetCalibratedGyroOffset(INT32 p); +vector3_t G_GetGamepadCalibratedGyro(INT32 p); +vector3_t G_PlayerInputSensor(UINT8 p, motionsensortype_e sensor); +INT32 G_GetGamepadTilt(INT32 p); + #ifdef __cplusplus } // extern "C" #endif diff --git a/src/k_hud.c b/src/k_hud.c index 19b515b39..0f039e5b7 100644 --- a/src/k_hud.c +++ b/src/k_hud.c @@ -32,6 +32,7 @@ #include "doomstat.h" #include "d_clisrv.h" #include "g_game.h" +#include "g_input.h" #include "p_local.h" #include "z_zone.h" #include "m_cond.h" diff --git a/src/st_stuff.c b/src/st_stuff.c index 84e74def2..0f5d84114 100644 --- a/src/st_stuff.c +++ b/src/st_stuff.c @@ -475,15 +475,10 @@ static void ST_drawMusicDebug(INT32 *height) ST_pushDebugString(height, va(" Song: %8s", mname)); } -static INT32 ST_getFixedFrac(fixed_t val) -{ - return ((abs(val) * 100)>>FRACBITS) % 100; -} - static void ST_drawImuDebug(INT32 *height) { INT32 pnum = stplyrnum; - vector3_t accel, gyro, grav, shake; + vector3_t accel, gyro, gyrocalibrated, grav, shake; if (demo.playback || (!P_IsMachineLocalPlayer(stplyr))) { ST_pushDebugString(height, va("Only for local player!!")); @@ -491,14 +486,16 @@ static void ST_drawImuDebug(INT32 *height) } accel = G_PlayerInputSensor(pnum, ACCELEROMETER); - gyro = G_PlayerInputSensor(pnum, GYROSCOPE); - grav = G_GetGamepadGravity(pnum); + gyro = G_PlayerInputSensor(pnum, GYROSCOPE); + gyrocalibrated = G_GetGamepadCalibratedGyro(pnum); + grav = G_GetGamepadGravity(pnum); shake = G_GetGamepadShake(pnum); - ST_pushDebugString(height, va(" Gyro :%4.2f, %4.2f,%4.2f", FixedToFloat(gyro.x), FixedToFloat(gyro.y), FixedToFloat(gyro.z))); - ST_pushDebugString(height, va("Accel :%4.2f, %4.2f,%4.2f", FixedToFloat(accel.x), FixedToFloat(accel.y), FixedToFloat(accel.z))); - ST_pushDebugString(height, va("Shake :%4.2f, %4.2f,%4.2f", FixedToFloat(shake.x), FixedToFloat(shake.y), FixedToFloat(shake.z))); - ST_pushDebugString(height, va(" Grav :%4.2f, %4.2f,%4.2f", FixedToFloat(grav.x), FixedToFloat(grav.y), FixedToFloat(grav.z))); + ST_pushDebugString(height, va(" Grav : %4.2f, %4.2f,%4.2f", FixedToFloat(grav.x), FixedToFloat(grav.y), FixedToFloat(grav.z))); + ST_pushDebugString(height, va(" Shake : %4.2f, %4.2f,%4.2f", FixedToFloat(shake.x), FixedToFloat(shake.y), FixedToFloat(shake.z))); + ST_pushDebugString(height, va("Cal. Gyro: %4.2f, %4.2f,%4.2f", FixedToFloat(gyrocalibrated.x), FixedToFloat(gyrocalibrated.y), FixedToFloat(gyrocalibrated.z))); + ST_pushDebugString(height, va(" Gyro : %4.2f, %4.2f,%4.2f", FixedToFloat(gyro.x), FixedToFloat(gyro.y), FixedToFloat(gyro.z))); + ST_pushDebugString(height, va(" Accel : %4.2f, %4.2f,%4.2f", FixedToFloat(accel.x), FixedToFloat(accel.y), FixedToFloat(accel.z))); } static void ST_drawRenderDebug(INT32 *height)