【Unity学习笔记】第二十 · 物理引擎脉络梳理（数值积分、碰撞检测、约束解决）

转载请注明出处: https://blog.csdn.net/weixin_44013533/article/details/139808452

作者：CSDN@|Ringleader|

物理引擎综述

物理引擎是利用物理规则模拟物体运动和碰撞的模块，以在重力、弹力、摩擦力等各种力作用下做出真实运动表现，并对碰撞、关节等约束做出合理反馈，从而方便开发者实现诸如角色控制、布娃娃系统、布料模拟、场景破坏等更复杂的物理效果。

常见的物理引擎有Box2d、Physx、Chaos、Bullet、Havok 等。游戏引擎通常集成一个或多个物理引擎供开发者选择。

游戏引擎包含两个独立的内存空间：一个是场景世界，用于被渲染系统渲染到屏幕；一个是物理世界，用于被物理系统计算模拟。如下图所示：

在使用物理系统之前，需要对场景中的对象进行配置，常见配置项是：

• rigidbody，表示在物理世界下的物体属性，比如质量、受阻力大小、是否受重力、刚体类型（dynamic/kinematic/trigger等）等；
• collider，表示在物理世界下物体的形状，主要用于碰撞检测。
  

Unity中rigidbody组件和不同类型的碰撞体   

在进行物理模拟前，会将场景世界数据同步到物理世界；模拟结束后再将物理世界数据同步到场景世界，最后由渲染器渲染呈现。

scene和物理间的数据同步   

不同物理引擎物理模拟流程存在差异，但核心步骤类似，主要包含：

• 积分运算：根据对象所受外力，利用欧拉、RK4等积分器计算其速度和位置；
• 碰撞检测：检测对象之间是否存在碰撞。为了加速检测，将碰撞检测分为broad phase和narrow phase两个阶段，broad phase利用包围体排序或空间管理算法筛选可能存在碰撞的collider pair，然后在narrow phase利用标准collider结构或者使用GJK+EPA通用算法确定碰撞并生成碰撞信息（碰撞点、穿透深度、碰撞法线等数据）用于后面的约束求解；
• 约束解决：对于存在碰撞约束和关节约束的对象，使用基于力、基于冲量或者基于位置的方法求解约束，得到新的速度和位置并更新。

物理引擎流程图参考   

下面将详细介绍整个物理引擎。

数值积分

首先得明确，游戏引擎是以时间步长$h$或者$\Delta t$为单位的离散系统，而我们现在的目标是：在一个物理循环开始时，我们在知道当前位置、速度、外力的情况下，如何计算本次循环结束时，对象的位置和速度？

根据牛顿第二定理，可以得到本次迭代新的速度和位置为：

$v_{t+\Delta t}=v_t+{\int }_t^{t+\Delta t}adt=v_t+{\int }_t^{t+\Delta t}\frac{F}{m}dt$

$x_{t+\Delta t}=x_t+{\int }_t^{t+\Delta t}vdt$

Δv与阴影面积有关，Δx也类似   

如上图和公式可以看到，因为系统的离散性，造成了确定对象新状态的复杂性，为了简化模拟，我们使用数值积分来取近似值。

我们将$v$和$x$统一用状态变量$s$来表示，其导函数$\dot{s}$用$f(t)$表示。

根据泰勒级数展开，

$s(t+h)=s(t)+\dot{s}(t)h+\frac{1}{2}\ddot{s}(t)h^2+\cdot \cdot \cdot$

取其前两项，便是欧拉积分 $s(t+h)=s(t)+\dot{s}(t)h$。

比较一开始的的解析积分形式，可以看到欧拉积分将状态导函数看成常数项，即认为在时间步长中求新速度看成外力保持不变、求新位置看成速度保持不变。

但导函数保持恒定又分为向前和向后一致，如下图所示：

数值积分近似的向前与向后近似   

由此产生显式欧拉和隐式欧拉。

• 显式欧拉
  $v(t+h)=v(t)+a(t)h$

  $x(t+h)=x(t)+v(t)h$

• 隐式欧拉
  $v(t+h)=v(t)+a(t+h)h$

  $x(t+h)=x(t)+v(t+h)h$

但显式欧拉不稳定，隐式欧拉需要迭代求解，所以常用半隐式欧拉法

• 半隐式欧拉
  $v(t+h)=v(t)+a(t)h$

  $x(t+h)=x(t)+v(t+h)h$

基于此就可以利用半隐式欧拉公式根据外力更新刚体速度和位置了。

此外关于数值积分稳定性与精度相关内容参考其他文献/书籍，比如《基于物理的建模与动画》第7章就详细介其他更高精度比如RK4等积分器。

碰撞检测

综述中我们了解到，碰撞检测分为broad phase和narrow phase两个阶段，broad phase利用SAP或空间管理算法筛选可能存在碰撞的collider pair，然后在narrow phase使用GJK+EPA通用算法确定碰撞并生成碰撞信息。

下面我们将依次介绍。

Broad Phase

包围体 Bounding Volume

包围体可视为一类简单的几何体，用于包围一个或多个具有复杂几何形状的对象。其中，较为常见的包围体是球体与盒体。包围体常用于前期相交剔除测试，以避免对原几何体进行成本高昂的直接测试。

• AABB(axis-aligned bounding boxes) 轴对齐包围盒
• OBB(oriented bounding boxes) 有向包围盒
• Circle/Sphere.
  

AABB包围盒可以用左下角和右上角顶点坐标唯一确定，如(Bx,By) (Ex,Ey)。（B为begin代表最小值，E为end代表最大值）

那么判断AABB包围盒相交，就两两比较顶点就行。但为了加速判断，使用了一种称作SAP的算法。

SAP（sweep-and-prune）

SAP本质上就是对包围盒顶点在各个轴进行投影，并对其坐标值进行排序，如下图所示。

比如在x轴向，排序结果是 （B2 （B1 E2) E1)，匹配（B2和E2) ，中间包含对象1，说明在x轴上投影2和1是相交的。然后剔除B2和E2（所谓prune），接着做相同匹配，直到全部坐标剔除。
同样思路投影在Y轴上。当在所有轴两者都相交，那么就将其保存在collider pair中，等待narrow phase精确检测。

这就是SAP核心思想。

当场景存在大量运动对象时，会频繁更新排序表，导致性能下降，这时可以使用改进的MBP（multi box pruning）算法，MBP算法主要在空间上进行分块以后在进行类似SAP算法计算，主要是优化动态元素的计算，分块以后动态元素的移动就只需要更新快内的数据，不需要全局更新，重而提高算法效率。

physx就是使用SAP和MBP作为它的broad phase算法。

其他引擎也使用诸如BVH层次包围体、网格划分、四叉树、BSP二叉树划分、八叉树OCtree等等不一而足。详细可参考《实时碰撞检测算法技术 （Real-Time Collision Detection)》或者其他博客，比如引擎架构剖析——物理引擎全解析（六）。

Narrow phase

到了碰撞检测的narrow phase，就需要确定可能碰撞的collider pair是否真正相交，以及获得碰撞点详细信息。

对于标准的碰撞体，比如球形、方形、胶囊型，我们可以针对性设计公式进行求解，对于复杂对象我们也可以分割成复合碰撞体进行判断，但面对精度要求更高的不规则Mesh collider就必须使用更通用的算法，那就是GJK+EPA，使用通用算法也省得两两判断各种形状的碰撞体了。

在介绍这个算法之前，需要介绍一些前置知识。

闵可夫斯基和

令A和B为两个点集，且令$\mathbf{a}$ 和$\mathbf{b}$为两个位置向量，对应A和B中的顶点。

• Minkowski和 $A\oplus B$定义为 ：
  A ⊕ B = { a + b : a ∈ A , b ∈ B } A\oplus B = \{\mathbf{a} +\mathbf{b} :\mathbf{a} ∈A,\mathbf{b} ∈B \} A⊕B={a+b:a∈A,b∈B}

• Minkowski差 $A\ominus B$定义为 ：
  A ⊖ B = { a − b : a ∈ A , b ∈ B } A\ominus B =\{ \mathbf{a} -\mathbf{b} :\mathbf{a} ∈A,\mathbf{b} ∈B \} A⊖B={a−b:a∈A,b∈B}

Minkowski和示意gif（闵氏差的话就先将B绕原点旋转180°再做类似和）   

一个重要的原理：两个多面体$A$和$B$的间距等价于其Minkowski差$C$（$C=A\ominus B$）与原点之间的距离。如果两凸包相交，意味着Minkowski差包含原点。

所以碰撞检测问题就变成两碰撞体顶点集的Minkowski差距离原点最近点问题。

支撑函数

支撑函数$S_A(d)$返回形状A边界上在向量$\mathbf{d}$上投影最高的点。从数学上讲，它与$\mathbf{d}$的点积最高。这被称为支撑点，此操作也成为支撑映射。

支撑函数将方向和形状作为输入，并返回支撑点作为输出。
定义支撑函数：

• $\text{Support}(\vec{d},A)=P\in A,(P\cdot \vec{d})\ge (Q\cdot \vec{d}),\forall Q\in A$

支撑函数有以下性质：

• $\text{Support}(\vec{d},-B)=-\text{Support}(-\vec{d},B)$

  两个形状的 Minkowski 和的支持函数可以表示为各个形状的支持函数之和：

• $\text{Support}(\vec{d},A\oplus B)=\text{Support}(\vec{d},A)+\text{Support}(\vec{d},B)$

因此，两种形状顶点集闵氏差的支持函数为：

• $\text{Support}(\vec{d},A\ominus B)=\text{Support}(\vec{d},A)-\text{Support}(-\vec{d},B)$

简记为：

• $s_{A\ominus B}(\vec{d})=s_A(\vec{d})-s_B(-\vec{d})$

计算两个形状的Minkowski差通常比较复杂，但利用支撑映射及其性质却能很简单地获得Minkowski差的支撑点。

Minkowski差的支撑点将在GJK算法中起到重要作用。

单纯形

n 维单纯形是 n 维中可以紧密平铺以填充空间的最小几何体；

例如，三角形是 2D 单纯形，四面体是 3D 单纯形。

那么在两个碰撞体顶点集的Minkowski差中，我们可以找到包含原点的点（0D 单纯形）、线（1D 单纯形）、三角形（2D 单纯形）或四面体（3D 单纯形）。

那么就将找闵氏差包含原点的问题，转换成闵氏差的单纯形子集包含原点问题。

GJK

GJK的主要思想就是利用碰撞体顶点集的Minkowski差的支撑点组成单纯形，判断这个单纯形是否包含原点。

我们将 “碰撞体顶点集的Minkowski差” 简记为CSO（配置空间对象）

以下是 GJK 的完整内容：

1. 将单纯形初始化为空集（技术上是 -1D 单纯形）。
2. 使用初始方向来找到 CSO 的支持点。
3. 将该支撑点添加到单纯形（现在单纯形有一个顶点）。
4. 找到单纯形中距离原点最近的点。
5. 如果最近的点是原点，则 CSO 包含原点，并且两个物体发生碰撞。结束 GJK。
6. 否则，通过丢弃顶点将单纯形降低到仍然包含最近点的尽可能最低的维度。
7. 使用从最近点到原点的方向寻找新的支撑点。
8. 如果新的支撑点在搜索方向上的位置不比最近点更远，则两个物体不会发生碰撞。结束 GJK。
9. 将支撑点作为新顶点添加到单纯形中。转到4。

用图形进行理解。

EPA 膨胀多边形算法

GJK 仅告诉我们两个形状是否发生碰撞。下一步则是利用EPA(Epanding Polytop Algorithm) 算法生成约束解决阶段所需的接触信息。

2D EPA 可视化流程：

约束求解

通过上个环节我们得到碰撞点、碰撞法线、穿透深度等碰撞信息，我们就可以着手解决碰撞了。

解决思路有两类，一个是基于误差的惩罚力方法，另一种就是基于约束的雅可比矩阵法。

惩罚力法根据误差大小（比如穿透深度）施加一定比例的惩罚力，使误差减小；为了预测误差变化，可以给惩罚力添加微分项，同时为了消除累计误差，还可以添加积分项。这有点像经典控制系统里的PID控制。详细可参考《基于物理的建模与动画 第11章》。

惩罚力法解决约束   

但由于惩罚力法是根据误差进行反馈，所以不适合关节等刚性约束。但其实现简单，广泛使用在Maya、3Dmax等软体系统中。《Physics-based animation Erleben 2005》

现在游戏引擎常用的还是基于约束的、使用雅可比矩阵+拉格朗日乘数法求解约束的方式。

基于约束又分为基于约束力的、基于冲量的和基于位置的。

下面将依次介绍，首先补充下基础知识点。

基础知识

约束

• 自由度（Degrees of Freedom）：刚体的自由度，定义为它具有的独立运动的数量。2D：3个自由度 （2平移自由度 | 1旋转自由度）3D：6个自由度 （3平移自由度 | 3 旋转自由度）

• 广义坐标：用来描述系统位形所需要的独立参数，或者最少参数。

  • 如下图单摆中重物m，可以用$(x,y)$坐标描述对象位置，但$x$和$y$显然不是独立的，满足$x^2+y^2=l^2$。使用广义坐标 $\theta$ 就能简单准确描述其位置，这个$\theta$就是广义坐标。
    
  • 一个重要特性：刚体的自由度=广义坐标数。

• 约束（Constraint）：在刚体中用来限制刚体运动自由度的被称作约束。限制位置的称作位置约束，限制速度的称作速度约束，速度约束也可以从位置约束求导得到。约束方程通常用符号 $C$表示。约束又分等式约束和不等式约束，比如上面单摆就是等式约束，地面碰撞就是不等式约束 。如果一个系统中约束力不做功，那么称之为理想约束。限制自由度的力就是约束力，如绳子连杆的拉力、地面支持力、接触的摩擦力等。

状态向量、质量、外力、约束的矩阵表示

• 状态向量

  假设系统有$n$个刚体，令 $\mathbf{q}$ 为具有所有刚体位置和角度的状态向量：

  $$\mathbf{q}=[{\mathbf{p}}_1,{\alpha }_1,{\mathbf{p}}_2,{\alpha }_2,...,{\mathbf{p}}_{\mathbf{n}},{\alpha }_n{]}^T=[q_1,q_2,...,q_n{]}^T$$

  其中${\mathbf{p}}_{\mathbf{i}}$是二维向量（3d物理就是三维向量），表示第$i$个刚体的位置，${\alpha }_i$是其角度，为标量。
  因此，2d物理$\mathbf{q}$有3 n 个元素（3d物理$\mathbf{q}$有4 n 个元素）。

• 质量矩阵
  定义质量矩阵 $M$ 为以下 $3n\times 3n$ 对角矩阵：
  M = [ m 1 m 1 I 1 m 2 m 2 I 2 ⋱ m n m n I n ] {\tiny M=

  [m1m1I1m2m2I2⋱mnmnIn]" role="presentation">⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢m1m1I1m2m2I2⋱mnmnIn⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥$$\left[\begin{array}{ccccccccccc}& m_1& & & & & & & & & \\ & & m_1& & & & & & & & \\ & & & I_1& & & & & & & \\ & & & & m_2& & & & & & \\ & & & & & m_2& & & & & \\ & & & & & & I_2& & & & \\ & & & & & & & \ddots & & & \\ & & & & & & & & m_n& & \\ & & & & & & & & & m_n& \\ & & & & & & & & & & I_n\end{array}\right]$$
  } M= ​​m1​​m1​​I1​​m2​​m2​​I2​​⋱​mn​​mn​​In​​ ​
  其中，$m_i$是第$i$个刚体的质量，$I_i$是其转动惯量。

• 力矩阵

  令$F$为一个全局力矢量，包含作用在每个物体上的力和力矩。它是外力和约束力的总和：

  $$F=F_{ext}+F_C=[{\mathbf{f}}_1,{\tau }_1,{\mathbf{f}}_2,{\tau }_2,...,{\mathbf{f}}_{\mathbf{n}},{\tau }_n{]}^T$$

  其中，$F$ 也有$3n$个元素，因为每个${\mathbf{f}}_{\mathbf{i}}$都是二维向量。

  那么牛顿第二运动定律可用如下表达式表达：

  $$\ddot{q}=M^{-1}F=M^{-1}(F_{ext}+F_C)$$

• 行为函数

  最后，让我们设置行为函数。假设有m 个约束，每个约束代表刚体链中的一个环节。我们将把所有行为函数分组为单个函数$C(\mathbf{q})$：
  C ( q ) = [ C 1 ( q ) , C 2 ( q ) , . . . , C n ( q ) ] T = [ C 1 ( q ) C 2 ( q ) ⋮ C n ( q ) ] C(\mathbf{q})=[C_1(\mathbf{q}),C_2(\mathbf{q}),...,C_n(\mathbf{q})]^T=

  [C1(q)C2(q)⋮Cn(q)]" role="presentation">⎡⎣⎢⎢⎢⎢⎢C1(q)C2(q)⋮Cn(q)⎤⎦⎥⎥⎥⎥⎥$$\left[\begin{array}{c}{C}_1(\mathbf{q})\\ C_2(\mathbf{q})\\ ⋮\\ C_n(\mathbf{q})\end{array}\right]$$
  C(q)=[C1​(q),C2​(q),...,Cn​(q)]T= ​C1​(q)C2​(q)⋮Cn​(q)​ ​

雅可比矩阵

如果我们希望 $C=0$，并且在整个模拟过程中保持不变，这意味着一阶导数 $\dot{C}$ 也必须为零。

同样，为了使 $\dot{C}=0$ ，也必须满足 $\ddot{C}=0$ 。

那么，根据链式法则：

$$若y=f(u)=f(g(x))，那么\frac{dy}{dx}=\frac{dy}{du}\cdot \frac{du}{dx}=f^{\prime }(g(x))g^{\prime }(x)$$

可知，$C$关于时间的导数可表示为：

$$\dot{C}=\frac{\partial C}{\partial \mathbf{q}}\dot{\mathbf{q}}=\mathbf{J}\dot{\mathbf{q}}=\mathbf{J}V$$

$\mathbf{J}$ 被称作$C$的雅可比矩阵。雅可比矩阵是梯度的推广，而梯度本身又是斜率的推广。$\mathbf{J}$ 的每一行都是每个行为函数的梯度。雅可比矩阵告诉我们每个行为函数如何对每个状态变量的变化做出反应。

定义：

J = [ ∂ C 1 ∂ q 1 ∂ C 1 ∂ q 2 ⋯ ∂ C 1 ∂ q 3 n ∂ C 2 ∂ q 1 ∂ C 2 ∂ q 2 ⋯ ∂ C 2 ∂ q 3 n ⋮ ⋮ ⋱ ⋮ ∂ C m ∂ q 1 ∂ C m ∂ q 2 ⋯ ∂ C m ∂ q 3 n ] {\small \mathbf{J}=

} J= ​∂q1​∂C1​​∂q1​∂C2​​⋮∂q1​∂Cm​​​∂q2​∂C1​​∂q2​∂C2​​⋮∂q2​∂Cm​​​⋯⋯⋱⋯​∂q3n​∂C1​​∂q3n​∂C2​​⋮∂q3n​∂Cm​​​ ​

其中，${\mathbf{J}}_{ij}$表示第 $i$ 个约束对第 $j$ 个广义坐标的微分；n为约束个数，m为广义坐标的个数。

这里有个很重的细节，$\mathbf{J}$若非满秩，也就是约束之间存在线性关系，或者说其中一个约束能从其他约束推导出来，那么$\mathbf{J}$就无法求其逆矩阵，关于它的线性方程组 $\mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T\lambda =b$就不能通过求逆得到$\lambda$了。但如果只有一个约束，那么$\mathbf{J}$为 $1\ast 3n$的矩阵，${\mathbf{M}}^{-1}$为 $3n\times 3n$ 对角矩阵，${\mathbf{J}}^T$为 $3n\ast 1$的矩阵，那么最终有效质量${\mathbf{J}}_{1,3n}{\mathbf{M}}_{1,3n}^{-1}{\mathbf{J}}_{1,3n}^T$就是个常数，常数可以取倒数，这也是后面顺序冲量法（SI）和投影高斯赛德尔法（PGS）能直接求拉格朗日乘数$\lambda$或${\lambda }_i$的原因。

拉格朗日乘子

根据
$$\dot{C}=\mathbf{J}\dot{\mathbf{q}}=0$$
以及理想约束下约束力不做功，约束力与速度正交，即:
$$F_c^T\dot{q}=0$$

可知，$F_c$与 $\mathbf{J}$ 平行，因此我们可以将约束力$F_C$写成 $\mathbf{J}$ 的倍数形式：

$$F_c={\mathbf{J}}^T\lambda$$

其中，向量$\lambda$有$m$个标量分量，在这个矩阵向量乘法中，每个分量${\lambda }_i$与$\mathbf{J}$的一行（即第 $i$ 个约束函数的梯度）相乘，然后将它们相加。即:

$${\mathbf{J}}^T\lambda =▽C_1{\lambda }_1+▽C_2{\lambda }_2+\cdots +▽C_m{\lambda }_m$$

$\lambda$ 就是拉格朗日乘子。

基于约束的求解

经过上面的矩阵推导，我们得到
{ C = 0 C ˙ = J q ˙ = 0 C ¨ = J ˙ q ˙ + J q ¨ F c = J T λ （ 1 ）

（1） ⎩ ⎨ ⎧​C=0C˙=Jq˙​=0C¨=J˙q˙​+Jq¨​Fc​=JTλ​（1）
那么就可以尝试求解约束了。约束求解分为基于力、基于冲量和基于位置三种方式，分别对应使用$\ddot{C}$、$\dot{C}$和$C$，通过求解拉格朗日乘数求解约束。

下面依次介绍。

基于力的求解

由上面（1）式可知：

$$\ddot{C}=\dot{J}\dot{q}+J\ddot{q}=\dot{J}\dot{q}+J{M}^{-1}(F_{ext}+F_c)=\dot{J}\dot{q}+J{M}^{-1}(F_{ext}+J^T\lambda )=0$$

可以得到：
$$\mathbf{J}{M}^{-1}{\mathbf{J}}^T\lambda =-\dot{\mathbf{J}}\dot{q}-\mathbf{J}{M}^{-1}{F}_{ext}（2）$$

其中 $\dot{\mathbf{J}}=\frac{\partial \mathbf{C}}{\partial \mathbf{q}}$

式（2）中只有λ是未知的，由此解这个线性方程组就能得到约束力$F_c$。那么对应速度和位置通过积分就能获得。

这就是基于约束力法解约束的思路。

基于冲量的求解

类似的，用额外冲量代替约束力，根据冲量定义：
$$I=M\Delta V={\int }_t^{t+\Delta t}Fdt$$

使用半隐式欧拉法的话，力认为是恒定的，那么

$$M\Delta V=F\Delta t=J^T\lambda \Delta t$$
$$\Delta V=M^{-1}{J}^T\lambda \Delta t$$

那么
$$\dot{C}=J\dot{q}=JV=J(V_{init}+\Delta V)=J(V_{init}+M^{-1}{J}^T\lambda \Delta t)=0$$
得到：

$$\mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T\lambda \Delta t=-\mathbf{J}{V}_{init}（3）$$

解式（3）这个线性方程组就能得到λ和Δv了，对应Δx也能得到。

如果只是通过上面解得出Δv，仅仅保证速度不会违反约束，而要保证位置同样不会违反约束，可以在速度约束加上Baumgarte 项，即$JV+b=0$，其中 $b=-\beta \frac{e}{\Delta t}(0<\beta <1)$，e代表误差项，例如碰撞约束中就是穿透深度，β代表约束违反修正速度。

基于位置的求解

基于位置的约束求解在使用$C=0$和前面有些差异，无法像$\dot{C}$、$\ddot{C}$那样得到雅可比矩阵$\mathbf{J}$，所以用泰勒级数对其展开：

$$C(x+\Delta x)\approx C(x)+\nabla C(x)\Delta x=C(x)+\mathbf{J}\Delta x=0$$

那么$$J\Delta x=-C(x)$$

如果使用半隐式欧拉法

$$\Delta x=v_{t+\Delta t}\Delta t=(v_{init}+a_t\Delta t)\Delta t=(v_{init}+M^{-1}{J}^T\lambda \Delta t)\Delta t$$

带入上面得到：
$$J{v}_{init}\Delta t+J{M}^{-1}{J}^T\lambda \Delta t^2=-C(x)$$

$$\mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T\lambda \Delta t^2=-\mathbf{J}{v}_{init}\Delta t-C(x)（4）$$

解这个线性方程组就能得到λ，然后Δx亦可得到，Δv=Δx/Δt。

PGS与SI

至此我们得到了不同形式的基于约束力法求解约束的线性方程组：

$$\{\begin{array}{l}\mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T\lambda =-\dot{\mathbf{J}}\dot{q}-\mathbf{J}{M}^{-1}{F}_{ext}（2）\\ \mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T\lambda \Delta t=-\mathbf{J}{V}_{init}（3）\\ \mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T\lambda \Delta t^2=-\mathbf{J}{v}_{init}\Delta t-C(x)（4）\end{array}$$

解上面的线性方程组就能求解约束。

首先判断方程组解的情况。

当$\mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T$满秩时，λ有唯一解，但物理模拟中约束个数、约束梯度的维数都无法保证，约束线性无关性也无法保证，导致$\mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T$秩无法保证。所以方程组可能有唯一解，可能无解，也可能有无数解。

然后考虑求解算法。

可用迭代法求解上述线性方程组，比如雅可比迭代法、高斯赛德尔迭代法（GS）、超松弛迭代法（SOR）、共轭梯度法等。

游戏引擎常用GS的改进型，即投影高斯赛德尔算法（PGS）及其等效算法顺序冲量法（SI）进行求解。

SI

前面也提到了，如果只有一个约束，那么最终有效质量${\mathbf{J}}_{1,3n}{\mathbf{M}}_{1,3n}^{-1}{\mathbf{J}}_{1,3n}^T$就是个常数，那么SI算法使用基于冲量的，最终对于每个约束迭代求解

$$\lambda \Delta t=\frac{-\mathbf{J}{V}_{init}}{\mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T}$$

为什么要迭代多次呢？因为不同约束之间存在影响， 调整满足了这个约束，另一个约束可能又违反了。也就是多次局部约束求解，最终达到全局约束求解。

SI还有加速迭代收敛速度，使用暖启动，也就是每次迭代λ初始值使用上一次迭代的值。

还有就是给不同约束分优先级，比如碰撞位置约束>碰撞速度约束>摩擦约束。

SI伪代码如下：

for i = 0:maxIterations
{
	for each constraint C
	{
		find lambda for C
		apply fix (impulse) to velocity
	}
}

PGS

PGS使用迭代公式求解线性方程组

思路和SI类似，可视化思想如下图所示：

• 两条绿线代表三维中的两个平面，黑点是两个平面上都没有的点。如果每个平面代表一个约束，并且这个点是我们世界的当前状态，那么没有约束被满足。

• 我们首先将点投影到第一个平面以满足第一个约束条件，然后将点投影到第二个平面以满足第一二个约束条件，再回到第一个平面，以此类推。

• 迭代越多，点就越接近全局解决方案，全局解决方案表示两个约束都得到满足的状态。这就是SI/GS背后的原理：我们将冲量应用于一对刚体，一次固定一个约束，然后我们一遍又一遍地迭代每个约束，最终收敛到一个全局解决方案。

具体GS/PGS算法参考附录博客

不等式约束

前面讨论的都是等式约束，当约束是不等式约束，问题就变成线性互补问题（LCP）了，当同时包含等式和不等式就是混合线性互补问题（MLCP）。

但SI和PGS求解方式还是类似，只不过会对迭代的各自λ累积值限制为大于等于0，即：
$$\sum {\lambda }_i\ge 0$$

至于为什么不等式约束也能使用类似雅可比矩阵+拉格朗日乘子法，很多文献都没解释清楚。

不过大家可以从另一个角度去理解线性互补问题，就是含不等式约束的最优问题。其解决方法就是使用含KKT条件的拉格朗日乘数法进行求解，将含约束的优化问题变成无约束优化。放到物理引擎求解中，就是将约束求解问题看成二次规划问题（QP）。

关于比较QP和LCP，我在这篇【Unity学习笔记】第十九 · 物理引擎约束求解解惑（LCP，最优，拉格朗日乘数法，SI，PGS，基于冲量法）文章略有讨论，大家可以酌情参考。

摩擦力

摩擦力研究不多，便不多讨论了。

按陈思格的说法，SI方法在考虑摩擦的影响时，是用了一种"作弊"(cheat/hack)的方式：令迭代中的摩擦冲量截断来满足摩擦定律。这种做法看似合理，实际上使得PGS方法无法保证收敛性。不过，从实际经验来看，在游戏物理模拟中是可以容忍这一点的。

同时需要注意的一点是，physx引擎两种算法PGS和TGS在处理摩擦方式不同：PGS求解器的最后三步位置迭代才求解摩擦约束，而TGS在每次迭代都会求解摩擦约束。

PhysX 4.0 SDK Rigid Body Dynamics

后记

至此，本文用不小的篇幅系统梳理了整个物理引擎脉络。乍一看好像也没什么，不就是数值积分、碰撞检测和约束求解三部分吗？但就像史铁生说的 「万事万物，你若预测它的未来，你就会说它有无数种可能，可你若回过头去看它的以往，你就会知道其实只有一条命定的路。」这一路走过，需要学习和验证的东西太多太多。但好在这条路最终还是走下来了，也感谢物理引擎这段时间的陪伴。

希望这篇文章对大家有所帮助，如果觉得本文不错的话，不要吝惜点赞、收藏和关注哦 ~ 谢谢大家！

参考目录

物理引擎综述 / 基础

• cocos 3D 物理系统
• GAMES104-现代游戏引擎：从入门到实践
  • GAMES104课程笔记10-Basics Concepts in Physics System
• 引擎架构剖析——物理引擎全解析（六）
• Video Game Physics Tutorial - Part I: An Introduction to Rigid Body Dynamics
• 物理引擎一些散乱知识点

碰撞检测

• 游戏物理引擎(二) 碰撞检测之Broad-Phase
• Video Game Physics Tutorial - Part II: Collision Detection for Solid Objects
• Collision Detection – CSO & Support Function
• Game Physics: Collision Detection – GJK
• Game Physics: Contact Generation – EPA

约束求解

• Constraint-Based Physics
• Game Physics: Resolution – Contact Constraints
• Position Based Dynamics与二次规划
• 基于约束的物理
• PhysX原理与实现：一般约束
• 物理引擎之约束求解(一)——线性互补问题
• Video Game Physics Tutorial - Part III: Constrained Rigid Body Simulation
• Game Physics Series 周明倫
• Karush-Kuhn-Tucker (KKT)条件

书籍

• 《实时碰撞检测算法技术 Real-time Collision Detection》
• 《游戏物理引擎开发 Game Physics Engine Development》
• 《基于物理的建模与动画 Foundations of Physically Based Modeling and Animation》