1697 lines
57 KiB
C++
1697 lines
57 KiB
C++
#ifndef QUICKHULL_HPP
|
|
#define QUICKHULL_HPP
|
|
|
|
#include <iostream>
|
|
#include <cmath>
|
|
#include <limits>
|
|
#include <memory>
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <vector>
|
|
#include <deque>
|
|
#include <unordered_map>
|
|
#include <fstream>
|
|
#include <cassert>
|
|
|
|
namespace quickhull
|
|
{
|
|
|
|
// ----------------------- BasicData Part -----------------------
|
|
|
|
template <typename T>
|
|
class Vector3
|
|
{
|
|
public:
|
|
Vector3() = default;
|
|
|
|
Vector3(T x, T y, T z) : x(x), y(y), z(z) {}
|
|
|
|
T x, y, z;
|
|
|
|
inline T dotProduct(const Vector3 &other) const
|
|
{
|
|
return x * other.x + y * other.y + z * other.z;
|
|
}
|
|
|
|
inline void normalize()
|
|
{
|
|
const T len = getLength();
|
|
x /= len;
|
|
y /= len;
|
|
z /= len;
|
|
}
|
|
|
|
inline Vector3 getNormalized() const
|
|
{
|
|
const T len = getLength();
|
|
return Vector3(x / len, y / len, z / len);
|
|
}
|
|
|
|
inline T getLength() const
|
|
{
|
|
return std::sqrt(x * x + y * y + z * z);
|
|
}
|
|
|
|
inline Vector3 operator-(const Vector3 &other) const
|
|
{
|
|
return Vector3(x - other.x, y - other.y, z - other.z);
|
|
}
|
|
|
|
inline Vector3 operator+(const Vector3 &other) const
|
|
{
|
|
return Vector3(x + other.x, y + other.y, z + other.z);
|
|
}
|
|
|
|
inline Vector3 &operator+=(const Vector3 &other)
|
|
{
|
|
x += other.x;
|
|
y += other.y;
|
|
z += other.z;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector3 &operator-=(const Vector3 &other)
|
|
{
|
|
x -= other.x;
|
|
y -= other.y;
|
|
z -= other.z;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector3 &operator*=(T c)
|
|
{
|
|
x *= c;
|
|
y *= c;
|
|
z *= c;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector3 &operator/=(T c)
|
|
{
|
|
x /= c;
|
|
y /= c;
|
|
z /= c;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector3 operator-() const
|
|
{
|
|
return Vector3(-x, -y, -z);
|
|
}
|
|
|
|
template <typename S>
|
|
inline Vector3 operator*(S c) const
|
|
{
|
|
return Vector3(x * c, y * c, z * c);
|
|
}
|
|
|
|
template <typename S>
|
|
inline Vector3 operator/(S c) const
|
|
{
|
|
return Vector3(x / c, y / c, z / c);
|
|
}
|
|
|
|
inline T getLengthSquared() const
|
|
{
|
|
return x * x + y * y + z * z;
|
|
}
|
|
|
|
inline bool operator!=(const Vector3 &o) const
|
|
{
|
|
return x != o.x || y != o.y || z != o.z;
|
|
}
|
|
|
|
// Projection onto another vector
|
|
inline Vector3 projection(const Vector3 &o) const
|
|
{
|
|
T C = dotProduct(o) / o.getLengthSquared();
|
|
return o * C;
|
|
}
|
|
|
|
inline Vector3 crossProduct(const Vector3 &rhs)
|
|
{
|
|
T a = y * rhs.z - z * rhs.y;
|
|
T b = z * rhs.x - x * rhs.z;
|
|
T c = x * rhs.y - y * rhs.x;
|
|
Vector3 product(a, b, c);
|
|
return product;
|
|
}
|
|
|
|
inline T getDistanceTo(const Vector3 &other) const
|
|
{
|
|
Vector3 diff = *this - other;
|
|
return diff.getLength();
|
|
}
|
|
|
|
inline T getSquaredDistanceTo(const Vector3 &other) const
|
|
{
|
|
const T dx = x - other.x;
|
|
const T dy = y - other.y;
|
|
const T dz = z - other.z;
|
|
return dx * dx + dy * dy + dz * dz;
|
|
}
|
|
};
|
|
|
|
// Overload also << operator for easy printing of debug data
|
|
template <typename T>
|
|
inline std::ostream &operator<<(std::ostream &os, const Vector3<T> &vec)
|
|
{
|
|
os << "(" << vec.x << "," << vec.y << "," << vec.z << ")";
|
|
return os;
|
|
}
|
|
|
|
template <typename T>
|
|
inline Vector3<T> operator*(T c, const Vector3<T> &v)
|
|
{
|
|
return Vector3<T>(v.x * c, v.y * c, v.z * c);
|
|
}
|
|
|
|
template <typename T>
|
|
class Plane
|
|
{
|
|
public:
|
|
Vector3<T> m_N;
|
|
|
|
// Signed distance (if normal is of length 1) to the plane from origin
|
|
T m_D;
|
|
|
|
// Normal length squared
|
|
T m_sqrNLength;
|
|
|
|
inline bool isPointOnPositiveSide(const Vector3<T> &Q) const
|
|
{
|
|
T d = m_N.dotProduct(Q) + m_D;
|
|
if (d >= 0)
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
Plane() = default;
|
|
|
|
// Construct a plane using normal N and any point P on the plane
|
|
Plane(const Vector3<T> &N, const Vector3<T> &P)
|
|
: m_N(N), m_D(-N.dotProduct(P)),
|
|
m_sqrNLength(m_N.x * m_N.x + m_N.y * m_N.y + m_N.z * m_N.z) {}
|
|
};
|
|
|
|
template <typename T>
|
|
struct Ray
|
|
{
|
|
const Vector3<T> m_S;
|
|
const Vector3<T> m_V;
|
|
const T m_VInvLengthSquared;
|
|
|
|
Ray(const Vector3<T> &S, const Vector3<T> &V)
|
|
: m_S(S), m_V(V), m_VInvLengthSquared(1 / m_V.getLengthSquared()) {}
|
|
};
|
|
|
|
template <typename T>
|
|
class Pool
|
|
{
|
|
std::vector<std::unique_ptr<T>> m_data;
|
|
|
|
public:
|
|
inline void clear()
|
|
{
|
|
m_data.clear();
|
|
}
|
|
|
|
inline void reclaim(std::unique_ptr<T> &ptr)
|
|
{
|
|
m_data.push_back(std::move(ptr));
|
|
}
|
|
|
|
inline std::unique_ptr<T> get()
|
|
{
|
|
if (m_data.size() == 0)
|
|
{
|
|
return std::unique_ptr<T>(new T());
|
|
}
|
|
auto it = m_data.end() - 1;
|
|
std::unique_ptr<T> r = std::move(*it);
|
|
m_data.erase(it);
|
|
return r;
|
|
}
|
|
};
|
|
|
|
template <typename T>
|
|
class VertexDataSource
|
|
{
|
|
const Vector3<T> *m_ptr;
|
|
size_t m_count;
|
|
|
|
public:
|
|
VertexDataSource(const Vector3<T> *ptr, size_t count)
|
|
: m_ptr(ptr), m_count(count) {}
|
|
|
|
VertexDataSource(const std::vector<Vector3<T>> &vec)
|
|
: m_ptr(&vec[0]), m_count(vec.size()) {}
|
|
|
|
VertexDataSource()
|
|
: m_ptr(nullptr), m_count(0) {}
|
|
|
|
inline VertexDataSource &operator=(const VertexDataSource &other) = default;
|
|
|
|
inline size_t size() const
|
|
{
|
|
return m_count;
|
|
}
|
|
|
|
inline const Vector3<T> &operator[](size_t index) const
|
|
{
|
|
return m_ptr[index];
|
|
}
|
|
|
|
inline const Vector3<T> *begin() const
|
|
{
|
|
return m_ptr;
|
|
}
|
|
|
|
inline const Vector3<T> *end() const
|
|
{
|
|
return m_ptr + m_count;
|
|
}
|
|
};
|
|
|
|
// ----------------------- MeshBuilder Part -----------------------
|
|
|
|
template <typename T>
|
|
class MeshBuilder
|
|
{
|
|
public:
|
|
struct HalfEdge
|
|
{
|
|
size_t m_endVertex;
|
|
size_t m_opp;
|
|
size_t m_face;
|
|
size_t m_next;
|
|
|
|
inline void disable()
|
|
{
|
|
m_endVertex = std::numeric_limits<size_t>::max();
|
|
}
|
|
|
|
inline bool isDisabled() const
|
|
{
|
|
return m_endVertex == std::numeric_limits<size_t>::max();
|
|
}
|
|
};
|
|
|
|
struct Face
|
|
{
|
|
size_t m_he;
|
|
Plane<T> m_P;
|
|
T m_mostDistantPointDist;
|
|
size_t m_mostDistantPoint;
|
|
size_t m_visibilityCheckedOnIteration;
|
|
std::uint8_t m_isVisibleFaceOnCurrentIteration : 1;
|
|
std::uint8_t m_inFaceStack : 1;
|
|
|
|
// Bit for each half edge assigned to this face,
|
|
// each being 0 or 1 depending on whether the
|
|
// edge belongs to horizon edge
|
|
std::uint8_t m_horizonEdgesOnCurrentIteration : 3;
|
|
std::unique_ptr<std::vector<size_t>> m_pointsOnPositiveSide;
|
|
|
|
Face() : m_he(std::numeric_limits<size_t>::max()),
|
|
m_mostDistantPointDist(0),
|
|
m_mostDistantPoint(0),
|
|
m_visibilityCheckedOnIteration(0),
|
|
m_isVisibleFaceOnCurrentIteration(0),
|
|
m_inFaceStack(0),
|
|
m_horizonEdgesOnCurrentIteration(0) {}
|
|
|
|
inline void disable()
|
|
{
|
|
m_he = std::numeric_limits<size_t>::max();
|
|
}
|
|
|
|
inline bool isDisabled() const
|
|
{
|
|
return m_he == std::numeric_limits<size_t>::max();
|
|
}
|
|
};
|
|
|
|
// Mesh data
|
|
std::vector<Face> m_faces;
|
|
std::vector<HalfEdge> m_halfEdges;
|
|
|
|
// When the mesh is modified and faces and half edges are
|
|
// removed from it, we do not actually remove them from
|
|
// the container vectors. Insted, they are marked as disabled
|
|
// which means that the indices can be reused when we need to
|
|
// add new faces and half edges to the mesh. We store the
|
|
// free indices in the following vectors.
|
|
std::vector<size_t> m_disabledFaces, m_disabledHalfEdges;
|
|
|
|
inline size_t addFace()
|
|
{
|
|
if (m_disabledFaces.size())
|
|
{
|
|
size_t index = m_disabledFaces.back();
|
|
auto &f = m_faces[index];
|
|
assert(f.isDisabled());
|
|
assert(!f.m_pointsOnPositiveSide);
|
|
f.m_mostDistantPointDist = 0;
|
|
m_disabledFaces.pop_back();
|
|
return index;
|
|
}
|
|
m_faces.emplace_back();
|
|
return m_faces.size() - 1;
|
|
}
|
|
|
|
inline size_t addHalfEdge()
|
|
{
|
|
if (m_disabledHalfEdges.size())
|
|
{
|
|
const size_t index = m_disabledHalfEdges.back();
|
|
m_disabledHalfEdges.pop_back();
|
|
return index;
|
|
}
|
|
m_halfEdges.emplace_back();
|
|
return m_halfEdges.size() - 1;
|
|
}
|
|
|
|
// Mark a face as disabled and return a pointer
|
|
// to the points that were on the positive of it.
|
|
inline std::unique_ptr<std::vector<size_t>> disableFace(size_t faceIndex)
|
|
{
|
|
auto &f = m_faces[faceIndex];
|
|
f.disable();
|
|
m_disabledFaces.push_back(faceIndex);
|
|
return std::move(f.m_pointsOnPositiveSide);
|
|
}
|
|
|
|
inline void disableHalfEdge(size_t heIndex)
|
|
{
|
|
auto &he = m_halfEdges[heIndex];
|
|
he.disable();
|
|
m_disabledHalfEdges.push_back(heIndex);
|
|
}
|
|
|
|
MeshBuilder() = default;
|
|
|
|
// Create a mesh with initial tetrahedron ABCD.
|
|
// Dot product of AB with the normal of triangle
|
|
// ABC should be negative.
|
|
inline void setup(size_t a, size_t b, size_t c, size_t d)
|
|
{
|
|
m_faces.clear();
|
|
m_halfEdges.clear();
|
|
m_disabledFaces.clear();
|
|
m_disabledHalfEdges.clear();
|
|
|
|
m_faces.reserve(4);
|
|
m_halfEdges.reserve(12);
|
|
|
|
// Create halfedges
|
|
HalfEdge AB;
|
|
AB.m_endVertex = b;
|
|
AB.m_opp = 6;
|
|
AB.m_face = 0;
|
|
AB.m_next = 1;
|
|
m_halfEdges.push_back(AB);
|
|
|
|
HalfEdge BC;
|
|
BC.m_endVertex = c;
|
|
BC.m_opp = 9;
|
|
BC.m_face = 0;
|
|
BC.m_next = 2;
|
|
m_halfEdges.push_back(BC);
|
|
|
|
HalfEdge CA;
|
|
CA.m_endVertex = a;
|
|
CA.m_opp = 3;
|
|
CA.m_face = 0;
|
|
CA.m_next = 0;
|
|
m_halfEdges.push_back(CA);
|
|
|
|
HalfEdge AC;
|
|
AC.m_endVertex = c;
|
|
AC.m_opp = 2;
|
|
AC.m_face = 1;
|
|
AC.m_next = 4;
|
|
m_halfEdges.push_back(AC);
|
|
|
|
HalfEdge CD;
|
|
CD.m_endVertex = d;
|
|
CD.m_opp = 11;
|
|
CD.m_face = 1;
|
|
CD.m_next = 5;
|
|
m_halfEdges.push_back(CD);
|
|
|
|
HalfEdge DA;
|
|
DA.m_endVertex = a;
|
|
DA.m_opp = 7;
|
|
DA.m_face = 1;
|
|
DA.m_next = 3;
|
|
m_halfEdges.push_back(DA);
|
|
|
|
HalfEdge BA;
|
|
BA.m_endVertex = a;
|
|
BA.m_opp = 0;
|
|
BA.m_face = 2;
|
|
BA.m_next = 7;
|
|
m_halfEdges.push_back(BA);
|
|
|
|
HalfEdge AD;
|
|
AD.m_endVertex = d;
|
|
AD.m_opp = 5;
|
|
AD.m_face = 2;
|
|
AD.m_next = 8;
|
|
m_halfEdges.push_back(AD);
|
|
|
|
HalfEdge DB;
|
|
DB.m_endVertex = b;
|
|
DB.m_opp = 10;
|
|
DB.m_face = 2;
|
|
DB.m_next = 6;
|
|
m_halfEdges.push_back(DB);
|
|
|
|
HalfEdge CB;
|
|
CB.m_endVertex = b;
|
|
CB.m_opp = 1;
|
|
CB.m_face = 3;
|
|
CB.m_next = 10;
|
|
m_halfEdges.push_back(CB);
|
|
|
|
HalfEdge BD;
|
|
BD.m_endVertex = d;
|
|
BD.m_opp = 8;
|
|
BD.m_face = 3;
|
|
BD.m_next = 11;
|
|
m_halfEdges.push_back(BD);
|
|
|
|
HalfEdge DC;
|
|
DC.m_endVertex = c;
|
|
DC.m_opp = 4;
|
|
DC.m_face = 3;
|
|
DC.m_next = 9;
|
|
m_halfEdges.push_back(DC);
|
|
|
|
// Create faces
|
|
Face ABC;
|
|
ABC.m_he = 0;
|
|
m_faces.push_back(std::move(ABC));
|
|
|
|
Face ACD;
|
|
ACD.m_he = 3;
|
|
m_faces.push_back(std::move(ACD));
|
|
|
|
Face BAD;
|
|
BAD.m_he = 6;
|
|
m_faces.push_back(std::move(BAD));
|
|
|
|
Face CBD;
|
|
CBD.m_he = 9;
|
|
m_faces.push_back(std::move(CBD));
|
|
}
|
|
|
|
inline std::array<size_t, 3> getVertexIndicesOfFace(const Face &f) const
|
|
{
|
|
std::array<size_t, 3> v;
|
|
const HalfEdge *he = &m_halfEdges[f.m_he];
|
|
v[0] = he->m_endVertex;
|
|
he = &m_halfEdges[he->m_next];
|
|
v[1] = he->m_endVertex;
|
|
he = &m_halfEdges[he->m_next];
|
|
v[2] = he->m_endVertex;
|
|
return v;
|
|
}
|
|
|
|
inline std::array<size_t, 2> getVertexIndicesOfHalfEdge(const HalfEdge &he) const
|
|
{
|
|
return {m_halfEdges[he.m_opp].m_endVertex, he.m_endVertex};
|
|
}
|
|
|
|
inline std::array<size_t, 3> getHalfEdgeIndicesOfFace(const Face &f) const
|
|
{
|
|
return {f.m_he, m_halfEdges[f.m_he].m_next, m_halfEdges[m_halfEdges[f.m_he].m_next].m_next};
|
|
}
|
|
};
|
|
|
|
// ----------------------- MathUtils Part -----------------------
|
|
|
|
namespace mathutils
|
|
{
|
|
|
|
template <typename T>
|
|
inline T getSquaredDistanceBetweenPointAndRay(const Vector3<T> &p, const Ray<T> &r)
|
|
{
|
|
const Vector3<T> s = p - r.m_S;
|
|
T t = s.dotProduct(r.m_V);
|
|
return s.getLengthSquared() - t * t * r.m_VInvLengthSquared;
|
|
}
|
|
|
|
// Note that the unit of distance returned is relative
|
|
// to plane's normal's length (divide by N.getNormalized()
|
|
// if needed to get the "real" distance).
|
|
template <typename T>
|
|
inline T getSignedDistanceToPlane(const Vector3<T> &v, const Plane<T> &p)
|
|
{
|
|
return p.m_N.dotProduct(v) + p.m_D;
|
|
}
|
|
|
|
template <typename T>
|
|
inline Vector3<T> getTriangleNormal(const Vector3<T> &a,
|
|
const Vector3<T> &b,
|
|
const Vector3<T> &c)
|
|
{
|
|
// We want to get (a-c).crossProduct(b-c)
|
|
// without constructing temp vectors
|
|
T x = a.x - c.x;
|
|
T y = a.y - c.y;
|
|
T z = a.z - c.z;
|
|
T rhsx = b.x - c.x;
|
|
T rhsy = b.y - c.y;
|
|
T rhsz = b.z - c.z;
|
|
T px = y * rhsz - z * rhsy;
|
|
T py = z * rhsx - x * rhsz;
|
|
T pz = x * rhsy - y * rhsx;
|
|
return Vector3<T>(px, py, pz);
|
|
}
|
|
|
|
} // namespace mathutils
|
|
|
|
// ----------------------- HalfEdgeMesh Part -----------------------
|
|
|
|
template <typename FloatType, typename IndexType>
|
|
class HalfEdgeMesh
|
|
{
|
|
public:
|
|
struct HalfEdge
|
|
{
|
|
IndexType m_endVertex;
|
|
IndexType m_opp;
|
|
IndexType m_face;
|
|
IndexType m_next;
|
|
};
|
|
|
|
struct Face
|
|
{
|
|
// Index of one of the half edges of this face
|
|
IndexType m_halfEdgeIndex;
|
|
};
|
|
|
|
std::vector<Vector3<FloatType>> m_vertices;
|
|
std::vector<Face> m_faces;
|
|
std::vector<HalfEdge> m_halfEdges;
|
|
|
|
HalfEdgeMesh(const MeshBuilder<FloatType> &builderObject,
|
|
const VertexDataSource<FloatType> &vertexData)
|
|
{
|
|
std::unordered_map<IndexType, IndexType> faceMapping;
|
|
std::unordered_map<IndexType, IndexType> halfEdgeMapping;
|
|
std::unordered_map<IndexType, IndexType> vertexMapping;
|
|
|
|
size_t i = 0;
|
|
for (const auto &face : builderObject.m_faces)
|
|
{
|
|
if (!face.isDisabled())
|
|
{
|
|
m_faces.push_back({static_cast<IndexType>(face.m_he)});
|
|
faceMapping[i] = m_faces.size() - 1;
|
|
|
|
const auto heIndices = builderObject.getHalfEdgeIndicesOfFace(face);
|
|
for (const auto heIndex : heIndices)
|
|
{
|
|
const IndexType vertexIndex = builderObject.m_halfEdges[heIndex].m_endVertex;
|
|
if (vertexMapping.count(vertexIndex) == 0)
|
|
{
|
|
m_vertices.push_back(vertexData[vertexIndex]);
|
|
vertexMapping[vertexIndex] = m_vertices.size() - 1;
|
|
}
|
|
}
|
|
}
|
|
i++;
|
|
}
|
|
|
|
i = 0;
|
|
for (const auto &halfEdge : builderObject.m_halfEdges)
|
|
{
|
|
if (!halfEdge.isDisabled())
|
|
{
|
|
m_halfEdges.push_back({static_cast<IndexType>(halfEdge.m_endVertex),
|
|
static_cast<IndexType>(halfEdge.m_opp),
|
|
static_cast<IndexType>(halfEdge.m_face),
|
|
static_cast<IndexType>(halfEdge.m_next)});
|
|
halfEdgeMapping[i] = m_halfEdges.size() - 1;
|
|
}
|
|
i++;
|
|
}
|
|
|
|
for (auto &face : m_faces)
|
|
{
|
|
assert(halfEdgeMapping.count(face.m_halfEdgeIndex) == 1);
|
|
face.m_halfEdgeIndex = halfEdgeMapping[face.m_halfEdgeIndex];
|
|
}
|
|
|
|
for (auto &he : m_halfEdges)
|
|
{
|
|
he.m_face = faceMapping[he.m_face];
|
|
he.m_opp = halfEdgeMapping[he.m_opp];
|
|
he.m_next = halfEdgeMapping[he.m_next];
|
|
he.m_endVertex = vertexMapping[he.m_endVertex];
|
|
}
|
|
}
|
|
};
|
|
|
|
// ----------------------- Convex Hull Part -----------------------
|
|
|
|
template <typename T>
|
|
class ConvexHull
|
|
{
|
|
std::unique_ptr<std::vector<Vector3<T>>> m_optimizedVertexBuffer;
|
|
VertexDataSource<T> m_vertices;
|
|
std::vector<size_t> m_indices;
|
|
|
|
public:
|
|
ConvexHull() {}
|
|
|
|
// Copy constructor
|
|
ConvexHull(const ConvexHull &o)
|
|
{
|
|
m_indices = o.m_indices;
|
|
if (o.m_optimizedVertexBuffer)
|
|
{
|
|
m_optimizedVertexBuffer.reset(new std::vector<Vector3<T>>(*o.m_optimizedVertexBuffer));
|
|
m_vertices = VertexDataSource<T>(*m_optimizedVertexBuffer);
|
|
}
|
|
else
|
|
{
|
|
m_vertices = o.m_vertices;
|
|
}
|
|
}
|
|
|
|
inline ConvexHull &operator=(const ConvexHull &o)
|
|
{
|
|
if (&o == this)
|
|
{
|
|
return *this;
|
|
}
|
|
m_indices = o.m_indices;
|
|
if (o.m_optimizedVertexBuffer)
|
|
{
|
|
m_optimizedVertexBuffer.reset(new std::vector<Vector3<T>>(*o.m_optimizedVertexBuffer));
|
|
m_vertices = VertexDataSource<T>(*m_optimizedVertexBuffer);
|
|
}
|
|
else
|
|
{
|
|
m_vertices = o.m_vertices;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
ConvexHull(ConvexHull &&o)
|
|
{
|
|
m_indices = std::move(o.m_indices);
|
|
if (o.m_optimizedVertexBuffer)
|
|
{
|
|
m_optimizedVertexBuffer = std::move(o.m_optimizedVertexBuffer);
|
|
o.m_vertices = VertexDataSource<T>();
|
|
m_vertices = VertexDataSource<T>(*m_optimizedVertexBuffer);
|
|
}
|
|
else
|
|
{
|
|
m_vertices = o.m_vertices;
|
|
}
|
|
}
|
|
|
|
inline ConvexHull &operator=(ConvexHull &&o)
|
|
{
|
|
if (&o == this)
|
|
{
|
|
return *this;
|
|
}
|
|
m_indices = std::move(o.m_indices);
|
|
if (o.m_optimizedVertexBuffer)
|
|
{
|
|
m_optimizedVertexBuffer = std::move(o.m_optimizedVertexBuffer);
|
|
o.m_vertices = VertexDataSource<T>();
|
|
m_vertices = VertexDataSource<T>(*m_optimizedVertexBuffer);
|
|
}
|
|
else
|
|
{
|
|
m_vertices = o.m_vertices;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
// Construct vertex and index buffers from
|
|
// half edge mesh and pointcloud
|
|
ConvexHull(const MeshBuilder<T> &mesh,
|
|
const VertexDataSource<T> &pointCloud,
|
|
bool CCW,
|
|
bool useOriginalIndices)
|
|
{
|
|
if (!useOriginalIndices)
|
|
{
|
|
m_optimizedVertexBuffer.reset(new std::vector<Vector3<T>>());
|
|
}
|
|
|
|
std::vector<bool> faceProcessed(mesh.m_faces.size(), false);
|
|
std::vector<size_t> faceStack;
|
|
// Map vertex indices from original point cloud
|
|
// to the new mesh vertex indices
|
|
std::unordered_map<size_t, size_t> vertexIndexMapping;
|
|
for (size_t i = 0; i < mesh.m_faces.size(); i++)
|
|
{
|
|
if (!mesh.m_faces[i].isDisabled())
|
|
{
|
|
faceStack.push_back(i);
|
|
break;
|
|
}
|
|
}
|
|
if (faceStack.size() == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
const size_t iCCW = CCW ? 1 : 0;
|
|
const size_t finalMeshFaceCount = mesh.m_faces.size() - mesh.m_disabledFaces.size();
|
|
m_indices.reserve(finalMeshFaceCount * 3);
|
|
|
|
while (faceStack.size())
|
|
{
|
|
auto it = faceStack.end() - 1;
|
|
size_t top = *it;
|
|
assert(!mesh.m_faces[top].isDisabled());
|
|
faceStack.erase(it);
|
|
if (faceProcessed[top])
|
|
{
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
faceProcessed[top] = true;
|
|
auto halfEdges = mesh.getHalfEdgeIndicesOfFace(mesh.m_faces[top]);
|
|
size_t adjacent[] = {mesh.m_halfEdges[mesh.m_halfEdges[halfEdges[0]].m_opp].m_face,
|
|
mesh.m_halfEdges[mesh.m_halfEdges[halfEdges[1]].m_opp].m_face,
|
|
mesh.m_halfEdges[mesh.m_halfEdges[halfEdges[2]].m_opp].m_face};
|
|
for (auto a : adjacent)
|
|
{
|
|
if (!faceProcessed[a] && !mesh.m_faces[a].isDisabled())
|
|
{
|
|
faceStack.push_back(a);
|
|
}
|
|
}
|
|
auto vertices = mesh.getVertexIndicesOfFace(mesh.m_faces[top]);
|
|
if (!useOriginalIndices)
|
|
{
|
|
for (auto &v : vertices)
|
|
{
|
|
auto itV = vertexIndexMapping.find(v);
|
|
if (itV == vertexIndexMapping.end())
|
|
{
|
|
m_optimizedVertexBuffer->push_back(pointCloud[v]);
|
|
vertexIndexMapping[v] = m_optimizedVertexBuffer->size() - 1;
|
|
v = m_optimizedVertexBuffer->size() - 1;
|
|
}
|
|
else
|
|
{
|
|
v = itV->second;
|
|
}
|
|
}
|
|
}
|
|
m_indices.push_back(vertices[0]);
|
|
m_indices.push_back(vertices[1 + iCCW]);
|
|
m_indices.push_back(vertices[2 - iCCW]);
|
|
}
|
|
}
|
|
|
|
if (!useOriginalIndices)
|
|
{
|
|
m_vertices = VertexDataSource<T>(*m_optimizedVertexBuffer);
|
|
}
|
|
else
|
|
{
|
|
m_vertices = pointCloud;
|
|
}
|
|
}
|
|
|
|
inline std::vector<size_t> &getIndexBuffer()
|
|
{
|
|
return m_indices;
|
|
}
|
|
|
|
inline const std::vector<size_t> &getIndexBuffer() const
|
|
{
|
|
return m_indices;
|
|
}
|
|
|
|
inline VertexDataSource<T> &getVertexBuffer()
|
|
{
|
|
return m_vertices;
|
|
}
|
|
|
|
inline const VertexDataSource<T> &getVertexBuffer() const
|
|
{
|
|
return m_vertices;
|
|
}
|
|
|
|
// Export the mesh to a Waveform OBJ file
|
|
inline void writeWaveformOBJ(const std::string &filename,
|
|
const std::string &objectName = "quickhull") const
|
|
{
|
|
std::ofstream objFile;
|
|
objFile.open(filename);
|
|
objFile << "o " << objectName << "\n";
|
|
for (const auto &v : getVertexBuffer())
|
|
{
|
|
objFile << "v " << v.x << " " << v.y << " " << v.z << "\n";
|
|
}
|
|
const auto &indBuf = getIndexBuffer();
|
|
size_t triangleCount = indBuf.size() / 3;
|
|
for (size_t i = 0; i < triangleCount; i++)
|
|
{
|
|
objFile << "f " << indBuf[i * 3] + 1 << " "
|
|
<< indBuf[i * 3 + 1] + 1 << " "
|
|
<< indBuf[i * 3 + 2] + 1 << "\n";
|
|
}
|
|
objFile.close();
|
|
}
|
|
};
|
|
|
|
// ----------------------- Quick Hull Part -----------------------
|
|
|
|
/*
|
|
* Implementation of the 3D QuickHull algorithm by Antti Kuukka
|
|
*
|
|
* No copyrights. What follows is 100% Public Domain.
|
|
*
|
|
*
|
|
*
|
|
* INPUT: a list of points in 3D space (for example, vertices of a 3D mesh)
|
|
*
|
|
* OUTPUT: a ConvexHull object which provides vertex and index buffers of
|
|
* the generated convex hull as a triangle mesh.
|
|
*
|
|
*
|
|
*
|
|
* The implementation is thread-safe if each thread is using its own
|
|
* QuickHull object.
|
|
*
|
|
*
|
|
* SUMMARY OF THE ALGORITHM:
|
|
* - Create initial simplex (tetrahedron) using extreme points.
|
|
* We have four faces now and they form a convex mesh M.
|
|
* - For each point, assign them to the first face for which they
|
|
* are on the positive side of (so each point is assigned to at
|
|
* most one face). Points inside the initial tetrahedron are left
|
|
* behind now and no longer affect the calculations.
|
|
* - Add all faces that have points assigned to them to Face Stack.
|
|
* - Iterate until Face Stack is empty:
|
|
* - Pop topmost face F from the stack
|
|
* - From the points assigned to F, pick the point P that is
|
|
* farthest away from the plane defined by F.
|
|
* - Find all faces of M that have P on their positive side.
|
|
* Let us call these the "visible faces".
|
|
* - Because of the way M is constructed, these faces are connected.
|
|
* Solve their horizon edge loop.
|
|
* - "Extrude to P": Create new faces by connecting P with the points
|
|
* belonging to the horizon edge. Add the new faces to M and remove
|
|
* the visible faces from M.
|
|
* - Each point that was assigned to visible faces is now assigned
|
|
* to at most one of the newly created faces.
|
|
* - Those new faces that have points assigned to them are added
|
|
* to the top of Face Stack.
|
|
* - M is now the convex hull.
|
|
*
|
|
* TO DO:
|
|
* - Implement a proper 2D QuickHull and use that to solve the degenerate 2D case
|
|
* (when all the points lie on the same plane in 3D space).
|
|
* */
|
|
|
|
struct DiagnosticsData
|
|
{
|
|
// How many times QuickHull failed to solve the horizon edge. Failures lead to
|
|
// degenerated convex hulls.
|
|
size_t m_failedHorizonEdges;
|
|
|
|
DiagnosticsData() : m_failedHorizonEdges(0) {}
|
|
};
|
|
|
|
template <typename T>
|
|
inline T defaultEps()
|
|
{
|
|
return 0.0000001;
|
|
}
|
|
|
|
template <typename T>
|
|
class QuickHull
|
|
{
|
|
using vec3 = Vector3<T>;
|
|
|
|
T m_epsilon, m_epsilonSquared, m_scale;
|
|
bool m_planar;
|
|
std::vector<vec3> m_planarPointCloudTemp;
|
|
VertexDataSource<T> m_vertexData;
|
|
MeshBuilder<T> m_mesh;
|
|
std::array<size_t, 6> m_extremeValues;
|
|
DiagnosticsData m_diagnostics;
|
|
|
|
// Temporary variables used during iteration process
|
|
std::vector<size_t> m_newFaceIndices;
|
|
std::vector<size_t> m_newHalfEdgeIndices;
|
|
std::vector<std::unique_ptr<std::vector<size_t>>> m_disabledFacePointVectors;
|
|
std::vector<size_t> m_visibleFaces;
|
|
std::vector<size_t> m_horizonEdges;
|
|
struct FaceData
|
|
{
|
|
size_t m_faceIndex;
|
|
// If the face turns out not to be visible, this half edge will be marked as horizon edge
|
|
size_t m_enteredFromHalfEdge;
|
|
FaceData(size_t fi, size_t he) : m_faceIndex(fi), m_enteredFromHalfEdge(he) {}
|
|
};
|
|
std::vector<FaceData> m_possiblyVisibleFaces;
|
|
std::deque<size_t> m_faceList;
|
|
|
|
// Create a half edge mesh representing the base tetrahedron from which the QuickHull
|
|
// iteration proceeds. m_extremeValues must be properly set up when this is called.
|
|
inline void setupInitialTetrahedron()
|
|
{
|
|
const size_t vertexCount = m_vertexData.size();
|
|
|
|
// If we have at most 4 points, just return a degenerate tetrahedron:
|
|
if (vertexCount <= 4)
|
|
{
|
|
size_t v[4] = {0,
|
|
std::min((size_t)1, vertexCount - 1),
|
|
std::min((size_t)2, vertexCount - 1),
|
|
std::min((size_t)3, vertexCount - 1)};
|
|
const Vector3<T> N = mathutils::getTriangleNormal(m_vertexData[v[0]],
|
|
m_vertexData[v[1]],
|
|
m_vertexData[v[2]]);
|
|
const Plane<T> trianglePlane(N, m_vertexData[v[0]]);
|
|
if (trianglePlane.isPointOnPositiveSide(m_vertexData[v[3]]))
|
|
{
|
|
std::swap(v[0], v[1]);
|
|
}
|
|
return m_mesh.setup(v[0], v[1], v[2], v[3]);
|
|
}
|
|
|
|
// Find two most distant extreme points.
|
|
T maxD = m_epsilonSquared;
|
|
std::pair<size_t, size_t> selectedPoints;
|
|
for (size_t i = 0; i < 6; i++)
|
|
{
|
|
for (size_t j = i + 1; j < 6; j++)
|
|
{
|
|
const T d = m_vertexData[m_extremeValues[i]].getSquaredDistanceTo(m_vertexData[m_extremeValues[j]]);
|
|
if (d > maxD)
|
|
{
|
|
maxD = d;
|
|
selectedPoints = {m_extremeValues[i], m_extremeValues[j]};
|
|
}
|
|
}
|
|
}
|
|
if (maxD == m_epsilonSquared)
|
|
{
|
|
// A degenerate case: the point cloud seems to consists of a single point
|
|
return m_mesh.setup(0,
|
|
std::min((size_t)1, vertexCount - 1),
|
|
std::min((size_t)2, vertexCount - 1),
|
|
std::min((size_t)3, vertexCount - 1));
|
|
}
|
|
assert(selectedPoints.first != selectedPoints.second);
|
|
|
|
// Find the most distant point to the line between the two chosen extreme points.
|
|
const Ray<T> r(m_vertexData[selectedPoints.first],
|
|
(m_vertexData[selectedPoints.second] - m_vertexData[selectedPoints.first]));
|
|
maxD = m_epsilonSquared;
|
|
size_t maxI = std::numeric_limits<size_t>::max();
|
|
const size_t vCount = m_vertexData.size();
|
|
for (size_t i = 0; i < vCount; i++)
|
|
{
|
|
const T distToRay = mathutils::getSquaredDistanceBetweenPointAndRay(m_vertexData[i], r);
|
|
if (distToRay > maxD)
|
|
{
|
|
maxD = distToRay;
|
|
maxI = i;
|
|
}
|
|
}
|
|
if (maxD == m_epsilonSquared)
|
|
{
|
|
// It appears that the point cloud belongs to a 1 dimensional
|
|
// subspace of R^3: convex hull has no volume => return a thin
|
|
// triangle. Pick any point other than selectedPoints.first and
|
|
// selectedPoints.second as the third point of the triangle
|
|
auto it = std::find_if(m_vertexData.begin(), m_vertexData.end(), [&](const vec3 &ve) {
|
|
return ve != m_vertexData[selectedPoints.first] && ve != m_vertexData[selectedPoints.second];
|
|
});
|
|
const size_t thirdPoint = (it == m_vertexData.end()) ? selectedPoints.first : std::distance(m_vertexData.begin(), it);
|
|
it = std::find_if(m_vertexData.begin(), m_vertexData.end(), [&](const vec3 &ve) {
|
|
return ve != m_vertexData[selectedPoints.first] && ve != m_vertexData[selectedPoints.second] && ve != m_vertexData[thirdPoint];
|
|
});
|
|
const size_t fourthPoint = (it == m_vertexData.end()) ? selectedPoints.first : std::distance(m_vertexData.begin(), it);
|
|
return m_mesh.setup(selectedPoints.first, selectedPoints.second, thirdPoint, fourthPoint);
|
|
}
|
|
|
|
// These three points form the base triangle for our tetrahedron.
|
|
assert(selectedPoints.first != maxI && selectedPoints.second != maxI);
|
|
std::array<size_t, 3> baseTriangle{selectedPoints.first, selectedPoints.second, maxI};
|
|
const Vector3<T> baseTriangleVertices[] = {m_vertexData[baseTriangle[0]],
|
|
m_vertexData[baseTriangle[1]],
|
|
m_vertexData[baseTriangle[2]]};
|
|
|
|
// Next step is to find the 4th vertex of the tetrahedron. We naturally
|
|
// choose the point farthest away from the triangle plane.
|
|
maxD = m_epsilon;
|
|
maxI = 0;
|
|
const Vector3<T> N = mathutils::getTriangleNormal(baseTriangleVertices[0],
|
|
baseTriangleVertices[1],
|
|
baseTriangleVertices[2]);
|
|
Plane<T> trianglePlane(N, baseTriangleVertices[0]);
|
|
for (size_t i = 0; i < vCount; i++)
|
|
{
|
|
const T d = std::abs(mathutils::getSignedDistanceToPlane(m_vertexData[i], trianglePlane));
|
|
if (d > maxD)
|
|
{
|
|
maxD = d;
|
|
maxI = i;
|
|
}
|
|
}
|
|
if (maxD == m_epsilon)
|
|
{
|
|
// All the points seem to lie on a 2D subspace of R^3. How to handle this? Well, let's add
|
|
// one extra point to the point cloud so that the convex hull will have volume.
|
|
m_planar = true;
|
|
const vec3 N1 = mathutils::getTriangleNormal(baseTriangleVertices[1],
|
|
baseTriangleVertices[2],
|
|
baseTriangleVertices[0]);
|
|
m_planarPointCloudTemp.clear();
|
|
m_planarPointCloudTemp.insert(m_planarPointCloudTemp.begin(),
|
|
m_vertexData.begin(),
|
|
m_vertexData.end());
|
|
const vec3 extraPoint = N1 + m_vertexData[0];
|
|
m_planarPointCloudTemp.push_back(extraPoint);
|
|
maxI = m_planarPointCloudTemp.size() - 1;
|
|
m_vertexData = VertexDataSource<T>(m_planarPointCloudTemp);
|
|
}
|
|
|
|
// Enforce CCW orientation (if user prefers clockwise orientation,
|
|
// swap two vertices in each triangle when final mesh is created)
|
|
const Plane<T> triPlane(N, baseTriangleVertices[0]);
|
|
if (triPlane.isPointOnPositiveSide(m_vertexData[maxI]))
|
|
{
|
|
std::swap(baseTriangle[0], baseTriangle[1]);
|
|
}
|
|
|
|
// Create a tetrahedron half edge mesh and compute planes defined by each triangle
|
|
m_mesh.setup(baseTriangle[0], baseTriangle[1], baseTriangle[2], maxI);
|
|
for (auto &f : m_mesh.m_faces)
|
|
{
|
|
auto v = m_mesh.getVertexIndicesOfFace(f);
|
|
const Vector3<T> &va = m_vertexData[v[0]];
|
|
const Vector3<T> &vb = m_vertexData[v[1]];
|
|
const Vector3<T> &vc = m_vertexData[v[2]];
|
|
const Vector3<T> N1 = mathutils::getTriangleNormal(va, vb, vc);
|
|
const Plane<T> plane(N1, va);
|
|
f.m_P = plane;
|
|
}
|
|
|
|
// Finally we assign a face for each vertex outside the tetrahedron
|
|
// (vertices inside the tetrahedron have no role anymore)
|
|
for (size_t i = 0; i < vCount; i++)
|
|
{
|
|
for (auto &face : m_mesh.m_faces)
|
|
{
|
|
if (addPointToFace(face, i))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Given a list of half edges, try to rearrange them so
|
|
// that they form a loop. Return true on success.
|
|
inline bool reorderHorizonEdges(std::vector<size_t> &horizonEdges)
|
|
{
|
|
const size_t horizonEdgeCount = horizonEdges.size();
|
|
for (size_t i = 0; i < horizonEdgeCount - 1; i++)
|
|
{
|
|
const size_t endVertex = m_mesh.m_halfEdges[horizonEdges[i]].m_endVertex;
|
|
bool foundNext = false;
|
|
for (size_t j = i + 1; j < horizonEdgeCount; j++)
|
|
{
|
|
const size_t beginVertex = m_mesh.m_halfEdges[m_mesh.m_halfEdges[horizonEdges[j]].m_opp].m_endVertex;
|
|
if (beginVertex == endVertex)
|
|
{
|
|
std::swap(horizonEdges[i + 1], horizonEdges[j]);
|
|
foundNext = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!foundNext)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
assert(m_mesh.m_halfEdges[horizonEdges[horizonEdges.size() - 1]].m_endVertex == m_mesh.m_halfEdges[m_mesh.m_halfEdges[horizonEdges[0]].m_opp].m_endVertex);
|
|
return true;
|
|
}
|
|
|
|
// Find indices of extreme values (max x, min x, max y, min y, max z, min z) for the given point cloud
|
|
inline std::array<size_t, 6> getExtremeValues()
|
|
{
|
|
std::array<size_t, 6> outIndices{0, 0, 0, 0, 0, 0};
|
|
T extremeVals[6] = {m_vertexData[0].x,
|
|
m_vertexData[0].x,
|
|
m_vertexData[0].y,
|
|
m_vertexData[0].y,
|
|
m_vertexData[0].z,
|
|
m_vertexData[0].z};
|
|
const size_t vCount = m_vertexData.size();
|
|
for (size_t i = 1; i < vCount; i++)
|
|
{
|
|
const Vector3<T> &pos = m_vertexData[i];
|
|
if (pos.x > extremeVals[0])
|
|
{
|
|
extremeVals[0] = pos.x;
|
|
outIndices[0] = i;
|
|
}
|
|
else if (pos.x < extremeVals[1])
|
|
{
|
|
extremeVals[1] = pos.x;
|
|
outIndices[1] = i;
|
|
}
|
|
if (pos.y > extremeVals[2])
|
|
{
|
|
extremeVals[2] = pos.y;
|
|
outIndices[2] = i;
|
|
}
|
|
else if (pos.y < extremeVals[3])
|
|
{
|
|
extremeVals[3] = pos.y;
|
|
outIndices[3] = i;
|
|
}
|
|
if (pos.z > extremeVals[4])
|
|
{
|
|
extremeVals[4] = pos.z;
|
|
outIndices[4] = i;
|
|
}
|
|
else if (pos.z < extremeVals[5])
|
|
{
|
|
extremeVals[5] = pos.z;
|
|
outIndices[5] = i;
|
|
}
|
|
}
|
|
return outIndices;
|
|
}
|
|
|
|
// Compute scale of the vertex data.
|
|
inline T getScale(const std::array<size_t, 6> &extremeValues)
|
|
{
|
|
{
|
|
T s = 0;
|
|
for (size_t i = 0; i < 6; i++)
|
|
{
|
|
const T *v = (const T *)(&m_vertexData[extremeValues[i]]);
|
|
v += i / 2;
|
|
auto a = std::abs(*v);
|
|
if (a > s)
|
|
{
|
|
s = a;
|
|
}
|
|
}
|
|
return s;
|
|
}
|
|
}
|
|
|
|
// Each face contains a unique pointer to a vector of indices.
|
|
// However, many - often most - faces do not have any points
|
|
// on the positive side of them especially at the the end of
|
|
// the iteration. When a face is removed from the mesh, its
|
|
// associated point vector, if such exists, is moved to the
|
|
// index vector pool, and when we need to add new faces with
|
|
// points on the positive side to the mesh, we reuse these
|
|
// vectors. This reduces the amount of std::vectors we have
|
|
// to deal with, and impact on performance is remarkable.
|
|
Pool<std::vector<size_t>> m_indexVectorPool;
|
|
inline std::unique_ptr<std::vector<size_t>> getIndexVectorFromPool();
|
|
inline void reclaimToIndexVectorPool(std::unique_ptr<std::vector<size_t>> &ptr);
|
|
|
|
// Associates a point with a face if the point resides on the positive
|
|
// side of the plane. Returns true if the points was on the positive side.
|
|
inline bool addPointToFace(typename MeshBuilder<T>::Face &f, size_t pointIndex);
|
|
|
|
// This will update m_mesh from which we create the ConvexHull
|
|
// object that getConvexHull function returns
|
|
inline void createConvexHalfEdgeMesh()
|
|
{
|
|
m_visibleFaces.clear();
|
|
m_horizonEdges.clear();
|
|
m_possiblyVisibleFaces.clear();
|
|
|
|
// Compute base tetrahedron
|
|
setupInitialTetrahedron();
|
|
assert(m_mesh.m_faces.size() == 4);
|
|
|
|
// Init face stack with those faces that have points assigned to them
|
|
m_faceList.clear();
|
|
for (size_t i = 0; i < 4; i++)
|
|
{
|
|
auto &f = m_mesh.m_faces[i];
|
|
if (f.m_pointsOnPositiveSide && f.m_pointsOnPositiveSide->size() > 0)
|
|
{
|
|
m_faceList.push_back(i);
|
|
f.m_inFaceStack = 1;
|
|
}
|
|
}
|
|
|
|
// Process faces until the face list is empty.
|
|
size_t iter = 0;
|
|
while (!m_faceList.empty())
|
|
{
|
|
iter++;
|
|
if (iter == std::numeric_limits<size_t>::max())
|
|
{
|
|
// Visible face traversal marks visited faces with iteration counter
|
|
//(to mark that the face has been visited on this iteration) and the
|
|
// max value represents unvisited faces. At this point we have to reset
|
|
// iteration counter. This shouldn't be an issue on 64 bit machines.
|
|
iter = 0;
|
|
}
|
|
|
|
const size_t topFaceIndex = m_faceList.front();
|
|
m_faceList.pop_front();
|
|
|
|
auto &tf = m_mesh.m_faces[topFaceIndex];
|
|
tf.m_inFaceStack = 0;
|
|
|
|
assert(!tf.m_pointsOnPositiveSide || tf.m_pointsOnPositiveSide->size() > 0);
|
|
if (!tf.m_pointsOnPositiveSide || tf.isDisabled())
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Pick the most distant point to this triangle plane
|
|
// as the point to which we extrude
|
|
const vec3 &activePoint = m_vertexData[tf.m_mostDistantPoint];
|
|
const size_t activePointIndex = tf.m_mostDistantPoint;
|
|
|
|
// Find out the faces that have our active point on their positive side
|
|
// (these are the "visible faces"). The face on top of the stack of course
|
|
// is one of them. At the same time, we create a list of horizon edges.
|
|
m_horizonEdges.clear();
|
|
m_possiblyVisibleFaces.clear();
|
|
m_visibleFaces.clear();
|
|
m_possiblyVisibleFaces.emplace_back(topFaceIndex, std::numeric_limits<size_t>::max());
|
|
while (m_possiblyVisibleFaces.size())
|
|
{
|
|
const auto faceData = m_possiblyVisibleFaces.back();
|
|
m_possiblyVisibleFaces.pop_back();
|
|
auto &pvf = m_mesh.m_faces[faceData.m_faceIndex];
|
|
assert(!pvf.isDisabled());
|
|
|
|
if (pvf.m_visibilityCheckedOnIteration == iter)
|
|
{
|
|
if (pvf.m_isVisibleFaceOnCurrentIteration)
|
|
{
|
|
continue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
const Plane<T> &P = pvf.m_P;
|
|
pvf.m_visibilityCheckedOnIteration = iter;
|
|
const T d = P.m_N.dotProduct(activePoint) + P.m_D;
|
|
if (d > 0)
|
|
{
|
|
pvf.m_isVisibleFaceOnCurrentIteration = 1;
|
|
pvf.m_horizonEdgesOnCurrentIteration = 0;
|
|
m_visibleFaces.push_back(faceData.m_faceIndex);
|
|
for (auto heIndex : m_mesh.getHalfEdgeIndicesOfFace(pvf))
|
|
{
|
|
if (m_mesh.m_halfEdges[heIndex].m_opp != faceData.m_enteredFromHalfEdge)
|
|
{
|
|
m_possiblyVisibleFaces.emplace_back(m_mesh.m_halfEdges[m_mesh.m_halfEdges[heIndex].m_opp].m_face, heIndex);
|
|
}
|
|
}
|
|
continue;
|
|
}
|
|
assert(faceData.m_faceIndex != topFaceIndex);
|
|
}
|
|
|
|
// The face is not visible. Therefore, the halfedge
|
|
// we came from is part of the horizon edge.
|
|
pvf.m_isVisibleFaceOnCurrentIteration = 0;
|
|
m_horizonEdges.push_back(faceData.m_enteredFromHalfEdge);
|
|
// Store which half edge is the horizon edge. The other half edges of
|
|
// the face will not be part of the final mesh so their data slots can by recycled.
|
|
const auto halfEdges = m_mesh.getHalfEdgeIndicesOfFace(m_mesh.m_faces[m_mesh.m_halfEdges[faceData.m_enteredFromHalfEdge].m_face]);
|
|
const std::int8_t ind = (halfEdges[0] == faceData.m_enteredFromHalfEdge) ? 0 : (halfEdges[1] == faceData.m_enteredFromHalfEdge ? 1 : 2);
|
|
m_mesh.m_faces[m_mesh.m_halfEdges[faceData.m_enteredFromHalfEdge].m_face].m_horizonEdgesOnCurrentIteration |= (1 << ind);
|
|
}
|
|
const size_t horizonEdgeCount = m_horizonEdges.size();
|
|
|
|
// Order horizon edges so that they form a loop. This may fail due to numerical
|
|
// instability in which case we give up trying to solve horizon edge for this
|
|
// point and accept a minor degeneration in the convex hull.
|
|
if (!reorderHorizonEdges(m_horizonEdges))
|
|
{
|
|
m_diagnostics.m_failedHorizonEdges++;
|
|
std::cerr << "Failed to solve horizon edge." << std::endl;
|
|
auto it = std::find(tf.m_pointsOnPositiveSide->begin(), tf.m_pointsOnPositiveSide->end(), activePointIndex);
|
|
tf.m_pointsOnPositiveSide->erase(it);
|
|
if (tf.m_pointsOnPositiveSide->size() == 0)
|
|
{
|
|
reclaimToIndexVectorPool(tf.m_pointsOnPositiveSide);
|
|
}
|
|
continue;
|
|
}
|
|
|
|
// Except for the horizon edges, all half edges of the visible faces
|
|
// can be marked as disabled. Their data slots will be reused.
|
|
// The faces will be disabled as well, but we need to remember the
|
|
// points that were on the positive side of them - therefore we
|
|
// save pointers to them.
|
|
m_newFaceIndices.clear();
|
|
m_newHalfEdgeIndices.clear();
|
|
m_disabledFacePointVectors.clear();
|
|
size_t disableCounter = 0;
|
|
for (auto faceIndex : m_visibleFaces)
|
|
{
|
|
auto &disabledFace = m_mesh.m_faces[faceIndex];
|
|
auto halfEdges = m_mesh.getHalfEdgeIndicesOfFace(disabledFace);
|
|
for (size_t j = 0; j < 3; j++)
|
|
{
|
|
if ((disabledFace.m_horizonEdgesOnCurrentIteration & (1 << j)) == 0)
|
|
{
|
|
if (disableCounter < horizonEdgeCount * 2)
|
|
{
|
|
// Use on this iteration
|
|
m_newHalfEdgeIndices.push_back(halfEdges[j]);
|
|
disableCounter++;
|
|
}
|
|
else
|
|
{
|
|
// Mark for reusal on later iteration step
|
|
m_mesh.disableHalfEdge(halfEdges[j]);
|
|
}
|
|
}
|
|
}
|
|
// Disable the face, but retain pointer to the points that were
|
|
// on the positive side of it. We need to assign those points to
|
|
// the new faces we create shortly.
|
|
auto t = std::move(m_mesh.disableFace(faceIndex));
|
|
if (t)
|
|
{
|
|
// Because we should not assign point vectors to faces unless needed...
|
|
assert(t->size());
|
|
m_disabledFacePointVectors.push_back(std::move(t));
|
|
}
|
|
}
|
|
if (disableCounter < horizonEdgeCount * 2)
|
|
{
|
|
const size_t newHalfEdgesNeeded = horizonEdgeCount * 2 - disableCounter;
|
|
for (size_t i = 0; i < newHalfEdgesNeeded; i++)
|
|
{
|
|
m_newHalfEdgeIndices.push_back(m_mesh.addHalfEdge());
|
|
}
|
|
}
|
|
|
|
// Create new faces using the edgeloop
|
|
for (size_t i = 0; i < horizonEdgeCount; i++)
|
|
{
|
|
const size_t AB = m_horizonEdges[i];
|
|
|
|
auto horizonEdgeVertexIndices = m_mesh.getVertexIndicesOfHalfEdge(m_mesh.m_halfEdges[AB]);
|
|
size_t A, B, C;
|
|
A = horizonEdgeVertexIndices[0];
|
|
B = horizonEdgeVertexIndices[1];
|
|
C = activePointIndex;
|
|
|
|
const size_t newFaceIndex = m_mesh.addFace();
|
|
m_newFaceIndices.push_back(newFaceIndex);
|
|
|
|
const size_t CA = m_newHalfEdgeIndices[2 * i + 0];
|
|
const size_t BC = m_newHalfEdgeIndices[2 * i + 1];
|
|
|
|
m_mesh.m_halfEdges[AB].m_next = BC;
|
|
m_mesh.m_halfEdges[BC].m_next = CA;
|
|
m_mesh.m_halfEdges[CA].m_next = AB;
|
|
|
|
m_mesh.m_halfEdges[BC].m_face = newFaceIndex;
|
|
m_mesh.m_halfEdges[CA].m_face = newFaceIndex;
|
|
m_mesh.m_halfEdges[AB].m_face = newFaceIndex;
|
|
|
|
m_mesh.m_halfEdges[CA].m_endVertex = A;
|
|
m_mesh.m_halfEdges[BC].m_endVertex = C;
|
|
|
|
auto &newFace = m_mesh.m_faces[newFaceIndex];
|
|
|
|
const Vector3<T> planeNormal = mathutils::getTriangleNormal(m_vertexData[A], m_vertexData[B], activePoint);
|
|
newFace.m_P = Plane<T>(planeNormal, activePoint);
|
|
newFace.m_he = AB;
|
|
|
|
m_mesh.m_halfEdges[CA].m_opp = m_newHalfEdgeIndices[i > 0 ? i * 2 - 1 : 2 * horizonEdgeCount - 1];
|
|
m_mesh.m_halfEdges[BC].m_opp = m_newHalfEdgeIndices[((i + 1) * 2) % (horizonEdgeCount * 2)];
|
|
}
|
|
|
|
// Assign points that were on the positive side
|
|
// of the disabled faces to the new faces.
|
|
for (auto &disabledPoints : m_disabledFacePointVectors)
|
|
{
|
|
assert(disabledPoints);
|
|
for (const auto &point : *(disabledPoints))
|
|
{
|
|
if (point == activePointIndex)
|
|
{
|
|
continue;
|
|
}
|
|
for (size_t j = 0; j < horizonEdgeCount; j++)
|
|
{
|
|
if (addPointToFace(m_mesh.m_faces[m_newFaceIndices[j]], point))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
// The points are no longer needed: we can move them to the vector pool for reuse.
|
|
reclaimToIndexVectorPool(disabledPoints);
|
|
}
|
|
|
|
// Increase face stack size if needed
|
|
for (const auto newFaceIndex : m_newFaceIndices)
|
|
{
|
|
auto &newFace = m_mesh.m_faces[newFaceIndex];
|
|
if (newFace.m_pointsOnPositiveSide)
|
|
{
|
|
assert(newFace.m_pointsOnPositiveSide->size() > 0);
|
|
if (!newFace.m_inFaceStack)
|
|
{
|
|
m_faceList.push_back(newFaceIndex);
|
|
newFace.m_inFaceStack = 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Cleanup
|
|
m_indexVectorPool.clear();
|
|
}
|
|
|
|
// Constructs the convex hull into a MeshBuilder object
|
|
// which can be converted to a ConvexHull or Mesh object
|
|
inline void buildMesh(const VertexDataSource<T> &pointCloud, bool CCW, bool useOriginalIndices, T eps)
|
|
{
|
|
// CCW is unused for now
|
|
(void)CCW;
|
|
// useOriginalIndices is unused for now
|
|
(void)useOriginalIndices;
|
|
|
|
if (pointCloud.size() == 0)
|
|
{
|
|
m_mesh = MeshBuilder<T>();
|
|
return;
|
|
}
|
|
m_vertexData = pointCloud;
|
|
|
|
// Very first: find extreme values and use them
|
|
// to compute the scale of the point cloud.
|
|
m_extremeValues = getExtremeValues();
|
|
m_scale = getScale(m_extremeValues);
|
|
|
|
// Epsilon we use depends on the scale
|
|
m_epsilon = eps * m_scale;
|
|
m_epsilonSquared = m_epsilon * m_epsilon;
|
|
|
|
// Reset diagnostics
|
|
m_diagnostics = DiagnosticsData();
|
|
|
|
// The planar case happens when all the points
|
|
// appear to lie on a two dimensional subspace of R^3.
|
|
m_planar = false;
|
|
createConvexHalfEdgeMesh();
|
|
if (m_planar)
|
|
{
|
|
const size_t extraPointIndex = m_planarPointCloudTemp.size() - 1;
|
|
for (auto &he : m_mesh.m_halfEdges)
|
|
{
|
|
if (he.m_endVertex == extraPointIndex)
|
|
{
|
|
he.m_endVertex = 0;
|
|
}
|
|
}
|
|
m_vertexData = pointCloud;
|
|
m_planarPointCloudTemp.clear();
|
|
}
|
|
}
|
|
|
|
// The public getConvexHull functions will
|
|
// setup a VertexDataSource object and call this
|
|
inline ConvexHull<T> getConvexHull(const VertexDataSource<T> &pointCloud, bool CCW, bool useOriginalIndices, T eps)
|
|
{
|
|
buildMesh(pointCloud, CCW, useOriginalIndices, eps);
|
|
return ConvexHull<T>(m_mesh, m_vertexData, CCW, useOriginalIndices);
|
|
}
|
|
|
|
public:
|
|
// Computes convex hull for a given point cloud.
|
|
// Params:
|
|
// pointCloud: a vector of of 3D points
|
|
// CCW: whether the output mesh triangles should have CCW orientation
|
|
// useOriginalIndices: should the output mesh use same vertex indices
|
|
// as the original point cloud. If this is false,
|
|
// then we generate a new vertex buffer which contains
|
|
// only the vertices that are part of the convex hull.
|
|
// eps: minimum distance to a plane to consider a point being on
|
|
// positive of it (for a point cloud with scale 1)
|
|
inline ConvexHull<T> getConvexHull(const std::vector<Vector3<T>> &pointCloud,
|
|
bool CCW,
|
|
bool useOriginalIndices,
|
|
T eps = defaultEps<T>())
|
|
{
|
|
{
|
|
VertexDataSource<T> vertexDataSource(pointCloud);
|
|
return getConvexHull(vertexDataSource, CCW, useOriginalIndices, eps);
|
|
}
|
|
}
|
|
|
|
// Computes convex hull for a given point cloud.
|
|
// Params:
|
|
// vertexData: pointer to the first 3D point of the point cloud
|
|
// vertexCount: number of vertices in the point cloud
|
|
// CCW: whether the output mesh triangles should have CCW orientation
|
|
// useOriginalIndices: should the output mesh use same vertex indices
|
|
// as the original point cloud. If this is false,
|
|
// then we generate a new vertex buffer which contains
|
|
// only the vertices that are part of the convex hull.
|
|
// eps: minimum distance to a plane to consider a point being on positive
|
|
// side of it (for a point cloud with scale 1)
|
|
inline ConvexHull<T> getConvexHull(const Vector3<T> *vertexData,
|
|
size_t vertexCount,
|
|
bool CCW,
|
|
bool useOriginalIndices,
|
|
T eps = defaultEps<T>())
|
|
{
|
|
{
|
|
VertexDataSource<T> vertexDataSource(vertexData, vertexCount);
|
|
return getConvexHull(vertexDataSource, CCW, useOriginalIndices, eps);
|
|
}
|
|
}
|
|
|
|
// Computes convex hull for a given point cloud. This function
|
|
// assumes that the vertex data resides in memory
|
|
// in the following format: x_0,y_0,z_0,x_1,y_1,z_1,...
|
|
// Params:
|
|
// vertexData: pointer to the X component of the first point of the point cloud.
|
|
// vertexCount: number of vertices in the point cloud
|
|
// CCW: whether the output mesh triangles should have CCW orientation
|
|
// useOriginalIndices: should the output mesh use same vertex indices as the
|
|
// original point cloud. If this is false, then we generate
|
|
// a new vertex buffer which contains only the vertices that
|
|
// are part of the convex hull.
|
|
// eps: minimum distance to a plane to consider a point being on positive side
|
|
// of it (for a point cloud with scale 1)
|
|
inline ConvexHull<T> getConvexHull(const T *vertexData,
|
|
size_t vertexCount,
|
|
bool CCW,
|
|
bool useOriginalIndices,
|
|
T eps = defaultEps<T>())
|
|
{
|
|
VertexDataSource<T> vertexDataSource((const vec3 *)vertexData, vertexCount);
|
|
return getConvexHull(vertexDataSource, CCW, useOriginalIndices, eps);
|
|
}
|
|
|
|
// Computes convex hull for a given point cloud. This function assumes
|
|
// that the vertex data resides in memory
|
|
// in the following format: x_0,y_0,z_0,x_1,y_1,z_1,...
|
|
// Params:
|
|
// vertexData: pointer to the X component of the first point of the point cloud.
|
|
// vertexCount: number of vertices in the point cloud
|
|
// CCW: whether the output mesh triangles should have CCW orientation
|
|
// eps: minimum distance to a plane to consider a point being on positive side
|
|
// of it (for a point cloud with scale 1)
|
|
// Returns:
|
|
// Convex hull of the point cloud as a mesh object with half edge structure.
|
|
inline HalfEdgeMesh<T, size_t> getConvexHullAsMesh(const T *vertexData,
|
|
size_t vertexCount,
|
|
bool CCW,
|
|
T eps = defaultEps<T>())
|
|
{
|
|
VertexDataSource<T> vertexDataSource((const vec3 *)vertexData, vertexCount);
|
|
buildMesh(vertexDataSource, CCW, false, eps);
|
|
return HalfEdgeMesh<T, size_t>(m_mesh, m_vertexData);
|
|
}
|
|
|
|
// Get diagnostics about last generated convex hull
|
|
inline const DiagnosticsData &getDiagnostics()
|
|
{
|
|
return m_diagnostics;
|
|
}
|
|
};
|
|
|
|
/*
|
|
* Inline function definitions
|
|
*/
|
|
|
|
template <typename T>
|
|
inline std::unique_ptr<std::vector<size_t>> QuickHull<T>::getIndexVectorFromPool()
|
|
{
|
|
auto r = std::move(m_indexVectorPool.get());
|
|
r->clear();
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
inline void QuickHull<T>::reclaimToIndexVectorPool(std::unique_ptr<std::vector<size_t>> &ptr)
|
|
{
|
|
const size_t oldSize = ptr->size();
|
|
if ((oldSize + 1) * 128 < ptr->capacity())
|
|
{
|
|
// Reduce memory usage! Huge vectors are needed at the beginning of iteration
|
|
// when faces have many points on their positive side. Later on, smaller vectors
|
|
// will suffice.
|
|
ptr.reset(nullptr);
|
|
return;
|
|
}
|
|
m_indexVectorPool.reclaim(ptr);
|
|
}
|
|
|
|
template <typename T>
|
|
inline bool QuickHull<T>::addPointToFace(typename MeshBuilder<T>::Face &f, size_t pointIndex)
|
|
{
|
|
const T D = mathutils::getSignedDistanceToPlane(m_vertexData[pointIndex], f.m_P);
|
|
if (D > 0 && D * D > m_epsilonSquared * f.m_P.m_sqrNLength)
|
|
{
|
|
if (!f.m_pointsOnPositiveSide)
|
|
{
|
|
f.m_pointsOnPositiveSide = std::move(getIndexVectorFromPool());
|
|
}
|
|
f.m_pointsOnPositiveSide->push_back(pointIndex);
|
|
if (D > f.m_mostDistantPointDist)
|
|
{
|
|
f.m_mostDistantPointDist = D;
|
|
f.m_mostDistantPoint = pointIndex;
|
|
}
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
} // namespace quickhull
|
|
|
|
#endif |