凸多面体重叠判断算法

admin1

【计算几何】凸多面体重叠判断算法：GJK 算法详解 & C++代码实现二维情形的凸多边形重叠判断

/*
二维向量叉乘
*/
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 = dot(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() ? dot(poly[i], poly[i + 1]) : dot(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);
}