Quaternion.cs

using System;
using Game.Shared.MathHelpers;
using UnityEngine;

#if !UNITY
namespace UnityEngine
{
    public struct Quaternion
    {
        public float x;
        public float y;
        public float z;
        public float w;

        private const float Tolerance = 0.0001f;
        private const double Piover360 = MathUtils.DegToRad_D * 0.5;

        public override string ToString()
        {
            return string.Format("({0} {1} {2} {3})", x, y, z, w);
        }

        public Quaternion(float x, float y, float z, float w)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }

        public static Quaternion identity
        {
            get { return new Quaternion(0f,0f,0f,1f); }
        }

        public void Normalize()
        {
            float mag2 = w*w + x*x + y*y + z*z;
            if (Math.Abs(mag2) > Tolerance && Math.Abs(mag2 - 1.0f) > Tolerance)
            {
                float mag = (float) Math.Sqrt(mag2);
                w /= mag;
                x /= mag;
                y /= mag;
                z /= mag;
            }
        }

        public static float Dot(Quaternion a, Quaternion b)
        {
            return (float)((double)a.x * (double)b.x + (double)a.y * (double)b.y + (double)a.z * (double)b.z + (double)a.w * (double)b.w);
        }

        public static Quaternion Lerp(Quaternion q1, Quaternion q2, float t)
        {
            return LerpUnclamped(q1, q2, MathUtils.Clamp(t, 0f, 1f));
        }

        public static Quaternion LerpUnclamped(Quaternion q1, Quaternion q2, float t)
        {
            Quaternion r = new Quaternion
            {
                x = q1.x + t*(q2.x - q1.x),
                y = q1.y + t*(q2.y - q1.y),
                z = q1.z + t*(q2.z - q1.z),
                w = q1.w + t*(q2.w - q1.w)
            };
            r.Normalize();
            return r;
        }

        public static float Angle(Quaternion q1, Quaternion q2)
        {
            double dot = (double)q1.x * q2.x + (double)q1.y * q2.y + (double)q1.z * q2.z + (double)q1.w * q2.w;
            return (float)(Math.Acos(Math.Min(Math.Abs(dot), 1.0)) * 2.0 * MathUtils.RadToDeg_D);
        }

        public static Quaternion RotateTowards(Quaternion from, Quaternion to, float maxDegreesDelta)
        {
            float num = Angle(from, to);
            if (num == 0f)
                return to;
            float t = maxDegreesDelta / num;
            return SlerpUnclamped(from, to, t > 1f ? 1f : t);
        }

        public static Quaternion Slerp(Quaternion q1, Quaternion q2, float t)
        {
            return SlerpUnclamped(q1, q2, MathUtils.Clamp(t, 0f, 1f));
        }

        public static Quaternion SlerpUnclamped(Quaternion q1, Quaternion q2, float t)
        {
            q1.Normalize();
            q2.Normalize();
            double dot = (double)q1.x * q2.x + (double)q1.y * q2.y + (double)q1.z * q2.z + (double)q1.w * q2.w;

            if (dot < 0f)
            {
                dot *= -1f;
                q2.x = -q2.x;
                q2.y = -q2.y;
                q2.z = -q2.z;
                q2.w = -q2.w;
            }
            if (Math.Abs(dot) < 1e-10)
            {
                dot = 1e-10;
            }

            double angle = Math.Acos(dot);
            double sina = Math.Sin(angle);
            float st0 = (float)(Math.Sin(angle * (1f - t)) / sina);
            float st1 = (float)(Math.Sin(angle * t) / sina);

            Quaternion r = new Quaternion
            {
                x = q1.x*st0 + q2.x*st1,
                y = q1.y*st0 + q2.y*st1,
                z = q1.z*st0 + q2.z*st1,
                w = q1.w*st0 + q2.w*st1
            };
            return r;
        }

        public void ToAxisAngle(ref Vector3 axis, ref float angle)
        {
            float scale = (float) Math.Sqrt(x*x + y*y + z*z);
            axis.x = x/scale;
            axis.y = y/scale;
            axis.z = z/scale;
            angle = (float) Math.Acos(w)*2.0f;
        }

        public static Quaternion FromAxis(Vector3 v, float angle)
        {
            angle *= 0.5f;
            v.Normalize();
            float sinAngle = (float)Math.Sin(angle);

            return new Quaternion(v.x*sinAngle, v.y*sinAngle, v.z*sinAngle, (float) Math.Cos(angle));
        }

        public Vector3 eulerAngles
        {
            get
            {
                double ex = MathUtils.RadToDeg_D * Math.Asin(2.0 * (w * x - y * z));
                double ey = MathUtils.RadToDeg_D * Math.Atan2(2.0 * w * y + 2.0 * z * x, 1.0 - 2.0 * (x * x + y * y));
                double ez = MathUtils.RadToDeg_D * Math.Atan2(2.0 * w * z + 2.0 * x * y, 1.0 - 2.0 * (z * z + x * x));

                while (ex < 0.0)
                    ex += 360.0;
                while (ey < 0.0)
                    ey += 360.0;
                while (ez < 0.0)
                    ez += 360.0;

                return new Vector3((float)ex, (float)ey, (float)ez);
            }
        }

        public static Quaternion Euler(Vector3 euler)
        {
            double y = euler.x * Piover360;
            double p = euler.y * Piover360;
            double r = euler.z * Piover360;

            float cosYaw = (float)Math.Cos(y);
            float sinYaw = (float)Math.Sin(y);
            float cosPitch = (float)Math.Cos(p);
            float sinPitch = (float)Math.Sin(p);
            float cosRoll = (float)Math.Cos(r);
            float sinRoll = (float)Math.Sin(r);
            Quaternion result;
            result.x = sinYaw * cosPitch * cosRoll + cosYaw * sinPitch * sinRoll;
            result.y = cosYaw * sinPitch * cosRoll - sinYaw * cosPitch * sinRoll;
            result.z = cosYaw * cosPitch * sinRoll - sinYaw * sinPitch * cosRoll;
            result.w = cosYaw * cosPitch * cosRoll + sinYaw * sinPitch * sinRoll;
            return result;
        }

        public Quaternion Conjugate
        {
            get { return new Quaternion(-x, -y, -z, w); }
        }

        public static Quaternion operator *(Quaternion q1, Quaternion q2)
        {
            return new Quaternion(
                q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y,
                q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z,
                q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x,
                q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z);
        }

        public static Vector3 operator *(Quaternion q, Vector3 v)
        {
            if (v.x == 0f && v.y == 0f && v.z == 0f)
                return v;

            Quaternion vecQuat, resQuat;
            vecQuat.x = v.x;
            vecQuat.y = v.y;
            vecQuat.z = v.z;
            vecQuat.w = 0.0f;

            resQuat = q*(vecQuat*q.Conjugate);

            return new Vector3(resQuat.x, resQuat.y, resQuat.z);
        }

        public static Quaternion LookRotation(Vector3 forward, Vector3 up)
        {
            Vector3 vector = forward.normalized;
            Vector3 vector2 = Vector3.Cross(up, vector).normalized;
            Vector3 vector3 = Vector3.Cross(vector, vector2).normalized;
            var m00 = vector2.x;
            var m01 = vector2.y;
            var m02 = vector2.z;
            var m10 = vector3.x;
            var m11 = vector3.y;
            var m12 = vector3.z;
            var m20 = vector.x;
            var m21 = vector.y;
            var m22 = vector.z;

            float num8 = (m00 + m11) + m22;
            Quaternion quaternion = new Quaternion();
            if (num8 > 0f)
            {
                var num = (float)Math.Sqrt(num8 + 1f);
                quaternion.w = num * 0.5f;
                num = 0.5f / num;
                quaternion.x = (m12 - m21) * num;
                quaternion.y = (m20 - m02) * num;
                quaternion.z = (m01 - m10) * num;
                return quaternion;
            }
            if ((m00 >= m11) && (m00 >= m22))
            {
                var num7 = (float)Math.Sqrt(((1f + m00) - m11) - m22);
                var num4 = 0.5f / num7;
                quaternion.x = 0.5f * num7;
                quaternion.y = (m01 + m10) * num4;
                quaternion.z = (m02 + m20) * num4;
                quaternion.w = (m12 - m21) * num4;
                return quaternion;
            }
            if (m11 > m22)
            {
                var num6 = (float)Math.Sqrt(((1f + m11) - m00) - m22);
                var num3 = 0.5f / num6;
                quaternion.x = (m10 + m01) * num3;
                quaternion.y = 0.5f * num6;
                quaternion.z = (m21 + m12) * num3;
                quaternion.w = (m20 - m02) * num3;
                return quaternion;
            }
            var num5 = (float)Math.Sqrt(((1f + m22) - m00) - m11);
            var num2 = 0.5f / num5;
            quaternion.x = (m20 + m02) * num2;
            quaternion.y = (m21 + m12) * num2;
            quaternion.z = 0.5f * num5;
            quaternion.w = (m01 - m10) * num2;
            return quaternion;
        }
    }
}
#endif

public static class QuaternionExtensions
{
    public static float GetEulerY(this Quaternion q)
    {
        return MathUtils.RadToDeg * (float)Math.Atan2(2f * q.y * q.w - 2f * q.x * q.z, 1 - 2f * (q.y * q.y + q.z * q.z));
    }

    public static float GetEulerYRad(this Quaternion q)
    {
        return (float)Math.Atan2(2f * q.y * q.w - 2f * q.x * q.z, 1 - 2f * (q.y * q.y + q.z * q.z));
    }
}