PhysX原理與實現:一般約束

1. 原理

從數學描述上看,約束是兩個物體的廣義坐標或廣義速度之間需要滿足的等式(或不等式)。遊戲物理中的種種Joint都是等式約束,而碰撞都是不等約束。從約束的變量來看,可以分為幾何(位置)約束和速度約束:當約束方程中隻含有廣義坐標時,是幾何約束,若含有廣義速度則為速度約束。可積分的速度約束和幾何約束是等價的,亦稱之為完整約束。

一方面,遊戲物理中的Joint約束,都是完整約束;另一方面,如前所述,因為Px仍然使用的是基於速度的動力學,Px使用雅可比矩陣來表示約束。雅可比矩陣 bm J 的第i行第j列表示的是第i個約束對第j個廣義坐標的微分,即 J_{ij}=frac{partial C_i}{partial q_j} 。則易得:

bm{dot{C}} =bm{J dot{q}} = bm{JV} tag{1}

這裡 bm C 是約束,約束為n個則有n行, bm V 是整個系統的廣義速度。約束的例子可以參考遊戲物理引擎(四) 約束。

最常見的約束形式是 C(bm{q}_0,bm{q}_1)=0 ,則對時間的一階微分得到的是 bm {JV}=0 。但有些時候我們希望以自己的方式來控制約束的求解,以及為瞭能描述速度約束,約束的形式記為

bm {JV} =bm{b} tag{2} 其中 bm b 為常量。但若要保證系統的能量不因為解約束而非物理地變化,需要在解算完”用來計算位置的速度“後,用 bm b =0 的情況再解算真正的剛體速度。

記時間步長為 h ,由剛體的運動定律(忽略角動量定理中的 bm Ltimes omega 項),有

bm{V'}={bm{V}}_{init}+h bm{M}^{-1}bm{F}c tag{3}

其中, bm{V'} 是約束解算後的剛體廣義速度, bm{V}_{init} 是已經計算完外力、外力矩的作用後的剛體速度, bm{F}_c 是約束力,質量矩陣為( bm E 為三階單位矩陣)

bm{M}=begin{bmatrix} m_0 bm{E} & bm{0} & bm{0} & bm{0} \ bm{0} & bm{I}_0 & bm{0} & bm{0} \ bm{0} & bm{0} & m_1bm{E} & bm{0} \ bm{0} & bm{0} & bm{0} & bm{I_1} end{bmatrix}tag{4}

在理論力學中我們知道,僅有位置約束時,約束力可以由拉格朗日乘子法求出,設有n個約束,各個約束的乘子為 lambda_i ,則約束i對物體0的約束力是 lambda_i[frac{partial C_i}{partial x_0}, frac{partial C_i}{partial y_0}, frac{partial C_i}{partial z_0}]^T = lambda_i nabla C_{i,0} 。當對廣義坐標取偏微分,則有bm{F}_c= bm{J}^Tbm{lambda} tag{5}

其中 bm{lambda} =[lambda_1 ,lambda_2, cdots , lambda_n]^T 。 在遊戲物理的一些介紹文章中(如Video Game Physics Tutorial – Part III: Constrained Rigid Body Simulation),上式是由約束力不做功這一假設導出的,依賴瞭(2)式中 bm b=0 ,也是合理的。

讓(2)式中 bm V = {bm{V'}} ,聯立(2)、(3)、(5)式,並把 h 吸收進 bm{lambda} ,最終得

bm{JM^{-1}J^Tlambda} = -bm{J}{bm{V}}_{init} + bm{b}tag{6}

bm A = bm{JM^{-1}J^T}bm B=-bm{J}{bm{V}}_{init} + bm{b} ,則為 bm{A}lambda=bm{B} 。在求解這一線性方程組得到 bm{lambda} 後,由(3)(5)兩式即可求出剛體的最終速度。註意這裡 bm A 是一個方陣。

那麼如何計算這個線性方程組呢? PhysX使用的是Projected Gauss-Seidel (PGS)方法。Gauss-Seidel(GS)方法是一種常見的迭代解線性方程組的方法,該方法要求 bm A 的對角元都非零,而不難驗證我們這裡的 bm A 的對角元都是大於0的。另外,GS方法在 bm A 嚴格按行/列對角占優,或者是對稱正定矩陣時,對任意初始 bm{lambda} 都能達到收斂。由於 bm M^{-1} 是一個對稱正定矩陣, bm A = bm{JM^{-1}J^T} 也是對稱正定矩陣。PGS方法和GS方法的區別僅僅在於,PGS在每次迭代後會將 lambda_i^{(k)} 截斷(clamp)到一個給定范圍內。

GS的迭代公式為(k為當前迭代次數): lambda^{(k)}_i = frac{B_i-sum_{j<i}A_{ij}lambda^{(k)}_j-sum_{j>i}A_{ij}lambda^{(k-1)}_j} {A_{ii}} tag{7}

利用 bm J_ibm J 的行向量,有 A_{ij}=bm{J}_i bm{M}^{-1}bm{J}_j^T ,與 bm B=-bm{J}{bm{V}}_{20} + bm{b} 代入上式有 lambda^{(k)}_i = frac{-bm{J}_i{bm{V}}_{init} + b_i-sum_{j<i}bm{J}_i bm{M}^{-1}bm{J}_j^Tlambda^{(k)}_j-sum_{j>i}bm{J}_i bm{M}^{-1}bm{J}_j^Tlambda^{(k-1)}_j} {bm{J}_i bm{M}^{-1}bm{J}_i^T} \ = frac{-bm{J}_i{bm{V}}_{init} + b_i-sum_{j<i}bm{J}_i Delta bm V_j^{(k)}-sum_{j>i}bm{J}_i Delta bm V_j^{(k-1)}} {bm{J}_i bm{M}^{-1}bm{J}_i^T}qquad\ =frac{1}{{bm{J}_i bm{M}^{-1}bm{J}_i^T}}left[b_i-bm J_i({bm{V}}_{init}+sum_{j<i}Delta bm V_j^{(k)}+sum_{j>i}Delta bm V_j^{(k-1)})right] tag{8}

其中 Delta bm V_j^{(k)} 就是第k次迭代得到的 lambda_j^{(k)} 對應的約束沖量造成的廣義速度增量。那麼上式的圓括號內的式子所代表的意思即為:不考慮當前約束i的作用,但包括其他所有已經算出的約束沖量,迭代到這一步時剛體的廣義速度。如此我們隻需要補上 Delta bm V_i^{(k-1)} 就能得到考慮瞭所有當前約束沖量的剛體速度,接上式有

lambda^{(k)}_i =frac{1}{{bm{J}_i bm{M}^{-1}bm{J}_i^T}}left[b_i-bm J_i({bm{V}}_{init}+sum_{j<i}Delta bm V_j^{(k)}+sum_{j>i}Delta bm V_j^{(k-1)}+Delta bm V_i^{(k-1)})+bm J_iDelta bm V_i^{(k-1)}right] \ = frac{1}{bm{J}_i bm{M}^{-1}bm{J}_i^T} left(b_i-bm J_ibm{V}_{{cur}} + bm{J}_i bm{M}^{-1}bm{J}_i^T lambda_i^{(k-1)} right) tag{9}

記有效質量 m^{eff}_{i}= frac{1}{bm{J}_i bm{M}^{-1}bm{J}_i^T} ,最終化簡得 lambda^{(k)}_i = m^{eff}_{i}left(b_i-bm J_ibm{V}_{{cur}}right)+lambda_i^{(k-1)} tag{10}

最終在計算實現時,PhysX使用的就是此式。

2. 實現

由於UE用的還是老版本的PhysX,這裡的實現也以UE4.26中附帶的PhysX 3.4.1版本為準。

PhysX將約束的計算分為setup和solver兩個環節,setup負責從Joint中構建出雅可比矩陣和每個迭代可以復用的物理量,solver負責每次迭代的解算過程。關於PhysX的解算框架會在以後的文章裡分析,這裡不再贅述。

2.1 Setup

在DyConstraintSetup.cpp中Line. 532的函數ConstraintHelper::setupSolverConstraint設置瞭一般的一維約束。

構建約束時的調用堆棧

setup中的第一部分是計算有效質量 m^{eff}_{i}= frac{1}{bm{J}_i bm{M}^{-1}bm{J}_i^T} ,在PhysX中, bm{J}_i 是由四個PxVec3表示的:[lin0, ang0, lin1, ang1]。因為質量矩陣的線性運動部分是對角的,易得為

m_0|lin0|^2 。在計算角向部分時,PhysX利用瞭 bm{M}^{-1} 的正定對稱性質,令 bm S =bm S ^T= sqrt{M^{-1}} ,則有 m^{eff}_{i}= frac{1}{bm{J}_i bm{M}^{-1}bm{J}_i^T} =frac{1}{bm{J}_i bm{SS^T}bm{J}_i^T}=frac{1}{bm{ Z Z}^T}=frac{1}{|bm Z|^2} tag{11} ,其中 bm{Z} = bm J_i bm S 是1×12的向量。

init(s, c.linear0, c.linear1, PxVec3(angSqrtInvInertia0[i].x, angSqrtInvInertia0[i].y, angSqrtInvInertia0[i].z),
PxVec3(angSqrtInvInertia1[i].x, angSqrtInvInertia1[i].y, angSqrtInvInertia1[i].z), c.minImpulse * driveScale, c.maxImpulse * driveScale);
s.ang0Writeback = c.angular0;
PxReal resp0 = s.lin0.magnitudeSquared() * prepDesc.data0->invMass * prepDesc.mInvMassScales.linear0 + s.ang0.magnitudeSquared() * prepDesc.mInvMassScales.angular0;
PxReal resp1 = s.lin1.magnitudeSquared() * prepDesc.data1->invMass * prepDesc.mInvMassScales.linear1 + s.ang1.magnitudeSquared() * prepDesc.mInvMassScales.angular1;
unitResponse = resp0 + resp1;
initVel = normalVel = prepDesc.data0->projectVelocity(c.linear0, c.angular0) - prepDesc.data1->projectVelocity(c.linear1, c.angular1);

赞(0)