迭代求解器

在构建完所有约束（接触、摩擦、关节）之后，我们得到一个巨大的、耦合的线性互补问题（LCP）：

$$K\lambda =b-Jv$$

并附带 $\lambda$ 的各种限制（如非负性）。对于一个有成百上千个接触点的复杂场景，一次性精确求解这个巨大的方程组（这被称为直接法）在计算上是不可行的。迭代求解器 (Iterative Solvers) 提供了一种高效得多的替代方案。它不追求一步到位找到精确解，而是通过多次（例如10-20次）简单、快速的迭代，逐步逼近最终的正确解。这种方法的思想简单、实现鲁棒，并且能够自然地处理不等式约束，是所有现代实时物理引擎的核心。

1. 为什么需要迭代？耦合问题

想象一个三个箱子堆叠的场景：A 在 B 上，B 在 C 上。B 的稳定不仅依赖于 C 给它的支撑力，也依赖于 A 施加给它的压力。C 的稳定则依赖于 A 和 B 共同的压力。所有这些约束都是耦合 (coupled) 在一起的。单独求解任何一个约束，而不考虑其他约束的影响，是得不到正确结果的。

迭代求解器正是通过“迭代”来解决这个耦合问题。在每一轮迭代中，它都会重新计算每个约束的冲量，而在计算约束 i 时，它会利用到其他约束 j 在本轮迭代中已经计算出的最新结果。这样，信息（如压力）就可以在约束之间“传播”开来。经过多次迭代，整个系统会逐渐“沉降”到一个满足所有约束的平衡状态。

2. 投影高斯-赛德尔 (PGS) / 顺序脉冲 (SI)

这是迄今为止在游戏物理引擎中最流行、最成功的迭代求解算法。算法按照一个固定的顺序，逐一处理列表中的每一个约束。对于每个约束，它计算并施加一个可以满足该约束的冲量增量 (impulse delta)。使用高斯-赛德尔 (Gauss-Seidel)的经典迭代方法。其核心思想是，在计算第 i 个未知数时，立即使用在本轮迭代中已经计算出的前 $i-1$ 个未知数的最新值。意味着当我们求解一个约束的冲量后，我们会立即用这个冲量去更新相关物体的速度。这样，在处理下一个约束时，它就能“看到”前一个约束所产生的效果。

3. 速度迭代求解

下面我们推导一下如果把 $\lambda$ 的迭代方式转为速度迭代的求解方式。

对于第 $i$ 个约束力，可以表示成

$$\begin{array}{rl}\sum _j{\mathbf{K}}_{ij}{\lambda }_j& ={\mathbf{c}}_i\\ {\mathbf{c}}_i& ={\mathbf{b}}_i-{\mathbf{J}}_i{\mathbf{v}}^{\prime }\end{array}$$

我们根据 $i,j$ 进行展开，

$$\begin{array}{rl}\sum _{j<i}{\mathbf{K}}_{ij}\lambda +{\mathbf{K}}_{ii}{\lambda }_i+\sum _{j>i}{\mathbf{K}}_{ij}{\lambda }_j& ={\mathbf{c}}_i\\ {\lambda }_i& =\frac{1}{{\mathbf{K}}_{ii}}\left({\mathbf{c}}_i-\sum _{j<i}{\mathbf{K}}_{ij}\lambda -\sum _{j>i}{\mathbf{K}}_{ij}{\lambda }_j\right)\end{array}$$

使用GS方程的迭代方式，我们根据 $j$ 来设置 $\lambda$ 迭代公式

$${\lambda }_j^k=\{\begin{array}{c}{\lambda }_j^{k+1}\\ {\lambda }_j^k\end{array}\begin{array}{c}j<i\\ j>i\end{array}$$

${\lambda }_i^{k+1}$ 的第 $k+1$ 次迭代，方程变成

$${\lambda }_i^{k+1}=\frac{1}{{\mathbf{K}}_{ii}}\left({\mathbf{c}}_i-\sum _{j<i}{\mathbf{K}}_{ij}{\lambda }^{k+1}-\sum _{j>i}{\mathbf{K}}_{ij}{\lambda }_j^k\right)$$

引入迭代的变化量 $\Delta {\lambda }_i^k$,

$$\begin{array}{rlr}\Delta {\lambda }_i^k& =& {\lambda }_i^{k+1}-{\lambda }_i^k\\ {\lambda }_i^{k+1}& =& {\lambda }_i^k+\Delta {\lambda }_i\end{array}$$

将下面的等式代入迭代的方程中，

$$\begin{array}{rl}{\mathbf{c}}_i& ={\mathbf{b}}_i-{\mathbf{J}}_i{\mathbf{v}}^{\prime }\\ \sum _{j<i}{\mathbf{K}}_{ij}{\lambda }^{k+1}& =\sum _{j<i}{\mathbf{J}}_i{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^{k+1}\\ \sum _{j>i}{\mathbf{K}}_{ij}{\lambda }_j^k& =\sum _{j>i}{\mathbf{J}}_i{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^k\\ {\mathbf{K}}_{ii}{\lambda }_i^k& ={\mathbf{J}}_i{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ii}{\lambda }_i^k\end{array}$$

把等式转为 $\Delta {\lambda }_i^k$ 迭代的等式,

$$\begin{array}{rl}{\lambda }_i^k+\Delta {\lambda }_i^k& =\frac{1}{{\mathbf{K}}_{ii}}\left({\mathbf{b}}_i-{\mathbf{J}}_i{\mathbf{v}}^{\prime }-\sum _{j<i}{\mathbf{J}}_i{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^{k+1}-\sum _{j>i}{\mathbf{J}}_i{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^k\right)\\ {\lambda }_i^k+\Delta {\lambda }_i^k& =\frac{1}{{\mathbf{K}}_{ii}}\left({\mathbf{b}}_i-{\mathbf{J}}_i\left({\mathbf{v}}^{\prime }+\sum _{j<i}{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^{k+1}+\sum _{j>i}{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^k\right)\right)\\ \Delta {\lambda }_i^k& =\frac{1}{{\mathbf{K}}_{ii}}\left({\mathbf{b}}_i-{\mathbf{J}}_i\left({\mathbf{v}}^{\prime }+\sum _{j<i}{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^{k+1}+\sum _{j>i}{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^k\right)-{\mathbf{K}}_{ii}{\lambda }_i^k\right)\\ \Delta {\lambda }_i^k& =\frac{1}{{\mathbf{K}}_{ii}}\left({\mathbf{b}}_i-{\mathbf{J}}_i\left({\mathbf{v}}^{\prime }+\sum _{j<i}{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^{k+1}+\sum _{j>i}{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^k+{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ii}{\lambda }_i^k\right)\right)\\ \Delta {\lambda }_i^k& =\frac{1}{{\mathbf{K}}_{ii}}\left({\mathbf{b}}_i-{\mathbf{J}}_i\left({\mathbf{v}}^{\prime }+\sum _{j<i}{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^{k+1}+\sum _{j\ge i}{\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^k\right)\right)\end{array}$$

前面我们知道

$$\mathbf{v}={\mathbf{v}}^{\prime }+{\mathbf{M}}^{-1}{\mathbf{J}}_{n+1}^T\lambda$$

使用 $\lambda$ 的GS定义 $\lambda =\left({\lambda }_1^{k+1},{\lambda }_2^{k+1},\cdots ,{\lambda }_i^k,{\lambda }_{i+1}^k\cdots \right)$ 代入上面，可以得到

$$\begin{array}{rl}{\mathbf{v}}^k& ={\mathbf{v}}^{\prime }+{\mathbf{M}}^{-1}{\mathbf{J}}_{n+1}^T\lambda \\ {\mathbf{v}}^k& ={\mathbf{v}}^{\prime }+{\mathbf{M}}^{-1}\sum _{j<i}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^{k+1}+{\mathbf{M}}^{-1}\sum _{j\ge i}{\left({\mathbf{J}}^T\right)}_{ij}{\lambda }_j^k\end{array}$$

我们发现此时 $\Delta {\lambda }_i^k$ 的等式可以利用上面的等式中 ${\mathbf{v}}^{\mathbf{k}}$ 进行整理

$$\Delta {\lambda }_i^k=\frac{1}{{\mathbf{K}}_{ii}}\left({\mathbf{b}}_i-{\mathbf{J}}_i{\mathbf{v}}^k\right)$$

这个 $\Delta {\lambda }_i^k$ 带来的速度改变为

$$\Delta {\mathbf{v}=\mathbf{M}}^{-1}{\left({\mathbf{J}}^T\right)}_i\Delta {\lambda }_i^k$$