↓ 跳过正文

物理模拟中的动力学模型

·2747 字·13 分钟
从0开始制作游戏物理引擎 - 这篇文章属于一个选集。
§ 13: 本文

本文说明了物理引擎中的接触求解的物理模型,以及将其转换成LCP问题和用PGS方法求解。

本文只讨论刚体模拟。

此物理模型包含⑤部分:

  1. 牛顿-欧拉方程
  2. 运动学映射:将位形参数对时间的导数和平动速度和角速度联系起来
  3. 对于永久关节的等式约束
  4. 对于法向接触的不等式约束
  5. 库伦摩擦定律

注:本文最后的“带有铰链和关节的约束”可能会等我看完相应论文后挪到单独的文章中。

瞬时间动力学的推导
#

刚体运动学映射
#

刚体的运动由其质心处的平动和以质心为原点的局部坐标系的转动组合而成。将平动位置$\pmb{x}$和转动的姿态(以四元数表示)$Q$放在一起组合成广义坐标:

$$ \pmb{q} = (\pmb{x}, Q) = \begin{bmatrix} \pmb{x}^T & Q_x & Q_y & Q_z & Q_w \end{bmatrix} $$

而其对应的广义速度为:

$$ \dot{\pmb{q}} = (\dot{\pmb{x}},\dot{Q}) = (\pmb{v},\ G\pmb{\omega}) \qquad \pmb{u} = (\pmb{v}, \pmb{\omega}) $$

对于平动,这里显然有 $\pmb{v} = \dot{\pmb{x}}$。对于转动,使用刚体动力学中的四元数表示法可以得到:

$$ \begin{aligned} \dot{Q} & = G \pmb{\omega} \\ G & = \frac{1}{2} \begin{bmatrix} -Q_x & -Q_y & -Q_z \\ Q_w & Q_z & -Q_y \\ -Q_z & Q_w & Q_x \\ Q_y & -Q_x & Q_w \\ \end{bmatrix} \end{aligned} $$

这样,我们可以得到总的运动学映射:

$$ \begin{aligned} \dot{q} & = Hu \\ H &= \begin{bmatrix} 1_{3\times 3} & 0 \\ 0 & G \\ \end{bmatrix} \end{aligned} $$

约束
#

有两种约束:

  1. 双边约束:$C(q_1, \cdots, q_n; u_1, \cdots, u_m, t)= 0$。比如各种铰链施加给连接到的刚体的约束。
  2. 单边约束:$C(q_1, \cdots, q_n; u_1, \cdots, u_m, t) \ge 0$。比如接触分离的约束。

考虑两个半径为$r_1, r_2$,球心为$\pmb{x_1}, \pmb{x_2}$的球体,他们之间的约束为:

$$ C(\pmb{x}_1, \pmb{x}_2) = \|\pmb{x}_1-\pmb{x}_2\|-(r_1+r_2) $$

此时,如果$C(\pmb{x_1}, \pmb{x_2}) = 0$,那么这两个球相贴。

但如果$C(\pmb{x_1}, \pmb{x_2}) \ge 0$,那么两球是相离/相贴的,即非相交状态。

就是因为存在单边约束,我们才必须将问题转换成LCP问题。如果只有双边约束,直接用拉格朗日乘子法就能解了。

我们将约束施加给刚体的力分解为垂直于刚体(法向),以及平行于刚体(切向)的力,这样便于后面融入库伦摩擦力。

对于无摩擦接触力来说,接触力只在法向存在。

那么此时,法向接触约束就是:

$$ C(\pmb{q}, t) \ge 0 $$

从物理角度看,该约束由物体间的法向接触力$\pmb{f}_n$维持,其中:

$$ \pmb{f}_n \ge 0 $$

而且显然他两是互补关系:

$$ 0 \le C(\pmb{q}, t) \perp \pmb{f}_n \ge 0 $$

这里我们用集合

$$ \begin{aligned} &\mathcal{U} = \{i : \text{接触}i\text{为点接触}\} \\ &\mathcal{B} = \{i : \text{接触}i\text{为关节接触}\} \end{aligned} $$

来表示点接触和关节接触。后面用${}^{u}C(\pmb{q}, t)$表示点接触。关节接触同理。

库伦摩擦力(Coulomb’s Law of Friction)
#

即我们高中学的摩擦力,是经验定律,由库伦从大量实验中得出。

但我们不能直接使用库伦模型。因为他是个不连续的定律。库伦定律是导致物理模型变复杂的罪魁祸首。

库伦摩擦力模型及其问题
#

在接触点处建立局部坐标系:法向$\pmb{n}$,切向$\pmb{t}, \pmb{o}$,这样接触力$\pmb{F}$可以写成:

$$ \begin{aligned} \pmb{F} &= \pmb{F}_n \pmb{n} + \pmb{F}_t \\ \pmb{F}_t &= F_t \pmb{t} + F_o \pmb{o} \end{aligned} $$

注意这里是接触力而不是外力。接触力是接触面施加在物体上的反作用力。在这里被分为面给物体的支撑力(法向),以及他们之间的摩擦力(切向)。

库伦摩擦模型中,将摩擦力分为静摩擦(Sticky) 和 动摩擦(Slide):

$$ \pmb{F}_t = \begin{cases} & \text{任意满足}||\pmb{F}_t|| \le \mu N \text{的}\pmb{F}_t\text{,通常由外力决定} & , ||\pmb{v}_t|| = 0 \\ & -\mu F_n \frac{\pmb{v}_t}{||\pmb{v}_t||} & , ||\pmb{v}_t|| \gt 0 \end{cases} $$

即,当物体没有移动时是静摩擦。而移动之后就是动摩擦,摩擦力大小是$F_n \mu$,方向为移动的反方向。

或者用几何的方式。库伦条件规定了所有允许的接触力:

$$ \begin{aligned} C(F_n) & = \{\pmb{F}_t : ||\pmb{F_t}|| \le \mu F_n \} \\ F_n &\ge 0 \\ \end{aligned} $$

这在图像上构成一个朝向接触面外的锥体(摩擦锥):

摩擦锥

这个锥体的定义可以写得更清楚一些:

$$ \begin{aligned} \mu^2 \pmb{F}_n^2 - \pmb{F}_t^2 - \pmb{F_o}^2 & \ge 0 \\ F_n & \ge 0 \\ \end{aligned} $$

库伦模型的公式不是连续的:在静态/动态摩擦转换的临界点,摩擦力会发生突变,导致数值积分不稳定。Stewart20001中给出了一个例子:

考虑滑块在斜面上滑动(向上或者向下)

斜面上的砖块

此砖块的运动方程是:

$$ m \frac{dv}{dt} = mg\sin \theta - \mu mg \cos \theta \operatorname{sgn} v $$

其中$\operatorname{sgn}$是符号函数。

此方程对于$v$是不连续的:

滑块滑动的不连续性

在$v = 0$处有一个间断点。

甚至这个方程有可能不存在解,因为滑块在某些情况下,以初始速度$v$滑动一段时间后会停止。但方程中当$v = 0$时的解是$\sin \theta = 0$,这显然是不合理的。

Stewart指出,虽然可以通过某些手段把不连续函数光滑,但在这个场景下几乎没有好处,必须另寻他法。

为了解决库伦函数不连续的问题,我们干脆不使用函数,而是将摩擦问题变成一个LCP问题,用LCP的方式去求解。

使用最大耗散原理重新表述库伦模型
#

在材料力学中有一个“最大耗散原理”(又称最大逸散功原理),他比库伦定律更能表述正确的摩擦模型。他是个通用定理,但对于摩擦力来说,是指给定法向接触力$\pmb{F}_n$,摩擦力$\pmb{F}_t$是在给定的法向接触力$\pmb{F}_n$所允许的一切可能摩擦力中,使能量耗散率$-\pmb{F}_t \cdot v_{rel}$最大的那个一个。其中$\pmb{v}_{rel}$是接触点处的相对速度。

也就是说,可以将库伦摩擦模型变成一个优化问题:

$$ \pmb{F}_t \in \arg \max_{\pmb{F}_t^{'} \in C(F_n)}(-\pmb{v}_t \cdot \pmb{F}_t^{'}) $$

那么,对于单边接触,各项同性的库伦摩擦的最大耗散问题就是:

$$ \begin{aligned} & \text{maximize} & -(\pmb{v}_t \cdot \pmb{F}_t) \\ & \text{subject to} & \mu^2 \pmb{F}_n^2 - \pmb{F}_t^2 \ge 0 \end{aligned} $$

这就变成一个凸优化问题(目标函数是线性函数)。

注意这里的自变量是向量$\pmb{F}_t$。

但是这个问题不能用KKT条件。因为在锥顶$F_{n} = 0$处,可行域塌缩成一个点(锥尖尖那里),这导致约束变成$\pmb{F}_t^2 \le 0 \Rightarrow \pmb{F}_t = 0$。这样KKT的平稳性条件就变成$-\pmb{v}_t + \lambda \nabla \pmb{F}_t^2 = 0 \Rightarrow \pmb{v}_t = 0$。这样只要滑动速度不为0(这肯定会被触发),KKT在这里就无解。

这时,可以使用更宽松的Fritz-John条件,这样可以写成:

$$ \begin{aligned} \nabla L: & -\lambda_0 \nabla (\pmb{v}_t\pmb{F}_t) + \lambda_1 \nabla(\mu^2 \pmb{F}_n^2 - \pmb{F}_t^2) \\ & = -\lambda_0 \nabla(v_t F_t + v_o F_o) + \lambda_1 \nabla(\mu^2F_n^2 - F_t^2 - F_o^2) \end{aligned} $$

而$\nabla$算子是对$\pmb{F}_t = \begin{bmatrix}F_t \\ F_o\end{bmatrix}$求导,所以这个条件就变成:

$$ \begin{aligned} & \frac{\partial L}{\partial F_t} = -\lambda_0 v_t - 2\lambda_1 F_t = 0 \\ & \frac{\partial L}{\partial F_o} = -\lambda_0 v_o - 2\lambda_1 F_o = 0 \\ \end{aligned} $$

取$\lambda_0 = 2\mu F_n$归一化,可以将式子变成:

$$ \begin{aligned} \mu F_n v_t + \lambda_1F_t &= 0 \\ \mu F_n v_o + \lambda_1F_o &= 0 \\ \end{aligned} $$

然后再给约束条件起个名$\alpha = \mu^2 \pmb{F}_n^2 - F_t^2 - F_o^2 \ge 0$,这样Fritz-John条件就是:

$$ \begin{aligned} & \mu F_n v_t + \lambda_1F_t = 0 \\ & \mu F_n v_o + \lambda_1F_o = 0 \\ \alpha =& \mu^2 F_n^2 - F_t^2 - F_o^2 \ge 0 \\ & 0 \le \alpha \perp \lambda_1 \ge 0 \end{aligned} $$

这类似于LCP问题,但关键是$\alpha$并不是个线性函数,所以并不能视为LCP。

在现在的大多数游戏物理中,通常是将摩擦锥变成近似的棱锥或者盒子,这样才能将摩擦锥融入LCP问题(LCP问题只能处理多个线性方程包围的问题)。

摩擦锥变摩擦棱锥

使用最大耗散原理重新表述关节约束
#

如果要在关节约束上增加摩擦,我们首先假设关节约束只有一个自由度(比如PhysX中的滑动关节,或者纯铰链关节)。此时摩擦力方向只有一个,需要固定一个最大摩擦力$\mu F_n = F_{max}$(因为铰链没有法向的说法,只能手动给一个最大摩擦力),然后将上面的FJ条件变成:

$$ \begin{aligned} & F_{max} v_t + \lambda_1F_t = 0 \\ \alpha =& F_{max}^2 - F_t^2 \ge 0 \\ & 0 \le \alpha \perp \lambda_1 \ge 0 \end{aligned} $$

如果自由度更多,并且自由度的量纲是同一类型(比如绕任意三轴移动的关节),那么就是:

$$ \begin{aligned} & F_{max} v_x + \lambda_1F_x = 0 \\ & F_{max} v_y + \lambda_1F_y = 0 \\ & F_{max} v_z + \lambda_1F_z = 0 \\ \alpha =&F_{max}^2 - F_x^2 - F_y^2 - F_z^2 \ge 0 \\ & 0 \le \alpha \perp \lambda_1 \ge 0 \end{aligned} $$

如果量纲不一样(比如一个平动 + 两个转动)

$$ \begin{aligned} & c v_x + \beta F_x = 0 \\ & c\ell^{2}\omega_y + \beta \tau_y = 0 \\ & c\ell^{2}\omega_z + \beta \tau_z = 0 \\ \alpha &= c^{2} - \left( F_x^{2} + \frac{\tau_y^{2}+\tau_z^{2}}{\ell^{2}} \right) \ge 0 \\ 0 &\le \alpha \perp \beta \ge 0 \end{aligned} $$

这里$c$是平动时最大的摩擦力。而$cl^2$则是转动时最大的摩擦力。这里也可以将$cl^2$单独设成一个变量,只是你要明白此变量里面蕴含了长度单位,这样两边的量纲才能对齐。

Painleve悖论
#

是指研究下面这个粉笔在黑板上书写时接触问题产生的悖论:

Painleve悖论

问题中只要求粉笔下端紧贴黑板,接触点处有切向速度,但是对于$\theta$没有要求。假设粉笔与地面接触点为$(x_c, y_c)$,向右为$x$轴正方向,向上为$y$轴正方向,粉笔向左移动,即$\dot{x} < 0$。并假设逆时针下$\theta$为正。此时摩擦力$F = + \mu N$,那么有:

$$ \begin{aligned} m \ddot{x} &= F \\ m \ddot{y} &= N - mg \\ J\ddot{\theta} &= \frac{l}{2}(+F \sin \theta - N \cos \theta) \\ y_c &= y - \frac{l}{2}\sin \theta \\ \end{aligned} $$

第三个式子用力矩公式$J\ddot{\theta} = \tau = r \times F$可以得到。

把这些方程带入到$F = +\mu N$得到:

$$ [\frac{1}{m} - \frac{l^2}{4J}\cos \theta (\mu \sin \theta - \cos \theta)]N = g - \frac{l}{2} \sin \theta \dot{\theta}^2 $$

这里令方括号内的式子为$\Delta$,而等式右边为$S$,这样式子就是:

$$ N = \frac{S}{\Delta} $$

显然在真实物理中,$N \ge 0$恒成立。但是Painleve指出存在不成立的情况:

  • $\Delta = 0$时,即存在临界摩擦系数$\mu_c = \frac{\cos^2 \theta + \frac{4J}{ml^2}}{\sin \theta \cos \theta}$时,$N$变成无穷大。此时没有有限解。
  • $\Delta < 0, S > 0$时,$N < 0$反而会将粉笔往黑板拉,此时方程无解。

这个条件是容易达成的,比如取$\theta = 45^\circ, J = \frac{ml^2}{12}$, 此时临界摩擦系数$\mu_c = \frac{5}{3}$。

当$\mu = \mu_c$时,$\Delta = 0$。而当$\mu > \mu_c$时,$\Delta < 0$。

那你说当$\Delta < 0$的时候,我直接强制令$N = 0$来模拟粉笔离开黑板的情况呢?也不行。因为此时$\ddot{y_c} = -g < 0$还是会给物体一个向下的加速度,物体还是会陷入黑板。

Painlevel悖论告诉我们:刚体+库伦摩擦这个物理模型在某些情况下会失效。但现在业界也没有更好的物理模型(反在在实时模拟中没有)。现阶段,你可以认为”理想的“求解器在$\Delta \le 0$的情况下存在未定义行为。实际工程上到底会发生什么完全由实际的求解器决定,不是由物理决定的。

如何解决Painleve悖论
#

明明使用的是牛顿和欧拉定理,但为什么会有Painleve悖论?难道是违背了物理原理?正是如此!因为物理模型太理想化了:

  • 点接触假设:假设接触区域是一个点,面积为0,不可形变。这导致力不是状态的连续函数。真实的柔性接触中:
    • 双方都会变形,法向力是一个函数$N = k\delta + c\dot{\delta}$,其中$\delta$是穿透深度。此函数是连续的单值函数。没有了柔度,力就无法表示成连续函数。
    • 接触有面积,有屈服应力(指材料在拉伸或压缩过程中,当应力达到某一数值后,应力几乎不再增加但应变迅速增大),这让法向接触力存在上限。而点接触下无法定义屈服应力,这导致法向接触力的上限是无限大。
  • 库仑定律假设:
    • 在$\pmb{v}_t = 0$处摩擦力方向未定义,这导致我们只能使用互补问题去定义而不是方程
  • 刚体的刚性假设:
    • 刚体碰撞是“原子时刻”:普通物体碰撞虽然说时间也很短,但是至少是个连续的变化过程。而刚体则是瞬时的,这导致原本可以描述运动的方程,在$t \rightarrow 0$下产生了奇点

要想深挖必须学习材料力学。这里就不再细说了。

如果将上述假设中去掉的东西补回去:

补回去变成得到代价
柔度弹簧–阻尼接触模型力律函数连续单值,ODE良定确定性消失,函数依赖没有物理意义的参数
有界力屈服/压溃有解,但解在边界需要引入材料模型
屈服面凸 QP / CCP凸问题,可证明总可解无真静摩擦、各向同性摩擦
弹性体FEM / XPBD物理真实FEM解算慢

用加速度表示接触约束
#

接触约束(单边与双边)可以通过对约束$C(\pmb{q}, t)$做泰勒展开来用加速度表示。这里一般用附加在约束上的两者之间的相对距离表示约束(正如约束一节展示的例子那样)。

令$\pmb{\bar{q}} = \pmb{q} + \Delta \pmb{q}$和$\bar{t} = t + \Delta t$。将其做二元函数泰勒展开:

$$ \begin{aligned} \bar{C}(\pmb{\bar{q}}, \bar{t}) &= C(\pmb{q}, t) + \frac{\partial C}{\partial \pmb{q}}\Delta \pmb{q} + \frac{\partial C}{\partial t}\Delta t + \frac{1}{2}((\Delta \pmb{q})^T \frac{\partial^2 C}{\partial \pmb{q}^2} \Delta \pmb{q} + 2\frac{\partial^2C}{\partial \pmb{q} \partial t}\Delta \pmb{q} \Delta t + \frac{\partial^2 C}{\partial t^2}\Delta t^2) \end{aligned} $$

然后把位移公式:

$$ \Delta \pmb{q} = \dot{\pmb{q}} \Delta t + \frac{1}{2}\ddot{\pmb{q}} \Delta t^2 $$

带入到上面的式子,就有:

$$ \begin{aligned} \bar{C}(\pmb{\bar{q}}, \bar{t}) - C(\pmb{q}, t) = \Delta \bar{C}(\bar{\pmb{q}}, \bar{t}) = &(\frac{\partial C}{\partial q}\dot{\pmb{q}} + \frac{\partial C}{\partial t})\Delta t + \\ &\frac{1}{2}(\frac{\partial C}{\partial q}\ddot{\pmb{q}} + \dot{\pmb{q}}^T\frac{\partial^2 C}{\partial q^2}\dot{\pmb{q}} + 2\frac{\partial^2 C}{\partial q\partial t} + \frac{\partial^2 C}{\partial t^2}) \Delta t^2 \end{aligned} $$

而根据位移公式$x = v t + \frac{1}{2}a t^2$:

$$ \begin{aligned} \Delta \bar{C} &= \left(\frac{\partial C}{\partial \pmb{q}}\dot{\pmb{q}} + \frac{\partial C}{\partial t}\right)\Delta t + \frac{1}{2}\left( \frac{\partial C}{\partial \pmb{q}}\ddot{\pmb{q}} + \dot{\pmb{q}}^{\mathsf T}\frac{\partial^2 C}{\partial \pmb{q}^2}\dot{\pmb{q}} + 2\frac{\partial^2 C}{\partial \pmb{q}\partial t}\dot{\pmb{q}} + \frac{\partial^2 C}{\partial t^2} \right)\Delta t^2 \end{aligned} $$

对比 $\Delta C = \nu\,\Delta t + \frac{1}{2}a\,\Delta t^2$:

$$ \begin{aligned} \nu &= \frac{\partial C}{\partial \pmb{q}}\dot{\pmb{q}} + \frac{\partial C}{\partial t} \\ a &= \frac{\partial C}{\partial \pmb{q}}\ddot{\pmb{q}} + \dot{\pmb{q}}^{\mathsf T}\frac{\partial^2 C}{\partial \pmb{q}^2}\dot{\pmb{q}} + 2\frac{\partial^2 C}{\partial \pmb{q}\partial t}\dot{\pmb{q}} + \frac{\partial^2 C}{\partial t^2} \end{aligned} $$

代入运动学关系 $\dot{\pmb q}=H\pmb u,\ \ddot{\pmb q}=\dot H\pmb u+H\dot{\pmb u}$:

$$ \begin{aligned} \nu &= J\pmb{u} + \frac{\partial C}{\partial t} \\ \pmb{a} &= J\dot{\pmb{u}} + \pmb{k} \\ \pmb{J} &= \frac{\partial C}{\partial \pmb{q}}H \\ \pmb{k} &= \dot{J}\pmb{u} + \frac{\partial^2 C}{\partial \pmb{q}\partial t}H\pmb{u} + \frac{\partial^2 C}{\partial t^2} \\ \end{aligned} $$

注意这里的$\pmb{v}, \pmb{a}$是相对速度和相对加速度。

再次回到库伦摩擦力模型:滑动摩擦的表述,要求相对速度$v \ne 0$。

但是当存在滚动摩擦时,比如一个球在空中边转圈边垂直下落到地面上,和地面接触的一瞬间时,由于其和地面并没有切向的速度,所以库伦模型只能给出摩擦力为0的错误结论。

仿照约束子集的定义,定义摩擦子集,其中$\mathcal{S}$是滑动摩擦,而$\mathcal{R}$是滚动摩擦:

$$ \begin{aligned} \mathcal{S} &= \{i : C_n(\pmb q, \pmb u, t) = 0, \pmb{v}_n = 0, \pmb{v}_t \ne 0\} \\ \mathcal{R} &= \{i : C_n(\pmb q, \pmb u, t) = 0, \pmb{v}_n = 0, \pmb{v}_t = 0\} \\ \end{aligned} $$

此时我们可以用加速度来判断物体的运动趋势。对于滚动摩擦就有:

$$ \begin{rcases} & {}^{u}\mu_i\, {}^{u}F_{in}\, {}^{u}a_{it} + {}^{u}\lambda_{i1}\, {}^{u}F_{it} = 0 \\ & {}^{u}\mu_i\, {}^{u}F_{in}\, {}^{u}a_{io} + {}^{u}\lambda_{i1}\, {}^{u}F_{io} = 0 \\ \phantom{}{}^{u}\alpha_i =& {}^{u}\mu_i^2\, {}^{u}F_{in}^2 - {}^{u}F_{it}^2 - {}^{u}F_{io}^2 \ge 0 \\ & 0 \le {}^{u}\alpha_i \perp {}^{u}\lambda_{i1} \ge 0 \end{rcases} \forall i \in \mathcal{U} \cap \mathcal{R} $$

只是将原来的速度替换成加速度。

变量左上角的角标$u$表示他位于$\mathcal{U}$集合中。这里看上去符号很多,其实意思是找到单边约束中满足滚动摩擦的约束。

而对于双边约束中的滚动摩擦,也有类似的公式(以一自由度关节为例):

$$ \begin{rcases} & {}^bF_{i\ max}{}^ba_{if} + {}^b\lambda_{i1} {}^b F_{if} = 0 \\ & {}^{b}\pmb{a}_i = {}^bF_{i\ max}^2 - {}^bF_{if}^2 \ge 0 \\ & 0 \le {}^{b}\alpha_i \perp {}^{b}\pmb \lambda_{i1} \ge 0 \\ \end{rcases} \forall i \in \mathcal{B} \cap \mathcal{R} $$

此时滚动摩擦不满足最大耗散原理,他可以在摩擦锥内部的任意位置(而非边界处)。因为这时相对速度为0,无法确定最大耗散。

牛顿欧拉方程
#

将 牛顿-欧拉方程 用矩阵的方式重新表述:

$$ \begin{aligned} M(\pmb{q})\dot{\pmb{u}} & = g(\pmb{q}, \pmb{u}, t) \\ M & = \begin{bmatrix} m \pmb{1}_{3 \times 3} & \pmb{0} \\ \pmb{0} & \pmb{I} \\ \end{bmatrix} \\ g &= \begin{bmatrix} \pmb{F} \\ \pmb{\tau} - \pmb{\omega} \times \pmb{I} \pmb{\omega} \end{bmatrix} \end{aligned} $$

这里$M$是由普通质量和惯性张量$I$构成的质量矩阵(6x6的)。而$g$则是原本牛顿-欧拉方程的右边构成。

将$g(\pmb{q}, \pmb{u}, t)$使用单边/双边解出处的法向力,摩擦力以及其他所有广义力之和表示:

$$ M(\pmb{q})\dot{\pmb{u}} = {}^{u}J_n(\pmb{q})^T {}^{u}\pmb{F}_n + {}^{u}J_f(\pmb{q})^T {}^{u}\pmb{F}_f + {}^{b}J_n(\pmb{q})^T {}^{b}\pmb{F}_n + {}^{b}J_f(\pmb{q})^T {}^{b}\pmb{F}_f + g_{ext}(\pmb{q}, \pmb{u}, t) $$

其中最后一项$g_{ext}(\pmb{q}, \pmb{u}, t)$是所有非接触力和力矩的合力。

瞬时动力学模型
#

当前的瞬时动力学模型
#

综合上面所有的方程,可以得到瞬时动力学模型:

$$ \begin{cases} \text{① 运动学:} & \dot{\pmb q}=H(\pmb q)\pmb u & (16) \\ \text{② 动力学:} & M\dot{\pmb u} = {}^{u}J_n^{\mathsf T}\,{}^{u}\pmb F_n + {}^{u}J_f^{\mathsf T}\,{}^{u}\pmb F_f + {}^{b}J_n^{\mathsf T}\,{}^{b}\pmb F_n + {}^{b}J_f^{\mathsf T}\,{}^{b}\pmb F_f + \pmb g_{ext} & (34)\\[6pt] \text{③ 关节:} & {}^{b}\pmb a_n = \pmb 0 & (30)\\ \text{④ 法向:} & \pmb 0 \le {}^{u}\pmb F_n \;\perp\; {}^{u}\pmb a_n \ge \pmb 0 & (31) \\ \text{⑤ 摩擦:} & (25)(27)\ \text{滑动}\quad (32)(33)\ \text{滚动} & \end{cases} $$

滑动:单边接触

$$ \begin{aligned} \mu_i{}^{u}F_{in}\,{}^{u}\nu_{it} + {}^{u}F_{it}\,{}^{u}\beta_i &= 0\\ \mu_i{}^{u}F_{in}\,{}^{u}\nu_{io} + {}^{u}F_{io}\,{}^{u}\beta_i &= 0\\ \phantom{}{}^{u}\alpha_i = \mu_i^2\,{}^{u}F_{in}^2 - {}^{u}F_{it}^2 - {}^{u}F_{io}^2 &\ge 0\\ 0 \le {}^{u}\alpha_i & \perp {}^{u}\beta_i \ge 0 \end{aligned} \qquad\text{(25)} $$

滑动:关节

$$ \begin{aligned} \phantom{}{}^{b}F_{io\max}\,{}^{b}\nu_{io} + {}^{b}F_{io}\,{}^{b}\beta_i &= 0\\ \phantom{}{}^{b}\alpha_i = {}^{b}F_{io\max}^2 - {}^{b}F_{io}^2 &\ge 0\\ 0 \le {}^{b}\alpha_i &\;\perp\; {}^{b}\beta_i \ge 0 \end{aligned} \qquad\text{(27)} $$

滚动:单边接触

$$ \begin{aligned} \mu_i\,{}^{u}F_{in}\,{}^{u}a_{it} + {}^{u}F_{it}\,{}^{u}\beta_i &= 0\\ \mu_i\,{}^{u}F_{in}\,{}^{u}a_{io} + {}^{u}F_{io}\,{}^{u}\beta_i &= 0\\ \phantom{}{}^{u}\alpha_i = \mu_i^2\,{}^{u}F_{in}^2 - {}^{u}F_{it}^2 - {}^{u}F_{io}^2 &\ge 0\\ 0 \le {}^{u}\alpha_i &\;\perp\; {}^{u}\beta_i \ge 0 \end{aligned} \qquad\text{(32)} $$

滚动:关节

$$ \begin{aligned} \phantom{}{}^{b}F_{io\max}\,{}^{b}a_{io} + {}^{b}F_{io}\,{}^{b}\beta_i &= 0\\ \phantom{}{}^{b}\alpha_i = {}^{b}F_{io\max}^2 - {}^{b}F_{io}^2 &\ge 0\\ 0 \le {}^{b}\alpha_i &\;\perp\; {}^{b}\beta_i \ge 0 \end{aligned} \qquad\text{(33)} $$

模型的离散化
#

需要离散化动力学模型以方便算法进行求解。

在离散化时动力学方程之前,我们要明白,由于Painleve悖论,在某些时候模型是无解的,这是无法避免的问题。

有一部分原因在于模型工作在加速度层,某些时候的力为无穷大。我们可以将模型重新表述成以冲量和速度为未知量的形式,以减缓Painleve悖论中的问题。

M. Anitescu和F. A. Potra在《Formulating Dynamic Multi-Rigid-Body Contact Problems with Friction as Solvable Linear Complementarity Problems》中小修了一下模型,加入了松弛变量让模型在任何时候都可解。但代价是不再精确满足库伦定律:摩擦力可以略微越出摩擦锥,这导致满足静摩擦条件时物体可能还会一点点滑移。但我们这里就不说这个了。


为了模型能够求解,我们将模型在步长$\Delta t$下离散化。使用后向欧拉离散:

$$ \dot{\pmb{u}}(t_{k+1}) \approx \frac{\pmb{u}^{(k+1)} - \pmb{u}^{(k)}}{\Delta t}, \dot{\pmb{q}}(t_{k+1}) \approx \frac{\pmb{q}^{(k+1)} - \pmb{q}^{(k)}}{\Delta t} $$

其中变量右上角角标表示当前变量来自第几次迭代。

首先离散化牛顿-欧拉方程:

$$ \begin{aligned} M^{(k+1)}\bigl(\pmb u^{(k+1)}-\pmb u^{(k)}\bigr) &= {}^{u}J_n^{(k+1)\mathsf T}\,{}^{u}\pmb p_n^{(k+1)} +{}^{u}J_f^{(k+1)\mathsf T}\,{}^{u}\pmb p_f^{(k+1)} \\ & +{}^{b}J_n^{(k+1)\mathsf T}\,{}^{b}\pmb p_n^{(k+1)} +{}^{b}J_f^{(k+1)\mathsf T}\,{}^{b}\pmb p_f^{(k+1)} +\pmb p_{ext}^{(k+1)} \\ \pmb{p}^{(k+1)} &= \Delta t F^{(k+1)} \\ \pmb{p}_{ext} &= \Delta t \pmb{g}_{ext} \end{aligned} $$

然后离散运动学映射:

$$ \pmb q^{(k+1)}-\pmb q^{(k)} =\Delta t\,H^{(k+1)}\,\pmb u^{(k+1)} $$

这里有两个问题:

  1. 因为$\pmb{q}$内包含单位四元数,而将$\Delta t\,H^{(k+1)}\,\pmb u^{(k+1)}$加到$\pmb{q}^{(k)}$后会增大四元数长度(转的越大增大越大)。解决办法是每次步进后重新单位化$\pmb{q}^{(k+1)}$的四元数部分。但在转动较大时依旧有问题。
  2. $H^{(k+1)}$是$\pmb{q}^{k+1}$的函数,这导致这方程是隐式的。这很难解。所以对$H^{(k+1)}$做一次泰勒展开,取其线性部分$H^{k}$将其变为显式的

这样最后离散化结果就是:

$$ \pmb q^{(k+1)}-\pmb q^{(k)} =\Delta t\,H^{(k)}\,\pmb u^{(k+1)} $$
#

对于四元数膨胀的问题,JoltPhysics的做法是位置和旋转分开算,使用真正的四元数步进:

$$ \pmb{q}^{(k+1)} = e^{\frac{1}{2}\omega \Delta t} \otimes \pmb{q}^{(k)} $$

在代码上就是(注意欧拉公式$e^{i\theta} = \cos \theta + i \cdot \sin \theta$,所以上面的其实是四元数公式):

float  len  = ω*dt 的长度;          // θ = ‖ω‖Δt
Vec3   axis = ω*dt / len;           // 单位轴
Quat   dq   = Quat::sRotation(axis, len);   // (cos(θ/2), sin(θ/2)·axis)
mRotation = (dq * mRotation).Normalized();

然后离散化双边约束,只需将约束进行泰勒展开然后丢掉高阶项:

$$ \phantom{}{}^{b}C_n^{(k)} +\bigl({}^{b}J_n\bigr)^{(k)}\,\pmb u^{(k+1)}\,\Delta t +\frac{\partial\,{}^{b}C_n^{(k)}}{\partial t}\,\Delta t = 0 $$

然后离散化单边约束的互补条件,同样也是将约束进行泰勒展开:

$$ \pmb 0 \;\le\; {}^{u}\pmb p_n^{(k+1)} \;\perp\; \phantom{}{}^{u}C_n^{(k)} +\bigl({}^{u}J_n\bigr)^{(k)}\,\pmb u^{(k+1)}\,\Delta t +\frac{\partial\,{}^{u}C_n^{(k)}}{\partial t}\,\Delta t \;\ge\; \pmb 0 $$

现在摩擦还是在摩擦锥中,原问题无法表示成LCP。通过多面体近似摩擦锥:

摩擦锥近似

这里锥体的边数由你决定。越多算法越慢,但结果越精确。摩擦如果在边界上,则一定指向边界上的某个顶点(如图b)。如果不指向边界则LCP有无穷多解。

这里用重心坐标去表示摩擦:

多边形的顶点(图b)记为$\vec{d}$,然后用这些顶点构的重心坐标$\alpha_i$去表示摩擦冲量:

$$ \begin{rcases} & {}^u\pmb{p}_{if}^{(k+1)} = {}^uD_i {}^u\alpha_i \\ & {}^u\pmb{D}_i = \begin{bmatrix} \vec{d_{i1}} & \vec{d_{i2}} & \cdots & \vec{d_{in}} \\ \end{bmatrix} \\ & \sum_{j=1}^n{}^u \alpha_{ij} \le \mu_i\ {}^u\pmb{p}_{in}^{(k+1)} \\ \end{rcases} \forall i \in \mathcal{U} $$

其中$\alpha_i = (\alpha_{i1}, \alpha_{i2}, \cdots, \alpha_{in})$是乘了半径的重心坐标构成的向量。所以第三个式子里他是小于等于半径的。

现在增加松弛变量$\beta_i \ge 0$,当$\beta_i = 0$时为滚动,此时摩擦冲量可以位于多边形内部的任意位置。而$\beta_i > 0$时为滑动,此时摩擦冲量必须位于边界上(满足最大耗散),而此时 $\sum_j^n {}^u\alpha_i^j = \mu_i {}^u\pmb{p}_{in}^{(k+1)}$。

这样我们就有互补条件:

$$ \begin{aligned} & 0 \le (\mu_i\ {}^u\pmb{p}_{in}^{(k+1)} - \pmb{e}_i^T\ {}^u\pmb{\alpha}_i) \perp \phantom{}{}^u\beta_i \ge 0 & \forall i \in \mathcal{U} \end{aligned} $$

其中$\pmb{e}_i^T$是分量个数为多边形顶点个数(排除锥顶),所有元素都为1的向量。只是为了将求和符号变成点乘而做的。

但这个条件只约束了摩擦在摩擦锥内,并未约束最大耗散原理。要使摩擦冲量在摩擦锥边界上,可以将摩擦子空间的相对速度$\pmb{v}_{if}^{(k+f)}$映射到$\pmb{d_{ij}}$上来产生新的互补条件:

$$ \begin{aligned} & 0 \le ({}^u\pmb{D}_i^T\ {}^u\pmb{J}_{if}\ \pmb{u}^{(k+1)} + \pmb{e}_i\ {}^u\beta_i) \perp \phantom{}{}^u\pmb{\alpha}_i \ge 0 & \forall i \in \mathcal{U} \end{aligned} $$

这里${}^u\pmb{D}_i^T {}^u\pmb{J}_{if}\pmb{u}^{(k+1)}$展开就是${}^u\pmb{D}_i^T \frac{\partial C}{\partial \pmb{q}}H\pmb{u}^{(k+1)} ={}^u\pmb{D}_i^T \frac{\partial C}{\partial \pmb{q}}\dot{\pmb{q}} = {}^u\pmb{D}_i^T C_{\pmb{q}} = {}^u\pmb{D}_i^T \nu$为$D^T$乘上$C$对时间的全导数。这里是将速度强制投影到摩擦锥的边界上。

注意:这里假设约束不是时变的,即$\nu = \frac{\partial C}{\partial \pmb{q}}\dot{\pmb{q}}$。

这样我们可以将所有的单边接触的互补条件堆叠在一起构成一个式子:

$$ \begin{aligned} & 0 \le ({}^u\pmb{D}^T\ {}^u\pmb{J}_f \pmb{u}^{(k+1)} + {}^u \pmb{E}\ {}^u\pmb{\beta}) \perp {}^u \pmb{\alpha} \ge 0 \\ & 0 \le ({}^u\pmb{U}^T\ {}^u\pmb{p}^{(k+1)} - {}^u \pmb{E}^T\ {}^u\pmb{\alpha}) \perp {}^u \pmb{\beta} \ge 0 \\ \end{aligned} $$

其中$\pmb{E}$是以${}^u\pmb{e}_i$为非零块的分块对角矩阵,他是用来对所有向量进行广播求和的,比如:

$$ \begin{aligned} E &= \begin{bmatrix} 1 & 0 \\ 1 & 0 \\ 1 & 0 \\ 1 & 0 \\ 0 & 1 \\ 0 & 1 \\ 0 & 1 \\ 0 & 1 \\ \end{bmatrix} \\ \pmb{\beta} &= \begin{bmatrix} \beta_{11} & \beta_{12} & \cdots & \beta_{1n} \\ \beta_{21} & \beta_{22} & \cdots & \beta_{2n} \\ \end{bmatrix} \\ E\pmb{\beta} &= \begin{bmatrix} \sum_i \beta_{1i} \\ \sum_i \beta_{2i} \\ \end{bmatrix} \end{aligned} $$

对于关节约束,则有:

$$ \begin{aligned} & 0 \le ({}^b\pmb{D}^T\ {}^b\pmb{J}_f\ \pmb{u}^{(k+1)} + {}^b \pmb{E}\ {}^b\pmb{\beta}) \perp {}^b \pmb{\alpha} \ge 0 \\ & 0 \le ({}^b\pmb{p}_{f\ max} - {}^b \pmb{E}^T\ {}^b\pmb{\alpha}) \perp {}^b \pmb{\beta} \ge 0 \\ \end{aligned} $$

其中${}^b\pmb{p}_{f\ max} = \Delta t\ {}^b \pmb{F}_{f max}$。

离散化模型总结
#

综合上述离散化结果,可以得到离散化模型:

线性化牛顿–欧拉

$$ \begin{aligned} & M^{(k)}\bigl(\pmb u^{(k+1)}-\pmb u^{(k)}\bigr) ={}^{u}J_n^{\mathsf T}{}^{u}\pmb p_n^{(k+1)} +{}^{u}J_f^{\mathsf T}{}^{u}D\,{}^{u}\pmb\alpha +{}^{b}J_n^{\mathsf T}{}^{b}\pmb p_n^{(k+1)} +{}^{b}J_f^{\mathsf T}{}^{b}D\,{}^{b}\pmb\alpha +\pmb p_{ext}^{(k)} & (41) \end{aligned} $$

线性化运动学映射

$$ \begin{aligned} & \pmb q^{(k+1)}-\pmb q^{(k)}=\Delta t\,H^{(k)}\pmb u^{(k+1)} & (42) \end{aligned} $$

线性化关节约束

$$ \begin{aligned} & {}^{b}J_n^{(k)}\pmb u^{(k+1)} +\frac{{}^{b}C_n^{(k)}}{\Delta t} +\frac{\partial{}^{b}C_n^{(k)}}{\partial t} =\pmb 0 & (43) \end{aligned} $$

线性化法向互补

$$ \begin{aligned} &\pmb 0\le{}^{u}\pmb p_n^{(k+1)} \;\perp\; \phantom{}{}^{u}J_n^{(k)}\pmb u^{(k+1)} +\frac{{}^{u}C_n^{(k)}}{\Delta t} +\frac{\partial{}^{u}C_n^{(k)}}{\partial t} \;\ge\pmb 0 & (44) \end{aligned} $$

单边摩擦

$$ \begin{aligned} &\begin{aligned} \pmb 0&\le\Bigl({}^{u}D^{\mathsf T}{}^{u}J_f\pmb u^{(k+1)}+{}^{u}E\,{}^{u}\pmb\beta\Bigr)\perp{}^{u}\pmb\alpha\ge\pmb 0\\ \pmb 0&\le\Bigl({}^{u}U\,{}^{u}\pmb p_n^{(k+1)}-{}^{u}E^{\mathsf T}{}^{u}\pmb\alpha\Bigr)\perp{}^{u}\pmb\beta\ge\pmb 0 \end{aligned} & (48) \end{aligned} $$

关节摩擦

$$ \begin{aligned} &\begin{aligned} \pmb 0&\le\Bigl({}^{b}D^{\mathsf T}{}^{b}J_f\pmb u^{(k+1)}+{}^{b}E\,{}^{b}\pmb\beta\Bigr)\perp{}^{b}\pmb\alpha\ge\pmb 0\\ \pmb 0&\le\Bigl({}^{b}\pmb p_{f\max}-{}^{b}E^{\mathsf T}{}^{b}\pmb\alpha\Bigr)\perp{}^{b}\pmb\beta\ge\pmb 0 \end{aligned} & (49) \end{aligned} $$

其中 ${}^b\pmb{p}_{f\ max} = \Delta t\ {}^b \pmb{F}_{f max}$

此时的模型是一个MLCP(混合线性互补问题),因为里面除了LCP条件之外还有额外的等式。

解瞬时动力学
#

虽然模型是一个MLCP,但发现未知广义位置$\pmb{q}^{(k+1)}$只出现在运动学映射方程中。所以,此MLCP可以解耦成:

  1. 以未知广义速度和冲量为未知量的LCP
  2. 以未知广义位置为未知量的运动学映射方程

这样我们可以先求LCP,然后将LCP的解带入运动学映射以更新广义位置。

可能存在的无解
#

在求解前需要知道,在数学上,如果去掉(43),(44)两式中的${}^uC_n^{(k)}, \frac{\partial {}^u C_n^{(k)}}{\partial t}\Delta t, \frac{\partial {}^b C_n^{(k)}}{\partial t}\Delta t$这三项的话,那么LCP就总是有解的。但在游戏物理中我们不能去掉这些(详见接下来的例子)。

从物理上理解:这三个量(其实两个量)分别表示:

  • ${}^uC_n^{(k)}$物体间的距离($> 0$相离,$= 0$相贴,$<0$相交)
  • $\frac{\partial {}^b C_n^{(k)}}{\partial t}$为间隙函数的改变量。一般来说此函数都是非时变的。但当存在其他改变物体广义坐标$\pmb{q}$的因素时(即无法完全由$C$表示约束),此函数会被动变成时变的,在游戏开发中主要是:
    • 动画驱动的刚体:由动画直接改变物体位置

而LCP不可解的情况只有在“物体被逼到重叠无法避免”的时候才不行。简单来说就是有外力导致物体不得不脱离关节约束。

比如一个Box被关节钉在地面上,此时有一个Kinematic类型(质量无限大)的物体往下移动压在Box上。这在游戏物理中是很可能发生的。

此时有两个方面会导致无解:

  • 不可行的初始条件:这对应于${}^uC_n^{(k)} < 0$。假设$\frac{\partial {}^b C_n^{(k)}}{\partial t}\Delta t = 0$ ,那么${}^uC_n^{(k)} \lt 0$表示物体互相穿透了,那么${}^u\pmb{J}_n \pmb{u} \ge -\frac{{}^uC_n}{\Delta t} \gt 0$需要求一个正的分离速度。如果此时只有接触那完全是可行的。但关键是问题中包含了限制自由度的关节,导致根本不可能存在此分离速度,导致问题可能无解。 这对于于Kinematic将Box压入地面时,由于约束的存在无法计算分离速度。
  • 物体或关节的轨迹指定为时间给定的函数:这对应于$\frac{\partial {}^b C_n^{(k)}}{\partial t} \ne 0$。此时方程变成${}^u\pmb{J}_n \pmb{u} \ge - (\frac{{}^uC_n}{\Delta t} + \frac{\partial {}^uC_n}{\partial t})$。而左端是盒子的速度为0,右端的$\frac{\partial {}^b C_n^{(k)}}{\partial t} \lt 0$,方程矛盾,所以无解。

这也很符合物理常识,正常的物体这样被挤压肯定就碎了,但是因为我们是刚体物理所以无法处理这件事。

时间步进LCP
#

将离散模型转换成矩阵形式:

所有的互补条件构成:

$$ \pmb 0 \;\le\; \begin{bmatrix} \phantom{}{}^{u}J_n^{(k)}\,\pmb u^{(k+1)} +\dfrac{{}^{u}C_n^{(k)}}{\Delta t} +\dfrac{\partial\,{}^{u}C_n^{(k)}}{\partial t} \\[12pt] \phantom{}{}^{u}D^{\mathsf T}\,{}^{u}J_f\,\pmb u^{(k+1)} +{}^{u}E\,{}^{u}\pmb\beta \\[12pt] \phantom{}{}^{b}D^{\mathsf T}\,{}^{b}J_f\,\pmb u^{(k+1)} +{}^{b}E\,{}^{b}\pmb\beta \\[12pt] \phantom{}{}^{u}U\,{}^{u}\pmb p_n^{(k+1)} -{}^{u}E^{\mathsf T}\,{}^{u}\pmb\alpha \\[12pt] \phantom{}{}^{b}\pmb p_{f\max} -{}^{b}E^{\mathsf T}\,{}^{b}\pmb\alpha \end{bmatrix} \;\perp\; \begin{bmatrix} \phantom{}{}^{u}\pmb p_n^{(k+1)}\\[8pt] \phantom{}{}^{u}\pmb\alpha\\[8pt] \phantom{}{}^{b}\pmb\alpha\\[8pt] \phantom{}{}^{u}\pmb\beta\\[8pt] \phantom{}{}^{b}\pmb\beta \end{bmatrix} \;\ge\;\pmb 0 \qquad (50) $$

而所有的等式条件构成:

$$ \begin{bmatrix} M & -{}^{b}J_n^{\mathsf T}\\[4pt] -{}^{b}J_n & 0 \end{bmatrix} \begin{bmatrix} \pmb u^{(k+1)}\\[4pt] \phantom{}{}^{b}\pmb p_n^{(k+1)} \end{bmatrix} = \begin{bmatrix} \pmb x_1\\[4pt] \pmb x_2 \end{bmatrix} \qquad (51) $$

其中

$$ \begin{aligned} \pmb x_1 &={}^{u}J_n^{\mathsf T}\,{}^{u}\pmb p_n^{(k+1)} +{}^{b}J_f^{\mathsf T}\,{}^{b}D\,{}^{b}\pmb\alpha +{}^{u}J_f^{\mathsf T}\,{}^{u}D\,{}^{u}\pmb\alpha +M\,\pmb u^{(k)} +\pmb p_{ext}\\[6pt] \pmb x_2 &=\frac{{}^{b}C_n^{(k)}}{\Delta t} +\frac{\partial\,{}^{b}C_n^{(k)}}{\partial t} \end{aligned} $$

为了解出$\pmb{u}, \pmb{p}_n$,给左端矩阵求逆:

$$ \begin{bmatrix} \pmb u^{(k+1)}\\[4pt] \phantom{}{}^{b}\pmb p_n^{(k+1)} \end{bmatrix} = \begin{bmatrix}S_{11} & S_{12}\\ S_{12}^{\mathsf T} & S_{22}\end{bmatrix} \begin{bmatrix} \pmb x_1\\[4pt] \pmb x_2 \end{bmatrix} \qquad (52) $$

这里:

$$ \begin{aligned} S_{11} &= M^{-1} + M^{-1}{}^{b}J_n^{\mathsf T}\,B^{-1}\,{}^{b}J_n\,M^{-1} && (53a)\\[6pt] S_{12} &=\; M^{-1}\,{}^{b}J_n^{\mathsf T}\,B^{-1} && (53b)\\[6pt] S_{22} &= B^{-1} && (53c) \end{aligned} $$

其中

$$ B \;=\; -{}^{b}J_n\,M^{-1}\,{}^{b}J_n^{\mathsf T} $$

把解出来的广义速度代回(50)式就有最后标准的LCP:

$$ \begin{aligned} A &= \begin{bmatrix} \phantom{}{}^{u}J_n S_{11}{}^{u}J_n^{\mathsf T} & {}^{u}J_n S_{11}{}^{u}J_D^{\mathsf T} & {}^{u}J_n S_{11}{}^{b}J_D^{\mathsf T} & 0 & 0 \\ \phantom{}{}^{u}J_D S_{11}{}^{u}J_n^{\mathsf T} & {}^{u}J_D S_{11}{}^{u}J_D^{\mathsf T} & {}^{u}J_D S_{11}{}^{b}J_D^{\mathsf T} & {}^{u}E & 0 \\ \phantom{}{}^{b}J_D S_{11}{}^{u}J_n^{\mathsf T} & {}^{b}J_D S_{11}{}^{u}J_D^{\mathsf T} & {}^{b}J_D S_{11}{}^{b}J_D^{\mathsf T} & 0 & {}^{b}E \\ \phantom{}{}^{u}U & -{}^{u}E^{\mathsf T} & 0 & 0 & 0 \\ 0 & 0 & -{}^{b}E^{\mathsf T} & 0 & 0 \end{bmatrix} \\ \pmb b &= \begin{bmatrix} \phantom{}{}^{u}J_n\,\pmb r +\dfrac{{}^{u}C_n^{(k)}}{\Delta t} +\dfrac{\partial\,{}^{u}C_n^{(k)}}{\partial t} \\[14pt] \phantom{}{}^{u}J_D\,\pmb r \\[8pt] \phantom{}{}^{b}J_D\,\pmb r \\[8pt] \pmb 0 \\[8pt] \phantom{}{}^{b}\pmb p_{f\max} \end{bmatrix} \\ \pmb x &= \begin{bmatrix} \phantom{}{}^{u}\pmb p_n\\[4pt] \phantom{}{}^{u}\pmb\alpha\\[4pt] \phantom{}{}^{b}\pmb\alpha\\[4pt] \phantom{}{}^{u}\pmb\beta\\[4pt] \phantom{}{}^{b}\pmb\beta \end{bmatrix} \\ & \pmb 0 \;\le\; \pmb x \;\perp\; A\,\pmb x + \pmb b \;\ge\; \pmb 0 \end{aligned} $$

其中:

$$ \begin{aligned} \pmb r &= S_{11}\bigl(M\pmb u^{(k)}+\pmb p_{ext}\bigr) + S_{12}\left(\dfrac{{}^{b}C_n^{(k)}}{\Delta t} + \dfrac{\partial\,{}^{b}C_n^{(k)}}{\partial t}\right) \\ \phantom{}{}^uJ_D &= {}^uD^T\ {}^uJ_f \end{aligned} $$

如果系统中不存在关节,可以将$A, b, x$中删掉第三,五行。并从$A$中删除第三,五列。

并且,如果 $\frac{{}^u C_n}{\Delta t} + \frac{\partial {}^uC_n}{\partial t} \ge 0$。那么解总是存在(另外需要$M$是对称正定的,但$M$总是对称正定的)。

带有铰链和关节的约束
#

铰链(articulated-body) 是由关节连接的一组刚体构成的系统。

铰链体

而之前的模型在带有铰链和关节约束时有一些缺陷:

  1. 铰链不能有环路,否则$A$矩阵奇异,LCP有多解。
  2. 计算LCP时,最好情况下(使用PGS时),第$i$个刚体可以得到前$i - 1$个关节+刚体的信息。但得不到后面的信息。所以如果要所有信息都及时更新,PGS至少得遍历关节个数的次数。这在关节很多时可能很慢

所以对于铰链我们需要构造一个更高效的求解方法。比如PhysX中的Joint系列就是用瞬时动力学模型,而Articulate系列就是用铰链的理论。

罚函数法
#

核心思路是不解瞬时动力学模型中的LCP。而是将关节用专门的罚函数法去求解。

简单来说,罚函数法就是当物体违反关节时,简单地加一个弹簧力回去,尝试让物体满足关节约束。

当约束为完整约束时,函数为:

$$ \pmb{F}_{penalty} = -\alpha\pmb{J}^T(\Omega^2 C + 2\Omega \mu \dot{C} + \ddot{C}) $$

这里:

  • $C$:约束函数

  • $J$:$C$的雅克比矩阵

  • $\alpha, \Omega, \mu$:用户参数,用于控制力大小

    • $\alpha$:弹簧参数(Spring Constant):控制整体软硬
    • $\Omega$:固有频率(Natural Frequency):误差回正的频率(越大弹簧越硬)
    • $\mu$:阻尼比(Damping Ratio) 这里$F_{penalty}$分为三部分:
  • $-\alpha \Omega^2 \pmb{J}^T C$:刚度项,确定弹簧刚度。单位违反约束时产生多大力的系数(与位置相关)。

  • $-2\alpha\Omega\mu\pmb{J}^T\dot{C}$:阻尼项,弹簧恢复的阻尼。单位违反约束时其速度对应产生多大系数(与速度相关)。

  • $-\alpha J^T\ddot{C}$:加速度前馈,把本来会产生的违反约束的加速度抵消掉,用来提高稳定性。

罚函数的优点就是算起来贼快,缺点则是:

  • 不精确。精确需要极大的$\alpha$,这可能导致刚性方程。刚性方程是指:

    刚性方程是一类含有多个相差极大的时间尺度、导致显式数值方法必须使用极小步长才能保持稳定的微分方程,其典型特征是雅可比矩阵特征值实部的最大与最小绝对值之比远大于 1

  • 力无上界:违反越大力越大,可能导致模拟爆炸

一般用于软体模拟,或想要“弹簧感”的关节。

对于非完整约束,函数为:

$$ \pmb{F}_{penalty} = -\alpha (\frac{\partial C}{\partial \pmb{u}}^T)(\mu C + \dot{C}) $$

拉格朗日乘数法
#

罚函数法是把约束像弹簧一样推回去。而拉格朗日乘数法则是得到防止违约所需的严格的力。

可用于仿真具有完整约束和非完整等式约束的系统。这些约束可以被转化成一般约束形式:

$$ \begin{aligned} & \pmb{J}(\pmb{q}, \pmb{u}, t)\dot{\pmb{u}} + \pmb{k}(\pmb{q}, \pmb{u}, t) = 0 & (55) \end{aligned} $$

在具有$m$维约束的$n$维系统中,矩阵$\pmb{J}$的维数为$m \times n$,向量$\pmb{k}$维数为$m$。

完整约束可以通过对$C$求两次导数得到此一般形式,非完整约束只需求一次导数。

对于完整约束$C(\pmb{q}, t) = 0$,其对应结果为:

$$ \begin{aligned} \pmb{J} &= \frac{\partial C}{\partial \pmb{q}} \pmb{H} \\ \pmb{k} &= \frac{\partial C}{\partial \pmb{q}}\frac{\partial \pmb{H}}{\partial t}\pmb{u} + \frac{\partial^2 C}{\partial \pmb{q} \partial t}\pmb{H}\pmb{u} + \frac{\partial^2 C}{\partial ^2 t} \end{aligned} $$

而对于非完整约束$C(\pmb{q}, \pmb{u}, t) = 0$ 则是:

$$ \begin{aligned} \pmb{J} &= \frac{\partial C}{\partial \pmb{u}} \\ \pmb{k} &= \frac{\partial C}{\partial \pmb{q}}\pmb{H}\pmb{u} + \frac{\partial C}{\partial t} \end{aligned} $$

拉格朗日乘数法需要计算出额外的约束力$\pmb{F}_c$。可以将力加到运动方程中得到:

$$ (\pmb{F}_{ext} + \pmb{F}_c) = \pmb{M}\pmb{\dot{u}} \Rightarrow \pmb{\dot{u}} = \pmb{M}^{-1}(\pmb{F}_{ext} + \pmb{F}_c) $$

代入(55)式:

$$ \pmb{J}\pmb{M}^{-1}\pmb{F}_c = -\pmb{J}\pmb{M}^{-1}\pmb{F}_{ext} - \pmb{k} $$

根据达郎贝尔原理,理想约束力在任何允许的虚位移上不做功:

$$ \pmb{F}_c \cdot \delta \pmb{r} = 0 $$

而“允许的虚位移”$\delta \pmb{r}$则是:

$$ \begin{aligned} \delta \pmb{r}: & \delta C = 0\\ \Rightarrow &\frac{\partial C}{\partial \pmb{q}}\delta \pmb{q} = \frac{\partial C}{\partial \pmb{q}}\pmb{H} \delta \pmb{u} = \pmb{J}\delta \pmb{u} = 0 \end{aligned} $$

那么也就是说,$\pmb{F}_c$要和$\pmb{J}$的零空间正交。由线性代数,$\pmb{J}$的零空间的正交补是$\pmb{J}^T$,那么有:

$$ \pmb{F}_c = \pmb{J}^T \pmb{\lambda} $$

其中$\pmb{\lambda}$是列向量。

也就是说,约束力总是作用在系统受约束的方向上。

最后得到拉格朗日乘子的线性方程组:

$$ \pmb{J}\pmb{M}^{-1}\pmb{J}^T \pmb{\lambda} = -\pmb{J}\pmb{M}^{-1}\pmb{F}_{ext} - \pmb{k} $$

左边就是$A$矩阵,右边是$b$列向量。

如果不存在冲突或者冗余的约束,$A$就是正定的。

此时可用Baraff在1996年提出的方法去解这个方程。

TODO:

基于冲量的方法
#

类似拉格朗日乘子法。但拉格朗日乘子法是看当前违约的量,然后施加力补救。而基于冲量的方法则是看一步之后将会发生的违约,然后用冲量去修正。

首先介绍“连接件(connector)”,是刚体的局部坐标系下的某个点或者向量。这两个向量用来确定一个约束。

比如球关节(两个刚体连在一起且只能做旋转,没有平移)的约束为:

$$ C(\pmb{P}_1, \pmb{P}_2) = ||\pmb{P}_1 - \pmb{P}_2|| = 0 $$

这里$\pmb{P}_1, \pmb{P}_2$是两个刚体各自坐标系里的固定点,也就是连接件。

这样,约束的一般表示就是:

$$ \begin{aligned} & \pmb{J}_{ij} = \sum_k \frac{\partial C_i}{\partial \pmb{P}_k} \frac{\partial \pmb{P}_k}{\partial q_k} \pmb{H}_j \\ & \pmb{k}_{i} = \frac{\partial C_i}{\partial t} + \sum_k \frac{\partial \pmb{P}_k}{\partial t} \\ \end{aligned} $$

冲量法的思路是:我这一帧直接忽略所有关节约束去模拟刚体(预演),然后尝试将刚体挪回正确的位置上。

最后要在速度积分前加到速度上的改变量是:

$$ \Delta v = \pmb{J}\pmb{M}^{-1}\pmb{J}^T\pmb{p} $$

TODO:not finish

数值解法
#

对于运动学相关,可以使用半隐式欧拉方法。

而对LCP求解,可以使用Lemke算法,Dantzig算法或者PGS。这些在LCP小记一文中已详细阐述。

参考
#

  • David Baraff. SIGGRAPH 94《Fast Contact Force Computation for Nopenetration Rigid Bodies》
  • David E.Stewart 2000 《Rigid-Body Dynamics with Friction and Impact》
  • Kenny Erleben 2012 《Interactive Simulation of Rigid Body Dynamics in Computer Graphics》

  1. David E.Stewart 2000 《Rigid-Body Dynamics with Friction and Impact》 ↩︎

从0开始制作游戏物理引擎 - 这篇文章属于一个选集。
§ 13: 本文