物理引擎中的剛體轉動2

• 旋轉矩陣R的微分

　　對於線性位移$x(t)$和線性速度$v(t)$，很容易得出$v(t)=\frac{d}{dt}x(t)$。那麼旋轉矩陣$R(t)$和角速度$\omega(t)$之間的關係是怎樣的呢？顯然$\dot{R}(t)$不等於$\omega(t)$，由於$R(t)$是一個矩陣，而$\omega(t)$是向量。爲了回答這個問題，回憶一下矩陣$R(t)$的實際意義。咱們知道$R(t)$的列向量就是其座標系在世界座標系中的方向向量，所以$\dot{R}(t)$的列向量描述了剛體旋轉時$x$，$y$，$z$軸的「速度」。下圖描述了一個剛體以角速度$\omega(t)$進行旋轉，考慮剛體上某一位置向量$r(t)$，其速度很容易推導出爲：$$\dot{r}(t)=\omega(t) \times r(t)$$

 

　　$t$時刻，剛體$x$軸的方向向量在世界座標系下的座標爲矩陣$R(t)$的第一列：$(r_{xx},r_{xy},r_{xz})^T$，則在$t$時刻$R(t)$第一列的微分就是該向量的變化率。根據上面的推論，咱們可獲得該微分爲：$\omega(t) \times (r_{xx},r_{xy},r_{xz})^T$　　

　　對$R(t)$的另外兩個列向量狀況也同樣。所以，咱們能夠寫出：

$$\dot{R(t)}=\begin{pmatrix}
\omega(t) \times\begin{pmatrix} r_{xx}\\ r_{xy}\\ r_{xz} \end{pmatrix}
& \omega(t) \times\begin{pmatrix} r_{yx}\\ r_{yy}\\ r_{yz} \end{pmatrix}
& \omega(t) \times\begin{pmatrix} r_{zx}\\ r_{zy}\\ r_{zz} \end{pmatrix}
\end{pmatrix}$$

　　根據角速度$\omega$定義斜對稱矩陣（skew-symmetric matrix）$\Omega$，斜對稱矩陣知足$\Omega^T=-\Omega$：

$$\Omega=\begin{pmatrix}
0 & -\omega_z & \omega_y\\
\omega_z & 0 & -\omega_x\\
-\omega_y & \omega_x & 0
\end{pmatrix}$$

　　能夠證實，對任意向量$P$均有：$$\omega \times P = \Omega P$$

 　　則旋轉矩陣$R(t)$的微分能夠用角速度的斜對稱矩陣表示爲：$$\dot{R}(t)=\Omega R(t)$$

 

• 慣性參考系中的Euler方程

　　一般Euler方程中各量均爲在與剛體固連的座標系中表達，但這並不意味着此方程只在與剛體固連的座標系中才成立。事實上很容易證實：對任一座標系均有$$I \dot{\omega}+\omega \times I \omega =N$$

　　式中各量均爲在上述給定座標系中的表達式。固然，將Euler方程表示在與剛體固連座標系中尤爲特殊的優勢，即慣性張量矩陣是常值矩陣，它能夠預先計算或辨識出來，這給Euler方程的應用帶來很大方便。若是歐拉方程在慣性座標系中描述，那麼慣性張量是個隨着剛體運動而變化的變量，須要實時計算該值。

 　　下面從動量矩定理開始推導慣性座標系下的歐拉方程。等式中物理量下標$I$表示該量在慣性參考系（Inertia frame）中描述，這裏剛體的慣性張量$I$再也不恆定，而是隨時間變化的量，所以在求導時要注意：

$$\boldsymbol\tau_I = \frac d{dt} (\mathrm I_I\, \boldsymbol \omega_I) =
\mathrm I_I \, \dot {\boldsymbol \omega}_I
+ \dot {\mathrm I}_I \,\boldsymbol \omega_I$$

 　　慣性參考系下的慣性張量$I_I$和剛體局部參考系下的慣性張量$I_b$（$I_b$是常量）之間的關係以下：$$I_I=RI_bR^T$$

　　其中，矩陣$R$是剛體局部座標系相對於慣性座標系（世界座標系）的變換，即$^I_bR$。考慮慣性張量$I_I$對時間的導數：$\dot{I}_I=\dot{R}I_bR^T+RI_b \dot{R}^T$

　　變換矩陣$R$的導數能夠表示爲：$\dot{R}=Sk(\omega_I)R$，$Sk(\omega_I)$是根據角速度$\omega_I$構造的斜對稱矩陣。則有：$\dot{I}_I=Sk(\omega_I)RI_bR^T+RI_bR^TSk^T(\omega_I)=Sk(\omega_I)I_I-I_ISk(\omega_I)$

　　因而$\dot{I}_I\omega_I=Sk(\omega_I)I_I\omega_I-I_ISk(\omega_I)\omega_I=\omega_I \times (I_I\omega_I)-I_I(\omega_I \times \omega_I)=\omega_I \times (I_I\omega_I)$

　　所以，慣性參考系中的歐拉公式爲：

$$\boldsymbol\tau_I =
\mathrm I_I \, \dot {\boldsymbol \omega}_I
+ \boldsymbol \omega_I \times (\mathrm I_I \,\boldsymbol \omega_I)$$

　　能夠看出，慣性參考系下歐拉公式與剛體局部座標系下歐拉公式形式一致，只是物理量的參照基準不一樣。

 

•  陀螺力矩

　　歐拉方程$I \dot{\omega}+\omega \times I\omega=\tau$中的$\omega \times I\omega$項在物理引擎中被稱爲陀螺力矩（gyroscopic torque），由於其有力矩的量綱。陀螺力矩的產生是因爲角速度與角動量方向不一致，$\omega \times I\omega$叉乘結果不爲零。

　　在某些遊戲物理引擎中爲了不復雜計算影響實時性和穩定性，一般會忽略陀螺力矩，即直接使用公式：$$I\dot{\omega}=\tau$$

　　那麼對於物理引擎中的剛體轉動1中描述的那個問題，添加$\omega \times I\omega$項後準確的計算以下面Mathematica代碼所示：

　　

　　畫出結果，能夠發現與bullet2.8三、ODE物理引擎中模擬的結果一致（仿真時間設爲5s）：

 　　

　　bullet物理引擎從2.83版本開始才默認開啓了陀螺力矩的計算。

// bullet 2.83
enum    btRigidBodyFlags
{
    BT_DISABLE_WORLD_GRAVITY = 1,
    ///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
    ///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
    ///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit
    BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2,
    BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4,
    BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8,
    BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY,
};



// bullet 2.82
enum    btRigidBodyFlags
{
    BT_DISABLE_WORLD_GRAVITY = 1,
    ///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability
    ///So generally it is best to not enable it. 
    ///If really needed, run at a high frequency like 1000 Hertz:    ///See Demos/GyroscopicDemo for an example use
    BT_ENABLE_GYROPSCOPIC_FORCE = 2
};

View Code

 　　bullet2.83中與陀螺力矩相關的計算以下面代碼所示：

// 四元數與向量乘法運算符重載
btQuaternion operator*(const btQuaternion& q, const btVector3& w)
{
    return btQuaternion( 
         q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
         q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
         q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
        -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
}


// 用四元數旋轉向量
btVector3 quatRotate(const btQuaternion& rotation, const btVector3& v) 
{
    btQuaternion q = rotation * v;
    q *= rotation.inverse();
    return btVector3(q.getX(),q.getY(),q.getZ());
}


// 對單位四元數來講其逆等於其共軛四元數
btQuaternion inverse() const
{
    return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
}


void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
{
    v0->setValue(0.        ,-z()        ,y());
    v1->setValue(z()    ,0.            ,-x());
    v2->setValue(-y()    ,x()    ,0.);
}
    

/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
///Solve33 is from Box2d, thanks to Erin Catto,
btVector3 solve33(const btVector3& b) const
{
    btVector3 col1 = getColumn(0);
    btVector3 col2 = getColumn(1);
    btVector3 col3 = getColumn(2);
    
    btScalar det = btDot(col1, btCross(col2, col3));
    if (btFabs(det)>SIMD_EPSILON) 
    {
        det = 1.0f / det;  // 若是方程組係數行列式不等於零，則方程組有惟一解
    }
    btVector3 x;
    x[0] = det * btDot(b, btCross(col2, col3));  // 參考《線性代數》克拉默法則
    x[1] = det * btDot(col1, btCross(b, col3));
    x[2] = det * btDot(col1, btCross(col2, b));
    return x;
}
    
    
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
{    
    btVector3 idl = getLocalInertia(); 
    btVector3 omega1 = getAngularVelocity();  // angular velocity expressed in inertia frame
    btQuaternion q = getWorldTransform().getRotation();  
    
    // Convert to body coordinates
    btVector3 omegab = quatRotate(q.inverse(), omega1);
    
    // Inertia tensor expressed in local frame
    btMatrix3x3 Ib;
    Ib.setValue(idl.x(),0,0,
                0,idl.y(),0,
                0,0,idl.z());
    
    btVector3 ibo = Ib*omegab;  // Iw

    // Residual vector
    btVector3 f = step * omegab.cross(ibo);  // f = dt· w×(Iw)
    
    btMatrix3x3 skew0;
    omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
    
    btVector3 om = Ib*omegab;
    btMatrix3x3 skew1;
    om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
    
    // Jacobian
    btMatrix3x3 J = Ib +  (skew0*Ib - skew1)*step;
    
//    btMatrix3x3 Jinv = J.inverse();
//    btVector3 omega_div = Jinv*f;
    btVector3 omega_div = J.solve33(f);
    
    // Single Newton-Raphson update
    omegab = omegab - omega_div;//Solve33(J, f);
    // Back to world coordinates
    btVector3 omega2 = quatRotate(q,omegab);
    btVector3 gf = omega2-omega1;
    return gf;
}


btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
{
    // use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
    // calculate using implicit euler step so it's stable.

    const btVector3 inertiaLocal = getLocalInertia();
    const btVector3 w0 = getAngularVelocity();

    btMatrix3x3 I;

    I = m_worldTransform.getBasis().scaled(inertiaLocal) *
        m_worldTransform.getBasis().transpose();  // I_world

    // use newtons method to find implicit solution for new angular velocity (w')
    // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 
    // df/dw' = I + 1xIw'*step + w'xI*step

    btVector3 w1 = w0;

    // one step of newton's method
    {
        const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
        const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);

        btVector3 dw;
        dw = dfw.solve33(fw);
        //const btMatrix3x3 dfw_inv = dfw.inverse();
        //dw = dfw_inv*fw;

        w1 -= dw;
    }

    btVector3 gf = (w1 - w0);
    return gf;
}

View Code

注意如下幾點：

　　1. 用四元數對向量進行旋轉變換（參考quaternion）。The components a vector can be transformed between any two different reference frame orientations using the following formula:$$V_B=Q\otimes V_A\otimes Q^*$$     where,

     V_A = vector (pure quaternion) in Frame A coordinates

     V_B = vector (pure quaternion) in Frame B coordinates

     Q = normalized quaternion for orientation of Frame B relative to Frame A.

     Q* = conjugate (or inverse) of Q

      = quaternion product

　　2. bullet2.83在btSequentialImpulseConstraintSolver約束求解器中求解約束，並更新速度（注意與以前版本的區別）。角速度更新主要應用了衝量矩(moment of impulse)/角衝量(angular impulse)定理。主要有如下3步：

// 根據衝量矩定理計算角動量的變化。這裏乘了逆慣性張量，得出的是dt時間段內角速度的變化量
// 計算外力矩引發的角速度變化。m_externalTorqueImpulse實際是角速度變化量
solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;

// 計算陀螺力矩引發的角速度變化並將其加到m_externalTorqueImpulse上
gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); // gyroForce實際是角速度變化量
solverBody.m_externalTorqueImpulse += gyroForce;

// 更新剛體角速度
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
        m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_externalTorqueImpulse);

View Code

　3. 陀螺力矩的計算原理。

　　根據歐拉方程很容易獲得$\Delta\omega=\omega_2-\omega_1=\Delta tI^{-1}(T-\omega_1 \times I\omega_1)$，其中$\omega_1$是前一步的角速度、$\omega_2$是下一步的角速度。可是直接使用該公式迭代計算結果很容易發散。例如將上面Mathematica代碼中的仿真時間延長（設爲50s），時間步長分別設爲1/30和1/60，運行結果以下左右兩幅圖所示：

 

　　下面是專業多體動力學軟件SIMPACK中的結果，對比能夠看出上面的計算明顯發散了（爲了增長計算穩定性能夠減少時間步長，但這麼作的代價很大）。 

　　關於仿真穩定性方面的問題能夠參考：前向後項差分和顯式隱式歐拉法、數值天氣預報、布料仿真中經常使用積分方法、用數值模擬行星運動，很可貴到穩定解？

　　Bullet2.83中使用下面的方法（參考Numerical Methods_Erin Catto）：

• stage 1 explicit（顯式計算外力矩引發的角速度變化）：$\Delta\omega_1=\Delta tI^{-1}T$
• stage 2 implicit（隱式計算陀螺力矩引發的角速度變化）：$\Delta\omega_2=-\Delta tI^{-1}(\omega_2\times I\omega_2)$

　　因而有：$\omega_2=\omega_1+\Delta \omega_1+\Delta\omega_2$

 　　對於stage1能夠根據上一步的角速度用公式直接計算出下一步角速度，這種方法是顯式的；而stage2中公式的右端包含有未知的下一步角速度，它其實是關於下一步角速度的一個函數方程，這種方法是隱式的。考慮到數值穩定性等因素，有時須要選用隱式方法。隱式方程一般用迭代法求解，而迭代過程的實質是逐步顯式化。

　　下面看看怎麼隱式計算陀螺力矩引發的角速度的變化。不考慮外力矩，則歐拉方程可寫爲：$$F(\omega_2)=I(\omega_2-\omega_1)+\Delta t \omega_2 \times I\omega_2=0$$

　　須要注意的一點是上面方程中的慣性張量與角速度都應以同一個座標系爲參考。若是角速度相對世界座標系描述，那麼慣性張量$I$也應是在世界座標系下描述；若是角速度相對剛體局部座標系描述，那麼慣性張量$I$也應是在剛體局部座標系下描述。對應兩種不一樣的座標系bullet2.83中有兩種不一樣的函數，分別爲computeGyroscopicImpulseImplicit_World 和 computeGyroscopicImpulseImplicit_Body。在世界座標系下計算須要將局部慣性張量轉換到世界座標系下，在局部座標系下計算須要將世界座標系中的角速度轉換到局部座標系下。bullet默認使用的是局部座標系下的計算函數computeGyroscopicImpulseImplicit_Body。

　　求解該方程，即$F(\omega_2)=0$，便可解得$\omega_2$。這是一個非線性方程/組，可以使用牛頓法（Newton-Raphson）進行求解。對多元非線性方程組$\mathbf{F(x)=0}$，$\mathbf{F(x)}$的雅可比矩陣$\mathbf{J=F'(x)}$。則非線性方程組的牛頓迭代法爲：$$\mathbf{x^{(k+1)}=x^{(k)}-J^{-1}(x^{(k)})F(x^{(k)})}$$

 　　牛頓迭代法在單根附近具備2階收斂，在步長較小的狀況下，迭代初始值與方程的根接近，能保證迭代快速收斂。這裏每次迭代初始點都是上一步的角速度$\omega_1$，所以牛頓迭代公式中的$F(x^{(k)})=\Delta t \omega_1 \times I\omega_1$

　　$F(\omega_2)$的雅可比矩陣爲：$J=\frac{d(I(\omega_2-\omega_1)+\Delta t \omega_2 \times I\omega_2)}{d\omega_2}=I+\Delta t \frac{d(\omega_2 \times I\omega_2)}{d\omega_2}$

　　其中：

$$\begin{align*}
d(\omega \times I\omega)&=\omega \times d(I\omega)+d\omega \times I\omega \\& = \omega \times Id\omega-I\omega\times d\omega \\&
=Skew(\omega)Id\omega-Skew(I\omega)d\omega
\end{align*}$$

　　因而有：$$\frac{d(\omega \times I\omega)}{d\omega}=Skew(\omega)I-Skew(I\omega)$$

　　因而可獲得函數$F$的雅可比矩陣爲：$$J=I+\Delta t[Skew(\omega)I-Skew(I\omega)]$$

　　根據這些推論，將其帶入迭代公式能夠計算出下一步角速度，並輸出角速度變化量。

　　4. 用角速度更新剛體姿態　　 

　　根據動力學方程積分求得角速度後須要更新剛體姿態，在bullet中實際上用的是下面的方法（參考論文：Practical Parameterization of Rotations Using the Exponential Map）

static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
{
    predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
//#define QUATERNION_DERIVATIVE
#ifdef QUATERNION_DERIVATIVE
    btQuaternion predictedOrn = curTrans.getRotation();
    predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
    predictedOrn.normalize();
#else
    //Exponential map
    //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

    btVector3 axis;
    btScalar    fAngle = angvel.length(); 
    //limit the angular motion
    if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
    {
        fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
    }

    if ( fAngle < btScalar(0.001) )
    {
        // use Taylor's expansions of sync function
        axis   = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
    }
    else
    {
        // sync(fAngle) = sin(c*fAngle)/t
        axis   = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
    }
    btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
    btQuaternion orn0 = curTrans.getRotation();

    btQuaternion predictedOrn = dorn * orn0;
    predictedOrn.normalize();
#endif
    predictedTransform.setRotation(predictedOrn);
}

View Code

 　　旋轉$\mathbf{q}$可由三維向量$\pmb{\omega}\in\mathbb{R^3}$來表示。向量幅值表明繞其旋轉的角度，單位向量$\pmb{\hat{\omega}}$表示旋轉軸的方向。對向量$\pmb{\omega}$進行單位化：$\pmb{\hat{\omega}}=\pmb{\omega}/|\pmb{\omega}|$，當其幅值很小接近零時容易引發計算不穩定。四元數$\mathbf{q}$可寫爲：$$\mathbf{q}=[\sin(\theta/2)\frac{\pmb{\omega}}{|\pmb{\omega}|},\cos(\theta/2)]^T$$

　　爲了不計算$\frac{\pmb{\omega}}{|\pmb{\omega}|}$時不穩定，將$\sin(\theta/2)$進行泰勒展開，則有：$\sin(\theta/2)=\frac{\theta}{2}-\frac{\theta^3}{48}+\frac{\theta^5}{3840}-...$

　　當$\theta$足夠小時，使用上面計算公式的前兩項來近似，即：$$\sin\frac{\theta}{2}=\frac{\theta}{2}-\frac{\theta^3}{48}$$

　　反之當$\theta$大小超過必定閾值時能夠直接計算。

　　根據：$$\begin{align*} q(t+\Delta t)&=\Delta q* q(t)\\&=(\cos\frac{\Delta \theta}{2}+\pmb{\hat{\omega}}\sin\frac{\Delta \theta}{2})q(t) \end{align*}$$

　　將$\Delta\theta=\left |\omega\right |\Delta t$帶入上面公式，便可求得剛體的新姿態$ q(t+\Delta t)$

 

•  穩定性、精度及優化問題

 　　用牛頓第二定律（動量矩定理）解決物理問題，其實是求解一個二階常微分方程的問題。用數值方法進行剛體動力學模擬時，會涉及數值穩定性、求解精度等一系列問題。遊戲物理引擎中對速度和位移進行積分時，一般用的都是歐拉法（Euler Integrator）、半隱式歐拉法（Semi-implicit Euler / Simplectic Euler）或隱式歐拉法（Implicit Euler method / Backward Euler method），除此以外還有龍格庫塔法（Runge Kutta）、Verlet integration等方法，能夠參考Numerical methods for ordinary differential equations。不一樣方法的求解精度、收斂性和計算速度都不同，所以要根據需求選擇合適的方法。

Numerical methods for integration
First-order methods	Euler method Backward Euler Semi-implicit Euler Exponential Euler
Second-order methods	Verlet integration Velocity Verlet Trapezoidal rule Beeman's algorithm Midpoint method Heun's method Newmark-beta method Leapfrog integration
Higher-order methods	Exponential integrators Runge–Kutta methods List of Runge–Kutta methods Linear multistep method General linear methods Backward differentiation formula Yoshida
Theory	Symplectic integrator

 

 

參考：

物理引擎中的剛體轉動1

剛體質量分佈與牛頓-歐拉方程

數值計算方案 

布料仿真中經常使用積分方法

前向後項差分和顯式隱式歐拉法

Numerical Methods_Erin Catto

Physically Based Modeling Rigid Body Simulation

Accurate and Efficient Simulation of Rigid Body Rotations

Euler's equation for rigid body rotation applied to inertia frame

Rigid Body Simulation I—Unconstrained Rigid Body Dynamics

Integration Basics—How to integrate the equations of motion