上一篇文章把旋转的基础补了。这一篇开始进入正题，关于物理模拟。
目前主流的物理引擎都是利用约束求解的方式进行物理模拟，其核心算法包括两点：
（1）如何使用约束对物理场景进行建模
（2）使用何种方式求解约束
那么接下来就从这两点分头讨论。主要参考Erin Catto 对 Box2D 系列文章的介绍，Physx 也基本采用了同样的物理模拟方法。

（1）约束构建 - 以接触约束为例

contact_constraint_1.png

在碰撞检测阶段结束后，我们可以获取一对物体上的接触点位置 $\left( P_{a_{col} }，P_{b_{col}} \right)$
接触约束要求 $P_{b_{col}}$ 在 $P_{a_{col}}$ 平面的法线指向的同侧： $(P_{b_{col}} - P_{a_{col}}) \cdot \pmb{n} \geq 0$

于是我们可以构建接触约束：
$C = （P_b + r_b - P_a - r_a）\cdot \pmb{n} \geq 0$

（2）构建约束求解方程

对于这样一个约束 $C(P_a, P_b, r_a, r_b) \geq 0$ ，当满足 $\dot C \geq 0$ 时， $C(t+\triangle t) = C(t) + \dot C(t) \triangle t$ 向满足约束的梯度改变。
因此可以改变为求解 $\dot C \geq 0$

因此约束可以重写为：
$\dot{C} = (v_b + \omega_b \times r_b - v_a - \omega_a \times r_a) \cdot n +(P_b + r_b - P_a - r_a) \cdot (\omega_a \times n)$

在 ErinCatto IterativeDynamics GDC2005 link 中 Contact Model 部分有说明，因为假定 penetration 一定很小，所以忽略第二部分
$\dot{C} = (v_b + \omega_b \times r_b - v_a - \omega_a \times r_a) \cdot n$

接下来以规范化的形式重写约束，简化推导过程
两个刚体的状态向量： $q = [P_a, r_a, P_b, r_b]$
雅克比矩阵： $J = \partial C_i/ \partial q_j$
$J_i = [-n, -r_a \times n, n, r_b \times n]$
假设有 K 个约束 $C_1, ... C_K$ ， S 个刚体，那么 $J$ 的维度为 $J_{K*3S}$
$C = Jq \geq 0$
从刚才推导可知 $\dot C = J \dot q \geq 0$ ，第二部分 $\dot J q$ 同样被忽略

不等式约束可以通过拉格朗日乘子法改写为等式，不过基于冲量的模拟后面会用 Projected Gauss Siedel (PGS) 加上不等式截断做简单处理。因此此处先按等式继续求解。
如果当前帧满足约束 $\dot C = J \dot q_1 = 0$
那么下一帧仍然满足约束的条件为： $\dot C = J\dot q_2 = J (\dot q_1 + \triangle \dot q) = 0$

$M$ 为包含质量和角速度的矩阵:
$M = \left[ \begin{matrix} \pmb{m}_a & 0 & 0 & 0\\ 0 & \pmb{I}_a & 0 & 0\\ 0 & 0 & \pmb{m}_b & 0\\ 0 & 0 & 0 & \pmb{I}_b \\ \end{matrix} \right]$
使用 semi-implicit euler integration 可以写出两帧之间速度的关系：
$\dot q_2^{*}$ 为约束前的下一帧速度：
$\dot q_2^{*} = \dot q_1 + \triangle t M^{-1} F_{ext}$

$\dot q_2$ 为约束后的下一帧速度:
$\dot q_2 =\dot q_2^{*} + \triangle t M^{-1} F_{c}$
这里和 PBD 非常像，都是用类似的积分方式构建步进迭代，PBD的文章网上非常多了，后面只会简单写一篇吧。

为了方便写公式，假设 $F_{ext} = 0$ ：
约束冲量 $P_c = F_{c} \triangle t$
到这里已经构建了方程 $M^{-1}P_c$

第一个 trick，为了沿梯度方向求解，可以限制 $P_c = J^T \lambda$ ，即 $P_c$ 与 $J^T$ 同方向

第二个 trick，由于这样直接求解速度会有 jitter，所以约束增加一个 bias
$\dot C = J (\dot q_1 + \triangle \dot q) + b = 0$
当 $F_{ext} = 0$ 时
$\triangle \dot q = M^{-1}P_c = M^{-1}J^{T}\lambda$
所以有
$JM^{-1}J^{T} \lambda + b = -\dot J \dot q_1$
这就是冲量模拟经典的方程形式。求解目标为 $\lambda$ , $b$
注意这只是一个约束 $C_k$ 对应的 $J_k$ ，总共有K 个这样的方程，构成约束方程组。

(3) Sequential Impulse 求解约束

为什么叫做 Sequential Impulse：因为对于每个 constraint， $P_{ck}$ 是逐个求解的

Sequential Impulse 算法步骤：
（1）计算所有的 $\dot q_2^{*}$

（2）for k = 1,...,K 的 k个方程：
从 $J_kM^{-1}J_k^{T} \lambda_k= -\dot J_k \dot q_1$ 求解每一个 $\lambda_k$ ，从而得到 $P_{ck} = J_k^T \lambda_k$

（3）求解 $\dot q2$ 作为下一步的 $\dot q1$
$\dot q_2 = \dot q_2^{*} + M^{-1}P_{ck}$

(4) 更新所有位置 $q2 = q1 + \triangle t \dot q_2$

由于是不等式约束，对每一个局部冲量 $P_{ck}$ 直接做截断。
$\lambda_k >= 0$
这也是一个来自（Erin,GDC2005) link 的一个近似解法。

其他形式的约束

等式约束的情况：distance constraints, revolute joints, prismatic joints, 以及许多其他的 joint 类型
例如距离约束：
$C = 1/2 \cdot [(P_b + r_b - P_a - r_a)^2 - d^2]$

不等式约束的情况： contact constraints, joint angle limits

使用约束模拟弹性

$v_{rel,-}$ 表示碰撞前相对速度 $v_{rel,+}$ 表示碰撞后相对速度
$v_{rel} = v_a - v_b$

弹性系数的定义(Unified Framework for Rigid Body Dynamics)：
$e = \frac {v_{rel,+} \cdot n} {v_{rel,-} \cdot n}$

假设 $v_{rel,+}$ 和 $v_{rel,-}$ 方向相反
弹性约束（04-GDC09_Catto_Erin_Solver）：
$v_{rel,+} \geq e v_{rel,-}$
$\dot C = v_{rel,+} + ev_{rel,-} \geq 0$
相当于把 bias 中增加了 $ev_{rel,-}$

使用约束模拟摩擦力

库伦模型（Coulomb friction model)
$f = \mu F_n$

先构建约束模型让切线方向速度为0
假设两个切线方向 $\pmb{u_1}, \pmb{u_2}$
$\dot C_{u_{1}} = (v_b + \omega_b \times r_b - v_a - \omega_a \times r_a) \cdot u_1 = 0$
$\dot C_{u_{2}} = (v_b + \omega_b \times r_b - v_a - \omega_a \times r_a) \cdot u_2 = 0$
摩擦力约束不需要像穿透约束一样增加稳定项 bias，作者说是经验结论。