Пример #1
0
        /// <summary>
        /// Returns the Euler-Angles from the quatarnion.
        /// </summary>
        public V3d GetEulerAngles()
        {
            var test = W * Y - X * Z;

            if (test > 0.49999) // singularity at north pole
            {
                return(new V3d(
                           -2 * Fun.Atan2(X, W),
                           Constant.PiHalf,
                           0));
            }
            if (test < -0.49999) // singularity at south pole
            {
                return(new V3d(
                           2 * Fun.Atan2(X, W),
                           -Constant.PiHalf,
                           0));
            }
            // From Wikipedia, conversion between quaternions and Euler angles.
            return(new V3d(
                       Fun.Atan2(2 * (W * X + Y * Z),
                                 1 - 2 * (X * X + Y * Y)),
                       Fun.AsinC(2 * test),
                       Fun.Atan2(2 * (W * Z + X * Y),
                                 1 - 2 * (Y * Y + Z * Z))));
        }