碰撞检测GJK算法论文解析三
- 再探Appendix Ⅱ
- 内容详解
- 再探The Distance Subalgorithm
- 内容详解
- 过程1
- 过程2
- 过程3
这里要先纠正上篇文章的一些错误,就是上篇文章的最后其实并没有证明定理3,而只是给出了仿射集系数向量 λ \lambda λ的解的形式,而没有进一步说明如何从定理3的各个条件推导出结论;然后是之前把余子式和代数余子式的概念混杂在一起了,所以文中有些描述也错了。对于以上两点疏忽,我向大家道个歉。我修改了上篇文章的一些描述,整体内容没有变化,所以这篇文章继续从附录二开始,然后讲完第五部分。
为防止混乱,我这里先说明一下,文章内有时候会用 y i y_i yi表示点,有时候又会用 x i x_i xi表示点,但无论如何,只要是同一类型的符号就代表是在同一个集合内的,关键是抓住定义,所以不用纠结用的是哪一个符号。
再探Appendix Ⅱ
内容详解
在上一篇文章的最后,我们得到了仿射集系数 λ i \lambda^i λi的形式,即下式:
λ i = Δ i ( Y S ) / Δ ( Y S ) \lambda^i = \Delta_i(Y_S) / \Delta(Y_S) λi=Δi(YS)/Δ(YS)然后进入引理1的分析。
引理1:当且仅当 Y S Y_S YS是仿射独立时,行列式 Δ ( Y S ) > 0 \Delta(Y_S) \gt 0 Δ(YS)>0。要证明这条引理,首先需要回顾(40)式和(41)式:
然后将 A S A_S AS第一列除第一个元素外都变成0,也就是论文里提到的第二行之后每一行加上 ( x 1 − x i ) ⋅ x 1 (x_1 - x_i) · x_1 (x1−xi)⋅x1(看到 ( x 1 − x i ) ⋅ x 1 (x_1 - x_i) · x_1 (x1−xi)⋅x1和第一列的元素有什么区别吗?就是差一个负号)。这样的话 A S A_S AS就会变成下面的矩阵:
∣ 1 1 . . . 1 0 ( x 2 − x 1 ) ⋅ x 2 − ( x 2 − x 1 ) ⋅ x 1 . . . ( x 2 − x 1 ) ⋅ x r − ( x 2 − x 1 ) ⋅ x 1 . . . . . . . . . . . . 0 ( x r − x 1 ) ⋅ x 2 − ( x r − x 1 ) ⋅ x 1 . . . ( x r − x 1 ) ⋅ x r − ( x r − x 1 ) ⋅ x 1 ∣ = ∣ 1 1 . . . 1 0 ( x 2 − x 1 ) ⋅ ( x 2 − x 1 ) . . . ( x 2 − x 1 ) ⋅ ( x r − x 1 ) . . . . . . . . . . . . 0 ( x r − x 1 ) ⋅ ( x 2 − x 1 ) . . . ( x r − x 1 ) ⋅ ( x r − x 1 ) ∣ \begin{vmatrix} 1 & 1 & ... & 1 \\ 0 & (x_2 - x_1)·x_2- (x_2 - x_1)·x_1& ... & (x_2 - x_1)·x_r- (x_2 - x_1)·x_1\\ ...& ... & ... & ...\\ 0 & (x_r - x_1)·x_2- (x_r - x_1)·x_1 & ... & (x_r - x_1)·x_r- (x_r - x_1)·x_1 \end{vmatrix} = \\ \begin{vmatrix} 1 & 1 & ... & 1 \\ 0 & (x_2 - x_1)·(x_2-x_1)& ... & (x_2 - x_1)·(x_r- x_1)\\ ...& ... & ... & ...\\ 0 & (x_r - x_1)·(x_2- x_1) & ... & (x_r - x_1)·(x_r- x_1) \end{vmatrix} ∣∣∣∣∣∣∣∣10...01(x2−x1)⋅x2−(x2−x1)⋅x1...(xr−x1)⋅x2−(xr−x1)⋅x1............1(x2−x1)⋅xr−(x2−x1)⋅x1...(xr−x1)⋅xr−(xr−x1)⋅x1∣∣∣∣∣∣∣∣=∣∣∣∣∣∣∣∣10...01(x2−x1)⋅(x2−x1)...(xr−x1)⋅(x2−x1)............1(x2−x1)⋅(xr−x1)...(xr−x1)⋅(xr−x1)∣∣∣∣∣∣∣∣等式的右边是不是很有规律?它可以变成 Q S T Q S Q^T_SQ_S QSTQS,其中 Q S Q_S QS是矩阵:
这里要注意, x i x_i xi是列主序的向量,所以 Q S Q_S QS确实是一个矩阵。当然你可以把 x i x_i xi都拆开再进行矩阵的乘法,不过得到的是一样的结果。根据行列式的计算公式有: d e t ( A S ) = d e t ( Q S T Q S ) det(A_S) = det(Q^T_SQ_S) det(AS)=det(QSTQS),所以 d e t ( A S ) ⩾ 0 det(A_S) \geqslant 0 det(AS)⩾0,并且矩阵 r a n k ( A S ) = r a n k ( Q S ) rank(A_S) = rank(Q_S) rank(AS)=rank(QS)(rank代表矩阵的秩),如果 d e t ( A S ) > 0 det(A_S) \gt 0 det(AS)>0那么代表 r a n k ( A S ) = r a n k ( Q S ) = r − 1 rank(A_S) = rank(Q_S) = r-1 rank(AS)=rank(QS)=r−1,又因为矩阵 A S A_S AS内的元素是集合 Y S Y_S YS内的点的线性组合,那么必然有 d i m Y = r − 1 dim Y = r - 1 dimY=r−1。回到第一篇文章,我们可以知道,如果 d i m a f f Y = d i m Y = r − 1 dim \space aff Y = dim Y= r - 1 dim affY=dimY=r−1,那么说明 Y S Y_S YS是仿射独立的。至此,引理1证明完毕。
之前提到(30)式有很多新出现的符号,我们还未了解,所以不讲。但是现在我们已经有足够的知识去理解了,而且接下来的内容也需要用到(30)式,所以接下来先返回到第五部分的(30)式,梳理完之后会回到第五部分,这里主要是接着上面的思路,所以没有把(30)式的分析放到第五部分。
(30)式内容如下:
这里面有三个式子,这里分别称为30.1、30.2以及30.3。(30.1)式我自己是觉得有点疑惑的,与其说是公式,不如说是补充定义,因为只有一个点的话计算这个值也没有什么意义。(30.3)式应该比较好理解,这个就是使用代数余子式计算行列式的公式。关键是(30.2)式,简单来说就是按照(41)式的规律,往 A S A_S AS矩阵加入 y j y_j yj向量后,再取第一行 y j y_j yj那列的代数余子式,这里以2阶的 A S A_S AS矩阵为例
A S = ∣ 1 1 ( y 2 − y 1 ) ⋅ y 1 ( y 2 − y 1 ) ⋅ y 2 ∣ A_S = \begin{vmatrix} 1 & 1 \\ (y_2 - y_1)·y_1 & (y_2 - y_1)·y_2 \end{vmatrix} AS=∣∣∣∣1(y2−y1)⋅y11(y2−y1)⋅y2∣∣∣∣那么加入 y j y_j yj后的矩阵 A S j A^j_S ASj则为:
A S j = ∣ 1 1 1 ( y 2 − y 1 ) ⋅ y 1 ( y 2 − y 1 ) ⋅ y 2 ( y 2 − y 1 ) ⋅ y j ( y j − y 1 ) ⋅ y 1 ( y j − y 1 ) ⋅ y 2 ( y j − y 1 ) ⋅ y j ∣ A^j_S= \begin{vmatrix} 1 & 1 & 1 \\ (y_2 - y_1)·y_1 & (y_2 - y_1)·y_2 & (y_2 - y_1)·y_j \\ (y_j - y_1)·y_1 & (y_j - y_1)·y_2 & (y_j - y_1)·y_j \\ \end{vmatrix} ASj=∣∣∣∣∣∣1(y2−y1)⋅y1(yj−y1)⋅y11(y2−y1)⋅y2(yj−y1)⋅y21(y2−y1)⋅yj(yj−y1)⋅yj∣∣∣∣∣∣
然后根据上一篇文章的内容,可以知道:
Δ j ( Y S ⋃ { y j } ) = ( − 1 ) ( 1 + 3 ) d e t ∣ ( y 2 − y 1 ) ⋅ y 1 ( y 2 − y 1 ) ⋅ y 2 ( y j − y 1 ) ⋅ y 1 ( y j − y 1 ) ⋅ y 2 ∣ \Delta_j(Y_S \bigcup \lbrace y_j \rbrace) = (-1)^{(1 + 3)}det\begin{vmatrix} (y_2 - y_1)·y_1 & (y_2 - y_1)·y_2 \\ (y_j - y_1)·y_1 & (y_j - y_1)·y_2 \\ \end{vmatrix} Δj(YS⋃{yj})=(−1)(1+3)det∣∣∣∣(y2−y1)⋅y1(yj−y1)⋅y1(y2−y1)⋅y2(yj−y1)⋅y2∣∣∣∣如果取最后一行的代数余子式来进行行列式计算,那么:
Δ j ( Y S ⋃ { y j } ) = ( − 1 ) ( 1 + 3 ) ∗ ( − 1 ) ( 1 + 2 ) ( ( y j − y 1 ) ⋅ y 1 ) ( ( y 2 − y 1 ) ⋅ y 2 ) + ( − 1 ) ( 1 + 3 ) ∗ ( − 1 ) ( 1 + 3 ) ( ( y j − y 1 ) ⋅ y 2 ) ( ( y 2 − y 1 ) ⋅ y 1 ) ( a ) \Delta_j(Y_S \bigcup \lbrace y_j \rbrace) = (-1)^{(1 + 3)}* (-1)^{(1 + 2)}((y_j - y_1)·y_1)((y_2 - y_1)·y_2) + (-1)^{(1 + 3)}* (-1)^{(1 + 3)}((y_j - y_1)·y_2)((y_2 - y_1)·y_1) \kern3em \lparen a \rparen Δj(YS⋃{yj})=(−1)(1+3)∗(−1)(1+2)((yj−y1)⋅y1)((y2−y1)⋅y2)+(−1)(1+3)∗(−1)(1+3)((yj−y1)⋅y2)((y2−y1)⋅y1)(a)然后我们仔细分析一下上面这个式子,有没有发现,其实 Δ 1 Y S = ( − 1 ) ( 1 + 1 ) ( ( y 2 − y 1 ) ⋅ y 2 ) \Delta_1Y_S=(-1)^{(1 + 1)}((y_2 - y_1)·y_2) Δ1YS=(−1)(1+1)((y2−y1)⋅y2)而 Δ 2 Y S = ( − 1 ) ( 1 + 2 ) ( ( y 2 − y 1 ) ⋅ y 1 ) \Delta_2Y_S=(-1)^{(1 + 2)}((y_2 - y_1)·y_1) Δ2YS=(−1)(1+2)((y2−y1)⋅y1)
那么(a)式可以写成:
Δ j ( Y S ⋃ { y j } ) = − 1 ∗ Δ 1 Y S ( ( y j − y 1 ) ⋅ y 1 ) + ( − 1 ) ∗ Δ 2 Y S ( ( y j − y 1 ) ⋅ y 2 ) \Delta_j(Y_S \bigcup \lbrace y_j \rbrace) = -1*\Delta_1Y_S((y_j - y_1)·y_1) + (-1)*\Delta_2Y_S((y_j - y_1)·y_2) Δj(YS⋃{yj})=−1∗Δ1YS((yj−y1)⋅y1)+(−1)∗Δ2YS((yj−y1)⋅y2)这个就是(30.2)式!理解(30.2)式非常重要,因为这是证明定理3的一个关键。想要进一步熟悉的不妨推导一下4阶的 A S A_S AS矩阵。
再探The Distance Subalgorithm
内容详解
有了上面几篇文章的基础,现在我们终于能去证明定理3了,这真的是一个漫长的过程,但也代表我们快要看到曙光了。这一部分虽然是在说第五部分,但是实际上证明的主题还是在附录二上,这样安排是为了思路能够更加连贯(我自己也不想每隔几行就插个标题,然后跳来跳去)。首先我们来看一下定理3的描述。
定理3:当且仅当
\kern3em 1) Δ ( Y S ) > 0 \Delta(Y_S) \gt 0 Δ(YS)>0;
\kern3em 2) ∀ i ∈ I S , Δ i ( Y S ) > 0 \forall i \in I_S,\Delta_i(Y_S) \gt 0 ∀i∈IS,Δi(YS)>0;
\kern3em 3) ∀ i ∈ I S ′ , Δ j ( Y S ⋃ { y j } ) ⩽ 0 \forall i \in I_S',\Delta_j(Y_S \bigcup \lbrace y_j \rbrace) \leqslant 0 ∀i∈IS′,Δj(YS⋃{yj})⩽0
时,集合 Y Y Y的凸包离原点最近的点能够以(29)式的形式进行表示。这里要注意的是 I S ′ I_S' IS′表示的是 I S I_S IS的补集,也就是说如果 Y = { y 1 , y 2 , y 5 , y 6 } Y = \lbrace y_1, y_2, y_5, y_6\rbrace Y={y1,y2,y5,y6},那么 I = { 1 , 2 , 5 , 6 } I = \lbrace 1, 2, 5, 6\rbrace I={1,2,5,6},然后 Y S = { y 1 , y 5 , y 6 } Y_S = \lbrace y_1, y_5, y_6\rbrace YS={y1,y5,y6},那么 I S = { 1 , 2 , 6 } I_S = \lbrace 1, 2, 6\rbrace IS={1,2,6}, I S ′ = { 5 } I_S' = \lbrace 5 \rbrace IS′={5}。下面是证明过程。
过程1
首先是根据(41)式,当且仅当下面式子成立:
y ⋅ ( y − y k ) = 0 ∀ k ∈ I S ( 43 ) y · (y - y_k) = 0 \space \space \forall k \in I_S \kern3em \lparen 43\rparen y⋅(y−yk)=0 ∀k∈IS(43)才有 y = v ( a f f Y S ) y =v(aff\space Y_S) y=v(aff YS)。这怎么得到的?我们回到(41)式,然后用 A S A_S AS矩阵的第二行去和 λ \lambda λ向量相乘,得到:
λ 1 ( x 2 − x 1 ) ⋅ x 1 + λ 2 ( x 2 − x 1 ) ⋅ x 2 + . . . + λ r ( x 2 − x 1 ) ⋅ x r = 0 \lambda^1 (x_2 - x_1)·x_1 + \lambda^2 (x_2 - x_1)·x_2 + ... +\lambda^r (x_2 - x_1)·x_r = 0 λ1(x2−x1)⋅x1+λ2(x2−x1)⋅x2+...+λr(x2−x1)⋅xr=0合并一下多项式,得到:
( λ 1 x 1 + λ 2 x 2 + . . . + λ r x r ) ⋅ ( x 2 − x 1 ) (\lambda^1 x_1 + \lambda^2 x_2 + ... +\lambda^r x_r)·(x_2 - x_1) (λ1x1+λ2x2+...+λrxr)⋅(x2−x1)根据(12)式,以及 y y y的定义,能得到:
y ⋅ ( x 2 − x 1 ) = 0 ( 1 ) \kern3em y·(x_2 - x_1) = 0 \kern3em(1) y⋅(x2−x1)=0(1)
同样的,我们能够得到以下式子:
y ⋅ ( x 3 − x 1 ) = 0 ( 2 ) \kern3em y·(x_3 - x_1) = 0 \kern3em(2) y⋅(x3−x1)=0(2)
. . . \kern3em\kern3em ... ...
y ⋅ ( x r − x 1 ) = 0 ( r − 1 ) \kern3em y·(x_r - x_1) = 0 \kern3em(r - 1) y⋅(xr−x1)=0(r−1)
然后将这r个式子乘以对应的系数 λ i + 1 \lambda^{i + 1} λi+1再相加,我们能得到:
y ⋅ ( λ 2 x 2 + . . . λ r x r − ( λ 2 + . . . λ r ) x 1 ) = 0 y·(\lambda^2x_2 + ...\lambda^rx_r - (\lambda^2+...\lambda^r)x_1) = 0 y⋅(λ2x2+...λrxr−(λ2+...λr)x1)=0然后 λ 2 x 2 + . . . λ r x r = y − λ 1 x 1 \lambda^2x_2 + ...\lambda^rx_r = y - \lambda^1x_1 λ2x2+...λrxr=y−λ1x1, ( λ 2 + . . . λ r ) = 1 − λ 1 (\lambda^2+...\lambda^r) = 1-\lambda^1 (λ2+...λr)=1−λ1,综合这些式子就能得到(43)式,从而也说明了 y = v ( a f f Y S ) y =v(aff\space Y_S) y=v(aff YS)。
过程2
设 y = v ( a f f Y S ) y =v(aff\space Y_S) y=v(aff YS),并且假定定理3的1)和2)都满足,那么从引理1以及(42)式出发,我们可以用(29)式和(31)式来表示 y y y,具体原因其实在文章叙述的过程中已经讲解了,这里就不再阐述。
过程3
定理3的条件1)和3)、(42)式加上(30)式表明 y ⋅ ( y k − y j ) ⩽ 0 y · (y_k - y_j) \leqslant 0 y⋅(yk−yj)⩽0,这个代入公式应该也不难理解,所以这里也不再阐述。
综合过程1、2、3,我们能得到以下信息:对于 ∀ x ∈ c o Y \forall x \in coY ∀x∈coY,我们有:
x = ∑ i = 1 v α i y i , ∑ i = 1 v α i = 1 , α i ⩾ 0 x = \sum^v_{i = 1}\alpha^iy_i, \space \sum^v_{i = 1}\alpha^i = 1, \alpha^i \geqslant 0 x=i=1∑vαiyi, i=1∑vαi=1,αi⩾0这里的点的符号又变了,但不需要太纠结,抓住 ∀ x ∈ c o Y \forall x \in coY ∀x∈coY就好,上面的式子就是(12)式。那么我们可以得到下面的式子:
y ⋅ ( y − x ) = ∑ i = 1 v α i y ⋅ ( y − y i ) ⩽ 0 y·(y - x) = \sum^v_{i = 1}\alpha^iy·(y - y_i) \leqslant 0 y⋅(y−x)=i=1∑vαiy⋅(y−yi)⩽0然后我们根据(43)式可以知道上面的式子等于0,那么这代表了 g c o Y ( y ) = 0 g_{coY}(y) = 0 gcoY(y)=0, g K ( x ) g_K(x) gK(x)的定义请参照(25)式,从而说明y就是离原点最近的点,即 v ( c o Y ) = y v(co Y) = y v(coY)=y。
可能大家会有点晕,最后我梳理一下,简单来说就是使用定理3来找距离原点最近的点,就是上面提到的 y y y,只要满足定理3,那么这个点就是我们要找的点。
那么,我们终于可以写出这个子算法的过程了:
Distance Subalgorithm:对有限集 Y = { y 1 , . . . , y v } ⊂ R m Y = \lbrace y_1,...,y_v \rbrace \subset R^m Y={y1,...,yv}⊂Rm,按一定的索引集构造集合 Y s Y_s Ys,其中 s = 1 , 2 , . . . , σ s = 1, 2, ..., \sigma s=1,2,...,σ, σ \sigma σ的定义为:
σ = ∑ k = 1 v [ v ! / ( k ! ( v − k ) ! ) ] \sigma = \sum^v_{k = 1}[v! / (k!(v - k)!)] σ=k=1∑v[v!/(k!(v−k)!)](这里要注意的是 s s s是一个数字,结合 σ \sigma σ的定义可知,在有4个点的情况下,我们可以设定 s = 1 s=1 s=1时, Y s = { y 1 } Y_s=\lbrace y_1\rbrace Ys={y1}; s = 2 s=2 s=2时, Y s = { y 2 } Y_s=\lbrace y_2\rbrace Ys={y2}; s = 15 s=15 s=15时, Y s = { y 1 , y 2 , y 3 , y 4 } Y_s=\lbrace y_1, y_2, y_3, y_4\rbrace Ys={y1,y2,y3,y4}),然后按照下面的步骤进行处理:
\kern3em 1.s = 1;
\kern3em 2.如果 Y s Y_s Ys中的顶点满足定理3的所有条件,那么使用(29)式以及(31)式对 v ( c o Y ) v(coY) v(coY)进行表示并且返回结果;
\kern3em 3.如果 s < σ s \lt \sigma s<σ,那么s = s + 1,然后返回到步骤1继续执行;
\kern3em 4.如果上述步骤都没有得到结果,说明算法出了问题,返回错误。
到此,GJK的主要内容已经阐述得差不多了,后面我会把第六部分也梳理出来,第六部分主要是让整个算法更加健壮。然后我会加上一个简单的例子,让大家能更好的理解。