Add all the files

src/input/motion/MotionSample.h

@@ -0,0 +1,319 @@
+#pragma once
+#include "util/math/vector3.h"
+#include "util/math/quaternion.h"
+
+struct Quat
+{
+	float w;
+	float x;
+	float y;
+	float z;
+
+	Quat()
+	{
+		w = 1.0f;
+		x = 0.0f;
+		y = 0.0f;
+		z = 0.0f;
+	}
+
+	Quat(float inW, float inX, float inY, float inZ)
+	{
+		w = inW;
+		x = inX;
+		y = inY;
+		z = inZ;
+	}
+
+	static Quat AngleAxis(float inAngle, float inX, float inY, float inZ)
+	{
+		Quat result = Quat(cosf(inAngle * 0.5f), inX, inY, inZ);
+		result.Normalize();
+		return result;
+	}
+
+	void Set(float inW, float inX, float inY, float inZ)
+	{
+		w = inW;
+		x = inX;
+		y = inY;
+		z = inZ;
+	}
+
+	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;
+	}
+
+	friend Quat operator*(Quat lhs, const Quat& rhs)
+	{
+		lhs *= rhs;
+		return lhs;
+	}
+
+	void Normalize()
+	{
+		//printf("Normalizing: %.4f, %.4f, %.4f, %.4f\n", w, x, y, z);
+		const float length = sqrtf(x * x + y * y + z * z);
+		float targetLength = 1.0f - w * w;
+		if (targetLength <= 0.0f || length <= 0.0f)
+		{
+			Set(1.0f, 0.0f, 0.0f, 0.0f);
+			return;
+		}
+		targetLength = sqrtf(targetLength);
+		const float fixFactor = targetLength / length;
+
+		x *= fixFactor;
+		y *= fixFactor;
+		z *= fixFactor;
+
+		//printf("Normalized: %.4f, %.4f, %.4f, %.4f\n", w, x, y, z);
+		return;
+	}
+
+	Quat Normalized() const
+	{
+		Quat result = *this;
+		result.Normalize();
+		return result;
+	}
+
+	void Conjugate()
+	{
+		x = -x;
+		y = -y;
+		z = -z;
+		return;
+	}
+
+	Quat Conjugated() const
+	{
+		Quat result = *this;
+		result.Conjugate();
+		return result;
+	}
+};
+
+// helper class to store unified motion data
+// supports retrieving values in their API-specific (VPAD, KPAD etc.) format
+class MotionSample
+{
+public:
+	MotionSample()
+	{
+	}
+
+	MotionSample(float acc[3], float accAcceleration, float gyro[3], float orientation[3],
+		float quaternion[4]
+	)
+	{
+		m_acc[0] = acc[0];
+		m_acc[1] = acc[1];
+		m_acc[2] = acc[2];
+		m_accAcceleration = accAcceleration;
+		m_gyro[0] = gyro[0];
+		m_gyro[1] = gyro[1];
+		m_gyro[2] = gyro[2];
+		m_orientation[0] = orientation[0];
+		m_orientation[1] = orientation[1];
+		m_orientation[2] = orientation[2];
+		m_q[0] = quaternion[0];
+		m_q[1] = quaternion[1];
+		m_q[2] = quaternion[2];
+		m_q[3] = quaternion[3];
+		m_accMagnitude = sqrtf(m_acc[0] * m_acc[0] + m_acc[1] * m_acc[1] + m_acc[2] * m_acc[2]);
+	}
+
+	void getVPADOrientation(float orientation[3])
+	{
+		orientation[0] = m_orientation[0];
+		orientation[1] = m_orientation[1];
+		orientation[2] = m_orientation[2];
+	}
+
+	void getVPADGyroChange(float gyro[3])
+	{
+		// filter noise
+		if (fabs(gyro[0]) < 0.012f)
+			gyro[0] = 0.0f;
+		if (fabs(gyro[1]) < 0.012f)
+			gyro[1] = 0.0f;
+		if (fabs(gyro[2]) < 0.012f)
+			gyro[2] = 0.0f;
+		// convert
+		gyro[0] = _radToOrientation(-m_gyro[0]);
+		gyro[1] = _radToOrientation(-m_gyro[1]);
+		gyro[2] = _radToOrientation(m_gyro[2]);
+	}
+
+	void getVPADAccelerometer(float acc[3])
+	{
+		acc[0] = -m_acc[0];
+		acc[1] = -m_acc[1];
+		acc[2] = m_acc[2];
+	}
+
+	float getVPADAccMagnitude()
+	{
+		return m_accMagnitude;
+	}
+
+	float getVPADAccAcceleration() // Possibly not entirely correct. Our results are smaller than VPAD API ones
+	{
+		return m_accAcceleration;
+	}
+
+	void getVPADAccXY(float accXY[2])
+	{
+		float invMag = 1.0f / m_accMagnitude;
+		float normAcc[3];
+		normAcc[0] = m_acc[0] * invMag;
+		normAcc[1] = m_acc[1] * invMag;
+		normAcc[2] = m_acc[2] * invMag;
+		accXY[0] = sqrtf(normAcc[0] * normAcc[0] + normAcc[1] * normAcc[1]);
+		accXY[1] = -sin(getAtanPitch(-normAcc[2], normAcc[0], -normAcc[1]));
+	}
+
+	void getXVector(float vOut[3], Quaternionf& q)
+	{
+		float X = q.x;
+		float Y = q.y;
+		float Z = q.z;
+		float W = q.w;
+		float xy = X * Y;
+		float xz = X * Z;
+		float yy = Y * Y;
+		float yw = Y * W;
+		float zz = Z * Z;
+		float zw = Z * W;
+		vOut[0] = 1.0f - 2.0f * (yy + zz); // x.x
+		vOut[2] = 2.0f * (xy + zw); // x.y
+		vOut[1] = 2.0f * (xz - yw); // x.z
+	}
+
+	void getVPADAttitudeMatrix(float mtx[9])
+	{
+		// VPADs attitude matrix has mixed axis handedness, the most sane way to replicate it is by generating Y and Z by rotating the X vector
+		Quaternionf qImu(m_q[0], m_q[1], m_q[2], m_q[3]);
+		Quaternionf qY = qImu * Quaternionf::FromAngleAxis(1.5708f * 1.0f, 0.0f, 0.0f, 1.0f);
+		Quaternionf qZ = qImu * Quaternionf::FromAngleAxis(1.5708f * 1.0f, 0.0f, 1.0f, 0.0f);
+ 		getXVector(mtx + 0, qImu);
+		getXVector(mtx + 3, qY);
+		getXVector(mtx + 6, qZ);
+	}
+
+	static float calculateAccAcceleration(float prevAcc[3], float currentAcc[3])
+	{
+		float ax = currentAcc[0] - prevAcc[0];
+		float ay = currentAcc[1] - prevAcc[1];
+		float az = currentAcc[2] - prevAcc[2];
+		return sqrtf(ax * ax + ay * ay + az * az);
+	}
+
+	void getAccelerometer(float acc[3])
+	{
+		acc[0] = m_acc[0];
+		acc[1] = m_acc[1];
+		acc[2] = m_acc[2];
+	}
+	void getGyrometer(float gyro[3])
+	{
+		gyro[0] = m_gyro[0];
+		gyro[1] = m_gyro[1];
+		gyro[2] = m_gyro[2];
+	}
+
+private:
+	static float _radToOrientation(float rad)
+	{
+		return rad / (2.0f * 3.14159265f);
+	}
+
+	static float getAtanPitch(float X, float Y, float Z)
+	{
+		return atan2f(-X, sqrtf(Y * Y + Z * Z));
+	}
+
+	// provided values
+	float m_gyro[3]{};
+	float m_acc[3]{};
+	float m_accAcceleration{};
+	float m_orientation[3]{};
+	float m_q[4]{};
+	// calculated values
+	float m_accMagnitude{};
+};
+
+/*
+
+Captured VPAD attitude values
+
+Assuming GamePad is in a direct line between player (holder) and the monitor
+
+DRC flat on table, screen facing up. Top pointing away (away from person, pointing towards monitor):
+1.00   -0.03   -0.00
+0.03    0.99   -0.13
+0.01    0.13    0.99
+
+Turned 45° to the right:
+ 0.71   -0.03    0.71
+ 0.12    0.99   -0.08
+-0.70    0.14    0.70
+
+Turned 45° to the right (top of GamePad pointing right now):
+ 0.08   -0.03    1.00			-> Z points towards person
+ 0.15    0.99    0.01
+-0.99    0.15    0.09			-> DRC Z-Axis now points towards X-minus
+
+Turned 90° to the right (top of gamepad now pointing towards holder, away from monitor):
+-1.00   -0.01    0.06
+ 0.00    0.99    0.15
+-0.06    0.15   -0.99
+
+Turned 90° to the right (pointing left):
+-0.17   -0.01   -0.99
+-0.13    0.99    0.02
+ 0.98    0.13   -0.17
+
+After another 90° we end up in the initial position:
+ 0.99   -0.03   -0.11
+ 0.01    0.99   -0.13
+ 0.12    0.12    0.99
+
+------
+From initial position, lean the GamePad on its left side. 45° up. So the screen is pointing to the top left
+ 0.66   -0.75   -0.03
+ 0.74    0.66   -0.11
+ 0.10    0.05    0.99
+
+Further 45°, GamePad now on its left, screen pointing left:
+-0.03   -1.00   -0.00
+ 0.99   -0.03   -0.15
+ 0.15   -0.01    0.99
+
+From initial position, lean the GamePad on its right side. 45° up. So the screen is pointing to the top right
+ 0.75    0.65   -0.11
+-0.65    0.76    0.07
+ 0.12    0.02    0.99
+
+From initial position, tilt the GamePad up 90° (bottom side remains in touch with surface):
+ 0.99   -0.05   -0.10
+-0.10    0.01   -0.99
+ 0.05    1.00    0.01
+
+From initial position, stand the GamePad on its top side:
+ 1.00   -0.01   -0.09
+ 0.09   -0.01    1.00
+-0.01   -1.00   -0.01
+
+Rotate GamePad 180° around x axis, so it now lies on its screen (top of GamePad pointing to holder):
+ 0.99   -0.03   -0.15
+-0.04   -1.00   -0.08
+-0.15    0.09   -0.99
+
+*/