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 }