GJK算法计算凸多边形之间的距离
GJK是空间距离检测算法，是由三位（Gilbert, Johnson, and Keerthi ）发明者的首字母组成的代称。
GJK算法首先要解决计算Minkowski和的问题。所谓Minkowski和，指A、B两个集合，
令A+B={x+y，其中x属于A，y属于B}即二者的Minkowski和。类似的可以定义负集与Minkowski差。
若A、B为凸多边形，顶点个数分别是n、m，则他们的Minkowski和一定是凸多边形且至多有n+m个顶点。
左上为多边形A、B，假设黑点为原点，三个图分别表示-B、A+B与A-B。A、B之间的点对距离即||x - y||，其中x属于A，y属于B；而A、B之间的距离就是||x-y||的最小值。因此，A、B之间的距离就是其Minkowski差A-B中的最小元素，这个最小指的是模最小，实际上就是距离原点的距离。因此，2个凸多边形的距离转化为点到凸多边形的距离，点指的是原点，凸多边形指的是原题中的2个凸包的Minkowski差。
Minkowski和的算法如下，将A、B的边按逆时针方向拆成向量（顺时针实际上也可以），如上图可以得到6个向量。将这些向量按极角排序，然后依次首尾相连即可得到凸包。
上述算法只是得到了和的形状与相对位置（因为首尾相连时出发的基点是原点），实际上该凸包还需做一个平移才能得到正确的坐标。如果排序时极角是取0~360度范围（NOTE：不必显式的求出极角，用先象限后叉积的方法排序），则求出A、B各自的最下最左点，将其坐标相加作为出发的基点即可。代码实现上这一点其实非常容易完成。
求出Minkowski差之后（求差与求和本质是一样的），剩下的就是求点到凸多边形的距离，这个问题又转化为求点到边的距离。一个凸多边形有n条边，点到这n条边的距离的最小值就是点到凸多边形的距离。而且，点到边的距离是具有确定单调性的，因此运气好的话不需要求出所有边的距离，只需扫描到极小值即可。点到边的距离也就是点到线段的距离，利用叉积计算、点积进行判断，很容易求得。
上述算法其实不是GJK算法，因为所求为凸多边形，而GJK算法可以用来求曲线凸包之间的距离（此情况下是一个数值逼近过程）。简单描述一下GJK的迭代过程，除了初始情况下，每一步迭代时均已求得凸包边缘上的3个点构成一个三角形Tk，然后求指定点p距离Tk最近的点，记作q，再将凸包投影到pq。qp方向上最远的投影点所对应的凸包上的点记作w，最后将Tk的3个点舍去1个，然后加上w形成新的Tk+1。最开始的3个点哪里来的？似乎可以随便选边缘上的3个点，最后应该可能也许大概一定可以迭代到结果，只是影响迭代次数而已。
Java版本代码片段
简化版本代码示例，便于理解算法实现流程。
import java.util.*;
public class GJKAlgorithm {
//求向量叉乘
public static double vectorCross(double[] vectorA, double[] vectorB) {
return vectorA[0] * vectorB[1] - vectorA[1] * vectorB[0];
}
//求向量模
public static double vectorMode(double[] vectorA) {
return Math.sqrt(vectorA[0] * vectorA[0] + vectorA[1] * vectorA[1]);
}
//求向量夹角
public static double vectorAngle(double[] vectorA, double[] vectorB) {
return Math.acos((vectorA[0] * vectorB[0] + vectorA[1] * vectorB[1])
/ (vectorMode(vectorA) * vectorMode(vectorB)));
}
//GJK算法
public static boolean gjkAlgorithm(double[][] polygonA, double[][] polygonB) {
//定义一个搜索方向
double[] searchDirection = {1, 0};
//定义一个差向量
double[] diffVector = {0, 0};
//定义一个点集合
List<double[]> simplexList = new ArrayList<>();
//根据搜索方向计算两个多边形的最小点
double[] minA = getMinPoint(polygonA, searchDirection);
double[] minB = getMinPoint(polygonB, searchDirection);
//将最小点差向量添加到点集合
diffVector[0] = minB[0] - minA[0];
diffVector[1] = minB[1] - minA[1];
simplexList.add(diffVector);
//确定搜索方向
searchDirection[0] = -diffVector[0];
searchDirection[1] = -diffVector[1];
//根据搜索方向计算两个多边形的最小点
minA = getMinPoint(polygonA, searchDirection);
minB = getMinPoint(polygonB, searchDirection);
//将最小点差向量添加到点集合
diffVector[0] = minB[0] - minA[0];
diffVector[1] = minB[1] - minA[1];
simplexList.add(diffVector);
//循环搜索
while (true) {
//确定搜索方向
searchDirection = getSearchDirection(simplexList);
//根据搜索方向计算两个多边形的最小点
minA = getMinPoint(polygonA, searchDirection);
minB = getMinPoint(polygonB, searchDirection);
//将最小点差向量添加到点集合
diffVector[0] = minB[0] - minA[0];
diffVector[1] = minB[1] - minA[1];
simplexList.add(diffVector);
//判断是否有解
boolean isSolution = isSolution(simplexList, searchDirection);
//如果有解，则返回true，否则返回false
if (isSolution == true) {
return true;
} else {
//如果没有解，则将最后一个点移除，重新搜索
simplexList.remove(simplexList.size() - 1);
}
}
}
//根据搜索方向计算两个多边形的最小点
public static double[] getMinPoint(double[][] polygon, double[] searchDirection) {
//定义最小点
double[] minPoint = {0, 0};
//定义最小投影值
double minProjection = 0;
//计算投影值
for (int i = 0; i < polygon.length; i++) {
double projection = polygon[i][0] * searchDirection[0] + polygon[i][1] * searchDirection[1];
//如果当前投影值小于最小投影值，则更新最小点
if (i == 0 || projection < minProjection) {
minProjection = projection;
minPoint[0] = polygon[i][0];
minPoint[1] = polygon[i][1];
}
}
return minPoint;
}
//确定搜索方向
public static double[] getSearchDirection(List<double[]> simplexList) {
//定义搜索方向
double[] searchDirection = {0, 0};
//定义差向量
double[] diffVector = {0, 0};
//定义夹角
double angle = 0;
//获取最后一个点
double[] lastPoint = simplexList.get(simplexList.size() - 1);
//计算差向量
diffVector[0] = -lastPoint[0];
diffVector[1] = -lastPoint[1];
//如果点集合中只有一个点，则搜索方向为差向量
if (simplexList.size() == 1) {
searchDirection[0] = diffVector[0];
searchDirection[1] = diffVector[1];
} else {
//获取倒数第二个点
double[] lastLastPoint = simplexList.get(simplexList.size() - 2);
//计算夹角
angle = vectorAngle(lastLastPoint, lastPoint);
//如果夹角小于90度，则搜索方向为差向量
if (angle < Math.PI / 2) {
searchDirection[0] = diffVector[0];
searchDirection[1] = diffVector[1];
} else {
//如果夹角大于90度，则搜索方向为叉乘结果
searchDirection[0] = vectorCross(lastPoint, lastLastPoint);
searchDirection[1] = -vectorCross(lastLastPoint, lastPoint);
}
}
return searchDirection;
}
//判断是否有解
public static boolean isSolution(List<double[]> simplexList, double[] searchDirection) {
//如果点集合中只有一个点，则返回false
if (simplexList.size() == 1) {
return false;
} else {
//获取最后一个点
double[] lastPoint = simplexList.get(simplexList.size() - 1);
//如果最后一个点与搜索方向夹角为0度，则返回true
if (vectorAngle(lastPoint, searchDirection) == 0) {
return true;
} else {
return false;
}
}
}
//测试用例
public static void main(String[] args) {
//定义多边形A
double[][] polygonA = {{1, 1}, {1, 4}, {4, 4}, {4, 1}};
//定义多边形B
double[][] polygonB = {{3, 3}, {3, 5}, {5, 5}, {5, 3}};
//调用GJK算法
boolean isCollision = gjkAlgorithm(polygonA, polygonB);
//打印结果
System.out.println(isCollision);
}
}
//输出结果：true
