Ejemplo n.º 1
0
        /// <summary>
        /// Converts a quaternion to an Euler rotation vector in Radians
        /// </summary>
        /// <param name="quat">The quaternion to convert to euler angles</param>
        /// <returns>Vector with x=roll, y=heading, z = pitch</returns>
        public static Vector3 QuaternionToEulerRadians(pm.Quaternion quat)
        {
            double qx      = quat.X;
            double qy      = quat.Y;
            double qz      = quat.Z;
            double qw      = quat.W;
            double heading = Math.Atan2((2 * qy * qw) - (2 * qx * qz), 1 - (2 * qy * qy) - (2 * qz * qz));
            double sin     = (2 * qx * qy) + (2 * qz * qw);

            // precision problems can rarely move this value slightly out of the valid range -1 <= sin <= 1
            // clamp it
            if (sin < -1)
            {
                sin = -1;
            }
            else if (sin > 1)
            {
                sin = 1;
            }

            double pitch = Math.Asin(sin);
            double roll  = Math.Atan2((2 * qx * qw) - (2 * qy * qz), 1 - (2 * qx * qx) - (2 * qz * qz));

            return(new Vector3((float)roll, (float)heading, (float)pitch));
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Converts an Euler rotation vector in Radians to a quaternion
        /// </summary>
        /// <param name="value">Vector with x=roll, y=heading, z = pitch</param>
        /// <returns>A quaternion representing the euler angles in value</returns>
        public static pm.Quaternion EulerRadiansToQuaternion(Vector3 value)
        {
            // prevent conversion errors (gimbal lock) by disallowing 0 degree values
            if (value.X == 0)
            {
                value.X = 0.00001f;
            }

            if (value.Y == 0)
            {
                value.Y = 0.00001f;
            }

            if (value.Z == 0)
            {
                value.Z = 0.00001f;
            }

            double heading = value.Y;
            double pitch   = value.Z;
            double roll    = value.X;
            double c1      = Math.Cos(heading / 2);
            double c2      = Math.Cos(pitch / 2);
            double c3      = Math.Cos(roll / 2);
            double s1      = Math.Sin(heading / 2);
            double s2      = Math.Sin(pitch / 2);
            double s3      = Math.Sin(roll / 2);

            Microsoft.Robotics.PhysicalModel.Quaternion quat = new Microsoft.Robotics.PhysicalModel.Quaternion();
            quat.W = (float)((c1 * c2 * c3) - (s1 * s2 * s3));
            quat.X = (float)((s1 * s2 * c3) + (c1 * c2 * s3));
            quat.Y = (float)((s1 * c2 * c3) + (c1 * s2 * s3));
            quat.Z = (float)((c1 * s2 * c3) - (s1 * c2 * s3));

            return(quat);
        }