unity四元數學習總結--FromToRotation

上節介紹了，算是LookRotation的內部實現，這一節來介紹下unity的FromToRotation函數。

FromToRotation（from，to）這個函數主要的作用是計算將物體從一個旋轉態轉移到另一個旋轉態的四元數，即將from旋轉至to需要的四元數，與上一節不同的時，此時旋轉對應歐拉角Z分量不一定爲0了。

 （1）FromToRotation 計算公式

主要計算思路是分別將兩個方向向量歸一化，兩個方向向量的中間向量，即midDir = fromDir + toDir，（需要先將兩個向量標準化）然後再將中間向量歸一化。獲取的中間向量的作用是FromToRotation的結果四元數相當於fromDir先繞中間向量旋轉180度，對應旋轉四元數爲Q1，再繞toDir旋轉180度，對應四元數爲Q2，總的旋轉四元數爲Q，假設原本的旋轉狀態爲A，旋轉之後的狀態爲B，存在

   Q2Q1AQ1‘Q2' = B                          （1）

   QAQ’               = B                          （2）                  

因此Q= Q2Q1，其中根據軸角對與四元數的關係（（w,x,y,z）= (Cos sita/2 , Sin sita/2 * x, Sin sita/2 * y, Sin sita/2 * z) ）

Q1 = （0，midDir.x，midDir.y，midDir.z）    = （0，-midDir.x，-midDir.y，-midDir.z）   

Q2 = （0，toDir.x，toDir.y，toDir.z）    =     （0，-toDir.x，-toDir.y，-toDir.z）

通過四元數的乘法可以得到最終的公式。

需要注意的是，四元數和負四元數代表的是同一種旋轉。這個在之前也有提到過。還有就是，兩個方向向量需要標準化才能使得中間向量等於二者之和，同時中間向量也需要標準化，即轉化爲單位向量。

有一種特殊情況就是在toDir和fromDir在同一條直線上時（兩個方向正好相反），需要進行一些特殊處理，因爲這個時候和向量就不唯一了，爲了與Unity保持一致，需要進行特殊處理。在處理時主要按照向量各分量是否爲0進行分類處理，根據各分量是否爲0，可以將情況分爲四個大的種類，八個小的種類，主要情況如下：

（1）有三個向量分量爲0

          這種情況下fromDir和toDir均爲零向量，可以成爲非法數據，此種情況下返回（1，0，0，0）（對應（w,x,y,z））

（2）有一個分量爲0

          這種情況下fromDir和toDir有三種情況，分別爲

         a、fromDir = （x,0,0） toDir = （-x,0,0）對應四元數Q = (0,0,1,0)

         b、fromDir = （0,y,0） toDir = （0,-y,0）對應四元數Q = (0,1,0,0)

         c、fromDir = （0,0,z） toDir = （0,0,-z）對應四元數Q = (0,1,0,0)

（2）有兩個分量爲0

          這種情況下fromDir和toDir有三種情況，分別爲

         a、fromDir = （x,y,0） toDir = （-x,-y,0）對應四元數Q = (0,0,1,0)

         b、fromDir = （x,0,z） toDir = （-x,0,-z）對應四元數Q = (0,x,0,z)

         c、fromDir = （0,y,z） toDir = （-x,0,-z）對應四元數Q = (0,x,y,0)

(3) 三個分量均不爲0

此時不能直接得出四元數，需要利用midDir計算四元數，此時midDir = （0,z,-y）,利用四元數的乘法即能求出最終的四元數

對應最終程序如下

/// <summary>
    /// 在兩個朝向之間構造一個四元數.  
    /// </summary>
    /// <param name="fromDir">單位向量</param>
    /// <param name="toDir">單位向量</param>
    /// <returns></returns>
    public static Quaternion FromToRotation(Vector3 fromDir, Vector3 toDir)
    {
        if (fromDir == Vector3.zero || toDir == Vector3.zero) return Quaternion.identity;
        Quaternion q;
        //最大幅值歸一化
        //fromDir
        float max = Mathf.Abs(fromDir.x);
        max = max > Mathf.Abs(fromDir.y) ? max : Mathf.Abs(fromDir.y);
        max = (max > Mathf.Abs(fromDir.z)) ? max : Mathf.Abs(fromDir.z);
        fromDir = fromDir / max;

        //toDir
        max = Mathf.Abs(toDir.x);
        max = (max > Mathf.Abs(toDir.y)) ? max : Mathf.Abs(toDir.y);
        max = (max > Mathf.Abs(toDir.z)) ? max : Mathf.Abs(toDir.z);
        toDir = toDir / max;

        //極小數歸零
        //最小閾值
        float miniThreshold = 0.001f;
        fromDir.x = Mathf.Abs(fromDir.x) <= miniThreshold ? 0 : fromDir.x;
        fromDir.y = Mathf.Abs(fromDir.y) <= miniThreshold ? 0 : fromDir.y;
        fromDir.z = Mathf.Abs(fromDir.z) <= miniThreshold ? 0 : fromDir.z;
        toDir.x = Mathf.Abs(toDir.x) <= miniThreshold ? 0 : toDir.x;
        toDir.y = Mathf.Abs(toDir.y) <= miniThreshold ?0: toDir.y;
        toDir.z = Mathf.Abs(toDir.z) <= miniThreshold ? 0 : toDir.z;

        Vector3 mid = (fromDir.normalized + toDir.normalized).normalized;
        if (mid == Vector3.zero)
        {
            //相反的兩個方向,和向量爲0，還原Unity，分爲八種情況
            //某一向量爲零返回四元數基值，已在開頭判定

            #region  分別查看向量各個分量爲零的值

            // 僅有一個向量不爲0,分爲僅有x y z 向量不爲0三種情況
            //X
            if (fromDir.x != 0 && fromDir.y == 0 && fromDir.z == 0)
                return new Quaternion(0, 1, 0, 0);
            //Y
            else if (fromDir.x == 0 && fromDir.y != 0 && fromDir.z == 0)
                return new Quaternion(1, 0, 0, 0);
            //Z 
            else if (fromDir.x == 0 && fromDir.y == 0 && fromDir.z != 0)
                return new Quaternion(1, 0, 0, 0);

            // 僅有一個向量爲0,分爲僅有x y z 向量爲0三種情況
            //X
            else if (fromDir.x == 0 && fromDir.y != 0 && fromDir.z != 0)
                return new Quaternion(1, 0, 0, 0);
            //Y
            else if (fromDir.x != 0 && fromDir.y == 0 && fromDir.z != 0)
            {
                float X = toDir.normalized.z;
                float Z = fromDir.normalized.x;

                //正負判定
                if (X + Z < 0 || (X + Z == 0 && X < 0))
                    return new Quaternion(-X, 0, -Z, 0);
                else
                    return new Quaternion(X, 0, Z, 0);
            }
            //Z 
            else if (fromDir.x != 0 && fromDir.y != 0 && fromDir.z == 0)
            {
                float X = toDir.normalized.y;
                float Y = fromDir.normalized.x;

                //正負判定
                if (X + Y < 0 || (X + Y == 0 && X < 0))
                    return new Quaternion(-X, -Y, 0, 0);
                else
                    return new Quaternion(X, Y, 0, 0);
            }
            else
            {
                //三個點都不爲0
                mid.y = fromDir.z;
                mid.z = toDir.y;
                mid = mid.normalized;
            }

            #endregion

        }
        q = new Quaternion(-toDir.normalized.x, -toDir.normalized.y, -toDir.normalized.z, 0) * new Quaternion(mid.normalized.x, mid.normalized.y, mid.normalized.z, 0);

        return q;
    }

需要注意的邊界條件已經給出了，其次需要注意的就是精度和計算溢出問題。

（2）四元數歐拉角之間的相互轉換

也是幾個公式的問題，用代碼給出好了

四元數轉歐拉角：

public static Vector3 QuanternionToEuler(Quaternion q)
    {
        Vector3 euler = Vector3.zero;
        float sp = (q.y * q.z - q.w * q.x) * (-1 - 1);
        //檢查萬向鎖，允許誤差
        if (Mathf.Abs(sp) > (1 - 0.001))
        {
            euler.x = sp * Mathf.PI/2 * Mathf.Rad2Deg;

            euler.y = Mathf.Atan2(-q.x * q.z + q.w * q.y, 0.5f - q.y * q.y - q.z * q.z) * Mathf.Rad2Deg;

            euler.z = 0;
        }
        else
        {
            euler.x = Mathf.Asin(sp) * Mathf.Rad2Deg;

            euler.y = Mathf.Atan2(q.x * q.z + q.w * q.y, 0.5f - q.y * q.y - q.x * q.x) * Mathf.Rad2Deg;

            euler.z = Mathf.Atan2(q.x * q.y + q.w * q.z, 0.5f - q.z * q.z - q.x * q.x) * Mathf.Rad2Deg;

        }
        if (euler.x < 0) euler.x += 360;
        if (euler.y < 0) euler.y += 360;
        if (euler.z < 0) euler.z += 360;
        return euler;
    }

 歐拉角轉四元數

 public static Quaternion EulerAnglesToQuaternion(Vector3 euler)
    {
        Quaternion q = new Quaternion();

        //x的一半，單位爲弧度
        float xDiv2 = euler.x / 2 * Mathf.Deg2Rad;

        //Y的一半，單位爲角度
        float yDiv2 = euler.y / 2 * Mathf.Deg2Rad;

        //Z的一半，單位爲角度
        float zDiv2 = euler.z / 2 * Mathf.Deg2Rad;

        float cosXDiv2 = Mathf.Cos(xDiv2);
        float sinXDiv2 = Mathf.Sin(xDiv2);

        float cosYDiv2 = Mathf.Cos(yDiv2);
        float sinYDiv2 = Mathf.Sin(yDiv2);

        if (zDiv2 == 0)
        {
            q.w = cosXDiv2 * cosYDiv2;
            q.x = sinXDiv2 * cosYDiv2;
            q.y = cosXDiv2 * sinYDiv2;
            q.z = -sinXDiv2 * sinYDiv2;
        }
        else
        {
            float cosZDiv2 = Mathf.Cos(zDiv2);
            float sinZDiv2 = Mathf.Sin(zDiv2);

            q.w = cosXDiv2 * cosYDiv2 * cosZDiv2 + sinXDiv2 * sinYDiv2 * sinZDiv2;
            q.x = sinXDiv2 * cosYDiv2 * cosZDiv2 + cosXDiv2 * sinYDiv2 * sinZDiv2;
            q.y = cosXDiv2 * sinYDiv2 * cosZDiv2 - sinXDiv2 * cosYDiv2 * sinZDiv2;
            q.z = cosXDiv2 * cosYDiv2 * sinZDiv2 - sinXDiv2 * sinYDiv2 * cosZDiv2;
        }
        return q;
    }