广义坐标
在介绍约束之前先介绍一下分析力学的一个概念：广义坐标
广义坐标是一组用来描述一个物理系统位形（即系统中所有质点的位置状态）的独立变量。它们不必是传统笛卡尔坐标 $(x, y, z)$，而是可以根据系统的约束和对称性灵活选择，只要能够唯一确定系统状态即可。
广义坐标通常用 $q_1,q_2,...q_n$ 来表示
例如：
双摆：广义坐标：$(\theta_1, \theta_2)$。总共有两个自由度，即两个角度，系统的所有质点位置都能通过这两个角度确定。
对广义坐标对时间求导得到的就是广义速度。
位置约束方程
位置约束方程是指对系统中物体或点的空间位置施加的限制条件的方程。它定义了系统在构型空间中必须满足的几何关系。
在数学上，一个位置约束是一个标量函数$C(\mathbf{q})$，它依赖于系统的广义坐标 $\mathbf{q}$，并要求方程满足：
$$ C(\mathbf{q}) = 0 $$
如果约束函数包含了多个约束方程，那么此时约束函数是一个向量值函数
这意味着系统必须处于某个特定的几何或物理配置中
例如：
- 如果 $q$ 表示二维平面上的一个点$(x,y)$，且这个点满足$C(\mathbf{q})=C(x,y)=x^2+y^2-r^2=0$，则这个位置约束方程表示的就是将这个点约束在以$(x,y)$为圆心 $r$ 为半径的圆上。
- 如果 $q$ 表示三维空间中的两个点$\mathbf{P}_A$和$\mathbf{P}_B$，且满足$C(\mathbf{q})=C(\mathbf{P}_A,\mathbf{P}_B)=(\mathbf{P}_A-\mathbf{P}_B)^2-d^2=0$，则这个位置约束方程描述了两点间距离固定为 $d$ 的刚性杆。
虽然位置约束定义了我们想要的状态，但在模拟中，我们通常不直接求解位置约束方程，有以下原因
- 位置约束方程通常不是线性函数求解起来非常困难，我们需要一种能够解决所有问题的方法，
- 数值积分后通常不满足位置约束方程，在约束求解后直接改变位置会导致严重的抖动问题
- 不适配我们的冲量方法
所以为了解决这些麻烦，我们通常使用速度约束方程。
速度约束方程
速度约束方程是通过对位置约束对时间求导得到的，它描述了系统在满足位置约束的前提下，其广义速度 $\dot {\mathbf{q}}$ 必须满足的条件。
$$ \dot C = \frac{d}{d t}C(\mathbf{q}) $$
因为广义坐标是关于时间 $t$ 的向量值函数即
$$ \mathbf{q}(t) =(q_1(t),q_2(t),...,q_n(t)) $$
很显然位置约束方程是个复合函数我们用链式法则展开
$$ \dot C = \nabla C \cdot \dot{ \mathbf{q}} $$
当约束函数是向量值函数时(即包含了多个约束时)
$$ \dot C = \mathbf{J} C \cdot \dot{ \mathbf{q}} $$
此时我们就得到了速度约束方程
$$ \dot C = \nabla C \cdot \dot{ \mathbf{q}}=0 $$
或者
$$ \dot C = \mathbf{J} C \cdot \dot{ \mathbf{q}}=0 $$
接下来将会以二维视角讲解如何进行冲量求解
质量矩阵
在刚体动力学中，当研究某个点的动力学行为（如外力作用点）时，施加一个力不仅会引起平动加速度，还可能引起角加速度（特别是当力的作用点偏离质心时）。为了统一描述这种平动与转动的耦合响应，我们需要一个能够综合反映物体惯性特性的数学结构。
因此，我们引入广义质量矩阵（Generalized Mass Matrix），它将质量 $m$ 和转动惯量 $I$ 统一组织在一个矩阵中，用于连接广义力与广义加速度。
$$ M=\begin{bmatrix} m & & & \\ & m & & \\ & & I & \\ \end{bmatrix} $$
我们可以写成分块矩阵的形式
$$ M=\begin{bmatrix} M_a & \\ & I_a \end{bmatrix} $$
动量矩阵
动量矩阵就可以表示为
$$ P=MV=\begin{bmatrix} m & & & \\ & m & & \\ & & & I\end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \\ \end{bmatrix} $$
或者
$$ P_a=MV=\begin{bmatrix} M_a & & \\ & &I_a \end{bmatrix} \begin{bmatrix} \vec{v_a}\\ \omega_a \\ \end{bmatrix} $$
牛顿第二定律
牛顿第二定理就可以写作
$$ F=MA=\begin{bmatrix} m & & \\ & m & \\ & & I \end{bmatrix}\begin{bmatrix} a_x \\ a_y \\ \alpha \end{bmatrix}=\begin{bmatrix} M_a & \\ & I\end{bmatrix}\begin{bmatrix} \vec{a} \\ \alpha \end{bmatrix} $$
冲量求解
下面我们从数值积分和动力学原理出发，推导出用于求解约束冲量的线性方程组。
我们的目的是更新速度使得满足速度约束方程
使用常微分方程数值方法中学到的前向欧拉法一阶数值积分（其实就是我们高中学的速度-加速度关系的离散形式）
$$ V_t= V_0+A \Delta t $$
其中
- $\Delta t$是物理帧
- $V_t$是更新后的速度
- $V_0$是更新前的速度
我们要得到更新后的速度，我们只需求得A就可以了！
使用牛顿第二定律
$$ A=M^{-1}F $$
代入速度更新公式
$$ V_t=V_0+M^{-1}F\Delta t $$
在分析力学中，约束力可表示为：
$$ F=\mathbf{J}^T\mathbf{\lambda}_f =\mathbf{J}^T\begin{bmatrix} \lambda_x \\ \lambda_y \\ \lambda\omega \end{bmatrix} $$
其中
- $\mathbf{J}$是速度约束的雅可比矩阵
- $\mathbf{\lambda}_f$是拉格朗日乘子向量，表示约束力的大小（在一个约束的情况下是一个标量）
代入有速度更新公式中
$$ V_t=V_0+M^{-1}\mathbf{J}^T\lambda_f\Delta t=V_0+M^{-1}\mathbf{J}^T\lambda_{imp} $$
我们将速度约束方程写成如下形式
$$ \dot C = \mathbf{J}V_t=0 $$
将速度更新方程代入
$$ \mathbf{J}(V_0+M^{-1}\mathbf{J}^T\lambda _{imp})=0 \\ $$
$$ \mathbf{J}M^{-1}\mathbf{J}^T\lambda _{imp}=-\mathbf{J}V_0 $$
这正是我们需要的一次线性方程方程组，满足
$$ A\vec x=\vec b $$
我们只需要求解该其次方程组的解即可得到更新所需的冲量
