碰撞检测有2个经典算法,一个是分离轴算法SAT,还有一个就是本文要介绍的GJK,GJK是三个人的名字首写大字母;这个算法的数学推导有点复杂,目前网上只有这篇
https://blog.csdn.net/u010016421/article/details/104788769/
相对比较详细的介绍了算法,其主要也是参考原始论文翻译的;考虑到原始论文比较难读,博主特地专门写一篇文章介绍,博主的这篇文章会必要的地方稍微简化一下
但是尽管如此,由于这个算法涉及到的数学概念较多,读者最好有一定的线性空间和线性代数的知识,否则的话会理解比较困难。
由于这篇文章没有绘图,全是公式推导,读者容易看着一头雾水,因此,先对整个算法的思路做一个概述;
1,首先,算法基于闵可夫斯基差(也是一个凸体),将两个凸体是否相交的问题转化为一个凸体是否包含原点的问题
2,巧妙的地方在于,该算法又不需要直接计算两个凸体的闵可夫斯基差,只需要一个和闵可夫斯基差有关的参数,而这个参数,直接利用原来的两个凸体就可以计算;
3,算法的整体思想是迭代进行的,我认为该算法实际上是数学上单纯形法的一种应用方式
4,3维的单纯形是一个四面体,2维度是一个三角形,但该三角形可以是三维空间的三角形;
5,为何要用单纯形,个人认为是出于数学处理上的方便,给定单纯形的顶点集,则单纯形本身可以用一个关于顶点对称的线性方程来描述集合;同时,单纯形上的点到原点的距离可以有很好的数学表示
6,算法其实是不断的构建 包含于闵可夫斯基差(这里只需理解为一个凸体)的单纯形,并使得原点到单纯形的距离越来越小;最终达到的状态就是单纯形到原点的距离就是闵可夫斯基差到原点的距离,
7,对于3维度空间而言,如果把闵可夫斯基差想象为一个四面体,则迭代最终的单纯形可能就是它的一个面,原点到这个面的距离就是原点到这个四面体的距离
8,由于单纯形到原点的距离是一个限制最优问题,所以文章先求出仿射空间到原点距离(非限制最优),如果单纯形是个三角形,则其仿射空间,就是这个三角形所在的平面,实际上就转化为平面到原点距离问题
9,通过线性方程,上述问题可以转化而为函数极值问题,其解可以有一个线性方程组来描述;
10,定理3给出了该方程组的行列式和代数余子式的递归构建方法,并且给出了判别准则:当这些行列式,和代数余子式满足什么关系时,对应的单纯形满足:该单纯形到原点的距离就是整个闵可夫斯基差到原点的距离,并且这是一个维度尽可能低的单纯形
下面开始正文
为了简化,本文将采用爱因斯坦求和表示法,
先给出一些概念定义:
1,:表示所有实数的集合
2, : 由形如的元组组成的集合,其中
3, 把中的元素看成向量,则可以向量加法以及数乘运算,则连同这两个运算可以看成系数域上的向量空间
4,凸体:(这里仅限为中的凸体) 的一个子集,其中任意两点连成的线段都属于该子集,则该子集就是一个凸体
5,集合的凸包:包含的最小凸体
集合的凸包用 来表示
6,集合和的距离:
定义为
7,闵可夫斯基和:
向量空间中的两个子集的闵可夫斯基和为:
闵可夫斯基差:
8,线性无关:一组向量线性无关当且仅当方程
只有唯一解
9,仿射无关:一组向量仿射无关当且仅当 线性无关
10,单纯形:仿射无关向量组成的集合的凸包
11,符号表示集合中距离原点最近的点(当K是凸体时是唯一的)
算法输入:中的两个凸体
输出:两个凸体的间距d
显然,该算法可以判断两个凸体是否相交,也就是根据输出d是否为0即可判断
算法思路:
记两个凸体为,显然,若两者相交,则其闵可夫斯基差必然包含原点;反之如果闵可夫斯基差包含原点,则必然相交
因此原问题转化为判断的闵可夫斯基差是否包含原点的问题
(容易证明(略):两个凸体的闵可夫斯基差也是凸体)
我们记为 的闵可夫斯基差
该算法没有直接计算闵可夫斯基差,而是想办法构造一个集合序列:
使得该序列中每个集合都是的子集,并且原点到的距离(见定义6,单个点看成由单个点组成的集合)逐渐变小,直到最终得到最小值,也就是目标值d;
我们限定在空间中讨论,构造函数如下:
这个函数的含义是,在集合中找到一个点,使得在方向上的有向投影值h最大,那么就是函数的值,
对应的使得函数取最大值的记作 ,有
算法并没有显示的构建闵可夫斯基差,而是计算,但是计算并不需要,利用可以计算
公式为:
在陈述算法之前,先证明一个定理:
定理1:设 是紧致凸集,定义为
那么有:
1)如果,存在一个点使得
2) 当且仅当
3) (据此,必有)
证明:
(1) 代入表达式,得到:
过点做超平面与垂直,意味着,夹角<90度,则如果该超平面的法向取为何同向,则位于该超平面的背面,从几何角度可以看出的存在性;
(2)根据1)如果,则意味着在超平面上,由于也在该超平面上,且与垂直,根据的定义,在中所有点中,它是投影到方向的有向长度最大的,而这个长度等于在方向投影的有向长度,因此超平面的背面没有中的点,且与超平面垂直,则;
反之,若 仍然根据超平面的性质,在超平面上的有向投影就是,则
(3) 根据2)中结论,
=> =>
任意
则
(,
与同向)
算法流程如下,给定紧致凸集,以及个位于中的初始点,执行如下操作:
1,令
2, 求出
3,计算,如果=0,令(根据定理1-2),算法结束;
4,计算是具备下述性质的集合:
(1) ,
(2),
(3)仿射无关且元素个数
5,令 ,令,返回第2步
关于的存在性证明暂时略过
下面说明,该算法的递降性得以保证,也就是
证明:
(根据定理1-1)
定理2:假设是一个凸多面体的顶点集,,则上述算法作用于将在有限步之后终止
证明:显然是有限集,设其非空子集个数为N,
根据以上分析,在算法迭代过程中,有
根据的唯一性,必有(i!=j),可是
,则其可能的不同个数是有限的,矛盾;
在上述算法中,第二步,要计算 ,以及第四步要计算,下面来说明这两个的计算方法:
不妨设 ,利用我们的方法,我们最终将得到如下形式的表达(这三个式子整体在后面称作 目标表达式):
1,
2,
3, 仿射独立
(实际上,从几何学的角度来看,的含义是,找到包含v(co(Y))的最小顶点集凸包,对于三维空间可以举一个特例如下:假设Y是一个单纯形(4面体)的顶点集,则如果v(co(Y))位于该四面体内部,则Y_I为4个顶点集合, 如果v(co(Y))位于某个面上而不在任意一个边上,则Y_I为这个面的3个顶点集,如果v(co(Y))位于某条边上而不在任何顶点,则Y_I为这个边的两个顶点,如果v(co(Y))位于顶点上,则Y_I就是这个顶点)
很显然,以上表示已经覆盖了 和,
由于的非空子集个数为 ,令取遍的非空子集, 则其中必有一个满足上述条件,下面我们给出一种方法,该方法对于每个求出一组对应的参数,并使得其中必有一个满足上述条件,这个方法如下:
用表示关于的补集
()
并且给出定理3:
目标表达式成立的充分必要条件是:
(1)
(2)
(3)
下面给出证明:
对于 ,我们来计算
其中
假设的元素个数为r,其元素为
则必有:
将 代入,并化简,得到
由于使得 取极小值,则必有
取极小值,
则有,显然该偏导数只与和有关的项有关系
将上式展开,合并同类项,其中和有关的项为:
求偏导并令其为0,得到方程:
=>
=>
(其中取遍)
=> (利用 )
(取遍)
=>
令取遍历
我们将得到,以下方程组:
其中,
从对称性可以看出,一般的,只要有:
(取遍)
下面我们计算上述矩阵的行列式:,
先复习一下矩阵的初等变换性质,
(1)将矩阵某行(列)乘以一个系数加到矩阵另外一行(列),矩阵的行列式不变;
(2)矩阵某行(列)乘以一个系数,行列式的值也乘以该系数
(3)交换矩阵两行(列),矩阵行列式变号
将第一行乘以加到第行得到(行列式不变):
按照第一行第一列展开,得到:
令矩阵
则有
(第行,列元素都为 )
我们设
显然
现在我们来看看对执行初等变换,将如何影响,稍加分析,有以下结论:
(1)对执行行变换,等价于对执行相同的行变换,
对执行列变换,等价于对执行相同的列变换
该结论记为定理X
现在,由于以上矩阵的推导是按照排序的顺序推导出来的,而是是中元素任意一个排序,现在我们研究,如果改变排序方法,对上述矩阵行列式的影响;先只考虑交换2个元素的情形,
(1)与的编号交换,都不等于1
此时,新矩阵相当于原矩阵,交换和的情形:
我们观察
如果把互换,可以等价为对该矩阵进行了一次行交换和一次列交换,则该矩阵的行列式在这个过程中值不变;
(2)与交换
由于我们关心的是行列式的值,我们利用:
将其写成 形式有:
现在将与交换,设变为,并且对应的展开为,则
现在我们对进行初等变换,根据定理X,只需要对和进行相应的行,列初等变换即可;
我们对进行如下行变换(等价于对A_I'进行相同行变换)
1)将所在行 ,A_I'行列式累积变号1次
- 再将其分别加到其余行, 则A'变为A, A_I'行列式累积变号1次
对进行如下行变换: - 将所在列 ,A_I'行列式累积变号2次
- 将其分别加到其余列, 则B'变为B,行列式累积变号2次;
综上,A_I'=A'B'进行以上变换 变为 A_I=AB,并且行列式保持不变;
由于任何排序均可以看成依次执行交换两个元素的变换组合而成,也就是证明了与选择的排序
那么我们可以记
令为中的(设,由于是取的子集,其下标可能会变化)所在列的第一行元素的代数余子式,
根据线性方程组解公式,有:
由于无论如何修改的排序,显然关联到的值不变,则对应不变
则的定义合法
根据以上的推理,我们还可以得到 引理1:
仿射独立等价于
这一点根据 的分解可以得到
至此,我们可以来证明定理3了,先把定理3相关公式罗列一遍:
用表示关于的补集
()
目标表达式
1,
2,
3, 仿射独立
成立的充分必要条件是:
(1)
(2)
(3)
首先,我么证明我们定义的与满足定理3的递归等式:
1)是平凡结论,显然成立
2) ()
将对应的矩阵列出为:(令)
计算其第一行最后一列的代数余子式(对应),为:
根据对称性,可以用任意替换) ,即证
3)
根据线性代数行列式计算法则,显然成立;
现在我们证明定理3的充分性:
几何上显然的事实是,等价于:
() ;
令,
首先,根据引理1:=>;
并且显然,根据之前推导 具备右端的形式;
同时利用,,, ,可以得到:
(将A4 代入A2)
将遍历中元素,每项乘以求和,得到:
同时,当时 ,根据
综上
同时,对任意,有方程:
则
则
根据的任意性,可令
则
根据定理1 :
下面证明必要性:
1,显然(B3)=>(C1) (B2),(A4),(C1)=>(C2) ,
只需要证明
假设 成立
根据定理1的结果 2,
有 =>
则对任意 ,因为 ,则
由于,而有恒等式:
(根据B1,点乘右边为0)
则必有:
根据(D1), ,再结合引理1,得到;
结合(B2)(C1)(A4),得到 (C2)
最后,根据有(D1):
将 减去
得到:
再根据(A2),(C2),得到(C3)