3D图形学在游戏开发中的，矩阵，四元数，欧拉角之间的底层转换算法。

Posted 2021-11-25

在游戏开发的过程中难免会遇到欧拉角和四元数直接的转换问题，如果有些过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进行的测试，不过这里提供的算法是通用的。