示例#1
0
        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;
        }
示例#2
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
        }
示例#3
0
        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;
        }
示例#4
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;
        }
示例#5
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
 }