public int DMPGetYawPitchRoll(float[] data, Quaternion q, float[] gravity)
{
// yaw: (about Z axis)
data[0] = (float) System.Math.Atan2(2 * q.X * q.Y - 2 * q.W * q.Z, 2 * q.W * q.W + 2 * q.X * q.X - 1);
// pitch: (nose up/down, about Y axis)
data[1] = (float) System.Math.Atan(gravity[0] / (float) System.Math.Sqrt(gravity[1] * gravity[1] + gravity[2] * gravity[2]));
// roll: (tilt left/right, about X axis)
data[2] = (float) System.Math.Atan(gravity[1] / (float) System.Math.Sqrt(gravity[0] * gravity[0] + gravity[2] * gravity[2]));
return 0;
}
public int DMPGetQuaternion(ref Quaternion q, byte[] buffer)
{
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
short[] tmp = new short[4];
int status = DMPGetQuaternion(tmp, buffer);
if (status == 0) {
q = new Quaternion(
(float) (tmp[0] / 16384.0f),
(float) (tmp[1] / 16384.0f),
(float) (tmp[2] / 16384.0f),
(float) (tmp[3] / 16384.0f)
);
return 0;
}
return status; // int16 return value, indicates error if this line is reached
}
public int DMPGetEuler(float[] data, Quaternion q)
{
data[0] = (float) System.Math.Atan2(2 * q.X * q.Y - 2 * q.W * q.Z, 2 * q.W * q.W + 2 * q.X * q.X - 1); // psi
data[1] = (float) -System.Math.Asin(2 * q.X * q.Z + 2 * q.W * q.Y); // theta
data[2] = (float) System.Math.Atan2(2 * q.Y * q.Z - 2 * q.W * q.X, 2 * q.W * q.W + 2 * q.Z * q.Z - 1); // phi
return 0;
}
public int DMPGetGravity(float[] v, Quaternion q)
{
v[0] = 2 * (q.X * q.Z - q.W * q.Y);
v[1] = 2 * (q.W * q.X + q.Y * q.Z);
v[2] = 2 * (q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z);
return 0;
}
public Quaternion GetProduct(Quaternion q)
{
// Quaternion multiplication is defined by:
// (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
// (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
// (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
// (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
return new Quaternion(
(_w * q.W) - (_x * q.X) - (_y * q.Y) - (_z * q.Z), // new w
(_w * q.X) + (_x * q.W) + (_y * q.Z) - (_z * q.Y), // new x
(_w * q.Y) - (_x * q.Z) + (_y * q.W) + (_z * q.X), // new y
(_w * q.Z) + (_x * q.Y) - (_y * q.X) + (_z * q.W)); // new z
}
