Zelda64Recomp/lib/GamepadMotionHelpers/GamepadMotion.hpp

// 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;
}