【Unity学习笔记】第二十 · 物理引擎脉络梳理（数值积分、碰撞检测、约束解决）
转载请注明出处: https://blog.csdn.net/weixin_44013533/article/details/139808452
作者：CSDN@|Ringleader|
物理引擎综述
物理引擎是利用物理规则模拟物体运动和碰撞的模块，以在重力、弹力、摩擦力等各种力作用下做出真实运动表现，并对碰撞、关节等约束做出合理反馈，从而方便开发者实现诸如角色控制、布娃娃系统、布料模拟、场景破坏等更复杂的物理效果。
常见的物理引擎有Box2d、Physx、Chaos、Bullet、Havok 等。游戏引擎通常集成一个或多个物理引擎供开发者选择。
游戏引擎包含两个独立的内存空间：一个是场景世界，用于被渲染系统渲染到屏幕；一个是物理世界，用于被物理系统计算模拟。如下图所示：
在使用物理系统之前，需要对场景中的对象进行配置，常见配置项是：
- rigidbody，表示在物理世界下的物体属性，比如质量、受阻力大小、是否受重力、刚体类型（dynamic/kinematic/trigger等）等；
- collider，表示在物理世界下物体的形状，主要用于碰撞检测。
Unity中rigidbody组件和不同类型的碰撞体
在进行物理模拟前，会将场景世界数据同步到物理世界；模拟结束后再将物理世界数据同步到场景世界，最后由渲染器渲染呈现。
scene和物理间的数据同步
不同物理引擎物理模拟流程存在差异，但核心步骤类似，主要包含：
- 积分运算：根据对象所受外力，利用欧拉、RK4等积分器计算其速度和位置；
- 碰撞检测：检测对象之间是否存在碰撞。为了加速检测，将碰撞检测分为broad phase和narrow phase两个阶段，broad phase利用包围体排序或空间管理算法筛选可能存在碰撞的collider pair，然后在narrow phase利用标准collider结构或者使用GJK+EPA通用算法确定碰撞并生成碰撞信息（碰撞点、穿透深度、碰撞法线等数据）用于后面的约束求解；
- 约束解决：对于存在碰撞约束和关节约束的对象，使用基于力、基于冲量或者基于位置的方法求解约束，得到新的速度和位置并更新。
物理引擎流程图参考
下面将详细介绍整个物理引擎。
数值积分
首先得明确，游戏引擎是以时间步长 h h h或者 Δ t Δt Δt为单位的离散系统，而我们现在的目标是：在一个物理循环开始时，我们在知道当前位置、速度、外力的情况下，如何计算本次循环结束时，对象的位置和速度？
根据牛顿第二定理，可以得到本次迭代新的速度和位置为：
v t + Δ t = v t + ∫ t t + Δ t a d t = v t + ∫ t t + Δ t F m d t v_{t+Δt} = v_{t} + \int_{t}^{t+Δt}adt=v_{t} + \int_{t}^{t+Δt}\frac{F}{m}dt vt+Δt=vt+∫tt+Δtadt=vt+∫tt+ΔtmFdt
x t + Δ t = x t + ∫ t t + Δ t v d t x_{t+Δt} = x_{t} + \int_{t}^{t+Δt}vdt xt+Δt=xt+∫tt+Δtvdt
Δv与阴影面积有关，Δx也类似
如上图和公式可以看到，因为系统的离散性，造成了确定对象新状态的复杂性，为了简化模拟，我们使用数值积分来取近似值。
我们将 v v v和 x x x统一用状态变量 s s s来表示，其导函数 s ˙ \dot{s} s˙用 f ( t ) f(t) f(t)表示。
根据泰勒级数展开，
s ( t + h ) = s ( t ) + s ˙ ( t ) h + 1 2 s ¨ ( t ) h 2 + ⋅ ⋅ ⋅ s(t+h)=s(t)+\dot{s}(t)h+\frac{1}{2}\ddot{s}(t)h^2+··· s(t+h)=s(t)+s˙(t)h+21s¨(t)h2+⋅⋅⋅
取其前两项，便是欧拉积分 s ( t + h ) = s ( t ) + s ˙ ( t ) h s(t+h)=s(t)+\dot{s}(t)h s(t+h)=s(t)+s˙(t)h。
比较一开始的的解析积分形式，可以看到欧拉积分将状态导函数看成常数项，即认为在时间步长中求新速度看成外力保持不变、求新位置看成速度保持不变。
但导函数保持恒定又分为向前和向后一致，如下图所示：
数值积分近似的向前与向后近似
由此产生显式欧拉和隐式欧拉。
-
显式欧拉
v ( t + h ) = v ( t ) + a ( t ) h v(t+h)=v(t)+a(t)h v(t+h)=v(t)+a(t)h
x ( t + h ) = x ( t ) + v ( t ) h x(t+h)=x(t)+v(t)h x(t+h)=x(t)+v(t)h
-
隐式欧拉
v ( t + h ) = v ( t ) + a ( t + h ) h v(t+h)=v(t)+a(t+h)h v(t+h)=v(t)+a(t+h)h
x ( t + h ) = x ( t ) + v ( t + h ) h x(t+h)=x(t)+v(t+h)h x(t+h)=x(t)+v(t+h)h
但显式欧拉不稳定，隐式欧拉需要迭代求解，所以常用半隐式欧拉法
-
半隐式欧拉
v ( t + h ) = v ( t ) + a ( t ) h v(t+h)=v(t)+a(t)h v(t+h)=v(t)+a(t)h
x ( t + h ) = x ( t ) + v ( t + h ) h x(t+h)=x(t)+v(t+h)h x(t+h)=x(t)+v(t+h)h
基于此就可以利用半隐式欧拉公式根据外力更新刚体速度和位置了。
此外关于数值积分稳定性与精度相关内容参考其他文献/书籍，比如《基于物理的建模与动画》第7章就详细介其他更高精度比如RK4等积分器。
碰撞检测
综述中我们了解到，碰撞检测分为broad phase和narrow phase两个阶段，broad phase利用SAP或空间管理算法筛选可能存在碰撞的collider pair，然后在narrow phase使用GJK+EPA通用算法确定碰撞并生成碰撞信息。
下面我们将依次介绍。
Broad Phase
包围体 Bounding Volume
包围体可视为一类简单的几何体，用于包围一个或多个具有复杂几何形状的对象。其中，较为常见的包围体是球体与盒体。包围体常用于前期相交剔除测试，以避免对原几何体进行成本高昂的直接测试。
- AABB(axis-aligned bounding boxes) 轴对齐包围盒
- OBB(oriented bounding boxes) 有向包围盒
- Circle/Sphere.
AABB包围盒可以用左下角和右上角顶点坐标唯一确定，如(Bx,By) (Ex,Ey)。（B为begin代表最小值，E为end代表最大值）
那么判断AABB包围盒相交，就两两比较顶点就行。但为了加速判断，使用了一种称作SAP的算法。
SAP（sweep-and-prune）
SAP本质上就是对包围盒顶点在各个轴进行投影，并对其坐标值进行排序，如下图所示。
比如在x轴向，排序结果是 （B2 （B1 E2) E1)，匹配（B2和E2) ，中间包含对象1，说明在x轴上投影2和1是相交的。然后剔除B2和E2（所谓prune），接着做相同匹配，直到全部坐标剔除。
同样思路投影在Y轴上。当在所有轴两者都相交，那么就将其保存在collider pair中，等待narrow phase精确检测。
这就是SAP核心思想。
当场景存在大量运动对象时，会频繁更新排序表，导致性能下降，这时可以使用改进的MBP（multi box pruning）算法，MBP算法主要在空间上进行分块以后在进行类似SAP算法计算，主要是优化动态元素的计算，分块以后动态元素的移动就只需要更新快内的数据，不需要全局更新，重而提高算法效率。
physx就是使用SAP和MBP作为它的broad phase算法。
其他引擎也使用诸如BVH层次包围体、网格划分、四叉树、BSP二叉树划分、八叉树OCtree等等不一而足。详细可参考《实时碰撞检测算法技术 （Real-Time Collision Detection)》或者其他博客，比如引擎架构剖析——物理引擎全解析（六）。
Narrow phase
到了碰撞检测的narrow phase，就需要确定可能碰撞的collider pair是否真正相交，以及获得碰撞点详细信息。
对于标准的碰撞体，比如球形、方形、胶囊型，我们可以针对性设计公式进行求解，对于复杂对象我们也可以分割成复合碰撞体进行判断，但面对精度要求更高的不规则Mesh collider就必须使用更通用的算法，那就是GJK+EPA，使用通用算法也省得两两判断各种形状的碰撞体了。
在介绍这个算法之前，需要介绍一些前置知识。
闵可夫斯基和
令A和B为两个点集，且令 a \mathbf{a} a 和 b \mathbf{b} b为两个位置向量，对应A和B中的顶点。
-
Minkowski和 A ⊕ B A \oplus B A⊕B定义为 ：
A ⊕ B = { a + b : a ∈ A , b ∈ B } A\oplus B = \{\mathbf{a} +\mathbf{b} :\mathbf{a} ∈A,\mathbf{b} ∈B \} A⊕B={a+b:a∈A,b∈B}
-
Minkowski差 A ⊖ B A \ominus B A⊖B定义为 ：
A ⊖ B = { a − b : a ∈ A , b ∈ B } A\ominus B =\{ \mathbf{a} -\mathbf{b} :\mathbf{a} ∈A,\mathbf{b} ∈B \} A⊖B={a−b:a∈A,b∈B}
Minkowski和示意gif（闵氏差的话就先将B绕原点旋转180°再做类似和）
一个重要的原理：两个多面体 A A A和 B B B的间距等价于其Minkowski差 C C C（ C = A ⊖ B C=A\ominus B C=A⊖B）与原点之间的距离。如果两凸包相交，意味着Minkowski差包含原点。
所以碰撞检测问题就变成两碰撞体顶点集的Minkowski差距离原点最近点问题。
支撑函数
支撑函数 S A ( d ) S_A(d) SA(d)返回形状A边界上在向量 d \mathbf{d} d上投影最高的点。从数学上讲，它与 d \mathbf{d} d的点积最高。这被称为支撑点，此操作也成为支撑映射。
支撑函数将方向和形状作为输入，并返回支撑点作为输出。
定义支撑函数：
-
Support
(
d
⃗
,
A
)
=
P
∈
A
,
(
P
⋅
d
⃗
)
≥
(
Q
⋅
d
⃗
)
,
∀
Q
∈
A
\text{Support}(\vec{d},A)=P ∈ A, (P·\vec{d}) ≥ (Q·\vec{d}), \forall Q∈ A
Support(d
,A)=P∈A,(P⋅d
)≥(Q⋅d
),∀Q∈A
支撑函数有以下性质：
-
Support ( d ⃗ , − B ) = − Support ( − d ⃗ , B ) \text{Support}(\vec{d}, -B) = -\text{Support}(-\vec{d}, B) Support(d ,−B)=−Support(−d ,B)
两个形状的 Minkowski 和的支持函数可以表示为各个形状的支持函数之和：
-
Support ( d ⃗ , A ⊕ B ) = Support ( d ⃗ , A ) + Support ( d ⃗ , B ) \text{Support}(\vec{d}, A \oplus B) = \text{Support}(\vec{d}, A) + \text{Support}(\vec{d}, B) Support(d ,A⊕B)=Support(d ,A)+Support(d ,B)
因此，两种形状顶点集闵氏差的支持函数为：
-
Support
(
d
⃗
,
A
⊖
B
)
=
Support
(
d
⃗
,
A
)
−
Support
(
−
d
⃗
,
B
)
\text{Support}(\vec{d}, A \ominus B) = \text{Support}(\vec{d}, A) - \text{Support}(-\vec{d}, B)
Support(d
,A⊖B)=Support(d
,A)−Support(−d
,B)
简记为：
-
s
A
⊖
B
(
d
⃗
)
=
s
A
(
d
⃗
)
−
s
B
(
−
d
⃗
)
s_{A \ominus B}(\vec{d}) = s_{A}(\vec{d}) - s_{B}(-\vec{d})
sA⊖B(d
)=sA(d
)−sB(−d
)
计算两个形状的Minkowski差通常比较复杂，但利用支撑映射及其性质却能很简单地获得Minkowski差的支撑点。
Minkowski差的支撑点将在GJK算法中起到重要作用。
单纯形
n 维单纯形是 n 维中可以紧密平铺以填充空间的最小几何体；
例如，三角形是 2D 单纯形，四面体是 3D 单纯形。
那么在两个碰撞体顶点集的Minkowski差中，我们可以找到包含原点的点（0D 单纯形）、线（1D 单纯形）、三角形（2D 单纯形）或四面体（3D 单纯形）。
那么就将找闵氏差包含原点的问题，转换成闵氏差的单纯形子集包含原点问题。
GJK
GJK的主要思想就是利用碰撞体顶点集的Minkowski差的支撑点组成单纯形，判断这个单纯形是否包含原点。
我们将 “碰撞体顶点集的Minkowski差” 简记为CSO（配置空间对象）
以下是 GJK 的完整内容：
- 将单纯形初始化为空集（技术上是 -1D 单纯形）。
- 使用初始方向来找到 CSO 的支持点。
- 将该支撑点添加到单纯形（现在单纯形有一个顶点）。
- 找到单纯形中距离原点最近的点。
- 如果最近的点是原点，则 CSO 包含原点，并且两个物体发生碰撞。结束 GJK。
- 否则，通过丢弃顶点将单纯形降低到仍然包含最近点的尽可能最低的维度。
- 使用从最近点到原点的方向寻找新的支撑点。
- 如果新的支撑点在搜索方向上的位置不比最近点更远，则两个物体不会发生碰撞。结束 GJK。
- 将支撑点作为新顶点添加到单纯形中。转到4。
用图形进行理解。
EPA 膨胀多边形算法
GJK 仅告诉我们两个形状是否发生碰撞。下一步则是利用EPA(Epanding Polytop Algorithm) 算法生成约束解决阶段所需的接触信息。
2D EPA 可视化流程：
约束求解
通过上个环节我们得到碰撞点、碰撞法线、穿透深度等碰撞信息，我们就可以着手解决碰撞了。
解决思路有两类，一个是基于误差的惩罚力方法，另一种就是基于约束的雅可比矩阵法。
惩罚力法根据误差大小（比如穿透深度）施加一定比例的惩罚力，使误差减小；为了预测误差变化，可以给惩罚力添加微分项，同时为了消除累计误差，还可以添加积分项。这有点像经典控制系统里的PID控制。详细可参考《基于物理的建模与动画 第11章》。
惩罚力法解决约束
但由于惩罚力法是根据误差进行反馈，所以不适合关节等刚性约束。但其实现简单，广泛使用在Maya、3Dmax等软体系统中。《Physics-based animation Erleben 2005》
现在游戏引擎常用的还是基于约束的、使用雅可比矩阵+拉格朗日乘数法求解约束的方式。
基于约束又分为基于约束力的、基于冲量的和基于位置的。
下面将依次介绍，首先补充下基础知识点。
基础知识
约束
-
自由度（Degrees of Freedom）：刚体的自由度，定义为它具有的独立运动的数量。2D：3个自由度 （2平移自由度 | 1旋转自由度）3D：6个自由度 （3平移自由度 | 3 旋转自由度）
-
广义坐标：用来描述系统位形所需要的独立参数，或者最少参数。
- 如下图单摆中重物m，可以用
(
x
,
y
)
(x,y)
(x,y)坐标描述对象位置，但
x
x
x和
y
y
y显然不是独立的，满足
x
2
+
y
2
=
l
2
x^2+y^2=l^2
x2+y2=l2。使用广义坐标
θ
\theta
θ 就能简单准确描述其位置，这个
θ
\theta
θ就是广义坐标。
- 一个重要特性：刚体的自由度=广义坐标数。
-
约束（Constraint）：在刚体中用来限制刚体运动自由度的被称作约束。限制位置的称作位置约束，限制速度的称作速度约束，速度约束也可以从位置约束求导得到。约束方程通常用符号 C C C表示。约束又分等式约束和不等式约束，比如上面单摆就是等式约束，地面碰撞就是不等式约束 。如果一个系统中约束力不做功，那么称之为理想约束。限制自由度的力就是约束力，如绳子连杆的拉力、地面支持力、接触的摩擦力等。
状态向量、质量、外力、约束的矩阵表示
-
状态向量
假设系统有 n n n个刚体，令 q \mathbf{q} q 为具有所有刚体位置和角度的状态向量：
q = [ p 1 , α 1 , p 2 , α 2 , . . . , p n , α n ] T = [ q 1 , q 2 , . . . , q n ] T \mathbf{q}=[\mathbf{p_{1}},\alpha_{1},\mathbf{p_{2}},\alpha_{2},...,\mathbf{p_{n}},\alpha_{n}]^T=[q_{1},q_{2},...,q_{n}]^T q=[p1,α1,p2,α2,...,pn,αn]T=[q1,q2,...,qn]T
其中 p i \mathbf{p_{i}} pi是二维向量（3d物理就是三维向量），表示第 i i i个刚体的位置， α i \alpha_{i} αi是其角度，为标量。
因此，2d物理 q \mathbf{q} q有3 n 个元素（3d物理 q \mathbf{q} q有4 n 个元素）。
-
质量矩阵
定义质量矩阵 M M M 为以下 3 n × 3 n 3 n × 3 n 3n×3n 对角矩阵：
M = [ m 1 m 1 I 1 m 2 m 2 I 2 ⋱ m n m n I n ] {\tiny M=\begin{bmatrix} &m_1 & & & & & & & & &\\ & &m_1 & & & & & & & &\\ & & &I_1 & & & & & & &\\ & & & &m_2 & & & & & &\\ & & & & &m_2 & & & & &\\ & & & & & &I_2 & & & &\\ & & & & & & &\ddots & & &\\ & & & & & & & &m_n & &\\ & & & & & & & & &m_n &\\ & & & & & & & & & &I_n\\ \end{bmatrix}} M= m1m1I1m2m2I2⋱mnmnIn
其中， m i m_i mi是第 i i i个刚体的质量， I i I_i Ii是其转动惯量。
-
力矩阵
令 F F F为一个全局力矢量，包含作用在每个物体上的力和力矩。它是外力和约束力的总和：
F = F e x t + F C = [ f 1 , τ 1 , f 2 , τ 2 , . . . , f n , τ n ] T F=F_{ext}+F_C=[\mathbf{f_1},\tau_1,\mathbf{f_2},\tau_2,...,\mathbf{f_n},\tau_n]^T F=Fext+FC=[f1,τ1,f2,τ2,...,fn,τn]T
其中， F F F 也有 3 n 3n 3n个元素，因为每个 f i \mathbf{f_i} fi都是二维向量。
那么牛顿第二运动定律可用如下表达式表达：
q ¨ = M − 1 F = M − 1 ( F e x t + F C ) \ddot{q}=M^{-1}F=M^{-1}(F_{ext}+F_C) q¨=M−1F=M−1(Fext+FC)
-
行为函数
最后，让我们设置行为函数。假设有m 个约束，每个约束代表刚体链中的一个环节。我们将把所有行为函数分组为单个函数 C ( q ) C(\mathbf{q}) C(q)：
C ( q ) = [ C 1 ( q ) , C 2 ( q ) , . . . , C n ( q ) ] T = [ C 1 ( q ) C 2 ( q ) ⋮ C n ( q ) ] C(\mathbf{q})=[C_1(\mathbf{q}),C_2(\mathbf{q}),...,C_n(\mathbf{q})]^T=\begin{bmatrix} C_1(\mathbf{q}) \\ C_2(\mathbf{q}) \\ \vdots \\ C_n(\mathbf{q}) \end{bmatrix} C(q)=[C1(q),C2(q),...,Cn(q)]T= C1(q)C2(q)⋮Cn(q)
雅可比矩阵
如果我们希望 C = 0 C=0 C=0，并且在整个模拟过程中保持不变，这意味着一阶导数 C ˙ \dot{C} C˙ 也必须为零。
同样，为了使 C ˙ = 0 \dot{C}=0 C˙=0 ，也必须满足 C ¨ = 0 \ddot{C}=0 C¨=0 。
那么，根据链式法则：
若 y = f ( u ) = f ( g ( x ) ) ，那么 d y d x = d y d u ⋅ d u d x = f ′ ( g ( x ) ) g ′ ( x ) 若y=f(u)=f(g(x))，那么 \frac{dy}{dx}=\frac{dy}{du}\cdot \frac{du}{dx} =f'(g(x))g'(x) 若y=f(u)=f(g(x))，那么dxdy=dudy⋅dxdu=f′(g(x))g′(x)
可知， C C C关于时间的导数可表示为：
C ˙ = ∂ C ∂ q q ˙ = J q ˙ = J V \dot{C}=\frac{\partial C}{\partial \mathbf{q}} \dot{\mathbf{q}} = \mathbf{J} \dot{\mathbf{q}} =\mathbf{J}V C˙=∂q∂Cq˙=Jq˙=JV
J \mathbf{J} J 被称作 C C C的雅可比矩阵。雅可比矩阵是梯度的推广，而梯度本身又是斜率的推广。 J \mathbf{J} J 的每一行都是每个行为函数的梯度。雅可比矩阵告诉我们每个行为函数如何对每个状态变量的变化做出反应。
定义：
J = [ ∂ C 1 ∂ q 1 ∂ C 1 ∂ q 2 ⋯ ∂ C 1 ∂ q 3 n ∂ C 2 ∂ q 1 ∂ C 2 ∂ q 2 ⋯ ∂ C 2 ∂ q 3 n ⋮ ⋮ ⋱ ⋮ ∂ C m ∂ q 1 ∂ C m ∂ q 2 ⋯ ∂ C m ∂ q 3 n ] {\small \mathbf{J}=\begin{bmatrix} \frac{\partial C_1}{\partial q_1} &\frac{\partial C_1}{\partial q_2} &\cdots &\frac{\partial C_1}{\partial q_{3n}} \\ \frac{\partial C_2}{\partial q_1} &\frac{\partial C_2}{\partial q_2} &\cdots &\frac{\partial C_2}{\partial q_{3n}} \\ \vdots &\vdots &\ddots &\vdots \\ \frac{\partial C_m}{\partial q_1} &\frac{\partial C_m}{\partial q_2} &\cdots &\frac{\partial C_m}{\partial q_{3n}} \\ \end{bmatrix}} J= ∂q1∂C1∂q1∂C2⋮∂q1∂Cm∂q2∂C1∂q2∂C2⋮∂q2∂Cm⋯⋯⋱⋯∂q3n∂C1∂q3n∂C2⋮∂q3n∂Cm
其中， J i j \mathbf{J}_{ij} Jij表示第 i i i 个约束对第 j j j 个广义坐标的微分；n为约束个数，m为广义坐标的个数。
这里有个很重的细节， J \mathbf{J} J若非满秩，也就是约束之间存在线性关系，或者说其中一个约束能从其他约束推导出来，那么 J \mathbf{J} J就无法求其逆矩阵，关于它的线性方程组 J M − 1 J T λ = b \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T\lambda=b JM−1JTλ=b就不能通过求逆得到 λ \lambda λ了。但如果只有一个约束，那么 J \mathbf{J} J为 1 ∗ 3 n 1*3n 1∗3n的矩阵， M − 1 \mathbf{M}^{-1} M−1为 3 n × 3 n 3 n × 3 n 3n×3n 对角矩阵， J T \mathbf{J}^T JT为 3 n ∗ 1 3n*1 3n∗1的矩阵，那么最终有效质量 J 1 , 3 n M 1 , 3 n − 1 J 1 , 3 n T \mathbf{J}_{1,3n}\mathbf{M}^{-1}_{1,3n}\mathbf{J}^T_{1,3n} J1,3nM1,3n−1J1,3nT就是个常数，常数可以取倒数，这也是后面顺序冲量法（SI）和投影高斯赛德尔法（PGS）能直接求拉格朗日乘数 λ \lambda λ或 λ i \lambda_i λi的原因。
拉格朗日乘子
根据
C ˙ = J q ˙ = 0 \dot{C}= \mathbf{J} \dot{\mathbf{q}} =0 C˙=Jq˙=0
以及理想约束下约束力不做功，约束力与速度正交，即:
F c T q ˙ = 0 F_c^T\dot{q}=0 FcTq˙=0
可知， F c F_c Fc与 J \mathbf{J} J 平行，因此我们可以将约束力 F C F_C FC写成 J \mathbf{J} J 的倍数形式：
F c = J T λ F_c= \mathbf{J}^T \mathbf{λ} Fc=JTλ
其中，向量 λ \mathbf{λ} λ有 m m m个标量分量，在这个矩阵向量乘法中，每个分量 λ i \mathbf{λ}_i λi与 J \mathbf{J} J的一行（即第 i i i 个约束函数的梯度）相乘，然后将它们相加。即:
J T λ = ▽ C 1 λ 1 + ▽ C 2 λ 2 + ⋯ + ▽ C m λ m \mathbf{J}^T \mathbf{λ}=\triangledown C_1 \lambda_1 + \triangledown C_2 \lambda_2 + \dots +\triangledown C_m \lambda_m JTλ=▽C1λ1+▽C2λ2+⋯+▽Cmλm
λ \mathbf{λ} λ 就是拉格朗日乘子。
基于约束的求解
经过上面的矩阵推导，我们得到
{ C = 0 C ˙ = J q ˙ = 0 C ¨ = J ˙ q ˙ + J q ¨ F c = J T λ （ 1 ） \begin{cases} C=0 \\ \dot{C}= \mathbf{J} \dot{\mathbf{q}} =0 \\ \ddot{C}=\dot{\mathbf{J}}\dot{\mathbf{q}}+J\ddot{\mathbf{q}} \\ F_c= \mathbf{J}^T \mathbf{λ} \end{cases} （1） ⎩ ⎨ ⎧C=0C˙=Jq˙=0C¨=J˙q˙+Jq¨Fc=JTλ（1）
那么就可以尝试求解约束了。约束求解分为基于力、基于冲量和基于位置三种方式，分别对应使用 C ¨ \ddot{C} C¨、 C ˙ \dot{C} C˙和 C C C，通过求解拉格朗日乘数求解约束。
下面依次介绍。
基于力的求解
由上面（1）式可知：
C ¨ = J ˙ q ˙ + J q ¨ = J ˙ q ˙ + J M − 1 ( F e x t + F c ) = J ˙ q ˙ + J M − 1 ( F e x t + J T λ ) = 0 \ddot{C}=\dot{J}\dot{q}+J\ddot{q} = \dot{J}\dot{q}+JM^{-1}(F_{ext}+F_c) = \dot{J}\dot{q}+JM^{-1}(F_{ext}+J^Tλ) =0 C¨=J˙q˙+Jq¨=J˙q˙+JM−1(Fext+Fc)=J˙q˙+JM−1(Fext+JTλ)=0
可以得到：
J M − 1 J T λ = − J ˙ q ˙ − J M − 1 F e x t （ 2 ） \mathbf{J}M^{-1}\mathbf{J}^T\mathbf{λ}=- \dot{\mathbf{J}}\dot{q}-\mathbf{J}M^{-1}F_{ext} （2） JM−1JTλ=−J˙q˙−JM−1Fext（2）
其中 J ˙ = ∂ C ∂ q \dot{\mathbf{J}}=\frac{\partial \mathbf{C}}{\partial \mathbf{q}} J˙=∂q∂C
式（2）中只有λ是未知的，由此解这个线性方程组就能得到约束力 F c F_c Fc。那么对应速度和位置通过积分就能获得。
这就是基于约束力法解约束的思路。
基于冲量的求解
类似的，用额外冲量代替约束力，根据冲量定义：
I = M Δ V = ∫ t t + Δ t F d t I=MΔV=\int_{t}^{t+Δt} Fdt I=MΔV=∫tt+ΔtFdt
使用半隐式欧拉法的话，力认为是恒定的，那么
M Δ V = F Δ t = J T λ Δ t MΔV=FΔt=J^TλΔt MΔV=FΔt=JTλΔt
Δ V = M − 1 J T λ Δ t ΔV=M^{-1}J^TλΔt ΔV=M−1JTλΔt
那么
C ˙ = J q ˙ = J V = J ( V i n i t + Δ V ) = J ( V i n i t + M − 1 J T λ Δ t ) = 0 \dot{C}=J\dot{q}=JV=J(V_{init}+ΔV)=J(V_{init}+M^{-1}J^TλΔt)=0 C˙=Jq˙=JV=J(Vinit+ΔV)=J(Vinit+M−1JTλΔt)=0
得到：
J M − 1 J T λ Δ t = − J V i n i t （ 3 ） \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T\mathbf{λ}Δt=-\mathbf{J}V_{init} （3） JM−1JTλΔt=−JVinit（3）
解式（3）这个线性方程组就能得到λ和Δv了，对应Δx也能得到。
如果只是通过上面解得出Δv，仅仅保证速度不会违反约束，而要保证位置同样不会违反约束，可以在速度约束加上Baumgarte 项，即 J V + b = 0 JV+b=0 JV+b=0，其中 b = − β e Δ t ( 0
-
- 如下图单摆中重物m，可以用
(
x
,
y
)
(x,y)
(x,y)坐标描述对象位置，但
x
x
x和
y
y
y显然不是独立的，满足
x
2
+
y
2
=
l
2
x^2+y^2=l^2
x2+y2=l2。使用广义坐标
θ
\theta
θ 就能简单准确描述其位置，这个
θ
\theta
θ就是广义坐标。
-
s
A
⊖
B
(
d
⃗
)
=
s
A
(
d
⃗
)
−
s
B
(
−
d
⃗
)
s_{A \ominus B}(\vec{d}) = s_{A}(\vec{d}) - s_{B}(-\vec{d})
sA⊖B(d
)=sA(d
)−sB(−d
)
-
Support
(
d
⃗
,
A
⊖
B
)
=
Support
(
d
⃗
,
A
)
−
Support
(
−
d
⃗
,
B
)
\text{Support}(\vec{d}, A \ominus B) = \text{Support}(\vec{d}, A) - \text{Support}(-\vec{d}, B)
Support(d
,A⊖B)=Support(d
,A)−Support(−d
,B)
-
-
Support
(
d
⃗
,
A
)
=
P
∈
A
,
(
P
⋅
d
⃗
)
≥
(
Q
⋅
d
⃗
)
,
∀
Q
∈
A
\text{Support}(\vec{d},A)=P ∈ A, (P·\vec{d}) ≥ (Q·\vec{d}), \forall Q∈ A
Support(d
,A)=P∈A,(P⋅d
)≥(Q⋅d
),∀Q∈A
-
-
-
