mirror of
https://github.com/Mr-Wiseguy/Zelda64Recomp.git
synced 2024-12-18 04:29:42 +00:00
1312 lines
39 KiB
C++
1312 lines
39 KiB
C++
// Copyright (c) 2020-2023 Julian "Jibb" Smart
|
|
// Released under the MIT license. See https://github.com/JibbSmart/GamepadMotionHelpers/blob/main/LICENSE for more info
|
|
// Version 9
|
|
|
|
#pragma once
|
|
|
|
#define _USE_MATH_DEFINES
|
|
#include <math.h>
|
|
#include <algorithm> // std::min, std::max and std::clamp
|
|
|
|
// You don't need to look at these. These will just be used internally by the GamepadMotion class declared below.
|
|
// You can ignore anything in namespace GamepadMotionHelpers.
|
|
class GamepadMotionSettings;
|
|
class GamepadMotion;
|
|
|
|
namespace GamepadMotionHelpers
|
|
{
|
|
struct GyroCalibration
|
|
{
|
|
float X;
|
|
float Y;
|
|
float Z;
|
|
float AccelMagnitude;
|
|
int NumSamples;
|
|
};
|
|
|
|
struct Quat
|
|
{
|
|
float w;
|
|
float x;
|
|
float y;
|
|
float z;
|
|
|
|
Quat();
|
|
Quat(float inW, float inX, float inY, float inZ);
|
|
void Set(float inW, float inX, float inY, float inZ);
|
|
Quat& operator*=(const Quat& rhs);
|
|
friend Quat operator*(Quat lhs, const Quat& rhs);
|
|
void Normalize();
|
|
Quat Normalized() const;
|
|
void Invert();
|
|
Quat Inverse() const;
|
|
};
|
|
|
|
struct Vec
|
|
{
|
|
float x;
|
|
float y;
|
|
float z;
|
|
|
|
Vec();
|
|
Vec(float inValue);
|
|
Vec(float inX, float inY, float inZ);
|
|
void Set(float inX, float inY, float inZ);
|
|
float Length() const;
|
|
float LengthSquared() const;
|
|
void Normalize();
|
|
Vec Normalized() const;
|
|
float Dot(const Vec& other) const;
|
|
Vec Cross(const Vec& other) const;
|
|
Vec Min(const Vec& other) const;
|
|
Vec Max(const Vec& other) const;
|
|
Vec Abs() const;
|
|
Vec Lerp(const Vec& other, float factor) const;
|
|
Vec Lerp(const Vec& other, const Vec& factor) const;
|
|
Vec& operator+=(const Vec& rhs);
|
|
friend Vec operator+(Vec lhs, const Vec& rhs);
|
|
Vec& operator-=(const Vec& rhs);
|
|
friend Vec operator-(Vec lhs, const Vec& rhs);
|
|
Vec& operator*=(const float rhs);
|
|
friend Vec operator*(Vec lhs, const float rhs);
|
|
Vec& operator/=(const float rhs);
|
|
friend Vec operator/(Vec lhs, const float rhs);
|
|
Vec& operator*=(const Quat& rhs);
|
|
friend Vec operator*(Vec lhs, const Quat& rhs);
|
|
Vec operator-() const;
|
|
};
|
|
|
|
struct SensorMinMaxWindow
|
|
{
|
|
Vec MinGyro;
|
|
Vec MaxGyro;
|
|
Vec MeanGyro;
|
|
Vec MinAccel;
|
|
Vec MaxAccel;
|
|
Vec MeanAccel;
|
|
Vec StartAccel;
|
|
int NumSamples = 0;
|
|
float TimeSampled = 0.f;
|
|
|
|
SensorMinMaxWindow();
|
|
void Reset(float remainder);
|
|
void AddSample(const Vec& inGyro, const Vec& inAccel, float deltaTime);
|
|
Vec GetMidGyro();
|
|
};
|
|
|
|
struct AutoCalibration
|
|
{
|
|
SensorMinMaxWindow MinMaxWindow;
|
|
Vec SmoothedAngularVelocityGyro;
|
|
Vec SmoothedAngularVelocityAccel;
|
|
Vec SmoothedPreviousAccel;
|
|
Vec PreviousAccel;
|
|
|
|
AutoCalibration();
|
|
void Reset();
|
|
bool AddSampleStillness(const Vec& inGyro, const Vec& inAccel, float deltaTime, bool doSensorFusion);
|
|
void NoSampleStillness();
|
|
bool AddSampleSensorFusion(const Vec& inGyro, const Vec& inAccel, float deltaTime);
|
|
void NoSampleSensorFusion();
|
|
void SetCalibrationData(GyroCalibration* calibrationData);
|
|
void SetSettings(GamepadMotionSettings* settings);
|
|
|
|
float Confidence = 0.f;
|
|
bool IsSteady() { return bIsSteady; }
|
|
|
|
private:
|
|
Vec MinDeltaGyro = Vec(1.f);
|
|
Vec MinDeltaAccel = Vec(0.25f);
|
|
float RecalibrateThreshold = 1.f;
|
|
float SensorFusionSkippedTime = 0.f;
|
|
float TimeSteadySensorFusion = 0.f;
|
|
float TimeSteadyStillness = 0.f;
|
|
bool bIsSteady = false;
|
|
|
|
GyroCalibration* CalibrationData;
|
|
GamepadMotionSettings* Settings;
|
|
};
|
|
|
|
struct Motion
|
|
{
|
|
Quat Quaternion;
|
|
Vec Accel;
|
|
Vec Grav;
|
|
|
|
Vec SmoothAccel = Vec();
|
|
float Shakiness = 0.f;
|
|
const float ShortSteadinessHalfTime = 0.25f;
|
|
const float LongSteadinessHalfTime = 1.f;
|
|
|
|
Motion();
|
|
void Reset();
|
|
void Update(float inGyroX, float inGyroY, float inGyroZ, float inAccelX, float inAccelY, float inAccelZ, float gravityLength, float deltaTime);
|
|
void SetSettings(GamepadMotionSettings* settings);
|
|
|
|
private:
|
|
GamepadMotionSettings* Settings;
|
|
};
|
|
|
|
enum CalibrationMode
|
|
{
|
|
Manual = 0,
|
|
Stillness = 1,
|
|
SensorFusion = 2,
|
|
};
|
|
|
|
// https://stackoverflow.com/a/1448478/1130520
|
|
inline CalibrationMode operator|(CalibrationMode a, CalibrationMode b)
|
|
{
|
|
return static_cast<CalibrationMode>(static_cast<int>(a) | static_cast<int>(b));
|
|
}
|
|
|
|
inline CalibrationMode operator&(CalibrationMode a, CalibrationMode b)
|
|
{
|
|
return static_cast<CalibrationMode>(static_cast<int>(a) & static_cast<int>(b));
|
|
}
|
|
|
|
inline CalibrationMode operator~(CalibrationMode a)
|
|
{
|
|
return static_cast<CalibrationMode>(~static_cast<int>(a));
|
|
}
|
|
|
|
// https://stackoverflow.com/a/23152590/1130520
|
|
inline CalibrationMode& operator|=(CalibrationMode& a, CalibrationMode b)
|
|
{
|
|
return (CalibrationMode&)((int&)(a) |= static_cast<int>(b));
|
|
}
|
|
|
|
inline CalibrationMode& operator&=(CalibrationMode& a, CalibrationMode b)
|
|
{
|
|
return (CalibrationMode&)((int&)(a) &= static_cast<int>(b));
|
|
}
|
|
}
|
|
|
|
// Note that I'm using a Y-up coordinate system. This is to follow the convention set by the motion sensors in
|
|
// PlayStation controllers, which was what I was using when writing in this. But for the record, Z-up is
|
|
// better for most games (XY ground-plane in 3D games simplifies using 2D vectors in navigation, for example).
|
|
|
|
// Gyro units should be degrees per second. Accelerometer should be g-force (approx. 9.8 m/s^2 = 1 g). If you're using
|
|
// radians per second, meters per second squared, etc, conversion should be simple.
|
|
|
|
class GamepadMotionSettings
|
|
{
|
|
public:
|
|
int MinStillnessSamples = 10;
|
|
float MinStillnessCollectionTime = 0.5f;
|
|
float MinStillnessCorrectionTime = 2.f;
|
|
float MaxStillnessError = 2.f;
|
|
float StillnessSampleDeteriorationRate = 0.2f;
|
|
float StillnessErrorClimbRate = 0.1f;
|
|
float StillnessErrorDropOnRecalibrate = 0.1f;
|
|
float StillnessCalibrationEaseInTime = 3.f;
|
|
float StillnessCalibrationHalfTime = 0.1f;
|
|
float StillnessConfidenceRate = 1.f;
|
|
|
|
float StillnessGyroDelta = -1.f;
|
|
float StillnessAccelDelta = -1.f;
|
|
|
|
float SensorFusionCalibrationSmoothingStrength = 2.f;
|
|
float SensorFusionAngularAccelerationThreshold = 20.f;
|
|
float SensorFusionCalibrationEaseInTime = 3.f;
|
|
float SensorFusionCalibrationHalfTime = 0.1f;
|
|
float SensorFusionConfidenceRate = 1.f;
|
|
|
|
float GravityCorrectionShakinessMaxThreshold = 0.4f;
|
|
float GravityCorrectionShakinessMinThreshold = 0.01f;
|
|
|
|
float GravityCorrectionStillSpeed = 1.f;
|
|
float GravityCorrectionShakySpeed = 0.1f;
|
|
|
|
float GravityCorrectionGyroFactor = 0.1f;
|
|
float GravityCorrectionGyroMinThreshold = 0.05f;
|
|
float GravityCorrectionGyroMaxThreshold = 0.25f;
|
|
|
|
float GravityCorrectionMinimumSpeed = 0.01f;
|
|
};
|
|
|
|
class GamepadMotion
|
|
{
|
|
public:
|
|
GamepadMotion();
|
|
|
|
void Reset();
|
|
|
|
void ProcessMotion(float gyroX, float gyroY, float gyroZ,
|
|
float accelX, float accelY, float accelZ, float deltaTime);
|
|
|
|
// reading the current state
|
|
void GetCalibratedGyro(float& x, float& y, float& z);
|
|
void GetGravity(float& x, float& y, float& z);
|
|
void GetProcessedAcceleration(float& x, float& y, float& z);
|
|
void GetOrientation(float& w, float& x, float& y, float& z);
|
|
void GetPlayerSpaceGyro(float& x, float& y, const float yawRelaxFactor = 1.41f);
|
|
static void CalculatePlayerSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float yawRelaxFactor = 1.41f);
|
|
void GetWorldSpaceGyro(float& x, float& y, const float sideReductionThreshold = 0.125f);
|
|
static void CalculateWorldSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float sideReductionThreshold = 0.125f);
|
|
|
|
// gyro calibration functions
|
|
void StartContinuousCalibration();
|
|
void PauseContinuousCalibration();
|
|
void ResetContinuousCalibration();
|
|
void GetCalibrationOffset(float& xOffset, float& yOffset, float& zOffset);
|
|
void SetCalibrationOffset(float xOffset, float yOffset, float zOffset, int weight);
|
|
float GetAutoCalibrationConfidence();
|
|
void SetAutoCalibrationConfidence(float newConfidence);
|
|
bool GetAutoCalibrationIsSteady();
|
|
|
|
GamepadMotionHelpers::CalibrationMode GetCalibrationMode();
|
|
void SetCalibrationMode(GamepadMotionHelpers::CalibrationMode calibrationMode);
|
|
|
|
void ResetMotion();
|
|
|
|
GamepadMotionSettings Settings;
|
|
|
|
private:
|
|
GamepadMotionHelpers::Vec Gyro;
|
|
GamepadMotionHelpers::Vec RawAccel;
|
|
GamepadMotionHelpers::Motion Motion;
|
|
GamepadMotionHelpers::GyroCalibration GyroCalibration;
|
|
GamepadMotionHelpers::AutoCalibration AutoCalibration;
|
|
GamepadMotionHelpers::CalibrationMode CurrentCalibrationMode;
|
|
|
|
bool IsCalibrating;
|
|
void PushSensorSamples(float gyroX, float gyroY, float gyroZ, float accelMagnitude);
|
|
void GetCalibratedSensor(float& gyroOffsetX, float& gyroOffsetY, float& gyroOffsetZ, float& accelMagnitude);
|
|
};
|
|
|
|
///////////// Everything below here are just implementation details /////////////
|
|
|
|
namespace GamepadMotionHelpers
|
|
{
|
|
inline Quat::Quat()
|
|
{
|
|
w = 1.0f;
|
|
x = 0.0f;
|
|
y = 0.0f;
|
|
z = 0.0f;
|
|
}
|
|
|
|
inline Quat::Quat(float inW, float inX, float inY, float inZ)
|
|
{
|
|
w = inW;
|
|
x = inX;
|
|
y = inY;
|
|
z = inZ;
|
|
}
|
|
|
|
inline static Quat AngleAxis(float inAngle, float inX, float inY, float inZ)
|
|
{
|
|
const float sinHalfAngle = sinf(inAngle * 0.5f);
|
|
Vec inAxis = Vec(inX, inY, inZ);
|
|
inAxis.Normalize();
|
|
inAxis *= sinHalfAngle;
|
|
Quat result = Quat(cosf(inAngle * 0.5f), inAxis.x, inAxis.y, inAxis.z);
|
|
return result;
|
|
}
|
|
|
|
inline void Quat::Set(float inW, float inX, float inY, float inZ)
|
|
{
|
|
w = inW;
|
|
x = inX;
|
|
y = inY;
|
|
z = inZ;
|
|
}
|
|
|
|
inline Quat& Quat::operator*=(const Quat& rhs)
|
|
{
|
|
Set(w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z,
|
|
w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y,
|
|
w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x,
|
|
w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w);
|
|
return *this;
|
|
}
|
|
|
|
inline Quat operator*(Quat lhs, const Quat& rhs)
|
|
{
|
|
lhs *= rhs;
|
|
return lhs;
|
|
}
|
|
|
|
inline void Quat::Normalize()
|
|
{
|
|
const float length = sqrtf(w * w + x * x + y * y + z * z);
|
|
const float fixFactor = 1.0f / length;
|
|
|
|
w *= fixFactor;
|
|
x *= fixFactor;
|
|
y *= fixFactor;
|
|
z *= fixFactor;
|
|
|
|
return;
|
|
}
|
|
|
|
inline Quat Quat::Normalized() const
|
|
{
|
|
Quat result = *this;
|
|
result.Normalize();
|
|
return result;
|
|
}
|
|
|
|
inline void Quat::Invert()
|
|
{
|
|
x = -x;
|
|
y = -y;
|
|
z = -z;
|
|
return;
|
|
}
|
|
|
|
inline Quat Quat::Inverse() const
|
|
{
|
|
Quat result = *this;
|
|
result.Invert();
|
|
return result;
|
|
}
|
|
|
|
inline Vec::Vec()
|
|
{
|
|
x = 0.0f;
|
|
y = 0.0f;
|
|
z = 0.0f;
|
|
}
|
|
|
|
inline Vec::Vec(float inValue)
|
|
{
|
|
x = inValue;
|
|
y = inValue;
|
|
z = inValue;
|
|
}
|
|
|
|
inline Vec::Vec(float inX, float inY, float inZ)
|
|
{
|
|
x = inX;
|
|
y = inY;
|
|
z = inZ;
|
|
}
|
|
|
|
inline void Vec::Set(float inX, float inY, float inZ)
|
|
{
|
|
x = inX;
|
|
y = inY;
|
|
z = inZ;
|
|
}
|
|
|
|
inline float Vec::Length() const
|
|
{
|
|
return sqrtf(x * x + y * y + z * z);
|
|
}
|
|
|
|
inline float Vec::LengthSquared() const
|
|
{
|
|
return x * x + y * y + z * z;
|
|
}
|
|
|
|
inline void Vec::Normalize()
|
|
{
|
|
const float length = Length();
|
|
if (length == 0.0)
|
|
{
|
|
return;
|
|
}
|
|
const float fixFactor = 1.0f / length;
|
|
|
|
x *= fixFactor;
|
|
y *= fixFactor;
|
|
z *= fixFactor;
|
|
return;
|
|
}
|
|
|
|
inline Vec Vec::Normalized() const
|
|
{
|
|
Vec result = *this;
|
|
result.Normalize();
|
|
return result;
|
|
}
|
|
|
|
inline Vec& Vec::operator+=(const Vec& rhs)
|
|
{
|
|
Set(x + rhs.x, y + rhs.y, z + rhs.z);
|
|
return *this;
|
|
}
|
|
|
|
inline Vec operator+(Vec lhs, const Vec& rhs)
|
|
{
|
|
lhs += rhs;
|
|
return lhs;
|
|
}
|
|
|
|
inline Vec& Vec::operator-=(const Vec& rhs)
|
|
{
|
|
Set(x - rhs.x, y - rhs.y, z - rhs.z);
|
|
return *this;
|
|
}
|
|
|
|
inline Vec operator-(Vec lhs, const Vec& rhs)
|
|
{
|
|
lhs -= rhs;
|
|
return lhs;
|
|
}
|
|
|
|
inline Vec& Vec::operator*=(const float rhs)
|
|
{
|
|
Set(x * rhs, y * rhs, z * rhs);
|
|
return *this;
|
|
}
|
|
|
|
inline Vec operator*(Vec lhs, const float rhs)
|
|
{
|
|
lhs *= rhs;
|
|
return lhs;
|
|
}
|
|
|
|
inline Vec& Vec::operator/=(const float rhs)
|
|
{
|
|
Set(x / rhs, y / rhs, z / rhs);
|
|
return *this;
|
|
}
|
|
|
|
inline Vec operator/(Vec lhs, const float rhs)
|
|
{
|
|
lhs /= rhs;
|
|
return lhs;
|
|
}
|
|
|
|
inline Vec& Vec::operator*=(const Quat& rhs)
|
|
{
|
|
Quat temp = rhs * Quat(0.0f, x, y, z) * rhs.Inverse();
|
|
Set(temp.x, temp.y, temp.z);
|
|
return *this;
|
|
}
|
|
|
|
inline Vec operator*(Vec lhs, const Quat& rhs)
|
|
{
|
|
lhs *= rhs;
|
|
return lhs;
|
|
}
|
|
|
|
inline Vec Vec::operator-() const
|
|
{
|
|
Vec result = Vec(-x, -y, -z);
|
|
return result;
|
|
}
|
|
|
|
inline float Vec::Dot(const Vec& other) const
|
|
{
|
|
return x * other.x + y * other.y + z * other.z;
|
|
}
|
|
|
|
inline Vec Vec::Cross(const Vec& other) const
|
|
{
|
|
return Vec(y * other.z - z * other.y,
|
|
z * other.x - x * other.z,
|
|
x * other.y - y * other.x);
|
|
}
|
|
|
|
inline Vec Vec::Min(const Vec& other) const
|
|
{
|
|
return Vec(x < other.x ? x : other.x,
|
|
y < other.y ? y : other.y,
|
|
z < other.z ? z : other.z);
|
|
}
|
|
|
|
inline Vec Vec::Max(const Vec& other) const
|
|
{
|
|
return Vec(x > other.x ? x : other.x,
|
|
y > other.y ? y : other.y,
|
|
z > other.z ? z : other.z);
|
|
}
|
|
|
|
inline Vec Vec::Abs() const
|
|
{
|
|
return Vec(x > 0 ? x : -x,
|
|
y > 0 ? y : -y,
|
|
z > 0 ? z : -z);
|
|
}
|
|
|
|
inline Vec Vec::Lerp(const Vec& other, float factor) const
|
|
{
|
|
return *this + (other - *this) * factor;
|
|
}
|
|
|
|
inline Vec Vec::Lerp(const Vec& other, const Vec& factor) const
|
|
{
|
|
return Vec(this->x + (other.x - this->x) * factor.x,
|
|
this->y + (other.y - this->y) * factor.y,
|
|
this->z + (other.z - this->z) * factor.z);
|
|
}
|
|
|
|
inline Motion::Motion()
|
|
{
|
|
Reset();
|
|
}
|
|
|
|
inline void Motion::Reset()
|
|
{
|
|
Quaternion.Set(1.f, 0.f, 0.f, 0.f);
|
|
Accel.Set(0.f, 0.f, 0.f);
|
|
Grav.Set(0.f, 0.f, 0.f);
|
|
SmoothAccel.Set(0.f, 0.f, 0.f);
|
|
Shakiness = 0.f;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The gyro inputs should be calibrated degrees per second but have no other processing. Acceleration is in G units (1 = approx. 9.8m/s^2)
|
|
/// </summary>
|
|
inline void Motion::Update(float inGyroX, float inGyroY, float inGyroZ, float inAccelX, float inAccelY, float inAccelZ, float gravityLength, float deltaTime)
|
|
{
|
|
if (!Settings)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// get settings
|
|
const float gravityCorrectionShakinessMinThreshold = Settings->GravityCorrectionShakinessMinThreshold;
|
|
const float gravityCorrectionShakinessMaxThreshold = Settings->GravityCorrectionShakinessMaxThreshold;
|
|
const float gravityCorrectionStillSpeed = Settings->GravityCorrectionStillSpeed;
|
|
const float gravityCorrectionShakySpeed = Settings->GravityCorrectionShakySpeed;
|
|
const float gravityCorrectionGyroFactor = Settings->GravityCorrectionGyroFactor;
|
|
const float gravityCorrectionGyroMinThreshold = Settings->GravityCorrectionGyroMinThreshold;
|
|
const float gravityCorrectionGyroMaxThreshold = Settings->GravityCorrectionGyroMaxThreshold;
|
|
const float gravityCorrectionMinimumSpeed = Settings->GravityCorrectionMinimumSpeed;
|
|
|
|
const Vec axis = Vec(inGyroX, inGyroY, inGyroZ);
|
|
const Vec accel = Vec(inAccelX, inAccelY, inAccelZ);
|
|
const float angleSpeed = axis.Length() * (float)M_PI / 180.0f;
|
|
const float angle = angleSpeed * deltaTime;
|
|
|
|
// rotate
|
|
Quat rotation = AngleAxis(angle, axis.x, axis.y, axis.z);
|
|
Quaternion *= rotation; // do it this way because it's a local rotation, not global
|
|
|
|
//printf("Quat: %.4f %.4f %.4f %.4f\n",
|
|
// Quaternion.w, Quaternion.x, Quaternion.y, Quaternion.z);
|
|
float accelMagnitude = accel.Length();
|
|
if (accelMagnitude > 0.0f)
|
|
{
|
|
const Vec accelNorm = accel / accelMagnitude;
|
|
// account for rotation when tracking smoothed acceleration
|
|
SmoothAccel *= rotation.Inverse();
|
|
//printf("Absolute Accel: %.4f %.4f %.4f\n",
|
|
// absoluteAccel.x, absoluteAccel.y, absoluteAccel.z);
|
|
const float smoothFactor = ShortSteadinessHalfTime <= 0.f ? 0.f : exp2f(-deltaTime / ShortSteadinessHalfTime);
|
|
Shakiness *= smoothFactor;
|
|
Shakiness = std::max(Shakiness, (accel - SmoothAccel).Length());
|
|
SmoothAccel = accel.Lerp(SmoothAccel, smoothFactor);
|
|
|
|
//printf("Shakiness: %.4f\n", Shakiness);
|
|
|
|
// update grav by rotation
|
|
Grav *= rotation.Inverse();
|
|
// we want to close the gap between grav and raw acceleration. What's the difference
|
|
const Vec gravToAccel = (accelNorm * -gravityLength) - Grav;
|
|
const Vec gravToAccelDir = gravToAccel.Normalized();
|
|
// adjustment rate
|
|
float gravCorrectionSpeed;
|
|
if (gravityCorrectionShakinessMinThreshold < gravityCorrectionShakinessMaxThreshold)
|
|
{
|
|
gravCorrectionSpeed = gravityCorrectionStillSpeed + (gravityCorrectionShakySpeed - gravityCorrectionStillSpeed) * std::clamp((Shakiness - gravityCorrectionShakinessMinThreshold) / (gravityCorrectionShakinessMaxThreshold - gravityCorrectionShakinessMinThreshold), 0.f, 1.f);
|
|
}
|
|
else
|
|
{
|
|
gravCorrectionSpeed = Shakiness < gravityCorrectionShakinessMaxThreshold ? gravityCorrectionStillSpeed : gravityCorrectionShakySpeed;
|
|
}
|
|
// we also limit it to be no faster than a given proportion of the gyro rate, or the minimum gravity correction speed
|
|
const float gyroGravCorrectionLimit = std::max(angleSpeed * gravityCorrectionGyroFactor, gravityCorrectionMinimumSpeed);
|
|
if (gravCorrectionSpeed > gyroGravCorrectionLimit)
|
|
{
|
|
float closeEnoughFactor;
|
|
if (gravityCorrectionGyroMinThreshold < gravityCorrectionGyroMaxThreshold)
|
|
{
|
|
closeEnoughFactor = std::clamp((gravToAccel.Length() - gravityCorrectionGyroMinThreshold) / (gravityCorrectionGyroMaxThreshold - gravityCorrectionGyroMinThreshold), 0.f, 1.f);
|
|
}
|
|
else
|
|
{
|
|
closeEnoughFactor = gravToAccel.Length() < gravityCorrectionGyroMaxThreshold ? 0.f : 1.f;
|
|
}
|
|
gravCorrectionSpeed = gyroGravCorrectionLimit + (gravCorrectionSpeed - gyroGravCorrectionLimit) * closeEnoughFactor;
|
|
}
|
|
const Vec gravToAccelDelta = gravToAccelDir * gravCorrectionSpeed * deltaTime;
|
|
if (gravToAccelDelta.LengthSquared() < gravToAccel.LengthSquared())
|
|
{
|
|
Grav += gravToAccelDelta;
|
|
}
|
|
else
|
|
{
|
|
Grav = accelNorm * -gravityLength;
|
|
}
|
|
|
|
const Vec gravityDirection = Grav.Normalized() * Quaternion.Inverse(); // absolute gravity direction
|
|
const float errorAngle = acosf(std::clamp(Vec(0.0f, -1.0f, 0.0f).Dot(gravityDirection), -1.f, 1.f));
|
|
const Vec flattened = Vec(0.0f, -1.0f, 0.0f).Cross(gravityDirection);
|
|
Quat correctionQuat = AngleAxis(errorAngle, flattened.x, flattened.y, flattened.z);
|
|
Quaternion = Quaternion * correctionQuat;
|
|
|
|
Accel = accel + Grav;
|
|
}
|
|
else
|
|
{
|
|
Grav *= rotation.Inverse();
|
|
Accel = Grav;
|
|
}
|
|
Quaternion.Normalize();
|
|
}
|
|
|
|
inline void Motion::SetSettings(GamepadMotionSettings* settings)
|
|
{
|
|
Settings = settings;
|
|
}
|
|
|
|
inline SensorMinMaxWindow::SensorMinMaxWindow()
|
|
{
|
|
Reset(0.f);
|
|
}
|
|
|
|
inline void SensorMinMaxWindow::Reset(float remainder)
|
|
{
|
|
NumSamples = 0;
|
|
TimeSampled = remainder;
|
|
}
|
|
|
|
inline void SensorMinMaxWindow::AddSample(const Vec& inGyro, const Vec& inAccel, float deltaTime)
|
|
{
|
|
if (NumSamples == 0)
|
|
{
|
|
MaxGyro = inGyro;
|
|
MinGyro = inGyro;
|
|
MeanGyro = inGyro;
|
|
MaxAccel = inAccel;
|
|
MinAccel = inAccel;
|
|
MeanAccel = inAccel;
|
|
StartAccel = inAccel;
|
|
NumSamples = 1;
|
|
TimeSampled += deltaTime;
|
|
return;
|
|
}
|
|
|
|
MaxGyro = MaxGyro.Max(inGyro);
|
|
MinGyro = MinGyro.Min(inGyro);
|
|
MaxAccel = MaxAccel.Max(inAccel);
|
|
MinAccel = MinAccel.Min(inAccel);
|
|
|
|
NumSamples++;
|
|
TimeSampled += deltaTime;
|
|
|
|
Vec delta = inGyro - MeanGyro;
|
|
MeanGyro += delta * (1.f / NumSamples);
|
|
delta = inAccel - MeanAccel;
|
|
MeanAccel += delta * (1.f / NumSamples);
|
|
}
|
|
|
|
inline Vec SensorMinMaxWindow::GetMidGyro()
|
|
{
|
|
return MeanGyro;
|
|
}
|
|
|
|
inline AutoCalibration::AutoCalibration()
|
|
{
|
|
CalibrationData = nullptr;
|
|
Reset();
|
|
}
|
|
|
|
inline void AutoCalibration::Reset()
|
|
{
|
|
MinMaxWindow.Reset(0.f);
|
|
Confidence = 0.f;
|
|
bIsSteady = false;
|
|
MinDeltaGyro = Vec(1.f);
|
|
MinDeltaAccel = Vec(0.25f);
|
|
RecalibrateThreshold = 1.f;
|
|
SensorFusionSkippedTime = 0.f;
|
|
TimeSteadySensorFusion = 0.f;
|
|
TimeSteadyStillness = 0.f;
|
|
}
|
|
|
|
inline bool AutoCalibration::AddSampleStillness(const Vec& inGyro, const Vec& inAccel, float deltaTime, bool doSensorFusion)
|
|
{
|
|
if (inGyro.x == 0.f && inGyro.y == 0.f && inGyro.z == 0.f &&
|
|
inAccel.x == 0.f && inAccel.y == 0.f && inAccel.z == 0.f)
|
|
{
|
|
// zeroes are almost certainly not valid inputs
|
|
return false;
|
|
}
|
|
|
|
if (!Settings)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!CalibrationData)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// get settings
|
|
const int minStillnessSamples = Settings->MinStillnessSamples;
|
|
const float minStillnessCollectionTime = Settings->MinStillnessCollectionTime;
|
|
const float minStillnessCorrectionTime = Settings->MinStillnessCorrectionTime;
|
|
const float maxStillnessError = Settings->MaxStillnessError;
|
|
const float stillnessSampleDeteriorationRate = Settings->StillnessSampleDeteriorationRate;
|
|
const float stillnessErrorClimbRate = Settings->StillnessErrorClimbRate;
|
|
const float stillnessErrorDropOnRecalibrate = Settings->StillnessErrorDropOnRecalibrate;
|
|
const float stillnessCalibrationEaseInTime = Settings->StillnessCalibrationEaseInTime;
|
|
const float stillnessCalibrationHalfTime = Settings->StillnessCalibrationHalfTime * Confidence;
|
|
const float stillnessConfidenceRate = Settings->StillnessConfidenceRate;
|
|
const float stillnessGyroDelta = Settings->StillnessGyroDelta;
|
|
const float stillnessAccelDelta = Settings->StillnessAccelDelta;
|
|
|
|
MinMaxWindow.AddSample(inGyro, inAccel, deltaTime);
|
|
// get deltas
|
|
const Vec gyroDelta = MinMaxWindow.MaxGyro - MinMaxWindow.MinGyro;
|
|
const Vec accelDelta = MinMaxWindow.MaxAccel - MinMaxWindow.MinAccel;
|
|
|
|
bool calibrated = false;
|
|
bool isSteady = false;
|
|
const Vec climbThisTick = Vec(stillnessSampleDeteriorationRate * deltaTime);
|
|
if (stillnessGyroDelta < 0.f)
|
|
{
|
|
if (Confidence < 1.f)
|
|
{
|
|
MinDeltaGyro += climbThisTick;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
MinDeltaGyro = Vec(stillnessGyroDelta);
|
|
}
|
|
if (stillnessAccelDelta < 0.f)
|
|
{
|
|
if (Confidence < 1.f)
|
|
{
|
|
MinDeltaAccel += climbThisTick;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
MinDeltaAccel = Vec(stillnessAccelDelta);
|
|
}
|
|
|
|
//printf("Deltas: %.4f %.4f %.4f; %.4f %.4f %.4f\n",
|
|
// gyroDelta.x, gyroDelta.y, gyroDelta.z,
|
|
// accelDelta.x, accelDelta.y, accelDelta.z);
|
|
|
|
if (MinMaxWindow.NumSamples >= minStillnessSamples && MinMaxWindow.TimeSampled >= minStillnessCollectionTime)
|
|
{
|
|
MinDeltaGyro = MinDeltaGyro.Min(gyroDelta);
|
|
MinDeltaAccel = MinDeltaAccel.Min(accelDelta);
|
|
}
|
|
else
|
|
{
|
|
RecalibrateThreshold = std::min(RecalibrateThreshold + stillnessErrorClimbRate * deltaTime, maxStillnessError);
|
|
return false;
|
|
}
|
|
|
|
// check that all inputs are below appropriate thresholds to be considered "still"
|
|
if (gyroDelta.x <= MinDeltaGyro.x * RecalibrateThreshold &&
|
|
gyroDelta.y <= MinDeltaGyro.y * RecalibrateThreshold &&
|
|
gyroDelta.z <= MinDeltaGyro.z * RecalibrateThreshold &&
|
|
accelDelta.x <= MinDeltaAccel.x * RecalibrateThreshold &&
|
|
accelDelta.y <= MinDeltaAccel.y * RecalibrateThreshold &&
|
|
accelDelta.z <= MinDeltaAccel.z * RecalibrateThreshold)
|
|
{
|
|
if (MinMaxWindow.NumSamples >= minStillnessSamples && MinMaxWindow.TimeSampled >= minStillnessCorrectionTime)
|
|
{
|
|
/*if (TimeSteadyStillness == 0.f)
|
|
{
|
|
printf("Still!\n");
|
|
}/**/
|
|
|
|
TimeSteadyStillness = std::min(TimeSteadyStillness + deltaTime, stillnessCalibrationEaseInTime);
|
|
const float calibrationEaseIn = stillnessCalibrationEaseInTime <= 0.f ? 1.f : TimeSteadyStillness / stillnessCalibrationEaseInTime;
|
|
|
|
const Vec calibratedGyro = MinMaxWindow.GetMidGyro();
|
|
|
|
const Vec oldGyroBias = Vec(CalibrationData->X, CalibrationData->Y, CalibrationData->Z) / std::max((float)CalibrationData->NumSamples, 1.f);
|
|
const float stillnessLerpFactor = stillnessCalibrationHalfTime <= 0.f ? 0.f : exp2f(-calibrationEaseIn * deltaTime / stillnessCalibrationHalfTime);
|
|
Vec newGyroBias = calibratedGyro.Lerp(oldGyroBias, stillnessLerpFactor);
|
|
Confidence = std::min(Confidence + deltaTime * stillnessConfidenceRate, 1.f);
|
|
isSteady = true;
|
|
|
|
if (doSensorFusion)
|
|
{
|
|
const Vec previousNormal = MinMaxWindow.StartAccel.Normalized();
|
|
const Vec thisNormal = inAccel.Normalized();
|
|
Vec angularVelocity = thisNormal.Cross(previousNormal);
|
|
const float crossLength = angularVelocity.Length();
|
|
if (crossLength > 0.f)
|
|
{
|
|
const float thisDotPrev = std::clamp(thisNormal.Dot(previousNormal), -1.f, 1.f);
|
|
const float angleChange = acosf(thisDotPrev) * 180.0f / (float)M_PI;
|
|
const float anglePerSecond = angleChange / MinMaxWindow.TimeSampled;
|
|
angularVelocity *= anglePerSecond / crossLength;
|
|
}
|
|
|
|
Vec axisCalibrationStrength = thisNormal.Abs();
|
|
Vec sensorFusionBias = (calibratedGyro - angularVelocity).Lerp(oldGyroBias, stillnessLerpFactor);
|
|
if (axisCalibrationStrength.x <= 0.7f)
|
|
{
|
|
newGyroBias.x = sensorFusionBias.x;
|
|
}
|
|
if (axisCalibrationStrength.y <= 0.7f)
|
|
{
|
|
newGyroBias.y = sensorFusionBias.y;
|
|
}
|
|
if (axisCalibrationStrength.z <= 0.7f)
|
|
{
|
|
newGyroBias.z = sensorFusionBias.z;
|
|
}
|
|
}
|
|
|
|
CalibrationData->X = newGyroBias.x;
|
|
CalibrationData->Y = newGyroBias.y;
|
|
CalibrationData->Z = newGyroBias.z;
|
|
|
|
CalibrationData->AccelMagnitude = MinMaxWindow.MeanAccel.Length();
|
|
CalibrationData->NumSamples = 1;
|
|
|
|
calibrated = true;
|
|
}
|
|
else
|
|
{
|
|
RecalibrateThreshold = std::min(RecalibrateThreshold + stillnessErrorClimbRate * deltaTime, maxStillnessError);
|
|
}
|
|
}
|
|
else if (TimeSteadyStillness > 0.f)
|
|
{
|
|
//printf("Moved!\n");
|
|
RecalibrateThreshold -= stillnessErrorDropOnRecalibrate;
|
|
if (RecalibrateThreshold < 1.f) RecalibrateThreshold = 1.f;
|
|
|
|
TimeSteadyStillness = 0.f;
|
|
MinMaxWindow.Reset(0.f);
|
|
}
|
|
else
|
|
{
|
|
RecalibrateThreshold = std::min(RecalibrateThreshold + stillnessErrorClimbRate * deltaTime, maxStillnessError);
|
|
MinMaxWindow.Reset(0.f);
|
|
}
|
|
|
|
bIsSteady = isSteady;
|
|
return calibrated;
|
|
}
|
|
|
|
inline void AutoCalibration::NoSampleStillness()
|
|
{
|
|
MinMaxWindow.Reset(0.f);
|
|
}
|
|
|
|
inline bool AutoCalibration::AddSampleSensorFusion(const Vec& inGyro, const Vec& inAccel, float deltaTime)
|
|
{
|
|
if (deltaTime <= 0.f)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (inGyro.x == 0.f && inGyro.y == 0.f && inGyro.z == 0.f &&
|
|
inAccel.x == 0.f && inAccel.y == 0.f && inAccel.z == 0.f)
|
|
{
|
|
// all zeroes are almost certainly not valid inputs
|
|
TimeSteadySensorFusion = 0.f;
|
|
SensorFusionSkippedTime = 0.f;
|
|
PreviousAccel = inAccel;
|
|
SmoothedPreviousAccel = inAccel;
|
|
SmoothedAngularVelocityGyro = GamepadMotionHelpers::Vec();
|
|
SmoothedAngularVelocityAccel = GamepadMotionHelpers::Vec();
|
|
return false;
|
|
}
|
|
|
|
if (PreviousAccel.x == 0.f && PreviousAccel.y == 0.f && PreviousAccel.z == 0.f)
|
|
{
|
|
TimeSteadySensorFusion = 0.f;
|
|
SensorFusionSkippedTime = 0.f;
|
|
PreviousAccel = inAccel;
|
|
SmoothedPreviousAccel = inAccel;
|
|
SmoothedAngularVelocityGyro = GamepadMotionHelpers::Vec();
|
|
SmoothedAngularVelocityAccel = GamepadMotionHelpers::Vec();
|
|
return false;
|
|
}
|
|
|
|
// in case the controller state hasn't updated between samples
|
|
if (inAccel.x == PreviousAccel.x && inAccel.y == PreviousAccel.y && inAccel.z == PreviousAccel.z)
|
|
{
|
|
SensorFusionSkippedTime += deltaTime;
|
|
return false;
|
|
}
|
|
|
|
if (!Settings)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// get settings
|
|
const float sensorFusionCalibrationSmoothingStrength = Settings->SensorFusionCalibrationSmoothingStrength;
|
|
const float sensorFusionAngularAccelerationThreshold = Settings->SensorFusionAngularAccelerationThreshold;
|
|
const float sensorFusionCalibrationEaseInTime = Settings->SensorFusionCalibrationEaseInTime;
|
|
const float sensorFusionCalibrationHalfTime = Settings->SensorFusionCalibrationHalfTime * Confidence;
|
|
const float sensorFusionConfidenceRate = Settings->SensorFusionConfidenceRate;
|
|
|
|
deltaTime += SensorFusionSkippedTime;
|
|
SensorFusionSkippedTime = 0.f;
|
|
bool calibrated = false;
|
|
bool isSteady = false;
|
|
|
|
// framerate independent lerp smoothing: https://www.gamasutra.com/blogs/ScottLembcke/20180404/316046/Improved_Lerp_Smoothing.php
|
|
const float smoothingLerpFactor = exp2f(-sensorFusionCalibrationSmoothingStrength * deltaTime);
|
|
// velocity from smoothed accel matches better if we also smooth gyro
|
|
const Vec previousGyro = SmoothedAngularVelocityGyro;
|
|
SmoothedAngularVelocityGyro = inGyro.Lerp(SmoothedAngularVelocityGyro, smoothingLerpFactor); // smooth what remains
|
|
const float gyroAccelerationMag = (SmoothedAngularVelocityGyro - previousGyro).Length() / deltaTime;
|
|
// get angle between old and new accel
|
|
const Vec previousNormal = SmoothedPreviousAccel.Normalized();
|
|
const Vec thisAccel = inAccel.Lerp(SmoothedPreviousAccel, smoothingLerpFactor);
|
|
const Vec thisNormal = thisAccel.Normalized();
|
|
Vec angularVelocity = thisNormal.Cross(previousNormal);
|
|
const float crossLength = angularVelocity.Length();
|
|
if (crossLength > 0.f)
|
|
{
|
|
const float thisDotPrev = std::clamp(thisNormal.Dot(previousNormal), -1.f, 1.f);
|
|
const float angleChange = acosf(thisDotPrev) * 180.0f / (float)M_PI;
|
|
const float anglePerSecond = angleChange / deltaTime;
|
|
angularVelocity *= anglePerSecond / crossLength;
|
|
}
|
|
SmoothedAngularVelocityAccel = angularVelocity;
|
|
|
|
// apply corrections
|
|
if (gyroAccelerationMag > sensorFusionAngularAccelerationThreshold || CalibrationData == nullptr)
|
|
{
|
|
/*if (TimeSteadySensorFusion > 0.f)
|
|
{
|
|
printf("Shaken!\n");
|
|
}/**/
|
|
TimeSteadySensorFusion = 0.f;
|
|
//printf("No calibration due to acceleration of %.4f\n", gyroAccelerationMag);
|
|
}
|
|
else
|
|
{
|
|
/*if (TimeSteadySensorFusion == 0.f)
|
|
{
|
|
printf("Steady!\n");
|
|
}/**/
|
|
|
|
TimeSteadySensorFusion = std::min(TimeSteadySensorFusion + deltaTime, sensorFusionCalibrationEaseInTime);
|
|
const float calibrationEaseIn = sensorFusionCalibrationEaseInTime <= 0.f ? 1.f : TimeSteadySensorFusion / sensorFusionCalibrationEaseInTime;
|
|
const Vec oldGyroBias = Vec(CalibrationData->X, CalibrationData->Y, CalibrationData->Z) / std::max((float)CalibrationData->NumSamples, 1.f);
|
|
// recalibrate over time proportional to the difference between the calculated bias and the current assumed bias
|
|
const float sensorFusionLerpFactor = sensorFusionCalibrationHalfTime <= 0.f ? 0.f : exp2f(-calibrationEaseIn * deltaTime / sensorFusionCalibrationHalfTime);
|
|
Vec newGyroBias = (SmoothedAngularVelocityGyro - SmoothedAngularVelocityAccel).Lerp(oldGyroBias, sensorFusionLerpFactor);
|
|
Confidence = std::min(Confidence + deltaTime * sensorFusionConfidenceRate, 1.f);
|
|
isSteady = true;
|
|
// don't change bias in axes that can't be affected by the gravity direction
|
|
Vec axisCalibrationStrength = thisNormal.Abs();
|
|
if (axisCalibrationStrength.x > 0.7f)
|
|
{
|
|
axisCalibrationStrength.x = 1.f;
|
|
}
|
|
if (axisCalibrationStrength.y > 0.7f)
|
|
{
|
|
axisCalibrationStrength.y = 1.f;
|
|
}
|
|
if (axisCalibrationStrength.z > 0.7f)
|
|
{
|
|
axisCalibrationStrength.z = 1.f;
|
|
}
|
|
newGyroBias = newGyroBias.Lerp(oldGyroBias, axisCalibrationStrength.Min(Vec(1.f)));
|
|
|
|
CalibrationData->X = newGyroBias.x;
|
|
CalibrationData->Y = newGyroBias.y;
|
|
CalibrationData->Z = newGyroBias.z;
|
|
|
|
CalibrationData->AccelMagnitude = thisAccel.Length();
|
|
|
|
CalibrationData->NumSamples = 1;
|
|
|
|
calibrated = true;
|
|
|
|
//printf("Recalibrating at a strength of %.4f\n", calibrationEaseIn);
|
|
}
|
|
|
|
SmoothedPreviousAccel = thisAccel;
|
|
PreviousAccel = inAccel;
|
|
|
|
//printf("Gyro: %.4f, %.4f, %.4f | Accel: %.4f, %.4f, %.4f\n",
|
|
// SmoothedAngularVelocityGyro.x, SmoothedAngularVelocityGyro.y, SmoothedAngularVelocityGyro.z,
|
|
// SmoothedAngularVelocityAccel.x, SmoothedAngularVelocityAccel.y, SmoothedAngularVelocityAccel.z);
|
|
|
|
bIsSteady = isSteady;
|
|
|
|
return calibrated;
|
|
}
|
|
|
|
inline void AutoCalibration::NoSampleSensorFusion()
|
|
{
|
|
TimeSteadySensorFusion = 0.f;
|
|
SensorFusionSkippedTime = 0.f;
|
|
PreviousAccel = GamepadMotionHelpers::Vec();
|
|
SmoothedPreviousAccel = GamepadMotionHelpers::Vec();
|
|
SmoothedAngularVelocityGyro = GamepadMotionHelpers::Vec();
|
|
SmoothedAngularVelocityAccel = GamepadMotionHelpers::Vec();
|
|
}
|
|
|
|
inline void AutoCalibration::SetCalibrationData(GyroCalibration* calibrationData)
|
|
{
|
|
CalibrationData = calibrationData;
|
|
}
|
|
|
|
inline void AutoCalibration::SetSettings(GamepadMotionSettings* settings)
|
|
{
|
|
Settings = settings;
|
|
}
|
|
|
|
} // namespace GamepadMotionHelpers
|
|
|
|
inline GamepadMotion::GamepadMotion()
|
|
{
|
|
IsCalibrating = false;
|
|
CurrentCalibrationMode = GamepadMotionHelpers::CalibrationMode::Manual;
|
|
Reset();
|
|
AutoCalibration.SetCalibrationData(&GyroCalibration);
|
|
AutoCalibration.SetSettings(&Settings);
|
|
Motion.SetSettings(&Settings);
|
|
}
|
|
|
|
inline void GamepadMotion::Reset()
|
|
{
|
|
GyroCalibration = {};
|
|
Gyro = {};
|
|
RawAccel = {};
|
|
Settings = GamepadMotionSettings();
|
|
Motion.Reset();
|
|
}
|
|
|
|
inline void GamepadMotion::ProcessMotion(float gyroX, float gyroY, float gyroZ,
|
|
float accelX, float accelY, float accelZ, float deltaTime)
|
|
{
|
|
if (gyroX == 0.f && gyroY == 0.f && gyroZ == 0.f &&
|
|
accelX == 0.f && accelY == 0.f && accelZ == 0.f)
|
|
{
|
|
// all zeroes are almost certainly not valid inputs
|
|
return;
|
|
}
|
|
|
|
float accelMagnitude = sqrtf(accelX * accelX + accelY * accelY + accelZ * accelZ);
|
|
|
|
if (IsCalibrating)
|
|
{
|
|
// manual calibration
|
|
PushSensorSamples(gyroX, gyroY, gyroZ, accelMagnitude);
|
|
AutoCalibration.NoSampleSensorFusion();
|
|
AutoCalibration.NoSampleStillness();
|
|
}
|
|
else if (CurrentCalibrationMode & GamepadMotionHelpers::CalibrationMode::Stillness)
|
|
{
|
|
AutoCalibration.AddSampleStillness(GamepadMotionHelpers::Vec(gyroX, gyroY, gyroZ), GamepadMotionHelpers::Vec(accelX, accelY, accelZ), deltaTime, CurrentCalibrationMode & GamepadMotionHelpers::CalibrationMode::SensorFusion);
|
|
AutoCalibration.NoSampleSensorFusion();
|
|
}
|
|
else
|
|
{
|
|
AutoCalibration.NoSampleStillness();
|
|
if (CurrentCalibrationMode & GamepadMotionHelpers::CalibrationMode::SensorFusion)
|
|
{
|
|
AutoCalibration.AddSampleSensorFusion(GamepadMotionHelpers::Vec(gyroX, gyroY, gyroZ), GamepadMotionHelpers::Vec(accelX, accelY, accelZ), deltaTime);
|
|
}
|
|
else
|
|
{
|
|
AutoCalibration.NoSampleSensorFusion();
|
|
}
|
|
}
|
|
|
|
float gyroOffsetX, gyroOffsetY, gyroOffsetZ;
|
|
GetCalibratedSensor(gyroOffsetX, gyroOffsetY, gyroOffsetZ, accelMagnitude);
|
|
|
|
gyroX -= gyroOffsetX;
|
|
gyroY -= gyroOffsetY;
|
|
gyroZ -= gyroOffsetZ;
|
|
|
|
Motion.Update(gyroX, gyroY, gyroZ, accelX, accelY, accelZ, accelMagnitude, deltaTime);
|
|
|
|
Gyro.x = gyroX;
|
|
Gyro.y = gyroY;
|
|
Gyro.z = gyroZ;
|
|
RawAccel.x = accelX;
|
|
RawAccel.y = accelY;
|
|
RawAccel.z = accelZ;
|
|
}
|
|
|
|
// reading the current state
|
|
inline void GamepadMotion::GetCalibratedGyro(float& x, float& y, float& z)
|
|
{
|
|
x = Gyro.x;
|
|
y = Gyro.y;
|
|
z = Gyro.z;
|
|
}
|
|
|
|
inline void GamepadMotion::GetGravity(float& x, float& y, float& z)
|
|
{
|
|
x = Motion.Grav.x;
|
|
y = Motion.Grav.y;
|
|
z = Motion.Grav.z;
|
|
}
|
|
|
|
inline void GamepadMotion::GetProcessedAcceleration(float& x, float& y, float& z)
|
|
{
|
|
x = Motion.Accel.x;
|
|
y = Motion.Accel.y;
|
|
z = Motion.Accel.z;
|
|
}
|
|
|
|
inline void GamepadMotion::GetOrientation(float& w, float& x, float& y, float& z)
|
|
{
|
|
w = Motion.Quaternion.w;
|
|
x = Motion.Quaternion.x;
|
|
y = Motion.Quaternion.y;
|
|
z = Motion.Quaternion.z;
|
|
}
|
|
|
|
inline void GamepadMotion::GetPlayerSpaceGyro(float& x, float& y, const float yawRelaxFactor)
|
|
{
|
|
CalculatePlayerSpaceGyro(x, y, Gyro.x, Gyro.y, Gyro.z, Motion.Grav.x, Motion.Grav.y, Motion.Grav.z, yawRelaxFactor);
|
|
}
|
|
|
|
inline void GamepadMotion::CalculatePlayerSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float yawRelaxFactor)
|
|
{
|
|
// take gravity into account without taking on any error from gravity. Explained in depth at http://gyrowiki.jibbsmart.com/blog:player-space-gyro-and-alternatives-explained#toc7
|
|
const float worldYaw = -(gravY * gyroY + gravZ * gyroZ);
|
|
const float worldYawSign = worldYaw < 0.f ? -1.f : 1.f;
|
|
y = worldYawSign * std::min(std::abs(worldYaw) * yawRelaxFactor, sqrtf(gyroY * gyroY + gyroZ * gyroZ));
|
|
x = gyroX;
|
|
}
|
|
|
|
inline void GamepadMotion::GetWorldSpaceGyro(float& x, float& y, const float sideReductionThreshold)
|
|
{
|
|
CalculateWorldSpaceGyro(x, y, Gyro.x, Gyro.y, Gyro.z, Motion.Grav.x, Motion.Grav.y, Motion.Grav.z, sideReductionThreshold);
|
|
}
|
|
|
|
inline void GamepadMotion::CalculateWorldSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float sideReductionThreshold)
|
|
{
|
|
// use the gravity direction as the yaw axis, and derive an appropriate pitch axis. Explained in depth at http://gyrowiki.jibbsmart.com/blog:player-space-gyro-and-alternatives-explained#toc6
|
|
const float worldYaw = -gravX * gyroX - gravY * gyroY - gravZ * gyroZ;
|
|
// project local pitch axis (X) onto gravity plane
|
|
const float gravDotPitchAxis = gravX;
|
|
GamepadMotionHelpers::Vec pitchAxis(1.f - gravX * gravDotPitchAxis,
|
|
-gravY * gravDotPitchAxis,
|
|
-gravZ * gravDotPitchAxis);
|
|
// normalize
|
|
const float pitchAxisLengthSquared = pitchAxis.LengthSquared();
|
|
if (pitchAxisLengthSquared > 0.f)
|
|
{
|
|
const float pitchAxisLength = sqrtf(pitchAxisLengthSquared);
|
|
const float lengthReciprocal = 1.f / pitchAxisLength;
|
|
pitchAxis *= lengthReciprocal;
|
|
|
|
const float flatness = std::abs(gravY);
|
|
const float upness = std::abs(gravZ);
|
|
const float sideReduction = sideReductionThreshold <= 0.f ? 1.f : std::clamp((std::max(flatness, upness) - sideReductionThreshold) / sideReductionThreshold, 0.f, 1.f);
|
|
|
|
x = sideReduction * pitchAxis.Dot(GamepadMotionHelpers::Vec(gyroX, gyroY, gyroZ));
|
|
}
|
|
else
|
|
{
|
|
x = 0.f;
|
|
}
|
|
|
|
y = worldYaw;
|
|
}
|
|
|
|
// gyro calibration functions
|
|
inline void GamepadMotion::StartContinuousCalibration()
|
|
{
|
|
IsCalibrating = true;
|
|
}
|
|
|
|
inline void GamepadMotion::PauseContinuousCalibration()
|
|
{
|
|
IsCalibrating = false;
|
|
}
|
|
|
|
inline void GamepadMotion::ResetContinuousCalibration()
|
|
{
|
|
GyroCalibration = {};
|
|
AutoCalibration.Reset();
|
|
}
|
|
|
|
inline void GamepadMotion::GetCalibrationOffset(float& xOffset, float& yOffset, float& zOffset)
|
|
{
|
|
float accelMagnitude;
|
|
GetCalibratedSensor(xOffset, yOffset, zOffset, accelMagnitude);
|
|
}
|
|
|
|
inline void GamepadMotion::SetCalibrationOffset(float xOffset, float yOffset, float zOffset, int weight)
|
|
{
|
|
if (GyroCalibration.NumSamples > 1)
|
|
{
|
|
GyroCalibration.AccelMagnitude *= ((float)weight) / GyroCalibration.NumSamples;
|
|
}
|
|
else
|
|
{
|
|
GyroCalibration.AccelMagnitude = (float)weight;
|
|
}
|
|
|
|
GyroCalibration.NumSamples = weight;
|
|
GyroCalibration.X = xOffset * weight;
|
|
GyroCalibration.Y = yOffset * weight;
|
|
GyroCalibration.Z = zOffset * weight;
|
|
}
|
|
|
|
inline float GamepadMotion::GetAutoCalibrationConfidence()
|
|
{
|
|
return AutoCalibration.Confidence;
|
|
}
|
|
|
|
inline void GamepadMotion::SetAutoCalibrationConfidence(float newConfidence)
|
|
{
|
|
AutoCalibration.Confidence = newConfidence;
|
|
}
|
|
|
|
inline bool GamepadMotion::GetAutoCalibrationIsSteady()
|
|
{
|
|
return AutoCalibration.IsSteady();
|
|
}
|
|
|
|
inline GamepadMotionHelpers::CalibrationMode GamepadMotion::GetCalibrationMode()
|
|
{
|
|
return CurrentCalibrationMode;
|
|
}
|
|
|
|
inline void GamepadMotion::SetCalibrationMode(GamepadMotionHelpers::CalibrationMode calibrationMode)
|
|
{
|
|
CurrentCalibrationMode = calibrationMode;
|
|
}
|
|
|
|
inline void GamepadMotion::ResetMotion()
|
|
{
|
|
Motion.Reset();
|
|
}
|
|
|
|
// Private Methods
|
|
|
|
inline void GamepadMotion::PushSensorSamples(float gyroX, float gyroY, float gyroZ, float accelMagnitude)
|
|
{
|
|
// accumulate
|
|
GyroCalibration.NumSamples++;
|
|
GyroCalibration.X += gyroX;
|
|
GyroCalibration.Y += gyroY;
|
|
GyroCalibration.Z += gyroZ;
|
|
GyroCalibration.AccelMagnitude += accelMagnitude;
|
|
}
|
|
|
|
inline void GamepadMotion::GetCalibratedSensor(float& gyroOffsetX, float& gyroOffsetY, float& gyroOffsetZ, float& accelMagnitude)
|
|
{
|
|
if (GyroCalibration.NumSamples <= 0)
|
|
{
|
|
gyroOffsetX = 0.f;
|
|
gyroOffsetY = 0.f;
|
|
gyroOffsetZ = 0.f;
|
|
accelMagnitude = 1.f;
|
|
return;
|
|
}
|
|
|
|
const float inverseSamples = 1.f / GyroCalibration.NumSamples;
|
|
gyroOffsetX = GyroCalibration.X * inverseSamples;
|
|
gyroOffsetY = GyroCalibration.Y * inverseSamples;
|
|
gyroOffsetZ = GyroCalibration.Z * inverseSamples;
|
|
accelMagnitude = GyroCalibration.AccelMagnitude * inverseSamples;
|
|
}
|