[scode type="blue"]如果行内公式没渲染请刷新页面[/scode]

角度约束

在物理引擎中，我们经常需要控制两个物体之间的相对角度，比如机械臂的关节角度或者两个刚体之间的旋转约束。这种约束在现实中就类似用铰链连接两个物体，使它们在某个期望夹角下旋转。

通过之前《物理引擎|基于冲量的约束求解》这篇文章我们知道，实现约束的关键是写出约束方程，然后求出Jacobian矩阵，最后用公式求解冲量。下面我们将详细介绍角度约束的约束方程和Jacobian矩阵的推导。

角度约束的约束方程

假设存在两个物体：物体A和物体B，我们希望它们的旋转轴之间保持一个期望夹角 $\theta_d$。

设物体A的参考方向为单位向量 $\mathbf{u}_A$，物体B的参考方向为单位向量 $\mathbf{u}_B$。我们希望这两个向量之间的夹角等于 $\theta_d$，约束方程可以写为：

三维情况下

$$ C = \mathbf{u}_A \cdot \mathbf{u}_B - \cos\theta_d = 0 $$

这里 $\mathbf{u}_A \cdot \mathbf{u}_B = \cos\theta$ 表示当前夹角，$C=0$ 表示夹角正好为期望值。

以下写法也正确，但通常不推荐

$$ C=arccos(\mathbf{u}_A\cdot\mathbf{u}_B)-\theta_d=0 $$

二维情况下

$$ C=\theta_A-\theta_B-\theta_d=0 $$

$\theta_A,\theta_B$ 分别表示物体的角度

Jacobian矩阵

三维推导

为了得到速度约束方程，我们对位置约束方程对时间求导：

$$ \dot C = \nabla C\cdot\dot{\mathbf{q}} = \begin{bmatrix}\frac{\partial C}{\partial \mathbf{u}_A}&\frac{\partial C}{\partial \mathbf{u}_B}\end{bmatrix}\begin{bmatrix}\dot{\mathbf{u}}_A\\\dot{\mathbf{u}}_B\end{bmatrix} $$

$$ \dot C = \begin{bmatrix}\mathbf{u}_B&{\mathbf{u}}_A\end{bmatrix}\begin{bmatrix}\dot{\mathbf{u}}_A\\\dot{\mathbf{u}}_B\end{bmatrix} $$

旋转向量的变化可以用角速度表示：

$$ \dot{\mathbf{u}}_A = \boldsymbol{\omega}_A \times \mathbf{u}_A, \quad \dot{\mathbf{u}}_B = \boldsymbol{\omega}_B \times \mathbf{u}_B $$

代入速度约束：

$$ \dot C = (\boldsymbol{\omega}_A \times \mathbf{u}_A) \cdot \mathbf{u}_B + \mathbf{u}_A \cdot (\boldsymbol{\omega}_B \times \mathbf{u}_B) $$

利用向量恒等式 $(\mathbf{a} \times \mathbf{b}) \cdot \mathbf{c} = \mathbf{a} \cdot (\mathbf{b} \times \mathbf{c})$，可以得到：

$$ \dot C = \boldsymbol{\omega}_A \cdot (\mathbf{u}_A \times \mathbf{u}_B) + \boldsymbol{\omega}_B \cdot (\mathbf{u}_B \times \mathbf{u}_A) $$

注意 $\mathbf{u}_B \times \mathbf{u}_A = - (\mathbf{u}_A \times \mathbf{u}_B)$，于是速度约束可以写成矩阵形式：

$$ \dot C = \begin{bmatrix} \mathbf{u}_A \times \mathbf{u}_B & -(\mathbf{u}_A \times \mathbf{u}_B) \end{bmatrix} \begin{bmatrix} \boldsymbol{\omega}_A \\ \boldsymbol{\omega}_B \end{bmatrix} = \mathbf{J} \cdot \boldsymbol{\Omega} $$

其中

$$ \mathbf{J} = \begin{bmatrix} \mathbf{u}_A \times \mathbf{u}_B & -(\mathbf{u}_A \times \mathbf{u}_B) \end{bmatrix}, \quad \boldsymbol{\Omega} = \begin{bmatrix} \boldsymbol{\omega}_A \\ \boldsymbol{\omega}_B \end{bmatrix} $$

为了保证程序通用性一般写作

$$ \mathbf{J} = \begin{bmatrix}0& \mathbf{u}_A \times \mathbf{u}_B & 0&-(\mathbf{u}_A \times \mathbf{u}_B) \end{bmatrix}, \quad \mathbf{V} = \begin{bmatrix}\mathbf{v}_A\\ \boldsymbol{\omega}_A \\\mathbf{v}_B\\ \boldsymbol{\omega}_B \end{bmatrix} $$

二维推导

为了得到速度约束方程，我们对位置约束方程对时间求导：

$$ \dot C =\nabla C\cdot\dot {\mathbf{q}}=\begin{bmatrix}\frac{\partial C}{\partial \theta_A}&\frac{\partial C}{\partial \theta_B}\end{bmatrix}\begin{bmatrix}\frac{d}{dt}\theta_A&\frac{d}{dt}\theta_B\end{bmatrix}^T=\begin{bmatrix}1&-1\end{bmatrix}\begin{bmatrix}\omega_A&\omega_B\end{bmatrix}^T $$

为了保证程序通用性一般写作

$$ \dot C =\begin{bmatrix}0&1&0&-1\end{bmatrix}\begin{bmatrix}\mathbf{v}_{A}\\ \omega_A \\ \mathbf{v}_{B} \\ \omega_B\end{bmatrix} $$

其中

$$ \mathbf{J} = \begin{bmatrix} 0 & 1 & 0 & -1 \end{bmatrix}, \quad \mathbf{V} = \begin{bmatrix}\mathbf{v}_{A}\\ \omega_A \\ \mathbf{v}_{B} \\ \omega_B\end{bmatrix} $$

[scode type="blue"]如果行内公式没渲染请刷新页面[/scode]

距离约束

在物理引擎中我们常常需要限制两个物体(或者点)之间的相对相对距离，这种效果在现实中就类似拿一根固定长度的刚体丝线连接两个物体上的某个点，这就不得不再次用到约束了。

通过之前《物理引擎中的约束》这篇文章我们了解到我们只需要写出我们需要的约束方程，然后找出Jacobian矩阵就可以通过之前那篇文章最后导出的公式求解冲量了。接下来我们将介绍解释碰撞约束的约束方程和推导Jacobian矩阵。

距离约束的约束方程

假设存在两个物体：物体A，物体B。我们要将 物体A 上的点$\mathbf{P}_A$连接到 物体B 上的点$\mathbf{P}_B$上。

我们的目的是使得这两个点之间的距离（在欧几里得度量下）在某个期望值 $d$下保持不变。为了实现这一目标，那么我们的位置约束方程可以这么写：

$$ C=(\mathbf{P}_A-\mathbf{P}_B)\cdot(\mathbf{P}_A-\mathbf{P}_B) -d^2=0 $$

表示的是两个点之间的距离与期望距离相等

事实上还可以这么写

$$ C=\sqrt{(\mathbf{P}_A-\mathbf{P}_B)\cdot(\mathbf{P}_A-\mathbf{P}_B) -d^2}=0 $$

$$ C=\sqrt{(\mathbf{P}_A-\mathbf{P}_B)\cdot(\mathbf{P}_A-\mathbf{P}_B)}-d = 0 $$

$$ C=\left \| \mathbf{P}_A-\mathbf{P}_B \right \|-d=0 $$

上述表示方式都是正确的，选择哪一种取决于具体的应用场景和个人偏好。第一种方式避免了开方运算，在某些数值计算中更为高效；而下面三种种方式则更加直观，直接表达了距离的概念。接下来采用第一种进行推导。

Jacobian矩阵

还记得我们《物理引擎中的约束》中提到的我们找到位置约束方程后，我们还需要对时间求导，得到速度约束方程。

在这里由于我们只有一个系统所以我们的约束方程退化成了多元函数，Jacobian 矩阵退化成了梯度。

$$ \dot C =\frac{\partial C}{\partial t}=\nabla C\cdot \begin{bmatrix} \dot {\mathbf{P} }_A \\ \dot {\mathbf{P} }_B \end{bmatrix} $$

其中

$$ \nabla C = (\frac{\partial C}{\partial \mathbf{P}_A},\frac{\partial C}{\partial \mathbf{P}_B})=(2(\mathbf{P}_A-\mathbf{P}_B),-2(\mathbf{P}_A-\mathbf{P}_B)) $$

我们需要将其展开，我们知道

$$ \mathbf{P}_A = \mathbf{c}_A + \mathbf{r}_A, \quad \mathbf{P}_B = \mathbf{c}_B + \mathbf{r}_B $$

其中：

• $\mathbf{c}_A, \mathbf{c}_B$ 是物体质心位置
• $\mathbf{r}_A, \mathbf{r}_B$ 是点相对于质心的局部偏移

我们知道

$$ \dot{\mathbf{P}}_A = \mathbf{v}_A + \boldsymbol{\omega}_A \times \mathbf{r}_A, \quad \dot{\mathbf{P}}_B = \mathbf{v}_B + \boldsymbol{\omega}_B \times \mathbf{r}_B $$

带入速度约束

$$ \dot{C} = 2 (\mathbf{P}_A - \mathbf{P}_B) \cdot \dot{\mathbf{P}}_A - 2 (\mathbf{P}_A - \mathbf{P}_B) \cdot \dot{\mathbf{P}}_B $$

$$ \dot{C} = 2 (\mathbf{P}_A - \mathbf{P}_B) \cdot \Big( (\mathbf{v}_A + \boldsymbol{\omega}_A \times \mathbf{r}_A) - (\mathbf{v}_B + \boldsymbol{\omega}_B \times \mathbf{r}_B) \Big) $$

$$ \dot C =2 (\mathbf{P}_A - \mathbf{P}_B)\cdot\mathbf{v}_A+2 \boldsymbol{\omega}_A\cdot( \mathbf{r}_A\times (\mathbf{P}_A - \mathbf{P}_B))-2 (\mathbf{P}_A - \mathbf{P}_B)\cdot\mathbf{v}_B-2 \boldsymbol{\omega}_B\cdot( \mathbf{r}_B\times (\mathbf{P}_A - \mathbf{P}_B)) $$

我们将其写成矩阵的形式

$$ \dot{C} = \mathbf{J} \cdot \mathbf{V} = \begin{bmatrix} 2 (\mathbf{P}_A - \mathbf{P}_B) & 2 (\mathbf{r}_A \times(\mathbf{P}_A - \mathbf{P}_B)) & -2 (\mathbf{P}_A - \mathbf{P}_B) & -2 (\mathbf{r}_B \times(\mathbf{P}_A - \mathbf{P}_B)) \end{bmatrix}\begin{bmatrix} \mathbf{v}_A \\ \boldsymbol{\omega}_A \\ \mathbf{v}_B \\ \boldsymbol{\omega}_B \end{bmatrix} $$

在带入我们的《物理引擎中的约束》最后的得到的方程我们可以解出冲量

[scode type="blue"]如果行内公式没渲染请刷新页面[/scode]
贝塞尔曲线

贝塞尔曲线（Bézier Curve）是一种在计算机图形学，图形设计软件（如 Adobe Illustrator）、动画制作、字体设计等领域中广泛使用的参数曲线，常用于设计平滑的曲线路径。

本文将介绍贝塞尔曲线的基本原理，公式推导。

贝塞尔曲线的基本原理

贝塞尔曲线由一组控制点定义。最常见的是一次、二次和三次贝塞尔曲线。

所以我们只介绍一次、二次和三次贝塞尔曲线。

贝塞尔曲线是通过一组有序控制点定义的参数函数，该函数将参数 t 映射到曲线上的空间点。

一次贝塞尔曲线

在平面中两个点可以确定一条线段，那么我们怎么表示线段上的任意一个点呢？

假设平面中存在任意两个点$\mathbf{P_1},\mathbf{P_2}$（注：这里的点指的是向量）

那么线段$\mathbf{P_2-P_1}$上的任意一个点$\mathbf{P}$的模长应该满足

$$ ||\mathbf{P_1}||\le||\mathbf{P}||\le||\mathbf{P_2}|| $$

且方向与$\mathbf{P_1}$的方向一致

那么$\mathbf{P}$可以由$\mathbf{P_1}$,$\mathbf{P_2-P_1}$和一个参数 $t$ 来决定

即

$$ \mathbf{P}=\mathbf{P_1}+t(\mathbf{P_2-P_1}),0\le t\le 1 $$

$\mathbf{P_1}$确定初始位置，$t(\mathbf{P_2-P_1})$控制范围

变换一下有

$$ \mathbf{P}=(1-t)\mathbf{P_1}+t\mathbf{P_2},0\le t\le 1 $$

对图形学熟悉的一眼能看出来这个公式是一个线性插值公式，也就是 lerp 表达式

标准lerp表达式为

$$ lerp(a,b,t) = (1-t)a+tb $$

二次贝塞尔曲线

两个点可以确定一个线段，那么三个点就可以确定两个线段，这样我们可以得到两个一次贝塞尔曲线。

假设平面上有三个点$\mathbf{P_1},\mathbf{P_2},\mathbf{P_3}$

那么我们可以的到两个lerp表达式

$$ lerp(\mathbf{P_1},\mathbf{P_2},t),\quad0\le t\le 1 $$

$$ lerp(\mathbf{P_2},\mathbf{P_3},t),\quad0\le t\le 1 $$

给定任意 $t$ 我们可以确定两个点$\mathbf{P_4},\mathbf{P_5}$

当我们 $t$ 取不同的值时$\mathbf{P_4},\mathbf{P_5}$的位置也会随之变化，如果我们将$\mathbf{P_4},\mathbf{P_5}$连接起来我们又会的到一条线段记作 $l$ ，很显然我们又得到一条lerp表达式

$$ lerp(\mathbf{P_4},\mathbf{P_5},t),\quad0\le t\le 1 $$

当我们的 $t$ 从0 到 1 取遍每一个数，那么线段 $l$ 也会随之动起来，当你把所有取不同数得到的 $l$ 画出来你会发现某个点的轨迹是一条曲线。

这个点的轨迹正是(注：$\mathbf{P_4},\mathbf{P_5}$是动点)

$$ lerp(\mathbf{P_4},\mathbf{P_5},t),\quad0\le t\le 1 $$

$$ lerp(lerp(\mathbf{P_1},\mathbf{P_2},t),lerp(\mathbf{P_2},\mathbf{P_3},t),t),\quad0\le t\le 1 $$

我们将表达式写出来

$$ \mathbf{P} = (1-t)[(1-t)\mathbf{P_1}+t\mathbf{P_2}]+t[(1-t)\mathbf{P_2}+t\mathbf{P_3}],\quad0\le t\le 1 $$

化简

$$ \mathbf{P} = (1-t)^2\mathbf{P_1}+2t(1-t)\mathbf{P_2}+t^2\mathbf{P_3},\quad0\le t\le 1 $$

这就是二次贝塞尔曲线

三次贝塞尔曲线

与二次贝塞尔曲线同理，4个点可以确定三条选段，得到三个lerp表达式

，这三个表达式可以确定3个点，这就退化到二次贝塞尔曲线了。

所以通过相同方式的推导我们可以的到三次贝塞尔曲线公式

$$ B(t) = (1-t)^3 P_0 + 3(1-t)^2 t P_1 + 3(1-t) t^2 P_2 + t^3 P_3, \quad t \in [0,1] $$

碰撞约束

在碰撞分离阶段除了使用碰撞法线直接分离物体之外，还有一种广泛用于各大刚体模拟引擎中的方法——碰撞约束。使用碰撞约束可以更加优雅的分离物体。

通过之前《物理引擎中的约束》这篇文章我们了解到我们只需要写出我们需要的约束方程，然后找出Jacobian矩阵就可以通过之前那篇文章最后导出的公式求解冲量了。接下来我们将介绍解释碰撞约束的约束方程和推导Jacobian矩阵。

碰撞约束的约束方程

假设物体A与物体B发生碰撞

想一想我们需要什么效果，我们希望物体A与物体B满足分离状态，我们需要使用符号表示出这个状态。这里再次给出约束方程的含义：约束方程是用于描述一个系统中各部件之间相对运动限制的数学表达式。通过这些方程，我们可以定义哪些运动是允许的，哪些是被禁止的。即当方程中的参数满足约束方程我们就不必调整物体状态，否则我们需要调整物体状态。

所以我们可以这么定义碰撞约束的位置约束方程

$$ C = (\mathbf{P}_a-\mathbf{P}_b)\cdot\mathbf{n}\ge0 $$

这个方程表示两点在法向方向上不能穿透（距离 ≥ 0），但可以分离（距离 > 0）或者接触（距离=0）。还可以这么理解当两个物体穿透的时候$$\mathbf{P}_a-\mathbf{P}_b$$与法线的点积小于0（不满足约束方程）剩下的情况（ ≥ 0）就是满足约束的情况了。

找到约束方程后我们还需要找到Jacobian矩阵

Jacobian矩阵

还记得我们《物理引擎中的约束》中提到的我们找到位置约束方程后，我们还需要对时间求导，得到速度约束方程。

对上面得到的碰撞约束方程对时间求一阶导数得到速度约束方程

$$ \dot{C} = \mathbf{n}\cdot\frac{d}{dt}\mathbf{P}_a- \mathbf{n}\cdot\frac{d}{dt}\mathbf{P}_b $$

• $ \frac{d}{dt}\mathbf{P}_a$就是碰撞点a的速度
• $ \frac{d}{dt}\mathbf{P}_b$就是碰撞点b的速度

容易得到点a的速度为

$$ \frac{d}{dt}\mathbf{P}_a=\mathbf{v}_a+(\omega_a\times\mathbf{r}_a) $$

容易得到点b的速度为

$$ \frac{d}{dt}\mathbf{P}_b=\mathbf{v}_b+(\omega_b\times\mathbf{r}_b) $$

带入速度约束方程化简有

$$ \dot{C} = \mathbf{n}\cdot\mathbf{v}_a+\omega_a\cdot(\mathbf{r}_a\times\mathbf{n})-\mathbf{n}\cdot\mathbf{v}_b-\omega_b\cdot(\mathbf{r}_b\times\mathbf{n}) $$

我们将其写成矩阵的形式

$$ \dot{C} = \begin{bmatrix} \mathbf{n} & \mathbf{r}_a\times\mathbf{n} & -n &-\mathbf{r}_b\times\mathbf{n} \end{bmatrix}\begin{bmatrix} \mathbf{v}_a \\ \omega_a\\ \mathbf{v}_b \\ \omega_b \end{bmatrix} $$

其中Jacobian就是

$$ J=\begin{bmatrix} \mathbf{n} & \mathbf{r}_a\times\mathbf{n} & -n &-\mathbf{r}_b\times\mathbf{n} \end{bmatrix} $$

注：Jacobian是我们最后应用不同物体冲量的方向例如

a->ApplyImpulseLinear(Vec2(Jacobian[0]*lambda, Jacobian[1]*lambda)); 
a->ApplyImpulseAngular(Jacobian[2]*lambda);                   
b->ApplyImpulseLinear(Vec2(Jacobian[3]*lambda, Jacobian[4]*lambda)); b->ApplyImpulseAngular(Jacobian[5]*lambda);           

求解冲量大小

根据我们的《物理引擎中的约束》最后的得到的方程我们可以解出冲量

$$ \lambda = -(JM^{-1}J^{T})^{-1}(JV) $$

带bias系数的形式

$$ \lambda = -(JM^{-1}J^{T})^{-1}(JV+b) $$

其中V为

$$ V=\begin{bmatrix} v_{ax} \\ v_{ay}\\ \omega_a\\ v_{bx} \\ v_{by} \\ \omega_b \end{bmatrix} $$

M的逆为

$$ M^{-1} = \begin{bmatrix} \frac{1}{m_a} & & & & & \\ & \frac{1}{m_a} & & & & \\ & & \frac{1}{I_a} & & & \\ & & & \frac{1}{m_b} & & \\ & & & & \frac{1}{m_b} & \\ & & & & &\frac{1}{I_b} \end{bmatrix} $$

JV是一个数，$JM^{-1}J^{T}$也是一个数，所以求出来$\lambda$也是一个数我们的公式是正确的

注:这里可以使用高斯-赛德尔（Gauss-Seidel）方法求解线性方程组