凹多边形分割多个凸多边形

记录一下工作中用到的凹多边形分割算法，具体分割思路如下图：利用向量叉乘找出凹点，然后进行延长分割为两部分，再进行递归分割，直至所有的多边形为凸多边形为止。

实现代码如下：

因为有需求所以进行了float和double类型重载。

#include "ConcavePolygonSegmentation.h"

/*  从多边形的有序的点数组获取顶点和三角面数据信息（凹凸多边形）
    参数1： 顶点数组
*/
TArray<TArray<FVector>> ConcavePolygonSegmentation::GetPolygonDataFromOrderVertexs(TArray<FVector> _points)
{
    TArray<TArray<FVector>> polygonDividedArr;
    if (_points.Num() > 3) {
        // 判断多边形是否逆时针
        bool _antiClockwise = PolygonIsAntiClockwise(_points);
        // 开始分割
        polygonDividedArr = DividePolygonIfConcave(_points, _antiClockwise); //递归

        if (polygonDividedArr.Num() > 0) {
            UE_LOG(LogTemp, Warning, TEXT("分割后多边形个数：%d"), polygonDividedArr.Num());
        }
    } else {
        UE_LOG(LogTemp, Warning, TEXT("传入图形不是多边形~~~~~~~~~~~"));
    }
    return polygonDividedArr;
}

TArray<TArray<glm::dvec3>> ConcavePolygonSegmentation::GetPolygonDataFromOrderVertexs(TArray<glm::dvec3> _points)
{

    TArray<TArray<glm::dvec3>> polygonDividedArr;
    if (_points.Num() > 3) {
        // 判断多边形是否逆时针
        bool _antiClockwise = PolygonIsAntiClockwise(_points);
        // 开始分割
        polygonDividedArr = DividePolygonIfConcave(_points, _antiClockwise); //递归

        if (polygonDividedArr.Num() > 0) {
            UE_LOG(LogTemp, Warning, TEXT("分割后多边形个数：%d"), polygonDividedArr.Num());
        }
    } else {
        UE_LOG(LogTemp, Warning, TEXT("传入图形不是多边形~~~~~~~~~~~"));
    }
    return polygonDividedArr;
}

//分割
/*检查多边形是否是凹多边形，如果是就切割
    参数1：点集
    参数2：需要返回的被切割多边形1
    参数2：需要返回的被切割多边形2
    */
TArray<TArray<FVector>> ConcavePolygonSegmentation::DividePolygonIfConcave(TArray<FVector> _points, bool _antiClockwise)
{
    TArray<TArray<FVector>> polygonDividedArr;
    int t_pointsNum = _points.Num();
    if (_points.Num() < 3) {
        return polygonDividedArr;
    } else if (t_pointsNum == 3) {
        polygonDividedArr.Add(_points);
        return polygonDividedArr;
    } else if (t_pointsNum > 3) {
        TArray<FVector> _dividePolygonA;
        TArray<FVector> _dividePolygonB;
        float t_hei = _points[0].Z;
        // 将所有的点放在同一个xy平面
        SetPointsZvalueBySpecify(_points, 0);

        FVector t_p1;
        FVector t_p2;
        // 将点集转换为首尾相接的向量集
        TArray<FVector> t_dirs = GetVectorArrByPointsArr(_points);

        bool t_divideResult = false;
        int t_indexNew = 0; //后一个向量下标，也是前一个向量终点的下标
        for (int i = 0; i < t_dirs.Num(); i++) {
            t_p1 = t_dirs[i];
            if (i == t_dirs.Num() - 1) {
                t_p2 = t_dirs[0];
                t_indexNew = 0;
            } else {
                t_p2 = t_dirs[i + 1];
                t_indexNew = i + 1;
            }

            float t_rotateDir = FVector::CrossProduct(t_p1, t_p2).Z;
            //检查出是凹多边形
            if ((t_rotateDir < -0.0f && _antiClockwise == true) || (t_rotateDir > 0.0f && _antiClockwise == false)) {
                UE_LOG(LogTemp, Warning, TEXT("是凹多边形~~~~~~~~~~~"));
                //求分割点
                t_divideResult = GetRayIntersectionOfVecInVecArr(t_dirs, _points, i, i, t_pointsNum - 1, _dividePolygonA, _dividePolygonB);
                if (t_divideResult == false) {
                    t_divideResult = GetRayIntersectionOfVecInVecArr(t_dirs, _points, i, 0, i - 1, _dividePolygonA, _dividePolygonB);
                }
                if (t_divideResult == false) {
                    UE_LOG(LogTemp, Error, TEXT("线段%d  没有得到分割点"), i);
                }
                break;
            }
        }

        if (t_divideResult == false) {
            SetPointsZvalueBySpecify(_points, t_hei);
            polygonDividedArr.Add(_points);
        } else {
            if (_dividePolygonA.Num() > 2) {
                SetPointsZvalueBySpecify(_dividePolygonA, t_hei);
                polygonDividedArr.Append(DividePolygonIfConcave(_dividePolygonA, _antiClockwise));
            }
            if (_dividePolygonB.Num() > 2) {
                SetPointsZvalueBySpecify(_dividePolygonB, t_hei);
                polygonDividedArr.Append(DividePolygonIfConcave(_dividePolygonB, _antiClockwise));
            }
        }
    }

    return polygonDividedArr;
}

TArray<TArray<glm::dvec3>> ConcavePolygonSegmentation::DividePolygonIfConcave(TArray<glm::dvec3> _points, bool _antiClockwise /*= true*/)
{
    TArray<TArray<glm::dvec3>> polygonDividedArr;
    int t_pointsNum = _points.Num();
    if (_points.Num() < 3) {
        return polygonDividedArr;
    } else if (t_pointsNum == 3) {
        polygonDividedArr.Add(_points);
        return polygonDividedArr;
    } else if (t_pointsNum > 3) {
        TArray<glm::dvec3> _dividePolygonA;
        TArray<glm::dvec3> _dividePolygonB;
        double t_hei = _points[0].z;
        // 将所有的点放在同一个xy平面
        SetPointsZvalueBySpecify(_points, 0);

        glm::dvec3 t_p1;
        glm::dvec3 t_p2;
        // 将点集转换为首尾相接的向量集
        TArray<glm::dvec3> t_dirs = GetVectorArrByPointsArr(_points);

        bool t_divideResult = false;
        int t_indexNew = 0; //后一个向量下标，也是前一个向量终点的下标
        for (int i = 0; i < t_dirs.Num(); i++) {
            t_p1 = t_dirs[i];
            if (i == t_dirs.Num() - 1) {
                t_p2 = t_dirs[0];
                t_indexNew = 0;
            } else {
                t_p2 = t_dirs[i + 1];
                t_indexNew = i + 1;
            }

            double t_rotateDir = CrossProduct(t_p1, t_p2).z;
            //检查出是凹多边形
            if ((t_rotateDir < -0.0f && _antiClockwise == true) || (t_rotateDir > 0.0f && _antiClockwise == false)) {
                UE_LOG(LogTemp, Warning, TEXT("是凹多边形~~~~~~~~~~~"));
                //求分割点
                t_divideResult = GetRayIntersectionOfVecInVecArr(t_dirs, _points, i, i, t_pointsNum - 1, _dividePolygonA, _dividePolygonB);
                if (t_divideResult == false) {
                    t_divideResult = GetRayIntersectionOfVecInVecArr(t_dirs, _points, i, 0, i - 1, _dividePolygonA, _dividePolygonB);
                }
                if (t_divideResult == false) {
                    UE_LOG(LogTemp, Error, TEXT("线段%d  没有得到分割点"), i);
                }
                break;
            }
        }

        if (t_divideResult == false) {
            SetPointsZvalueBySpecify(_points, t_hei);
            polygonDividedArr.Add(_points);
        } else {
            if (_dividePolygonA.Num() > 2) {
                SetPointsZvalueBySpecify(_dividePolygonA, t_hei);
                polygonDividedArr.Append(DividePolygonIfConcave(_dividePolygonA, _antiClockwise));
            }
            if (_dividePolygonB.Num() > 2) {
                SetPointsZvalueBySpecify(_dividePolygonB, t_hei);
                polygonDividedArr.Append(DividePolygonIfConcave(_dividePolygonB, _antiClockwise));
            }
        }
    }
    return polygonDividedArr;
}

bool ConcavePolygonSegmentation::PolygonIsAntiClockwise(const TArray<FVector>& p)
{
    int n = p.Num();
    //计算多边形面积
    if (n < 3)
        return true;
    double s = p[0].Y * (p[n - 1].X - p[1].X);
    for (int i = 1; i < n; ++i)
        s += p[i].Y * (p[i - 1].X - p[((i + 1) >= n ? 0 : (i + 1))].X);
    s = s * 0.5;
    return s > 0.f;
}

bool ConcavePolygonSegmentation::PolygonIsAntiClockwise(const TArray<glm::dvec3>& p)
{
    int n = p.Num();
    //计算多边形面积
    if (n < 3)
        return true;
    double s = p[0].y * (p[n - 1].x - p[1].x);
    for (int i = 1; i < n; ++i)
        s += p[i].y * (p[i - 1].x - p[((i + 1) >= n ? 0 : (i + 1))].x);
    s = s * 0.5;
    return s > 0.f;
}

// 给定点数组的Z值统一化
bool ConcavePolygonSegmentation::SetPointsZvalueBySpecify(TArray<FVector>& _points, float _zValue)
{
    if (_points.Num() > 0) {
        for (int i = 0; i < _points.Num(); i++) {
            _points[i].Z = _zValue;
        }
        return true;
    }
    return false;
}

bool ConcavePolygonSegmentation::SetPointsZvalueBySpecify(TArray<glm::dvec3>& _points, double _zValue)
{
    if (_points.Num() > 0) {
        for (int i = 0; i < _points.Num(); i++) {
            _points[i].z = _zValue;
        }
        return true;
    }
    return false;
}

/*根据点数组获取向量数组
    */
TArray<FVector> ConcavePolygonSegmentation::GetVectorArrByPointsArr(const TArray<FVector> _points)
{
    TArray<FVector> t_res;
    int t_pointsNum = _points.Num();
    if (t_pointsNum > 1) {
        FVector t_p1;
        FVector t_p2;
        for (int i = 0; i < _points.Num(); i++) {
            t_p1 = _points[i];
            if (i == t_pointsNum - 1) {
                t_p2 = _points[0];
            } else {
                t_p2 = _points[i + 1];
            }
            t_res.Add(t_p2 - t_p1);
        }
    }
    return t_res;
}

TArray<glm::dvec3> ConcavePolygonSegmentation::GetVectorArrByPointsArr(const TArray<glm::dvec3> _points)
{
    TArray<glm::dvec3> t_res;
    int t_pointsNum = _points.Num();
    if (t_pointsNum > 1) {
        glm::dvec3 t_p1;
        glm::dvec3 t_p2;
        for (int i = 0; i < _points.Num(); i++) {
            t_p1 = _points[i];
            if (i == t_pointsNum - 1) {
                t_p2 = _points[0];
            } else {
                t_p2 = _points[i + 1];
            }
            t_res.Add(t_p2 - t_p1);
        }
    }
    return t_res;
}

TArray<FVector> ConcavePolygonSegmentation::GetPointsByIndexRange(const TArray<FVector> _points, int startIndex, int endIndex)
{
    TArray<FVector> pts;
    if (startIndex <= endIndex) {
        int idx = startIndex;
        while (idx <= endIndex && idx < _points.Num()) {
            pts.Add(_points[idx]);
            idx++;
        }
    } else {
        int idx = startIndex;
        while (idx < _points.Num()) {
            pts.Add(_points[idx]);
            idx++;
        }
        idx = 0;
        while (idx <= endIndex && idx < _points.Num()) {
            pts.Add(_points[idx]);
            idx++;
        }
    }
    return pts;
}

TArray<glm::dvec3> ConcavePolygonSegmentation::GetPointsByIndexRange(const TArray<glm::dvec3> _points, int startIndex, int endIndex)
{
    TArray<glm::dvec3> pts;
    if (startIndex <= endIndex) {
        int idx = startIndex;
        while (idx <= endIndex && idx < _points.Num()) {
            pts.Add(_points[idx]);
            idx++;
        }
    } else {
        int idx = startIndex;
        while (idx < _points.Num()) {
            pts.Add(_points[idx]);
            idx++;
        }
        idx = 0;
        while (idx <= endIndex && idx < _points.Num()) {
            pts.Add(_points[idx]);
            idx++;
        }
    }
    return pts;
}

/*从向量数组中获取一个向量在这个数组中的延长线与其他向量的交点
    注意：顺序必须先从这个向量的下标开始，不能是0;交点不包括向量端点
    参数1：方向向量数组
    参数2：对应的点数组（长度需保持一致）
    参数3：这个向量的下标
    参数4，5：开始和结束下标
    参数6，7: 根据交点被切分的两组点数组
    返回值：true 为成功,反之无
    */
bool ConcavePolygonSegmentation::GetRayIntersectionOfVecInVecArr(
    const TArray<FVector> _dirs,
    const TArray<FVector> _points,
    const int _vecIndex, //这个向量的下标
    const int _beginIndex, //开始和结束下标
    const int _endIndex,
    TArray<FVector>& _dividePolygonA, TArray<FVector>& _dividePolygonB) //根据交点被切分的两组点数组
{
    int t_dirsNum = _dirs.Num(); //向量个数
    int t_pointsNum = _points.Num(); //点的个数

    if (t_dirsNum > 3 && t_pointsNum > 3) {
        if (t_dirsNum == t_pointsNum) {
            if (_beginIndex >= 0 && _beginIndex < t_dirsNum) {
                if (_endIndex >= 0 && _endIndex < t_dirsNum) {
                    int t_indexNew; //向量头对应点的下标
                    if (_vecIndex == (t_dirsNum - 1)) //为向量组最后一个向量
                        t_indexNew = 0;
                    else
                        t_indexNew = _vecIndex + 1;

                    FVector t_beginA = _points[_vecIndex]; //凹处第一个向量起点
                   /* FVector OutDir;
                    float OutLength;
                    _dirs[_vecIndex].ToDirectionAndLength(OutDir, OutLength);*/
                    float k = 1e14f;
                    //FVector ExtendedVector = OutDir * k * OutLength;
                    FVector ExtendedVector = _dirs[_vecIndex] * k ;
                    FVector t_endA = t_beginA + ExtendedVector; //用线段代替射线

                    FVector t_intersectionPoint;
                    for (int j = _beginIndex; j <= _endIndex; j++) {
                        if (j != _vecIndex && j != t_indexNew) { //不是向量_vecIndex的端点
                            FVector t_beginB = _points[j];
                            if (LineIntersect3D(t_beginA, t_endA, t_beginB, t_beginB + _dirs[j], t_intersectionPoint)) {
                                //给分割的多边形点组加点
                                UE_LOG(LogTemp, Warning, TEXT("凹点向量下标: %d,  相交向量下标: %d"), _vecIndex, j);
                                _dividePolygonA = GetPointsByIndexRange(_points, t_indexNew, j);
                                _dividePolygonA.Add(t_intersectionPoint);
                                UE_LOG(LogTemp, Warning, TEXT("_dividePolygonA向量数组个数： %d"), _dividePolygonA.Num());

                                _dividePolygonB = GetPointsByIndexRange(_points, j, t_indexNew - 1);
                                if (_dividePolygonB.Num() > 0) {
                                    _dividePolygonB[0] = t_intersectionPoint;
                                }
                                UE_LOG(LogTemp, Warning, TEXT("_dividePolygonB向量数组个数： %d"), _dividePolygonB.Num());
                                return true;
                            }
                        }
                    }
                }
            }
        }
    }

    return false;
}

bool ConcavePolygonSegmentation::GetRayIntersectionOfVecInVecArr(const TArray<glm::dvec3> _dirs, const TArray<glm::dvec3> _points, const int _vecIndex, const int _beginIndex, const int _endIndex, TArray<glm::dvec3>& _dividePolygonA, TArray<glm::dvec3>& _dividePolygonB)
{
    int t_dirsNum = _dirs.Num(); //向量个数
    int t_pointsNum = _points.Num(); //点的个数

    if (t_dirsNum > 3 && t_pointsNum > 3) {
        if (t_dirsNum == t_pointsNum) {
            if (_beginIndex >= 0 && _beginIndex < t_dirsNum) {
                if (_endIndex >= 0 && _endIndex < t_dirsNum) {
                    int t_indexNew; //向量头对应点的下标
                    if (_vecIndex == (t_dirsNum - 1)) //为向量组最后一个向量
                        t_indexNew = 0;
                    else
                        t_indexNew = _vecIndex + 1;

                    glm::dvec3 t_beginA = _points[_vecIndex]; //凹处第一个向量起点
                    double k = 1e14;
                    glm::dvec3 ExtendedVector = Multiplication(_dirs[_vecIndex], k) ;
                    glm::dvec3 t_endA = t_beginA + ExtendedVector; //用线段代替射线

                    glm::dvec3 t_intersectionPoint;
                    for (int j = _beginIndex; j <= _endIndex; j++) {
                        if (j != _vecIndex && j != t_indexNew) { //不是向量_vecIndex的端点
                            glm::dvec3 t_beginB = _points[j];
                            if (LineIntersect3D(t_beginA, t_endA, t_beginB, t_beginB + _dirs[j], t_intersectionPoint)) {
                                //给分割的多边形点组加点
                                UE_LOG(LogTemp, Warning, TEXT("凹点向量下标: %d,  相交向量下标: %d"), _vecIndex, j);
                                _dividePolygonA = GetPointsByIndexRange(_points, t_indexNew, j);
                                _dividePolygonA.Add(t_intersectionPoint);
                                UE_LOG(LogTemp, Warning, TEXT("_dividePolygonA向量数组个数： %d"), _dividePolygonA.Num());

                                _dividePolygonB = GetPointsByIndexRange(_points, j, t_indexNew - 1);
                                if (_dividePolygonB.Num() > 0) {
                                    _dividePolygonB[0] = t_intersectionPoint;
                                }
                                UE_LOG(LogTemp, Warning, TEXT("_dividePolygonB向量数组个数： %d"), _dividePolygonB.Num());
                                return true;
                            }
                        }
                    }
                }
            }
        }
    }
    return false;
}

bool ConcavePolygonSegmentation::LineIntersect3D(const FVector& line1P1, const FVector& line1P2, const FVector& line2P1, const FVector& line2P2, FVector& intersectP)
{
    FVector v1 = line1P2 - line1P1;
    FVector v2 = line2P2 - line2P1;

    if (abs(FVector::DotProduct(v1, v2)) == v1.Size() * v2.Size()) {
        // 两线平行
        return false;
    }

    FVector startPointSeg = line2P1 - line1P1;
    FVector vecS1 = FVector::CrossProduct(v1, v2); // 有向面积1
    FVector vecS2 = FVector::CrossProduct(startPointSeg, v2); // 有向面积2
    float num = FVector::DotProduct(startPointSeg, vecS1);

    // 判断两这直线是否共面
    if (num >= 1E-05f || num <= -1E-05f) {
        return false;
    }
    // 有向面积比值，利用点乘是因为结果可能是正数或者负数　　　　　　　　　　　　　　　　
    if (abs(vecS1.SizeSquared()) < 1e-5f) {
        return false;
    }
    float num2 = FVector::DotProduct(vecS2, vecS1) / vecS1.SizeSquared();
    intersectP = line1P1 + v1 * num2;
    FVector v11 = intersectP - line2P1;
    //是否在线段2延长线上
    if (v11.Size() / v2.Size() > 1)
        return false;
    return true;
}

bool ConcavePolygonSegmentation::LineIntersect3D(const glm::dvec3& line1P1, const glm::dvec3& line1P2, const glm::dvec3& line2P1, const glm::dvec3& line2P2, glm::dvec3& intersectP)
{
    glm::dvec3 v1 = line1P2 - line1P1;
    glm::dvec3 v2 = line2P2 - line2P1;

    if (abs(DotProduct(v1, v2)) == Size(v1) * Size(v2)) {
        // 两线平行
        return false;
    }

    glm::dvec3 startPointSeg = line2P1 - line1P1;
    glm::dvec3 vecS1 = CrossProduct(v1, v2); // 有向面积1
    glm::dvec3 vecS2 = CrossProduct(startPointSeg, v2); // 有向面积2
    double num = DotProduct(startPointSeg, vecS1);

    // 判断两这直线是否共面
    if (num >= 1E-05f || num <= -1E-05f) {
        return false;
    }
    // 有向面积比值，利用点乘是因为结果可能是正数或者负数　　　　　　　　　　　　　　　　
    if (abs(SizeSquared(vecS1)) < 1e-5f) {
        return false;
    }
    double num2 = DotProduct(vecS2, vecS1) /SizeSquared(vecS1);
    intersectP = line1P1 + v1 * num2;
    glm::dvec3 v11 = intersectP - line2P1;
    //是否在线段2延长线上
    if (Size(v11) / Size(v2) > 1)
        return false;
    return true;
}

glm::dvec3 ConcavePolygonSegmentation::CrossProduct(const glm::dvec3 &v1, const glm::dvec3 &v2)
{
    return glm::dvec3(
        v1.y * v2.z - v1.z * v2.y,
        v1.z* v2.x - v1.x * v2.z,
        v1.x * v2.y - v1.y * v2.x);
}

double ConcavePolygonSegmentation::DotProduct(const glm::dvec3& v1, const glm::dvec3& v2)
{
    return v1.x * v2.x + v1.y * v2.y + v1.z* v2.z;
}

double ConcavePolygonSegmentation::Size(const glm::dvec3& v1)
{
    return FMath::Sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z);
}

double ConcavePolygonSegmentation::SizeSquared(const glm::dvec3& v1)
{
    return v1.x * v1.x + v1.y * v1.y + v1.z * v1.z;
}

glm::dvec3 ConcavePolygonSegmentation::Multiplication(const glm::dvec3& v1, const double Scale)
{
    return glm::dvec3(v1.x * Scale, v1.y * Scale, v1.z * Scale);
}