-
旋轉矩陣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.83、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 };
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; }
注意以下幾點：
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,
= 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);
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); }
旋轉$\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。不同方法的求解精度、收斂性和計算速度都不一樣，因此要根據需求選擇合適的方法。
| First-order methods | |
|---|---|
| Second-order methods | |
| Higher-order methods | |
| Theory | |
參考：
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