碰撞检测GJK算法论文解析三
这里要先纠正上篇文章的一些错误,就是上篇文章的最后其实并没有证明定理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的主要内容已经阐述得差不多了,后面我会把第六部分也梳理出来,第六部分主要是让整个算法更加健壮。然后我会加上一个简单的例子,让大家能更好的理解。