散点云处理笔记（三）：快速凸包算法（Quick Hull Algorithm）

散点云处理笔记（三）：快速凸包算法（Quick Hull Algorithm）

​ 凸包问题是计算几何中的一个经典问题，目标是找到包含给定点集的最小凸多边形(二维情形)或凸多面体(三维情形)。它解决的问题在于找到一个凸多边形或凸多边形，这个多边形包含了给定的 n 个点，使得这个多边形是包含这些点的最小凸集合。本笔记首先给出三维散点凸包算法的数学推导，可以从“凸包”这个基本概念出发，一步步深入到算法背后的数学原理，最后给出快速凸包算法的。

1. 最小凸包的数学定义

​ 最小凸包问题（Convex Hull Problem）的数学表达可以从集合论、凸分析和计算几何三个角度进行形式化定义。核心思想是：在包含给定点集的所有凸集中，寻找体积（或表面积、维度）最小的那个。下面从凸组合、交集、对偶优化表达的几个角度给出最小凸包问题的定义。

• Definition 1.1 基于凸组合的定义（构造性表达）

  设点集\(\mathbf{P}=\{\mathbf{p}_1,\mathbf{p}_2,\cdots,\mathbf{p}_n\}\subset\mathbb{R}^d\)，其中凸包\(\mathbf{conv}(\mathbf{P})\) 是\(\mathbf{P}\) 中所有点的凸组合构成的集合：

  \[\mathbf{conv}(\mathbf{P})=\left\{\sum_{i=1}^{n}\lambda_i\mathbf{p}_i\bigg|\lambda_i\geq0,\sum_{i=1}^n\lambda_i=1\right\} \tag{1} \]

  这个定义直接给出了凸包作为点集的构造方式，但不易直接用于算法设计。

• Definition 1.2 基于交集的定义（几何表达）

  凸包也可以表示为所有包含 \(\mathbf{P}\)的凸集的交集：

  \[\mathbf{conv}(\mathbf{P})=\bigcap\{\mathbf{C}\subseteq\mathbb{R}^d\big|\mathbf{C}\text{是凸包且}\mathbf{P}\subseteq{\mathbf{C}}\} \tag{2} \]

  这是“最小凸集”的精确数学刻画：它唯一且必然存在。

• Definition 1.3 基于支撑函数的对偶表达（优化视角）

  在凸分析中，凸集可以用其支撑函数 \(h_c(\mathbf{u})=\sup_{\mathbf{x}\in{\mathbf{c}}}\mathbf{u}^{T}\mathbf{x}\) 描述。凸包问题等于求一个凸包多面体\(H\)，使得其支撑函数等于点集支撑函数的上确界：

  \[h_{H}(\mathbf{u})=\max_{\mathbf{p}\in{P}}\mathbf{u}^T\mathbf{p}，\forall{\mathbf{u}\in{\mathbb{R}^d}} \tag{3} \]

  此时\(H=\mathbf{conv}(\mathbf{P})\)。这个表达与版空间表示紧密相关： 凸包是所有满足\(\mathbf{u}^T\mathbf{x}\leq{\max_{\mathbf{p}\in{P}}\mathbf{u}^T\mathbf{p}}\)的半空间的交集。

• Definition 1.4 凸包问题离散形式（计算几何常用）

  在实际算法中，我们通常要求输出凸包的顶点集（极点）及其面/边的邻接关系。因此，凸包问题可表述为：

  给定有限点集\(P\subset\mathbb{\mathbb{R}^d}\)，找出最小的子集\(V\subseteq{P}\)，使得\(\mathbf{conv}(V)=\mathbf{conv}(P)\)，并给出\(\mathbf{conv}(V)\)的\(\mathbf{facet}(d-1\text{维度})\) 的几何与拓朴信息。
  其中，\(\mathbf{p}\in{P}\)是极点（凸包顶点）当且仅当它不能表示为\(P\{p\}\)中点的凸组合。

• Definition 1.5 优化形式的等价表述（用于数学规划）

  凸包问题还可以写成线性规划的对偶形式：寻找一个凸多面体 \(Q\)，使得 \(P\subseteq{Q}\)且\(Q\)的某种度量（如体积）最小。但在离散情况下，这通常转化为枚举所有可能的外包多面体并比较。

​ 最小凸包问题的核心数学表达是 “包含给定点集的最小凸集”，它可以用凸组合、凸集交、支撑函数等方式等价定义。在计算几何中，我们通常寻求输出该凸集的顶点和面结构。

2.有符号体积 (Signed Volume),有符号距离（Signed Distance）与几何判定

​ 有符号体积（Signed Volume）是三维凸包算法中最核心的几何判定工具。它本质上是一个行列式，能够告诉我们四个点构成的四面体是“正向”还是“反向”，从而判断点与平面的相对位置、面的可见性以及三角形的定向。

• Definition 2.1 有符号体积的定义

  给定三维空间中的四个点\(A,B,C,D\)，它们构成一个四面体。有效符号体积由以下行列式给出：

  \[Volume(A,B,C,D)=\frac{1}{6}\left|\begin{matrix}A_x,A_y,A_z, 1\\ B_x,B_y,B_z,1\\ C_x,C_y,C_z,1\\ D_x,D_y,D_z,1 \end{matrix}\right| \tag{4} \]

  这个行列式的几何意义：从点\(\mathbf{A}\)到\(\mathbf{B},\mathbf{C},\mathbf{D}\) 的三个向量的混合积(Scalar Triple Product) 的\(\frac{1}{6}\) 倍：

  \[Volume(A,B,C,D)=\frac{1}{6}\cdot{\left((B-A)\times(C-A)\right)}\cdot(D-A) \tag{5} \]

  其中\(\times\)是向量叉乘，\(\cdot\)是点乘。

符号体积的符号含义可以揭示了四个点的空间关系：

• 正体积：\(D\)位于平面\(ABC\) 的正侧（即按照右侧法则，\(A\rightarrow{B}\rightarrow{C}\) 的朝向，\(D\)在法向量指向的一侧）。
• 负体积：D位于平面 \(ABC\)的负侧 (另一侧)。
• 零体积： 四点共面（退化四面体）。

注意：符号的具体正负依赖于顶点\(A,B,C\)的排列顺序。通常约定 \(A,B,C\) 按逆时针方向（从外部看）排列时，外法向指向外，此时正体积表示点在外部。其在凸包构建的作用如下：

2.1 点与平面的定向测试（Orientation Test）

在构建凸包时，经常需要判断一个点 \(P\) 是在某个平面 \(F\)的内侧还是外侧。

取平面上的三个非共线点（例如一个三角形的三个顶点 A,B,C），计算有符号体积 Volume(A,B,C,P)：

• 如果 \(volume>0\)：点 PP 在平面的正侧（通常定义为外部）。
• 如果 \(volume<0\)：点 PP 在平面的负侧（内部）。
• 如果 \(volume=0\)：点 PP 在平面上（共面）。

这个测试是增量法和 Quick hull 算法中剔除内部点和判断可见性的基础。

2.2 面的可见性测试（Visibility Test）

当向现有凸包添加一个新点 \(P\)时，必须找出凸包上哪些面“可见”——即从点 \(P\)看过去，这些面朝外的一面能被看到。
对于凸包上的每个面 \(F\)，我们已知它的外法向（由顶点顺序约定）。计算有符号体积\(Volume(A,B,C,P)\)，其中 \(A,B,C\)是 \(F\)的顶点且顺序与法向一致：

• 如果$ Volume>0$，表示 P位于面的外侧（可见）。
• 如果 \(Volume<0\)，表示 P 位于面的内侧（不可见，被遮挡）。

所有可见面的边界构成一个闭合的“地平线”（horizon），然后需要删除这些可见面，并将 \(P\)与地平线上的每条边连接，形成新面。

2.3 三角形朝向一致性（Orientation Consistency）

在输出凸包或进行三角剖分时，需要确保所有三角形的顶点排列顺序一致（例如全部为逆时针，从外部看）。通过计算有符号体积可以检查和修正朝向：

• 计算三角形 \(ABC\)与一个已知在凸包外部的点 \(P\)的体积，若为负则说明三角形朝向相反，需要翻转顶点顺序。

有符号体积是连接代数（行列式）与几何（定向、可见性）的桥梁。在三维凸包算法中，它几乎出现在每一个关键决策点：判断点是否在凸包内部、确定新点能“看到”哪些面、维持凸包所有面法向一致。理解并正确使用有符号体积，就等于掌握了实现三维凸包算法的数学钥匙。

• Definition 2.2 有符号距离的定义

对于由法向量 n（单位向量）和一点 \(\mathbf{p}_0\)确定的平面，点 x 到该平面的有符号距离为：

\[d=\mathbf{n}\cdot(\mathbf{x}-\mathbf{p}_0) \tag{6} \]

其中：

1. \(d>0\)：点位于平面正侧（法向量指向的一侧）。
2. \(d<0\)：点位于平面正侧（法向量指向的一侧）。
3. \(d=0\)：点在平面上。

3.最小凸包算法的算法总结

基于上述数学定义，主要的凸包算法遵循不同的构造思路。

• 增量法 (Incremental Algorithm)：归纳法的算法体现
  它本质上是数学归纳法在算法中的直接体现。其核心思想是：

  1. 归纳基础：从一个小的初始凸包（如一个四面体）开始。
  2. 归纳步骤：假设已经为前 k 个点构造好了凸包 CH(P_k)，现在加入一个新的点 p。
  3. 更新凸包：如果 p 在当前凸包内部，则凸包不变；如果 p 在外部，则需要删除所有能被 p 看到的“可见面”，这些面的边界会形成一个闭合的“地平线”。最后，将 p 与这条地平线上的每条边相连，形成新的三角面，从而更新凸包。

• 分治法 (Divide and Conquer)：归并思想的挑战
  该算法的思路是将点集递归地分成两半，分别计算左右两部分的凸包，最后将两个凸包“缝合”成一个完整的凸包。

  • 难点：缝合过程远比二维复杂，需要计算两个分离凸多面体的“外壳”（即同时与两个凸包相切的支撑面），这在三维空间中的计算相当复杂。
  • 地位：尽管分治法在理论上可以达到最优的 O(n log n) 时间复杂度，但由于实现难度高，在实际应用中不如随机化的增量算法或Quick hull普及。

• Quick hull算法：分治思想与几何选择
  正如其名，它借鉴了快速排序 (Quick Sort) 的“分治”哲学。与增量法每次处理一个随机点不同，Quick hull每次都选择距离当前面最远的点作为新的顶点。这个策略能让算法快速地逼近凸包形状，从而在平均情况下达到很高的效率。

算法	平均/期望复杂度	最坏复杂度	主要特点
增量法 (随机化)	O(n log n)	O(n²)	实现相对简单，思想直观，通过随机化保证平均性能。
Quick hull	O(n log n)	O(n²)	效率高，是Qhull等知名库的实现基础。
分治法	O(n log n)	O(n log n)	理论上最优，但实现复杂，常数较大。

4. Quick Hull 算法原理

​ 快速凸包算法也可以看成是增量算法的一种变种，与随机增量算法相比，它们的不同就在于每次迭代从面的外部点集中选择的点不同。随机增量算法从外部点集中随机的选择一个点，但是快速凸包算法是选择距离最远的一个点，著名的开源代码Qhull、 CGAL都有快速凸包算法的C++实现。本篇文章介绍三维的快速凸包算法的原理和实现。其基本思想：

​ 在介绍快速三维凸包算法前，先介绍算法中会频繁使用到的两个基本的几何操作：

1. 选取散点集上的3个散点\(\{v_1,v_2,v_3\}\)，确定一个平面。

   平面可以用方程\(\mathbf{n}\cdot\mathbf{P}+d=0\)表示，其平面的法向量\(\mathbf{n}=(v_2-v_1)\times(v_3-v_1),d=-\mathbf{n}\cdot{v_1}\)，叉积的顺序影响法向量的方向，若\(\mathbf{n}\) 是零向量，说明\(v_1,v_2,v_3\)三点共线。

2. 计算三维空间上点到平面的有符号距离。

   设三维点为\(P\),平面为\(\Gamma:\mathbf{n}\cdot{P}+d=0\)，那么点\(P\)到平面\(\Gamma\)的符号距离表示围为\(dist(P)=(\mathbf{n}\cdot{P}+d)/||\mathbf{n}||\)。：

   • 若\(dist(P)>0\)，则称点P在平面\(\Gamma\) 的上方(Above the Plane).
   • 若\(dist(P)<0\)，则称点P在平面\(\Gamma\)的下方(Below the Plane).
   • 若\(dist(P)=0\)，则称点P在平面\(\Gamma\) 上(On the Plane)。

   快速凸包算法是基于Beneath Beyond理论，增量算法也同样基于该理论，该理论如下所示，这里只考虑三维空间下的凸包：

   设\(H\)是一个\(R^3\) 空间上的凸包，\(p\) 是\(H\)上的一个点。\(F\)是凸包\(conv(p\bigcup{H})\) 上的面，当且仅当：

   • \(F\)是凸包\(H\) 的一个面且点\(p\)在面\(F\)的下方；
   • \(F\) 不是凸包\(H\)内的一个面，\(F\)是由凸包\(H\)的边和点\(p\)构成，点\(p\)在该边相邻的一个面的上方，在该边相邻的另一个面的下方。

   若点\(p\)在\(H\)内，则\(conv(p\bigcup{H})\)与\(H\) 重合，显然，结论成立。若点\(p\)在\(H\)外，分为两种情况，\(H\)是由极点\(\{p_0,p_1,p_3,p_3,p_4,p_5\}\) 构成的凸包，\(conv(p\bigcup{H})\) 是由极点\(\{p,p_0,p_1,p_2,p_3,p_4,p_5\}\) 构成的凸包。对于凸包\(H\)来说，点\(p\)在三角形面片\(\triangle{p_0p_3p_4}\) ，\(\triangle{p_0 p_5 p_1}\) ,\(\triangle{p_2 p_3 p_4}\) ，\(\triangle{p_2 p_5 p_3}\) 的上方，点\(p\)在三角形面片\(\triangle{p_0p_1p_4}\)，\(\triangle{p_0p_5p_1}\)，\(\triangle{p_2p_4p_1}\)，\(\triangle{p_2p_1p_5}\) 的下方，因此在边\(\overline{p_2 p_4}\), \(\overline{p_4p_0}\),\(\overline{p_0p_5}\),\(\overline{p_5p_2}\) 一侧的面片是在点\(p\) 的上方，另一侧的面片在点\(p\)的下方，这样的边为临界边。当一个面\(F\)在\(conv(p\bigcup{H})\) 上时，若点p在面\(F\)的下方，则\(F\)是\(H\)的一个面，否则，它是由点p与临界边构成的面片。

5. Quick Hull算法步骤

输入： 散点集\(\mathbf{P}=\{\mathbf{p}_1,\mathbf{p}_2,\mathbf{p}_3,\cdots,\mathbf{p}_n\}\subset \mathbb{R}^3\)

步骤1：构建初始凸包（四面体）

1. 找到沿 x,y,z 方向的极值点（最大 / 最小坐标）。
2. 从中选取四个不共面点\(v_0,v_1,v_2,v_3\)，构建初始四面体。
3. 对 4 个三角面分别计算平面方程\(\Gamma_i\)与外法向量\(\mathbf{n}_i\)。

利用符号体积进行四点不共面判断：

\[V=\frac{1}{6}|(\mathbf{v}_1-\mathbf{v}_0)\cdot[(\mathbf{v}_2-\mathbf{v}_0)\times(\mathbf{v}_3-\mathbf{v}_0)]|\neq0 \]

步骤2：为每个面建立外侧点集

对每个面\(\Gamma_i\)，将所有满足

\[dist(\Gamma_i,\mathbf{p})>0 \]

的点归入该面的外侧点集\(\mathbf{S}_{\Gamma_i}\) 。

步骤3： 递归扩展凸包（主循环）

循环直到所有面的外侧点集为空：

（1）选择全局最远外侧点：遍历所有非空\(S_{\Gamma_i}\)找到最远点\(\mathbf{p}^*\)

\[\mathbf{p}^*=argmax_{\mathbf{p}\in{S_{\Gamma_i}}}|dist(\mathbf{p},\Gamma_i)| \]

\(\mathbf{p}^*\) 点必为新凸包顶点。

（2）标记所有可见面，一个面\(\Gamma_{i}\) 对\(\mathbf{p}_*\)可见，当且仅当:

\[dist(\mathbf{p}^*,\Gamma_i)>\epsilon \]

所有可见面将被删除。

(3) 提取地平线（Horizon）:地平线是满足以下条件的边：边属于一个可见面、一个不可见面这些边构成闭合多边形环。

(4) 对地平线每条边 (a,b)，构造新三角面：\(\Gamma_{new}=(\mathbf{p}^*,\mathbf{a},\mathbf{b})\) 计算其外法向量与平面方程，保证法向量向外，保证凸性。

（5）外侧点重分配数学依据，原属于\(F_{vis}\)的外侧点，必须重新测试到新面\(\Gamma_{new}\) 的有向距离：

\[dist(\Gamma_new,\mathbf{p})>0 \]

满足则归入新面外侧集，否则视为内部点。

步骤4：清理与合并共面点

删除重复面、退化面；将落在凸包面上的点归入对应面，不影响凸包结构。

6. C++其代码实现

基础库源码链接，参见这里，下面是快速凸包算法的源码实现。

#define _CRTDBG_MAP_ALLOC
#include <stdlib.h>      // C/C++标准库 
#include <crtdbg.h>      // CRT调试库，用于内存泄漏检测
#include <time.h>        // 用于计时和随机数种子

#include "./include/SrGeometricTools.h"  // 自定义几何工具头文件：提供点/向量的基本运算（加减乘除、点乘、叉乘等）
#include "./include/SrDataType.h"        // 自定义数据类型头文件：定义SrPoint3D/SrVector3等基础几何类型

#include <list>      //双向链表，存储顶点、面片集合
#include <assert.h>  //断言，用于调试时检查程序逻辑合法性
#include <map>       //映射表，用于边界边管理、顶点/面片索引映射 

//-----------------------------宏定义----------------------------------
//面片标记：无状态
#define  FACET_NULL         0x00
//面片标记：已访问（用于搜索可见面片）
#define  FACET_VISITED      0x01
//面片标记：边界面片（用于构建新凸包面）
#define  FACET_BORDER       0x02

//----------------------------类型重定义-------------------------------
//三维点类型
typedef SrPoint3D                   cPoint;
//三维向量类型
typedef SrVector3                   cVector;

//---------------------------类前置声明---------------------------------
class cFacet;
class cEdge;
class cOutsideSet;
class cVertex;

//顶点列表 & 迭代器
typedef std::list<cVertex*>         VertexList;
typedef VertexList::iterator        VertexIterator;

//面片列表 & 迭代器
typedef std::list<cFacet*>          FacetList;
typedef FacetList::iterator         FacetIterator;

// 边界边映射：<顶点, 对应的边界边>
typedef std::map<cVertex*, cEdge*>  BoundaryEdgeMap;
typedef BoundaryEdgeMap::iterator   BoundaryEdgeIterator;

//-----------------------------顶点类-------------------------------------
class cVertex
{
public:
    cVertex()
    {
        mOnHull = false;                       // 默认不是凸包顶点
        mPoint.x = mPoint.y = mPoint.z = 0.0f; // 坐标初始化为0
    }
public:
    bool            mOnHull;            //Indicate whether or not the point is the extreme point of the convex polyhedron.
    cPoint          mPoint;             //The location information of this vertex.
};

//---------------------------外部点集合类-------------------------
//存储某个面片“外侧”的所有点（这些点会用于扩展凸包）
class cOutsideSet
{
public:
    cOutsideSet() {}
    ~cOutsideSet()
    {
        mVertexList.clear();         // 清空点集，释放链表资源
    }
public:
    VertexList  mVertexList;            //Container of outside point set.
};

//---------------------------平面类：三维空间中的平面----------------
class cPlane
{
public:
    cPlane() {}
    ~cPlane() {}

    // 通过三个不共线的点初始化一个平面
    bool initPlane(const cPoint& p0, const cPoint& p1, const cPoint& p2)
    {
        mNormal = (p1 - p0).cross(p2 - p0);
        if (mNormal.isZero())
            return false;
        //It's not necessary to normalize the vector here.
        //normal.normalize();
        mD = -mNormal.dot(p0);
        return true;
    }

    // 判断平面是否有效（法向量非零）
    bool isValid() const
    {
        return !mNormal.isZero();
    }

    // 计算点到平面的有向距离
    SrReal distance(const cPoint& point) const
    {
        return mNormal.dot(point) + mD;
    }

    // 判断点是否在平面的正方向一侧（凸包可见性判断核心）
    bool isOnPositiveSide(const cPoint& p) const
    {
        return GREATER(distance(p), 0);
    }

public:
    cVector     mNormal;    // 平面法向量
    SrReal      mD;         // 平面方程参数
};


// --------------------------- 面片类：凸包的三角面片 ---------------------------
class cFacet
{
public:
    
    //构造函数
    cFacet()
    {
        // 邻面初始化为空
        mNeighbor[0] = NULL;
        mNeighbor[1] = NULL;
        mNeighbor[2] = NULL;
        // 顶点初始化为空
        mVertex[0] = NULL;
        mVertex[1] = NULL;
        mVertex[2] = NULL;

        mOutsideSet = NULL;          //外部点集为空
        mVisitFlag = FACET_NULL;     //访问标记初始化
        mIterator = NULL;            //迭代器指针为空

    }

    // 清空面片数据
    void clear()
    {
        mNeighbor[0] = NULL;
        mNeighbor[1] = NULL;
        mNeighbor[2] = NULL;
        mVertex[0] = NULL;
        mVertex[1] = NULL;
        mVertex[2] = NULL;
        mOutsideSet = NULL;
        mVisitFlag = FACET_NULL;

    }

    //析构函数
    ~cFacet()
    {
        // 释放迭代器内存
        if (mIterator)
        {
            delete mIterator;
            mIterator = NULL;
        }
        // 释放外部点集
        if (mOutsideSet)
        {
            delete mOutsideSet;
            mOutsideSet = NULL;
        }
    }
    // 用三个顶点初始化三角面片
    void initFace(cVertex* p0, cVertex* p1, cVertex* p2)
    {
        mVertex[0] = p0;
        mVertex[1] = p1;
        mVertex[2] = p2;
    }

    // 设置面片的三个相邻面片
    void setNeighbors(cFacet* f0, cFacet* f1, cFacet* f2)
    {
        mNeighbor[0] = f0;
        mNeighbor[1] = f1;
        mNeighbor[2] = f2;
    }

    //找到此外部点集中距离当前面片最远的顶点
    const VertexIterator furthestVertex()const
    {
        ASSERT(mOutsideSet != NULL);
        cPlane plane;
        ASSERT(plane.initPlane(mVertex[0]->mPoint, mVertex[1]->mPoint, mVertex[2]->mPoint));
        VertexIterator iter = mOutsideSet->mVertexList.begin();
        VertexIterator iterVertex;
        SrReal maxDist = 0, dist;

        // 遍历所有外部点，找距离最大的点
        for (; iter != mOutsideSet->mVertexList.end(); iter++)
        {
            dist = plane.mNormal.dot((*iter)->mPoint) + plane.mD;
            if (LESS(maxDist, dist))
            {
                maxDist = dist;
                iterVertex = iter;
            }
        }
        return iterVertex;
    }
public:
    cOutsideSet* mOutsideSet;        // 该面片外侧的点集:The outside point set.
    cVertex* mVertex[3];             // 面片的三个顶点:The point information of this facet.
    cFacet* mNeighbor[3];            // 面片的三个相邻面片:Indicate the neighbor facets of this one.
    unsigned char   mVisitFlag;      // 访问标记（搜索可见面时使用）:Indicate the flag in the process of determining the visible face set.
    FacetIterator* mIterator;        // 指向面片在列表中的迭代器（用于快速删除）:Indicate the location in the pending face list.
};

//----------------------边缘类-----------------------------------------
//用于构建凸包扩展时的边界环
class cEdge
{
public:
    cEdge()
    {
        mPoint[0] = NULL;
        mPoint[1] = NULL;
        mNeighbor[0] = NULL;
        mNeighbor[1] = NULL;
    }

    //设置边的两个端点
    void setEndpoint(cVertex* p0, cVertex* p1)
    {
        mPoint[0] = p0;
        mPoint[1] = p1;
    }

    //设置边相邻的两个面片
    void setNeighbor(cFacet* f0, cFacet* f1)
    {
        mNeighbor[0] = f0;
        mNeighbor[1] = f1;
    }
public:
    cVertex* mPoint[2];              // 边的两个顶点: The point information of this edge.
    cFacet* mNeighbor[2];            // 边相邻的两个面片: The neighbor facets of this edge.
};



//-----------------------凸包输出结构----------------------------------------------
//面片索引结构
typedef struct _tFacet
{
    int   mFInx[3];  // 相邻面片索引
    int   mVInx[3];  // 顶点索引
}tFacet;

//最终凸包数据结构（供外部使用）
typedef struct
{
    SrPoint3D* mVertes;      // 凸包顶点数组
    tFacet* mFacet;          // 凸包面片数组
    int         mNumVertes;  // 顶点数量
    int         mNumFacet;   // 面片数量
}tHull;


// 导出凸包到 TXT 文件，给 MATLAB 可视化
void exportHullToFile(tHull* hull, const char* filename)
{
    FILE* f = fopen(filename, "w");
    if (!f) return;

    // 第一行：顶点数 面片数
    fprintf(f, "%d %d\n", hull->mNumVertes, hull->mNumFacet);

    // 输出所有顶点 x y z
    for (int i = 0; i < hull->mNumVertes; i++)
    {
        fprintf(f, "%.6f %.6f %.6f\n",
            hull->mVertes[i].x,
            hull->mVertes[i].y,
            hull->mVertes[i].z);
    }

    // 输出所有面片：三个顶点索引（MATLAB从1开始，所以+1）
    for (int i = 0; i < hull->mNumFacet; i++)
    {
        fprintf(f, "%d %d %d\n",
            hull->mFacet[i].mVInx[0] + 1,
            hull->mFacet[i].mVInx[1] + 1,
            hull->mFacet[i].mVInx[2] + 1);
    }

    fclose(f);
    printf("凸包数据已导出到: %s\n", filename);
}

//导出散点序列
bool exportPoints3dset(SrPoint3D *point3dset,int nPooints,const char* filename_output){

    FILE* f = fopen(filename_output, "w");

    if(f==nullptr){
        return false;
    }

    for(int ipoint = 0; ipoint < nPooints; ipoint++){
        fprintf(f,"%.6f %.6f %.6f\n",point3dset[ipoint].x,point3dset[ipoint].y,point3dset[ipoint].z);
    }

    fclose(f);

    return true;
}
//-----------------------------------------------------
class QuickHull
{
public:
    // 对外接口：输入点集，输出凸包
    bool quickHull(SrPoint3D* points, int numPoint, tHull* resultHull);

private:
    // 判断三点是否共线
    bool collinear(const cPoint& p0, const cPoint& p1, const cPoint& p2);
    // 判断四点是否共面
    bool coplanar(const cPoint& p0, const cPoint& p1, const cPoint& p2, const cPoint& p3);
    // 释放面片列表内存
    void deallocate(FacetList& ftList);
    //从指定点出发，找到所有“可见”的面片（会被删除的面）
    void findVisibleFacet(cVertex* furPoint, cFacet* f, FacetList& visibleSet, BoundaryEdgeMap& boudaryMap);
    // 用边界环和新顶点构建新面片
    void constructNewFacets(cVertex* point, FacetList& newFacetList, BoundaryEdgeMap& boundary);
    // 为每个面片分配外部点
    void determineOutsideSet(FacetList& facetList, VertexList& allVertex);
    // 更新待处理面片列表
    void updateFacetPendList(FacetList& facetPendList, FacetList& newFacetList, cFacet*& head);
    // 划分点到对应面片的外部集合
    void partitionOutsideSet(FacetList& facetPendList, FacetList& newFacetList, VertexList& allVertex, cFacet*& head);
    // 收集所有可见面的外部点（合并后重新分配）
    void gatherOutsideSet(FacetList& facetPendList, FacetList& visFacetList, VertexList& visOutsideSet);
    // QuickHull 核心递归迭代过程
    void quickHullScan(FacetList& facetPendList, cFacet*& head);
    // 初始化初始四面体（凸包初始种子）
    bool initTetrahedron(VertexList& vertexes, FacetList& tetrahedron);
    // 递归遍历凸包，导出顶点和面片索引
    void recursiveExport(cFacet*, std::map<cVertex*, int>&, std::map<cFacet*, int>&, int&, int&, std::list<cFacet*>&);
    // 将凸包整理成输出结构
    void exportHull(cFacet* head, tHull* hull);

};




bool QuickHull::quickHull(SrPoint3D* points, int numPoint, tHull* resultHull)
{
    // If the first and last point are equal the collinearity test some lines below will always be true.
    int i, sizePoint;
    for (i = numPoint - 1; i > 0; i--)
        if (points[i].x != points[0].x ||
            points[i].y != points[0].y ||
            points[i].z != points[0].z)
            break;

    sizePoint = i + 1;
    if (sizePoint <= 3)
        return false;

    //Copy all the vertexes into the vertex list.
    VertexList vertexList;
    cVertex* newVertex;
    VertexIterator vertexIter;
    cVertex** buffer = new cVertex * [sizePoint];
    for (i = 0; i < sizePoint; i++)
    {
        newVertex = new cVertex;
        newVertex->mPoint.x = points[i].x;
        newVertex->mPoint.y = points[i].y;
        newVertex->mPoint.z = points[i].z;
        vertexList.push_back(newVertex);
        buffer[i] = newVertex;
    }
    cFacet* head;
    FacetList facetPendList, newFacetList;
    //Initialize the first tetrahedron.
    if (!initTetrahedron(vertexList, newFacetList))
    {//If it fails, free the storage allocated to the vertex structure.
        for (i = 0; i < sizePoint; i++)
            delete buffer[i];
        delete[]buffer;
        return false;
    }

    partitionOutsideSet(facetPendList, newFacetList, vertexList, head);

    // If there exist no vertexes outside the hull, the tetrahedron is the hull.Or else, go into the if-exp.
    if (!facetPendList.empty())
    {
        quickHullScan(facetPendList, head);
    }

    //Export the facets and vertexes to the tHull structure.
    //Free the storage of the facets.
    exportHull(head, resultHull);

    //Free the storage of the vertexes.
    for (i = 0; i < sizePoint; i++)
        delete buffer[i];
    delete[]buffer;

    return true;
}

bool QuickHull::collinear(const cPoint& p0, const cPoint& p1, const cPoint& p2)
{
    cVector normal = (p1 - p0).cross(p2 - p0);
    if (EQUAL(normal.magnitudeSquared(), 0))
        return true;
    return false;
}

bool QuickHull::coplanar(const cPoint& p0, const cPoint& p1, const cPoint& p2, const cPoint& p3)
{
    SrVector3 normal = (p1 - p0).cross(p2 - p0);
    if (EQUAL(normal.dot(p3 - p0), 0))
        return true;
    return false;
}

void QuickHull::deallocate(FacetList& ftList)
{
    FacetIterator fcIter;
    for (fcIter = ftList.begin(); fcIter != ftList.end(); fcIter++)
    {
        cFacet* vlue = *fcIter;
        delete vlue;
    }
    ftList.clear();
}

void QuickHull::findVisibleFacet(cVertex* furPoint, cFacet* f, FacetList& visibleSet, BoundaryEdgeMap& boudaryMap)
{
    f->mVisitFlag = FACET_VISITED;
    visibleSet.push_back(f);
    FacetIterator facetIter = visibleSet.begin();
    cEdge* boundaryEdge = NULL;
    int i;
    cPlane plane;

    for (; facetIter != visibleSet.end(); facetIter++)
    {
        for (i = 0; i < 3; i++)
        {
            cFacet* neighbor = (*facetIter)->mNeighbor[i];
            if (neighbor->mVisitFlag == FACET_NULL)
            {
                neighbor->mVisitFlag = FACET_VISITED;
                ASSERT(plane.initPlane(neighbor->mVertex[0]->mPoint, neighbor->mVertex[1]->mPoint, neighbor->mVertex[2]->mPoint));
                if (plane.isOnPositiveSide(furPoint->mPoint))
                {

                    visibleSet.push_back(neighbor);
                }
                else
                {
                    neighbor->mVisitFlag = FACET_BORDER;
                    boundaryEdge = new cEdge;
                    boundaryEdge->setNeighbor(*facetIter, neighbor);
                    boundaryEdge->setEndpoint((*facetIter)->mVertex[i], (*facetIter)->mVertex[(i + 1) % 3]);
                    boudaryMap.insert(std::make_pair((*facetIter)->mVertex[i], boundaryEdge));

                }
            }
            else if (neighbor->mVisitFlag == FACET_BORDER)
            {
                boundaryEdge = new cEdge();
                boundaryEdge->setNeighbor(*facetIter, neighbor);
                boundaryEdge->setEndpoint((*facetIter)->mVertex[i], (*facetIter)->mVertex[(i + 1) % 3]);
                boudaryMap.insert(std::make_pair((*facetIter)->mVertex[i], boundaryEdge));
            }
        }
    }
}

void QuickHull::constructNewFacets(cVertex* point, FacetList& newFacetList, BoundaryEdgeMap& boundary)
{
    ASSERT(boundary.size() >= 3);
    BoundaryEdgeIterator current = boundary.begin();

    //The boundary edges are closed.
    current = boundary.begin();

    cEdge* edge = current->second;
    int i;
    while (!boundary.empty())
    {
        edge = current->second;
        cFacet* facet = new cFacet();
        //Clear the mVisitFlag. Because it's set FACET_BORDER in findVisibleFacet().
        edge->mNeighbor[1]->mVisitFlag = FACET_NULL;
        //Update neighbor facet of the invisible facet , at least one edge of which belong to the boundary.
        for (i = 0; i < 3; i++)
        {
            if (edge->mNeighbor[1]->mNeighbor[i] == edge->mNeighbor[0])
            {
                ASSERT(edge->mNeighbor[1]->mNeighbor[i]->mVisitFlag == FACET_VISITED);
                edge->mNeighbor[1]->mNeighbor[i] = facet;
                break;
            }
        }
        facet->mVertex[0] = point;
        facet->mVertex[1] = edge->mPoint[0];
        facet->mVertex[2] = edge->mPoint[1];
        facet->mNeighbor[1] = edge->mNeighbor[1];
        newFacetList.push_back(facet);
        boundary.erase(current);
        current = boundary.find(edge->mPoint[1]);
        delete edge;

    }
    //Update the neighbor facets of new facets.
    FacetIterator curFacet = newFacetList.begin();
    FacetIterator lastFacet = newFacetList.end();
    lastFacet--;
    for (; curFacet != newFacetList.end(); lastFacet = curFacet, curFacet++)
    {
        (*curFacet)->mNeighbor[0] = *lastFacet;
        (*lastFacet)->mNeighbor[2] = *curFacet;
    }
}

void QuickHull::determineOutsideSet(FacetList& facetList, VertexList& allVertex)
{
    FacetIterator facetIter;
    VertexIterator vertexIter, tempVertexIter;
    cPlane plane;
    for (facetIter = facetList.begin(); facetIter != facetList.end(); facetIter++)
    {
        ASSERT(plane.initPlane((*facetIter)->mVertex[0]->mPoint, (*facetIter)->mVertex[1]->mPoint, (*facetIter)->mVertex[2]->mPoint));
        for (vertexIter = allVertex.begin(); vertexIter != allVertex.end(); )
        {
            if (plane.isOnPositiveSide((*vertexIter)->mPoint))
            {//Update the outside vertex set of every new facet.
                tempVertexIter = vertexIter;
                tempVertexIter++;
                if (!(*facetIter)->mOutsideSet)
                {
                    (*facetIter)->mOutsideSet = new cOutsideSet();
                }
                (*facetIter)->mOutsideSet->mVertexList.splice((*facetIter)->mOutsideSet->mVertexList.end(), allVertex, vertexIter);
                vertexIter = tempVertexIter;
            }
            else
            {
                vertexIter++;
            }
        }
    }
}

void QuickHull::updateFacetPendList(FacetList& facetPendList, FacetList& newFacetList, cFacet*& head)
{
    //If there exist new facets with nonempty outside vertex set, push them back into the pending facet list.
    FacetIterator facetIter;
    FacetIterator tempFacetIter;
    for (facetIter = newFacetList.begin(); facetIter != newFacetList.end(); )
    {
        if ((*facetIter)->mOutsideSet)
        {
            tempFacetIter = facetIter;
            tempFacetIter++;
            facetPendList.splice(facetPendList.end(), newFacetList, facetIter);
            facetIter = facetPendList.end();
            facetIter--;
            if (!(*facetIter)->mIterator)
            {
                (*facetIter)->mIterator = new FacetIterator;
            }
            *(*facetIter)->mIterator = facetIter;
            facetIter = tempFacetIter;
        }
        else
        {
            head = (*facetIter);
            facetIter++;
        }
    }
}

void QuickHull::partitionOutsideSet(FacetList& facetPendList, FacetList& newFacetList, VertexList& allVertex, cFacet*& head)
{
    // For each facet, look at each unassigned point and decide if it belongs to the outside set of this facet.
    determineOutsideSet(newFacetList, allVertex);
    // Add all the facets with non-empty outside sets to the set of facets for further consideration
    updateFacetPendList(facetPendList, newFacetList, head);
}

void QuickHull::gatherOutsideSet(FacetList& facetPendList, FacetList& visFacetList, VertexList& visOutsideSet)
{
    FacetIterator facetIter;
    FacetIterator tempFacetIter;
    for (facetIter = visFacetList.begin(); facetIter != visFacetList.end(); )
    {
        //Copy the outside set of the visible facet into the visOutsideSet.
        cOutsideSet*& outsideSet = (*facetIter)->mOutsideSet;

        tempFacetIter = facetIter;
        tempFacetIter++;
        if (outsideSet)
        {
            visOutsideSet.splice(visOutsideSet.end(), outsideSet->mVertexList, outsideSet->mVertexList.begin(), outsideSet->mVertexList.end());
        }
        //If some facets in the visible set exist in the pending list, remove them.
        if ((*facetIter)->mIterator /*&& *(*facetIter)->mIterator!=facetPendList.end()*/)
        {
            facetPendList.erase(*(*facetIter)->mIterator);
        }
        //The visible triangles are unuseful.So deallocate them.
        facetIter = tempFacetIter;
    }
}

void QuickHull::quickHullScan(FacetList& facetPendList, cFacet*& head)
{
    FacetList       visFacetList;
    VertexList      visOutsideSet;
    FacetList       newFaceList;
    FacetIterator   facetIter;
    BoundaryEdgeMap boundary;
    cPlane plane;

    while (!facetPendList.empty())
    {
        ASSERT(visOutsideSet.empty());
        ASSERT(visFacetList.empty());
        ASSERT(boundary.empty());
        ASSERT(newFaceList.empty());

        FacetIterator f = facetPendList.begin();
        cFacet* facet = *f;
        //There must be at least one vertex.
        VertexIterator furVertexIter = facet->furthestVertex();
        cVertex* furVertex = *furVertexIter;
        facet->mOutsideSet->mVertexList.erase(furVertexIter);

        //Find the visible facet set by the furthest vertex .
        findVisibleFacet(furVertex, facet, visFacetList, boundary);

        ASSERT(!visFacetList.empty());
        ASSERT(!boundary.empty());

        gatherOutsideSet(facetPendList, visFacetList, visOutsideSet);

        constructNewFacets(furVertex, newFaceList, boundary);
        ASSERT(!newFaceList.empty());

        partitionOutsideSet(facetPendList, newFaceList, visOutsideSet, head);

        //Free the storage of the visible facet set.
        deallocate(visFacetList);
        visOutsideSet.clear();
        newFaceList.clear();
    }
}

bool QuickHull::initTetrahedron(VertexList& vertexes, FacetList& tetrahedron)
{
    //Check weather or not all the vertexes are collinear.
    VertexIterator ptIndex1 = vertexes.begin();
    VertexIterator ptIndex2 = ptIndex1;
    VertexIterator ptIndex3 = vertexes.end();
    ptIndex2++;
    ptIndex3--;
    while (ptIndex2 != ptIndex3 && collinear((*ptIndex1)->mPoint, (*ptIndex2)->mPoint, (*ptIndex3)->mPoint))
        ptIndex2++;
    //All the vertexes are collinear.
    if (ptIndex2 == ptIndex3)
        return false;

    // Find the vertexes that have minimum value, maximum value in the x-dimension and minimum value
    // in the y-dimension.
    VertexIterator minx = vertexes.begin(), maxx = vertexes.begin(), miny = vertexes.begin();
    VertexIterator vertexIter = vertexes.begin();
    for (; vertexIter != vertexes.end(); vertexIter++)
    {
        if ((*vertexIter)->mPoint.x < (*minx)->mPoint.x)
            minx = vertexIter;
        else if ((*vertexIter)->mPoint.x > (*maxx)->mPoint.x)
            maxx = vertexIter;
        else if ((*vertexIter)->mPoint.y < (*miny)->mPoint.y)
            miny = vertexIter;
    }
    //If the three maximum and maximum vertexes found above aren't collinear, initialize the first tetrahedron by using them.
    if (!collinear((*minx)->mPoint, (*maxx)->mPoint, (*miny)->mPoint))
    {
        ptIndex1 = minx;
        ptIndex2 = maxx;
        ptIndex3 = miny;
    }
    cPlane plane;
    ASSERT(plane.initPlane((*ptIndex1)->mPoint, (*ptIndex2)->mPoint, (*ptIndex3)->mPoint));

    //Find the vertexes that have minimum and maximum distance from the plane.
    SrReal minDist = plane.distance((*vertexes.begin())->mPoint);
    SrReal maxDist = minDist;
    vertexIter = vertexes.begin();
    VertexIterator  minPtIndex = vertexIter, maxPtIndex = vertexIter;
    vertexIter++;
    for (; vertexIter != vertexes.end(); vertexIter++)
    {
        SrReal dist = plane.distance((*vertexIter)->mPoint);
        if (dist < minDist)
        {
            minDist = dist;
            minPtIndex = vertexIter;
        }
        if (dist > maxDist)
        {
            maxDist = dist;
            maxPtIndex = vertexIter;
        }
    }
    VertexIterator extIndex = maxPtIndex;
    if (coplanar((*ptIndex1)->mPoint, (*ptIndex2)->mPoint, (*ptIndex3)->mPoint, (*extIndex)->mPoint))
    {
        //swap initP0 and  initP2. It's important for the direction of normal of the constructed plane.
        cVertex tmp = *(*ptIndex1);
        *(*ptIndex1) = *(*ptIndex3);
        *(*ptIndex3) = tmp;
        extIndex = minPtIndex;
        if (coplanar((*ptIndex1)->mPoint, (*ptIndex2)->mPoint, (*ptIndex3)->mPoint, (*extIndex)->mPoint))
        {// All of the vertexes are on the same plane.
            return false;
        }
    }

    //Initialize the first tetrahedron.
    cFacet* f0 = new cFacet();
    cFacet* f1 = new cFacet();
    cFacet* f2 = new cFacet();
    cFacet* f3 = new cFacet();

    f0->initFace(*ptIndex1, *ptIndex3, *ptIndex2);
    f1->initFace(*ptIndex1, *ptIndex2, *extIndex);
    f2->initFace(*ptIndex1, *extIndex, *ptIndex3);
    f3->initFace(*ptIndex2, *ptIndex3, *extIndex);

    f0->setNeighbors(f2, f3, f1);
    f1->setNeighbors(f0, f3, f2);
    f2->setNeighbors(f1, f3, f0);
    f3->setNeighbors(f0, f2, f1);

    vertexes.erase(ptIndex1);
    vertexes.erase(ptIndex2);
    vertexes.erase(ptIndex3);
    vertexes.erase(extIndex);

    tetrahedron.push_back(f0);
    tetrahedron.push_back(f1);
    tetrahedron.push_back(f2);
    tetrahedron.push_back(f3);

    return true;
}

void QuickHull::recursiveExport(cFacet* head, std::map<cVertex*, int>& vMap, std::map<cFacet*, int>& fMap, int& vId, int& fId, std::list<cFacet*>& fList)
{
    head->mVisitFlag = FACET_VISITED;
    fList.push_back(head);
    fMap.insert(std::make_pair(head, fId++));
    int i;
    for (i = 0; i < 3; i++)
    {
        if (vMap.find(head->mVertex[i]) == vMap.end())
        {
            vMap.insert(std::make_pair(head->mVertex[i], vId++));
        }
    }
    for (i = 0; i < 3; i++)
    {
        if (head->mNeighbor[i]->mVisitFlag != FACET_VISITED)
        {
            recursiveExport(head->mNeighbor[i], vMap, fMap, vId, fId, fList);
        }
    }
}

void QuickHull::exportHull(cFacet* head, tHull* hull)
{
    std::map<cVertex*, int> vMap;
    std::map<cFacet*, int>  fMap;
    std::list<cFacet*> fList;
    int vId = 0, fId = 0;

    recursiveExport(head, vMap, fMap, vId, fId, fList);

    hull->mNumVertes = vMap.size();
    hull->mVertes = new SrPoint3D[hull->mNumVertes];

    std::map<cVertex*, int>::iterator mapIter;
    for (mapIter = vMap.begin(); mapIter != vMap.end(); mapIter++)
    {
        ASSERT(mapIter->second < hull->mNumVertes);
        hull->mVertes[mapIter->second].x = mapIter->first->mPoint.x;
        hull->mVertes[mapIter->second].y = mapIter->first->mPoint.y;
        hull->mVertes[mapIter->second].z = mapIter->first->mPoint.z;
    }

    hull->mNumFacet = fList.size();
    hull->mFacet = new tFacet[hull->mNumFacet];

    std::list<cFacet*>::iterator fIter;
    int v0, v1, v2, fIndx = 0;
    cFacet* tempFacet;
    for (fIter = fList.begin(); fIter != fList.end(); )
    {
        v0 = vMap[(*fIter)->mVertex[0]];
        v1 = vMap[(*fIter)->mVertex[1]];
        v2 = vMap[(*fIter)->mVertex[2]];
        ASSERT(v0 < hull->mNumVertes);
        ASSERT(v1 < hull->mNumVertes);
        ASSERT(v2 < hull->mNumVertes);

        hull->mFacet[fIndx].mVInx[0] = v0;
        hull->mFacet[fIndx].mVInx[1] = v1;
        hull->mFacet[fIndx].mVInx[2] = v2;

        hull->mFacet[fIndx].mFInx[0] = fMap[(*fIter)->mNeighbor[0]];
        hull->mFacet[fIndx].mFInx[1] = fMap[(*fIter)->mNeighbor[1]];
        hull->mFacet[fIndx].mFInx[2] = fMap[(*fIter)->mNeighbor[2]];

        fIndx++;
        tempFacet = *fIter;
        fIter++;
        delete tempFacet;
    }
}



bool isConvex(tHull* hull)
{
    int i, j;
    SrVector3D normal;
    SrReal  d;
    for (i = 0; i < hull->mNumFacet; i++)
    {
        SrPoint3D v0 = hull->mVertes[hull->mFacet[i].mVInx[0]];
        SrPoint3D v1 = hull->mVertes[hull->mFacet[i].mVInx[1]];
        SrPoint3D v2 = hull->mVertes[hull->mFacet[i].mVInx[2]];
        normal = (v1 - v0).cross(v2 - v0);
        d = -normal.dot(v0);
        for (j = 0; j < hull->mNumVertes; j++)
        {
            SrReal result = normal.dot(hull->mVertes[0]) + d;
            if (GREATER(result, 0))
                return false;
        }
    }
    return true;
}

void testQuickHull3D()
{
    int numPoint = 1000, i;
    SrPoint3D* point = new SrPoint3D[numPoint];

    for (i = 0; i < numPoint; i++)
    {
        point[i].x = (float)rand();
        point[i].y = (float)rand();
        point[i].z = (float)rand();
    }

    tHull hull;
    QuickHull hull3d;

    double timeCount = clock();

    exportPoints3dset(point,numPoint,"point3d.txt");

    if (hull3d.quickHull(point, numPoint, &hull))
    {
        timeCount = (clock() - timeCount) / CLOCKS_PER_SEC;
        printf("time:%.4f\n", timeCount);
        printf("Number of Facets:%d, Number of vertexes:%d\n", hull.mNumFacet, hull.mNumVertes);

        ASSERT(isConvex(&hull));

        exportHullToFile(&hull, "convexhull_result.txt");

        delete[] hull.mVertes;
        delete[] hull.mFacet;
    }

    delete[]point;

}


int main(void)
{
    testQuickHull3D();
    _CrtDumpMemoryLeaks();
    return 0;
}