基于EPA&GJK算法，对手部模型碰撞的检测

Epa&Gjk算法的学习

 

基于EPA&GJK算法，对手部模型碰撞的检测

作者：zmcpp

 

基础知识的学习

GJK算法

• 用于计算两个凸多面体之间的碰撞检测，以及最近距离
• 时间复杂度：O(M+N)
• 推荐速读的文章：https://zhuanlan.zhihu.com/p/178583914

前置知识

1、二维向量的点乘与叉乘——点乘用于判断向量是否同向，叉乘用于判断第二个向量与第一个向量的位置关系（大于0==左侧，小于0==右侧）

　　知识源于：https://zhuanlan.zhihu.com/p/672847151

代码：

double product(double* v1,double* v2){//叉乘

  return v1[0] * v2[1] - v1[1] * v2[0];

}

double dot(double* v1, double* v2){//点乘

  return v1[0] * v2[0] + v1[1] * v2[1];

}

 

2、凸多边形——任意两顶点间的线段位于多边形的内部或边上。

  此时我们可用向量的叉乘来判断一个多边形是否为凸多边形，思路如下：

  顶点按照逆时针（顺时针）排序，依次求叉积，若结果均大于0（小于0），则为凸多边形，若有小于0（大于0）的结果，则为凹多边形

  代码：

bool isConvex(vector<double*> poly){

  // 判断多边形是否为顺时针

  bool clockwise = product(new double[]{poly[1][0] - poly[0][0], poly[1][1] - poly[0][1]}, new double[]{-1, 0}) >= 0;

  for (int i = 0; i < poly.size(); i++){

         double d = i + 1 < poly.size() ? product(poly[i], poly[i + 1]) : product(poly[i], poly[0]);

         if ((clockwise && d > 0) || (!clockwise && d < 0)){

                return false;

         }

  }

  return true;

}

3、闵可夫斯基差（亦称闵可夫斯基和）

　　定理：如果 A 和 B 是两个凸多边形，则 A + B 也是凸多边形。 

 　　如何理解：关于闵可夫斯基距离，假设其在二维空间内，那么其p值的含义为：对距离xy两个方向上给予的权重的不同。p值越大，x&y中较大者的权重更高，较小者的权重更小。此时：p=1，为权重相同时的距离，通常称为L1范数或城市街区距离；p=2时，为直线距离；p>2时，较大者的权重逐渐升高，当p趋向于正无穷时，为仅考虑二维中的最大差异。在城市导航问题中，Manhattan距离（p=1)可能更合适；而在物理空间计算中，Euclidean距离(p=2)通常更为常用。对于需要强调较大差异的场景，较高的p值（如p=3的情况）可能是有用的；而当任何维度的最大差异是最重要的因素时，Chebyshev距离(p->正无穷)可能是首选。

　　运用：GJK算法的核心就是闵可夫斯基差，即若两个多边形相交，则它们的闵可夫斯基差必然包括原点

4、单纯形

　　　　k 阶单纯形（simplex），指的是k维空间中的多胞形，该多胞形是 k+1 个顶点组成的凸                 　　包。在 GJK 算法中，单纯形被大量使用。单纯形指的是点、线段、三角形或四面体。　　           　　例如，0阶单纯形是点，1阶单纯形是线段，2阶单纯形是三角形，3阶单纯形是四面体。

5、Support函数

　　作用是计算多边形在给定方向上的最远点。

　　在向量 d 方向的最远点为 v0 点。这里在寻找给定方向上的最远点时，需要用到向量的点乘。我们可以遍历每个顶点和向量d的点乘，找到点乘值最大的顶点，它就是向量 d 方向的最远点。这个点也被称为支撑点。

　　因为在构建单纯形时，我们希望尽可能得到闵可夫斯基差的顶点，而不是其内部的一个点，这样产生的单纯形才能包含最大的区域，增加算法的快速收敛性。

　　下方代码为：实现常规多边形、圆形和椭圆形的support函数。

/*
Support 函数（常规多边形）
*/
double* support(vector<double*> poly, double* direction){
    int maxIndex = 0;
    double maxDot = dot(new double[]{poly[0][0], poly[0][1]}, direction);
    for (int i = 1; i < poly.size(); i++){
        double d = dot(new double[]{poly[i][0], poly[i][1]}, direction);
        if (d > maxDot){
            maxDot = d;
            maxIndex = i;
        }
    }
    return new double[]{poly[maxIndex][0], poly[maxIndex][1]};
}

/*
计算两个二维向量的夹角（度）[0,PI]
*/
double calc2DVectorsAngle(double* v1, double* v2){
    double d1 = sqrt(pow(v1[0] - 0, 2) + pow(v1[1] - 0, 2));
    double d2 = sqrt(pow(v2[0] - 0, 2) + pow(v2[1] - 0, 2));
    // 获取弧度夹角 [0,PI]
    return acos((v1[0] * v2[0] + v1[1] * v2[1]) / (d1*d2));
}

/*
Support 函数（圆形）
*/
double* supportCircle(double* centerPoint,double r, double* direction){
    // 获取theta
    double theta = calc2DVectorsAngle(direction, new double[]{1, 0});
    if (direction[1] < 0){
        theta = 2 * PI - theta;
    }
    // 根据圆的参数方程返回支撑点
    return new double[]{centerPoint[0] + r * cos(theta), centerPoint[1] + r * sin(theta)};
}

/*
Support 函数（椭圆形）
*/
double* supportEillpse(double* centerPoint, double a,double b, double* direction){
    // 获取theta
    double theta = calc2DVectorsAngle(direction, new double[]{1, 0});
    if (direction[1] < 0){
        theta = 2 * PI - theta;
    }
    // 根据椭圆的参数方程返回支撑点
    return new double[]{centerPoint[0] + a * cos(theta), centerPoint[1] + b * sin(theta)};
}

GJK算法讲解

概述：

　　给定两个多边形 A 和 B，以及一个初始方向，通过迭代的方式构建、更新单纯形，并判断单纯形是否包含原点，若包含原点则两个多边形相交，否则不相交。

　　退出循环的终止条件为：当计算得到新的顶点与搜索方向点乘积小于等于 0 的时候，退出迭代。

　　　　1、单纯形包含远点

　　　　2、两图形的边已遍历结束。

 

迭代方式：

• 通过随机的方式获取初始方向
• 根据初始方向，用 Support 函数分别得到两个多边形的支撑点，再做闵可夫斯基差，获得第一个顶点，并放到单纯形中
• 根据初始方向，用 Support 函数分别得到两个多边形的支撑点，再做闵可夫斯基差，获得第一个顶点，并放到单纯形中。
• 以第一个顶点面向原点的方向作为第二次迭代方向。
• 根据第二次迭代方向，用 Support 函数分别得到两个多边形的支撑点，再做闵可夫斯基差，获  得第二个顶点，此时对第二个顶点做过原点检查，如果没有通过检查则能够断定两个多边形没有重叠（程序结束），否则继续下面的步骤
• 将第二个顶点加入单纯形
• 以第一个和第二个顶点构成的线段的面向原点的法向量方向作为第三次迭代的方向
  根据第三次迭代方向，用 Support 函数分别得到两个多边形的支撑点，再做闵可夫斯基差，获得第三个顶点，此时对第三个顶点做过原点检查，如果没有通过检查则能够断定两个多边形没有重叠（程序结束），否则继续下面的步骤
• 将第三个顶点加入单纯形

开始循环：
判断当前单纯形中三个顶点组成的三角形是否包含原点，如果包含则可以断定两个多边形重叠（程序结束），否则进行下面的步骤
以当前单纯形中三个顶点组成的三角形中最靠近原点的边B的面向原点的法向量方向作为下一次的迭代方向D
根据迭代方向D，用 Support 函数分别得到两个多边形的支撑点，再做闵可夫斯基差，获得新的顶点
对新的顶点做过原点检查，如果没有通过检查则能够断定两个多边形没有重叠（程序结束），否则继续下面的步骤
如果新的顶点已经存在于当前单纯形，那么可以断定两个多边形没有重叠（程序结束），否则继续下面的步骤
以新的顶点和边B的两个端点构成新的单纯形，返回到循环的第一步

#include "base.hpp"

Hit GJK(const std::vector<Vector2D>& convex1, const Vector2D origin1, const std::vector<Vector2D>& convex2, const Vector2D origin2)
{
    Hit hit;
    hit.is_collision=false;
    Vector2D direct ;
    if (origin1 == origin2)
        direct = Vector2D(1.f, 0);
    else
        direct = Math::Normalize(origin2 - origin1);

    Triangle semple;
    semple.point[0]=support(convex1, convex2, direct);
    if (semple.point[0].dot(direct) < 0)
        return hit;
    semple.point[1] =support(convex1, convex2, -semple.point[0]);
    if (semple.point[1].dot(-semple.point[0]) < 0)
        return hit;
    for (;;)
    {
        direct = GetFaceOriginVector(semple.point[1], semple.point[0]);
        Vector2D minkowski_d = support(convex1, convex2, direct);

        semple.point[2] = minkowski_d;

        if (minkowski_d.dot(direct) < 0)
            return hit;
        if (minkowski_d == semple.point[1]|| minkowski_d == semple.point[2])
            return hit;
        if (OriginInTriangle(semple))
            return EPA(semple,convex1,convex2);    
    }
}

Vector2D support(const std::vector<Vector2D>& convex1, const std::vector<Vector2D>& convex2, const Vector2D& direct)
{
    Vector2D A, B;
    A = convex1[0];
    double distance = direct.dot(convex1[0]);
    for (std::size_t i = 1; i < convex1.size(); i++)
    {
        double t = direct.dot(convex1[i]);
        if (t > distance)
        {
            distance = t;
            A = convex1[i];
        }
    }

    Vector2D n_direct = -direct;
    B = convex2[0];
    distance = n_direct.dot(convex2[0]);
    for (std::size_t i = 1; i < convex2.size(); i++)
    {
        double t = n_direct.dot(convex2[i]);
        if (t > distance)
        {
            distance = t;
            B = convex2[i];
        }
    }
    return A - B;
}

//检测三角形是否包含原点
bool OriginInTriangle(Triangle& semple)
{
    Vector2D AB = semple.point[1] - semple.point[2];
    Vector2D AC = semple.point[0] - semple.point[2];
    Vector2D AO = -semple.point[2];
    Vector2D nor =Math::Normalize(Math::TripleCorss(AB,AC,AC));
    if (nor.dot(AO) >= 0)
        {
            semple.point[1] = semple.point[2];
            return false;
        }
    nor = Math::Normalize(Math::TripleCorss(AC, AB, AB));
    if (nor.dot(AO) >= 0)
        {
            semple.point[0] = semple.point[1];
            semple.point[1] = semple.point[2];
            return false;
        }
    return true;
}

//获得线段面向原点的向量
Vector2D GetFaceOriginVector(const Vector2D& a, const Vector2D& b)
{
    Vector2D delta = a - b;
    Vector2D nor = Math::Normalize(Vector2D(delta.Y, -delta.X));
    if (nor.dot(a) < 0)
        return nor;
    return -nor;
}