3D圖形學在游戲開發中的，矩陣，四元數，歐拉角之間的底層轉換

在游戲開發的過程中難免會遇到歐拉角和四元數直接的轉換問題，如果有些過shader的朋友，肯定也遇到過四元數，歐拉角和矩陣直接的轉換問題，這里我把這幾種格式直接的轉換算法寫在這里有需要的朋友可以拿去有，別忘了，點贊關注。廢話不多說，直接上代碼、

四元數轉矩陣的底層算法：

public Quaternion QuaternionMatrix(float w, float x, float y, float z)
    {
        Matrix4x4 matrix = new Matrix4x4();

        matrix.m00 = 1f - 2 * SetSquare(y) - 2 * SetSquare(z);
        matrix.m01 = 2f * (x * y) - 2f * (w * z);
        matrix.m02 = 2f * (x * z) + 2f * (w * y);
        matrix.m03 = 0.0f;

        matrix.m10 = 2f * (x * y) + 2f * (w * z);
        matrix.m11 = 1f - 2f * SetSquare(x) - 2f * SetSquare(z);
        matrix.m12 = 2f * (y * z) - 2f * (w * x);
        matrix.m13 = 0.0f;

        matrix.m20 = 2f * (x * z) - 2f * (w * y);
        matrix.m21 = 2f * (y * z) + 2f * (w * x);
        matrix.m22 = 1f - 2f * SetSquare(x) - 2f * SetSquare(y);
        matrix.m23 = 0.0f;

        matrix.m30 = 0.0f;
        matrix.m31 = 0.0f;
        matrix.m32 = 0.0f;
        matrix.m33 = 0.0f;

        float qw = Mathf.Sqrt(1f + matrix.m00 + matrix.m11 + matrix.m22) / 2;
        float wq = 4 * qw;
        float qx = (matrix.m21 - matrix.m12) / wq;
        float qy = (matrix.m02 - matrix.m20) / wq;
        float qz = (matrix.m10 - matrix.m01) / wq;
        return new Quaternion(qx, qy, qz, qw);
    }

矩陣轉四元數的底層算法：

 public Quaternion MatrixToQuaternion(Matrix4x4 matrix)
    {
        float qw = Mathf.Sqrt(1f + matrix.m00 + matrix.m11 + matrix.m22) / 2;
        float w = 4 * qw;
        float qx = (matrix.m21 - matrix.m12) / w;
        float qy = (matrix.m02 - matrix.m20) / w;
        float qz = (matrix.m10 - matrix.m01) / w;
        return new Quaternion(qx, qy, qz, qw);
    }

四元數轉歐拉角的底層算法實現：
這里的四元數轉歐拉角我要特別做一下說明，這里我有四種實現方式，其中有三種是解決個別角度問題的

可以直接拿去用的算法

public Vector3 QauToE4(float x_, float y_, float z_, float w_)
    {

>         float check = 2.0f * (-y_ * z_ + w_ * x_);****
        if (check < -0.995f)
        {
            return new Vector3(
                -90.0f,
                0.0f,
                -Mathf.Atan2(2.0f * (x_ * z_ - w_ * y_), 1.0f - 2.0f * (y_ * y_ + z_ * z_)) * M_RADTODEG
            );
        }
        else if (check > 0.995f)
        {
            return new Vector3(
                90.0f,
                0.0f,
                Mathf.Atan2(2.0f * (x_ * z_ - w_ * y_), 1.0f - 2.0f * (y_ * y_ + z_ * z_)) * M_RADTODEG
            );
        }
        else
        {
            return new Vector3(
              Mathf.Asin(check) * M_RADTODEG,
                Mathf.Atan2(2.0f * (x_ * z_ + w_ * y_), 1.0f - 2.0f * (x_ * x_ + y_ * y_)) * M_RADTODEG,
                Mathf.Atan2(2.0f * (x_ * y_ + w_ * z_), 1.0f - 2.0f * (x_ * x_ + z_ * z_)) * M_RADTODEG
            );
        }
    }

解決個別角度問題的算法

public Vector3 QuaToE(float x, float y, float z, float w)
    {
        float h, p, b;

        float sp = -2.0f * (y * z + w * x);
        if (Mathf.Abs(sp) > 0.9999f)
        {
            p = 1.570796f * sp;

            h = Mathf.Atan2(-x * z - w * y, 0.5f - y * y - z * z);
            b = 0.0f;
        }
        else
        {
            p = Mathf.Asin(sp);
            h = Mathf.Atan2(x * z - w * y, 0.5f - x * x - y * y);
            b = Mathf.Atan2(x * y - w * z, 0.5f - x * x - z * z);
        }
        return new Vector3(h, p, b);
    }

    public Vector3 QuaToE2(float x, float y, float z, float w)
    {
        float h, p, b;
        float sp = -2.0f * (y * z - w * x);

        if (Mathf.Abs(sp) > 0.9999f)
        {
            p = 1.570796f * sp;
            h = Mathf.Atan2(-x * z + w * y, 0.5f - y * y - z * z);
            b = 0.0f;
        }
        else
        {
            p = Mathf.Asin(sp);
            h = Mathf.Atan2(x * z + w * y, 0.5f - x * x - y * y);
            b = Mathf.Atan2(x * y + w * z, 0.5f - x * x - z * z);
        }
        return new Vector3(h, p, b);
    }
    public Vector3 QauToE3(float x, float y, float z, float w)
    {

        Vector3 euler;
        const float Epsilon = 0.0009765625f;
        const float Threshold = 0.5f - Epsilon;

        float TEST = w * y - x * z;

        if (TEST < -Threshold || TEST > Threshold) // 奇異姿態,俯仰角為±90°
        {
            float sign = Mathf.Sign(TEST);

            euler.z = -2 * sign * (float)Mathf.Atan2(x, w); // yaw

            euler.y = sign * (float)(3.1415926f / 2.0); // pitch

            euler.x = 0; // roll

        }
        else
        {
            euler.x = Mathf.Atan2(2 * (y * z + w * x), w * w - x * x - y * y + z * z);
            euler.y = Mathf.Asin(-2 * (x * z - w * y));
            euler.z = Mathf.Atan2(2 * (x * y + w * z), w * w + x * x - y * y - z * z);
        }
        return euler;
    }

歐拉角轉四元數算法：

public Quaternion E4ToQua(float x, float y, float z)
    {
        float w_, x_, y_, z_;
        x *= M_DEGTORAD_2;
        y *= M_DEGTORAD_2;
        z *= M_DEGTORAD_2;
        float sinX = Mathf.Sin(x);
        float cosX = Mathf. Cos(x);
        float sinY = Mathf.Sin(y);
        float cosY = Mathf.Cos(y);
        float sinZ = Mathf.Sin(z);
        float cosZ = Mathf.Cos(z);

        w_ = cosY * cosX * cosZ + sinY * sinX * sinZ;
        x_ = cosY * sinX * cosZ + sinY * cosX * sinZ;
        y_ = sinY * cosX * cosZ - cosY * sinX * sinZ;
        z_ = cosY * cosX * sinZ - sinY * sinX * cosZ;
        return new Quaternion(x_, y_, z_, w_);
    }

算法測試：
這里使用的unity進行的測試，不過這里提供的算法是通用的。