凸多面体重叠判断算法：GJK 算法详解 & C++代码实现二维情...

admin1

/*
二维向量叉乘
*/
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];
}


/*
三维向量叉乘
*/
double* product3D(double* v1, double* v2){
        return new double[]{v1[1] * v2[2] - v2[1] * v1[2],-(v1[0] * v2[2] - v2[0] * v1[2]),v1[0] * v2[1] - v2[0] * v1[1]};
}


/*
判断一个多边形是否为凸多边形
*/
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;
}


/*
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)};
}


/*
传入三个点，计算点p0到p1和p2构成的边的距离
*/
double calcPointToLineDistance(double* p0,double* p1,double* p2){
        double k = (p2[1] - p1[1]) / (p2[0] - p1[0]);
        double A = k;
        double B = -1;
        double C = p1[1] - k * p1[0];
        return abs(A * p0[0] + B * p0[1] + C) / sqrt(A*A+B*B);
}









/*
描述：本代码实现了GJK算法求解二维情形下两个凸多边形的重叠判断问题
时间：2023-1-21 15:41
作者：WSKH
博客：wskh0929.blog.csdn.net
*/

#include <iostream>
#include <vector>
// 圆周率
#define PI acos(-1)
// 浮点型误差精度
#define ERROR 0.0000001
using namespace std;

// 原点坐标
double* origin = new double[]{0, 0};

/*
打印单纯形信息
*/
void printSimplex(int epoch, vector<double*> simplex){
        cout << "---------------------------------- 第 " << epoch << " 次迭代 , 单纯形的顶点信息 ----------------------------------" << endl;
        for (int i = 0; i < simplex.size(); i++){
                cout << "(" <<simplex[i][0] << " , " << simplex[i][1] << ")" << endl;
        }
}

/*
打印终止信息
flag = 0: 过原点检查不通过，提前返回 false
flag = 1: 三角形包含原点，提前返回 true
flag = 2: 新顶点重复存在，提前返回 false
*/
void printStopInfo(int epoch, int flag){
        cout << "第 " << epoch << " 次迭代：";
        switch (flag)
        {
        case 0:
                cout << "过原点检查不通过，提前返回 false" << endl;
                break;
        case 1:
                cout << "三角形包含原点，提前返回 true" << endl;
                break;
        case 2:
                cout << "新顶点重复存在，提前返回 false" << endl;
                break;
        default:
                break;
        }
}

/*
判断两个二维向量/点是否相等
*/
bool isEquals(double* p1,double* p2){
        return abs(p1[0] - p2[0]) < ERROR && abs(p1[1] - p2[1]);
}

/*
计算两个点的距离
*/
double calcPointToPointDistance(double* p1, double* p2){
        return sqrt(pow(p1[0] - p2[0],2) + pow(p1[1] - p2[1],2));
}

/*
二维向量相减
*/
double* diff(double* v1, double* v2){
        return new double[]{v1[0] - v2[0], v1[1] - v2[1]};
}

/*
二维向量相加
*/
double* sum(double* v1, double* v2){
        return new double[]{v1[0] + v2[0], v1[1] + v2[1]};
}

/*
二维向量叉乘
*/
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];
}

/*
三维向量叉乘
*/
double* product3D(double* v1, double* v2){
        return new double[]{v1[1] * v2[2] - v2[1] * v1[2],-(v1[0] * v2[2] - v2[0] * v1[2]),v1[0] * v2[1] - v2[0] * v1[1]};
}

/*
判断一个多边形是否为凸多边形
*/
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;
}

/*
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)};
}

/*
根据给定方向和多边形，获取单纯形新顶点
*/
double* getNewVertex(vector<double*> poly1, vector<double*> poly2, double* direction){
        double* supportPoint1 = support(poly1, direction);
        double* supportPoint2 = support(poly2, new double[]{-direction[0], -direction[1]});
        return diff(supportPoint1, supportPoint2);
}

/*
获取初始方向
*/
double* getInitDirection(){
        return new double[]{1, 0};
}

/*
判断两个点是否过原点
*/
bool isCrossingOrigin(double* A,double* B){
        return dot(A, diff(origin, B)) >= 0;
}

/*
传入三个点，计算点p0到p1和p2构成的边的距离
*/
double calcPointToLineDistance(double* p0,double* p1,double* p2){
        double k = (p2[1] - p1[1]) / (p2[0] - p1[0]);
        double A = k;
        double B = -1;
        double C = p1[1] - k * p1[0];
        return abs(A * p0[0] + B * p0[1] + C) / sqrt(A*A+B*B);
}

/*
传入两个点，获取它构成的边面向原点的法向量
*/
double* getLineFaceToOriginVector(double* A, double* B){
        double* AB = diff(B, A);
        double* AO = diff(origin, A);
        double* res = product3D(new double[]{AB[0], AB[1], 0}, new double[]{AO[0], AO[1],0});
        return product3D(res, new double[]{AB[0], AB[1], 0});
}

/*
传入三个点，根据海伦公式计算三角形面积
*/
double calcTriangleArea(double* p1, double* p2, double* p3){
        double a = calcPointToPointDistance(p1, p2);
        double b = calcPointToPointDistance(p1, p3);
        double c = calcPointToPointDistance(p2, p3);
        double p = (a + b + c) / 2.0;
        return sqrt(p*(p-a)*(p-b)*(p-c));
}

/*
传入三个点，判断由三个点组成的三角形是否包含原点
*/
bool isContainOrigin(double* p1, double* p2, double* p3){
        double s1 = calcTriangleArea(origin,p1,p2);
        double s2 = calcTriangleArea(origin, p1, p3);
        double s3 = calcTriangleArea(origin, p2, p3);
        double s = calcTriangleArea(p1, p2, p3);
        return abs(s1 + s2 + s3 - s) < ERROR;
}

/*
GJK算法（返回两个多边形是否重叠）
*/
bool GJK(vector<double*> poly1, vector<double*> poly2){
        // 初始化单纯形
        vector<double*> simplex;
        // 第1次迭代
        double* direction = getInitDirection();
        double* vertex = getNewVertex(poly1,poly2,direction);
        simplex.push_back(vertex);
        printSimplex(1, simplex);
        // 第2次迭代
        direction = diff(origin, vertex);
        vertex = getNewVertex(poly1, poly2, direction);
        // 过原点检查
        if (!isCrossingOrigin(simplex[0], vertex)){
                printStopInfo(2, 0);
                return false;
        }
        simplex.push_back(vertex);
        printSimplex(2, simplex);
        // 第3次迭代
        direction = getLineFaceToOriginVector(simplex[1], simplex[0]);
        vertex = getNewVertex(poly1, poly2, direction);
        // 过原点检查
        if (!isCrossingOrigin(simplex[0], vertex)){
                printStopInfo(3, 0);
                return false;
        }
        simplex.push_back(vertex);
        printSimplex(3, simplex);
        // 开始循环
        for (int epoch = 4;; epoch++){
                // 判断当前单纯形的三个顶点组成的三角形是否包含原点
                if (isContainOrigin(simplex[0], simplex[1], simplex[2])){
                        printStopInfo(epoch - 1, 1);
                        return true;
                }
                // 找到三角形离原点最近的边
                double minDistance;
                int minIndex1 = -1;
                int minIndex2 = -1;
                for (int i = 0; i < simplex.size(); i++){
                        for (int j = i + 1; j < simplex.size(); j++){
                                double distance = calcPointToLineDistance(origin, simplex[i], simplex[j]);
                                if (minIndex1 == -1 || distance < minDistance){
                                        minDistance = distance;
                                        minIndex1 = i;
                                        minIndex2 = j;
                                }
                        }
                }
                // 找方向
                direction = getLineFaceToOriginVector(simplex[minIndex1], simplex[minIndex2]);
                vertex = getNewVertex(poly1, poly2, direction);
                // 是否存在于当前单纯形检查
                for (int i = 0; i < simplex.size(); i++){
                        if (isEquals(simplex[i], vertex)){
                                printStopInfo(epoch, 2);
                                return false;
                        }
                }
                // 过原点检查
                if (!isCrossingOrigin(simplex[0], vertex)){
                        printStopInfo(epoch, 0);
                        return false;
                }
                // 更新单纯形
                double* vertex1 = simplex[minIndex1];
                double* vertex2 = simplex[minIndex2];
                simplex.clear();
                simplex.push_back(vertex);
                simplex.push_back(vertex1);
                simplex.push_back(vertex2);
                printSimplex(epoch, simplex);
        }
}

/*
程序主函数
*/
int main(){
        // 返回码
        int returnCode = 0;

        // 创建两个多边形 poly1 和 poly2
        vector<double*> poly1;
        poly1.push_back(new double[]{0,0});
        poly1.push_back(new double[]{3,0});
        poly1.push_back(new double[]{3,3});
        poly1.push_back(new double[]{0,3});
        vector<double*> poly2;
        // 重叠测试
        //poly2.push_back(new double[]{2,2});
        //poly2.push_back(new double[]{5,2});
        //poly2.push_back(new double[]{5,5});
        //poly2.push_back(new double[]{2,5});

        // 不重叠测试
        poly2.push_back(new double[]{3, 3});
        poly2.push_back(new double[]{5, 3});
        poly2.push_back(new double[]{3, 5});
        poly2.push_back(new double[]{3, 5});

        // 检验两个多边形是否为凸
        if (!isConvex(poly1)){
                cout << "错误: poly1 为凹多边形" << endl;
                returnCode = -1;
        }
        if (!isConvex(poly2)){
                cout << "错误: poly2 为凹多边形" << endl;
                returnCode = -1;
        }

        // 调用GJK算法进行重叠判断
        if (returnCode == 0){
                bool isOverlap = GJK(poly1, poly2);
                cout << "重叠判断结果为: 两个多边形 <" << (isOverlap ? "重叠" : "未重叠") << ">" << endl;
        }
       
        system("pause");
        return returnCode;
}