(四元素)Quaterninos

 

(四元素)Quaterninos 

2013-07-17 08:42 4046人閱讀 評論(0) 收藏 舉報

      歐拉描述法。它使用最簡單的x,y,z值來分別表示在x，y，z軸上的旋轉角度，其取值爲0-360(或者0-2pi），一般使用roll，pitch，yaw來表示這些分量的旋轉值。需要注意的是，這裏的旋轉是針對世界座標系說的，這意味着第一次的旋轉不會影響第二、三次的轉軸，簡單的說，三角度系統無法表現任意軸的旋轉，只要一開始旋轉，物體本身就失去了任意軸的自主性，這也就導致了萬向軸鎖（Gimbal Lock）的問題。歐拉描述中針對x,y,z的旋轉描述是世界座標系下的值，所以當任意一軸旋轉90°的時候會導致該軸同其他軸重合，此時旋轉被重合的軸可能沒有任何效果，這就是Gimbal Lock。

 

     四元素。還有一種是軸角的描述方法（即我一直以爲的四元數的表示法），這種方法比歐拉描述要好，它避免了Gimbal Lock，它使用一個3維向量表示轉軸和一個角度分量表示繞此轉軸的旋轉角度，即(x,y,z,angle)，一般表示爲(x,y,z,w)或者(v,w)。但這種描述法卻不適合插值。

 

w = cos(theta/2)
x  = ax * sin(theta/2)
y  = ay * sin(theta/2)
z  = az * sin(theta/2)

 

      其中(ax,ay,az)表示軸的矢量，theta表示繞此軸的旋轉角度，爲什麼是這樣？和軸、角描述到底有什麼不同？這是因爲軸角描述的“四元組”並不是一個空間下的東西，首先（ax,ay,az）是一個3維座標下的矢量，而theta則是級座標下的角度，簡單的將他們組合到一起並不能保證他們插值結果的穩定性，因爲他們無法歸一化，所以不能保證最終插值後得到的矢量長度（經過旋轉變換後兩點之間的距離）相等，而四元數在是在一個統一的4維空間中，方便歸一化來插值，又能方便的得到軸、角這樣用於3D圖像的信息數據，所以用四元數再合適不過了。

      關於四元數的運算法則和推導這裏有篇詳細的文章介紹，重要的是一點，類似與Matrix的四元數的乘法是不可交換的，四元數的乘法的意義也類似於Matrix的乘法－可以將兩個旋轉合併，例如：

Q=Q1*Q2
 
表示Q的是先做Q2的旋轉，再做Q1的旋轉的結果，而多個四元數的旋轉也是可以合併的，根據四元數乘法的定義，可以算出兩個四元數做一次乘法需要16次乘法和加法，而3x3的矩陣則需要27運算，所以當有多次旋轉操作時，使用四元數可以獲得更高的計算效率。

 

 

關於插值

 

使用四元數的原因就是在於它非常適合插值，這是因爲他是一個可以規格化的4維向量，最簡單的插值算法就是線性插值，公式如：

q(t)=(1-t)q1+t q2

但這個結果是需要規格化的，否則q(t)的單位長度會發生變化，所以

q(t)=(1-t)q1+t q2 / || (1-t)q1+t q2 ||

如圖:

 

儘管線性插值很有效，但不能以恆定的速率描述q1到q2之間的曲線，這也是其弊端，我們需要找到一種插值方法使得q1->q(t)之間的夾角θ是線性的，即θ(t)=(1-t)θ1+t*θ2，這樣我們得到了球形線性插值函數q(t)，如下：

q(t)=q1 * sinθ(1-t)/sinθ + q2 * sinθt/sineθ

如果使用D3D，可以直接使用D3DXQuaternionSlerp函數就可以完成這個插值過程。

 

 

矩陣，歐拉角，四元素之間的相互轉換

 

矩陣轉四元素：

FQuat::FQuat( const FMatrix& M )
{
    //const MeReal *const t = (MeReal *) tm;
    FLOAT   s;

    // Check diagonal (trace)
    const FLOAT tr = M.M[0][0] + M.M[1][1] + M.M[2][2];

    if (tr > 0.0f)
    {
        FLOAT InvS = appInvSqrt(tr + 1.f);
        this->W = 0.5f * (1.f / InvS);
        s = 0.5f * InvS;

        this->X = (M.M[1][2] - M.M[2][1]) * s;
        this->Y = (M.M[2][0] - M.M[0][2]) * s;
        this->Z = (M.M[0][1] - M.M[1][0]) * s;
    }
    else
    {
        // diagonal is negative
        INT i = 0;

        if (M.M[1][1] > M.M[0][0])
            i = 1;

        if (M.M[2][2] > M.M[i][i])
            i = 2;

        static const INT nxt[3] = { 1, 2, 0 };
        const INT j = nxt[i];
        const INT k = nxt[j];

        s = M.M[i][i] - M.M[j][j] - M.M[k][k] + 1.0f;

        FLOAT InvS = appInvSqrt(s);

        FLOAT qt[4];
        qt[i] = 0.5f * (1.f / InvS);

        s = 0.5f * InvS;

        qt[3] = (M.M[j][k] - M.M[k][j]) * s;
        qt[j] = (M.M[i][j] + M.M[j][i]) * s;
        qt[k] = (M.M[i][k] + M.M[k][i]) * s;

        this->X = qt[0];
        this->Y = qt[1];
        this->Z = qt[2];
        this->W = qt[3];
    }
}


//
// MSM: Fast float inverse square root using SSE.
// Accurate to within 1 LSB.
//
FORCEINLINE FLOAT appInvSqrt( FLOAT F )
{
    const FLOAT fThree = 3.0f;
    const FLOAT fOneHalf = 0.5f;
    FLOAT temp;

    __asm
    {
        movss   xmm1,[F]
        rsqrtss xmm0,xmm1           // 1/sqrt estimate (12 bits)

        // Newton-Raphson iteration (X1 = 0.5*X0*(3-(Y*X0)*X0))
        movss   xmm3,[fThree]
        movss   xmm2,xmm0
        mulss   xmm0,xmm1           // Y*X0
        mulss   xmm0,xmm2           // Y*X0*X0
        mulss   xmm2,[fOneHalf]     // 0.5*X0
        subss   xmm3,xmm0           // 3-Y*X0*X0
        mulss   xmm3,xmm2           // 0.5*X0*(3-Y*X0*X0)
        movss   [temp],xmm3
    }

    return temp;
}

矩陣轉歐拉角

FRotator FMatrix::Rotator() const
{
    const FVector       XAxis   = GetAxis( 0 );
    const FVector       YAxis   = GetAxis( 1 );
    const FVector       ZAxis   = GetAxis( 2 );

    FRotator    Rotator = FRotator(
                                    appRound(appAtan2( XAxis.Z, appSqrt(Square(XAxis.X)+Square(XAxis.Y)) ) * 32768.f / PI),
                                    appRound(appAtan2( XAxis.Y, XAxis.X ) * 32768.f / PI),
                                    0
                                );

    const FVector       SYAxis  = FRotationMatrix( Rotator ).GetAxis(1);
    Rotator.Roll        = appRound(appAtan2( ZAxis | SYAxis, YAxis | SYAxis ) * 32768.f / PI);
    return Rotator;
}

FRotator( INT InPitch, INT InYaw, INT InRoll )
    :   Pitch(InPitch), Yaw(InYaw), Roll(InRoll) {}

 

歐拉角轉矩陣

 

 

FRotationMatrix(const FRotator& Rot)
    {
        const FLOAT SR  = GMath.SinTab(Rot.Roll);
        const FLOAT SP  = GMath.SinTab(Rot.Pitch);
        const FLOAT SY  = GMath.SinTab(Rot.Yaw);
        const FLOAT CR  = GMath.CosTab(Rot.Roll);
        const FLOAT CP  = GMath.CosTab(Rot.Pitch);
        const FLOAT CY  = GMath.CosTab(Rot.Yaw);

        M[0][0] = CP * CY;
        M[0][1] = CP * SY;
        M[0][2] = SP;
        M[0][3] = 0.f;

        M[1][0] = SR * SP * CY - CR * SY;
        M[1][1] = SR * SP * SY + CR * CY;
        M[1][2] = - SR * CP;
        M[1][3] = 0.f;

        M[2][0] = -( CR * SP * CY + SR * SY );
        M[2][1] = CY * SR - CR * SP * SY;
        M[2][2] = CR * CP;
        M[2][3] = 0.f;

        M[3][0] = 0.f;
        M[3][1] = 0.f;
        M[3][2] = 0.f;
        M[3][3] = 1.f;
    }
class FGlobalMath
{
public:
    // Constants.
    enum {ANGLE_SHIFT   = 2};       // Bits to right-shift to get lookup value.
    enum {ANGLE_BITS    = 14};      // Number of valid bits in angles.
    enum {NUM_ANGLES    = 16384};   // Number of angles that are in lookup table.
    enum {ANGLE_MASK    =  (((1<<ANGLE_BITS)-1)<<(16-ANGLE_BITS))};

    // Basic math functions.
    FORCEINLINE FLOAT SinTab( int i ) const
    {
        return TrigFLOAT[((i>>ANGLE_SHIFT)&(NUM_ANGLES-1))];
    }
    FORCEINLINE FLOAT CosTab( int i ) const
    {
        return TrigFLOAT[(((i+16384)>>ANGLE_SHIFT)&(NUM_ANGLES-1))];
    }
    FLOAT SinFloat( FLOAT F ) const
    {
        return SinTab(appTrunc((F*65536.f)/(2.f*PI)));
    }
    FLOAT CosFloat( FLOAT F ) const
    {
        return CosTab(appTrunc((F*65536.f)/(2.f*PI)));
    }

    // Constructor.
    FGlobalMath();

private:
    // Tables.
    FLOAT  TrigFLOAT        [NUM_ANGLES];
};

 

四元素轉矩陣

 

FQuatRotationTranslationMatrix(const FQuat& Q, const FVector& Origin)
{
    const FLOAT x2 = Q.X + Q.X;  const FLOAT y2 = Q.Y + Q.Y;  const FLOAT z2 = Q.Z + Q.Z;
    const FLOAT xx = Q.X * x2;   const FLOAT xy = Q.X * y2;   const FLOAT xz = Q.X * z2;
    const FLOAT yy = Q.Y * y2;   const FLOAT yz = Q.Y * z2;   const FLOAT zz = Q.Z * z2;
    const FLOAT wx = Q.W * x2;   const FLOAT wy = Q.W * y2;   const FLOAT wz = Q.W * z2;

    M[0][0] = 1.0f - (yy + zz); M[1][0] = xy - wz;              M[2][0] = xz + wy;          M[3][0] = Origin.X;
    M[0][1] = xy + wz;          M[1][1] = 1.0f - (xx + zz);     M[2][1] = yz - wx;          M[3][1] = Origin.Y;
    M[0][2] = xz - wy;          M[1][2] = yz + wx;              M[2][2] = 1.0f - (xx + yy); M[3][2] = Origin.Z;
    M[0][3] = 0.0f;             M[1][3] = 0.0f;                 M[2][3] = 0.0f;             M[3][3] = 1.0f;
}

QuatRotationTranslationMatrix( *this, FVector(0.f) )

 四元素與歐拉角的轉換都是通過矩陣來做中間跳板！

 

從一個向量旋轉到另一個向量的四元素：

FQuat FQuatFindBetween(const FVector& vec1, const FVector& vec2)
{
    const FVector cross = vec1 ^ vec2;
    const FLOAT crossMag = cross.Size();

    if(crossMag < KINDA_SMALL_NUMBER)
    {
        const FLOAT Dot = vec1 | vec2;
        if(Dot > -KINDA_SMALL_NUMBER)
        {
            return FQuat::Identity; // no rotation
        }
        else
        {
            // rotation by 180 degrees around a vector orthogonal to vec1 & vec2
            FVector Vec = vec1.SizeSquared() > vec2.SizeSquared() ? vec1 : vec2;
            Vec.Normalize();

            FVector AxisA, AxisB;
            Vec.FindBestAxisVectors(AxisA, AxisB);

            return FQuat(AxisA.X, AxisA.Y, AxisA.Z, 0.f); // (axis*sin(pi/2), cos(pi/2)) = (axis, 0)
        }
    }

    FLOAT angle = appAsin(crossMag);

    const FLOAT dot = vec1 | vec2;
    if(dot < 0.0f)
    {
        angle = PI - angle;
    }

    const FLOAT sinHalfAng = appSin(0.5f * angle);
    const FLOAT cosHalfAng = appCos(0.5f * angle);
    const FVector axis = cross / crossMag;

    return FQuat(
        sinHalfAng * axis.X,
        sinHalfAng * axis.Y,
        sinHalfAng * axis.Z,
        cosHalfAng );
}

#define KINDA_SMALL_NUMBER  (1.e-4)

FQuat( FLOAT InX, FLOAT InY, FLOAT InZ, FLOAT InA )
    :   X(InX), Y(InY), Z(InZ), W(InA)
    {}