gyro calibration, spectator aiming using gyro
also moves all of the motion sensor stuff to g_input.c
This commit is contained in:
minenice55
parent
6ee6291fd8
commit
9730882fe4
6 changed files with 387 additions and 273 deletions
251
src/g_game.c
251
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);
|
||||
|
|
|
|||
28
src/g_game.h
28
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);
|
||||
|
|
|
|||
330
src/g_input.c
330
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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in a new issue