using Microsoft.Xna.Framework;

namespace KavTest.Extensions
{
    public static class QuaternionExtensions
    {
        public static Vector3 EulerAngles(this Quaternion q)
        {
            Vector3 angles;

            double sinr_cosp = 2 * (q.W * q.X + q.Y * q.Z);
            double cosr_cosp = 1 - 2 * (q.X * q.X + q.Y * q.Y);
            angles.X = (float) System.Math.Atan2(sinr_cosp, cosr_cosp);

            double sinp = 2 * (q.W * q.Y - q.Z * q.X);
            if (System.Math.Abs(sinp) >= 1)
            {
                angles.Y = (float) System.Math.PI / 2 * System.Math.Sign(sinp);
            }
            else
            {
                angles.Y = (float) System.Math.Asin(sinp);
            }

            double siny_cosp = 2 * (q.W * q.Z + q.X * q.Y);
            double cosy_cosp = 1 - 2 * (q.Y * q.Y + q.Z * q.Z);
            angles.Z = (float) System.Math.Atan2(siny_cosp, cosy_cosp);

            return angles;
        }
    }
}