unity-doc-矩阵与四元数

矩阵

空间矩阵

空间矩阵，其实就是一个坐标系，它是以一个标准的3维坐标系做参考，第1列的前3个值，是x轴在标准坐标系的方向矢量；第2列的前3个值，是y轴在标准坐标系的方向矢量；第3列的前3个值，是z轴在标准坐标系的方向矢量；

4*4矩阵的最后一个变量0表示该矩阵表示有方向，1表示无方向（点）

原矩阵用于将本地空间坐标转换成世界空间坐标，逆矩阵用于将世界空间坐标转换成本地空间坐标。

标准矩阵图示

围绕x轴旋转30度的矩阵图示

逆矩阵

某矩阵旋转一定角度的逆矩阵就是，该矩阵反向旋转该矩阵后得到的矩阵
标准矩阵的逆矩阵就是自身，因为他旋转0度和反旋转0度是一样的

围绕x轴旋转30度的逆矩阵图示

unity矩阵实现

相关公式看 graphics-doc-数学

XMatrix

using System;
using UnityEngine;

namespace XMath
{
    /// <summary>
    /// 矩阵公式类
    /// </summary>
    public static class XMatrix
    {
        /// <summary>
        /// 计算本地坐标转换成世界坐标
        /// </summary>
        /// <param name="reference">参考对象，也就是要计算本地坐标的父亲节点</param>
        /// <param name="localPos">要计算的本地坐标</param>
        /// <returns>转换后的世界坐标</returns>
        public static Matrix4x4 MakeLocalPosToWorldPosMatrix(Transform reference)
        {
            Vector3 referenceWorldPos = reference.position;
            Vector3 referenceAngle = reference.eulerAngles;
            Vector3 referenceScale = reference.localScale;
            float radianX = referenceAngle.x * Mathf.PI / 180;
            float radianY = referenceAngle.y * Mathf.PI / 180;
            float radianZ = referenceAngle.z * Mathf.PI / 180;
            
            Matrix4x4 matrixTrans = new Matrix4x4(
                new Vector4(1, 0, 0, 0),
                new Vector4(0, 1, 0, 0),
                new Vector4(0, 0, 1, 0),
                new Vector4(referenceWorldPos.x, referenceWorldPos.y, referenceWorldPos.z, 1)
            );
            
            Matrix4x4 matrixRotZ = new Matrix4x4(
                new Vector4(Mathf.Cos(radianZ), Mathf.Sin(radianZ), 0, 0),
                new Vector4(-Mathf.Sin(radianZ), Mathf.Cos(radianZ), 0, 0),
                new Vector4(0, 0, 1, 0),
                new Vector4(0, 0, 0, 1));
 
            Matrix4x4 matrixRotX = new Matrix4x4(
                new Vector4(1, 0, 0, 0),
                new Vector4(0, Mathf.Cos(radianX), Mathf.Sin(radianX), 0),
                new Vector4(0, -Mathf.Sin(radianX), Mathf.Cos(radianX), 0),
                new Vector4(0, 0, 0, 1));
 
            Matrix4x4 matrixRotY = new Matrix4x4(
                new Vector4(Mathf.Cos(radianY), 0, -Mathf.Sin(radianY), 0),
                new Vector4(0, 1, 0, 0),
                new Vector4(Mathf.Sin(radianY), 0, Mathf.Cos(radianY), 0),
                new Vector4(0, 0, 0, 1));
 
            Matrix4x4 matrixScale = (new Matrix4x4(
                new Vector4(referenceScale.x, 0, 0, 0),
                new Vector4(0, referenceScale.y, 0, 0),
                new Vector4(0, 0, referenceScale.z, 0),
                new Vector4(0, 0, 0, 1)
            )) ;

            // 矩阵运算，从最右边开始
            // Matrix4x4 mw = matrixRotZ * matrixScale;
            // mw = matrixRotX * mw;
            // mw = matrixRotY * mw;
            // mw = matrixTrans * mw;
            Matrix4x4 mw = matrixTrans * matrixRotY * matrixRotX * matrixRotZ * matrixScale;
            return mw;
        }
       
        /// <summary>
        /// 构建世界到本地坐标矩阵
        /// </summary>
        /// <param name="reference"></param>
        /// <returns></returns>
        public static Matrix4x4 MakeWorldToLocalPosMatrix(Transform reference)
        {
            Vector3 referenceWorldPos = reference.position;
            Vector3 referenceAngle = reference.eulerAngles;
            Vector3 referenceScale = reference.localScale;
            float radianX = referenceAngle.x * Mathf.PI / 180;
            float radianY = referenceAngle.y * Mathf.PI / 180;
            float radianZ = referenceAngle.z * Mathf.PI / 180;
            
            Matrix4x4 matrixTrans = new Matrix4x4(
                new Vector4(1, 0, 0, 0),
                new Vector4(0, 1, 0, 0),
                new Vector4(0, 0, 1, 0),
                new Vector4(-referenceWorldPos.x, -referenceWorldPos.y, -referenceWorldPos.z, 1)
            );
            
            Matrix4x4 matrixRotZ = new Matrix4x4(
                new Vector4(Mathf.Cos(-radianZ), Mathf.Sin(-radianZ), 0, 0),
                new Vector4(-Mathf.Sin(-radianZ), Mathf.Cos(-radianZ), 0, 0),
                new Vector4(0, 0, 1, 0),
                new Vector4(0, 0, 0, 1));
 
            Matrix4x4 matrixRotX = new Matrix4x4(
                new Vector4(1, 0, 0, 0),
                new Vector4(0, Mathf.Cos(-radianX), Mathf.Sin(-radianX), 0),
                new Vector4(0, -Mathf.Sin(-radianX), Mathf.Cos(-radianX), 0),
                new Vector4(0, 0, 0, 1));
 
            Matrix4x4 matrixRotY = new Matrix4x4(
                new Vector4(Mathf.Cos(-radianY), 0, -Mathf.Sin(-radianY), 0),
                new Vector4(0, 1, 0, 0),
                new Vector4(Mathf.Sin(-radianY), 0, Mathf.Cos(-radianY), 0),
                new Vector4(0, 0, 0, 1));
 
            Matrix4x4 matrixScale = (new Matrix4x4(
                new Vector4(1/ referenceScale.x, 0, 0, 0),
                new Vector4(0, 1/ referenceScale.y, 0, 0),
                new Vector4(0, 0, 1/ referenceScale.z, 0),
                new Vector4(0, 0, 0, 1)
            )) ;
            
            // 都是逆矩阵
            // 相乘的循序也要发着
            Matrix4x4 wm = matrixScale *  matrixRotZ * matrixRotX * matrixRotY * matrixTrans;
            return wm;
        }
        
        /// <summary>
        /// 本地坐标转换成世界坐标
        /// 这个方式不需要通过角度运算，因为unity的各个朝向已经进行的角度旋转运算
        /// </summary>
        /// <param name="reference">参考对象，也就是要计算本地坐标的父亲节点</param>
        /// <returns>本地转世界坐标矩阵</returns>
        public static Matrix4x4 MakeLocalPosToWorldPosMatrixWithDir(Transform reference)
        {
            Vector3 referenceWorldPos = reference.position;
            Vector3 referenceRight = reference.right;
            Vector3 referenceUp = reference.up;
            Vector3 referenceForward = reference.forward;
            Vector3 referenceScale = reference.localScale;
            
            Matrix4x4 matrixTrans = new Matrix4x4(
                new Vector4(1, 0,0, 0),
                new Vector4(0, 1, 0, 0),
                new Vector4(0, 0, 1, 0),
                new Vector4(referenceWorldPos.x, referenceWorldPos.y, referenceWorldPos.z, 1)
            );
            
            Matrix4x4 matrixRot = new Matrix4x4(
                new Vector4(referenceRight.x, referenceRight.y, referenceRight.z, 0),
                new Vector4(referenceUp.x, referenceUp.y, referenceUp.z, 0),
                new Vector4(referenceForward.x, referenceForward.y, referenceForward.z, 0),
                new Vector4(0, 0, 0, 1)
            );
            
            Matrix4x4 matrixScale = (new Matrix4x4(
                new Vector4(referenceScale.x, 0, 0, 0),
                new Vector4(0, referenceScale.y, 0, 0),
                new Vector4(0, 0, referenceScale.z, 0),
                new Vector4(0, 0, 0, 1)
            )) ;

            Matrix4x4 mw = matrixTrans * matrixRot * matrixScale;
            return mw;
        }
        
        /// <summary>
        /// 构建世界坐标转本地坐标矩阵
        /// </summary>
        /// <param name="reference">参考对象，也就是要计算世界坐标转换的节点</param>
        /// <returns>世界转本地坐标的矩阵</returns>
        public static Matrix4x4 MakeWorldPosToLocalPosMatrixWithDir(Transform reference)
        {
            Vector3 referenceWorldPos = reference.position;
            Vector3 referenceRight = reference.right;
            Vector3 referenceUp = reference.up;
            Vector3 referenceForward = reference.forward;
            Vector3 referenceScale = reference.localScale;
            
            Matrix4x4 matrixTrans = new Matrix4x4(
                new Vector4(1, 0, 0, 0),
                new Vector4(0, 1, 0, 0),
                new Vector4(0, 0, 1, 0),
                new Vector4(-referenceWorldPos.x, -referenceWorldPos.y, -referenceWorldPos.z, 1)
            );
            
            // 旋转矩阵是正交矩阵
            // 转置矩阵就是逆矩阵
            Matrix4x4 matrixRot = new Matrix4x4(
                new Vector4(referenceRight.x, referenceUp.x, referenceForward.x, 0),
                new Vector4(referenceRight.y, referenceUp.y, referenceForward.y, 0),
                new Vector4(referenceRight.z, referenceUp.z, referenceForward.z, 0),
                new Vector4(0, 0, 0, 1)
            );
            
            Matrix4x4 matrixScale = (new Matrix4x4(
                new Vector4(1 / referenceScale.x, 0, 0, 0),
                new Vector4(0, 1 / referenceScale.y, 0, 0),
                new Vector4(0, 0, 1 / referenceScale.z, 0),
                new Vector4(0, 0, 0, 1)
            )) ;

            Matrix4x4 wm = matrixScale * matrixRot * matrixTrans;
            return wm;
        }
        
        /// <summary>
        /// 计算本地坐标转换成世界坐标
        /// </summary>
        /// <param name="reference">参考对象，也就是要计算本地坐标的父亲节点</param>
        /// <param name="localPos">要计算的本地坐标</param>
        /// <returns>转换后的世界坐标</returns>
        public static Vector3 LocalPosToWorldPos(Transform reference, Vector3 localPos)
        {
            Matrix4x4 mw = MakeLocalPosToWorldPosMatrix(reference);
            Vector4 localPos4 = localPos;
            localPos4.w = 1;
            
            Vector3 worldPos = mw * localPos4;
            return worldPos;
        }

        /// <summary>
        /// 世界坐标转换成本地坐标
        /// </summary>
        /// <param name="reference">参考的对象，也就是要将世界坐标换成本地坐标的对象</param>
        /// <param name="worldPos">世界坐标</param>
        /// <returns>转换后的本地坐标</returns>
        public static Vector3 WorldToLocalPos(Transform reference, Vector3 worldPos)
        {
            Matrix4x4 wm = MakeWorldToLocalPosMatrix(reference);
            Vector4 localPos4 = worldPos;
            localPos4.w = 1;
            
            Vector3 localPos = wm * localPos4;
            return localPos;
        }

        /// <summary>
        /// 本地坐标转换成世界坐标
        /// 这个方式不需要通过角度运算，因为unity的各个朝向已经进行的角度旋转运算
        /// </summary>
        /// <param name="reference">参考对象，也就是要计算本地坐标的父亲节点</param>
        /// <param name="localPos">要计算的本地坐标</param>
        /// <returns>转换后的世界坐标</returns>
        public static Vector3 LocalPosToWorldPosWithDir(Transform reference, Vector3 localPos)
        {
            Matrix4x4 mw = MakeLocalPosToWorldPosMatrixWithDir(reference);
            Vector4 localPos4 = localPos;
            localPos4.w = 1;
            
            Vector3 worldPos = mw * localPos4;
            return worldPos;
        }
        
        /// <summary>
        /// 世界坐标转换成本地坐标
        /// 这个方式不需要通过角度运算，因为unity的各个朝向已经进行的角度旋转运算
        /// </summary>
        /// <param name="reference">参考对象，也就是要计算世界坐标的节点</param>
        /// <param name="worldPos">要计算的世界坐标</param>
        /// <returns>转换后的本地坐标</returns>
        public static Vector3 WorldPosToLocalPosWithDir(Transform reference, Vector3 worldPos)
        {
            Matrix4x4 wm = MakeWorldPosToLocalPosMatrixWithDir(reference);
            Vector4 worldPos4 = worldPos;
            worldPos4.w = 1;
            
            Vector3 localPos = wm * worldPos4;
            return localPos;
        }

        /// <summary>
        /// 矩阵转换成字符串
        /// </summary>
        /// <param name="mx">要处理矩阵</param>
        /// <returns>字符串格式</returns>
        private static string MatrixToString(Matrix4x4 mx)
        {
            string s = "";
            for (int i = 0; i < 4; i++)
            {
                Vector4 row = mx.GetRow(i);
                s += string.Format("({0:f2}, {1:f2}, {2:f2}, {3:f2})\n", row.x, row.y, row.z, row.w);
            }

            return s;
        }
    }

}

MatrixTest

using System;
using System.Collections.Generic;
using UnityEngine;

namespace XMath
{
    /// <summary>
    /// 矩阵测试类型
    /// </summary>
    public enum MatrixTestType
    {
        LOCAL_TO_WORLD,         // 本地转世界
        LOCAL_TO_WORLD_DIR,     // 本地转世界-通过方向向量构建
        WORLD_TO_LOCAL,         // 世界转本地
        WORLD_TO_LOCAL_DIR,     // 世界转本地-通过方向向量构建
    }
    
    public class MatrixTest : MonoBehaviour
    {
        public MatrixTestType testType;         // 测试类型
        
        public Transform tfLocalToWorld;        // 本地转世界节点-要获取本地坐标的节点
        public Transform tfLocalToWorldSet;     // 本地转世界节点-要设置世界坐标的节点

        public Transform tfWorldToLocal;        // 世界转本地节点-要获取世界坐标的节点
        public Transform tfWorldToLocalSet;     // 世界转本地节点-要设置本地坐标的节点
        
        void Update()
        {
            switch (testType)
            {
                case MatrixTestType.LOCAL_TO_WORLD:
                    TestLocalToWorld();
                    break;
                case MatrixTestType.LOCAL_TO_WORLD_DIR:
                    TestLocalToWorldWithDir();
                    break;
                case MatrixTestType.WORLD_TO_LOCAL:
                    TestWorldToLocal();
                    break;
                case MatrixTestType.WORLD_TO_LOCAL_DIR:
                    TestWorldToLocalWithDir();
                    break;
            }

            bool testLocal = testType == MatrixTestType.LOCAL_TO_WORLD || testType == MatrixTestType.LOCAL_TO_WORLD_DIR; 
            
            tfLocalToWorld.gameObject.SetActive(testLocal);
            tfLocalToWorldSet.gameObject.SetActive(testLocal);
            tfWorldToLocal.gameObject.SetActive(!testLocal);
            tfWorldToLocalSet.gameObject.SetActive(!testLocal);
        }
        
        /// <summary>
        /// 测试本地坐标转世界坐标
        /// </summary>
        private void TestLocalToWorld()
        {
            Vector3 localPos = tfLocalToWorld.localPosition;
            Vector3 worldPos = XMatrix.LocalPosToWorldPos(transform, localPos);

            tfLocalToWorldSet.position = worldPos;
        }
        
        /// <summary>
        /// 测试本地坐标转世界坐标
        /// </summary>
        private void TestLocalToWorldWithDir()
        {
            Vector3 localPos = tfLocalToWorld.localPosition;
            Vector3 worldPos = XMatrix.LocalPosToWorldPosWithDir(transform, localPos);

            tfLocalToWorldSet.position = worldPos;
        }

        /// <summary>
        /// 测试世界坐标转本地坐标
        /// </summary>
        private void TestWorldToLocal()
        {
            Vector3 worldPos = tfWorldToLocal.position;
            Vector3 localPos = XMatrix.WorldToLocalPos(transform, worldPos);

            tfWorldToLocalSet.localPosition = localPos;
        }
        
        /// <summary>
        /// 测试世界坐标转本地坐标
        /// </summary>
        private void TestWorldToLocalWithDir()
        {
            Vector3 worldPos = tfWorldToLocal.position;
            Vector3 localPos = XMatrix.WorldPosToLocalPosWithDir(transform, worldPos);

            tfWorldToLocalSet.localPosition = localPos;
        }
        
        /// <summary>
        /// 绘制直线
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="dir">方向</param>
        /// <param name="color">颜色</param>
        private void DrawLine(Vector3 start, Vector3 dir, Color color)
        {
            Color old = Gizmos.color;
            Gizmos.color = color;
            
            Gizmos.DrawLine(start,  start + dir * 5);
            
            Gizmos.color = old;
        }

        /// <summary>
        /// 绘制本地到世界矩阵
        /// </summary>
        /// <param name="matrix">要绘制的矩阵</param>
        private void DrawLocalToWorldMatrix(Matrix4x4 matrix)
        {
            Vector4 row0 = matrix.GetRow(0);
            Vector4 row1 = matrix.GetRow(1);
            Vector4 row2 = matrix.GetRow(2);

            Vector3 axleX = new Vector3(row0.x, row1.x, row2.x);
            Vector3 axleY = new Vector3(row0.y, row1.y, row2.y);
            Vector3 axleZ = new Vector3(row0.z, row1.z, row2.z);
                
            DrawLine(transform.position, axleX, Color.red);
            DrawLine(transform.position, axleY, Color.green);
            DrawLine(transform.position, axleZ, Color.blue);
        }
        
        /// <summary>
        /// 绘制世界到本地矩阵
        /// </summary>
        /// <param name="matrix">要绘制的矩阵</param>
        private void DrawWorldToLocalMatrix(Matrix4x4 matrix)
        {
            Vector4 row0 = matrix.GetRow(0);
            Vector4 row1 = matrix.GetRow(1);
            Vector4 row2 = matrix.GetRow(2);

            Vector3 axleX = new Vector3(row0.x, row0.y, row0.z);
            Vector3 axleY = new Vector3(row1.x, row1.y, row1.z);
            Vector3 axleZ = new Vector3(row2.x, row2.y, row2.z);
                
            DrawLine(transform.position, axleX, Color.red);
            DrawLine(transform.position, axleY, Color.green);
            DrawLine(transform.position, axleZ, Color.blue);
        }
        
        private void OnDrawGizmos()
        {
            switch (testType)
            {
                case MatrixTestType.LOCAL_TO_WORLD:
                    DrawLocalToWorldMatrix(XMatrix.MakeLocalPosToWorldPosMatrix(transform));
                    break;
                case MatrixTestType.LOCAL_TO_WORLD_DIR:
                    DrawLocalToWorldMatrix(XMatrix.MakeLocalPosToWorldPosMatrixWithDir(transform));
                    break;
                case MatrixTestType.WORLD_TO_LOCAL:
                    DrawWorldToLocalMatrix(XMatrix.MakeWorldToLocalPosMatrix(transform));
                    break;
                case MatrixTestType.WORLD_TO_LOCAL_DIR:
                    DrawWorldToLocalMatrix(XMatrix.MakeWorldPosToLocalPosMatrixWithDir(transform));
                    break;
            }
        }
    }
}

测试场景图

四元数

在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。

绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。

• 比较

  欧拉角：非常直观，我们可以很容易理解它的意思，也能想象出对应的空间位置，但是存在万向锁现象，导致后面有很多数学问题。
  旋转矩阵：旋转矩阵有9个元素，计算繁杂，尤其是求微分时，而且也不直观。
  四元数：没有奇点，能表征任何旋转关系，而且表示简单，只有四个元素，计算量小，但是不直观，能进行增量旋转，给定方位的表达方式有两种，互为负（欧拉角有无数种表达方式）。

XQuaternion

using UnityEngine;

namespace XMath
{
    /// <summary>
    /// 四元素公式类
    /// </summary>
    public struct XQuaternion
    {
        public float x;
        public float y;
        public float z;
        public float w;
        
        public XQuaternion(float x, float y, float z, float w)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }

        /// <summary>
        /// 欧拉角转换成四元数
        /// </summary>
        /// <param name="x">角度x</param>
        /// <param name="y">角度y</param>
        /// <param name="z">角度z</param>
        /// <returns>unity的四元数</returns>
        public static Quaternion AngleToQuaternion(float x, float y, float z)
        {
            float rad2 = Mathf.PI / 360;
            
            float cX = Mathf.Cos(x * rad2);
            float sX = Mathf.Sin(x * rad2);

            float cY = Mathf.Cos(y * rad2);
            float sY = Mathf.Sin(y * rad2);

            float cZ = Mathf.Cos(z * rad2);
            float sZ = Mathf.Sin(z * rad2);

            XQuaternion qX = new XQuaternion(sX, 0, 0, cX);
            XQuaternion qY = new XQuaternion(0, sY, 0, cY);
            XQuaternion qZ = new XQuaternion(0, 0, sZ, cZ);
            XQuaternion qXYZ = (qY * qX) * qZ;

            Quaternion qUnity = new Quaternion(qXYZ.x, qXYZ.y, qXYZ.z, qXYZ.w);
            return qUnity;        
        }

        /// <summary>
        /// 四元数转换成欧拉角
        /// </summary>
        /// <param name="q">要转换的四元数</param>
        /// <returns>欧拉角</returns>
        public static Vector3 QuaternionToAngle(Quaternion q)
        {
            Matrix4x4 m = QuaternionToMatrix(q);
            
            Vector3 v = new Vector3();
            if (m[1, 2] < 1)
            {
                if (m[1, 2] > -1)
                {
                    v.x = Mathf.Asin(-m[1, 2]);
                    v.y = Mathf.Atan2(m[0, 2], m[2, 2]);
                    v.z = Mathf.Atan2(m[1, 0], m[1, 1]);
                }
                else
                {
                    v.x = Mathf.PI * 0.5f;
                    v.y = Mathf.Atan2(m[0, 1], m[0, 0]);
                    v.z = 0;
                }
            }
            else
            {
                v.x = -Mathf.PI * 0.5f;
                v.y = Mathf.Atan2(-m[0, 1], m[0, 0]);
                v.z = 0;
            }

            for (int i = 0; i < 3; i++)
            {
                if (v[i] < 0)
                {
                    v[i] += 2 * Mathf.PI;
                }
                else if (v[i] > 2 * Mathf.PI)
                {
                    v[i] -= 2 * Mathf.PI;
                }
            }

            v *= 180 / Mathf.PI;

            return v;
        }

        /// <summary>
        /// 四元数转换成矩阵
        /// </summary>
        /// <param name="q">要转换的四元数</param>
        /// <returns>旋转矩阵</returns>
        public static Matrix4x4 QuaternionToMatrix(Quaternion q)
        {
            Matrix4x4 m = new Matrix4x4();

            float x = q.x * 2;
            float y = q.y * 2;
            float z = q.z * 2;
            float xx = q.x * x;
            float yy = q.y * y;
            float zz = q.z * z;
            float xy = q.x * y;
            float xz = q.x * z;
            float yz = q.y * z;
            float wx = q.w * x;
            float wy = q.w * y;
            float wz = q.w * z;

            m[0] = 1.0f - (yy + zz);
            m[1] = xy + wz;
            m[2] = xz - wy;
            m[3] = 0.0F;

            m[4] = xy - wz;
            m[5] = 1.0f - (xx + zz);
            m[6] = yz + wx;
            m[7] = 0.0F;

            m[8] = xz + wy;
            m[9] = yz - wx;
            m[10] = 1.0f - (xx + yy);
            m[11] = 0.0F;

            m[12] = 0.0F;
            m[13] = 0.0F;
            m[14] = 0.0F;
            m[15] = 1.0F;

            return m;
        }
        
        /// <summary>
        /// 矩阵转换成四元数
        /// </summary>
        /// <param name="m"></param>
        /// <returns></returns>
        public static Quaternion MatrixToQuaternion(Matrix4x4 m)
        {
            XQuaternion quat = new XQuaternion();

            float fTrace = m[0, 0] + m[1, 1] + m[2, 2];
            float root;

            if (fTrace > 0)
            {
                root = Mathf.Sqrt(fTrace + 1);
                quat.w = 0.5f * root;
                root = 0.5f / root;
                quat.x = (m[2, 1] - m[1, 2]) * root;
                quat.y = (m[0, 2] - m[2, 0]) * root;
                quat.z = (m[1, 0] - m[0, 1]) * root;
            }
            else
            {
                int[] s_iNext = new int[] { 1, 2, 0 };
                int i = 0;
                if (m[1, 1] > m[0, 0])
                {
                    i = 1;
                }
                if (m[2, 2] > m[i, i])
                {
                    i = 2;
                }
                int j = s_iNext[i];
                int k = s_iNext[j];

                root = Mathf.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1);
                
                quat[i] = 0.5f * root;
                root = 0.5f / root;
                quat.w = (m[k, j] - m[j, k]) * root;
                quat[j] = (m[j, i] + m[i, j]) * root;
                quat[k] = (m[k, i] + m[i, k]) * root;
            }
            
            float nor = Mathf.Sqrt(Dot(quat, quat));
            Quaternion q = new Quaternion(quat.x / nor, quat.y / nor, quat.z / nor, quat.w / nor);

            return q;
        }

        /// <summary>
        /// 点乘
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static float Dot(XQuaternion a, XQuaternion b)
        {
            return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
        }
        
        public static XQuaternion operator *(XQuaternion lhs, XQuaternion rhs)
        {
            return new XQuaternion(lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y,
                lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z,
                lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x,
                lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z);
        }
        
        private float this[int index]
        {
            set
            {
                switch (index)
                {
                    case 0:
                        x = value;
                        break;
                    case 1:
                        y = value;
                        break;
                    case 2:
                        z = value;
                        break;
                    case 3:
                        w = value;
                        break;
                }
            }
        }
    }
}

QuaternionTest

using System;
using UnityEngine;
using Random = UnityEngine.Random;

namespace XMath
{
    public class QuaternionTest : MonoBehaviour
    {
        public Transform tfAngle;           // 要进行角度校验的节点-自己计算公式
        public Transform tfAngleUnity;      // 要进行角度检验的节点-unity内置

        public Transform tfQuaternion;      // 要进行四元数校验的节点-自己计算公式
        public Transform tfQuaternionUnity; // 要进行四元数校验的节点-unity内容

        public Vector3 angle;               // 角度检验的角度
        public Vector3 quaternion;          // 四元素检验的角度

        void Update()
        {
            angle.x += Random.Range(0, 5);
            angle.y += Random.Range(0, 5);
            angle.z += Random.Range(0, 5);
            
            quaternion.x += Random.Range(0, 5);
            quaternion.y += Random.Range(0, 5);
            quaternion.z += Random.Range(0, 5);
            
            Quaternion q = XQuaternion.AngleToQuaternion(angle.x, angle.y, angle.z);
            Quaternion qUnity = Quaternion.Euler(angle);

            tfQuaternion.rotation = q;
            tfQuaternionUnity.rotation = qUnity;

            
            Vector3 a = XQuaternion.QuaternionToAngle(Quaternion.Euler(quaternion));
            Vector3 aUnity = quaternion;
            
            tfAngle.rotation = Quaternion.Euler(a);
            tfAngleUnity.rotation = Quaternion.Euler(aUnity);
        }
    }
}

测试场景图

欧拉角

万象锁

当前物体旋转到与y轴垂直的时候（+-90度），这时候我们会发现，旋转另外两个角度，他们的作用是一样的，会丢失了一个旋转方向的作用，该现象称万向锁。

万向锁演示