diff --git a/src/d_main.cpp b/src/d_main.cpp index 0362fddb7..63a725e63 100644 --- a/src/d_main.cpp +++ b/src/d_main.cpp @@ -220,6 +220,13 @@ void D_ProcessEvents(void) // i have to reset this somewhere or else your camera just glides away! for (size_t i = 0; i < 4; i++) gamekeydown[0][KEY_MOUSEMOVE + i] = 0; + + // for (size_t dev = 0; dev < MAXDEVICES; dev++) + // { + // gamekeydown[dev][KEY_GYROSCOPE1+0] = 0; + // gamekeydown[dev][KEY_GYROSCOPE1+1] = 0; + // gamekeydown[dev][KEY_GYROSCOPE1+2] = 0; + // } for (; eventtail != eventhead; eventtail = (eventtail+1) & (MAXEVENTS-1)) { diff --git a/src/g_game.c b/src/g_game.c index 4484dbd07..31b7216a8 100644 --- a/src/g_game.c +++ b/src/g_game.c @@ -1450,6 +1450,16 @@ static vector3_t *QuaternionMulVec3(vector3_t *out, vector3_t *a, vector4_t *b) return out; } +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); +} + +static fixed_t FV3_Length(const vector3_t *vec) +{ + return FixedSqrt(FV3_LengthSquared(vec)); +} + static vector4_t *QuaternionInvert(vector4_t *out) { FV4_Load(out, @@ -1483,113 +1493,135 @@ inline static vector4_t AngleAxis(fixed_t angle, fixed_t x, fixed_t y, fixed_t z // math sourced from this article // http://gyrowiki.jibbsmart.com/blog:finding-gravity-with-sensor-fusion -#define Interpolator (FRACUNIT/4) +// state +vector3_t localgravityvectors[MAXSPLITSCREENPLAYERS]; +void G_UpdateGamepadGravity(INT32 p, vector3_t gyro, vector3_t accel) +{ + vector3_t invAccel = {-accel.x, -accel.y, -accel.z}; + fixed_t angleRate = FixedMul(FV3_Length(&gyro), FRACUNIT/TICRATE); + vector4_t invRotation = AngleAxis( + angleRate, + -gyro.x, + -gyro.y, + -gyro.z + ); + + // update gravity by rotation + QuaternionMulVec3(&localgravityvectors[p], &localgravityvectors[p], &invRotation); + + // nudge towards gravity according to current acceleration + FV3_Load(&localgravityvectors[p], + localgravityvectors[p].x + FixedMul(invAccel.x - localgravityvectors[p].x, FRACUNIT/10), + localgravityvectors[p].y + FixedMul(invAccel.y - localgravityvectors[p].y, FRACUNIT/10), + localgravityvectors[p].z + FixedMul(invAccel.z - localgravityvectors[p].z, FRACUNIT/10) + ); +} + +#if 0 // thresholds of trust for accel shakiness. less shakiness = more trust -#define ShakinessMaxThreshold (4*FRACUNIT/10) +#define ShakinessMaxThreshold (9*FRACUNIT/10) #define ShakinessMinThreshold (FRACUNIT/10) // 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 (FRACUNIT/10) // if our old gravity vector is close enough to our new one, limit further corrections to this proportion of the rotation speed -#define CorrectionGyroFactor (FRACUNIT/10) +#define CorrectionGyroFactor (FRACUNIT/5) // 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 (FRACUNIT/10) -// state fixed_t localshakinessfac[MAXSPLITSCREENPLAYERS]; vector3_t localsmoothedaccel[MAXSPLITSCREENPLAYERS]; -vector3_t localgravityvectors[MAXSPLITSCREENPLAYERS]; void G_UpdateGamepadGravity(INT32 p, vector3_t gyro, vector3_t accel) { // convert gyro input to reverse rotation - const vector3_t invAccel = {-accel.x, -accel.y, -accel.z}; - fixed_t correctionRate = CorrectionMinimumSpeed; - fixed_t correctionMagnitude, gravityDeltaMagnitude, angleRate, correctionLimit; + const fixed_t smoothFactor = 924*FRACUNIT/1000; + vector3_t invAccelNorm = {-accel.x, -accel.y, -accel.z}; + fixed_t gravCorrectionRate = 0; + fixed_t angleRate; + fixed_t correctionLimit; vector4_t invRotation = AngleAxis( + FixedMul(FV3_Length(&gyro), FRACUNIT/TICRATE), -gyro.x, -gyro.y, - -gyro.z, - FixedMul(FV3_Magnitude(&gyro), FRACUNIT/TICRATE) + -gyro.z ); - vector3_t gravityDelta = {0}; - vector3_t gravityDeltaDirection = {0}; + vector3_t gravityToAccel = {0}; + vector3_t gravityToAccelDirection = {0}; vector3_t fv3_temp = {0}; - - // rotate gravity vector - QuaternionMulVec3(&localgravityvectors[p], &localgravityvectors[p], &invRotation); - QuaternionMulVec3(&localsmoothedaccel[p], &localsmoothedaccel[p], &invRotation); - FV3_SubEx(&accel, &localsmoothedaccel[p], &fv3_temp); - localshakinessfac[p] = max( - FixedMul(localshakinessfac[p], Interpolator), - FV3_Magnitude(&fv3_temp) - ); - FV3_Load(&localsmoothedaccel[p], - Easing_Linear(Interpolator, accel.x, localsmoothedaccel[p].x), - Easing_Linear(Interpolator, accel.y, localsmoothedaccel[p].y), - Easing_Linear(Interpolator, accel.z, localsmoothedaccel[p].z) - ); - FV3_SubEx(&invAccel, &localgravityvectors[p], &gravityDelta); - FV3_NormalizeEx(&gravityDelta, &gravityDeltaDirection); + if (FV3_Length(&accel) <= 0) + { + QuaternionMulVec3(&localgravityvectors[p], &localgravityvectors[p], &invRotation); + return; + } + + // rotate gravity vector + QuaternionMulVec3(&localsmoothedaccel[p], &localsmoothedaccel[p], &invRotation); + localshakinessfac[p] = FixedMul(localshakinessfac[p], smoothFactor), + FV3_SubEx(&accel, &localsmoothedaccel[p], &fv3_temp); + localshakinessfac[p] = max(localshakinessfac[p], FV3_Length(&fv3_temp)); + 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])); + + // update gravity by rotation + QuaternionMulVec3(&localgravityvectors[p], &localgravityvectors[p], &invRotation); + FV3_SubEx(&invAccelNorm, &localgravityvectors[p], &gravityToAccel); + FV3_NormalizeEx(&gravityToAccel, &gravityToAccelDirection); if (ShakinessMaxThreshold > ShakinessMinThreshold) { fixed_t stillness = CLAMP(FixedDiv((localshakinessfac[p] - ShakinessMinThreshold), (ShakinessMaxThreshold - ShakinessMinThreshold)), 0, FRACUNIT); - correctionRate = CorrectionStillRate + FixedMul(stillness, (CorrectionShakyRate - CorrectionStillRate)); - } - else if (localshakinessfac[p] > ShakinessMaxThreshold) - { - correctionRate = CorrectionShakyRate; + gravCorrectionRate = CorrectionStillRate + FixedMul((CorrectionShakyRate - CorrectionStillRate), stillness); } else { - correctionRate = CorrectionStillRate; + gravCorrectionRate = localshakinessfac[p] < ShakinessMaxThreshold ? CorrectionStillRate : CorrectionShakyRate; } // limit in proportion to rotation rate - angleRate = FixedMul(FV3_Magnitude(&gyro), M_PI_FIXED)/180; - correctionLimit = FixedMul(FixedMul(angleRate, FV3_Magnitude(&localgravityvectors[p])), CorrectionGyroFactor); - if (correctionRate > correctionLimit) { + angleRate = FixedMul(FixedMul(FV3_Length(&gyro), M_PI_FIXED)/180, FRACUNIT/TICRATE); + correctionLimit = max(FixedMul(angleRate, CorrectionGyroFactor), CorrectionMinimumSpeed); + if (gravCorrectionRate > correctionLimit) { fixed_t closeEnoughFactor; if (CorrectionGyroMaxThreshold > CorrectionGyroMinThreshold) { - closeEnoughFactor = CLAMP(FixedDiv((FV3_Magnitude(&gravityDelta) - CorrectionGyroMinThreshold), (CorrectionGyroMaxThreshold - CorrectionGyroMinThreshold)), 0, FRACUNIT); - } - else if (FV3_Magnitude(&gravityDelta) > CorrectionGyroMaxThreshold) - { - closeEnoughFactor = FRACUNIT; - } + closeEnoughFactor = CLAMP(FixedDiv((FV3_Length(&gravityToAccel) - CorrectionGyroMinThreshold), (CorrectionGyroMaxThreshold - CorrectionGyroMinThreshold)), 0, FRACUNIT); + } else { - closeEnoughFactor = 0; + closeEnoughFactor = FV3_Length(&gravityToAccel) < CorrectionGyroMaxThreshold ? 0 : FRACUNIT; } - correctionRate = correctionRate + FixedMul((correctionLimit - correctionRate), closeEnoughFactor); + gravCorrectionRate = correctionLimit + FixedMul((gravCorrectionRate - correctionLimit), closeEnoughFactor); } + + CONS_Debug(DBG_IMU, "Correction Rate: %4.2f\n", FixedToFloat(gravCorrectionRate)); - // finally, let's always allow a little bit of correction - correctionRate = max(correctionRate, CorrectionMinimumSpeed); - - FV3_Load(&gravityDeltaDirection, - FixedMul(FixedMul(correctionRate, FRACUNIT/TICRATE), gravityDeltaDirection.x), - FixedMul(FixedMul(correctionRate, FRACUNIT/TICRATE), gravityDeltaDirection.y), - FixedMul(FixedMul(correctionRate, FRACUNIT/TICRATE), gravityDeltaDirection.z) + FV3_Load(&fv3_temp, + FixedMul(gravityToAccelDirection.x, FixedMul(gravCorrectionRate, FRACUNIT/TICRATE)), + FixedMul(gravityToAccelDirection.y, FixedMul(gravCorrectionRate, FRACUNIT/TICRATE)), + FixedMul(gravityToAccelDirection.z, FixedMul(gravCorrectionRate, FRACUNIT/TICRATE)) ); - correctionMagnitude = FV3_Magnitude(&gravityDeltaDirection); - gravityDeltaMagnitude = FV3_Magnitude(&gravityDelta); - if (FixedMul(correctionMagnitude, correctionMagnitude) > FixedMul(gravityDeltaMagnitude, gravityDeltaMagnitude)) + if (FV3_LengthSquared(&fv3_temp) < FV3_LengthSquared(&gravityToAccel)) { - FV3_Add(&localgravityvectors[p], &gravityDeltaDirection); + FV3_Add(&localgravityvectors[p], &fv3_temp); } else { - FV3_Add(&localgravityvectors[p], &gravityDelta); + FV3_Normalize(&invAccelNorm); + FV3_Add(&localgravityvectors[p], &invAccelNorm); } } +#endif #define TILTRANGE 35*FRACUNIT fixed_t G_GetGamepadTilt(INT32 p) diff --git a/src/sdl/i_video.cpp b/src/sdl/i_video.cpp index 5ce00126b..2077b6fa9 100644 --- a/src/sdl/i_video.cpp +++ b/src/sdl/i_video.cpp @@ -1070,7 +1070,7 @@ static void Impl_HandleControllerSensorEvent(SDL_GamepadSensorEvent evt) // data[2]: delta roll in rad/s // we convert to degrees per second before passing it to the event case SDL_SENSOR_GYRO: -#define RAD2DEG 57.295779513f +#define RAD2DEG -57.295779513f // CONS_Debug(DBG_IMU, "Got Gyro event %4.2f %4.2f %4.2f\n", RAD2DEG * evt.data[0], RAD2DEG * evt.data[1], RAD2DEG * evt.data[2]); event.type = ev_gyroscope; event.data1 = FLOAT_TO_FIXED(RAD2DEG * evt.data[0]);