本章目录

凸体距离与接触几何

动力学方程需要知道接触发生在哪里、两个物体沿哪个方向互相阻挡。碰撞查询从形状和位姿出发,给出距离、方向和作用位置,为下一章构造接触雅可比准备几何数据。

本章先从球和盒的支持点建立凸体距离算法,再把相交情形扩展到穿透深度。单纯形上的最近点是贯穿两种算法的基本计算;保存它的重心权,还能把结果带回原来的两个物体。

数学约定

凸体 A,B 都用世界坐标表示。单位法线 \boldsymbol n 按几何顺序由 A 指向 B;有符号距离 d 在分离时为正,在穿透时为负。向量加减、长度与内积采用欧氏空间的定义,长度单位为米。源码依据本地 MuJoCo 3.10.0。

01

距离、见证点与接触记录

让两个物体逐渐靠近,碰撞计算需要先回答三个几何问题:沿哪个方向最接近、两个表面点在哪里、它们之间还相隔多远。球体的对称性让这三个量都能从球心连线读出,适合用来建立接触记录的含义。

设两个球的中心分别为 \boldsymbol c_A,\boldsymbol c_B,半径为 r_A,r_B。两个中心之间的连线同时给出了最近表面的方向。令中心距为 l=\|\boldsymbol c_B-\boldsymbol c_A\|>0,便有

\boldsymbol n=\frac{\boldsymbol c_B-\boldsymbol c_A}{l},\qquad d=l-r_A-r_B.
(10.1)

沿中心连线取得两个表面点:

\boldsymbol p_A=\boldsymbol c_A+r_A\boldsymbol n,\qquad \boldsymbol p_B=\boldsymbol c_B-r_B\boldsymbol n.

它们称为一对见证点。将两式相减,恰好得到 \boldsymbol p_B-\boldsymbol p_A=d\boldsymbol n。分离时,这一线段跨越间隙;穿透时,两个表面点的先后次序交换,带符号的关系仍然成立。中心重合时,球的对称性使法线有多个同样有效的选择,程序需要选定一个方向。

\boldsymbol p_B-\boldsymbol p_A=d\boldsymbol n,\qquad \boldsymbol p=\frac{\boldsymbol p_A+\boldsymbol p_B}{2}.
(10.2)

后一式取见证点的中点作为接触位置。一般凸体的查询也采用这一组数据:有符号距离、法线、见证点和中点。在数值迭代尚未收敛时,它们构成近似结果,同时保留距离上下界和终止原因。

例题:球与水平地面

地面为几何 A,球为几何 B。球半径为 r=0.2\,\mathrm m,球心位于 (0,0,0.19)\,\mathrm m。于是

\boldsymbol n=\begin{bmatrix}0\\0\\1\end{bmatrix},\quad d=0.19-0.2=-0.01\,\mathrm m,\quad \boldsymbol p_A=\begin{bmatrix}0\\0\\0\end{bmatrix}\mathrm m,\quad \boldsymbol p_B=\begin{bmatrix}0\\0\\-0.01\end{bmatrix}\mathrm m.

接触中点位于地面下方 0.005\,\mathrm m。这组数据将在后续章节用于构造约束与接触力。

从形体集合到一对凸体

场景中有很多几何体时,先通过包围体和空间层次结构筛出距离可能较近的形体对,再对每一对运行精确查询。这两步分别称为宽相位与窄相位。轴对齐包围盒的一维区间在三个世界坐标轴上都重叠时,两个包围盒相交;宽相位据此排除大量形体对。

在空间中转动球心连线,同时比较分离、相切与穿透三个状态。两个表面点各自固定在沿法线选出的球面位置上,带符号距离记录它们的相对次序。

思考保持球心连线方向不变,逐渐增大半径直到重叠,法线需要反向吗?

法线由球心连线的方向确定,半径变化时它保持相同。距离 d=l-r_A-r_B 从正变负,关系 \boldsymbol p_B-\boldsymbol p_A=d\boldsymbol n 便自动记录见证点先后次序的交换。符号变化体现在距离中。

实验 01球体距离、见证点与法线

改变球心距离与半径,并切换三维视图或中心连线截面。比较表面点在分离、相切和穿透时的位置。

默认半径之和为 1.2\,\mathrm m。球心距为 1.7\,\mathrm m 时,间隙为 0.5\,\mathrm m;减小到 0.9\,\mathrm m 时,有符号距离为 -0.3\,\mathrm m。穿透状态下,甲的表面点越过乙的表面点,式(10.2)由这个顺序变化自然保留负号。

窄相位按形状选择算法。球—平面、球—球等组合有简短的解析计算;一般凸体可以统一使用支持映射。凹形物体可先分成若干凸块,分别查询后汇集接触。MuJoCo 的碰撞分派表 mjCOLLISIONFUNC、网格凸壳与高度场专用路径,体现了这种按几何结构组织计算的方法。

一个面状接触区域可能需要多个接触点共同支撑。距离查询给出一对见证点,接触生成还可以围绕该特征补充多个点,并合并位置过近的记录。后文的单次 GJK/EPA 推导描述其中一对见证点。

02

支持映射与支持平面

球的最近方向由球心连线给出,长方体或凸网格则会随姿态改变最近的面、边或顶点。可以先解决一个更局部的问题:给定一个方向,形体在哪一点沿这个方向伸得最远?想象一张垂直于查询方向的平面从外侧靠近形体,它首次接触的点就是支持点。

一个支持点由当前形状和查询方向确定。形状从局部坐标放到世界坐标时,仍采用第二章的旋转与平移关系;查询方向要先转回局部坐标,求点后再转回世界坐标。

沿给定方向观察一个凸体,投影最远的点构成该方向的支持特征。给定非零查询向量 \boldsymbol u,定义支持点与支持函数:

s_A(\boldsymbol u)\in\operatorname*{arg\,max}_{\boldsymbol a\in A}\boldsymbol u^{\mathsf T}\boldsymbol a,\qquad h_A(\boldsymbol u)=\boldsymbol u^{\mathsf T}s_A(\boldsymbol u).
(10.3)

支持点是向量,支持函数是标量。同一方向可能对应一条边或整个面,其中任何一个最大投影点都可作为支持点。若查询方向归一化为单位向量,支持函数就是支持平面到原点的带符号距离:

\boldsymbol u^{\mathsf T}\boldsymbol x=h_A(\boldsymbol u),\qquad \boldsymbol u^{\mathsf T}\boldsymbol a\le h_A(\boldsymbol u)\quad(\boldsymbol a\in A,\ \|\boldsymbol u\|=1).

球与有向盒

中心为 \boldsymbol c、半径为 r 的球,沿方向的支持点为

s(\boldsymbol u)=\boldsymbol c+r\frac{\boldsymbol u}{\|\boldsymbol u\|},\qquad h(\boldsymbol u)=\boldsymbol u^{\mathsf T}\boldsymbol c+r\|\boldsymbol u\|.
推导球的最大投影s(u)=c+r\,u/\|u\|

球内任意一点可以写成 \boldsymbol c+\boldsymbol y,其中 \|\boldsymbol y\|\le r。由 Cauchy–Schwarz 不等式,

\boldsymbol u^{\mathsf T}(\boldsymbol c+\boldsymbol y)=\boldsymbol u^{\mathsf T}\boldsymbol c+\boldsymbol u^{\mathsf T}\boldsymbol y\le\boldsymbol u^{\mathsf T}\boldsymbol c+\|\boldsymbol u\|\,\|\boldsymbol y\|\le\boldsymbol u^{\mathsf T}\boldsymbol c+r\|\boldsymbol u\|.

取 \boldsymbol y=r\boldsymbol u/\|\boldsymbol u\| 时两个不等式同时取等号,因而得到支持点。

有向盒的中心为 \boldsymbol c,半边长为 (h_1,h_2,h_3),姿态矩阵 R 将盒内局部坐标转成世界坐标。先把方向转到局部系,再按每个分量的正负选择顶点:

\boldsymbol u_b=R^{\mathsf T}\boldsymbol u,\qquad p_i=\begin{cases}h_i,&(u_b)_i\ge0,\\-h_i,&(u_b)_i<0,\end{cases}\qquad s(\boldsymbol u)=\boldsymbol c+R\boldsymbol p.
(10.4)

盒的三个局部坐标分别取区间端点,就能同时达到最大投影。每条边对支持函数的贡献是半边长乘以相应方向分量的绝对值。

推导有向盒的三个独立最大值h(u)=u^{\mathsf T}c+\sum_i h_i|(R^{\mathsf T}u)_i|

盒中一点为 \boldsymbol c+R\boldsymbol p,满足 -h_i\le p_i\le h_i。它在查询方向上的投影为

\boldsymbol u^{\mathsf T}(\boldsymbol c+R\boldsymbol p)=\boldsymbol u^{\mathsf T}\boldsymbol c+(R^{\mathsf T}\boldsymbol u)^{\mathsf T}\boldsymbol p=\boldsymbol u^{\mathsf T}\boldsymbol c+\sum_{i=1}^{3}(u_b)_i p_i.

三个坐标的区间互相独立。系数为正时取上端点,为负时取下端点,为零时区间中所有取值都给出相同投影。按式(10.4)统一取正端点,便有

h(\boldsymbol u)=\boldsymbol u^{\mathsf T}\boldsymbol c+\sum_{i=1}^{3}h_i|(u_b)_i|.

这也解释了盒的支持函数随方向变化时出现的折点:支持顶点在这些方向发生切换。

盒的顶点沿查询方向怎样切换,可以从支持函数的转折处看出来。圆以原点为中心时,单位方向上的支持距离恒等于半径;旋转矩形的支持距离则是两个绝对值项之和。下面把几何支持直线与同一个函数的曲线并列观察。

实验 02支持点与支持函数

转动查询方向,观察支持直线和支持顶点的变化。切换到函数曲线,可以比较圆的恒定支持距离与旋转矩形的分段光滑曲线。

支持顶点跳到相邻顶点时,支持平面仍连续移动,支持函数通常保持连续而出现斜率变化。GJK 只需要最大投影值和一个达到它的支持点,因此可以直接处理这种形状特征的切换。

有限顶点凸多面体可以遍历顶点求最大内积;带邻接信息的网格还可以从上一顶点沿邻边搜索。支持映射将这些形状细节留在一个接口内,后续距离算法只需反复提供查询方向。

三维支持查询把直线换成平面。下面沿两个角度改变查询方向,同时转动盒体,观察支持特征怎样在顶点、边和面之间切换。

实验 03三维盒体的支持平面

转动查询方向与盒体,检查支持点落在平面上,而全部盒顶点都位于平面的同一侧。

当某个局部方向分量经过零时,相应角点坐标会从一端切换到另一端。恰好为零的瞬间,该方向上的整条边或整个面具有相同投影;支持接口只需返回其中一个最大点。

03

Minkowski 差与见证点恢复

直接寻找两个表面的最近点,需要同时移动两侧候选点。把每一对点的差看成一个新点后,两个变量可以合成一个几何对象:距离变成新集合到原点的距离,重叠则变成原点是否在集合内部。

为了统一处理两个形体,把它们的所有点差组成一个集合:

D=A-B=\{\boldsymbol a-\boldsymbol b:\boldsymbol a\in A,\ \boldsymbol b\in B\}.
(10.5)

这个集合称为闵可夫斯基差(Minkowski difference)。两个凸体的点对可以取凸组合,因此它们的差仍为凸集。两形体共有一点,等价于某对点差为零;分离时的表面距离,则等价于差集到原点的距离:

A\cap B\ne\varnothing\ \Longleftrightarrow\ \boldsymbol0\in D,\qquad d_*=\min_{\boldsymbol x\in D}\|\boldsymbol x\|.
(10.6)

d_* 是非负距离;相交时等于零,穿透深度需要进一步研究差集的边界。对盒子可以显式构造所有顶点差,但对一般形状,直接查询差集的支持点更方便。

查询差集的支持点,可以分别在两个形体上查询相反的方向,再将结果相减。原形体的支持接口因此能够直接用于差集,差集的凸性也由原形体的凸性保留。

推导差集的凸性与支持映射s_{A-B}(u)=s_A(u)-s_B(-u)

取两个差点 \boldsymbol x_1=\boldsymbol a_1-\boldsymbol b_1、\boldsymbol x_2=\boldsymbol a_2-\boldsymbol b_2。对 0\le t\le1,

(1-t)\boldsymbol x_1+t\boldsymbol x_2=\big((1-t)\boldsymbol a_1+t\boldsymbol a_2\big)-\big((1-t)\boldsymbol b_1+t\boldsymbol b_2\big)\in D.

因此 D 为凸集。再沿方向 \boldsymbol u 最大化投影:

\max_{\boldsymbol a\in A,\boldsymbol b\in B}\boldsymbol u^{\mathsf T}(\boldsymbol a-\boldsymbol b)=\max_{\boldsymbol a\in A}\boldsymbol u^{\mathsf T}\boldsymbol a+\max_{\boldsymbol b\in B}(-\boldsymbol u)^{\mathsf T}\boldsymbol b.

两项互相独立,各选一个支持点即可达到等号:

s_D(\boldsymbol u)=s_A(\boldsymbol u)-s_B(-\boldsymbol u),\qquad h_D(\boldsymbol u)=h_A(\boldsymbol u)+h_B(-\boldsymbol u).

差集的最近点只有一个三维位置,但接触还需要知道它来自两个物体上的哪里。因此每次支持查询都同时记录原形体的两个点。后续所有线段、三角面和四面体计算都沿用这份来源信息,才能把距离结果转换为接触点。

保留每个差点的来源

一次支持查询保存三组向量:\boldsymbol a_i=s_A(\boldsymbol u_i)、\boldsymbol b_i=s_B(-\boldsymbol u_i) 以及 \boldsymbol w_i=\boldsymbol a_i-\boldsymbol b_i。后续计算得到差集中的凸组合时,同一组权重可以分别作用于原形体:

\boldsymbol x=\sum_i\beta_i\boldsymbol w_i,\qquad \boldsymbol p_A=\sum_i\beta_i\boldsymbol a_i,\qquad \boldsymbol p_B=\sum_i\beta_i\boldsymbol b_i,\qquad \sum_i\beta_i=1,\quad\beta_i\ge0.
(10.7)

将差点定义代入,就有 \boldsymbol x=\boldsymbol p_A-\boldsymbol p_B。凸性保证两个恢复点仍属于各自形体。迭代收敛后,得到最近表面上的见证点;沿途的组合点则用于观察近似的改进。

分离时,差点由 B 指向 A,所以法线要取 \boldsymbol n=-\boldsymbol x/\|\boldsymbol x\|。这一负号来自差集采用 A-B 的定义。交换形体顺序后,距离保持相同,见证点交换,唯一的接触法线反向。

区间差与有符号距离

先在数轴上核对差集的含义。取 A=[-1,1]\,\mathrm m、B=[3,4]\,\mathrm m。所有两点之差中,最小值来自甲的左端减乙的右端,最大值来自甲的右端减乙的左端:

A-B=[-1-4,\;1-3]=[-5,-2]\,\mathrm m.

原点在差集之外,最近点为 -2\,\mathrm m。对应见证点为 p_A=1\,\mathrm m,p_B=3\,\mathrm m,法线朝右、距离为正,因此 p_B-p_A=2=dn。

再取 A=[-1,2]\,\mathrm m、B=[1,4]\,\mathrm m,差集变成 [-5,1]\,\mathrm m,包含原点。此时分开区间所需的最小平移由最近边界给出:向右移动乙区间一米即可相切。见证点为 p_A=2\,\mathrm m,p_B=1\,\mathrm m,故 n=1,d=-1\,\mathrm m,仍满足 p_B-p_A=dn。

图例区间差的分离与重叠

在两个数值预设之间切换,再平移乙区间。观察原点何时进入差集,以及最近边界对应哪一对见证端点。

区间的最近边界只有两个候选,可以直接比较。三维凸体的边界包含许多面、边和顶点,后面用单纯形逐步确定距离,用多面体边界逐步确定穿透深度;有符号距离与见证点之间仍采用式(10.2)的关系。

04

单纯形上的最近点

二维空间中的点、线段、三角形,三维空间中的点、线段、三角形、四面体,统称为单纯形。给定少量支持点,先在它们组成的单纯形上求离原点最近的点,再决定下一次查询的方向。下面从线段开始建立这一计算。

线段投影与端点分支

线段端点为 \boldsymbol a,\boldsymbol b,令 \boldsymbol e=\boldsymbol b-\boldsymbol a。线上一点为 \boldsymbol x(t)=\boldsymbol a+t\boldsymbol e,线段对应 0\le t\le1。最近点参数为

t_* =\operatorname{clip}\!\left(-\frac{\boldsymbol a^{\mathsf T}\boldsymbol e}{\boldsymbol e^{\mathsf T}\boldsymbol e},0,1\right),\qquad \boldsymbol x_*=(1-t_*)\boldsymbol a+t_*\boldsymbol b.
(10.8)
推导从平方距离得到截断投影t_*=\operatorname{clip}(-a^{\mathsf T}e/\|e\|^2,0,1)

先允许参数沿整条直线变化,最小化平方距离的一半:

F(t)=\frac12\|\boldsymbol a+t\boldsymbol e\|^2=\frac12\boldsymbol a^{\mathsf T}\boldsymbol a+t\boldsymbol a^{\mathsf T}\boldsymbol e+\frac12t^2\boldsymbol e^{\mathsf T}\boldsymbol e.
F^{\prime}(t)=\boldsymbol a^{\mathsf T}\boldsymbol e+t\boldsymbol e^{\mathsf T}\boldsymbol e,\qquad F^{\prime\prime}(t)=\|\boldsymbol e\|^2>0.

令一阶导数为零得到未截断参数。严格凸的二次函数离极小点越远,值越大,因此把参数限制到闭区间时,只需截断到最近端点。若端点重合,则直接取这一点。权重为 (1-t_*,t_*);零权重的端点可从单纯形移除。

例题:水平线段

取 \boldsymbol a=(2,1)、\boldsymbol b=(-1,1),则 \boldsymbol e=(-3,0),

t_*=-\frac{(2,1)\cdot(-3,0)}9=\frac23,\qquad \boldsymbol x_* =\frac13(2,1)+\frac23(-1,1)=(0,1).

最近距离为 1,两端点的权重分别为 1/3 和 2/3。

三角形的平面投影

取三角形顶点 \boldsymbol a,\boldsymbol b,\boldsymbol c,令 \boldsymbol e=\boldsymbol b-\boldsymbol a、\boldsymbol f=\boldsymbol c-\boldsymbol a。平面内的点写为 \boldsymbol x=\boldsymbol a+u\boldsymbol e+v\boldsymbol f。最近的平面投影应同时垂直于两条边方向,因此有 \boldsymbol e^{\mathsf T}\boldsymbol x=0、\boldsymbol f^{\mathsf T}\boldsymbol x=0。将点的表达代入,就得到关于两个参数的线性方程。其系数均为边向量的内积,组成 Gram 矩阵:

G=\begin{bmatrix}\boldsymbol e\cdot\boldsymbol e&\boldsymbol e\cdot\boldsymbol f\\\boldsymbol e\cdot\boldsymbol f&\boldsymbol f\cdot\boldsymbol f\end{bmatrix},\qquad b_g=-\begin{bmatrix}\boldsymbol e\cdot\boldsymbol a\\\boldsymbol f\cdot\boldsymbol a\end{bmatrix}.

先解两个平面参数,再把它们转为三个顶点的权重:

G\begin{bmatrix}u\\v\end{bmatrix}=b_g,\qquad\beta=(1-u-v,u,v).
(10.9)

三项权重都非负时,投影落在三角形内,就是最近点。出现负权重时,最近点位于边界:分别求三条线段的最近点,比较平方距离,保留最小者。共线的三个顶点也走线段分支。

推导Gram 方程、显式解与边界选择\Delta=\|e\|^2\|f\|^2-(e^{\mathsf T}f)^2

最小化 F(u,v)=\|\boldsymbol a+u\boldsymbol e+v\boldsymbol f\|^2/2。分别求导,

\frac{\partial F}{\partial u}=\boldsymbol e^{\mathsf T}\boldsymbol a+u\boldsymbol e^{\mathsf T}\boldsymbol e+v\boldsymbol e^{\mathsf T}\boldsymbol f=0,\qquad \frac{\partial F}{\partial v}=\boldsymbol f^{\mathsf T}\boldsymbol a+u\boldsymbol f^{\mathsf T}\boldsymbol e+v\boldsymbol f^{\mathsf T}\boldsymbol f=0.

为展开二阶消元,暂记 E=\boldsymbol e\cdot\boldsymbol e、F_0=\boldsymbol f\cdot\boldsymbol f、H=\boldsymbol e\cdot\boldsymbol f、a_e=\boldsymbol a\cdot\boldsymbol e、a_f=\boldsymbol a\cdot\boldsymbol f。方程行列式及解为

\Delta=EF_0-H^2,\qquad u=\frac{Ha_f-F_0a_e}{\Delta},\qquad v=\frac{Ha_e-Ea_f}{\Delta}.

非共线时 \Delta>0;它等于两边张成的平行四边形面积的平方。解落在三角形外时,三角形内部没有满足零梯度的极小点,所以约束极小点落在三条边上。逐边比较就覆盖了边内部和全部顶点。数值上可比较 \Delta/(EF_0) 与无量纲阈值,以识别接近共线的情形。

二维实现可以直接解 \boldsymbol a+u\boldsymbol e+v\boldsymbol f=0。记二维叉积为 [\boldsymbol x,\boldsymbol y]=x_1y_2-x_2y_1,两边分别与 \boldsymbol f,\boldsymbol e 作叉积:

u=-\frac{[\boldsymbol a,\boldsymbol f]}{[\boldsymbol e,\boldsymbol f]},\qquad v=-\frac{[\boldsymbol e,\boldsymbol a]}{[\boldsymbol e,\boldsymbol f]}.

这给出本章二维程序所用的重心权。三维三角形仍使用式(10.9),因为原点通常位于它的平面之外。

三点不共线时,重心权描述最近点落在三角形的哪一部分:三个正权重对应面内,两项非零对应边,一项为一对应顶点。三点共线后,同一点可能有多组权重,所在特征应按实际线段凸包判断。原点之外的查询点可以通过整体平移转成同一个计算问题。

实验 04最近点的面、边与顶点分支

移动查询点,观察权重从三个非零项变成两个或一个。第三顶点高度取 0.6\,\mathrm m 时三点共线,可以检查退化分支。

跨越分支边界时,最近点连续移动,某些权重逐渐变为零。保留有贡献的顶点,既保存了当前最近点,也缩小了下一轮 GJK 需要处理的单纯形。

三维四面体

对四面体顶点 \boldsymbol a,\boldsymbol b,\boldsymbol c,\boldsymbol d,先判断原点能否写成非负重心组合。令

E=\begin{bmatrix}\boldsymbol b-\boldsymbol a&\boldsymbol c-\boldsymbol a&\boldsymbol d-\boldsymbol a\end{bmatrix},\qquad E\begin{bmatrix}u\\v\\w\end{bmatrix}=-\boldsymbol a,\qquad \beta=(1-u-v-w,u,v,w).

矩阵可逆且四项权重非负时,原点就在四面体内,最近距离为零。否则分别求四个三角形面的最近点,比较后保留最小者。四点共面时也通过这些面退化到三角形、线段或顶点。

推导四面体的体积坐标与外部最近点\beta_i\geq0,\quad\sum_i\beta_i=1

令三条边为 \boldsymbol e,\boldsymbol f,\boldsymbol g。有向体积的六倍为 V=\boldsymbol e\cdot(\boldsymbol f\times\boldsymbol g)。对 u\boldsymbol e+v\boldsymbol f+w\boldsymbol g=-\boldsymbol a 依次与三组边叉积作内积:

u=\frac{(-\boldsymbol a)\cdot(\boldsymbol f\times\boldsymbol g)}V,\quad v=\frac{\boldsymbol e\cdot((-\boldsymbol a)\times\boldsymbol g)}V,\quad w=\frac{\boldsymbol e\cdot(\boldsymbol f\times(-\boldsymbol a))}V.

这些比例就是相应子四面体与整个四面体的有向体积比。若原点在四面体外,内部的平方距离梯度为 \boldsymbol x,只有原点才会使它为零;因此最近点必在边界。四个面覆盖整个边界,逐面求最近点可以完整处理面、棱和顶点分支。实际求解采用线性方程或带尺度判断的行列式计算,并为接近零体积的情况保留降维路径。

四面体比三角形增加了一个内部区域。查询点进入内部后,距离降为零;在外部时,最近点依次可能落在三角面、棱或顶点。下面的体积图将四项权重与这些区域对应起来,并允许把四面体压成一个平面凸包。

实验 05四面体的最近点与退化

移动查询点,比较内部、三角面、棱和顶点上的最近点;重心权与被选中的几何元素同步显示。第四顶点降到底面高度时,四点共面,最近点计算退化为三角形问题。透明表面便于从不同视角观察内部。

外部查询最终都会归结到三角面及其边界。四点共面后,体积方程失去可逆性,逐面和逐边比较仍能找到实际凸包的最近点,这正是降维分支的用途。

05

GJK 距离迭代

Gilbert–Johnson–Keerthi 距离算法(GJK)从少量差集支持点开始,在当前单纯形上求最近点,再向原点一侧寻找新的支持点。每轮只保留对最近点有贡献的顶点,因而三维计算始终围绕至多四个顶点展开。决定下一方向的几何关系如下:

\boldsymbol x=\operatorname*{arg\,min}_{\boldsymbol y\in S}\|\boldsymbol y\|,\qquad \boldsymbol s=s_D\!\left(-\frac{\boldsymbol x}{\|\boldsymbol x\|}\right).
(10.10)

S 为当前支持点的凸包。沿 -\boldsymbol x 的支持查询寻找最靠近原点一侧的边界。新点与旧单纯形形成更大的凸包,再求最近点后,距离上界只能保持或减小。

距离上下界

当前点属于差集,所以它的长度提供上界。支持查询又给出了整个差集在当前方向上的最小投影,由此得到下界:

U=\|\boldsymbol x\|,\qquad L=\max\!\left(0,\frac{\boldsymbol x^{\mathsf T}\boldsymbol s}{\|\boldsymbol x\|}\right),\qquad L\le d_*\le U.
(10.11)
推导支持平面的距离下界L\leq d_*\leq U

由 \boldsymbol s 在负方向上最大化投影,对任意 \boldsymbol y\in D 都有

-\boldsymbol x^{\mathsf T}\boldsymbol s\ge-\boldsymbol x^{\mathsf T}\boldsymbol y,\qquad \boldsymbol x^{\mathsf T}\boldsymbol y\ge\boldsymbol x^{\mathsf T}\boldsymbol s.

再用内积不等式 \boldsymbol x^{\mathsf T}\boldsymbol y\le\|\boldsymbol x\|\,\|\boldsymbol y\|,可得

\|\boldsymbol y\|\ge\frac{\boldsymbol x^{\mathsf T}\boldsymbol s}{\|\boldsymbol x\|}.

长度同时非负,因而取两者的较大值。对所有差点取最小值,便得到下界。当前单纯形包含于 D,所以 d_*\le\|\boldsymbol x\|。每轮可以保留历史下界的最大值,使显示的区间持续收紧。

只判断是否相交时,L>0 已经证明分离。需要精确距离时,则继续迭代,直到 U-L\le\varepsilon,其中 \varepsilon 的单位为长度。若只关心某个查询范围内的距离,L 已超过该范围时也可提前返回。

例如当前区间为 [0.3,0.8]\,\mathrm m,分离已经得到证明,但距离估计仍可能相差半米。继续查询到 [0.499,0.501]\,\mathrm m 后,采用上界报告距离时误差才被限制在 0.002\,\mathrm m 内。相交判断和距离查询由此可以使用不同的停止标准。

平方距离的收敛间隙

MuJoCo 的原生 GJK 还使用平方距离目标 f(\boldsymbol x)=\|\boldsymbol x\|^2/2 的凸优化间隙。令

G=\boldsymbol x^{\mathsf T}(\boldsymbol x-\boldsymbol s).

G 的单位是长度平方。它对目标误差和距离误差分别给出

0\le f(\boldsymbol x)-f(\boldsymbol x_*)\le G,\qquad (U-d_*)^2\le U^2-d_*^2\le2G.
(10.12)
推导凸优化间隙到长度误差(U-d_*)^2\leq2G

把二次函数在当前点附近精确展开:

f(\boldsymbol x_*)=f(\boldsymbol x)+\boldsymbol x^{\mathsf T}(\boldsymbol x_*-\boldsymbol x)+\frac12\|\boldsymbol x_*-\boldsymbol x\|^2\ge f(\boldsymbol x)+\boldsymbol x^{\mathsf T}(\boldsymbol x_*-\boldsymbol x).

移项,并利用支持点在 \boldsymbol x 方向上投影最小:

f(\boldsymbol x)-f(\boldsymbol x_*)\le\boldsymbol x^{\mathsf T}(\boldsymbol x-\boldsymbol x_*)\le\boldsymbol x^{\mathsf T}(\boldsymbol x-\boldsymbol s)=G.

代入平方距离目标并乘以二,得到 U^2-d_*^2\le2G。又因 U\ge d_*\ge0,

(U-d_*)^2\le(U-d_*)(U+d_*)=U^2-d_*^2.

因此 G\le\varepsilon^2/2 足以保证长度误差不超过 \varepsilon。当本次支持查询给出的投影下界为正时,G=U(U-L),可以直接比较两种终止量。

完整迭代顺序

  1. 取一个非零初始方向,查询并保存支持点及其两个来源。
  2. 求当前单纯形到原点的最近点和重心权,移除零权重顶点。
  3. 若最近距离已小于接触容差,转入接近接触或穿透的处理。
  4. 沿最近点的反方向查询新支持点,更新距离下界;检查距离区间或平方间隙。
  5. 尚未收敛时,将新支持点加入单纯形,返回第二步。

实现还要处理重复支持点、最近点停滞、降维和迭代次数上限。达到数值容差的结果可标记为收敛;重复点导致区间仍过宽时,应保留“停滞”状态与现有界限。原点距离进入容差只表示几何上已非常接近,穿透深度还由下一步检查。接触法线在棱角、对称形体或零距离处可能有多个有效选择。

一个新的支持点能把当前距离缩小多少,要同时查看当前单纯形和支持平面。下面每步保存同一轮的最近点、下一支持点与距离界;切换到原形体时,同一组重心权恢复出两侧见证点。

实验 06GJK 的单纯形、见证点与距离界

逐步推进支持查询,比较差集中的最近点和原形体中的见证点。调节第二个盒子的位置可经过分离、接触和穿透状态;距离曲线显示当前步骤所在的上下界。

例如将两盒转角和位置保持在初始值,图中从旧最近点 x_k 出发,先沿 -x_k 查询支持点 s_k,再求 \operatorname{conv}(S_k\cup\{s_k\}) 上的最近点。若新最近点位于一条边上,就按式(10.8)计算两个权重;上界更新为这条投影线段的长度,下界更新为式(10.11)的支持平面投影。动画的一次推进对应这一整次几何更新。

距离上界来自已经找到的一对可行点,下界来自整张支持平面。两者接近时,算法既有一对近似最近点,也有一个限制剩余误差的几何证据。原点进入当前凸包后,非负距离已降到零,下一步转而求边界的穿透深度。

三维中,单纯形还可能成为具有体积的四面体。下面的旋转盒体与四面体具有不同的面法线,支持点带有三个坐标;逐步比较当前最近点、支持平面和由重心权恢复的见证点。

实验 07三维 GJK 的单纯形更新

先观察旋转盒体与四面体,再切换到差集逐步查询支持点。改变间距和四面体姿态,比较最近特征、距离界及两侧见证点。

最近点落在面上时,用三个权重组合支持点;原点进入四面体内部时,用四个非负权重重建原点,距离达到零。边和顶点情形继续使用前面的低维公式,因此同一算法会在不同维数的几何特征之间切换。

06

EPA 穿透深度与多面体扩张

两个凸体发生正深度穿透时,原点位于差集内部。为了将 B 沿向量 \boldsymbol t 平移到刚好分离,差集随之变成 D-\boldsymbol t。原点落在新差集边界,等价于 \boldsymbol t 落在原差集边界。因此最短分离平移的长度为

\delta=\min_{\boldsymbol x\in\partial D}\|\boldsymbol x\|,\qquad d=-\delta.
(10.13)

扩张多面体算法(Expanding Polytope Algorithm,EPA)从一个包含原点的内接凸多面体开始,不断沿最近边界的外法线补充支持点,使该内接多面体逼近真实边界。二维对应多边形,三维对应由三角面连接起来的多面体。

GJK 在相交时的目标已经达到零,内部最近点无法继续描述穿透有多深。EPA 改为寻找包围原点的边界:把第二个形体沿最近边界点平移,就能使原点退出差集内部。这个目标变化解释了两种算法为何连续使用,却分别需要最近点和最近面。

初始多面体

GJK 结束时可能只留下穿过原点的线段或三角形。二维可以沿线段的两个垂直方向补充支持点;三维则沿三角形的正、负法线扩展,再建立包围原点的凸壳。初始多面体需要有完整维数,并检查原点位于各面内侧。若几何本身只在一点、边或面上接触,则穿透深度为零;近共面面片和重复顶点需要在建壳过程中处理。

最近面与深度上下界

设当前内接多面体为 P\subset D,原点在其内部。选离原点最近的面,单位外法线为 \boldsymbol n,面到原点的距离为 L>0。沿这个方向查询真实支持点:

\boldsymbol x=L\boldsymbol n,\qquad \boldsymbol s=s_D(\boldsymbol n),\qquad U=\boldsymbol n^{\mathsf T}\boldsymbol s,\qquad L\le\delta\le U.
(10.14)
推导内接多面体与支持平面的界限L\leq\delta\leq U

将多面体各个面的外法线写为 \boldsymbol n_i,面距离为 l_i>0。其内部满足 \boldsymbol n_i^{\mathsf T}\boldsymbol y\le l_i。令 L=\min_i l_i,则所有满足 \|\boldsymbol y\|\le L 的点都有

\boldsymbol n_i^{\mathsf T}\boldsymbol y\le\|\boldsymbol y\|\le L\le l_i.

以原点为中心、半径为 L 的球完全位于 P 内,也就位于 D 内,故真实边界距离至少为 L。对于最小面,点 L\boldsymbol n 满足其他所有面的内侧条件,并落在该面上,所以正是面内的最近点。

真实差集满足 \boldsymbol n^{\mathsf T}\boldsymbol y\le U。从原点沿 \boldsymbol n 发出射线,它与真实边界的首次交点可写为 \rho\boldsymbol n,且 \rho\le U。全方向的最短边界距离至多为这一方向的距离,故 \delta\le\rho\le U。

当 U-L\le\varepsilon 时,穿透深度的误差被夹在长度容差内。新支持点加入后,内接多面体扩大,下界逐渐提高。每次查询给出的上界都有效,还可保留历史上界的最小值。MuJoCo 使用这一历史最小值;本章二维程序显示本次查询的上界,便于与当前支持直线对应。

可见面与地平线

在三维中,一个三角面的单位外法线由有向边叉积求得:

\boldsymbol n=\frac{(\boldsymbol b-\boldsymbol a)\times(\boldsymbol c-\boldsymbol a)}{\|(\boldsymbol b-\boldsymbol a)\times(\boldsymbol c-\boldsymbol a)\|},\qquad l=\boldsymbol n^{\mathsf T}\boldsymbol a.

顶点绕序应使原点在内侧,即 l\ge0。新支持点 \boldsymbol s 若满足 \boldsymbol n^{\mathsf T}(\boldsymbol s-\boldsymbol a)>0,就在该面的外侧,这个面称为从新点可见的面。

扩张时,可见面被替换为新支持点与地平线围成的一圈三角面。更新保持凸包封闭,并保留朝外的法线和相邻面之间一致的边绕序;这些几何条件使下一轮的距离界继续有效。

说明三维 EPA 的拓扑更新

从最近面出发,沿面之间的邻接关系寻找所有可见面。凸多面体中,这些面组成朝向新点的一片表面。连接两个可见面的边在更新后消失;一侧可见、一侧保留的边构成这片区域的边界,称为地平线。

  1. 记录地平线上的有向边以及每条边外侧保留的邻面,再删除可见面。
  2. 将每条地平线边与新支持点连成一个三角面。新面的边绕序与相邻保留面相反,使共享边两侧的面法线都朝外。
  3. 检查新三角面的面积和原点内侧条件;为有效面计算法线、平面距离及原点投影的重心权。
  4. 连接新面之间以及新面与保留面之间的邻接索引,更新候选最近面集合。
  5. 重新选择距离最小的有效面,再做支持查询。

若叉积长度接近零,新面面积太小,应触发退化处理;若查询点重复或容量、迭代次数耗尽,则返回当前有效界限与终止状态。维护有向边和邻面索引,是三维实现中除距离计算之外的主要工作。二维只需删除新点可见的连续边链,再接入新点;小型实现也可以每次对旧顶点与新点重建凸包。

穿透见证点与法线方向

最近面的投影点由三个顶点的重心权表示,按式(10.7)恢复原形体中的点。由于投影位于外法线方向,

\boldsymbol p_A-\boldsymbol p_B=\boldsymbol x\simeq\delta\boldsymbol n,\qquad \boldsymbol p_B-\boldsymbol p_A\simeq-\delta\boldsymbol n=d\boldsymbol n.

因此 EPA 的面外法线直接对应本章从 A 指向 B 的接触法线,沿它移动 B 可以解除穿透。与第五节的分离状态相比,差点归一化时的符号随有符号距离一起改变。见证点相同或深度为零时,可延用最近有效方向,或由几何特征确定方向。

将旋转矩形与凸五边形交叠,支持点会随查询方向切换到不同顶点。GJK 先找到包含原点的单纯形;EPA 在这个结果上补充边界点,逐步找到差集最近的边。原形体视图中的见证点随所选最近边同步改变。

实验 08EPA 的逐步扩张

旋转矩形与凸五边形相交。改变五边形的位置与姿态,逐步查看最近边、支持点、见证点和穿透深度上下界。

一次扩张先从当前最近边取得单位外法线 n,再从两个多边形的顶点投影中取得 s_D(n),计算上界 U=n^{\mathsf T}s_D(n)。加入新点后重建边界,重新选择最近边得到下一轮的 L。当新支持点已经位于当前支持边上时,这一方向的两条界线重合,扩张可以结束。

当上下界收紧,所选边逐渐贴近真实边界,恢复出的点差也逐渐接近 \delta n。把深度取负后,接触法线和见证点便与分离状态共用 p_B-p_A=dn 这一带符号关系。

把平面截面扩展到空间后,EPA 的边界由三角面组成。新支持点能够看到哪些旧面,就需要删除哪些面;可见区域的边界留下一个有向边环,将它与新点连接便形成新的封闭表面。

实验 09三维 EPA 的可见面与边界扩张

沿用旋转盒体与四面体的相交状态,逐步查看最近三角面、新支持点、可见面与边界边。改变四面体姿态,比较接触法线与深度。

本实验先运行同一对凸体的 GJK,再从包含原点的四面体建立 EPA 边界;若 GJK 只留下低维单纯形,则补充支持方向构造具有体积的初始凸包。每一步的橙色最近面给出深度下界,沿其外法线的真实支持平面给出上界。旋转视角可以看到新点与面之间的前后关系;切换到原形体时,同一组三角形重心权恢复两侧见证点。

07

接触标架与相对运动

距离和法线确定后,还需要两个互相垂直的切向,以描述摩擦和接触速度。先从世界三个坐标轴中选出与法线最不平行的一根轴 \boldsymbol e,将它投影到切平面并归一化:

\boldsymbol w=\boldsymbol e-(\boldsymbol e^{\mathsf T}\boldsymbol n)\boldsymbol n,\qquad \boldsymbol t_1=\boldsymbol w/\|\boldsymbol w\|,\qquad\boldsymbol t_2=\boldsymbol n\times\boldsymbol t_1.

第一步去掉轴在法线上的分量,第二步只改变剩余向量的长度,第三步确定右手方向。把三根单位轴按行排成矩阵:

C=\begin{bmatrix}\boldsymbol n^{\mathsf T}\\\boldsymbol t_1^{\mathsf T}\\\boldsymbol t_2^{\mathsf T}\end{bmatrix}.
(10.15)

C 的三行依次是法向、第一切向和第二切向;它把世界向量转到接触坐标。选择最不平行的轴,可以让归一化前的投影长度远离零。切向基在轴选择改变时可能切换,所表示的切平面保持相同。

接触标架是一个右手正交基。世界速度乘以 C 得到接触分量,接触力乘以 C^{\mathsf T} 返回世界分量;变换前后,力与相对速度的内积保持相同。

推导正交性、右手方向与功率f^{\mathsf T}v=f_c^{\mathsf T}v_c

由投影余量的定义 \boldsymbol w=\boldsymbol e-(\boldsymbol e^{\mathsf T}\boldsymbol n)\boldsymbol n。由 \boldsymbol n^{\mathsf T}\boldsymbol n=1,

\boldsymbol n^{\mathsf T}\boldsymbol w=\boldsymbol n^{\mathsf T}\boldsymbol e-(\boldsymbol e^{\mathsf T}\boldsymbol n)=0.

所以 \boldsymbol t_1 垂直于法线,且长度为一;两单位正交向量的叉积也为单位向量,并同时垂直于二者。于是 CC^{\mathsf T}=I。行向量的标量三重积给出

\det C=(\boldsymbol n\times\boldsymbol t_1)^{\mathsf T}\boldsymbol t_2=\|\boldsymbol t_2\|^2=1.

若世界相对速度为 \boldsymbol v,接触坐标的力为 \boldsymbol f_c,则

\boldsymbol v_c=C\boldsymbol v,\qquad \boldsymbol f=C^{\mathsf T}\boldsymbol f_c,\qquad \boldsymbol f^{\mathsf T}\boldsymbol v=\boldsymbol f_c^{\mathsf T}C\boldsymbol v=\boldsymbol f_c^{\mathsf T}\boldsymbol v_c.

转置回传力的关系再次来自同一功率在两套坐标中的相等。

固定朝向球的接触速度

对两个刚体,应在选定接触位置计算各自点速度,再取 \boldsymbol v=\boldsymbol v_B-\boldsymbol v_A。法向速度 v_n=\boldsymbol n^{\mathsf T}\boldsymbol v 为正时,间隙正在增大。本章贯穿例中的球保持姿态固定、仅质心平移;静止地面的点速度为零,所以相对速度就是球的平移速度。

C=\begin{bmatrix}0&0&1\\1&0&0\\0&1&0\end{bmatrix},\qquad \boldsymbol v=\begin{bmatrix}0.8\\0\\-0.1\end{bmatrix}\mathrm{m/s},\qquad \boldsymbol v_c=\begin{bmatrix}-0.1\\0.8\\0\end{bmatrix}\mathrm{m/s}.

球以 0.1\,\mathrm{m/s} 向地面靠近,同时沿第一切向运动。下一章将把刚体点速度写成广义速度的线性映射,从这里得到接触雅可比。

08

候选范围、激活边界与源码

碰撞记录可以在形体表面仍有间隙时建立。这样便能在进入激活区间之前保留附近的接触几何。这里把两几何参数混合后得到的 margin 记为 m_c,把 gap 记为 g_c。对本章球—平面查询,本地 MuJoCo 3.10.0 的规则是

\text{候选范围:}\quad d\le m_c+g_c,\qquad \text{进入接触约束:}\quad d<m_c,\qquad r_n=d-m_c.
(10.16)

d 描述真实形体间的有符号距离,r_n 描述相对激活边界的位置误差。两者相差一个固定偏移,后面的参考加速度与软接触模型将使用后者。刚好位于 d=m_c 的记录被排除在约束之外;球—平面解析查询在候选边界上保留等号。

例题:两层距离阈值

取 m_c=0.02\,\mathrm m、g_c=0.005\,\mathrm m,候选距离上限为 0.025\,\mathrm m。

  • d=0.03\,\mathrm m:位于候选范围之外。
  • d=0.022\,\mathrm m:保存候选记录,保持约束未激活。
  • d=0.015\,\mathrm m:真实表面仍有间隙,约束已激活,r_n=-0.005\,\mathrm m。
  • d=-0.01\,\mathrm m:真实表面穿透,r_n=-0.03\,\mathrm m。

球心高度等于 r+d。半径为 0.2\,\mathrm m 时,候选高度为 0.225\,\mathrm m,激活高度为 0.22\,\mathrm m。

接触数据要从几何量转成交给动力学的坐标与位置误差。下面同时保留真实表面、候选外边界与激活边界:球体尚有正间隙时,也可能已经进入约束;接触三轴则决定随后各行速度和力的正方向。

实验 10球—平面接触标架与距离阈值

升降球心,依次观察候选记录、约束激活和真实穿透。旋转视角可核对接触三轴的方向,图例可以分别保留法线、切向和阈值标线。

几何距离决定当前表面关系,减去激活边界才得到恢复误差。下一章将沿这里的法线和两个切向投影相对点速度,把同一条接触记录写成约束雅可比的各行。

从支持点到接触数组

本地 engine_collision_convex.c 的 mjc_sphereSupport 与 mjc_boxSupport 对应第二节的支持映射。盒的实现先用姿态矩阵转置变换查询方向,再选局部角点;零方向分量选正端点。原生支持接口接收单位方向,所以球的代码直接把半径乘以方向。

engine_collision_gjk.c 中,Vertex 同时保存 vert、vert1 和 vert2,分别对应差点及两个来源。gjkSupport 沿当前最近点的反方向查询;subdistance 根据顶点数分派最近单纯形计算,再按重心权缩减顶点。平滑支持形体的平方间隙阈值为 \varepsilon^2/2;离散形体使用精确顶点查询,并配合重复点和停滞判断。

同一文件中的 epa 选择最近面、更新历史上界并调用 horizon 扩张;epaWitness 通过三角面的仿射坐标恢复见证点。刚体凸体的查询范围还可通过支持点膨胀加入几何计算:双方各加半个查询范围,差集总共扩大一个查询范围。凸体包装函数再把该偏移加回距离,恢复真实形体的 dist,并取两见证点中点作为 pos。沿原法线对称膨胀保持中点相同。

球—平面的专用路径位于 engine_collision_primitive.c 的 mjraw_PlaneSphere。代码直接计算球心到平面的距离减半径,并使用 \boldsymbol c_B-(r+d/2)\boldsymbol n 得到接触中点,与第一节的见证点平均完全一致。

engine_collision_driver.c 的窄相位调用将 margin + gap 传给几何查询;随后 mj_setContact 写入 includemargin = margin,并以 dist >= includemargin 设置 exclude。mju_makeFrame 补全接触三轴。每个 contact.frame 按行保存法向、第一切向和第二切向,共九个数;算法选出的切向基可以与本章构造不同,读数与力的坐标变换使用实际存储的这组基。

接触记录还包含形体编号、摩擦、软约束参数及 efc_address。最后一项把接触连接到求解器约束行。完整碰撞概述可参照 本地计算文档 的 Collision detection,以及 官方计算文档;参数边界以上述本地版本的调用与字段赋值为依据。

09

实现与练习

三维实验使用本地 collision3d-math.js:支持点携带两侧见证点,最近单纯形通过候选特征和重心权计算,EPA 保存三角面与可见边界。阅读时可先跟随下列二维 Python 程序核对每步代数,再对照三维模块中的同名操作。

collision2d.py 将支持映射、最近单纯形、GJK、EPA 和见证点恢复组成一个仅使用 Python 标准库的二维模块。形状可以是凸多边形或解析圆。box(center, half, angle) 用中心、半边长和逆时针弧度建立旋转矩形;query(a, b) 返回带符号距离、法线、两个见证点、界限、迭代记录和终止状态。

from collision2d import box, query

a = box((0, 0), (1, 1))
b = box((1.5, 0), (1, 1))
result = query(a, b)
print(result["status"])           # converged
print(result["signed_distance"])  # -0.5
print(result["normal"])           # (1.0, 0.0)
print(result["lower"], result["upper"])  # 深度区间 [0.5, 0.5]

gjk 的 lower/upper 表示非负分离距离的界;epa 的同名字段表示非负穿透深度的界。query 用 algorithm 标明采用的阶段,并另给出 signed_distance。状态 converged 表示区间达到容差,stalled 表示支持点重复而区间仍未充分收紧,iteration_limit 表示达到迭代上限。gjk 单独返回的 near_origin 继续交由 EPA 检查;低维接触可返回 degenerate。调用方据此决定接受近似、调整容差或采用几何特例。

程序中的绝对距离容差默认是 10^{-9} 个长度单位,角度用弧度。几何尺度变化时,绝对容差也应按所需精度调整。最近三角形用相对面积判断退化;二维 EPA 每次重建凸包,使删除可见边链的结果容易检查。三维扩展沿用相同支持点结构和界限,替换为第四节的四面体距离计算及第六节的面邻接更新。

check_collision2d.py 使用解析线段、独立的多边形分离轴、逐边距离以及圆的公式核对结果。运行时从本章所在目录执行:

python3 examples/check_collision2d.py

检查同时覆盖包含关系、刚好接触、重复顶点、共线退化、形体顺序交换、共同平移、尺度变化与迭代上限。每次查询还可检查两个不变量:重心权非负且和为一,见证点差与带符号距离乘法线相符。

1. 两球的分离与穿透

两球中心分别为 (0,0,0) 和 (l,0,0),半径为 0.8\,\mathrm m 与 0.6\,\mathrm m。分别取 l=3\,\mathrm m 和 l=1\,\mathrm m,求法线、距离、见证点及中点。

解答中心连线上的带符号关系

两种情形的法线均为 \boldsymbol n=(1,0,0)。距离为 d=l-1.4,见证点为 \boldsymbol p_A=(0.8,0,0) 和 \boldsymbol p_B=(l-0.6,0,0)。

l=3:\quad d=1.6,\quad \boldsymbol p_B=(2.4,0,0),\quad \boldsymbol p=(1.6,0,0);
l=1:\quad d=-0.4,\quad \boldsymbol p_B=(0.4,0,0),\quad \boldsymbol p=(0.6,0,0).

各长度单位均为米。第二种情形中 \boldsymbol p_B-\boldsymbol p_A=(-0.4,0,0)=d\boldsymbol n;把第二个球沿正横向平移 0.4\,\mathrm m 后恰好接触。

2. 旋转盒的支持点

盒中心为 (1,0,0)\,\mathrm m,半边长为 (1,0.5,0.25)\,\mathrm m,姿态为绕世界竖直轴旋转 \pi/2。沿 \boldsymbol u=(1,1,0) 查询支持点,求支持函数;零分量采用正端点。

解答局部方向与世界顶点
R=\begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix},\qquad R^{\mathsf T}\boldsymbol u=\begin{bmatrix}1\\-1\\0\end{bmatrix},\qquad \boldsymbol p=\begin{bmatrix}1\\-0.5\\0.25\end{bmatrix}\mathrm m.
s(\boldsymbol u)=\begin{bmatrix}1\\0\\0\end{bmatrix}+R\boldsymbol p=\begin{bmatrix}1.5\\1\\0.25\end{bmatrix}\mathrm m,\qquad h(\boldsymbol u)=1.5+1=2.5\,\mathrm m.

若使用单位方向 \boldsymbol u/\sqrt2,支持点保持相同,支持函数变成 2.5/\sqrt2\,\mathrm m。支持平面到原点的距离采用后一数值。

3. 线段权重与见证点

两次支持查询得到 \boldsymbol a_1=(3,1)、\boldsymbol b_1=(1,0),以及 \boldsymbol a_2=(0,2)、\boldsymbol b_2=(1,1)。求差线段的最近点,再恢复两个见证点。

解答同一组重心权的三次应用

差点为 \boldsymbol w_1=(2,1)、\boldsymbol w_2=(-1,1),由式(10.8)得 t=2/3。于是

\boldsymbol x=\frac13\boldsymbol w_1+\frac23\boldsymbol w_2=(0,1),\quad \boldsymbol p_A=\frac13(3,1)+\frac23(0,2)=(1,5/3),\quad \boldsymbol p_B=\frac13(1,0)+\frac23(1,1)=(1,2/3).

相减得到 \boldsymbol p_A-\boldsymbol p_B=(0,1)。若这已是整个差集的最近点,则距离为 1,从第一形体指向第二形体的法线为 (0,-1),并满足式(10.2)。

4. 三维三角形的最近点

取 \boldsymbol a=(-1,-1,1)、\boldsymbol b=(2,-1,1)、\boldsymbol c=(-1,2,1)。写出 Gram 方程,求原点到三角形的距离及三项权重。

解答平面投影的内部检验

两条边为 \boldsymbol e=(3,0,0)、\boldsymbol f=(0,3,0),所以

\begin{bmatrix}9&0\\0&9\end{bmatrix}\begin{bmatrix}u\\v\end{bmatrix}=\begin{bmatrix}3\\3\end{bmatrix},\qquad u=v=\frac13,\qquad\beta=(1/3,1/3,1/3).

三项均为正,投影位于面内。重建得到 \boldsymbol x=\boldsymbol a+\boldsymbol e/3+\boldsymbol f/3=(0,0,1),最近距离为 1。该结果也与三角形所在平面 z=1 的距离一致。

5. GJK 的区间与平方间隙

某轮当前最近点为 \boldsymbol x=(2,1)\,\mathrm m,新支持点为 \boldsymbol s=(1,-1)\,\mathrm m。求本轮距离上下界和 G。仅判断相交时能否结束?要求距离误差小于 0.01\,\mathrm m 时能否结束?

解答分离证据与精度要求
U=\sqrt5\simeq2.23607\,\mathrm m,\quad \boldsymbol x^{\mathsf T}\boldsymbol s=1\,\mathrm{m^2},\quad L=\frac1{\sqrt5}\simeq0.44721\,\mathrm m.

下界为正,已经证明两形体分离。距离区间宽度仍为 U-L=4/\sqrt5\simeq1.78885\,\mathrm m,需要继续查询才能满足给定精度。

G=(2,1)\cdot((2,1)-(1,-1))=(2,1)\cdot(1,2)=4\,\mathrm{m^2}.

平方间隙标准要求 G\le0.01^2/2=0.00005\,\mathrm{m^2},本轮也未满足。两种判断分别回答几何分离和距离精度问题。

6. EPA 的停止与穿透符号

EPA 当前最近面距离为 0.3\,\mathrm m,该方向支持投影为 0.34\,\mathrm m。下一轮得到 L=0.335\,\mathrm m、U=0.338\,\mathrm m。长度容差为 0.005\,\mathrm m,判断两轮是否结束,并给出最终有符号距离区间。

解答正深度区间到负距离区间

第一轮区间宽度为 0.04\,\mathrm m,继续扩张;第二轮为 0.003\,\mathrm m,满足容差。因此

0.335\le\delta\le0.338\,\mathrm m,\qquad -0.338\le d\le-0.335\,\mathrm m.

按最近面恢复见证点时,采用当前近似 \hat\delta=L,有 \boldsymbol p_A-\boldsymbol p_B=L\boldsymbol n。报告距离 \hat d=-L,两点关系就保持 \boldsymbol p_B-\boldsymbol p_A=\hat d\boldsymbol n,距离误差不超过当前区间宽度。

7. 接触坐标中的功率

使用第七节球—平面的接触矩阵,相对速度为 (0.8,0,-0.1)\,\mathrm{m/s}。接触坐标力为 \boldsymbol f_c=(20,-2,0)\,\mathrm N,求世界力,并在两套坐标中核对功率。

解答转置回传与点积相等
\boldsymbol f=C^{\mathsf T}\boldsymbol f_c=\begin{bmatrix}-2\\0\\20\end{bmatrix}\mathrm N,\qquad \boldsymbol v_c=C\boldsymbol v=\begin{bmatrix}-0.1\\0.8\\0\end{bmatrix}\mathrm{m/s}.
\boldsymbol f^{\mathsf T}\boldsymbol v=(-2)(0.8)+20(-0.1)=-3.6\,\mathrm W,\qquad \boldsymbol f_c^{\mathsf T}\boldsymbol v_c=20(-0.1)+(-2)(0.8)=-3.6\,\mathrm W.

负功率表示这一瞬时接触力组合从相对运动中取出能量。其法向和切向分量分别贡献 -2\,\mathrm W 与 -1.6\,\mathrm W。

8. 接触阈值与程序检查

半径为 0.2\,\mathrm m 的球依次处于 z=0.23,0.222,0.215,0.19\,\mathrm m。采用本章 margin 与 gap,逐项求几何距离、位置误差、候选状态和激活状态,再用 plane_circle 核对。

解答高度、间隙与恢复误差

逐项使用 d=z-0.2 与 r_n=d-0.02:

\begin{array}{c|r|r|c|c}z\ (\mathrm m)&d\ (\mathrm m)&r_n\ (\mathrm m)&\text{候选}&\text{激活}\\\hline0.230&0.030&0.010&\text{否}&\text{否}\\0.222&0.022&0.002&\text{是}&\text{否}\\0.215&0.015&-0.005&\text{是}&\text{是}\\0.190&-0.010&-0.030&\text{是}&\text{是}\end{array}
from collision2d import plane_circle

for z in (0.230, 0.222, 0.215, 0.190):
    c = plane_circle((0, z), 0.2, margin=0.02, gap=0.005)
    print(z, round(c["signed_distance"], 3),
          round(c["residual"], 3), c["candidate"], c["active"])

在精确激活边界上,判定使用严格小于;在球—平面候选边界上,使用小于等于。程序校验边界时应留意十进制输入的浮点表示,可以直接对距离值构造测试,并为输出的数学比较留出舍入误差范围。