约束数学构建

在物理引擎中，约束 (Constraint)是一个用来限制物体运动的规则。无论是防止一个物体穿透另一个物体（接触约束），还是用一个铰链将两个物体连接在一起（关节约束），其本质都是在描述一个系统不应进入的“禁止状态”。很多状态可以用约束方程来描述，然后通过一个通用的约束求解器 (Constraint Solver)来求解。

1. 约束方程 $C(\mathbf{x})=0$

所有约束的出发点都是一个描述几何关系的方程。我们希望这个方程的值在任何时候都等于（或大于等于）零。

• 等式约束 (Equality Constraint): $C(\mathbf{x})=0$。例如，一个球关节约束要求两个物体上的两个锚点${\mathbf{p}}_A$和${\mathbf{p}}_B$必须重合。其约束方程就是：

$$C(\mathbf{x})={\mathbf{p}}_A-{\mathbf{p}}_B=0$$

• 不等式约束 (Inequality Constraint): $C(\mathbf{x})\ge 0$。例如，一个非穿透约束要求两个物体之间的距离（或穿透深度的相反数）必须大于等于零。 $$C(\mathbf{x})=distance(A,B)\ge 0$$

这里的$\mathbf{x}$代表了系统中所有物体的广义坐标（位置和朝向）。

2. 速度级约束：$\dot{C}(\mathbf{x})=0$

直接在位置层面$C(\mathbf{x})=0$，求解约束非常困难，因为它是一个非线性方程组。一种更有效的方法是求解速度约束。我们对约束方程$C(\mathbf{x})$求时间导数，得到$\dot{C}$，并要求$\dot{C}=0$。这意味着我们要求物体的速度必须满足一个条件，使得约束在下一瞬间仍然被满足。

$$\dot{C}=\frac{\partial C}{\partial \mathbf{x}}\frac{d\mathbf{x}}{dt}=\mathbf{Jv}=0$$

其中，$\mathbf{v}$：系统的广义速度向量，包含了所有物体的线速度 $v_i$ 和角速度 ${\omega }_i$。

$\mathbf{J}=\frac{\partial C}{\partial \mathbf{x}}$，是雅可比矩阵 (Jacobian Matrix)。这是整个约束动力学的核心。它描述了当物体的速度发生变化时，约束方程 $C$ 的值会如何变化。$\mathbf{J}$ 每一行对应一个约束，每一列对应一个物体的某个速度分量。

3. 冲量与雅可比的关系

约束是通过施加力或冲量来满足的。一个约束冲量 $\lambda$ 会导致物体的速度发生变化 $\Delta \mathbf{v}$。

$$\mathbf{M}\Delta \mathbf{v}={\mathbf{J}}^T\lambda$$

其中： $\lambda$ 拉格朗日乘子 (Lagrange Multiplier)，在基于冲量的求解器中，它代表了满足约束所需的冲量大小。$M$ 广义质量矩阵，一个包含了所有物体质量和惯性张量的对角矩阵。${\mathbf{J}}^T$ 雅可比矩阵的转置，它将一个标量冲量 $\lambda$ 分配到各个物体的线动量和角动量上。 从上式可以解出速度变化 $\Delta \mathbf{v}$：

$$\Delta \mathbf{v}=M^{{⁻}^1}{J}^T\lambda$$

4. 约束方程

目标是找到一个冲量 $\lambda$，考虑外力${\mathbf{F}}_{ext}$后，施加该冲量后的新速度

$${\mathbf{v}}_{new}={\mathbf{v}}_{old}+M^{-1}{\mathbf{F}}_{ext}\Delta t+M^{{⁻}^1}{J}^T\lambda$$

能够满足速度级约束 $J\cdot {\mathbf{v}}_{new}=0$（或更通用的$J\cdot {\mathbf{v}}_{new}=b$，其中$b$是偏置向量，用来修正位置）。

$$\begin{array}{rl}\mathbf{J}({\mathbf{v}}_{old}+\Delta t{\mathbf{M}}^{-1}({\mathbf{F}}_{ext}+{\mathbf{J}}^T\lambda ))& =\mathbf{b}\\ \mathbf{J}({\mathbf{v}}_{old}+\Delta t{\mathbf{M}}^{-1}{\mathbf{F}}_{ext})+\Delta t{\mathbf{JM}}^{-1}{\mathbf{J}}^{T}v& =\mathbf{b}\\ {\mathbf{Jv}}_n^{\prime }+{\mathbf{JM}}^{-1}{\mathbf{J}}^T\lambda & =\mathbf{b}\\ {\mathbf{JM}}^{-1}{\mathbf{J}}^T\lambda & ={\mathbf{b}-\mathbf{Jv}}^{\prime }\end{array}$$

定义${\mathbf{v}}^{\prime }$为仅受外力，不考虑约束力后的速度，

$${\mathbf{v}}^{\prime }={\mathbf{v}}_{old}+\Delta t{\mathbf{M}}^{-1}{\mathbf{F}}_{ext}$$

此时，这个方程对应线性方程组

$$\mathbf{K}\lambda =\mathbf{c}$$

这里 $\mathbf{K}=\mathbf{J}{\mathbf{M}}^{-1}{\mathbf{J}}^T$，$\mathbf{c}=\mathbf{b}-\mathbf{J}{\mathbf{v}}^{\prime }$。

这里$\mathbf{K}$，被称为有效质量 (Effective Mass)。它代表了在约束方向上，系统抵抗速度变化的惯性。${\mathbf{v}}^{\prime }$是碰撞前考虑外力后的速度。$b$是目标相对速度。对于简单的接触，$b=0$。为了修正位置误差（穿透），我们可以引入一个 Baumgarte 稳定化项

$$b=-\beta /\Delta t\ast C(x)$$

总结

约束的构建是将物理问题转化为数学问题的关键一步。通过定义一个几何约束方程$C(x)$，并对其求导，我们得到了一个线性的速度级约束 $J\cdot v=0$。雅可比矩阵 J 在此过程中扮演了核心角色，它像一座桥梁，连接了物体的运动速度和约束的变化。最终，我们得到了一个关于未知约束冲量$\lambda$的线性方程 $K\lambda =b-J{v}^{\prime }$ 。这个方程是统一的、普适的，无论是接触、摩擦还是关节，都可以被转化为这个形式。