From 55620db6c69e9850d6c3044212d79138331cd125 Mon Sep 17 00:00:00 2001 From: Evangel Date: Wed, 13 Jul 2022 17:58:17 +1000 Subject: [PATCH 1/3] Started further cleanup. Most of it's fixing indentation and member variable names. There's a couple of refactors. I don't like assumptions about memory allocations that were made in a number of places. Reference style is the interface. No indentation for top level declarations. Class/struct names are PascalCase. Variable names are camelCase, member variable names are m_camelCase, function names are PascalCase, member function names are PascalCase. Class member functions are defined out of line. Removed the multiple namespace VHACD parts. There's now only two namespace VHACD, the interface at the top, and the implementation. Interesting thing to note. Determinant3x3(double[3][3], double*) and Determinant3x3(Googol[3][3]) both return -determinant of their argument. They also do a bunch of looping to create the cofactor matrix that is used for Laplace expansion. The order for that is a(ei - hf) - b(di * gf) + c(dh * ge), but the Determinant3x3 function reverses the sign of a, b, and c. ie -a(...) + b(...) - c(...) There's another determinant function, inline double det(...) that calculates it correctly. I've fixed Determinant3x3 by manually unrolling the loops, removing the Determinant2x2 function that was only used in there, and removing the det function. Determinant3x3 was only used in ConvexHullFace::Evalue(), so that also returns a proper value. ConvexHullFace::Evalue() is only used in ConvexHull::CalculateConvexHull3d() as a greater than comparison to zero, so I fixed those up. Hopefully the new code is easier to understand, as I had a hell of a time understanding what was going on in Determinant3x3() and why it was different to det(). Removed VHACD::List. It was only used in ConvexHull::CalculateConvexHull3d(). Replaced it with std::list. No change in performance, but more reduced code. Unfortunate part of that bit of code is both boundaryFaces and ConvexHull::m_list are std::list, so it ends up being a mess of std::list::iterator with the iterators referring to different containers. The original code was no better, with VHACD::List::ndNode* taking the place of std::list::iterator. KdTreeNodeBundle is another linked list. Replaced the list part with std::list. Could replace it with std::forward_list since it's only storing elements, never iterated. Expanded it into a generic NodeBundle class and used it in ConvexHull for memory allocations. ConvexHull was using a std::vector for memory allocations, but had to make sure it never got reallocated. This way, it allocates in bundles, same as KdTree does, which retains pointer stability. Removed VHACD::Swap. std::swap is in the header and does the same thing. VHACD::Sort does a three way comparison, so is not suitable for replacement for std::sort Used = default where possible for constructors, along with in-class member initializers. Added compatible type constructor to VHACD::Vect3. This allows to construct a VHACD::Vect3 from a VHACD::Vect3. Helps in ConvexHullFace::Evalue(), as both matrix and exactMatrix can be initialized using vector math, which I think better represents what's going on there. Replaced lots of double(1.0f) or similar with double(1.0) or whatever was in parentheses. With a find/replace on double, this can allow customization of the real type, eg for allowing it to run with float, long double, or __float128 for whatever reason if someone wants it. ConvexHull::BuildHull(), ConvexHull::BuildTreeNew(), and ConvexHull::BuildTreeOld() were doing something strange. BuildHull() was allocating and initializing a std::vector with treeCount + 256 elements. ie it was allocating and constructing a bunch of classes of non-trivial type. It was then passing a pointer to the allocated memory and the size of the allocated memory to ConvexHull::InitVertexArray which then passed it along to ConvexHull::BuildTree*(). BuildTree*() was then placement newing either a ConvexHullAABB3DPointCluster or ConvexHullAABBTreeNode on the pointer, incrementing the pointer by the size of the constructed type, and doing it all over again. Essentially, it was slicing up the memory of other objects. This would've been an issue if either of them had non-trivial destructors, but they didn't. It did mean that reading through the code was harder than necessary since what was this placement new doing here? Where'd the memory come from? What's this type cast doing? It wasn't saving memory either, since the same amount of memory was allocated regardless. In short, it was weird code to construct a tree. I changed it to just emplace_back a new element, and then take the address of said element. There's asserts to check that the std::vector doesn't reallocate since pointers to the allocated elements are used. Moved BoundsAABB from a struct inside AABBTreeImpl (renamed AABBTree) to a utility class in the interface. There was duplicated code and it makes sense for the AABB to be part of the ConvexHull, given there's an AABB in there already. Didn't replace the mBmin and mBmax members because I think I've fiddled with the API enough. Also didn't rename them for the same reason. Volume had a duplicate AABB bounds computing function, replaced with BoundsAABB where required. VHACD::Voxel is constructed from 3 uint32_t's, where each should only be in the range [0,1024), but this isn't enforced anywhere. Added asserts to ensure it's never constructed with values outside of that range. VoxelHull::FindConcavity{X,Y,Z} was 90% duplicate code. Only really changed in the inner loops. Replaced with a call to FindConcavity that takes an index and then switches on it in relevant places. Removed RaycastMesh and Voxelize classes. They just passed their arguments onto AABBTree and Volume respectively. Implemented the required parts directly in AABBTree and Volume's functions. Removed ShrinkWrap class. It was just a function. Removed manual memory management in VHACDImpl. Lots of new and delete and storing the pointers in vectors and manually deleting them before clearing the vector. Now they're vectors of std::unique_ptr so a lot fewer deletes and no need to make sure it's gone. Fixed memory leak in VHACDImpl::findNearestConvexHull. It was allocating AABBTrees and storing them in a vector, but was never deleting them, only clearing the vector. Renamed MyHACD_API to VHACDAsyncImpl to better describe what it is. Changed m_VHACD from pointer to plain member. It's allocated in the constructor anyway, so let's just allocate it at the same time. There's more I'd like to do, but it always ends up being c++14, 17, or 20. The line change count is misleading, since it's primarily unindenting code. --- app/TestVHACD.cpp | 4 +- include/VHACD.h | 13092 +++++++++++++++++++++----------------------- 2 files changed, 6146 insertions(+), 6950 deletions(-) diff --git a/app/TestVHACD.cpp b/app/TestVHACD.cpp index 634ed128..dc043910 100644 --- a/app/TestVHACD.cpp +++ b/app/TestVHACD.cpp @@ -56,7 +56,8 @@ bool getTrueFalse(const char *option,bool &value) return ret; } -class Logging : public VHACD::IVHACD::IUserCallback, public VHACD::IVHACD::IUserLogger +class Logging : public VHACD::IVHACD::IUserCallback, + public VHACD::IVHACD::IUserLogger { public: Logging(void) @@ -571,6 +572,7 @@ int main(int argc,const char **argv) fprintf(fph,"endsolid %s\n", hullName); } } + fclose(fph); } } } diff --git a/include/VHACD.h b/include/VHACD.h index a14eea98..a15744e0 100644 --- a/include/VHACD.h +++ b/include/VHACD.h @@ -103,7 +103,7 @@ // that plane splitting would be far easier to implement working in voxel space. // // V-HACD has been integrated into UE4, Blender, and a number of other projects. -// This new release, version4, is a siginficant refactor of the code to fix +// This new release, version4, is a significant refactor of the code to fix // some bugs, improve performance, and to make the codebase easier to maintain // going forward. @@ -115,6 +115,19 @@ namespace VHACD { +// #ifdef VHACD_SCALAR +// using VhScalar = VHACD_SCALAR; +// #else +// using VhScalar = double; +// #endif +// +// #ifdef VHACD_VECTOR +// using VhVector = VHACD_VECTOR; +// #else +// template > +// using VhVector = std::vector; +// #endif + struct Vertex { double mX; @@ -146,74 +159,75 @@ struct Triangle Triangle(uint32_t i0, uint32_t i1, uint32_t i2) : mI0(i0), mI1(i1), mI2(i2) {} }; -} // namespace VHACD - -namespace nd -{ - namespace VHACD - { - template -class Vect3 { +class Vector3 +{ public: /* - * Getters - */ + * Getters + */ T& operator[](size_t i); const T& operator[](size_t i) const; - T& getX(); - T& getY(); - T& getZ(); - const T& getX() const; - const T& getY() const; - const T& getZ() const; + T& GetX(); + T& GetY(); + T& GetZ(); + const T& GetX() const; + const T& GetY() const; + const T& GetZ() const; /* - * Normalize and norming - */ + * Normalize and norming + */ T Normalize(); - Vect3 Normalized(); + Vector3 Normalized(); T GetNorm() const; T GetNormSquared() const; int LongestAxis() const; /* - * Vector-vector operations - */ - Vect3& operator=(const Vect3& rhs); - Vect3& operator+=(const Vect3& rhs); - Vect3& operator-=(const Vect3& rhs); + * Vector-vector operations + */ + Vector3& operator=(const Vector3& rhs); + Vector3& operator+=(const Vector3& rhs); + Vector3& operator-=(const Vector3& rhs); - Vect3 CWiseMul(const Vect3& rhs) const; - Vect3 Cross(const Vect3& rhs) const; - T Dot(const Vect3& rhs) const; - Vect3 operator+(const Vect3& rhs) const; - Vect3 operator-(const Vect3& rhs) const; + Vector3 CWiseMul(const Vector3& rhs) const; + Vector3 Cross(const Vector3& rhs) const; + T Dot(const Vector3& rhs) const; + Vector3 operator+(const Vector3& rhs) const; + Vector3 operator-(const Vector3& rhs) const; /* - * Vector-scalar operations - */ - Vect3& operator-=(T a); - Vect3& operator+=(T a); - Vect3& operator/=(T a); - Vect3& operator*=(T a); + * Vector-scalar operations + */ + Vector3& operator-=(T a); + Vector3& operator+=(T a); + Vector3& operator/=(T a); + Vector3& operator*=(T a); - Vect3 operator*(T rhs) const; - Vect3 operator/(T rhs) const; + Vector3 operator*(T rhs) const; + Vector3 operator/(T rhs) const; /* - * Unary operations - */ - Vect3 operator-() const; + * Unary operations + */ + Vector3 operator-() const; + + /* + * Comparison operators + */ + bool operator<(const Vector3& rhs) const; + bool operator>(const Vector3& rhs) const; /* - * Comparison operators + * Returns true if all elements of *this are greater than or equal to all elements of rhs, coefficient wise + * LE is less than or equal */ - bool operator<(const Vect3& rhs) const; - bool operator>(const Vect3& rhs) const; + bool CWiseAllGE(const Vector3& rhs) const; + bool CWiseAllLE(const Vector3& rhs) const; - Vect3 CWiseMin(const Vect3& rhs) const; - Vect3 CWiseMax(const Vect3& rhs) const; + Vector3 CWiseMin(const Vector3& rhs) const; + Vector3 CWiseMax(const Vector3& rhs) const; T MinCoeff() const; T MaxCoeff() const; @@ -221,28 +235,57 @@ class Vect3 { T MaxCoeff(uint32_t& idx) const; /* - * Constructors - */ - Vect3() = default; - Vect3(T a); - Vect3(T x, T y, T z); - Vect3(const Vect3& rhs); - ~Vect3() = default; + * Constructors + */ + Vector3() = default; + Vector3(T a); + Vector3(T x, T y, T z); + Vector3(const Vector3& rhs); + ~Vector3() = default; + + template + Vector3(const Vector3& rhs); - Vect3(const ::VHACD::Vertex&); - Vect3(const ::VHACD::Triangle&); + Vector3(const VHACD::Vertex&); + Vector3(const VHACD::Triangle&); - operator ::VHACD::Vertex() const; + operator VHACD::Vertex() const; private: - std::array mData; + std::array m_data{ T(0.0) }; }; -} -}// namespace nd::VHACD +typedef VHACD::Vector3 Vect3; -namespace VHACD +struct BoundsAABB { + BoundsAABB() = default; + BoundsAABB(const std::vector& points); + BoundsAABB(const Vect3& min, + const Vect3& max); + + BoundsAABB Union(const BoundsAABB& b); + + bool Intersects(const BoundsAABB& b) const; + + double SurfaceArea() const; + double Volume() const; + + BoundsAABB Inflate(double ratio) const; + + VHACD::Vect3 ClosestPoint(const VHACD::Vect3& p) const; + + VHACD::Vect3& GetMin(); + VHACD::Vect3& GetMax(); + const VHACD::Vect3& GetMin() const; + const VHACD::Vect3& GetMax() const; + + VHACD::Vect3 GetSize() const; + VHACD::Vect3 GetCenter() const; + + VHACD::Vect3 m_min{ double(0.0) }; + VHACD::Vect3 m_max{ double(0.0) }; +}; /** * This enumeration determines how the voxels as filled to create a solid @@ -288,13 +331,13 @@ class IVHACD virtual void Update(const double overallProgress, const double stageProgress, const char* const stage, - const char *operation) = 0; + const char* operation) = 0; // This is an optional user callback which is only called when running V-HACD asynchronously. // This is a callback performed to notify the user that the // convex decomposition background process is completed. This call back will occur from // a different thread so the user should take that into account. - virtual void NotifyVHACDComplete(void) + virtual void NotifyVHACDComplete() { } }; @@ -329,14 +372,17 @@ class IVHACD class ConvexHull { public: - std::vector m_points; - std::vector m_triangles; - - double m_volume{0}; // The volume of the convex hull - nd::VHACD::Vect3 m_center{0, 0, 0}; // The centroid of the convex hull - uint32_t m_meshId{0}; // A unique id for this convex hull - nd::VHACD::Vect3 mBmin; // Bounding box minimum of the AABB - nd::VHACD::Vect3 mBmax; // Bounding box maximum of he AABB + std::vector m_points; + std::vector m_triangles; + + double m_volume{ 0 }; // The volume of the convex hull + VHACD::Vect3 m_center{ 0, 0, 0 }; // The centroid of the convex hull + uint32_t m_meshId{ 0 }; // A unique id for this convex hull +// VHACD::Vect3 m_bMin; // Should be these to keep consistent naming style, but don't want to break API +// VHACD::Vect3 m_bMax; // at least not yet, or any more than currently +// VHACD::BoundsAABB m_bounds; // Should be this to properly reflect what it is + VHACD::Vect3 mBmin; // Bounding box minimum of the AABB + VHACD::Vect3 mBmax; // Bounding box maximum of the AABB }; /** @@ -348,20 +394,20 @@ class IVHACD IUserCallback* m_callback{nullptr}; // Optional user provided callback interface for progress IUserLogger* m_logger{nullptr}; // Optional user provided callback interface for log messages IUserTaskRunner* m_taskRunner{nullptr}; // Optional user provided interface for creating tasks - uint32_t m_maxConvexHulls{64}; // The maximum number of convex hulls to produce - uint32_t m_resolution{400000}; // The voxel resolution to use - double m_minimumVolumePercentErrorAllowed{1}; // if the voxels are within 1% of the volume of the hull, we consider this a close enough approximation - uint32_t m_maxRecursionDepth{14}; // The maximum recursion depth + uint32_t m_maxConvexHulls{ 64 }; // The maximum number of convex hulls to produce + uint32_t m_resolution{ 400000 }; // The voxel resolution to use + double m_minimumVolumePercentErrorAllowed{ 1 }; // if the voxels are within 1% of the volume of the hull, we consider this a close enough approximation + uint32_t m_maxRecursionDepth{ 14 }; // The maximum recursion depth bool m_shrinkWrap{true}; // Whether or not to shrinkwrap the voxel positions to the source mesh on output FillMode m_fillMode{ FillMode::FLOOD_FILL }; // How to fill the interior of the voxelized mesh - uint32_t m_maxNumVerticesPerCH{64}; // The maximum number of vertices allowed in any output convex hull + uint32_t m_maxNumVerticesPerCH{ 64 }; // The maximum number of vertices allowed in any output convex hull bool m_asyncACD{ true }; // Whether or not to run asynchronously, taking advantage of additonal cores - uint32_t m_minEdgeLength{2}; // Once a voxel patch has an edge length of less than 4 on all 3 sides, we don't keep recursing - bool m_findBestPlane{false}; // Whether or not to attempt to split planes along the best location. Experimental feature. False by default. + uint32_t m_minEdgeLength{ 2 }; // Once a voxel patch has an edge length of less than 4 on all 3 sides, we don't keep recursing + bool m_findBestPlane{ false }; // Whether or not to attempt to split planes along the best location. Experimental feature. False by default. }; /** - * Will cause the convex decomposition operation to be canceled early. No results will be produced but the background operaiton will end as soon as it can. + * Will cause the convex decomposition operation to be canceled early. No results will be produced but the background operation will end as soon as it can. */ virtual void Cancel() = 0; @@ -417,12 +463,12 @@ class IVHACD /** * Releases any memory allocated by the V-HACD class */ - virtual void Clean(void) = 0; // release internally allocated memory + virtual void Clean() = 0; // release internally allocated memory /** * Releases this instance of the V-HACD class */ - virtual void Release(void) = 0; // release IVHACD + virtual void Release() = 0; // release IVHACD // Will compute the center of mass of the convex hull decomposition results and return it // in 'centerOfMass'. Returns false if the center of mass could not be computed. @@ -432,7 +478,7 @@ class IVHACD // In asynchronous mode, this returns true if the background thread is not still actively computing // a new solution. In an asynchronous config the 'IsReady' call will report any update or log // messages in the caller's current thread. - virtual bool IsReady(void) const + virtual bool IsReady() const { return true; } @@ -448,16 +494,17 @@ class IVHACD * @return : Returns which convex hull this position is closest to. */ virtual uint32_t findNearestConvexHull(const double pos[3], - double &distanceToHull) = 0; + double& distanceToHull) = 0; protected: - virtual ~IVHACD(void) + virtual ~IVHACD() { } }; -IVHACD* CreateVHACD(void); // Create a synchronous (blocking) implementation of V-HACD -IVHACD* CreateVHACD_ASYNC(void); // Create an asynchronous (non-blocking) implementation of V-HACD +IVHACD* CreateVHACD(); // Create a synchronous (blocking) implementation of V-HACD +IVHACD* CreateVHACD_ASYNC(); // Create an asynchronous (non-blocking) implementation of V-HACD + } // namespace VHACD @@ -477,6 +524,7 @@ IVHACD* CreateVHACD_ASYNC(void); // Create an asynchronous (non-blocking) imp #include #include #include +#include #include #include #include @@ -488,3662 +536,3323 @@ IVHACD* CreateVHACD_ASYNC(void); // Create an asynchronous (non-blocking) imp #ifdef _MSC_VER #pragma warning(push) -#pragma warning(disable:4100 4189 4456 4701 4702 4127 4996) +#pragma warning(disable:4100 4127 4189 4244 4456 4701 4702 4996) #endif // _MSC_VER #ifdef __GNUC__ #pragma GCC diagnostic push +// Minimum set of warnings used for cleanup +// #pragma GCC diagnostic warning "-Wall" +// #pragma GCC diagnostic warning "-Wextra" +// #pragma GCC diagnostic warning "-Wpedantic" // #pragma GCC diagnostic warning "-Wold-style-cast" -// #pragma GCC diagnostic warning "-Wreorder" -// #pragma GCC diagnostic warning "-Wunused-variable" -// #pragma GCC diagnostic warning "-Wignored-qualifiers" // #pragma GCC diagnostic warning "-Wnon-virtual-dtor" -// #pragma GCC diagnostic warning "-Wuninitialized" +// #pragma GCC diagnostic warning "-Wshadow" #endif // __GNUC__ -#define VHACD_SAFE_RELEASE(x) if ( x ) { x->release(); x = nullptr; } - // Scoped Timer -namespace VHACD -{ +namespace VHACD { class Timer { public: - Timer() : mStartTime(std::chrono::high_resolution_clock::now()) + Timer() + : m_startTime(std::chrono::high_resolution_clock::now()) { } - void reset() + void Reset() { - mStartTime = std::chrono::high_resolution_clock::now(); + m_startTime = std::chrono::high_resolution_clock::now(); } - double getElapsedSeconds() + double GetElapsedSeconds() { - auto s = peekElapsedSeconds(); - reset(); + auto s = PeekElapsedSeconds(); + Reset(); return s; } - double peekElapsedSeconds() + double PeekElapsedSeconds() { auto now = std::chrono::high_resolution_clock::now(); - std::chrono::duration diff = now - mStartTime; + std::chrono::duration diff = now - m_startTime; return diff.count(); } private: - std::chrono::time_point mStartTime; + std::chrono::time_point m_startTime; }; class ScopedTime { public: - ScopedTime(const char *action,VHACD::IVHACD::IUserLogger *logger) : mAction(action), mLogger(logger) + ScopedTime(const char* action, + VHACD::IVHACD::IUserLogger* logger) + : m_action(action) + , m_logger(logger) { - mTimer.reset(); + m_timer.Reset(); } - ~ScopedTime(void) + ~ScopedTime() { - double dtime = mTimer.getElapsedSeconds(); - if( mLogger ) + double dtime = m_timer.GetElapsedSeconds(); + if( m_logger ) { char scratch[512]; - snprintf(scratch,sizeof(scratch),"%s took %0.5f seconds", mAction, dtime); - mLogger->Log(scratch); + snprintf(scratch, + sizeof(scratch),"%s took %0.5f seconds", + m_action, + dtime); + m_logger->Log(scratch); } } - const char *mAction{nullptr}; - Timer mTimer; - VHACD::IVHACD::IUserLogger *mLogger{nullptr}; + const char* m_action{ nullptr }; + Timer m_timer; + VHACD::IVHACD::IUserLogger* m_logger{ nullptr }; }; -} // namespace VHACD - -//*********************************************************************************************** -// ConvexHull generation code by Julio Jerez -//*********************************************************************************************** +/* + * Out of line definitions + */ -template -inline T Max(T a, T b) +template +T clamp(const T& v, const T& lo, const T& hi) { - return (a > b) ? a : b; + if (v < lo) + { + return lo; + } + if (v > hi) + { + return hi; + } + return v ; } -template -inline T Min(T a, T b) +/* + * Getters + */ +template +inline T& Vector3::operator[](size_t i) { - return (a < b) ? a : b; + return m_data[i]; } -namespace nd -{ -namespace VHACD +template +inline const T& Vector3::operator[](size_t i) const { - /* - * Out of line definitions - */ - - /* - * Getters - */ - template - inline T& Vect3::operator[](size_t i) - { - return mData[i]; - } - - template - inline const T& Vect3::operator[](size_t i) const - { - return mData[i]; - } + return m_data[i]; +} - template - inline T& Vect3::getX() - { - return mData[0]; - } +template +inline T& Vector3::GetX() +{ + return m_data[0]; +} - template - inline T& Vect3::getY() - { - return mData[1]; - } +template +inline T& Vector3::GetY() +{ + return m_data[1]; +} - template - inline T& Vect3::getZ() - { - return mData[2]; - } +template +inline T& Vector3::GetZ() +{ + return m_data[2]; +} - template - inline const T& Vect3::getX() const - { - return mData[0]; - } +template +inline const T& Vector3::GetX() const +{ + return m_data[0]; +} - template - inline const T& Vect3::getY() const - { - return mData[1]; - } +template +inline const T& Vector3::GetY() const +{ + return m_data[1]; +} - template - inline const T& Vect3::getZ() const - { - return mData[2]; - } +template +inline const T& Vector3::GetZ() const +{ + return m_data[2]; +} - /* - * Normalize and norming - */ - template - inline T Vect3::Normalize() - { - T n = GetNorm(); - if (n != 0.0) (*this) /= n; - return n; - } +/* + * Normalize and norming + */ +template +inline T Vector3::Normalize() +{ + T n = GetNorm(); + if (n != T(0.0)) (*this) /= n; + return n; +} - template - inline Vect3 Vect3::Normalized() - { - Vect3 ret = *this; - T n = GetNorm(); - if (n != 0.0) ret /= n; - return ret; - } +template +inline Vector3 Vector3::Normalized() +{ + Vector3 ret = *this; + T n = GetNorm(); + if (n != T(0.0)) ret /= n; + return ret; +} - template - inline T Vect3::GetNorm() const - { - return std::sqrt(GetNormSquared()); - } +template +inline T Vector3::GetNorm() const +{ + return std::sqrt(GetNormSquared()); +} - template - inline T Vect3::GetNormSquared() const - { - return this->Dot(*this); - } +template +inline T Vector3::GetNormSquared() const +{ + return this->Dot(*this); +} - template - inline int Vect3::LongestAxis() const - { - if (getX() > getY() && getX() > getZ()) - return 0; - if (getY() > getZ()) - return 1 ; - return 2; - } +template +inline int Vector3::LongestAxis() const +{ + auto it = std::max_element(m_data.begin(), m_data.end()); + return int(std::distance(m_data.begin(), it)); +} - /* - * Vector-vector operations - */ - template - inline Vect3& Vect3::operator=(const Vect3& rhs) - { - getX() = rhs.getX(); - getY() = rhs.getY(); - getZ() = rhs.getZ(); - return *this; - } +/* + * Vector-vector operations + */ +template +inline Vector3& Vector3::operator=(const Vector3& rhs) +{ + GetX() = rhs.GetX(); + GetY() = rhs.GetY(); + GetZ() = rhs.GetZ(); + return *this; +} - template - inline Vect3& Vect3::operator+=(const Vect3& rhs) - { - getX() += rhs.getX(); - getY() += rhs.getY(); - getZ() += rhs.getZ(); - return *this; - } +template +inline Vector3& Vector3::operator+=(const Vector3& rhs) +{ + GetX() += rhs.GetX(); + GetY() += rhs.GetY(); + GetZ() += rhs.GetZ(); + return *this; +} - template - inline Vect3& Vect3::operator-=(const Vect3& rhs) - { - getX() -= rhs.getX(); - getY() -= rhs.getY(); - getZ() -= rhs.getZ(); - return *this; - } +template +inline Vector3& Vector3::operator-=(const Vector3& rhs) +{ + GetX() -= rhs.GetX(); + GetY() -= rhs.GetY(); + GetZ() -= rhs.GetZ(); + return *this; +} - template - inline Vect3 Vect3::CWiseMul(const Vect3& rhs) const - { - return Vect3(getX() * rhs.getX(), - getY() * rhs.getY(), - getZ() * rhs.getZ()); - } +template +inline Vector3 Vector3::CWiseMul(const Vector3& rhs) const +{ + return Vector3(GetX() * rhs.GetX(), + GetY() * rhs.GetY(), + GetZ() * rhs.GetZ()); +} - template - inline Vect3 Vect3::Cross(const Vect3& rhs) const - { - return Vect3(getY() * rhs.getZ() - getZ() * rhs.getY(), - getZ() * rhs.getX() - getX() * rhs.getZ(), - getX() * rhs.getY() - getY() * rhs.getX()); - } +template +inline Vector3 Vector3::Cross(const Vector3& rhs) const +{ + return Vector3(GetY() * rhs.GetZ() - GetZ() * rhs.GetY(), + GetZ() * rhs.GetX() - GetX() * rhs.GetZ(), + GetX() * rhs.GetY() - GetY() * rhs.GetX()); +} - template - inline T Vect3::Dot(const Vect3& rhs) const - { - return getX() * rhs.getX() + getY() * rhs.getY() + getZ() * rhs.getZ(); - } +template +inline T Vector3::Dot(const Vector3& rhs) const +{ + return GetX() * rhs.GetX() + + GetY() * rhs.GetY() + + GetZ() * rhs.GetZ(); +} - template - inline Vect3 Vect3::operator+(const Vect3& rhs) const - { - return Vect3(getX() + rhs.getX(), - getY() + rhs.getY(), - getZ() + rhs.getZ()); - } +template +inline Vector3 Vector3::operator+(const Vector3& rhs) const +{ + return Vector3(GetX() + rhs.GetX(), + GetY() + rhs.GetY(), + GetZ() + rhs.GetZ()); +} - template - inline Vect3 Vect3::operator-(const Vect3& rhs) const - { - return Vect3(getX() - rhs.getX(), - getY() - rhs.getY(), - getZ() - rhs.getZ()); - } +template +inline Vector3 Vector3::operator-(const Vector3& rhs) const +{ + return Vector3(GetX() - rhs.GetX(), + GetY() - rhs.GetY(), + GetZ() - rhs.GetZ()); +} - template - inline Vect3 operator*(T lhs, const Vect3 & rhs) - { - return Vect3(lhs * rhs.getX(), - lhs * rhs.getY(), - lhs * rhs.getZ()); - } +template +inline Vector3 operator*(T lhs, const Vector3& rhs) +{ + return Vector3(lhs * rhs.GetX(), + lhs * rhs.GetY(), + lhs * rhs.GetZ()); +} - /* - * Vector-scalar operations - */ - template - inline Vect3& Vect3::operator-=(T a) - { - getX() -= a; - getY() -= a; - getZ() -= a; - return *this; - } +/* + * Vector-scalar operations + */ +template +inline Vector3& Vector3::operator-=(T a) +{ + GetX() -= a; + GetY() -= a; + GetZ() -= a; + return *this; +} - template - inline Vect3& Vect3::operator+=(T a) - { - getX() += a; - getY() += a; - getZ() += a; - return *this; - } +template +inline Vector3& Vector3::operator+=(T a) +{ + GetX() += a; + GetY() += a; + GetZ() += a; + return *this; +} - template - inline Vect3& Vect3::operator/=(T a) - { - getX() /= a; - getY() /= a; - getZ() /= a; - return *this; - } +template +inline Vector3& Vector3::operator/=(T a) +{ + GetX() /= a; + GetY() /= a; + GetZ() /= a; + return *this; +} - template - inline Vect3& Vect3::operator*=(T a) - { - getX() *= a; - getY() *= a; - getZ() *= a; - return *this; - } +template +inline Vector3& Vector3::operator*=(T a) +{ + GetX() *= a; + GetY() *= a; + GetZ() *= a; + return *this; +} - template - inline Vect3 Vect3::operator*(T rhs) const - { - return Vect3(getX() * rhs, - getY() * rhs, - getZ() * rhs); - } +template +inline Vector3 Vector3::operator*(T rhs) const +{ + return Vector3(GetX() * rhs, + GetY() * rhs, + GetZ() * rhs); +} - template - inline Vect3 Vect3::operator/(T rhs) const - { - return Vect3(getX() / rhs, - getY() / rhs, - getZ() / rhs); - } +template +inline Vector3 Vector3::operator/(T rhs) const +{ + return Vector3(GetX() / rhs, + GetY() / rhs, + GetZ() / rhs); +} - /* - * Unary operations - */ - template - inline Vect3 Vect3::operator-() const - { - return Vect3(-getX(), - -getY(), - -getZ()); - } +/* + * Unary operations + */ +template +inline Vector3 Vector3::operator-() const +{ + return Vector3(-GetX(), + -GetY(), + -GetZ()); +} - /* - * Comparison operators - */ - template - inline bool Vect3::operator<(const Vect3& rhs) const +/* + * Comparison operators + */ +template +inline bool Vector3::operator<(const Vector3& rhs) const +{ + if (GetX() == rhs.GetX()) { - if (getX() == rhs[0]) + if (GetY() == rhs.GetY()) { - if (getY() == rhs[1]) - { - return (getZ() < rhs[2]); - } - return (getY() < rhs[1]); + return (GetZ() < rhs.GetZ()); } - return (getX() < rhs[0]); + return (GetY() < rhs.GetY()); } + return (GetX() < rhs.GetX()); +} - template - inline bool Vect3::operator>(const Vect3& rhs) const +template +inline bool Vector3::operator>(const Vector3& rhs) const +{ + if (GetX() == rhs.GetX()) { - if (getX() == rhs[0]) + if (GetY() == rhs.GetY()) { - if (getY() == rhs[1]) - { - return (getZ() > rhs[2]); - } - return (getY() > rhs[1]); + return (GetZ() > rhs.GetZ()); } - return (getX() > rhs[0]); + return (GetY() > rhs.GetY()); } + return (GetX() > rhs.GetZ()); +} - template - inline Vect3 Vect3::CWiseMin(const Vect3& rhs) const - { - return Vect3(Min(getX(), rhs.getX()), - Min(getY(), rhs.getY()), - Min(getZ(), rhs.getZ())); - } +template +inline bool Vector3::CWiseAllGE(const Vector3& rhs) const +{ + return GetX() >= rhs.GetX() + && GetY() >= rhs.GetY() + && GetZ() >= rhs.GetZ(); +} - template - inline Vect3 Vect3::CWiseMax(const Vect3& rhs) const - { - return Vect3(Max(getX(), rhs.getX()), - Max(getY(), rhs.getY()), - Max(getZ(), rhs.getZ())); - } +template +inline bool Vector3::CWiseAllLE(const Vector3& rhs) const +{ + return GetX() <= rhs.GetX() + && GetY() <= rhs.GetY() + && GetZ() <= rhs.GetZ(); +} - template - inline T Vect3::MinCoeff() const - { - if (getX() < getY() && getX() < getZ()) - { - return getX(); - } - if (getY() < getZ()) - { - return getY(); - } - return getZ(); - } +template +inline Vector3 Vector3::CWiseMin(const Vector3& rhs) const +{ + return Vector3(std::min(GetX(), rhs.GetX()), + std::min(GetY(), rhs.GetY()), + std::min(GetZ(), rhs.GetZ())); +} - template - inline T Vect3::MaxCoeff() const - { - if (getX() > getY() && getX() > getZ()) - { - return getX(); - } - if (getY() > getZ()) - { - return getY(); +template +inline Vector3 Vector3::CWiseMax(const Vector3& rhs) const +{ + return Vector3(std::max(GetX(), rhs.GetX()), + std::max(GetY(), rhs.GetY()), + std::max(GetZ(), rhs.GetZ())); +} + +template +inline T Vector3::MinCoeff() const +{ + return *std::min_element(m_data.begin(), m_data.end()); +} + +template +inline T Vector3::MaxCoeff() const +{ + return *std::max_element(m_data.begin(), m_data.end()); +} + +template +inline T Vector3::MinCoeff(uint32_t& idx) const +{ + auto it = std::min_element(m_data.begin(), m_data.end()); + idx = uint32_t(std::distance(m_data.begin(), it)); + return *it; +} + +template +inline T Vector3::MaxCoeff(uint32_t& idx) const +{ + auto it = std::max_element(m_data.begin(), m_data.end()); + idx = uint32_t(std::distance(m_data.begin(), it)); + return *it; +} + +/* + * Constructors + */ +template +inline Vector3::Vector3(T a) + : m_data{a, a, a} +{ +} + +template +inline Vector3::Vector3(T x, T y, T z) + : m_data{x, y, z} +{ +} + +template +inline Vector3::Vector3(const Vector3& rhs) + : m_data{rhs.m_data} +{ +} + +template +template +inline Vector3::Vector3(const Vector3& rhs) + : m_data{T(rhs.GetX()), T(rhs.GetY()), T(rhs.GetZ())} +{ +} + +template +inline Vector3::Vector3(const VHACD::Vertex& rhs) + : Vector3(rhs.mX, rhs.mY, rhs.mZ) +{ + static_assert(std::is_same::value, "Vertex to Vector3 constructor only enabled for double"); +} + +template +inline Vector3::Vector3(const VHACD::Triangle& rhs) + : Vector3(rhs.mI0, rhs.mI1, rhs.mI2) +{ + static_assert(std::is_same::value, "Triangle to Vector3 constructor only enabled for uint32_t"); +} + +template +inline Vector3::operator VHACD::Vertex() const +{ + static_assert(std::is_same::value, "Vector3 to Vertex conversion only enable for double"); + return ::VHACD::Vertex( GetX(), GetY(), GetZ()); +} + +inline BoundsAABB::BoundsAABB(const std::vector& points) + : m_min(points[0]) + , m_max(points[0]) +{ + for (uint32_t i = 1; i < points.size(); ++i) + { + const VHACD::Vertex& p = points[i]; + m_min = m_min.CWiseMin(p); + m_max = m_max.CWiseMax(p); + } +} + +inline BoundsAABB::BoundsAABB(const VHACD::Vect3& min, + const VHACD::Vect3& max) + : m_min(min) + , m_max(max) +{ +} + +BoundsAABB BoundsAABB::Union(const BoundsAABB& b) +{ + return BoundsAABB(GetMin().CWiseMin(b.GetMin()), + GetMax().CWiseMax(b.GetMax())); +} + +inline bool VHACD::BoundsAABB::Intersects(const VHACD::BoundsAABB& b) const +{ + if ( ( GetMin().GetX() > b.GetMax().GetX()) + || (b.GetMin().GetX() > GetMax().GetX())) + return false; + if ( ( GetMin().GetY() > b.GetMax().GetY()) + || (b.GetMin().GetY() > GetMax().GetY())) + return false; + if ( ( GetMin().GetZ() > b.GetMax().GetZ()) + || (b.GetMin().GetZ() > GetMax().GetZ())) + return false; + return true; +} + +double BoundsAABB::SurfaceArea() const +{ + VHACD::Vect3 d = GetMax() - GetMin(); + return double(2.0) * (d.GetX() * d.GetY() + d.GetX() * d.GetZ() + d.GetY() * d.GetZ()); +} + +inline double VHACD::BoundsAABB::Volume() const +{ + VHACD::Vect3 d = GetMax() - GetMin(); + return d.GetX() * d.GetY() * d.GetZ(); +} + +inline BoundsAABB VHACD::BoundsAABB::Inflate(double ratio) const +{ + double inflate = (GetMin() - GetMax()).GetNorm() * double(0.5) * ratio; + return BoundsAABB(GetMin() - inflate, + GetMax() + inflate); +} + +inline VHACD::Vect3 VHACD::BoundsAABB::ClosestPoint(const VHACD::Vect3& p) const +{ + return p.CWiseMax(GetMin()).CWiseMin(GetMax()); +} + +inline VHACD::Vect3& VHACD::BoundsAABB::GetMin() +{ + return m_min; +} + +inline VHACD::Vect3& VHACD::BoundsAABB::GetMax() +{ + return m_max; +} + +inline const VHACD::Vect3& VHACD::BoundsAABB::GetMin() const +{ + return m_min; +} + +inline const VHACD::Vect3& VHACD::BoundsAABB::GetMax() const +{ + return m_max; +} + +inline VHACD::Vect3 VHACD::BoundsAABB::GetSize() const +{ + return GetMax() - GetMin(); +} + +inline VHACD::Vect3 VHACD::BoundsAABB::GetCenter() const +{ + return (GetMin() + GetMax()) * double(0.5); +} + +/* + * Relies on three way comparison, which std::sort doesn't use + */ +template +void Sort(T* const array, int elements) +{ + const int batchSize = 8; + int stack[1024][2]; + + stack[0][0] = 0; + stack[0][1] = elements - 1; + int stackIndex = 1; + const dCompareKey comparator; + while (stackIndex) + { + stackIndex--; + int lo = stack[stackIndex][0]; + int hi = stack[stackIndex][1]; + if ((hi - lo) > batchSize) + { + int mid = (lo + hi) >> 1; + if (comparator.Compare(array[lo], array[mid]) > 0) + { + std::swap(array[lo], + array[mid]); + } + if (comparator.Compare(array[mid], array[hi]) > 0) + { + std::swap(array[mid], + array[hi]); + } + if (comparator.Compare(array[lo], array[mid]) > 0) + { + std::swap(array[lo], + array[mid]); + } + int i = lo + 1; + int j = hi - 1; + const T pivot(array[mid]); + do + { + while (comparator.Compare(array[i], pivot) < 0) + { + i++; + } + while (comparator.Compare(array[j], pivot) > 0) + { + j--; + } + + if (i <= j) + { + std::swap(array[i], + array[j]); + i++; + j--; + } + } while (i <= j); + + if (i < hi) + { + stack[stackIndex][0] = i; + stack[stackIndex][1] = hi; + stackIndex++; + } + if (lo < j) + { + stack[stackIndex][0] = lo; + stack[stackIndex][1] = j; + stackIndex++; + } + assert(stackIndex < int(sizeof(stack) / (2 * sizeof(stack[0][0])))); + } + } + + int stride = batchSize + 1; + if (elements < stride) + { + stride = elements; + } + for (int i = 1; i < stride; ++i) + { + if (comparator.Compare(array[0], array[i]) > 0) + { + std::swap(array[0], + array[i]); + } + } + + for (int i = 1; i < elements; ++i) + { + int j = i; + const T tmp(array[i]); + for (; comparator.Compare(array[j - 1], tmp) > 0; --j) + { + assert(j > 0); + array[j] = array[j - 1]; + } + array[j] = tmp; + } +} + +/* +Maintaining comment due to attribution +Purpose: + +TRIANGLE_AREA_3D computes the area of a triangle in 3D. + +Modified: + +22 April 1999 + +Author: + +John Burkardt + +Parameters: + +Input, double X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, the (getX,getY,getZ) +coordinates of the corners of the triangle. + +Output, double TRIANGLE_AREA_3D, the area of the triangle. +*/ +double ComputeArea(const VHACD::Vect3& p1, + const VHACD::Vect3& p2, + const VHACD::Vect3& p3) +{ + /* + Find the projection of (P3-P1) onto (P2-P1). + */ + double base = (p2 - p1).GetNorm(); + /* + The height of the triangle is the length of (P3-P1) after its + projection onto (P2-P1) has been subtracted. + */ + double height; + if (base == double(0.0)) + { + height = double(0.0); + } + else + { + double dot = (p3 - p1).Dot(p2 - p1); + double alpha = dot / (base * base); + + VHACD::Vect3 a = p3 - p1 - alpha * (p2 - p1); + height = a.GetNorm(); + } + + return double(0.5) * base * height; +} + +bool ComputeCentroid(const std::vector& points, + const std::vector& indices, + VHACD::Vect3& center) + +{ + bool ret = false; + if (points.size()) + { + center = VHACD::Vect3(0); + + VHACD::Vect3 numerator(0); + double denominator = 0; + + for (uint32_t i = 0; i < indices.size(); i++) + { + uint32_t i1 = indices[i].mI0; + uint32_t i2 = indices[i].mI1; + uint32_t i3 = indices[i].mI2; + + const VHACD::Vect3& p1 = points[i1]; + const VHACD::Vect3& p2 = points[i2]; + const VHACD::Vect3& p3 = points[i3]; + + // Compute the average of the sum of the three positions + VHACD::Vect3 sum = (p1 + p2 + p3) / 3; + + // Compute the area of this triangle + double area = ComputeArea(p1, + p2, + p3); + + numerator += (sum * area); + + denominator += area; + } + double recip = 1 / denominator; + center = numerator * recip; + ret = true; + } + return ret; +} + +double Determinant3x3(const std::array& matrix, + double& error) +{ + double det = double(0.0); + error = double(0.0); + + double a01xa12 = matrix[0].GetY() * matrix[1].GetZ(); + double a02xa11 = matrix[0].GetZ() * matrix[1].GetY(); + error += (std::abs(a01xa12) + std::abs(a02xa11)) * std::abs(matrix[2].GetX()); + det += (a01xa12 - a02xa11) * matrix[2].GetX(); + + double a00xa12 = matrix[0].GetX() * matrix[1].GetZ(); + double a02xa10 = matrix[0].GetZ() * matrix[1].GetX(); + error += (std::abs(a00xa12) + std::abs(a02xa10)) * std::abs(matrix[2].GetY()); + det -= (a00xa12 - a02xa10) * matrix[2].GetY(); + + double a00xa11 = matrix[0].GetX() * matrix[1].GetY(); + double a01xa10 = matrix[0].GetY() * matrix[1].GetX(); + error += (std::abs(a00xa11) + std::abs(a01xa10)) * std::abs(matrix[2].GetZ()); + det += (a00xa11 - a01xa10) * matrix[2].GetZ(); + + return det; +} + +double ComputeMeshVolume(const std::vector& vertices, + const std::vector& indices) +{ + double volume = 0; + for (uint32_t i = 0; i < indices.size(); i++) + { + const std::array m = { + vertices[indices[i].mI0], + vertices[indices[i].mI1], + vertices[indices[i].mI2] + }; + double placeholder; + volume += Determinant3x3(m, + placeholder); + } + + volume *= (double(1.0) / double(6.0)); + if (volume < 0) + volume *= -1; + return volume; +} + +/* + * To minimize memory allocations while maintaining pointer stability. + * Used in KdTreeNode and ConvexHull, as both use tree data structures that rely on pointer stability + * Neither rely on random access or iteration + * They just dump elements into a memory pool, then refer to pointers to the elements + * All elements are default constructed in NodeStorage's m_nodes array + */ +template +class NodeBundle +{ + struct NodeStorage { + bool IsFull() const; + + T& GetNextNode(); + + std::size_t m_index; + std::array m_nodes; + }; + + std::list m_list; + typename std::list::iterator m_head{ m_list.end() }; + +public: + T& GetNextNode(); + + T& GetFirstNode(); + + void Clear(); +}; + +template +bool NodeBundle::NodeStorage::IsFull() const +{ + return m_index == MaxBundleSize; +} + +template +T& NodeBundle::NodeStorage::GetNextNode() +{ + assert(m_index < MaxBundleSize); + T& ret = m_nodes[m_index]; + m_index++; + return ret; +} + +template +T& NodeBundle::GetNextNode() +{ + /* + * || short circuits, so doesn't dereference if m_bundle == m_bundleHead.end() + */ + if ( m_head == m_list.end() + || m_head->IsFull()) + { + m_head = m_list.emplace(m_list.end()); + } + + return m_head->GetNextNode(); +} + +template +T& NodeBundle::GetFirstNode() +{ + assert(m_head != m_list.end()); + return m_list.front().m_nodes[0]; +} + +template +void NodeBundle::Clear() +{ + m_list.clear(); +} + +/* + * Returns index of highest set bit in x + */ +inline int dExp2(int x) +{ + int exp; + for (exp = -1; x; x >>= 1) + { + exp++; + } + return exp; +} + +/* + * Reverses the order of the bits in v and returns the result + * Does not put fill any of the bits higher than the highest bit in v + * Only used to calculate index of ndNormalMap::m_normal when tessellating a triangle + */ +inline int dBitReversal(int v, + int base) +{ + int x = 0; + int power = dExp2(base) - 1; + do + { + x += (v & 1) << power; + v >>= 1; + power--; + } while (v); + return x; +} + +class Googol +{ + #define VHACD_GOOGOL_SIZE 4 +public: + Googol() = default; + Googol(double value); + + operator double() const; + Googol operator+(const Googol &A) const; + Googol operator-(const Googol &A) const; + Googol operator*(const Googol &A) const; + Googol operator/ (const Googol &A) const; + + Googol& operator+= (const Googol &A); + Googol& operator-= (const Googol &A); + + bool operator>(const Googol &A) const; + bool operator>=(const Googol &A) const; + bool operator<(const Googol &A) const; + bool operator<=(const Googol &A) const; + bool operator==(const Googol &A) const; + bool operator!=(const Googol &A) const; + + Googol Abs() const; + Googol Floor() const; + Googol InvSqrt() const; + Googol Sqrt() const; + + void ToString(char* const string) const; + +private: + void NegateMantissa(std::array& mantissa) const; + void CopySignedMantissa(std::array& mantissa) const; + int NormalizeMantissa(std::array& mantissa) const; + void ShiftRightMantissa(std::array& mantissa, + int bits) const; + uint64_t CheckCarrier(uint64_t a, uint64_t b) const; + + int LeadingZeros(uint64_t a) const; + void ExtendedMultiply(uint64_t a, + uint64_t b, + uint64_t& high, + uint64_t& low) const; + void ScaleMantissa(uint64_t* out, + uint64_t scale) const; + + int m_sign{ 0 }; + int m_exponent{ 0 }; + std::array m_mantissa{ 0 }; + +public: + static Googol m_zero; + static Googol m_one; + static Googol m_two; + static Googol m_three; + static Googol m_half; +}; + +Googol Googol::m_zero(double(0.0)); +Googol Googol::m_one(double(1.0)); +Googol Googol::m_two(double(2.0)); +Googol Googol::m_three(double(3.0)); +Googol Googol::m_half(double(0.5)); + +Googol::Googol(double value) +{ + int exp; + double mantissa = fabs(frexp(value, &exp)); + + m_exponent = exp; + m_sign = (value >= 0) ? 0 : 1; + + m_mantissa[0] = uint64_t(double(uint64_t(1) << 62) * mantissa); +} + +Googol::operator double() const +{ + double mantissa = (double(1.0) / double(uint64_t(1) << 62)) * double(m_mantissa[0]); + mantissa = ldexp(mantissa, m_exponent) * (m_sign ? double(-1.0) : double(1.0)); + return mantissa; +} + +Googol Googol::operator+(const Googol &A) const +{ + Googol tmp; + if (m_mantissa[0] && A.m_mantissa[0]) + { + std::array mantissa0; + std::array mantissa1; + std::array mantissa; + + CopySignedMantissa(mantissa0); + A.CopySignedMantissa(mantissa1); + + int exponentDiff = m_exponent - A.m_exponent; + int exponent = m_exponent; + if (exponentDiff > 0) + { + ShiftRightMantissa(mantissa1, + exponentDiff); + } + else if (exponentDiff < 0) + { + exponent = A.m_exponent; + ShiftRightMantissa(mantissa0, + -exponentDiff); + } + + uint64_t carrier = 0; + for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) + { + uint64_t m0 = mantissa0[i]; + uint64_t m1 = mantissa1[i]; + mantissa[i] = m0 + m1 + carrier; + carrier = CheckCarrier(m0, m1) | CheckCarrier(m0 + m1, carrier); + } + + int sign = 0; + if (int64_t(mantissa[0]) < 0) + { + sign = 1; + NegateMantissa(mantissa); + } + + int bits = NormalizeMantissa(mantissa); + if (bits <= (-64 * VHACD_GOOGOL_SIZE)) + { + tmp.m_sign = 0; + tmp.m_exponent = 0; + } + else + { + tmp.m_sign = sign; + tmp.m_exponent = int(exponent + bits); + } + + tmp.m_mantissa = mantissa; + } + else if (A.m_mantissa[0]) + { + tmp = A; + } + else + { + tmp = *this; + } + + return tmp; +} + +Googol Googol::operator-(const Googol &A) const +{ + Googol tmp(A); + tmp.m_sign = !tmp.m_sign; + return *this + tmp; +} + +Googol Googol::operator*(const Googol &A) const +{ + if (m_mantissa[0] && A.m_mantissa[0]) + { + std::array mantissaAcc{ 0 }; + for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) + { + uint64_t a = m_mantissa[i]; + if (a) + { + uint64_t mantissaScale[2 * VHACD_GOOGOL_SIZE] = { 0 }; + A.ScaleMantissa(&mantissaScale[i], a); + + uint64_t carrier = 0; + for (int j = 0; j < 2 * VHACD_GOOGOL_SIZE; j++) + { + const int k = 2 * VHACD_GOOGOL_SIZE - 1 - j; + uint64_t m0 = mantissaAcc[k]; + uint64_t m1 = mantissaScale[k]; + mantissaAcc[k] = m0 + m1 + carrier; + carrier = CheckCarrier(m0, m1) | CheckCarrier(m0 + m1, carrier); + } + } + } + + uint64_t carrier = 0; + int bits = LeadingZeros(mantissaAcc[0]) - 2; + for (int i = 0; i < 2 * VHACD_GOOGOL_SIZE; i++) + { + const int k = 2 * VHACD_GOOGOL_SIZE - 1 - i; + uint64_t a = mantissaAcc[k]; + mantissaAcc[k] = (a << uint64_t(bits)) | carrier; + carrier = a >> uint64_t(64 - bits); + } + + int exp = m_exponent + A.m_exponent - (bits - 2); + + Googol tmp; + tmp.m_sign = m_sign ^ A.m_sign; + tmp.m_exponent = exp; + for (std::size_t i = 0; i < tmp.m_mantissa.size(); ++i) + { + tmp.m_mantissa[i] = mantissaAcc[i]; + } + + return tmp; + } + return Googol(double(0.0)); +} + +Googol Googol::operator/(const Googol &A) const +{ + Googol tmp(double(1.0) / A); + tmp = tmp * (m_two - A * tmp); + tmp = tmp * (m_two - A * tmp); + bool test = false; + int passes = 0; + do + { + passes++; + Googol tmp0(tmp); + tmp = tmp * (m_two - A * tmp); + test = tmp0 == tmp; + } while (test && (passes < (2 * VHACD_GOOGOL_SIZE))); + return (*this) * tmp; +} + +Googol& Googol::operator+=(const Googol &A) +{ + *this = *this + A; + return *this; +} + +Googol& Googol::operator-=(const Googol &A) +{ + *this = *this - A; + return *this; +} + +bool Googol::operator>(const Googol &A) const +{ + Googol tmp(*this - A); + return double(tmp) > double(0.0); +} + +bool Googol::operator>=(const Googol &A) const +{ + Googol tmp(*this - A); + return double(tmp) >= double(0.0); +} + +bool Googol::operator<(const Googol &A) const +{ + Googol tmp(*this - A); + return double(tmp) < double(0.0); +} + +bool Googol::operator<=(const Googol &A) const +{ + Googol tmp(*this - A); + return double(tmp) <= double(0.0); +} + +bool Googol::operator==(const Googol &A) const +{ + return m_sign == A.m_sign + && m_exponent == A.m_exponent + && m_mantissa == A.m_mantissa; +} + +bool Googol::operator!=(const Googol &A) const +{ + return !(*this == A); +} + +Googol Googol::Abs() const +{ + Googol tmp(*this); + tmp.m_sign = 0; + return tmp; +} + +Googol Googol::Floor() const +{ + if (m_exponent < 1) + { + return Googol(double(0.0)); + } + int bits = m_exponent + 2; + int start = 0; + while (bits >= 64) + { + bits -= 64; + start++; + } + + Googol tmp(*this); + for (int i = VHACD_GOOGOL_SIZE - 1; i > start; i--) + { + tmp.m_mantissa[i] = 0; + } + // some compilers do no like this and I do not know why is that + //uint64_t mask = (-1LL) << (64 - bits); + uint64_t mask(~0ULL); + mask <<= (64 - bits); + tmp.m_mantissa[start] &= mask; + return tmp; +} + +Googol Googol::InvSqrt() const +{ + const Googol& me = *this; + Googol x(double(1.0) / sqrt(me)); + + int test = 0; + int passes = 0; + do + { + passes++; + Googol tmp(x); + x = m_half * x * (m_three - me * x * x); + test = (x != tmp); + } while (test && (passes < (2 * VHACD_GOOGOL_SIZE))); + return x; +} + +Googol Googol::Sqrt() const +{ + return *this * InvSqrt(); +} + +void Googol::ToString(char* const string) const +{ + Googol tmp(*this); + Googol base(double(10.0)); + while (double(tmp) > double(1.0)) + { + tmp = tmp / base; + } + + int index = 0; + while (tmp.m_mantissa[0]) + { + tmp = tmp * base; + Googol digit(tmp.Floor()); + tmp -= digit; + double val = digit; + string[index] = char(val) + '0'; + index++; + } + string[index] = 0; +} + +void Googol::NegateMantissa(std::array& mantissa) const +{ + uint64_t carrier = 1; + for (int i = mantissa.size() - 1; i >= 0; i--) + { + uint64_t a = ~mantissa[i] + carrier; + if (a) + { + carrier = 0; + } + mantissa[i] = a; + } +} + +void Googol::CopySignedMantissa(std::array& mantissa) const +{ + mantissa = m_mantissa; + if (m_sign) + { + NegateMantissa(mantissa); + } +} + +int Googol::NormalizeMantissa(std::array& mantissa) const +{ + int bits = 0; + if (int64_t(mantissa[0] * 2) < 0) + { + bits = 1; + ShiftRightMantissa(mantissa, 1); + } + else + { + while (!mantissa[0] && bits > (-64 * VHACD_GOOGOL_SIZE)) + { + bits -= 64; + for (int i = 1; i < VHACD_GOOGOL_SIZE; i++) { + mantissa[i - 1] = mantissa[i]; + } + mantissa[VHACD_GOOGOL_SIZE - 1] = 0; + } + + if (bits > (-64 * VHACD_GOOGOL_SIZE)) + { + int n = LeadingZeros(mantissa[0]) - 2; + if (n > 0) + { + uint64_t carrier = 0; + for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) + { + uint64_t a = mantissa[i]; + mantissa[i] = (a << n) | carrier; + carrier = a >> (64 - n); + } + bits -= n; + } + else if (n < 0) + { + // this is very rare but it does happens, whee the leading zeros of the mantissa is an exact multiple of 64 + uint64_t carrier = 0; + int shift = -n; + for (int i = 0; i < VHACD_GOOGOL_SIZE; i++) + { + uint64_t a = mantissa[i]; + mantissa[i] = (a >> shift) | carrier; + carrier = a << (64 - shift); + } + bits -= n; + } } - return getZ(); + } + return bits; +} + +void Googol::ShiftRightMantissa(std::array& mantissa, + int bits) const +{ + uint64_t carrier = 0; + if (int64_t(mantissa[0]) < int64_t(0)) + { + carrier = uint64_t(-1); } - template - inline T Vect3::MinCoeff(uint32_t& idx) const + while (bits >= 64) { - if (getX() < getY() && getX() < getZ()) + for (int i = VHACD_GOOGOL_SIZE - 2; i >= 0; i--) { - idx = 0; - return getX(); + mantissa[i + 1] = mantissa[i]; } - if (getY() < getZ()) + mantissa[0] = carrier; + bits -= 64; + } + + if (bits > 0) + { + carrier <<= (64 - bits); + for (int i = 0; i < VHACD_GOOGOL_SIZE; i++) { - idx = 1; - return getY(); + uint64_t a = mantissa[i]; + mantissa[i] = (a >> bits) | carrier; + carrier = a << (64 - bits); } - idx = 2; - return getZ(); } +} + +uint64_t Googol::CheckCarrier(uint64_t a, uint64_t b) const +{ + return ((uint64_t(-1) - b) < a) ? uint64_t(1) : 0; +} + +int Googol::LeadingZeros(uint64_t a) const +{ + #define VHACD_COUNTBIT(mask, add) \ + do { \ + uint64_t test = a & mask; \ + n += test ? 0 : add; \ + a = test ? test : (a & ~mask); \ + } while (false) + + int n = 0; + VHACD_COUNTBIT(0xffffffff00000000LL, 32); + VHACD_COUNTBIT(0xffff0000ffff0000LL, 16); + VHACD_COUNTBIT(0xff00ff00ff00ff00LL, 8); + VHACD_COUNTBIT(0xf0f0f0f0f0f0f0f0LL, 4); + VHACD_COUNTBIT(0xccccccccccccccccLL, 2); + VHACD_COUNTBIT(0xaaaaaaaaaaaaaaaaLL, 1); + + return n; +} + +void Googol::ExtendedMultiply(uint64_t a, + uint64_t b, + uint64_t& high, + uint64_t& low) const +{ + uint64_t bLow = b & 0xffffffff; + uint64_t bHigh = b >> 32; + uint64_t aLow = a & 0xffffffff; + uint64_t aHigh = a >> 32; + + uint64_t l = bLow * aLow; + + uint64_t c1 = bHigh * aLow; + uint64_t c2 = bLow * aHigh; + uint64_t m = c1 + c2; + uint64_t carrier = CheckCarrier(c1, c2) << 32; + + uint64_t h = bHigh * aHigh + carrier; + + uint64_t ml = m << 32; + uint64_t ll = l + ml; + uint64_t mh = (m >> 32) + CheckCarrier(l, ml); + uint64_t hh = h + mh; + + low = ll; + high = hh; +} + +void Googol::ScaleMantissa(uint64_t* dst, + uint64_t scale) const +{ + uint64_t carrier = 0; + for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) + { + if (m_mantissa[i]) + { + uint64_t low; + uint64_t high; + ExtendedMultiply(scale, + m_mantissa[i], + high, + low); + uint64_t acc = low + carrier; + carrier = CheckCarrier(low, + carrier); + carrier += high; + dst[i + 1] = acc; + } + else + { + dst[i + 1] = carrier; + carrier = 0; + } + + } + dst[0] = carrier; +} + +Googol Determinant3x3(const std::array, 3>& matrix) +{ + Googol det = double(0.0); + + Googol a01xa12 = matrix[0].GetY() * matrix[1].GetZ(); + Googol a02xa11 = matrix[0].GetZ() * matrix[1].GetY(); + det += (a01xa12 - a02xa11) * matrix[2].GetX(); + + Googol a00xa12 = matrix[0].GetX() * matrix[1].GetZ(); + Googol a02xa10 = matrix[0].GetZ() * matrix[1].GetX(); + det -= (a00xa12 - a02xa10) * matrix[2].GetY(); + + Googol a00xa11 = matrix[0].GetX() * matrix[1].GetY(); + Googol a01xa10 = matrix[0].GetY() * matrix[1].GetX(); + det += (a00xa11 - a01xa10) * matrix[2].GetZ(); + return det; +} + +class HullPlane : public VHACD::Vect3 +{ +public: + HullPlane(const HullPlane&) = default; + HullPlane(double x, + double y, + double z, + double w); + + HullPlane(const VHACD::Vect3& p, + double w); + + HullPlane(const VHACD::Vect3& p0, + const VHACD::Vect3& p1, + const VHACD::Vect3& p2); + + HullPlane Scale(double s) const; + + HullPlane& operator=(const HullPlane& rhs); + + double Evalue(const VHACD::Vect3 &point) const; + + double& GetW(); + const double& GetW() const; + +private: + double m_w; +}; + +HullPlane::HullPlane(double x, + double y, + double z, + double w) + : VHACD::Vect3(x, y, z) + , m_w(w) +{ +} + +HullPlane::HullPlane(const VHACD::Vect3& p, + double w) + : VHACD::Vect3(p) + , m_w(w) +{ +} + +HullPlane::HullPlane(const VHACD::Vect3& p0, + const VHACD::Vect3& p1, + const VHACD::Vect3& p2) + : VHACD::Vect3((p1 - p0).Cross(p2 - p0)) + , m_w(-Dot(p0)) +{ +} + +HullPlane HullPlane::Scale(double s) const +{ + return HullPlane(*this * s, + m_w * s); +} + +HullPlane& HullPlane::operator=(const HullPlane& rhs) +{ + GetX() = rhs.GetX(); + GetY() = rhs.GetY(); + GetZ() = rhs.GetZ(); + m_w = rhs.m_w; + return *this; +} + +double HullPlane::Evalue(const VHACD::Vect3& point) const +{ + return Dot(point) + m_w; +} + +double& HullPlane::GetW() +{ + return m_w; +} + +const double& HullPlane::GetW() const +{ + return m_w; +} + +class ConvexHullFace +{ +public: + ConvexHullFace() = default; + double Evalue(const std::vector& pointArray, + const VHACD::Vect3& point) const; + HullPlane GetPlaneEquation(const std::vector& pointArray, + bool& isValid) const; + + std::array m_index; +private: + int m_mark{ 0 }; + std::array::iterator, 3> m_twin; + + friend class ConvexHull; +}; + +double ConvexHullFace::Evalue(const std::vector& pointArray, + const VHACD::Vect3& point) const +{ + const VHACD::Vect3& p0 = pointArray[m_index[0]]; + const VHACD::Vect3& p1 = pointArray[m_index[1]]; + const VHACD::Vect3& p2 = pointArray[m_index[2]]; + + std::array matrix = { p2 - p0, p1 - p0, point - p0 }; + double error; + double det = Determinant3x3(matrix, + error); + + // the code use double, however the threshold for accuracy test is the machine precision of a float. + // by changing this to a smaller number, the code should run faster since many small test will be considered valid + // the precision must be a power of two no smaller than the machine precision of a double, (1<<48) + // float64(1<<30) can be a good value + + // double precision = double (1.0f) / double (1<<30); + double precision = double(1.0) / double(1 << 24); + double errbound = error * precision; + if (fabs(det) > errbound) + { + return det; + } + + const VHACD::Vector3 p0g = pointArray[m_index[0]]; + const VHACD::Vector3 p1g = pointArray[m_index[1]]; + const VHACD::Vector3 p2g = pointArray[m_index[2]]; + const VHACD::Vector3 pointg = point; + std::array, 3> exactMatrix = { p2g - p0g, p1g - p0g, pointg - p0g }; + return Determinant3x3(exactMatrix); +} + +HullPlane ConvexHullFace::GetPlaneEquation(const std::vector& pointArray, + bool& isvalid) const +{ + const VHACD::Vect3& p0 = pointArray[m_index[0]]; + const VHACD::Vect3& p1 = pointArray[m_index[1]]; + const VHACD::Vect3& p2 = pointArray[m_index[2]]; + HullPlane plane(p0, p1, p2); + + isvalid = false; + double mag2 = plane.Dot(plane); + if (mag2 > double(1.0e-16)) + { + isvalid = true; + plane = plane.Scale(double(1.0) / sqrt(mag2)); + } + return plane; +} + +class ConvexHullVertex : public VHACD::Vect3 +{ +public: + ConvexHullVertex() = default; + ConvexHullVertex(const ConvexHullVertex&) = default; + ConvexHullVertex& operator=(const ConvexHullVertex& rhs) = default; + using VHACD::Vect3::operator=; + + int m_mark{ 0 }; +}; + + +class ConvexHullAABBTreeNode +{ + #define VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE 8 +public: + ConvexHullAABBTreeNode() = default; + ConvexHullAABBTreeNode(ConvexHullAABBTreeNode* parent); + + VHACD::Vect3 m_box[2]; + ConvexHullAABBTreeNode* m_left{ nullptr }; + ConvexHullAABBTreeNode* m_right{ nullptr }; + ConvexHullAABBTreeNode* m_parent{ nullptr }; + + size_t m_count; + std::array m_indices; +}; + +ConvexHullAABBTreeNode::ConvexHullAABBTreeNode(ConvexHullAABBTreeNode* parent) + : m_parent(parent) +{ +} + +class ConvexHull +{ + class ndNormalMap; + +public: + ConvexHull(const ConvexHull& source); + ConvexHull(const std::vector<::VHACD::Vertex>& vertexCloud, + double distTol, + int maxVertexCount = 0x7fffffff); + ~ConvexHull() = default; + + const std::vector& GetVertexPool() const; + + const std::list& GetList() const { return m_list; } + +private: + void BuildHull(const std::vector<::VHACD::Vertex>& vertexCloud, + double distTol, + int maxVertexCount); + + void GetUniquePoints(std::vector& points); + int InitVertexArray(std::vector& points, + NodeBundle& memoryPool); + + ConvexHullAABBTreeNode* BuildTreeNew(std::vector& points, + std::vector& memoryPool) const; + ConvexHullAABBTreeNode* BuildTreeOld(std::vector& points, + NodeBundle& memoryPool); + ConvexHullAABBTreeNode* BuildTreeRecurse(ConvexHullAABBTreeNode* const parent, + ConvexHullVertex* const points, + int count, + int baseIndex, + NodeBundle& memoryPool) const; + + std::list::iterator AddFace(int i0, + int i1, + int i2); + + void CalculateConvexHull3D(ConvexHullAABBTreeNode* vertexTree, + std::vector& points, + int count, + double distTol, + int maxVertexCount); + + int SupportVertex(ConvexHullAABBTreeNode** const tree, + const std::vector& points, + const VHACD::Vect3& dir, + const bool removeEntry = true) const; + double TetrahedrumVolume(const VHACD::Vect3& p0, + const VHACD::Vect3& p1, + const VHACD::Vect3& p2, + const VHACD::Vect3& p3) const; + + std::list m_list; + VHACD::Vect3 m_aabbP0{ 0 }; + VHACD::Vect3 m_aabbP1{ 0 }; + double m_diag{ 0.0 }; + std::vector m_points; +}; + +class ConvexHull::ndNormalMap +{ +public: + ndNormalMap(); + + static const ndNormalMap& GetNormalMap(); + + void TessellateTriangle(int level, + const VHACD::Vect3& p0, + const VHACD::Vect3& p1, + const VHACD::Vect3& p2, + int& count); + + std::array m_normal; + int m_count{ 128 }; +}; + +const ConvexHull::ndNormalMap& ConvexHull::ndNormalMap::GetNormalMap() +{ + static ndNormalMap normalMap; + return normalMap; +} + +void ConvexHull::ndNormalMap::TessellateTriangle(int level, + const VHACD::Vect3& p0, + const VHACD::Vect3& p1, + const VHACD::Vect3& p2, + int& count) +{ + if (level) + { + assert(fabs(p0.Dot(p0) - double(1.0)) < double(1.0e-4)); + assert(fabs(p1.Dot(p1) - double(1.0)) < double(1.0e-4)); + assert(fabs(p2.Dot(p2) - double(1.0)) < double(1.0e-4)); + VHACD::Vect3 p01(p0 + p1); + VHACD::Vect3 p12(p1 + p2); + VHACD::Vect3 p20(p2 + p0); + + p01 = p01 * (double(1.0) / p01.GetNorm()); + p12 = p12 * (double(1.0) / p12.GetNorm()); + p20 = p20 * (double(1.0) / p20.GetNorm()); + + assert(fabs(p01.GetNormSquared() - double(1.0)) < double(1.0e-4)); + assert(fabs(p12.GetNormSquared() - double(1.0)) < double(1.0e-4)); + assert(fabs(p20.GetNormSquared() - double(1.0)) < double(1.0e-4)); + + TessellateTriangle(level - 1, p0, p01, p20, count); + TessellateTriangle(level - 1, p1, p12, p01, count); + TessellateTriangle(level - 1, p2, p20, p12, count); + TessellateTriangle(level - 1, p01, p12, p20, count); + } + else + { + /* + * This is just m_normal[index] = n.Normalized(), but due to tiny floating point errors, causes + * different outputs, so I'm leaving it + */ + HullPlane n(p0, p1, p2); + n = n.Scale(double(1.0) / n.GetNorm()); + n.GetW() = double(0.0); + int index = dBitReversal(count, + int(m_normal.size())); + m_normal[index] = n; + count++; + assert(count <= int(m_normal.size())); + } +} + +ConvexHull::ndNormalMap::ndNormalMap() +{ + VHACD::Vect3 p0(double( 1.0), double( 0.0), double( 0.0)); + VHACD::Vect3 p1(double(-1.0), double( 0.0), double( 0.0)); + VHACD::Vect3 p2(double( 0.0), double( 1.0), double( 0.0)); + VHACD::Vect3 p3(double( 0.0), double(-1.0), double( 0.0)); + VHACD::Vect3 p4(double( 0.0), double( 0.0), double( 1.0)); + VHACD::Vect3 p5(double( 0.0), double( 0.0), double(-1.0)); + + int count = 0; + int subdivisions = 2; + TessellateTriangle(subdivisions, p4, p0, p2, count); + TessellateTriangle(subdivisions, p0, p5, p2, count); + TessellateTriangle(subdivisions, p5, p1, p2, count); + TessellateTriangle(subdivisions, p1, p4, p2, count); + TessellateTriangle(subdivisions, p0, p4, p3, count); + TessellateTriangle(subdivisions, p5, p0, p3, count); + TessellateTriangle(subdivisions, p1, p5, p3, count); + TessellateTriangle(subdivisions, p4, p1, p3, count); +} + +ConvexHull::ConvexHull(const std::vector<::VHACD::Vertex>& vertexCloud, + double distTol, + int maxVertexCount) +{ + if (vertexCloud.size() >= 4) + { + BuildHull(vertexCloud, + distTol, + maxVertexCount); + } +} + +const std::vector& ConvexHull::GetVertexPool() const +{ + return m_points; +} + +void ConvexHull::BuildHull(const std::vector<::VHACD::Vertex>& vertexCloud, + double distTol, + int maxVertexCount) +{ + size_t treeCount = vertexCloud.size() / (VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE >> 1); + treeCount = std::max(treeCount, size_t(4)) * 2; + + std::vector points(vertexCloud.size()); + /* + * treePool provides a memory pool for the AABB tree + * Each node is either a leaf or non-leaf node + * Non-leaf nodes have up to 8 vertices + * Vertices are specified by the m_indices array and are accessed via the points array + * + * Later on in ConvexHull::SupportVertex, the tree is used directly + * It differentiates between ConvexHullAABBTreeNode and ConvexHull3DPointCluster by whether the m_left and m_right + * pointers are null or not + * + * Pointers have to be stable + */ + NodeBundle treePool; + for (size_t i = 0; i < vertexCloud.size(); ++i) + { + points[i] = VHACD::Vect3(vertexCloud[i]); + } + int count = InitVertexArray(points, + treePool); + + if (m_points.size() >= 4) + { + CalculateConvexHull3D(&treePool.GetFirstNode(), + points, + count, + distTol, + maxVertexCount); + } +} + +void ConvexHull::GetUniquePoints(std::vector& points) +{ + class CompareVertex + { + public: + int Compare(const ConvexHullVertex& elementA, const ConvexHullVertex& elementB) const + { + for (int i = 0; i < 3; i++) + { + if (elementA[i] < elementB[i]) + { + return -1; + } + else if (elementA[i] > elementB[i]) + { + return 1; + } + } + return 0; + } + }; + + int count = int(points.size()); + Sort(points.data(), + count); + + int indexCount = 0; + CompareVertex compareVetex; + for (int i = 1; i < count; ++i) + { + for (; i < count; ++i) + { + if (compareVetex.Compare(points[indexCount], points[i])) + { + indexCount++; + points[indexCount] = points[i]; + break; + } + } + } + points.resize(indexCount + 1); +} + +ConvexHullAABBTreeNode* ConvexHull::BuildTreeRecurse(ConvexHullAABBTreeNode* const parent, + ConvexHullVertex* const points, + int count, + int baseIndex, + NodeBundle& memoryPool) const +{ + ConvexHullAABBTreeNode* tree = nullptr; + + assert(count); + VHACD::Vect3 minP( double(1.0e15)); + VHACD::Vect3 maxP(-double(1.0e15)); + if (count <= VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE) + { + ConvexHullAABBTreeNode& clump = memoryPool.GetNextNode(); + + clump.m_count = count; + for (int i = 0; i < count; ++i) + { + clump.m_indices[i] = i + baseIndex; + + const VHACD::Vect3& p = points[i]; + minP = minP.CWiseMin(p); + maxP = maxP.CWiseMax(p); + } + + clump.m_left = nullptr; + clump.m_right = nullptr; + tree = &clump; + } + else + { + VHACD::Vect3 median(0); + VHACD::Vect3 varian(0); + for (int i = 0; i < count; ++i) + { + const VHACD::Vect3& p = points[i]; + minP = minP.CWiseMin(p); + maxP = maxP.CWiseMax(p); + median += p; + varian += p.CWiseMul(p); + } + + varian = varian * double(count) - median.CWiseMul(median); + int index = 0; + double maxVarian = double(-1.0e10); + for (int i = 0; i < 3; ++i) + { + if (varian[i] > maxVarian) + { + index = i; + maxVarian = varian[i]; + } + } + VHACD::Vect3 center(median * (double(1.0) / double(count))); + + double test = center[index]; + + int i0 = 0; + int i1 = count - 1; + do + { + for (; i0 <= i1; i0++) + { + double val = points[i0][index]; + if (val > test) + { + break; + } + } + + for (; i1 >= i0; i1--) + { + double val = points[i1][index]; + if (val < test) + { + break; + } + } + + if (i0 < i1) + { + std::swap(points[i0], + points[i1]); + i0++; + i1--; + } + } while (i0 <= i1); + + if (i0 == 0) + { + i0 = count / 2; + } + if (i0 >= (count - 1)) + { + i0 = count / 2; + } + + tree = &memoryPool.GetNextNode(); + + assert(i0); + assert(count - i0); + + tree->m_left = BuildTreeRecurse(tree, + points, + i0, + baseIndex, + memoryPool); + tree->m_right = BuildTreeRecurse(tree, + &points[i0], + count - i0, + i0 + baseIndex, + memoryPool); + } + + assert(tree); + tree->m_parent = parent; + /* + * WARNING: Changing the compiler conversion of 1.0e-3f changes the results of the convex decomposition + * Inflate the tree's bounding box slightly + */ + tree->m_box[0] = minP - VHACD::Vect3(double(1.0e-3f)); + tree->m_box[1] = maxP + VHACD::Vect3(double(1.0e-3f)); + return tree; +} + +ConvexHullAABBTreeNode* ConvexHull::BuildTreeOld(std::vector& points, + NodeBundle& memoryPool) +{ + GetUniquePoints(points); + int count = int(points.size()); + if (count < 4) + { + return nullptr; + } + return BuildTreeRecurse(nullptr, + points.data(), + count, + 0, + memoryPool); +} + +ConvexHullAABBTreeNode* ConvexHull::BuildTreeNew(std::vector& points, + std::vector& memoryPool) const +{ + class dCluster + { + public: + VHACD::Vect3 m_sum{ double(0.0) }; + VHACD::Vect3 m_sum2{ double(0.0) }; + int m_start{ 0 }; + int m_count{ 0 }; + }; + + dCluster firstCluster; + firstCluster.m_count = int(points.size()); + + for (int i = 0; i < firstCluster.m_count; ++i) + { + const VHACD::Vect3& p = points[i]; + firstCluster.m_sum += p; + firstCluster.m_sum2 += p.CWiseMul(p); + } + + int baseCount = 0; + const int clusterSize = 16; - template - inline T Vect3::MaxCoeff(uint32_t& idx) const + if (firstCluster.m_count > clusterSize) { - if (getX() > getY() && getX() < getZ()) - { - idx = 0; - return getX(); - } - if (getY() > getZ()) + dCluster spliteStack[128]; + spliteStack[0] = firstCluster; + size_t stack = 1; + + while (stack) { - idx = 1; - return getY(); + stack--; + dCluster cluster (spliteStack[stack]); + + const VHACD::Vect3 origin(cluster.m_sum * (double(1.0) / cluster.m_count)); + const VHACD::Vect3 variance2(cluster.m_sum2 * (double(1.0) / cluster.m_count) - origin.CWiseMul(origin)); + double maxVariance2 = variance2.MaxCoeff(); + + if ( (cluster.m_count <= clusterSize) + || (stack > (sizeof(spliteStack) / sizeof(spliteStack[0]) - 4)) + || (maxVariance2 < 1.e-4f)) + { + // no sure if this is beneficial, + // the array is so small that seem too much overhead + //int maxIndex = 0; + //double min_x = 1.0e20f; + //for (int i = 0; i < cluster.m_count; ++i) + //{ + // if (points[cluster.m_start + i].getX() < min_x) + // { + // maxIndex = i; + // min_x = points[cluster.m_start + i].getX(); + // } + //} + //Swap(points[cluster.m_start], points[cluster.m_start + maxIndex]); + // + //for (int i = 2; i < cluster.m_count; ++i) + //{ + // int j = i; + // ConvexHullVertex tmp(points[cluster.m_start + i]); + // for (; points[cluster.m_start + j - 1].getX() > tmp.getX(); --j) + // { + // assert(j > 0); + // points[cluster.m_start + j] = points[cluster.m_start + j - 1]; + // } + // points[cluster.m_start + j] = tmp; + //} + + int count = cluster.m_count; + for (int i = cluster.m_count - 1; i > 0; --i) + { + for (int j = i - 1; j >= 0; --j) + { + VHACD::Vect3 error(points[cluster.m_start + j] - points[cluster.m_start + i]); + double mag2 = error.Dot(error); + if (mag2 < double(1.0e-6)) + { + points[cluster.m_start + j] = points[cluster.m_start + i]; + count--; + break; + } + } + } + + assert(baseCount <= cluster.m_start); + for (int i = 0; i < count; ++i) + { + points[baseCount] = points[cluster.m_start + i]; + baseCount++; + } + } + else + { + const int firstSortAxis = variance2.LongestAxis(); + double axisVal = origin[firstSortAxis]; + + int i0 = 0; + int i1 = cluster.m_count - 1; + + const int start = cluster.m_start; + while (i0 < i1) + { + while ( (points[start + i0][firstSortAxis] <= axisVal) + && (i0 < i1)) + { + ++i0; + }; + + while ( (points[start + i1][firstSortAxis] > axisVal) + && (i0 < i1)) + { + --i1; + } + + assert(i0 <= i1); + if (i0 < i1) + { + std::swap(points[start + i0], + points[start + i1]); + ++i0; + --i1; + } + } + + while ( (points[start + i0][firstSortAxis] <= axisVal) + && (i0 < cluster.m_count)) + { + ++i0; + }; + + #ifdef _DEBUG + for (int i = 0; i < i0; ++i) + { + assert(points[start + i][firstSortAxis] <= axisVal); + } + + for (int i = i0; i < cluster.m_count; ++i) + { + assert(points[start + i][firstSortAxis] > axisVal); + } + #endif + + VHACD::Vect3 xc(0); + VHACD::Vect3 x2c(0); + for (int i = 0; i < i0; ++i) + { + const VHACD::Vect3& x = points[start + i]; + xc += x; + x2c += x.CWiseMul(x); + } + + dCluster cluster_i1(cluster); + cluster_i1.m_start = start + i0; + cluster_i1.m_count = cluster.m_count - i0; + cluster_i1.m_sum -= xc; + cluster_i1.m_sum2 -= x2c; + spliteStack[stack] = cluster_i1; + assert(cluster_i1.m_count > 0); + stack++; + + dCluster cluster_i0(cluster); + cluster_i0.m_start = start; + cluster_i0.m_count = i0; + cluster_i0.m_sum = xc; + cluster_i0.m_sum2 = x2c; + assert(cluster_i0.m_count > 0); + spliteStack[stack] = cluster_i0; + stack++; + } } - idx = 2; - return getZ(); } - /* - * Constructors - */ - template - inline Vect3::Vect3(T a) - : mData{a, a, a} - { - } - - template - inline Vect3::Vect3(T x, T y, T z) - : mData{x, y, z} - { - } - - template - inline Vect3::Vect3(const Vect3& rhs) - : mData{rhs.mData} - { - } - - template - inline Vect3::Vect3(const ::VHACD::Vertex& rhs) - : Vect3(rhs.mX, rhs.mY, rhs.mZ) - { - static_assert(std::is_same::value, "Vertex to Vect3 constructor only enabled for double"); - } - - template - inline Vect3::Vect3(const ::VHACD::Triangle& rhs) - : Vect3(rhs.mI0, rhs.mI1, rhs.mI2) - { - static_assert(std::is_same::value, "Triangle to Vect3 constructor only enabled for uint32_t"); - } - - template - inline Vect3::operator ::VHACD::Vertex() const - { - static_assert(std::is_same::value, "Vect3 to Vertex conversion only enable for double"); - return ::VHACD::Vertex(getX(), getY(), getZ()); - } -} -}// namespace nd::VHACD - -namespace nd -{ - namespace VHACD - { - #define VHACD_GOOGOL_SIZE 4 - - class Googol; - Googol Determinant2x2(const Googol matrix[2][2]); - Googol Determinant3x3(const Googol matrix[3][3]); - double Determinant2x2(const double matrix[2][2], double* const error); - double Determinant3x3(const double matrix[3][3], double* const error); - - inline int dExp2(int x) - { - int exp; - for (exp = -1; x; x >>= 1) - { - exp++; - } - return exp; - } - - inline int dBitReversal(int v, int base) - { - int x = 0; - int power = dExp2(base) - 1; - do - { - x += (v & 1) << power; - v >>= 1; - power--; - } while (v); - return x; - } - - template - class List - { - public: - class ndNode - { - ndNode(ndNode* const prev, ndNode* const next) - :m_info() - , m_next(next) - , m_prev(prev) - { - if (m_prev) - { - m_prev->m_next = this; - } - if (m_next) - { - m_next->m_prev = this; - } - } - - ndNode(const T &info, ndNode* const prev, ndNode* const next) - :m_info(info) - ,m_next(next) - ,m_prev(prev) - { - if (m_prev) - { - m_prev->m_next = this; - } - if (m_next) - { - m_next->m_prev = this; - } - } - - ~ndNode() - { - } - - void Unlink() - { - if (m_prev) - { - m_prev->m_next = m_next; - } - if (m_next) - { - m_next->m_prev = m_prev; - } - m_prev = nullptr; - m_next = nullptr; - } - - void AddLast(ndNode* const node) - { - m_next = node; - node->m_prev = this; - } - - void AddFirst(ndNode* const node) - { - m_prev = node; - node->m_next = this; - } - - public: - T& GetInfo() - { - return m_info; - } - - ndNode *GetNext() const - { - return m_next; - } - - ndNode *GetPrev() const - { - return m_prev; - } - - private: - T m_info; - ndNode *m_next; - ndNode *m_prev; - friend class List; - }; - - public: - List() - :m_first(nullptr) - , m_last(nullptr) - , m_count(0) - { - } - - ~List() - { - RemoveAll(); - } - - void RemoveAll() - { - for (ndNode *node = m_first; node; node = m_first) - { - m_count--; - m_first = node->GetNext(); - node->Unlink(); - delete node; - } - m_last = nullptr; - m_first = nullptr; - } - - ndNode* Append() - { - m_count++; - if (m_first == nullptr) - { - m_first = new ndNode(nullptr, nullptr); - m_last = m_first; - } - else - { - m_last = new ndNode(m_last, nullptr); - } - return m_last; - } - - ndNode* Append(const T &element) - { - m_count++; - if (m_first == nullptr) - { - m_first = new ndNode(element, nullptr, nullptr); - m_last = m_first; - } - else - { - m_last = new ndNode(element, m_last, nullptr); - } - return m_last; - } - - ndNode* Addtop(const T &element) - { - m_count++; - if (m_last == nullptr) - { - m_last = new ndNode(element, nullptr, nullptr); - m_first = m_last; - } - else - { - m_first = new ndNode(element, nullptr, m_first); - } - return m_first; - } - - int GetCount() const - { - return m_count; - } - - //operator int() const; - - ndNode* GetLast() const - { - return m_last; - } - - ndNode* GetFirst() const - { - return m_first; - } - - void Remove(ndNode* const node) - { - Unlink(node); - delete node; - } - - void Unlink(ndNode* const node) - { - m_count--; - if (node == m_first) - { - m_first = m_first->GetNext(); - } - if (node == m_last) - { - m_last = m_last->GetPrev(); - } - node->Unlink(); - } - - void Remove(const T &element) - { - ndNode *const node = Find(element); - if (node) - { - Remove(node); - } - } - - ndNode* Find(const T &element) const - { - ndNode *node; - for (node = m_first; node; node = node->GetNext()) - { - if (element == node->m_info) - { - break; - } - } - return node; - } - - private: - ndNode* m_first; - ndNode* m_last; - int m_count; - friend class ndNode; - }; - - class hullPlane : public VHACD::Vect3 - { - public: - hullPlane(double x, double y, double z, double w) - : VHACD::Vect3(x, y, z) - , m_w(w) - { - } - - hullPlane(const VHACD::Vect3 &P0, - const VHACD::Vect3 &P1, - const VHACD::Vect3 &P2) - : VHACD::Vect3((P1 - P0).Cross(P2 - P0)) - { - m_w = -Dot(P0); - } - - hullPlane Scale(double s) const - { - return hullPlane(getX() * s, getY() * s, getZ() * s, m_w * s); - } - - inline hullPlane operator= (const hullPlane &rhs) - { - getX() = rhs.getX(); - getY() = rhs.getY(); - getZ() = rhs.getZ(); - m_w = rhs.m_w; - return *this; - } - - inline VHACD::Vect3 operator*(const VHACD::Vect3 & rhs) const - { - return VHACD::Vect3(getX() * rhs.getX(), - getY() * rhs.getY(), - getZ() * rhs.getZ()); - } - - double Evalue(const VHACD::Vect3 &point) const - { - return Dot(point) + m_w; - } - - double m_w; - }; - - class Googol - { - public: - Googol(void); - Googol(double value); - - operator double() const; - Googol operator+ (const Googol &A) const; - Googol operator- (const Googol &A) const; - Googol operator* (const Googol &A) const; - Googol operator/ (const Googol &A) const; - - Googol operator+= (const Googol &A); - Googol operator-= (const Googol &A); - - bool operator> (const Googol &A) const; - bool operator>= (const Googol &A) const; - bool operator< (const Googol &A) const; - bool operator<= (const Googol &A) const; - bool operator== (const Googol &A) const; - bool operator!= (const Googol &A) const; - - Googol Abs() const; - Googol Sqrt() const; - Googol InvSqrt() const; - Googol Floor() const; - - void Trace() const; - void ToString(char* const string) const; - - private: - void InitFloatFloat(double value); - void NegateMantissa(uint64_t* const mantissa) const; - void CopySignedMantissa(uint64_t* const mantissa) const; - int NormalizeMantissa(uint64_t* const mantissa) const; - uint64_t CheckCarrier(uint64_t a, uint64_t b) const; - void ShiftRightMantissa(uint64_t* const mantissa, int bits) const; - - int LeadingZeros(uint64_t a) const; - void ExtendeMultiply(uint64_t a, uint64_t b, uint64_t& high, uint64_t& low) const; - void ScaleMantissa(uint64_t* const out, uint64_t scale) const; - - int m_sign; - int m_exponent; - uint64_t m_mantissa[VHACD_GOOGOL_SIZE]; - - public: - static Googol m_zero; - static Googol m_one; - static Googol m_two; - static Googol m_three; - static Googol m_half; - }; - - template - inline void Swap(T& A, T& B) - { - T tmp(A); - A = B; - B = tmp; - } - - template - void Sort(T* const array, int elements) - { - const int batchSize = 8; - int stack[1024][2]; - - stack[0][0] = 0; - stack[0][1] = elements - 1; - int stackIndex = 1; - const dCompareKey comparator; - while (stackIndex) - { - stackIndex--; - int lo = stack[stackIndex][0]; - int hi = stack[stackIndex][1]; - if ((hi - lo) > batchSize) - { - int mid = (lo + hi) >> 1; - if (comparator.Compare(array[lo], array[mid]) > 0) - { - Swap(array[lo], array[mid]); - } - if (comparator.Compare(array[mid], array[hi]) > 0) - { - Swap(array[mid], array[hi]); - } - if (comparator.Compare(array[lo], array[mid]) > 0) - { - Swap(array[lo], array[mid]); - } - int i = lo + 1; - int j = hi - 1; - const T pivot(array[mid]); - do - { - while (comparator.Compare(array[i], pivot) < 0) - { - i++; - } - while (comparator.Compare(array[j], pivot) > 0) - { - j--; - } - - if (i <= j) - { - Swap(array[i], array[j]); - i++; - j--; - } - } while (i <= j); - - if (i < hi) - { - stack[stackIndex][0] = i; - stack[stackIndex][1] = hi; - stackIndex++; - } - if (lo < j) - { - stack[stackIndex][0] = lo; - stack[stackIndex][1] = j; - stackIndex++; - } - assert(stackIndex < int(sizeof(stack) / (2 * sizeof(stack[0][0])))); - } - } - - int stride = batchSize + 1; - if (elements < stride) - { - stride = elements; - } - for (int i = 1; i < stride; ++i) - { - if (comparator.Compare(array[0], array[i]) > 0) - { - Swap(array[0], array[i]); - } - } - - for (int i = 1; i < elements; ++i) - { - int j = i; - const T tmp(array[i]); - for (; comparator.Compare(array[j - 1], tmp) > 0; --j) - { - assert(j > 0); - array[j] = array[j - 1]; - } - array[j] = tmp; - } - } - } -} - -namespace nd -{ - namespace VHACD - { - #define VHACD_ABS(a) ((a) >= 0.0 ? (a) : -(a)) - - Googol Googol::m_zero(0.0); - Googol Googol::m_one(1.0); - Googol Googol::m_two(2.0); - Googol Googol::m_three(3.0); - Googol Googol::m_half(0.5); - - Googol::Googol(void) - :m_sign(0) - ,m_exponent(0) - { - memset(m_mantissa, 0, sizeof(m_mantissa)); - } - - Googol::Googol(double value) - :m_sign(0) - , m_exponent(0) - { - int exp; - double mantissa = fabs(frexp(value, &exp)); - - m_exponent = int(exp); - m_sign = (value >= 0) ? 0 : 1; - - memset(m_mantissa, 0, sizeof(m_mantissa)); - m_mantissa[0] = uint64_t(double(uint64_t(1) << 62) * mantissa); - } - - void Googol::CopySignedMantissa(uint64_t* const mantissa) const - { - memcpy(mantissa, m_mantissa, sizeof(m_mantissa)); - if (m_sign) - { - NegateMantissa(mantissa); - } - } - - Googol::operator double() const - { - double mantissa = (double(1.0f) / double(uint64_t(1) << 62)) * double(m_mantissa[0]); - mantissa = ldexp(mantissa, m_exponent) * (m_sign ? double(-1.0f) : double(1.0f)); - return mantissa; - } - - Googol Googol::operator+ (const Googol &A) const - { - Googol tmp; - if (m_mantissa[0] && A.m_mantissa[0]) - { - uint64_t mantissa0[VHACD_GOOGOL_SIZE]; - uint64_t mantissa1[VHACD_GOOGOL_SIZE]; - uint64_t mantissa[VHACD_GOOGOL_SIZE]; - - CopySignedMantissa(mantissa0); - A.CopySignedMantissa(mantissa1); - - int exponetDiff = m_exponent - A.m_exponent; - int exponent = m_exponent; - if (exponetDiff > 0) - { - ShiftRightMantissa(mantissa1, exponetDiff); - } - else if (exponetDiff < 0) - { - exponent = A.m_exponent; - ShiftRightMantissa(mantissa0, -exponetDiff); - } - - uint64_t carrier = 0; - for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) - { - uint64_t m0 = mantissa0[i]; - uint64_t m1 = mantissa1[i]; - mantissa[i] = m0 + m1 + carrier; - carrier = CheckCarrier(m0, m1) | CheckCarrier(m0 + m1, carrier); - } - - int sign = 0; - if (int64_t(mantissa[0]) < 0) - { - sign = 1; - NegateMantissa(mantissa); - } - - int bits = NormalizeMantissa(mantissa); - if (bits <= (-64 * VHACD_GOOGOL_SIZE)) - { - tmp.m_sign = 0; - tmp.m_exponent = 0; - } - else - { - tmp.m_sign = sign; - tmp.m_exponent = int(exponent + bits); - } - - memcpy(tmp.m_mantissa, mantissa, sizeof(m_mantissa)); - } - else if (A.m_mantissa[0]) - { - tmp = A; - } - else - { - tmp = *this; - } - - return tmp; - } - - Googol Googol::operator- (const Googol &A) const - { - Googol tmp(A); - tmp.m_sign = !tmp.m_sign; - return *this + tmp; - } - - void Googol::ScaleMantissa(uint64_t* const dst, uint64_t scale) const - { - uint64_t carrier = 0; - for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) - { - if (m_mantissa[i]) - { - uint64_t low; - uint64_t high; - ExtendeMultiply(scale, m_mantissa[i], high, low); - uint64_t acc = low + carrier; - carrier = CheckCarrier(low, carrier); - carrier += high; - dst[i + 1] = acc; - } - else - { - dst[i + 1] = carrier; - carrier = 0; - } - - } - dst[0] = carrier; - } - - Googol Googol::operator* (const Googol &A) const - { - if (m_mantissa[0] && A.m_mantissa[0]) - { - uint64_t mantissaAcc[VHACD_GOOGOL_SIZE * 2]; - memset(mantissaAcc, 0, sizeof(mantissaAcc)); - for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) - { - uint64_t a = m_mantissa[i]; - if (a) - { - uint64_t mantissaScale[2 * VHACD_GOOGOL_SIZE]; - memset(mantissaScale, 0, sizeof(mantissaScale)); - A.ScaleMantissa(&mantissaScale[i], a); - - uint64_t carrier = 0; - for (int j = 0; j < 2 * VHACD_GOOGOL_SIZE; j++) - { - const int k = 2 * VHACD_GOOGOL_SIZE - 1 - j; - uint64_t m0 = mantissaAcc[k]; - uint64_t m1 = mantissaScale[k]; - mantissaAcc[k] = m0 + m1 + carrier; - carrier = CheckCarrier(m0, m1) | CheckCarrier(m0 + m1, carrier); - } - } - } - - uint64_t carrier = 0; - //int bits = uint64_t(LeadingZeros (mantissaAcc[0]) - 2); - int bits = LeadingZeros(mantissaAcc[0]) - 2; - for (int i = 0; i < 2 * VHACD_GOOGOL_SIZE; i++) - { - const int k = 2 * VHACD_GOOGOL_SIZE - 1 - i; - uint64_t a = mantissaAcc[k]; - mantissaAcc[k] = (a << uint64_t(bits)) | carrier; - carrier = a >> uint64_t(64 - bits); - } - - int exp = m_exponent + A.m_exponent - (bits - 2); - - Googol tmp; - tmp.m_sign = m_sign ^ A.m_sign; - tmp.m_exponent = int(exp); - memcpy(tmp.m_mantissa, mantissaAcc, sizeof(m_mantissa)); - - return tmp; - } - return Googol(0.0); - } - - Googol Googol::operator/ (const Googol &A) const - { - Googol tmp(1.0 / A); - tmp = tmp * (m_two - A * tmp); - tmp = tmp * (m_two - A * tmp); - int test = 0; - int passes = 0; - do - { - passes++; - Googol tmp0(tmp); - tmp = tmp * (m_two - A * tmp); - test = memcmp(&tmp0, &tmp, sizeof(Googol)); - } while (test && (passes < (2 * VHACD_GOOGOL_SIZE))); - return (*this) * tmp; - } - - Googol Googol::Abs() const - { - Googol tmp(*this); - tmp.m_sign = 0; - return tmp; - } - - Googol Googol::Floor() const - { - if (m_exponent < 1) - { - return Googol(0.0); - } - int bits = m_exponent + 2; - int start = 0; - while (bits >= 64) - { - bits -= 64; - start++; - } - - Googol tmp(*this); - for (int i = VHACD_GOOGOL_SIZE - 1; i > start; i--) - { - tmp.m_mantissa[i] = 0; - } - // some compilers do no like this and I do not know why is that - //uint64_t mask = (-1LL) << (64 - bits); - uint64_t mask(~0ULL); - mask <<= (64 - bits); - tmp.m_mantissa[start] &= mask; - return tmp; - } - - Googol Googol::InvSqrt() const - { - const Googol& me = *this; - Googol x(1.0f / sqrt(me)); - - int test = 0; - int passes = 0; - do - { - passes++; - Googol tmp(x); - x = m_half * x * (m_three - me * x * x); - test = memcmp(&x, &tmp, sizeof(Googol)); - } while (test && (passes < (2 * VHACD_GOOGOL_SIZE))); - return x; - } - - Googol Googol::Sqrt() const - { - return *this * InvSqrt(); - } - - void Googol::ToString(char* const string) const - { - Googol tmp(*this); - Googol base(10.0); - while (double(tmp) > 1.0) - { - tmp = tmp / base; - } - - int index = 0; - while (tmp.m_mantissa[0]) - { - tmp = tmp * base; - Googol digit(tmp.Floor()); - tmp -= digit; - double val = digit; - string[index] = char(val) + '0'; - index++; - } - string[index] = 0; - } - - void Googol::NegateMantissa(uint64_t* const mantissa) const - { - uint64_t carrier = 1; - for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) - { - uint64_t a = ~mantissa[i] + carrier; - if (a) - { - carrier = 0; - } - mantissa[i] = a; - } - } - - void Googol::ShiftRightMantissa(uint64_t* const mantissa, int bits) const - { - uint64_t carrier = 0; - if (int64_t(mantissa[0]) < int64_t(0)) - { - carrier = uint64_t(-1); - } - - while (bits >= 64) - { - for (int i = VHACD_GOOGOL_SIZE - 2; i >= 0; i--) - { - mantissa[i + 1] = mantissa[i]; - } - mantissa[0] = carrier; - bits -= 64; - } - - if (bits > 0) - { - carrier <<= (64 - bits); - for (int i = 0; i < VHACD_GOOGOL_SIZE; i++) - { - uint64_t a = mantissa[i]; - mantissa[i] = (a >> bits) | carrier; - carrier = a << (64 - bits); - } - } - } - - int Googol::LeadingZeros(uint64_t a) const - { - #define VHACD_COUNTBIT(mask,add) \ - { \ - uint64_t test = a & mask; \ - n += test ? 0 : add; \ - a = test ? test : (a & ~mask); \ - } - - int n = 0; - VHACD_COUNTBIT(0xffffffff00000000LL, 32); - VHACD_COUNTBIT(0xffff0000ffff0000LL, 16); - VHACD_COUNTBIT(0xff00ff00ff00ff00LL, 8); - VHACD_COUNTBIT(0xf0f0f0f0f0f0f0f0LL, 4); - VHACD_COUNTBIT(0xccccccccccccccccLL, 2); - VHACD_COUNTBIT(0xaaaaaaaaaaaaaaaaLL, 1); - - return n; - } - - int Googol::NormalizeMantissa(uint64_t* const mantissa) const - { - int bits = 0; - if (int64_t(mantissa[0] * 2) < 0) - { - bits = 1; - ShiftRightMantissa(mantissa, 1); - } - else - { - while (!mantissa[0] && bits > (-64 * VHACD_GOOGOL_SIZE)) - { - bits -= 64; - for (int i = 1; i < VHACD_GOOGOL_SIZE; i++) { - mantissa[i - 1] = mantissa[i]; - } - mantissa[VHACD_GOOGOL_SIZE - 1] = 0; - } - - if (bits > (-64 * VHACD_GOOGOL_SIZE)) - { - int n = LeadingZeros(mantissa[0]) - 2; - if (n > 0) - { - uint64_t carrier = 0; - for (int i = VHACD_GOOGOL_SIZE - 1; i >= 0; i--) - { - uint64_t a = mantissa[i]; - mantissa[i] = (a << n) | carrier; - carrier = a >> (64 - n); - } - bits -= n; - } - else if (n < 0) - { - // this is very rare but it does happens, whee the leading zeros of the mantissa is an exact multiple of 64 - uint64_t carrier = 0; - int shift = -n; - for (int i = 0; i < VHACD_GOOGOL_SIZE; i++) - { - uint64_t a = mantissa[i]; - mantissa[i] = (a >> shift) | carrier; - carrier = a << (64 - shift); - } - bits -= n; - } - } - } - return bits; - } - - uint64_t Googol::CheckCarrier(uint64_t a, uint64_t b) const - { - return ((uint64_t(-1) - b) < a) ? uint64_t(1) : 0; - } - - void Googol::ExtendeMultiply(uint64_t a, uint64_t b, uint64_t& high, uint64_t& low) const - { - uint64_t bLow = b & 0xffffffff; - uint64_t bHigh = b >> 32; - uint64_t aLow = a & 0xffffffff; - uint64_t aHigh = a >> 32; - - uint64_t l = bLow * aLow; - - uint64_t c1 = bHigh * aLow; - uint64_t c2 = bLow * aHigh; - uint64_t m = c1 + c2; - uint64_t carrier = CheckCarrier(c1, c2) << 32; - - uint64_t h = bHigh * aHigh + carrier; - - uint64_t ml = m << 32; - uint64_t ll = l + ml; - uint64_t mh = (m >> 32) + CheckCarrier(l, ml); - uint64_t hh = h + mh; - - low = ll; - high = hh; - } - - Googol Googol::operator+= (const Googol &A) - { - *this = *this + A; - return *this; - } - - Googol Googol::operator-= (const Googol &A) - { - *this = *this - A; - return *this; - } - - bool Googol::operator> (const Googol &A) const - { - Googol tmp(*this - A); - return double(tmp) > 0.0; - } - - bool Googol::operator>= (const Googol &A) const - { - Googol tmp(*this - A); - return double(tmp) >= 0.0; - } - - bool Googol::operator< (const Googol &A) const - { - Googol tmp(*this - A); - return double(tmp) < 0.0; - } - - bool Googol::operator<= (const Googol &A) const - { - Googol tmp(*this - A); - return double(tmp) <= 0.0; - } - - bool Googol::operator== (const Googol &A) const - { - Googol tmp(*this - A); - return double(tmp) == 0.0; - } - - bool Googol::operator!= (const Googol &A) const - { - Googol tmp(*this - A); - return double(tmp) != 0.0; - } - - void Googol::Trace() const - { - //dTrace (("%f ", double (*this))); - } - - double Determinant2x2(const double matrix[2][2], double* const error) - { - double a00xa11 = matrix[0][0] * matrix[1][1]; - double a01xa10 = matrix[0][1] * matrix[1][0]; - *error = VHACD_ABS(a00xa11) + VHACD_ABS(a01xa10); - return a00xa11 - a01xa10; - } - - double Determinant3x3(const double matrix[3][3], double* const error) - { - double sign = double(-1.0f); - double det = double(0.0f); - double accError = double(0.0f); - for (int i = 0; i < 3; i++) - { - double cofactor[2][2]; - for (int j = 0; j < 2; j++) - { - int k0 = 0; - for (int k = 0; k < 3; k++) - { - if (k != i) - { - cofactor[j][k0] = matrix[j][k]; - k0++; - } - } - } - - double parcialError; - double minorDet = Determinant2x2(cofactor, &parcialError); - accError += parcialError * VHACD_ABS(matrix[2][i]); - det += sign * minorDet * matrix[2][i]; - sign *= double(-1.0f); - } - - *error = accError; - return det; - } - - Googol Determinant2x2(const Googol matrix[2][2]) - { - Googol a00xa11(matrix[0][0] * matrix[1][1]); - Googol a01xa10(matrix[0][1] * matrix[1][0]); - return a00xa11 - a01xa10; - } - - Googol Determinant3x3(const Googol matrix[3][3]) - { - Googol negOne(double(-1.0f)); - Googol sign(double(-1.0f)); - Googol det = double(0.0f); - for (int i = 0; i < 3; i++) - { - Googol cofactor[2][2]; - for (int j = 0; j < 2; j++) - { - int k0 = 0; - for (int k = 0; k < 3; k++) - { - if (k != i) - { - cofactor[j][k0] = matrix[j][k]; - k0++; - } - } - } - - Googol minorDet(Determinant2x2(cofactor)); - det = det + sign * minorDet * matrix[2][i]; - sign = sign * negOne; - } - return det; - } - } -} - - -namespace nd -{ -namespace VHACD -{ - class ConvexHullVertex; - class ConvexHullAABBTreeNode; - - class ConvexHullFace + points.resize(baseCount); + if (baseCount < 4) { - public: - ConvexHullFace(); - double Evalue(const VHACD::Vect3* const pointArray, - const VHACD::Vect3& point) const; - hullPlane GetPlaneEquation(const VHACD::Vect3* const pointArray, - bool& isValid) const; + return nullptr; + } + VHACD::Vect3 sum(0); + VHACD::Vect3 sum2(0); + VHACD::Vect3 minP(double( 1.0e15)); + VHACD::Vect3 maxP(double(-1.0e15)); + class dTreeBox + { public: - int m_index[3]; + VHACD::Vect3 m_min; + VHACD::Vect3 m_max; + VHACD::Vect3 m_sum; + VHACD::Vect3 m_sum2; + ConvexHullAABBTreeNode* m_parent; + ConvexHullAABBTreeNode** m_child; + int m_start; + int m_count; + }; - private: - int m_mark; - List::ndNode* m_twin[3]; + for (int i = 0; i < baseCount; ++i) + { + const VHACD::Vect3& p = points[i]; + sum += p; + sum2 += p.CWiseMul(p); + minP = minP.CWiseMin(p); + maxP = maxP.CWiseMax(p); + } - friend class ConvexHull; - }; + dTreeBox treeBoxStack[128]; + treeBoxStack[0].m_start = 0; + treeBoxStack[0].m_count = baseCount; + treeBoxStack[0].m_sum = sum; + treeBoxStack[0].m_sum2 = sum2; + treeBoxStack[0].m_min = minP; + treeBoxStack[0].m_max = maxP; + treeBoxStack[0].m_child = nullptr; + treeBoxStack[0].m_parent = nullptr; - class ConvexHull : public List + int stack = 1; + ConvexHullAABBTreeNode* root = nullptr; + while (stack) { - class ndNormalMap; + stack--; + dTreeBox box(treeBoxStack[stack]); + if (box.m_count <= VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE) + { + assert(memoryPool.size() != memoryPool.capacity() + && "memoryPool is going to be reallocated, pointers will be invalid"); + memoryPool.emplace_back(); + ConvexHullAABBTreeNode& clump = memoryPool.back(); - public: - ConvexHull(const ConvexHull& source); - ConvexHull(const std::vector<::VHACD::Vertex>& vertexCloud, - double distTol, - int maxVertexCount = 0x7fffffff); - ~ConvexHull() = default; + clump.m_count = box.m_count; + for (int i = 0; i < box.m_count; ++i) + { + clump.m_indices[i] = i + box.m_start; + } + clump.m_box[0] = box.m_min; + clump.m_box[1] = box.m_max; - const std::vector>& GetVertexPool() const; + if (box.m_child) + { + *box.m_child = &clump; + } - private: - void BuildHull(const std::vector<::VHACD::Vertex>& vertexCloud, - double distTol, - int maxVertexCount); - - void GetUniquePoints(std::vector& points); - int InitVertexArray(std::vector& points, - void* const memoryPool, - int maxMemSize); - - ConvexHullAABBTreeNode* BuildTreeNew(std::vector& points, - char** const memoryPool, - int& maxMemSize) const; - ConvexHullAABBTreeNode* BuildTreeOld(std::vector& points, - char** const memoryPool, - int& maxMemSize); - ConvexHullAABBTreeNode* BuildTreeRecurse(ConvexHullAABBTreeNode* const parent, - ConvexHullVertex* const points, - int count, - int baseIndex, - char** const memoryPool, - int& maxMemSize) const; - - ndNode* AddFace(int i0, int i1, int i2); - - void CalculateConvexHull3d(ConvexHullAABBTreeNode* vertexTree, - std::vector& points, - int count, - double distTol, - int maxVertexCount); - - int SupportVertex(ConvexHullAABBTreeNode** const tree, - const std::vector& points, - const VHACD::Vect3& dir, - const bool removeEntry = true) const; - double TetrahedrumVolume(const VHACD::Vect3& p0, - const VHACD::Vect3& p1, - const VHACD::Vect3& p2, - const VHACD::Vect3& p3) const; - - VHACD::Vect3 m_aabbP0; - VHACD::Vect3 m_aabbP1; - double m_diag; - std::vector> m_points; - }; -} -} + if (!root) + { + root = &clump; + } + } + else + { + const VHACD::Vect3 origin(box.m_sum * (double(1.0) / box.m_count)); + const VHACD::Vect3 variance2(box.m_sum2 * (double(1.0) / box.m_count) - origin.CWiseMul(origin)); -namespace nd -{ - namespace VHACD - { - - #define VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE 8 - - ConvexHullFace::ConvexHullFace() - { - m_mark = 0; - m_twin[0] = nullptr; - m_twin[1] = nullptr; - m_twin[2] = nullptr; - } - - hullPlane ConvexHullFace::GetPlaneEquation(const VHACD::Vect3* const pointArray, - bool& isvalid) const - { - const VHACD::Vect3& p0 = pointArray[m_index[0]]; - const VHACD::Vect3& p1 = pointArray[m_index[1]]; - const VHACD::Vect3& p2 = pointArray[m_index[2]]; - hullPlane plane(p0, p1, p2); - - isvalid = false; - double mag2 = plane.Dot(plane); - if (mag2 > 1.0e-16f) - { - isvalid = true; - plane = plane.Scale(1.0f / sqrt(mag2)); - } - return plane; - } - - double ConvexHullFace::Evalue(const VHACD::Vect3* const pointArray, - const VHACD::Vect3& point) const - { - const VHACD::Vect3& p0 = pointArray[m_index[0]]; - const VHACD::Vect3& p1 = pointArray[m_index[1]]; - const VHACD::Vect3& p2 = pointArray[m_index[2]]; - - double matrix[3][3]; - for (int i = 0; i < 3; ++i) - { - matrix[0][i] = p2[i] - p0[i]; - matrix[1][i] = p1[i] - p0[i]; - matrix[2][i] = point[i] - p0[i]; - } - - double error; - double det = Determinant3x3(matrix, &error); - - // the code use double, however the threshold for accuracy test is the machine precision of a float. - // by changing this to a smaller number, the code should run faster since many small test will be considered valid - // the precision must be a power of two no smaller than the machine precision of a double, (1<<48) - // float64(1<<30) can be a good value - - // double precision = double (1.0f) / double (1<<30); - double precision = double(1.0f) / double(1 << 24); - double errbound = error * precision; - if (fabs(det) > errbound) - { - return det; - } - - Googol exactMatrix[3][3]; - for (int i = 0; i < 3; ++i) - { - exactMatrix[0][i] = Googol(p2[i]) - Googol(p0[i]); - exactMatrix[1][i] = Googol(p1[i]) - Googol(p0[i]); - exactMatrix[2][i] = Googol(point[i]) - Googol(p0[i]); - } - return Determinant3x3(exactMatrix); - } - - class ConvexHullVertex : public nd::VHACD::Vect3 - { - public: - int m_mark; - }; - - class ConvexHullAABBTreeNode - { - public: - //ConvexHullAABBTreeNode(ConvexHullAABBTreeNode* const parent) - ConvexHullAABBTreeNode() - :m_left(nullptr) - ,m_right(nullptr) - ,m_parent(nullptr) - { - } - - ConvexHullAABBTreeNode(ConvexHullAABBTreeNode* const parent) - :m_left(nullptr) - ,m_right(nullptr) - ,m_parent(parent) - { - } - - nd::VHACD::Vect3 m_box[2]; - ConvexHullAABBTreeNode* m_left; - ConvexHullAABBTreeNode* m_right; - ConvexHullAABBTreeNode* m_parent; - }; - - class ConvexHull3dPointCluster : public ConvexHullAABBTreeNode - { - public: - ConvexHull3dPointCluster() - :ConvexHullAABBTreeNode() - { - } - - ConvexHull3dPointCluster(ConvexHullAABBTreeNode* const parent) - :ConvexHullAABBTreeNode(parent) - { - } - - int m_count; - int m_indices[VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE]; - }; - - class ConvexHull::ndNormalMap - { - public: - ndNormalMap() - : m_count(sizeof(m_normal) / sizeof(m_normal[0])) - { - nd::VHACD::Vect3 p0( 1.0, 0.0, 0.0); - nd::VHACD::Vect3 p1(-1.0, 0.0, 0.0); - nd::VHACD::Vect3 p2( 0.0, 1.0, 0.0); - nd::VHACD::Vect3 p3( 0.0, -1.0, 0.0); - nd::VHACD::Vect3 p4( 0.0, 0.0, 1.0); - nd::VHACD::Vect3 p5( 0.0, 0.0, -1.0); - - int count = 0; - int subdivitions = 2; - TessellateTriangle(subdivitions, p4, p0, p2, count); - TessellateTriangle(subdivitions, p0, p5, p2, count); - TessellateTriangle(subdivitions, p5, p1, p2, count); - TessellateTriangle(subdivitions, p1, p4, p2, count); - TessellateTriangle(subdivitions, p0, p4, p3, count); - TessellateTriangle(subdivitions, p5, p0, p3, count); - TessellateTriangle(subdivitions, p1, p5, p3, count); - TessellateTriangle(subdivitions, p4, p1, p3, count); - } - - static const ndNormalMap& GetNormaMap() - { - static ndNormalMap normalMap; - return normalMap; - } - - void TessellateTriangle(int level, - const nd::VHACD::Vect3& p0, - const nd::VHACD::Vect3& p1, - const nd::VHACD::Vect3& p2, - int& count) - { - if (level) - { - assert(fabs(p0.Dot(p0) - double(1.0f)) < double(1.0e-4f)); - assert(fabs(p1.Dot(p1) - double(1.0f)) < double(1.0e-4f)); - assert(fabs(p2.Dot(p2) - double(1.0f)) < double(1.0e-4f)); - nd::VHACD::Vect3 p01(p0 + p1); - nd::VHACD::Vect3 p12(p1 + p2); - nd::VHACD::Vect3 p20(p2 + p0); - - p01 = p01 * (1.0 / p01.GetNorm()); - p12 = p12 * (1.0 / p12.GetNorm()); - p20 = p20 * (1.0 / p20.GetNorm()); - - assert(fabs(p01.GetNormSquared() - double(1.0f)) < double(1.0e-4f)); - assert(fabs(p12.GetNormSquared() - double(1.0f)) < double(1.0e-4f)); - assert(fabs(p20.GetNormSquared() - double(1.0f)) < double(1.0e-4f)); - - TessellateTriangle(level - 1, p0, p01, p20, count); - TessellateTriangle(level - 1, p1, p12, p01, count); - TessellateTriangle(level - 1, p2, p20, p12, count); - TessellateTriangle(level - 1, p01, p12, p20, count); - } - else - { - hullPlane n(p0, p1, p2); - n = n.Scale(double(1.0f) / sqrt(n.Dot(n))); - n.m_w = double(0.0f); - int index = dBitReversal(count, sizeof(m_normal) / sizeof(m_normal[0])); - m_normal[index] = n; - count++; - assert(count <= int(sizeof(m_normal) / sizeof(m_normal[0]))); - } - } - - nd::VHACD::Vect3 m_normal[128]; - int m_count; - }; - - ConvexHull::ConvexHull(const std::vector<::VHACD::Vertex>& vertexCloud, - double distTol, - int maxVertexCount) - : List() - , m_aabbP0(0) - , m_aabbP1(0) - , m_diag() - , m_points() - { - m_points.resize(0); - if (vertexCloud.size() >= 4) - { - BuildHull(vertexCloud, - distTol, - maxVertexCount); - } - } - - const std::vector>& ConvexHull::GetVertexPool() const - { - return m_points; - } - - void ConvexHull::BuildHull(const std::vector<::VHACD::Vertex>& vertexCloud, - double distTol, - int maxVertexCount) - { - size_t treeCount = vertexCloud.size() / (VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE >> 1); - if (treeCount < 4) - { - treeCount = 4; - } - treeCount *= 2; - - std::vector points(vertexCloud.size()); - std::vector treePool(treeCount + 256); - - for (int i = 0; i < vertexCloud.size(); ++i) - { - nd::VHACD::Vect3& vertex = points[i]; - vertex = nd::VHACD::Vect3(vertexCloud[i]); - points[i].m_mark = 0; - } - int count = InitVertexArray(points, - treePool.data(), - sizeof(ConvexHull3dPointCluster) * int(treePool.size())); - - if (m_points.size() >= 4) - { - CalculateConvexHull3d(treePool.data(), - points, - count, - distTol, - maxVertexCount); - } - } - - void ConvexHull::GetUniquePoints(std::vector& points) - { - class CompareVertex - { - public: - int Compare(const ConvexHullVertex& elementA, const ConvexHullVertex& elementB) const - { - for (int i = 0; i < 3; i++) - { - if (elementA[i] < elementB[i]) - { - return -1; - } - else if (elementA[i] > elementB[i]) - { - return 1; - } - } - return 0; - } - }; - - int count = int(points.size()); - Sort(&points[0], count); - - int indexCount = 0; - CompareVertex compareVetex; - for (int i = 1; i < count; ++i) - { - for (; i < count; ++i) - { - if (compareVetex.Compare(points[indexCount], points[i])) - { - indexCount++; - points[indexCount] = points[i]; - break; - } - } - } - points.resize(indexCount + 1); - } - - ConvexHullAABBTreeNode* ConvexHull::BuildTreeRecurse(ConvexHullAABBTreeNode* const parent, - ConvexHullVertex* const points, - int count, - int baseIndex, - char** memoryPool, - int& maxMemSize) const - { - ConvexHullAABBTreeNode* tree = nullptr; - - assert(count); - nd::VHACD::Vect3 minP(double(1.0e15f)); - nd::VHACD::Vect3 maxP(-double(1.0e15f)); - if (count <= VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE) - { - ConvexHull3dPointCluster* const clump = new (*memoryPool) ConvexHull3dPointCluster(); - *memoryPool += sizeof(ConvexHull3dPointCluster); - maxMemSize -= sizeof(ConvexHull3dPointCluster); - assert(maxMemSize >= 0); - - assert(clump); - clump->m_count = count; - for (int i = 0; i < count; ++i) - { - clump->m_indices[i] = i + baseIndex; - - const nd::VHACD::Vect3& p = points[i]; - minP = minP.CWiseMin(p); - maxP = maxP.CWiseMax(p); - } - - clump->m_left = nullptr; - clump->m_right = nullptr; - tree = clump; - } - else - { - nd::VHACD::Vect3 median(0); - nd::VHACD::Vect3 varian(0); - for (int i = 0; i < count; ++i) - { - const nd::VHACD::Vect3& p = points[i]; - minP = minP.CWiseMin(p); - maxP = maxP.CWiseMax(p); - median += p; - varian += p.CWiseMul(p); - } - - varian = varian * double(count) - median.CWiseMul(median); - int index = 0; - double maxVarian = double(-1.0e10f); - for (int i = 0; i < 3; ++i) - { - if (varian[i] > maxVarian) - { - index = i; - maxVarian = varian[i]; - } - } - nd::VHACD::Vect3 center(median * (1.0 / double(count))); - - double test = center[index]; - - int i0 = 0; - int i1 = count - 1; - do - { - for (; i0 <= i1; i0++) - { - double val = points[i0][index]; - if (val > test) - { - break; - } - } - - for (; i1 >= i0; i1--) - { - double val = points[i1][index]; - if (val < test) - { - break; - } - } - - if (i0 < i1) - { - Swap(points[i0], points[i1]); - i0++; - i1--; - } - } while (i0 <= i1); - - if (i0 == 0) - { - i0 = count / 2; - } - if (i0 >= (count - 1)) - { - i0 = count / 2; - } - - tree = new (*memoryPool) ConvexHullAABBTreeNode(); - *memoryPool += sizeof(ConvexHullAABBTreeNode); - maxMemSize -= sizeof(ConvexHullAABBTreeNode); - assert(maxMemSize >= 0); - - assert(i0); - assert(count - i0); - - tree->m_left = BuildTreeRecurse(tree, points, i0, baseIndex, memoryPool, maxMemSize); - tree->m_right = BuildTreeRecurse(tree, &points[i0], count - i0, i0 + baseIndex, memoryPool, maxMemSize); - } - - assert(tree); - tree->m_parent = parent; - tree->m_box[0] = minP - nd::VHACD::Vect3(double(1.0e-3f)); - tree->m_box[1] = maxP + nd::VHACD::Vect3(double(1.0e-3f)); - return tree; - } - - ConvexHullAABBTreeNode* ConvexHull::BuildTreeOld(std::vector& points, - char** const memoryPool, - int& maxMemSize) - { - GetUniquePoints(points); - int count = int(points.size()); - if (count < 4) - { - return nullptr; - } - return BuildTreeRecurse(nullptr, &points[0], count, 0, memoryPool, maxMemSize); - } - - ConvexHullAABBTreeNode* ConvexHull::BuildTreeNew(std::vector& points, - char** const memoryPool, - int& maxMemSize) const - { - class dCluster - { - public: - nd::VHACD::Vect3 m_sum; - nd::VHACD::Vect3 m_sum2; - int m_start; - int m_count; - }; - - dCluster firstCluster; - firstCluster.m_start = 0; - firstCluster.m_count = int (points.size()); - firstCluster.m_sum = nd::VHACD::Vect3(0); - firstCluster.m_sum2 = nd::VHACD::Vect3(0); - - for (int i = 0; i < firstCluster.m_count; ++i) - { - const nd::VHACD::Vect3& p = points[i]; - firstCluster.m_sum += p; - firstCluster.m_sum2 += p.CWiseMul(p); - } - - int baseCount = 0; - const int clusterSize = 16; - - if (firstCluster.m_count > clusterSize) - { - dCluster spliteStack[128]; - spliteStack[0] = firstCluster; - int stack = 1; - - while (stack) - { - stack--; - dCluster cluster (spliteStack[stack]); - - const nd::VHACD::Vect3 origin(cluster.m_sum * (1.0 / cluster.m_count)); - const nd::VHACD::Vect3 variance2(cluster.m_sum2 * (1.0 / cluster.m_count) - origin.CWiseMul(origin)); - double maxVariance2 = Max(Max(variance2.getX(), variance2.getY()), variance2.getZ()); - - if ( (cluster.m_count <= clusterSize) - || (stack > (sizeof(spliteStack) / sizeof(spliteStack[0]) - 4)) - || (maxVariance2 < 1.e-4f)) - { - // no sure if this is beneficial, - // the array is so small that seem too much overhead - //int maxIndex = 0; - //double min_x = 1.0e20f; - //for (int i = 0; i < cluster.m_count; ++i) - //{ - // if (points[cluster.m_start + i].getX() < min_x) - // { - // maxIndex = i; - // min_x = points[cluster.m_start + i].getX(); - // } - //} - //Swap(points[cluster.m_start], points[cluster.m_start + maxIndex]); - // - //for (int i = 2; i < cluster.m_count; ++i) - //{ - // int j = i; - // ConvexHullVertex tmp(points[cluster.m_start + i]); - // for (; points[cluster.m_start + j - 1].getX() > tmp.getX(); --j) - // { - // assert(j > 0); - // points[cluster.m_start + j] = points[cluster.m_start + j - 1]; - // } - // points[cluster.m_start + j] = tmp; - //} - - int count = cluster.m_count; - for (int i = cluster.m_count - 1; i > 0; --i) - { - for (int j = i - 1; j >= 0; --j) - { - nd::VHACD::Vect3 error(points[cluster.m_start + j] - points[cluster.m_start + i]); - double mag2 = error.Dot(error); - if (mag2 < 1.0e-6) - { - points[cluster.m_start + j] = points[cluster.m_start + i]; - count--; - break; - } - } - } - - assert(baseCount <= cluster.m_start); - for (int i = 0; i < count; ++i) - { - points[baseCount] = points[cluster.m_start + i]; - baseCount++; - } - } - else - { - int firstSortAxis = 0; - if ((variance2.getY() >= variance2.getX()) && (variance2.getY() >= variance2.getZ())) - { - firstSortAxis = 1; - } - else if ((variance2.getZ() >= variance2.getX()) && (variance2.getZ() >= variance2.getY())) - { - firstSortAxis = 2; - } - double axisVal = origin[firstSortAxis]; - - int i0 = 0; - int i1 = cluster.m_count - 1; - - const int start = cluster.m_start; - while (i0 < i1) - { - while ((points[start + i0][firstSortAxis] <= axisVal) && (i0 < i1)) - { - ++i0; - }; - - while ((points[start + i1][firstSortAxis] > axisVal) && (i0 < i1)) - { - --i1; - } - - assert(i0 <= i1); - if (i0 < i1) - { - Swap(points[start + i0], points[start + i1]); - ++i0; - --i1; - } - } - - while ((points[start + i0][firstSortAxis] <= axisVal) && (i0 < cluster.m_count)) - { - ++i0; - }; - - #ifdef _DEBUG - for (int i = 0; i < i0; ++i) - { - assert(points[start + i][firstSortAxis] <= axisVal); - } - - for (int i = i0; i < cluster.m_count; ++i) - { - assert(points[start + i][firstSortAxis] > axisVal); - } - #endif - - nd::VHACD::Vect3 xc(0); - nd::VHACD::Vect3 x2c(0); - for (int i = 0; i < i0; ++i) - { - const nd::VHACD::Vect3& x = points[start + i]; - xc += x; - x2c += x.CWiseMul(x); - } - - dCluster cluster_i1(cluster); - cluster_i1.m_start = start + i0; - cluster_i1.m_count = cluster.m_count - i0; - cluster_i1.m_sum -= xc; - cluster_i1.m_sum2 -= x2c; - spliteStack[stack] = cluster_i1; - assert(cluster_i1.m_count > 0); - stack++; - - dCluster cluster_i0(cluster); - cluster_i0.m_start = start; - cluster_i0.m_count = i0; - cluster_i0.m_sum = xc; - cluster_i0.m_sum2 = x2c; - assert(cluster_i0.m_count > 0); - spliteStack[stack] = cluster_i0; - stack++; - } - } - } - - points.resize(baseCount); - if (baseCount < 4) - { - return nullptr; - } - - nd::VHACD::Vect3 sum(0); - nd::VHACD::Vect3 sum2(0); - nd::VHACD::Vect3 minP(1.0e15); - nd::VHACD::Vect3 maxP(-1.0e15); - class dTreeBox - { - public: - nd::VHACD::Vect3 m_min; - nd::VHACD::Vect3 m_max; - nd::VHACD::Vect3 m_sum; - nd::VHACD::Vect3 m_sum2; - ConvexHullAABBTreeNode* m_parent; - ConvexHullAABBTreeNode** m_child; - int m_start; - int m_count; - }; - - for (int i = 0; i < baseCount; ++i) - { - const nd::VHACD::Vect3& p = points[i]; - sum += p; - sum2 += p.CWiseMul(p); - minP = minP.CWiseMin(p); - maxP = maxP.CWiseMax(p); - } - - dTreeBox treeBoxStack[128]; - treeBoxStack[0].m_start = 0; - treeBoxStack[0].m_count = baseCount; - treeBoxStack[0].m_sum = sum; - treeBoxStack[0].m_sum2 = sum2; - treeBoxStack[0].m_min = minP; - treeBoxStack[0].m_max = maxP; - treeBoxStack[0].m_child = nullptr; - treeBoxStack[0].m_parent = nullptr; - - int stack = 1; - ConvexHullAABBTreeNode* root = nullptr; - while (stack) - { - stack--; - dTreeBox box (treeBoxStack[stack]); - if (box.m_count <= VHACD_CONVEXHULL_3D_VERTEX_CLUSTER_SIZE) - { - ConvexHull3dPointCluster* const clump = new (*memoryPool) ConvexHull3dPointCluster(box.m_parent); - *memoryPool += sizeof(ConvexHull3dPointCluster); - maxMemSize -= sizeof(ConvexHull3dPointCluster); - assert(maxMemSize >= 0); - - assert(clump); - clump->m_count = box.m_count; - for (int i = 0; i < box.m_count; ++i) - { - clump->m_indices[i] = i + box.m_start; - } - clump->m_box[0] = box.m_min; - clump->m_box[1] = box.m_max; - - if (box.m_child) - { - *box.m_child = clump; - } - - if (!root) - { - root = clump; - } - } - else - { - const nd::VHACD::Vect3 origin(box.m_sum * (1.0 / box.m_count)); - const nd::VHACD::Vect3 variance2(box.m_sum2 * (1.0 / box.m_count) - origin.CWiseMul(origin)); - - int firstSortAxis = 0; - if ((variance2.getY() >= variance2.getX()) && (variance2.getY() >= variance2.getZ())) - { - firstSortAxis = 1; - } - else if ((variance2.getZ() >= variance2.getX()) && (variance2.getZ() >= variance2.getY())) - { - firstSortAxis = 2; - } - double axisVal = origin[firstSortAxis]; - - int i0 = 0; - int i1 = box.m_count - 1; - - const int start = box.m_start; - while (i0 < i1) - { - while ((points[start + i0][firstSortAxis] <= axisVal) && (i0 < i1)) - { - ++i0; - }; - - while ((points[start + i1][firstSortAxis] > axisVal) && (i0 < i1)) - { - --i1; - } - - assert(i0 <= i1); - if (i0 < i1) - { - Swap(points[start + i0], points[start + i1]); - ++i0; - --i1; - } - } - - while ((points[start + i0][firstSortAxis] <= axisVal) && (i0 < box.m_count)) - { - ++i0; - }; - - #ifdef _DEBUG - for (int i = 0; i < i0; ++i) - { - assert(points[start + i][firstSortAxis] <= axisVal); - } - - for (int i = i0; i < box.m_count; ++i) - { - assert(points[start + i][firstSortAxis] > axisVal); - } - #endif - - ConvexHullAABBTreeNode* const node = new (*memoryPool) ConvexHullAABBTreeNode(box.m_parent); - *memoryPool += sizeof(ConvexHullAABBTreeNode); - maxMemSize -= sizeof(ConvexHullAABBTreeNode); - assert(maxMemSize >= 0); - - node->m_box[0] = box.m_min; - node->m_box[1] = box.m_max; - if (box.m_child) - { - *box.m_child = node; - } - - if (!root) - { - root = node; - } - - { - nd::VHACD::Vect3 xc(0); - nd::VHACD::Vect3 x2c(0); - nd::VHACD::Vect3 p0(1.0e15); - nd::VHACD::Vect3 p1(-1.0e15); - for (int i = i0; i < box.m_count; ++i) - { - const nd::VHACD::Vect3& p = points[start + i]; - xc += p; - x2c += p.CWiseMul(p); - p0 = p0.CWiseMin(p); - p1 = p1.CWiseMax(p); - } - - dTreeBox cluster_i1(box); - cluster_i1.m_start = start + i0; - cluster_i1.m_count = box.m_count - i0; - cluster_i1.m_sum = xc; - cluster_i1.m_sum2 = x2c; - cluster_i1.m_min = p0; - cluster_i1.m_max = p1; - cluster_i1.m_parent = node; - cluster_i1.m_child = &node->m_right; - treeBoxStack[stack] = cluster_i1; - assert(cluster_i1.m_count > 0); - stack++; - } - - { - nd::VHACD::Vect3 xc(0); - nd::VHACD::Vect3 x2c(0); - nd::VHACD::Vect3 p0(1.0e15); - nd::VHACD::Vect3 p1(-1.0e15); - for (int i = 0; i < i0; ++i) - { - const nd::VHACD::Vect3& p = points[start + i]; - xc += p; - x2c += p.CWiseMul(p); - p0 = p0.CWiseMin(p); - p1 = p1.CWiseMax(p); - } - - dTreeBox cluster_i0(box); - cluster_i0.m_start = start; - cluster_i0.m_count = i0; - cluster_i0.m_min = p0; - cluster_i0.m_max = p1; - cluster_i0.m_sum = xc; - cluster_i0.m_sum2 = x2c; - cluster_i0.m_parent = node; - cluster_i0.m_child = &node->m_left; - assert(cluster_i0.m_count > 0); - treeBoxStack[stack] = cluster_i0; - stack++; - } - } - } - - return root; - } - - int ConvexHull::SupportVertex(ConvexHullAABBTreeNode** const treePointer, - const std::vector& points, - const nd::VHACD::Vect3& dirPlane, - const bool removeEntry) const - { - #define VHACD_STACK_DEPTH_3D 64 - double aabbProjection[VHACD_STACK_DEPTH_3D]; - const ConvexHullAABBTreeNode *stackPool[VHACD_STACK_DEPTH_3D]; - - nd::VHACD::Vect3 dir(dirPlane); - - int index = -1; - int stack = 1; - stackPool[0] = *treePointer; - aabbProjection[0] = double(1.0e20f); - double maxProj = double(-1.0e20f); - int ix = (dir[0] > double(0.0f)) ? 1 : 0; - int iy = (dir[1] > double(0.0f)) ? 1 : 0; - int iz = (dir[2] > double(0.0f)) ? 1 : 0; - while (stack) - { - stack--; - double boxSupportValue = aabbProjection[stack]; - if (boxSupportValue > maxProj) - { - const ConvexHullAABBTreeNode* const me = stackPool[stack]; - - if (me->m_left && me->m_right) - { - const nd::VHACD::Vect3 leftSupportPoint(me->m_left->m_box[ix].getX(), - me->m_left->m_box[iy].getY(), - me->m_left->m_box[iz].getZ()); - double leftSupportDist = leftSupportPoint.Dot(dir); - - const nd::VHACD::Vect3 rightSupportPoint(me->m_right->m_box[ix].getX(), - me->m_right->m_box[iy].getY(), - me->m_right->m_box[iz].getZ()); - double rightSupportDist = rightSupportPoint.Dot(dir); - - if (rightSupportDist >= leftSupportDist) - { - aabbProjection[stack] = leftSupportDist; - stackPool[stack] = me->m_left; - stack++; - assert(stack < VHACD_STACK_DEPTH_3D); - aabbProjection[stack] = rightSupportDist; - stackPool[stack] = me->m_right; - stack++; - assert(stack < VHACD_STACK_DEPTH_3D); - } - else - { - aabbProjection[stack] = rightSupportDist; - stackPool[stack] = me->m_right; - stack++; - assert(stack < VHACD_STACK_DEPTH_3D); - aabbProjection[stack] = leftSupportDist; - stackPool[stack] = me->m_left; - stack++; - assert(stack < VHACD_STACK_DEPTH_3D); - } - } - else - { - ConvexHull3dPointCluster* const cluster = (ConvexHull3dPointCluster*)me; - for (int i = 0; i < cluster->m_count; ++i) - { - const ConvexHullVertex& p = points[cluster->m_indices[i]]; - assert(p.getX() >= cluster->m_box[0].getX()); - assert(p.getX() <= cluster->m_box[1].getX()); - assert(p.getY() >= cluster->m_box[0].getY()); - assert(p.getY() <= cluster->m_box[1].getY()); - assert(p.getZ() >= cluster->m_box[0].getZ()); - assert(p.getZ() <= cluster->m_box[1].getZ()); - if (!p.m_mark) - { - //assert(p.m_w == double(0.0f)); - double dist = p.Dot(dir); - if (dist > maxProj) - { - maxProj = dist; - index = cluster->m_indices[i]; - } - } - else if (removeEntry) - { - cluster->m_indices[i] = cluster->m_indices[cluster->m_count - 1]; - cluster->m_count = cluster->m_count - 1; - i--; - } - } - - if (cluster->m_count == 0) - { - ConvexHullAABBTreeNode* const parent = cluster->m_parent; - if (parent) - { - ConvexHullAABBTreeNode* const sibling = (parent->m_left != cluster) ? parent->m_left : parent->m_right; - assert(sibling != cluster); - ConvexHullAABBTreeNode* const grandParent = parent->m_parent; - if (grandParent) - { - sibling->m_parent = grandParent; - if (grandParent->m_right == parent) - { - grandParent->m_right = sibling; - } - else - { - grandParent->m_left = sibling; - } - } - else - { - sibling->m_parent = nullptr; - *treePointer = sibling; - } - } - } - } - } - } - - assert(index != -1); - return index; - } - - double ConvexHull::TetrahedrumVolume(const nd::VHACD::Vect3& p0, - const nd::VHACD::Vect3& p1, - const nd::VHACD::Vect3& p2, - const nd::VHACD::Vect3& p3) const - { - const nd::VHACD::Vect3 p1p0(p1 - p0); - const nd::VHACD::Vect3 p2p0(p2 - p0); - const nd::VHACD::Vect3 p3p0(p3 - p0); - return p3p0.Dot(p1p0.Cross(p2p0)); - } - - int ConvexHull::InitVertexArray(std::vector& points, - void* const memoryPool, - int maxMemSize) - { - #if 1 - ConvexHullAABBTreeNode* tree = BuildTreeOld(points, (char**)&memoryPool, maxMemSize); - #else - ConvexHullAABBTreeNode* tree = BuildTreeNew(points, (char**)&memoryPool, maxMemSize); - #endif - int count = int(points.size()); - if (count < 4) - { - m_points.resize(0); - return 0; - } - - m_points.resize(count); - m_aabbP0 = tree->m_box[0]; - m_aabbP1 = tree->m_box[1]; - - nd::VHACD::Vect3 boxSize(tree->m_box[1] - tree->m_box[0]); - m_diag = boxSize.GetNorm(); - const ndNormalMap& normalMap = ndNormalMap::GetNormaMap(); - - int index0 = SupportVertex(&tree, - points, - normalMap.m_normal[0]); - m_points[0] = points[index0]; - points[index0].m_mark = 1; - - bool validTetrahedrum = false; - nd::VHACD::Vect3 e1(0.0); - for (int i = 1; i < normalMap.m_count; ++i) - { - int index = SupportVertex(&tree, - points, - normalMap.m_normal[i]); - assert(index >= 0); - - e1 = points[index] - m_points[0]; - double error2 = e1.GetNormSquared(); - if (error2 > (double(1.0e-4f) * m_diag * m_diag)) - { - m_points[1] = points[index]; - points[index].m_mark = 1; - validTetrahedrum = true; - break; - } - } - if (!validTetrahedrum) - { - m_points.resize(0); - assert(0); - return count; - } - - validTetrahedrum = false; - nd::VHACD::Vect3 e2(0.0); - nd::VHACD::Vect3 normal(0.0); - for (int i = 2; i < normalMap.m_count; ++i) - { - int index = SupportVertex(&tree, - points, - normalMap.m_normal[i]); - assert(index >= 0); - e2 = points[index] - m_points[0]; - normal = e1.Cross(e2); - double error2 = normal.GetNorm(); - if (error2 > (double(1.0e-4f) * m_diag * m_diag)) - { - m_points[2] = points[index]; - points[index].m_mark = 1; - validTetrahedrum = true; - break; - } - } - - if (!validTetrahedrum) - { - m_points.resize(0); - assert(0); - return count; - } - - // find the largest possible tetrahedron - validTetrahedrum = false; - nd::VHACD::Vect3 e3(0.0); - - index0 = SupportVertex(&tree, points, normal); - e3 = points[index0] - m_points[0]; - double err2 = normal.Dot(e3); - if (fabs(err2) > (double(1.0e-6f) * m_diag * m_diag)) - { - // we found a valid tetrahedral, about and start build the hull by adding the rest of the points - m_points[3] = points[index0]; - points[index0].m_mark = 1; - validTetrahedrum = true; - } - if (!validTetrahedrum) - { - nd::VHACD::Vect3 n(-normal); - int index = SupportVertex(&tree, - points, - n); - e3 = points[index] - m_points[0]; - double error2 = normal.Dot(e3); - if (fabs(error2) > (double(1.0e-6f) * m_diag * m_diag)) - { - // we found a valid tetrahedral, about and start build the hull by adding the rest of the points - m_points[3] = points[index]; - points[index].m_mark = 1; - validTetrahedrum = true; - } - } - if (!validTetrahedrum) - { - for (int i = 3; i < normalMap.m_count; ++i) - { - int index = SupportVertex(&tree, - points, - normalMap.m_normal[i]); - assert(index >= 0); - - //make sure the volume of the fist tetrahedral is no negative - e3 = points[index] - m_points[0]; - double error2 = normal.Dot(e3); - if (fabs(error2) > (double(1.0e-6f) * m_diag * m_diag)) - { - // we found a valid tetrahedral, about and start build the hull by adding the rest of the points - m_points[3] = points[index]; - points[index].m_mark = 1; - validTetrahedrum = true; - break; - } - } - } - if (!validTetrahedrum) - { - // the points do not form a convex hull - m_points.resize(0); - return count; - } - - m_points.resize(4); - double volume = TetrahedrumVolume(m_points[0], m_points[1], m_points[2], m_points[3]); - if (volume > double(0.0f)) - { - Swap(m_points[2], m_points[3]); - } - assert(TetrahedrumVolume(m_points[0], m_points[1], m_points[2], m_points[3]) < double(0.0f)); - return count; - } - - ConvexHull::ndNode* ConvexHull::AddFace(int i0, int i1, int i2) - { - ndNode* const node = Append(); - ConvexHullFace& face = node->GetInfo(); - - face.m_index[0] = i0; - face.m_index[1] = i1; - face.m_index[2] = i2; - return node; - } - - void ConvexHull::CalculateConvexHull3d(ConvexHullAABBTreeNode* vertexTree, - std::vector& points, - int count, - double distTol, - int maxVertexCount) - { - distTol = fabs(distTol) * m_diag; - ndNode* const f0Node = AddFace(0, 1, 2); - ndNode* const f1Node = AddFace(0, 2, 3); - ndNode* const f2Node = AddFace(2, 1, 3); - ndNode* const f3Node = AddFace(1, 0, 3); - - ConvexHullFace* const f0 = &f0Node->GetInfo(); - ConvexHullFace* const f1 = &f1Node->GetInfo(); - ConvexHullFace* const f2 = &f2Node->GetInfo(); - ConvexHullFace* const f3 = &f3Node->GetInfo(); - - f0->m_twin[0] = f3Node; - f0->m_twin[1] = f2Node; - f0->m_twin[2] = f1Node; - - f1->m_twin[0] = f0Node; - f1->m_twin[1] = f2Node; - f1->m_twin[2] = f3Node; - - f2->m_twin[0] = f0Node; - f2->m_twin[1] = f3Node; - f2->m_twin[2] = f1Node; - - f3->m_twin[0] = f0Node; - f3->m_twin[1] = f1Node; - f3->m_twin[2] = f2Node; - - List boundaryFaces; - boundaryFaces.Append(f0Node); - boundaryFaces.Append(f1Node); - boundaryFaces.Append(f2Node); - boundaryFaces.Append(f3Node); - - m_points.resize(count); - - count -= 4; - maxVertexCount -= 4; - int currentIndex = 4; - - std::vector stackPool; - std::vector coneListPool; - std::vector deleteListPool; - - stackPool.resize(1024 + count); - coneListPool.resize(1024 + count); - deleteListPool.resize(1024 + count); - - ndNode** const stack = &stackPool[0]; - ndNode** const coneList = &stackPool[0]; - ndNode** const deleteList = &deleteListPool[0]; - - while (boundaryFaces.GetCount() && count && (maxVertexCount > 0)) - { - // my definition of the optimal convex hull of a given vertex count, - // is the convex hull formed by a subset of the input vertex that minimizes the volume difference - // between the perfect hull formed from all input vertex and the hull of the sub set of vertex. - // When using a priority heap this algorithms will generate the an optimal of a fix vertex count. - // Since all Newton's tools do not have a limit on the point count of a convex hull, I can use either a stack or a queue. - // a stack maximize construction speed, a Queue tend to maximize the volume of the generated Hull approaching a perfect Hull. - // For now we use a queue. - // For general hulls it does not make a difference if we use a stack, queue, or a priority heap. - // perfect optimal hull only apply for when build hull of a limited vertex count. - // - // Also when building Hulls of a limited vertex count, this function runs in constant time. - // yes that is correct, it does not makes a difference if you build a N point hull from 100 vertex - // or from 100000 vertex input array. - - // using a queue (some what slower by better hull when reduced vertex count is desired) - bool isvalid; - ndNode* const faceNode = boundaryFaces.GetLast()->GetInfo(); - ConvexHullFace* const face = &faceNode->GetInfo(); - hullPlane planeEquation(face->GetPlaneEquation(&m_points[0], isvalid)); - - int index = 0; - double dist = 0; - nd::VHACD::Vect3 p; - if (isvalid) - { - index = SupportVertex(&vertexTree, points, planeEquation); - p = points[index]; - dist = planeEquation.Evalue(p); - } - - if (isvalid && (dist >= distTol) && (face->Evalue(&m_points[0], p) > double(0.0f))) - { - stack[0] = faceNode; - - int stackIndex = 1; - int deletedCount = 0; - - while (stackIndex) - { - stackIndex--; - ndNode* const node1 = stack[stackIndex]; - ConvexHullFace* const face1 = &node1->GetInfo(); - - if (!face1->m_mark && (face1->Evalue(&m_points[0], p) > double(0.0f))) - { - #ifdef _DEBUG - for (int i = 0; i < deletedCount; ++i) - { - assert(deleteList[i] != node1); - } - #endif - - deleteList[deletedCount] = node1; - deletedCount++; - assert(deletedCount < int(deleteListPool.size())); - face1->m_mark = 1; - for (int i = 0; i < 3; ++i) - { - ndNode* const twinNode = face1->m_twin[i]; - assert(twinNode); - ConvexHullFace* const twinFace = &twinNode->GetInfo(); - if (!twinFace->m_mark) - { - stack[stackIndex] = twinNode; - stackIndex++; - assert(stackIndex < int(stackPool.size())); - } - } - } - } - - m_points[currentIndex] = points[index]; - points[index].m_mark = 1; - - int newCount = 0; - for (int i = 0; i < deletedCount; ++i) - { - ndNode* const node1 = deleteList[i]; - ConvexHullFace* const face1 = &node1->GetInfo(); - assert(face1->m_mark == 1); - for (int j0 = 0; j0 < 3; j0++) - { - ndNode* const twinNode = face1->m_twin[j0]; - ConvexHullFace* const twinFace = &twinNode->GetInfo(); - if (!twinFace->m_mark) - { - int j1 = (j0 == 2) ? 0 : j0 + 1; - ndNode* const newNode = AddFace(currentIndex, face1->m_index[j0], face1->m_index[j1]); - boundaryFaces.Addtop(newNode); - - ConvexHullFace* const newFace = &newNode->GetInfo(); - newFace->m_twin[1] = twinNode; - for (int k = 0; k < 3; k++) - { - if (twinFace->m_twin[k] == node1) - { - twinFace->m_twin[k] = newNode; - } - } - coneList[newCount] = newNode; - newCount++; - assert(newCount < int(coneListPool.size())); - } - } - } - - for (int i = 0; i < newCount - 1; ++i) - { - ndNode* const nodeA = coneList[i]; - ConvexHullFace* const faceA = &nodeA->GetInfo(); - assert(faceA->m_mark == 0); - for (int j = i + 1; j < newCount; j++) - { - ndNode* const nodeB = coneList[j]; - ConvexHullFace* const faceB = &nodeB->GetInfo(); - assert(faceB->m_mark == 0); - if (faceA->m_index[2] == faceB->m_index[1]) - { - faceA->m_twin[2] = nodeB; - faceB->m_twin[0] = nodeA; - break; - } - } - - for (int j = i + 1; j < newCount; j++) - { - ndNode* const nodeB = coneList[j]; - ConvexHullFace* const faceB = &nodeB->GetInfo(); - assert(faceB->m_mark == 0); - if (faceA->m_index[1] == faceB->m_index[2]) - { - faceA->m_twin[0] = nodeB; - faceB->m_twin[2] = nodeA; - break; - } - } - } - - for (int i = 0; i < deletedCount; ++i) - { - ndNode* const node = deleteList[i]; - boundaryFaces.Remove(node); - Remove(node); - } - - maxVertexCount--; - currentIndex++; - count--; - } - else - { - boundaryFaces.Remove(faceNode); - } - } - m_points.resize(currentIndex); - } - } -} + int firstSortAxis = 0; + if ((variance2.GetY() >= variance2.GetX()) && (variance2.GetY() >= variance2.GetZ())) + { + firstSortAxis = 1; + } + else if ((variance2.GetZ() >= variance2.GetX()) && (variance2.GetZ() >= variance2.GetY())) + { + firstSortAxis = 2; + } + double axisVal = origin[firstSortAxis]; -//*********************************************************************************************** -// End of ConvexHull generation code by Julio Jerez -//*********************************************************************************************** + int i0 = 0; + int i1 = box.m_count - 1; -// VertexIndex support -namespace VERTEX_INDEX -{ + const int start = box.m_start; + while (i0 < i1) + { + while ((points[start + i0][firstSortAxis] <= axisVal) && (i0 < i1)) + { + ++i0; + }; -class KdTreeNode; + while ((points[start + i1][firstSortAxis] > axisVal) && (i0 < i1)) + { + --i1; + } -typedef std::vector KdTreeNodeVector; + assert(i0 <= i1); + if (i0 < i1) + { + std::swap(points[start + i0], + points[start + i1]); + ++i0; + --i1; + } + } -enum Axes -{ - X_AXIS = 0, - Y_AXIS = 1, - Z_AXIS = 2 -}; + while ((points[start + i0][firstSortAxis] <= axisVal) && (i0 < box.m_count)) + { + ++i0; + }; -class KdTreeFindNode -{ -public: - KdTreeFindNode(void) - { - mNode = 0; - mDistance = 0; - } - KdTreeNode* mNode; - double mDistance; -}; + #ifdef _DEBUG + for (int i = 0; i < i0; ++i) + { + assert(points[start + i][firstSortAxis] <= axisVal); + } -class KdTreeNode; -class KdTreeNodeBundle; + for (int i = i0; i < box.m_count; ++i) + { + assert(points[start + i][firstSortAxis] > axisVal); + } + #endif -class KdTree -{ -public: - KdTree() = default; + assert(memoryPool.size() != memoryPool.capacity() + && "memoryPool is going to be reallocated, pointers will be invalid"); + memoryPool.emplace_back(); + ConvexHullAABBTreeNode& node = memoryPool.back(); - ~KdTree() { reset(); } + node.m_box[0] = box.m_min; + node.m_box[1] = box.m_max; + if (box.m_child) + { + *box.m_child = &node; + } - const VHACD::Vertex& getPosition(uint32_t index) const; + if (!root) + { + root = &node; + } - uint32_t search(const nd::VHACD::Vect3& pos, - double radius, - uint32_t maxObjects, - KdTreeFindNode* found) const; + { + VHACD::Vect3 xc(0); + VHACD::Vect3 x2c(0); + VHACD::Vect3 p0(double( 1.0e15)); + VHACD::Vect3 p1(double(-1.0e15)); + for (int i = i0; i < box.m_count; ++i) + { + const VHACD::Vect3& p = points[start + i]; + xc += p; + x2c += p.CWiseMul(p); + p0 = p0.CWiseMin(p); + p1 = p1.CWiseMax(p); + } - void reset(); + dTreeBox cluster_i1(box); + cluster_i1.m_start = start + i0; + cluster_i1.m_count = box.m_count - i0; + cluster_i1.m_sum = xc; + cluster_i1.m_sum2 = x2c; + cluster_i1.m_min = p0; + cluster_i1.m_max = p1; + cluster_i1.m_parent = &node; + cluster_i1.m_child = &node.m_right; + treeBoxStack[stack] = cluster_i1; + assert(cluster_i1.m_count > 0); + stack++; + } - uint32_t add(const VHACD::Vertex& v); + { + VHACD::Vect3 xc(0); + VHACD::Vect3 x2c(0); + VHACD::Vect3 p0(double( 1.0e15)); + VHACD::Vect3 p1(double(-1.0e15)); + for (int i = 0; i < i0; ++i) + { + const VHACD::Vect3& p = points[start + i]; + xc += p; + x2c += p.CWiseMul(p); + p0 = p0.CWiseMin(p); + p1 = p1.CWiseMax(p); + } - KdTreeNode* getNewNode(uint32_t index); + dTreeBox cluster_i0(box); + cluster_i0.m_start = start; + cluster_i0.m_count = i0; + cluster_i0.m_min = p0; + cluster_i0.m_max = p1; + cluster_i0.m_sum = xc; + cluster_i0.m_sum2 = x2c; + cluster_i0.m_parent = &node; + cluster_i0.m_child = &node.m_left; + assert(cluster_i0.m_count > 0); + treeBoxStack[stack] = cluster_i0; + stack++; + } + } + } - uint32_t getNearest(const nd::VHACD::Vect3& pos, - double radius, - bool& _found) const; // returns the nearest possible neighbor's index. + return root; +} - const std::vector& getVertices() const; - std::vector&& takeVertices(); +int ConvexHull::SupportVertex(ConvexHullAABBTreeNode** const treePointer, + const std::vector& points, + const VHACD::Vect3& dirPlane, + const bool removeEntry) const +{ +#define VHACD_STACK_DEPTH_3D 64 + double aabbProjection[VHACD_STACK_DEPTH_3D]; + ConvexHullAABBTreeNode* stackPool[VHACD_STACK_DEPTH_3D]; + + VHACD::Vect3 dir(dirPlane); + + int index = -1; + int stack = 1; + stackPool[0] = *treePointer; + aabbProjection[0] = double(1.0e20); + double maxProj = double(-1.0e20); + int ix = (dir[0] > double(0.0)) ? 1 : 0; + int iy = (dir[1] > double(0.0)) ? 1 : 0; + int iz = (dir[2] > double(0.0)) ? 1 : 0; + while (stack) + { + stack--; + double boxSupportValue = aabbProjection[stack]; + if (boxSupportValue > maxProj) + { + ConvexHullAABBTreeNode* me = stackPool[stack]; + + /* + * If the node is not a leaf node... + */ + if (me->m_left && me->m_right) + { + const VHACD::Vect3 leftSupportPoint(me->m_left->m_box[ix].GetX(), + me->m_left->m_box[iy].GetY(), + me->m_left->m_box[iz].GetZ()); + double leftSupportDist = leftSupportPoint.Dot(dir); + + const VHACD::Vect3 rightSupportPoint(me->m_right->m_box[ix].GetX(), + me->m_right->m_box[iy].GetY(), + me->m_right->m_box[iz].GetZ()); + double rightSupportDist = rightSupportPoint.Dot(dir); + + /* + * ...push the shorter side first + * So we can explore the tree in the larger side first + */ + if (rightSupportDist >= leftSupportDist) + { + aabbProjection[stack] = leftSupportDist; + stackPool[stack] = me->m_left; + stack++; + assert(stack < VHACD_STACK_DEPTH_3D); + aabbProjection[stack] = rightSupportDist; + stackPool[stack] = me->m_right; + stack++; + assert(stack < VHACD_STACK_DEPTH_3D); + } + else + { + aabbProjection[stack] = rightSupportDist; + stackPool[stack] = me->m_right; + stack++; + assert(stack < VHACD_STACK_DEPTH_3D); + aabbProjection[stack] = leftSupportDist; + stackPool[stack] = me->m_left; + stack++; + assert(stack < VHACD_STACK_DEPTH_3D); + } + } + /* + * If it is a node... + */ + else + { + ConvexHullAABBTreeNode* cluster = me; + for (size_t i = 0; i < cluster->m_count; ++i) + { + const ConvexHullVertex& p = points[cluster->m_indices[i]]; + assert(p.GetX() >= cluster->m_box[0].GetX()); + assert(p.GetX() <= cluster->m_box[1].GetX()); + assert(p.GetY() >= cluster->m_box[0].GetY()); + assert(p.GetY() <= cluster->m_box[1].GetY()); + assert(p.GetZ() >= cluster->m_box[0].GetZ()); + assert(p.GetZ() <= cluster->m_box[1].GetZ()); + if (!p.m_mark) + { + //assert(p.m_w == double(0.0f)); + double dist = p.Dot(dir); + if (dist > maxProj) + { + maxProj = dist; + index = cluster->m_indices[i]; + } + } + else if (removeEntry) + { + cluster->m_indices[i] = cluster->m_indices[cluster->m_count - 1]; + cluster->m_count = cluster->m_count - 1; + i--; + } + } - uint32_t getVcount(void) const; + if (cluster->m_count == 0) + { + ConvexHullAABBTreeNode* const parent = cluster->m_parent; + if (parent) + { + ConvexHullAABBTreeNode* const sibling = (parent->m_left != cluster) ? parent->m_left : parent->m_right; + assert(sibling != cluster); + ConvexHullAABBTreeNode* const grandParent = parent->m_parent; + if (grandParent) + { + sibling->m_parent = grandParent; + if (grandParent->m_right == parent) + { + grandParent->m_right = sibling; + } + else + { + grandParent->m_left = sibling; + } + } + else + { + sibling->m_parent = nullptr; + *treePointer = sibling; + } + } + } + } + } + } -private: - KdTreeNode* mRoot = nullptr; - KdTreeNodeBundle* mBundle = nullptr; - KdTreeNodeBundle* mBundleHead = nullptr; + assert(index != -1); + return index; +} - std::vector mVertices; -}; +double ConvexHull::TetrahedrumVolume(const VHACD::Vect3& p0, + const VHACD::Vect3& p1, + const VHACD::Vect3& p2, + const VHACD::Vect3& p3) const +{ + const VHACD::Vect3 p1p0(p1 - p0); + const VHACD::Vect3 p2p0(p2 - p0); + const VHACD::Vect3 p3p0(p3 - p0); + return p3p0.Dot(p1p0.Cross(p2p0)); +} -class KdTreeNode +int ConvexHull::InitVertexArray(std::vector& points, + NodeBundle& memoryPool) +// std::vector& memoryPool) { -public: - KdTreeNode() = default; +#if 1 + ConvexHullAABBTreeNode* tree = BuildTreeOld(points, + memoryPool); +#else + ConvexHullAABBTreeNode* tree = BuildTreeNew(points, (char**)&memoryPool, maxMemSize); +#endif + int count = int(points.size()); + if (count < 4) + { + m_points.resize(0); + return 0; + } - KdTreeNode(uint32_t index) : mIndex(index) {} + m_points.resize(count); + m_aabbP0 = tree->m_box[0]; + m_aabbP1 = tree->m_box[1]; - ~KdTreeNode() = default; + VHACD::Vect3 boxSize(tree->m_box[1] - tree->m_box[0]); + m_diag = boxSize.GetNorm(); + const ndNormalMap& normalMap = ndNormalMap::GetNormalMap(); - void add(KdTreeNode* node, - Axes dim, - const KdTree* iface) + int index0 = SupportVertex(&tree, + points, + normalMap.m_normal[0]); + m_points[0] = points[index0]; + points[index0].m_mark = 1; + + bool validTetrahedrum = false; + VHACD::Vect3 e1(double(0.0)); + for (int i = 1; i < normalMap.m_count; ++i) { - Axes axis = X_AXIS; - uint32_t idx = 0; - switch (dim) + int index = SupportVertex(&tree, + points, + normalMap.m_normal[i]); + assert(index >= 0); + + e1 = points[index] - m_points[0]; + double error2 = e1.GetNormSquared(); + if (error2 > (double(1.0e-4) * m_diag * m_diag)) { - case X_AXIS: - idx = 0; - axis = Y_AXIS; - break; - case Y_AXIS: - idx = 1; - axis = Z_AXIS; - break; - case Z_AXIS: - idx = 2; - axis = X_AXIS; + m_points[1] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; break; } + } + if (!validTetrahedrum) + { + m_points.resize(0); + assert(0); + return count; + } - const VHACD::Vertex& nodePosition = iface->getPosition(node->mIndex); - const VHACD::Vertex& position = iface->getPosition(mIndex); - if (nodePosition[idx] <= position[idx]) + validTetrahedrum = false; + VHACD::Vect3 e2(double(0.0)); + VHACD::Vect3 normal(double(0.0)); + for (int i = 2; i < normalMap.m_count; ++i) + { + int index = SupportVertex(&tree, + points, + normalMap.m_normal[i]); + assert(index >= 0); + e2 = points[index] - m_points[0]; + normal = e1.Cross(e2); + double error2 = normal.GetNorm(); + if (error2 > (double(1.0e-4) * m_diag * m_diag)) { - if (mLeft) - mLeft->add(node, axis, iface); - else - mLeft = node; + m_points[2] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; + break; } - else + } + + if (!validTetrahedrum) + { + m_points.resize(0); + assert(0); + return count; + } + + // find the largest possible tetrahedron + validTetrahedrum = false; + VHACD::Vect3 e3(double(0.0)); + + index0 = SupportVertex(&tree, + points, + normal); + e3 = points[index0] - m_points[0]; + double err2 = normal.Dot(e3); + if (fabs(err2) > (double(1.0e-6) * m_diag * m_diag)) + { + // we found a valid tetrahedral, about and start build the hull by adding the rest of the points + m_points[3] = points[index0]; + points[index0].m_mark = 1; + validTetrahedrum = true; + } + if (!validTetrahedrum) + { + VHACD::Vect3 n(-normal); + int index = SupportVertex(&tree, + points, + n); + e3 = points[index] - m_points[0]; + double error2 = normal.Dot(e3); + if (fabs(error2) > (double(1.0e-6) * m_diag * m_diag)) { - if (mRight) - mRight->add(node, axis, iface); - else - mRight = node; + // we found a valid tetrahedral, about and start build the hull by adding the rest of the points + m_points[3] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; } } + if (!validTetrahedrum) + { + for (int i = 3; i < normalMap.m_count; ++i) + { + int index = SupportVertex(&tree, + points, + normalMap.m_normal[i]); + assert(index >= 0); - uint32_t getIndex() const + //make sure the volume of the fist tetrahedral is no negative + e3 = points[index] - m_points[0]; + double error2 = normal.Dot(e3); + if (fabs(error2) > (double(1.0e-6) * m_diag * m_diag)) + { + // we found a valid tetrahedral, about and start build the hull by adding the rest of the points + m_points[3] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; + break; + } + } + } + if (!validTetrahedrum) { - return mIndex; - }; + // the points do not form a convex hull + m_points.resize(0); + return count; + } - void search(Axes axis, - const nd::VHACD::Vect3& pos, - double radius, - uint32_t& count, - uint32_t maxObjects, - KdTreeFindNode* found, - const KdTree* iface) + m_points.resize(4); + double volume = TetrahedrumVolume(m_points[0], + m_points[1], + m_points[2], + m_points[3]); + if (volume > double(0.0)) { - const nd::VHACD::Vect3 position = iface->getPosition(mIndex); + std::swap(m_points[2], + m_points[3]); + } + assert(TetrahedrumVolume(m_points[0], m_points[1], m_points[2], m_points[3]) < double(0.0)); + return count; +} - const nd::VHACD::Vect3 d = pos - position; +std::list::iterator ConvexHull::AddFace(int i0, + int i1, + int i2) +{ + ConvexHullFace face; + face.m_index[0] = i0; + face.m_index[1] = i1; + face.m_index[2] = i2; - KdTreeNode* search1 = 0; - KdTreeNode* search2 = 0; + std::list::iterator node = m_list.emplace(m_list.end(), face); + return node; +} - uint32_t idx = 0; - switch (axis) - { - case X_AXIS: - idx = 0; - axis = Y_AXIS; - break; - case Y_AXIS: - idx = 1; - axis = Z_AXIS; - break; - case Z_AXIS: - idx = 2; - axis = X_AXIS; - break; - } +void ConvexHull::CalculateConvexHull3D(ConvexHullAABBTreeNode* vertexTree, + std::vector& points, + int count, + double distTol, + int maxVertexCount) +{ + distTol = fabs(distTol) * m_diag; + std::list::iterator f0Node = AddFace(0, 1, 2); + std::list::iterator f1Node = AddFace(0, 2, 3); + std::list::iterator f2Node = AddFace(2, 1, 3); + std::list::iterator f3Node = AddFace(1, 0, 3); - if (d[idx] <= 0) // JWR if we are to the left - { - search1 = mLeft; // JWR then search to the left - if (-d[idx] < radius) // JWR if distance to the right is less than our search radius, continue on the right - // as well. - search2 = mRight; - } - else - { - search1 = mRight; // JWR ok, we go down the left tree - if (d[idx] < radius) // JWR if the distance from the right is less than our search radius - search2 = mLeft; - } + ConvexHullFace& f0 = *f0Node; + ConvexHullFace& f1 = *f1Node; + ConvexHullFace& f2 = *f2Node; + ConvexHullFace& f3 = *f3Node; + + f0.m_twin[0] = f3Node; + f0.m_twin[1] = f2Node; + f0.m_twin[2] = f1Node; + + f1.m_twin[0] = f0Node; + f1.m_twin[1] = f2Node; + f1.m_twin[2] = f3Node; + + f2.m_twin[0] = f0Node; + f2.m_twin[1] = f3Node; + f2.m_twin[2] = f1Node; + + f3.m_twin[0] = f0Node; + f3.m_twin[1] = f1Node; + f3.m_twin[2] = f2Node; + + std::list::iterator> boundaryFaces; + boundaryFaces.push_back(f0Node); + boundaryFaces.push_back(f1Node); + boundaryFaces.push_back(f2Node); + boundaryFaces.push_back(f3Node); + + m_points.resize(count); + + count -= 4; + maxVertexCount -= 4; + int currentIndex = 4; + + /* + * Some are iterators into boundaryFaces, others into m_list + */ + std::vector::iterator> stack; + std::vector::iterator> coneList; + std::vector::iterator> deleteList; + + stack.reserve(1024 + count); + coneList.reserve(1024 + count); + deleteList.reserve(1024 + count); + + while (boundaryFaces.size() && count && (maxVertexCount > 0)) + { + // my definition of the optimal convex hull of a given vertex count, + // is the convex hull formed by a subset of the input vertex that minimizes the volume difference + // between the perfect hull formed from all input vertex and the hull of the sub set of vertex. + // When using a priority heap this algorithms will generate the an optimal of a fix vertex count. + // Since all Newton's tools do not have a limit on the point count of a convex hull, I can use either a stack or a queue. + // a stack maximize construction speed, a Queue tend to maximize the volume of the generated Hull approaching a perfect Hull. + // For now we use a queue. + // For general hulls it does not make a difference if we use a stack, queue, or a priority heap. + // perfect optimal hull only apply for when build hull of a limited vertex count. + // + // Also when building Hulls of a limited vertex count, this function runs in constant time. + // yes that is correct, it does not makes a difference if you build a N point hull from 100 vertex + // or from 100000 vertex input array. + + // using a queue (some what slower by better hull when reduced vertex count is desired) + bool isvalid; + std::list::iterator faceNode = boundaryFaces.back(); + ConvexHullFace& face = *faceNode; + HullPlane planeEquation(face.GetPlaneEquation(m_points, isvalid)); + + int index = 0; + double dist = 0; + VHACD::Vect3 p; + if (isvalid) + { + index = SupportVertex(&vertexTree, + points, + planeEquation); + p = points[index]; + dist = planeEquation.Evalue(p); + } + + if ( isvalid + && (dist >= distTol) + && (face.Evalue(m_points, p) < double(0.0))) + { + stack.push_back(faceNode); + + deleteList.clear(); + while (stack.size()) + { + std::list::iterator node1 = stack.back(); + ConvexHullFace& face1 = *node1; + + stack.pop_back(); + + if (!face1.m_mark && (face1.Evalue(m_points, p) < double(0.0))) + { + #ifdef _DEBUG + for (const auto* node : deleteList) + { + assert(node != node1); + } + #endif + + deleteList.push_back(node1); + face1.m_mark = 1; + for (std::list::iterator& twinNode : face1.m_twin) + { + ConvexHullFace& twinFace = *twinNode; + if (!twinFace.m_mark) + { + stack.push_back(twinNode); + } + } + } + } - double r2 = radius * radius; - double m = d.GetNormSquared(); + m_points[currentIndex] = points[index]; + points[index].m_mark = 1; - if (m < r2) - { - switch (count) + coneList.clear(); + for (std::list::iterator node1 : deleteList) { - case 0: - found[count].mNode = this; - found[count].mDistance = m; - break; - case 1: - if (m < found[0].mDistance) + ConvexHullFace& face1 = *node1; + assert(face1.m_mark == 1); + for (std::size_t j0 = 0; j0 < face1.m_twin.size(); ++j0) { - if (maxObjects == 1) - { - found[0].mNode = this; - found[0].mDistance = m; - } - else + std::list::iterator twinNode = face1.m_twin[j0]; + ConvexHullFace& twinFace = *twinNode; + if (!twinFace.m_mark) { - found[1] = found[0]; - found[0].mNode = this; - found[0].mDistance = m; + std::size_t j1 = (j0 == 2) ? 0 : j0 + 1; + std::list::iterator newNode = AddFace(currentIndex, + face1.m_index[j0], + face1.m_index[j1]); + boundaryFaces.push_front(newNode); + ConvexHullFace& newFace = *newNode; + + newFace.m_twin[1] = twinNode; + for (std::size_t k = 0; k < twinFace.m_twin.size(); ++k) + { + if (twinFace.m_twin[k] == node1) + { + twinFace.m_twin[k] = newNode; + } + } + coneList.push_back(newNode); } } - else if (maxObjects > 1) + } + + for (std::size_t i = 0; i < coneList.size() - 1; ++i) + { + std::list::iterator nodeA = coneList[i]; + ConvexHullFace& faceA = *nodeA; + assert(faceA.m_mark == 0); + for (std::size_t j = i + 1; j < coneList.size(); j++) { - found[1].mNode = this; - found[1].mDistance = m; + std::list::iterator nodeB = coneList[j]; + ConvexHullFace& faceB = *nodeB; + assert(faceB.m_mark == 0); + if (faceA.m_index[2] == faceB.m_index[1]) + { + faceA.m_twin[2] = nodeB; + faceB.m_twin[0] = nodeA; + break; + } } - break; - default: - { - bool inserted = false; - for (uint32_t i = 0; i < count; i++) + for (std::size_t j = i + 1; j < coneList.size(); j++) { - if (m < found[i].mDistance) // if this one is closer than a pre-existing one... + std::list::iterator nodeB = coneList[j]; + ConvexHullFace& faceB = *nodeB; + assert(faceB.m_mark == 0); + if (faceA.m_index[1] == faceB.m_index[2]) { - // insertion sort... - uint32_t scan = count; - if (scan >= maxObjects) - scan = maxObjects - 1; - for (uint32_t j = scan; j > i; j--) - { - found[j] = found[j - 1]; - } - found[i].mNode = this; - found[i].mDistance = m; - inserted = true; + faceA.m_twin[0] = nodeB; + faceB.m_twin[2] = nodeA; break; } } + } - if (!inserted && count < maxObjects) + for (std::list::iterator node : deleteList) + { + auto it = std::find(boundaryFaces.begin(), + boundaryFaces.end(), + node); + if (it != boundaryFaces.end()) { - found[count].mNode = this; - found[count].mDistance = m; + boundaryFaces.erase(it); } + m_list.erase(node); } - break; - } - count++; - if (count > maxObjects) + + maxVertexCount--; + currentIndex++; + count--; + } + else + { + auto it = std::find(boundaryFaces.begin(), + boundaryFaces.end(), + faceNode); + if (it != boundaryFaces.end()) { - count = maxObjects; + boundaryFaces.erase(it); } } + } + m_points.resize(currentIndex); +} +//*********************************************************************************************** +// End of ConvexHull generation code by Julio Jerez +//*********************************************************************************************** - if (search1) - search1->search(axis, pos, radius, count, maxObjects, found, iface); - - if (search2) - search2->search(axis, pos, radius, count, maxObjects, found, iface); - } +class KdTreeNode; -private: - void setLeft(KdTreeNode* left) - { - mLeft = left; - }; - void setRight(KdTreeNode* right) - { - mRight = right; - }; +enum Axes +{ + X_AXIS = 0, + Y_AXIS = 1, + Z_AXIS = 2 +}; - KdTreeNode* getLeft(void) - { - return mLeft; - } - KdTreeNode* getRight(void) - { - return mRight; - } +class KdTreeFindNode +{ +public: + KdTreeFindNode() = default; - uint32_t mIndex = 0; - KdTreeNode* mLeft = nullptr; - KdTreeNode* mRight = nullptr; + KdTreeNode* m_node{ nullptr }; + double m_distance{ 0.0 }; }; +class KdTree +{ +public: + KdTree() = default; + + const VHACD::Vertex& GetPosition(uint32_t index) const; + + uint32_t Search(const VHACD::Vect3& pos, + double radius, + uint32_t maxObjects, + KdTreeFindNode* found) const; + + uint32_t Add(const VHACD::Vertex& v); -# define MAX_BUNDLE_SIZE \ - 1024 // 1024 nodes at a time, to minimize memory allocation and guarantee that pointers are persistent. + KdTreeNode& GetNewNode(uint32_t index); -class KdTreeNodeBundle + uint32_t GetNearest(const VHACD::Vect3& pos, + double radius, + bool& _found) const; // returns the nearest possible neighbor's index. + + const std::vector& GetVertices() const; + std::vector&& TakeVertices(); + + uint32_t GetVCount() const; + +private: + KdTreeNode* m_root{ nullptr }; + NodeBundle m_bundle; + + std::vector m_vertices; +}; + +class KdTreeNode { public: - KdTreeNodeBundle() = default; + KdTreeNode() = default; + KdTreeNode(uint32_t index); - bool isFull() const - { - return mIndex == MAX_BUNDLE_SIZE; - } + void Add(KdTreeNode& node, + Axes dim, + const KdTree& iface); - KdTreeNode* getNextNode() - { - assert(mIndex < MAX_BUNDLE_SIZE); - KdTreeNode* ret = &mNodes[mIndex]; - mIndex++; - return ret; - } + uint32_t GetIndex() const; + + void Search(Axes axis, + const VHACD::Vect3& pos, + double radius, + uint32_t& count, + uint32_t maxObjects, + KdTreeFindNode* found, + const KdTree& iface); - KdTreeNodeBundle* mNext = nullptr; - uint32_t mIndex = 0; - KdTreeNode mNodes[MAX_BUNDLE_SIZE]; +private: + uint32_t m_index = 0; + KdTreeNode* m_left = nullptr; + KdTreeNode* m_right = nullptr; }; -const VHACD::Vertex& KdTree::getPosition(uint32_t index) const +const VHACD::Vertex& KdTree::GetPosition(uint32_t index) const { - assert(index < mVertices.size()); - return mVertices[index]; + assert(index < m_vertices.size()); + return m_vertices[index]; } -uint32_t KdTree::search(const nd::VHACD::Vect3& pos, +uint32_t KdTree::Search(const VHACD::Vect3& pos, double radius, uint32_t maxObjects, KdTreeFindNode* found) const { - if (!mRoot) + if (!m_root) return 0; uint32_t count = 0; - mRoot->search(X_AXIS, pos, radius, count, maxObjects, found, this); + m_root->Search(X_AXIS, pos, radius, count, maxObjects, found, *this); return count; } -void KdTree::reset() -{ - mRoot = nullptr; - mVertices.clear(); - KdTreeNodeBundle* bundle = mBundleHead; - while (bundle) - { - KdTreeNodeBundle* next = bundle->mNext; - delete bundle; - bundle = next; - } - mBundle = nullptr; - mBundleHead = nullptr; -} - -uint32_t KdTree::add(const VHACD::Vertex& v) +uint32_t KdTree::Add(const VHACD::Vertex& v) { - uint32_t ret = uint32_t(mVertices.size()); - mVertices.emplace_back(v); - KdTreeNode* node = getNewNode(ret); - if (mRoot) + uint32_t ret = uint32_t(m_vertices.size()); + m_vertices.emplace_back(v); + KdTreeNode& node = GetNewNode(ret); + if (m_root) { - mRoot->add(node, - X_AXIS, - this); + m_root->Add(node, + X_AXIS, + *this); } else { - mRoot = node; + m_root = &node; } return ret; } -KdTreeNode* KdTree::getNewNode(uint32_t index) +KdTreeNode& KdTree::GetNewNode(uint32_t index) { - if (mBundle == 0) - { - mBundle = new KdTreeNodeBundle; - mBundleHead = mBundle; - } - if (mBundle->isFull()) - { - KdTreeNodeBundle* bundle = new KdTreeNodeBundle; - mBundle->mNext = bundle; - mBundle = bundle; - } - KdTreeNode* node = mBundle->getNextNode(); - new (node) KdTreeNode(index); + KdTreeNode& node = m_bundle.GetNextNode(); + node = KdTreeNode(index); return node; } -uint32_t KdTree::getNearest(const nd::VHACD::Vect3& pos, +uint32_t KdTree::GetNearest(const VHACD::Vect3& pos, double radius, bool& _found) const // returns the nearest possible neighbor's index. { uint32_t ret = 0; _found = false; - KdTreeFindNode found[1]; - uint32_t count = search(pos, radius, 1, found); + KdTreeFindNode found; + uint32_t count = Search(pos, radius, 1, &found); if (count) { - KdTreeNode* node = found[0].mNode; - ret = node->getIndex(); + KdTreeNode* node = found.m_node; + ret = node->GetIndex(); _found = true; } return ret; } -const std::vector& KdTree::getVertices() const +const std::vector& KdTree::GetVertices() const { - return mVertices; + return m_vertices; } -std::vector&& KdTree::takeVertices() +std::vector&& KdTree::TakeVertices() { - return std::move(mVertices); + return std::move(m_vertices); } -uint32_t KdTree::getVcount() const +uint32_t KdTree::GetVCount() const { - return uint32_t(mVertices.size()); + return uint32_t(m_vertices.size()); } -}; // end of namespace VERTEX_INDEX - -//******************************************************************************************************************** -// Prototypes for the handful of float math routines we use -//******************************************************************************************************************** -namespace VHACD -{ - -// Compute centroid of a triangle mesh; takes area of each triangle into account weighted average -// bool fm_computeCentroid(uint32_t vcount, // number of input data points -// const VHACD::Vertex* points, // starting address of points array. -// uint32_t triangleCount, -// const VHACD::Triangle* indices, -// double* center); - -// double fm_computeMeshVolume(const VHACD::Vertex* vertices, -// uint32_t tcount, -// const VHACD::Triangle* indices); - -// void fm_inflateMinMax(double bmin[3], -// double bmax[3], -// double ratio); -// void fm_getAABB(uint32_t vcount, -// const double* points, -// uint32_t pstride, -// double bmin[3], -// double bmax[3]); -// bool fm_intersectAABB(const double bmin1[3], -// const double bmax1[3], -// const double bmin2[3], -// const double bmax2[3]); -// void fm_combineAABB(const double bmin1[3], -// const double bmax1[3], -// const double bmin2[3], -// const double bmax2[3], -// double bmin[3], -// double bmax[3]); -// double fm_volumeAABB(const double bmin[3], -// const double bmax[3]); - -//******************************************************************************************************************** -// Implementation of the handful of FloatMath methods we actually use -//******************************************************************************************************************** - -static double triangle_area_3d(const nd::VHACD::Vect3& p1, - const nd::VHACD::Vect3& p2, - const nd::VHACD::Vect3& p3) - -/**********************************************************************/ - -/* -Purpose: - -TRIANGLE_AREA_3D computes the area of a triangle in 3D. - -Modified: - -22 April 1999 - -Author: - -John Burkardt - -Parameters: - -Input, double X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3, the (getX,getY,getZ) -coordinates of the corners of the triangle. - -Output, double TRIANGLE_AREA_3D, the area of the triangle. -*/ +KdTreeNode::KdTreeNode(uint32_t index) + : m_index(index) { - /* - Find the projection of (P3-P1) onto (P2-P1). - */ - double base = (p2 - p1).GetNorm(); - /* - The height of the triangle is the length of (P3-P1) after its - projection onto (P2-P1) has been subtracted. - */ - double height; - if (base == 0.0) - { - - height = 0.0; - } - else - { - double dot = (p3 - p1).Dot(p2 - p1); - double alpha = dot / (base * base); - - double a = p3.getX() - p1.getX() - alpha * (p2.getX() - p1.getX()); - double b = p3.getY() - p1.getY() - alpha * (p2.getY() - p1.getY()); - double c = p3.getZ() - p1.getZ() - alpha * (p2.getZ() - p1.getZ()); - - height = std::sqrt(a * a + b * b + c * c); - } - - return 0.5f * base * height; } -double fm_computeArea(const nd::VHACD::Vect3& p1, - const nd::VHACD::Vect3& p2, - const nd::VHACD::Vect3& p3) +void KdTreeNode::Add(KdTreeNode& node, + Axes dim, + const KdTree& tree) { - return triangle_area_3d(p1, - p2, - p3); -} - -bool fm_computeCentroid(const std::vector& points, - const std::vector& indices, - nd::VHACD::Vect3& center) + Axes axis = X_AXIS; + uint32_t idx = 0; + switch (dim) + { + case X_AXIS: + idx = 0; + axis = Y_AXIS; + break; + case Y_AXIS: + idx = 1; + axis = Z_AXIS; + break; + case Z_AXIS: + idx = 2; + axis = X_AXIS; + break; + } -{ - bool ret = false; - if (points.size()) + const VHACD::Vertex& nodePosition = tree.GetPosition(node.m_index); + const VHACD::Vertex& position = tree.GetPosition(m_index); + if (nodePosition[idx] <= position[idx]) { - center = nd::VHACD::Vect3(0); - - nd::VHACD::Vect3 numerator(0); - double denominator = 0; - - for (uint32_t i = 0; i < indices.size(); i++) - { - uint32_t i1 = indices[i].mI0; - uint32_t i2 = indices[i].mI1; - uint32_t i3 = indices[i].mI2; - - const nd::VHACD::Vect3& p1 = points[i1]; - const nd::VHACD::Vect3& p2 = points[i2]; - const nd::VHACD::Vect3& p3 = points[i3]; - - // Compute the average of the sum of the three positions - nd::VHACD::Vect3 sum = (p1 + p2 + p3) / 3; - - // Compute the area of this triangle - double area = fm_computeArea(p1, - p2, - p3); - - numerator += (sum * area); - - denominator += area; - } - double recip = 1 / denominator; - center = numerator * recip; - ret = true; + if (m_left) + m_left->Add(node, axis, tree); + else + m_left = &node; } - return ret; -} - -inline double det(const VHACD::Vertex& p1, - const VHACD::Vertex& p2, - const VHACD::Vertex& p3) -{ - return p1.mX * p2.mY * p3.mZ + - p2.mX * p3.mY * p1.mZ + - p3.mX * p1.mY * p2.mZ - - p1.mX * p3.mY * p2.mZ - - p2.mX * p1.mY * p3.mZ - - p3.mX * p2.mY * p1.mZ; -} - -double fm_computeMeshVolume(const std::vector& vertices, - const std::vector& indices) -{ - double volume = 0; - for (uint32_t i = 0; i < indices.size(); i++) + else { - const VHACD::Vertex& p1 = vertices[indices[i].mI0]; - const VHACD::Vertex& p2 = vertices[indices[i].mI1]; - const VHACD::Vertex& p3 = vertices[indices[i].mI2]; - volume += det(p1, - p2, - p3); // compute the volume of the tetrahedran relative to the origin. + if (m_right) + m_right->Add(node, axis, tree); + else + m_right = &node; } - - volume *= (1.0f / 6.0f); - if (volume < 0) - volume *= -1; - return volume; -} - -void fm_inflateMinMax(nd::VHACD::Vect3& bmin, - nd::VHACD::Vect3& bmax, - double ratio) -{ - double inflate = (bmin - bmax).GetNorm() * 0.5 * ratio; - bmin -= inflate; - bmax += inflate; } -void fm_combineAABB(const nd::VHACD::Vect3& bmin1, - const nd::VHACD::Vect3& bmax1, - const nd::VHACD::Vect3& bmin2, - const nd::VHACD::Vect3& bmax2, - nd::VHACD::Vect3& bmin, - nd::VHACD::Vect3& bmax) +uint32_t KdTreeNode::GetIndex() const { - bmin = bmin1.CWiseMin(bmin2); - bmax = bmax1.CWiseMax(bmax2); + return m_index; } -double fm_volumeAABB(const nd::VHACD::Vect3& bmin, - const nd::VHACD::Vect3& bmax) +void KdTreeNode::Search(Axes axis, + const VHACD::Vect3& pos, + double radius, + uint32_t& count, + uint32_t maxObjects, + KdTreeFindNode* found, + const KdTree& iface) { - double dx = bmax.getX() - bmin.getX(); - double dy = bmax.getY() - bmin.getY(); - double dz = bmax.getZ() - bmin.getZ(); - return dx*dy*dz; -} + const VHACD::Vect3 position = iface.GetPosition(m_index); -bool fm_intersectAABB(const nd::VHACD::Vect3& bmin1, - const nd::VHACD::Vect3& bmax1, - const nd::VHACD::Vect3& bmin2, - const nd::VHACD::Vect3& bmax2) -{ - if ((bmin1.getX() > bmax2.getX()) || (bmin2.getX() > bmax1.getX())) - return false; - if ((bmin1.getY() > bmax2.getY()) || (bmin2.getY() > bmax1.getY())) - return false; - if ((bmin1.getZ() > bmax2.getZ()) || (bmin2.getZ() > bmax1.getZ())) - return false; - return true; -} + const VHACD::Vect3 d = pos - position; -void fm_getAABB(const std::vector& points, - nd::VHACD::Vect3& bmin, - nd::VHACD::Vect3& bmax) -{ - bmin = points[0]; - bmax = points[0]; + KdTreeNode* search1 = 0; + KdTreeNode* search2 = 0; - for (uint32_t i = 1; i < points.size(); i++) + uint32_t idx = 0; + switch (axis) { - const VHACD::Vertex& p = points[i]; - bmin = bmin.CWiseMin(p); - bmax = bmax.CWiseMax(p); + case X_AXIS: + idx = 0; + axis = Y_AXIS; + break; + case Y_AXIS: + idx = 1; + axis = Z_AXIS; + break; + case Z_AXIS: + idx = 2; + axis = X_AXIS; + break; } -} -class MyVertexIndex -{ -public: - MyVertexIndex(double granularity, - bool snapToGrid) - : mSnapToGrid(snapToGrid) - , mGranularity(granularity) + if (d[idx] <= 0) // JWR if we are to the left { + search1 = m_left; // JWR then search to the left + if (-d[idx] < radius) // JWR if distance to the right is less than our search radius, continue on the right + // as well. + search2 = m_right; } - - double snapToGrid(double p) + else { - double m = fmod(p, mGranularity); - p -= m; - return p; + search1 = m_right; // JWR ok, we go down the left tree + if (d[idx] < radius) // JWR if the distance from the right is less than our search radius + search2 = m_left; } - uint32_t getIndex(const nd::VHACD::Vect3& _p, - bool& newPos) // get index for a vector double - { - uint32_t ret; - - newPos = false; - - nd::VHACD::Vect3 p; + double r2 = radius * radius; + double m = d.GetNormSquared(); - if (mSnapToGrid) + if (m < r2) + { + switch (count) + { + case 0: { - p[0] = snapToGrid(_p[0]); - p[1] = snapToGrid(_p[1]); - p[2] = snapToGrid(_p[2]); + found[count].m_node = this; + found[count].m_distance = m; + break; } - else + case 1: + { + if (m < found[0].m_distance) + { + if (maxObjects == 1) + { + found[0].m_node = this; + found[0].m_distance = m; + } + else + { + found[1] = found[0]; + found[0].m_node = this; + found[0].m_distance = m; + } + } + else if (maxObjects > 1) + { + found[1].m_node = this; + found[1].m_distance = m; + } + break; + } + default: { - p[0] = _p[0]; - p[1] = _p[1]; - p[2] = _p[2]; + bool inserted = false; + + for (uint32_t i = 0; i < count; i++) + { + if (m < found[i].m_distance) // if this one is closer than a pre-existing one... + { + // insertion sort... + uint32_t scan = count; + if (scan >= maxObjects) + scan = maxObjects - 1; + for (uint32_t j = scan; j > i; j--) + { + found[j] = found[j - 1]; + } + found[i].m_node = this; + found[i].m_distance = m; + inserted = true; + break; + } + } + + if (!inserted && count < maxObjects) + { + found[count].m_node = this; + found[count].m_distance = m; + } + } + break; } - bool found; - ret = mKdTree.getNearest(p, mGranularity, found); - if (!found) + count++; + + if (count > maxObjects) { - newPos = true; - ret = mKdTree.add(VHACD::Vertex(p.getX(), p.getY(), p.getZ())); + count = maxObjects; } - - return ret; } - const std::vector& getVertices() const - { - return mKdTree.getVertices(); - } - std::vector&& takeVertices() - { - return std::move(mKdTree.takeVertices()); - } + if (search1) + search1->Search(axis, pos, radius, count, maxObjects, found, iface); + + if (search2) + search2->Search(axis, pos, radius, count, maxObjects, found, iface); +} + +class VertexIndex +{ +public: + VertexIndex(double granularity, + bool snapToGrid); + + VHACD::Vect3 SnapToGrid(VHACD::Vect3 p); + + uint32_t GetIndex(VHACD::Vect3 p, + bool& newPos); + + const std::vector& GetVertices() const; + + std::vector&& TakeVertices(); - uint32_t getVcount() const - { - return mKdTree.getVcount(); - } + uint32_t GetVCount() const; - bool saveAsObj(const char* fname, + bool SaveAsObj(const char* fname, uint32_t tcount, uint32_t* indices) { @@ -4154,7 +3863,7 @@ class MyVertexIndex { ret = true; - const std::vector& v = getVertices(); + const std::vector& v = GetVertices(); for (uint32_t i = 0; i < v.size(); ++i) { fprintf(fph, "v %0.9f %0.9f %0.9f\r\n", @@ -4180,255 +3889,177 @@ class MyVertexIndex } private: - bool mSnapToGrid : 1; - double mGranularity; - VERTEX_INDEX::KdTree mKdTree; + bool m_snapToGrid : 1; + double m_granularity; + KdTree m_KdTree; }; -} // namespace VHACD +VertexIndex::VertexIndex(double granularity, + bool snapToGrid) + : m_snapToGrid(snapToGrid) + , m_granularity(granularity) +{ +} -//******************************************************************************************************************** -// Defining the Voxel class -//******************************************************************************************************************** -namespace VHACD +VHACD::Vect3 VertexIndex::SnapToGrid(VHACD::Vect3 p) { + for (int i = 0; i < 3; ++i) + { + double m = fmod(p[i], m_granularity); + p[i] -= m; + } + return p; +} -#define VHACD_VOXEL_BITS 10 -#define VHACD_VOXEL_BITS2 20 -#define VHACD_VOXEL_BIT_MASK ((1<> VHACD_VOXEL_BITS2); - y = (mVoxel >> VHACD_VOXEL_BITS) & VHACD_VOXEL_BIT_MASK; - z = mVoxel & VHACD_VOXEL_BIT_MASK; - } +const std::vector& VertexIndex::GetVertices() const +{ + return m_KdTree.GetVertices(); +} - inline uint32_t getX() const - { - return (mVoxel >> VHACD_VOXEL_BITS2); - } +std::vector&& VertexIndex::TakeVertices() +{ + return std::move(m_KdTree.TakeVertices()); +} - inline uint32_t getY() const - { - return (mVoxel >> VHACD_VOXEL_BITS) & VHACD_VOXEL_BIT_MASK; - } +uint32_t VertexIndex::GetVCount() const +{ + return m_KdTree.GetVCount(); +} - inline uint32_t getZ() const - { - return mVoxel & VHACD_VOXEL_BIT_MASK; - } +/* + * A wrapper class for 3 10 bit integers packed into a 32 bit integer + * Layout is [PAD][X][Y][Z] + * Pad is bits 31-30, X is 29-20, Y is 19-10, and Z is 9-0 + */ +class Voxel +{ + /* + * Specify all of them for consistency + */ + static constexpr int VoxelBitsZStart = 0; + static constexpr int VoxelBitsYStart = 10; + static constexpr int VoxelBitsXStart = 20; + static constexpr int VoxelBitMask = 0x03FF; // bits 0 through 9 inclusive +public: + Voxel() = default; - inline uint32_t getVoxelAddress() const - { - return mVoxel; - } + Voxel(uint32_t index); - uint32_t mVoxel{0}; - }; + Voxel(uint32_t x, + uint32_t y, + uint32_t z); - class VoxelHash - { - public: - size_t operator() (const Voxel &p) const - { - return size_t(p.mVoxel); // xor the x,y,z location to compute a hash - } - }; + bool operator==(const Voxel &v) const; - class VoxelPosition - { - public: - VoxelPosition(void) { } - VoxelPosition(double _x,double _y,double _z) : x(_x),y(_y),z(_z) {} - double x; - double y; - double z; - }; + VHACD::Vector3 GetVoxel() const; - using VoxelSet = std::unordered_set< Voxel, VoxelHash >; - using VoxelPositionMap = std::unordered_map< Voxel,VoxelPosition, VoxelHash >; - using VoxelVector = std::vector< Voxel >; -} + uint32_t GetX() const; + uint32_t GetY() const; + uint32_t GetZ() const; -//******************************************************************************************************************** -// Defining the SimpleMesh class -//******************************************************************************************************************** -namespace VHACD -{ + uint32_t GetVoxelAddress() const; -class SimpleMesh -{ -public: - std::vector mVertices; - std::vector mIndices; +private: + uint32_t m_voxel{ 0 }; }; +Voxel::Voxel(uint32_t index) + : m_voxel(index) +{ } -//****************************************************************************************** -// Declaration of the AABBTree class -//****************************************************************************************** - -namespace VHACD +Voxel::Voxel(uint32_t x, + uint32_t y, + uint32_t z) + : m_voxel((x << VoxelBitsXStart) | (y << VoxelBitsYStart) | (z << VoxelBitsZStart)) { + assert(x < 1024 && "Voxel constructed with X outside of range"); + assert(y < 1024 && "Voxel constructed with Y outside of range"); + assert(z < 1024 && "Voxel constructed with Z outside of range"); +} -/*======================== 0-tests ========================*/ +bool Voxel::operator==(const Voxel& v) const +{ + return m_voxel == v.m_voxel; +} -#define VHACD_AXISTEST_X01(a, b, fa, fb) \ - p0 = a * v0[1] - b * v0[2]; \ - p2 = a * v2[1] - b * v2[2]; \ - if (p0 < p2) \ - { \ - min = p0; \ - max = p2; \ - } \ - else \ - { \ - min = p2; \ - max = p0; \ - } \ - rad = fa * boxhalfsize[1] + fb * boxhalfsize[2]; \ - if (min > rad || max < -rad) \ - return 0; +VHACD::Vector3 Voxel::GetVoxel() const +{ + return VHACD::Vector3(GetX(), GetY(), GetZ()); +} -#define VHACD_AXISTEST_X2(a, b, fa, fb) \ - p0 = a * v0[1] - b * v0[2]; \ - p1 = a * v1[1] - b * v1[2]; \ - if (p0 < p1) \ - { \ - min = p0; \ - max = p1; \ - } \ - else \ - { \ - min = p1; \ - max = p0; \ - } \ - rad = fa * boxhalfsize[1] + fb * boxhalfsize[2]; \ - if (min > rad || max < -rad) \ - return 0; +uint32_t Voxel::GetX() const +{ + return (m_voxel >> VoxelBitsXStart) & VoxelBitMask; +} -/*======================== 1-tests ========================*/ - -#define VHACD_AXISTEST_Y02(a, b, fa, fb) \ - p0 = -a * v0[0] + b * v0[2]; \ - p2 = -a * v2[0] + b * v2[2]; \ - if (p0 < p2) \ - { \ - min = p0; \ - max = p2; \ - } \ - else \ - { \ - min = p2; \ - max = p0; \ - } \ - rad = fa * boxhalfsize[0] + fb * boxhalfsize[2]; \ - if (min > rad || max < -rad) \ - return 0; +uint32_t Voxel::GetY() const +{ + return (m_voxel >> VoxelBitsYStart) & VoxelBitMask; +} -#define VHACD_AXISTEST_Y1(a, b, fa, fb) \ - p0 = -a * v0[0] + b * v0[2]; \ - p1 = -a * v1[0] + b * v1[2]; \ - if (p0 < p1) \ - { \ - min = p0; \ - max = p1; \ - } \ - else \ - { \ - min = p1; \ - max = p0; \ - } \ - rad = fa * boxhalfsize[0] + fb * boxhalfsize[2]; \ - if (min > rad || max < -rad) \ - return 0; +uint32_t Voxel::GetZ() const +{ + return (m_voxel >> VoxelBitsZStart) & VoxelBitMask; +} -/*======================== 2-tests ========================*/ - - -#define VHACD_AXISTEST_Z12(a, b, fa, fb) \ - p1 = a * v1[0] - b * v1[1]; \ - p2 = a * v2[0] - b * v2[1]; \ - if (p2 < p1) \ - { \ - min = p2; \ - max = p1; \ - } \ - else \ - { \ - min = p1; \ - max = p2; \ - } \ - rad = fa * boxhalfsize[0] + fb * boxhalfsize[1]; \ - if (min > rad || max < -rad) \ - return 0; +uint32_t Voxel::GetVoxelAddress() const +{ + return m_voxel; +} -#define VHACD_AXISTEST_Z0(a, b, fa, fb) \ - p0 = a * v0[0] - b * v0[1]; \ - p1 = a * v1[0] - b * v1[1]; \ - if (p0 < p1) \ - { \ - min = p0; \ - max = p1; \ - } \ - else \ - { \ - min = p1; \ - max = p0; \ - } \ - rad = fa * boxhalfsize[0] + fb * boxhalfsize[1]; \ - if (min > rad || max < -rad) \ - return 0; +struct SimpleMesh +{ + std::vector m_vertices; + std::vector m_indices; +}; -inline bool IntersectRayAABB(const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& dir, - const nd::VHACD::Vect3& min, - const nd::VHACD::Vect3& max, - double& t, - nd::VHACD::Vect3* /*normal*/) +/*======================== 0-tests ========================*/ +inline bool IntersectRayAABB(const VHACD::Vect3& start, + const VHACD::Vect3& dir, + const VHACD::BoundsAABB& bounds, + double& t) { //! calculate candidate plane on each axis bool inside = true; - nd::VHACD::Vect3 ta(-1.0); + VHACD::Vect3 ta(double(-1.0)); //! use unrolled loops for (uint32_t i = 0; i < 3; ++i) { - if (start[i] < min[i]) + if (start[i] < bounds.GetMin()[i]) { - if (dir[i] != 0.0f) - ta[i] = (min[i] - start[i]) / dir[i]; + if (dir[i] != double(0.0)) + ta[i] = (bounds.GetMin()[i] - start[i]) / dir[i]; inside = false; } - else if (start[i] > max[i]) + else if (start[i] > bounds.GetMax()[i]) { - if (dir[i] != 0.0f) - ta[i] = (max[i] - start[i]) / dir[i]; + if (dir[i] != double(0.0)) + ta[i] = (bounds.GetMax()[i] - start[i]) / dir[i]; inside = false; } } @@ -4436,7 +4067,7 @@ inline bool IntersectRayAABB(const nd::VHACD::Vect3& start, //! if point inside all planes if (inside) { - t = 0.0f; + t = double(0.0); return true; } @@ -4445,22 +4076,28 @@ inline bool IntersectRayAABB(const nd::VHACD::Vect3& start, uint32_t taxis; double tmax = ta.MaxCoeff(taxis); - if (tmax < 0.0f) + if (tmax < double(0.0)) return false; //! check that the intersection point lies on the plane we picked //! we don't test the axis of closest intersection for precision reasons //! no eps for now - double eps = 0.0f; + double eps = double(0.0); - nd::VHACD::Vect3 hit = start + dir * tmax; + VHACD::Vect3 hit = start + dir * tmax; - if ((hit.getX() < min.getX() - eps || hit.getX() > max.getX() + eps) && taxis != 0) + if (( hit.GetX() < bounds.GetMin().GetX() - eps + || hit.GetX() > bounds.GetMax().GetX() + eps) + && taxis != 0) return false; - if ((hit.getY() < min.getY() - eps || hit.getY() > max.getY() + eps) && taxis != 1) + if (( hit.GetY() < bounds.GetMin().GetY() - eps + || hit.GetY() > bounds.GetMax().GetY() + eps) + && taxis != 1) return false; - if ((hit.getZ() < min.getZ() - eps || hit.getZ() > max.getZ() + eps) && taxis != 2) + if (( hit.GetZ() < bounds.GetMin().GetZ() - eps + || hit.GetZ() > bounds.GetMax().GetZ() + eps) + && taxis != 2) return false; //! output results @@ -4470,150 +4107,157 @@ inline bool IntersectRayAABB(const nd::VHACD::Vect3& start, } // Moller and Trumbore's method -inline bool IntersectRayTriTwoSided(const nd::VHACD::Vect3& p, - const nd::VHACD::Vect3& dir, - const nd::VHACD::Vect3& a, - const nd::VHACD::Vect3& b, - const nd::VHACD::Vect3& c, +inline bool IntersectRayTriTwoSided(const VHACD::Vect3& p, + const VHACD::Vect3& dir, + const VHACD::Vect3& a, + const VHACD::Vect3& b, + const VHACD::Vect3& c, double& t, double& u, double& v, double& w, double& sign, - nd::VHACD::Vect3* normal) + VHACD::Vect3* normal) { - nd::VHACD::Vect3 ab = b - a; - nd::VHACD::Vect3 ac = c - a; - nd::VHACD::Vect3 n = ab.Cross(ac); + VHACD::Vect3 ab = b - a; + VHACD::Vect3 ac = c - a; + VHACD::Vect3 n = ab.Cross(ac); double d = -dir.Dot(n); - double ood = 1.0f / d; // No need to check for division by zero here as infinity aritmetic will save us... - nd::VHACD::Vect3 ap = p - a; + double ood = double(1.0) / d; // No need to check for division by zero here as infinity aritmetic will save us... + VHACD::Vect3 ap = p - a; - t = (ap.Dot(n)) * ood; - if (t < 0.0f) + t = ap.Dot(n) * ood; + if (t < double(0.0)) + { return false; + } - nd::VHACD::Vect3 e = -dir.Cross(ap); - v = (ac.Dot(e)) * ood; - if (v < 0.0f || v > 1.0f) // ...here... + VHACD::Vect3 e = -dir.Cross(ap); + v = ac.Dot(e) * ood; + if (v < double(0.0) || v > double(1.0)) // ...here... + { return false; - w = -(ab.Dot(e)) * ood; - if (w < 0.0f || v + w > 1.0f) // ...and here + } + w = -ab.Dot(e) * ood; + if (w < double(0.0) || v + w > double(1.0)) // ...and here + { return false; + } - u = 1.0f - v - w; + u = double(1.0) - v - w; if (normal) + { *normal = n; + } sign = d; return true; } -inline nd::VHACD::Vect3 ClosestPointToAABB(const nd::VHACD::Vect3& p, - const nd::VHACD::Vect3& lower, - const nd::VHACD::Vect3& upper) -{ - nd::VHACD::Vect3 c; - - for (int i = 0; i < 3; ++i) - { - double v = p[i]; - if (v < lower[i]) - v = lower[i]; - if (v > upper[i]) - v = upper[i]; - c[i] = v; - } - - return c; -} - // RTCD 5.1.5, page 142 -inline nd::VHACD::Vect3 ClosestPointOnTriangle(const nd::VHACD::Vect3& a, - const nd::VHACD::Vect3& b, - const nd::VHACD::Vect3& c, - const nd::VHACD::Vect3& p, - double& v, - double& w) +inline VHACD::Vect3 ClosestPointOnTriangle(const VHACD::Vect3& a, + const VHACD::Vect3& b, + const VHACD::Vect3& c, + const VHACD::Vect3& p, + double& v, + double& w) { - nd::VHACD::Vect3 ab = b - a; - nd::VHACD::Vect3 ac = c - a; - nd::VHACD::Vect3 ap = p - a; + VHACD::Vect3 ab = b - a; + VHACD::Vect3 ac = c - a; + VHACD::Vect3 ap = p - a; double d1 = ab.Dot(ap); double d2 = ac.Dot(ap); - if (d1 <= 0.0f && d2 <= 0.0f) + if ( d1 <= double(0.0) + && d2 <= double(0.0)) { - v = 0.0f; - w = 0.0f; + v = double(0.0); + w = double(0.0); return a; } - nd::VHACD::Vect3 bp = p - b; + VHACD::Vect3 bp = p - b; double d3 = ab.Dot(bp); double d4 = ac.Dot(bp); - if (d3 >= 0.0f && d4 <= d3) + if ( d3 >= double(0.0) + && d4 <= d3) { - v = 1.0f; - w = 0.0f; + v = double(1.0); + w = double(0.0); return b; } double vc = d1 * d4 - d3 * d2; - if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) + if ( vc <= double(0.0) + && d1 >= double(0.0) + && d3 <= double(0.0)) { v = d1 / (d1 - d3); - w = 0.0f; + w = double(0.0); return a + v * ab; } - nd::VHACD::Vect3 cp = p - c; + VHACD::Vect3 cp = p - c; double d5 = ab.Dot(cp); double d6 = ac.Dot(cp); - if (d6 >= 0.0f && d5 <= d6) + if (d6 >= double(0.0) && d5 <= d6) { - v = 0.0f; - w = 1.0f; + v = double(0.0); + w = double(1.0); return c; } double vb = d5 * d2 - d1 * d6; - if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) + if ( vb <= double(0.0) + && d2 >= double(0.0) + && d6 <= double(0.0)) { - v = 0.0f; + v = double(0.0); w = d2 / (d2 - d6); return a + w * ac; } double va = d3 * d6 - d5 * d4; - if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) + if ( va <= double(0.0) + && (d4 - d3) >= double(0.0) + && (d5 - d6) >= double(0.0)) { w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - v = 1.0f - w; + v = double(1.0) - w; return b + w * (c - b); } - double denom = 1.0f / (va + vb + vc); + double denom = double(1.0) / (va + vb + vc); v = vb * denom; w = vc * denom; return a + ab * v + ac * w; } - -class AABBTreeImpl +class AABBTree { public: - AABBTreeImpl() = default; - AABBTreeImpl(AABBTreeImpl&&) = default; - AABBTreeImpl& operator=(AABBTreeImpl&&) = default; + AABBTree() = default; + AABBTree(AABBTree&&) = default; + AABBTree& operator=(AABBTree&&) = default; + + AABBTree(const std::vector& vertices, + const std::vector& indices); + + bool TraceRay(const VHACD::Vect3& start, + const VHACD::Vect3& to, + double& outT, + double& faceSign, + VHACD::Vect3& hitLocation) const; - AABBTreeImpl(const std::vector& vertices, - const std::vector& indices); + bool TraceRay(const VHACD::Vect3& start, + const VHACD::Vect3& dir, + uint32_t& insideCount, + uint32_t& outsideCount) const; - bool TraceRay(const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& dir, + bool TraceRay(const VHACD::Vect3& start, + const VHACD::Vect3& dir, double& outT, double& u, double& v, @@ -4621,54 +4265,13 @@ class AABBTreeImpl double& faceSign, uint32_t& faceIndex) const; + VHACD::Vect3 GetCenter() const; + VHACD::Vect3 GetMinExtents() const; + VHACD::Vect3 GetMaxExtents() const; - nd::VHACD::Vect3 GetCenter() const - { - return (m_nodes[0].m_minExtents + m_nodes[0].m_maxExtents) * 0.5f; - } - nd::VHACD::Vect3 GetMinExtents() const - { - return m_nodes[0].m_minExtents; - } - nd::VHACD::Vect3 GetMaxExtents() const - { - return m_nodes[0].m_maxExtents; - } - - bool raycast(const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& dir, - double& outT, - double& u, - double& v, - double& w, - double& faceSign, - uint32_t& faceIndex) const - { - return TraceRay(start, - dir, - outT, - u, - v, - w, - faceSign, - faceIndex); - } - - bool getClosestPointWithinDistance(const nd::VHACD::Vect3& point, + bool GetClosestPointWithinDistance(const VHACD::Vect3& point, double maxDistance, - nd::VHACD::Vect3& closestPoint) - { - double dis, v, w; - uint32_t faceIndex; - bool hit = GetClosestPointWithinDistance(point, - maxDistance, - dis, - v, - w, - faceIndex, - closestPoint); - return hit; - } + VHACD::Vect3& closestPoint) const; private: struct Node @@ -4680,41 +4283,22 @@ class AABBTreeImpl }; uint32_t* m_faces{ nullptr }; - nd::VHACD::Vect3 m_minExtents{ 0.0 }; - nd::VHACD::Vect3 m_maxExtents{ 0.0 }; + VHACD::BoundsAABB m_extents; }; - - struct BoundsAABB + struct FaceSorter { - BoundsAABB() = default; - BoundsAABB(const nd::VHACD::Vect3& min, - const nd::VHACD::Vect3& max) - : m_min(min) - , m_max(max) - { - } - - inline double GetVolume() const - { - nd::VHACD::Vect3 e = m_max - m_min; - return (e.getX() * e.getY() * e.getZ()); - } + FaceSorter(const std::vector& positions, + const std::vector& indices, + uint32_t axis); - inline double GetSurfaceArea() const - { - nd::VHACD::Vect3 e = m_max - m_min; - return 2.0f * (e.getX() * e.getY() + e.getX() * e.getZ() + e.getY() * e.getZ()); - } + bool operator()(uint32_t lhs, uint32_t rhs) const; - inline void Union(const BoundsAABB& b) - { - m_min = m_min.CWiseMin(b.m_min); - m_max = m_max.CWiseMax(b.m_max); - } + double GetCentroid(uint32_t face) const; - nd::VHACD::Vect3 m_min{ 0.0 }; - nd::VHACD::Vect3 m_max{ 0.0 }; + const std::vector& m_vertices; + const std::vector& m_indices; + uint32_t m_axis; }; // partition the objects and return the number of objects in the lower partition @@ -4732,8 +4316,8 @@ class AABBTreeImpl uint32_t numFaces); void TraceRecursive(uint32_t nodeIndex, - const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& dir, + const VHACD::Vect3& start, + const VHACD::Vect3& dir, double& outT, double& u, double& v, @@ -4742,179 +4326,205 @@ class AABBTreeImpl uint32_t& faceIndex) const; - bool GetClosestPointWithinDistance(const nd::VHACD::Vect3& point, + bool GetClosestPointWithinDistance(const VHACD::Vect3& point, const double maxDis, double& dis, double& v, double& w, uint32_t& faceIndex, - nd::VHACD::Vect3& closest) const; + VHACD::Vect3& closest) const; void GetClosestPointWithinDistanceSqRecursive(uint32_t nodeIndex, - const nd::VHACD::Vect3& point, + const VHACD::Vect3& point, double& outDisSq, double& outV, double& outW, uint32_t& outFaceIndex, - nd::VHACD::Vect3& closest) const; - - void CalculateFaceBounds(uint32_t* faces, - uint32_t numFaces, - nd::VHACD::Vect3& outMinExtents, - nd::VHACD::Vect3& outMaxExtents); - - uint32_t GetNumFaces() const - { - return uint32_t(m_indices->size()); - } + VHACD::Vect3& closest) const; - uint32_t GetNumNodes() const - { - return uint32_t(m_nodes.size()); - } + VHACD::BoundsAABB CalculateFaceBounds(uint32_t* faces, + uint32_t numFaces); // track the next free node uint32_t m_freeNode; - const std::vector* m_vertices{nullptr}; - - const std::vector* m_indices{nullptr}; + const std::vector* m_vertices{ nullptr }; + const std::vector* m_indices{ nullptr }; std::vector m_faces; std::vector m_nodes; - std::vector m_faceBounds; + std::vector m_faceBounds; // stats - uint32_t m_treeDepth = 0; - uint32_t m_innerNodes = 0; - uint32_t m_leafNodes = 0; + uint32_t m_treeDepth{ 0 }; + uint32_t m_innerNodes{ 0 }; + uint32_t m_leafNodes{ 0 }; - uint32_t s_depth{0}; + uint32_t s_depth{ 0 }; }; -AABBTreeImpl::AABBTreeImpl(const std::vector& vertices, - const std::vector& indices) - : m_vertices(&vertices) - , m_indices(&indices) +AABBTree::FaceSorter::FaceSorter(const std::vector& positions, + const std::vector& indices, + uint32_t axis) + : m_vertices(positions) + , m_indices(indices) + , m_axis(axis) { - Build(); } -namespace +inline bool AABBTree::FaceSorter::operator()(uint32_t lhs, + uint32_t rhs) const { + double a = GetCentroid(lhs); + double b = GetCentroid(rhs); -struct FaceSorter -{ - FaceSorter(const std::vector& positions, - const std::vector& indices, - uint32_t axis) - : m_vertices(positions) - , m_indices(indices) - , m_axis(axis) - { - } - - inline bool operator()(uint32_t lhs, uint32_t rhs) const + if (a == b) { - double a = GetCentroid(lhs); - double b = GetCentroid(rhs); - - if (a == b) - return lhs < rhs; - else - return a < b; + return lhs < rhs; } - - inline double GetCentroid(uint32_t face) const + else { - const nd::VHACD::Vect3& a = m_vertices[m_indices[face].mI0]; - const nd::VHACD::Vect3& b = m_vertices[m_indices[face].mI1]; - const nd::VHACD::Vect3& c = m_vertices[m_indices[face].mI2]; - - return (a[m_axis] + b[m_axis] + c[m_axis]) / 3.0f; + return a < b; } +} - const std::vector& m_vertices; - const std::vector& m_indices; - uint32_t m_axis; -}; - +inline double AABBTree::FaceSorter::GetCentroid(uint32_t face) const +{ + const VHACD::Vect3& a = m_vertices[m_indices[face].mI0]; + const VHACD::Vect3& b = m_vertices[m_indices[face].mI1]; + const VHACD::Vect3& c = m_vertices[m_indices[face].mI2]; -} // anonymous namespace + return (a[m_axis] + b[m_axis] + c[m_axis]) / double(3.0); +} -void AABBTreeImpl::CalculateFaceBounds(uint32_t* faces, - uint32_t numFaces, - nd::VHACD::Vect3& outMinExtents, - nd::VHACD::Vect3& outMaxExtents) +AABBTree::AABBTree(const std::vector& vertices, + const std::vector& indices) + : m_vertices(&vertices) + , m_indices(&indices) { - nd::VHACD::Vect3 minExtents(FLT_MAX); - nd::VHACD::Vect3 maxExtents(-FLT_MAX); + Build(); +} - // calculate face bounds - for (uint32_t i = 0; i < numFaces; ++i) +bool AABBTree::TraceRay(const VHACD::Vect3& start, + const VHACD::Vect3& to, + double& outT, + double& faceSign, + VHACD::Vect3& hitLocation) const +{ + VHACD::Vect3 dir = to - start; + double distance = dir.Normalize(); + double u, v, w; + uint32_t faceIndex; + bool hit = TraceRay(start, + dir, + outT, + u, + v, + w, + faceSign, + faceIndex); + if (hit) { - nd::VHACD::Vect3 a = (*m_vertices)[(*m_indices)[faces[i]].mI0]; - nd::VHACD::Vect3 b = (*m_vertices)[(*m_indices)[faces[i]].mI1]; - nd::VHACD::Vect3 c = (*m_vertices)[(*m_indices)[faces[i]].mI2]; - - minExtents = a.CWiseMin(minExtents); - maxExtents = a.CWiseMax(maxExtents); - - minExtents = b.CWiseMin(minExtents); - maxExtents = b.CWiseMax(maxExtents); - - minExtents = c.CWiseMin(minExtents); - maxExtents = c.CWiseMax(maxExtents); + hitLocation = start + dir * outT; } - outMinExtents = minExtents; - outMaxExtents = maxExtents; + if (hit && outT > distance) + { + hit = false; + } + return hit; } -void AABBTreeImpl::Build() +bool AABBTree::TraceRay(const VHACD::Vect3& start, + const VHACD::Vect3& dir, + uint32_t& insideCount, + uint32_t& outsideCount) const { - const uint32_t numFaces = uint32_t(m_indices->size()); - - // build initial list of faces - m_faces.reserve(numFaces); - - // calculate bounds of each face and store - m_faceBounds.reserve(numFaces); - - std::vector stack; - for (uint32_t i = 0; i < numFaces; ++i) + double outT, u, v, w, faceSign; + uint32_t faceIndex; + bool hit = TraceRay(start, + dir, + outT, + u, + v, + w, + faceSign, + faceIndex); + if (hit) { - BoundsAABB top; - CalculateFaceBounds(&i, - 1, - top.m_min, - top.m_max); - - m_faces.push_back(i); - m_faceBounds.push_back(top); + if (faceSign >= 0) + { + insideCount++; + } + else + { + outsideCount++; + } } + return hit; +} + +bool AABBTree::TraceRay(const VHACD::Vect3& start, + const VHACD::Vect3& dir, + double& outT, + double& u, + double& v, + double& w, + double& faceSign, + uint32_t& faceIndex) const +{ + outT = FLT_MAX; + TraceRecursive(0, + start, + dir, + outT, + u, + v, + w, + faceSign, + faceIndex); + return (outT != FLT_MAX); +} - m_nodes.reserve(uint32_t(numFaces * 1.5f)); +VHACD::Vect3 AABBTree::GetCenter() const +{ + return m_nodes[0].m_extents.GetCenter(); +} - // allocate space for all the nodes - m_freeNode = 1; +VHACD::Vect3 AABBTree::GetMinExtents() const +{ + return m_nodes[0].m_extents.GetMin(); +} - // start building - BuildRecursive(0, - m_faces.data(), - numFaces); +VHACD::Vect3 AABBTree::GetMaxExtents() const +{ + return m_nodes[0].m_extents.GetMax(); +} - assert(s_depth == 0); +bool AABBTree::GetClosestPointWithinDistance(const VHACD::Vect3& point, + double maxDistance, + VHACD::Vect3& closestPoint) const +{ + double dis, v, w; + uint32_t faceIndex; + bool hit = GetClosestPointWithinDistance(point, + maxDistance, + dis, + v, + w, + faceIndex, + closestPoint); + return hit; } // partion faces around the median face -uint32_t AABBTreeImpl::PartitionMedian(Node& n, - uint32_t* faces, - uint32_t numFaces) +uint32_t AABBTree::PartitionMedian(Node& n, + uint32_t* faces, + uint32_t numFaces) { FaceSorter predicate(*m_vertices, *m_indices, - (n.m_maxExtents - n.m_minExtents).LongestAxis()); + n.m_extents.GetSize().LongestAxis()); std::nth_element(faces, faces + numFaces / 2, faces + numFaces, @@ -4924,11 +4534,10 @@ uint32_t AABBTreeImpl::PartitionMedian(Node& n, } // partion faces based on the surface area heuristic -uint32_t AABBTreeImpl::PartitionSAH(Node& n, - uint32_t* faces, - uint32_t numFaces) +uint32_t AABBTree::PartitionSAH(Node&, + uint32_t* faces, + uint32_t numFaces) { -// (n); uint32_t bestAxis = 0; uint32_t bestIndex = 0; double bestCost = FLT_MAX; @@ -4947,19 +4556,19 @@ uint32_t AABBTreeImpl::PartitionSAH(Node& n, std::vector cumulativeLower(numFaces); std::vector cumulativeUpper(numFaces); - BoundsAABB lower; - BoundsAABB upper; + VHACD::BoundsAABB lower; + VHACD::BoundsAABB upper; for (uint32_t i = 0; i < numFaces; ++i) { lower.Union(m_faceBounds[faces[i]]); upper.Union(m_faceBounds[faces[numFaces - i - 1]]); - cumulativeLower[i] = lower.GetSurfaceArea(); - cumulativeUpper[numFaces - i - 1] = upper.GetSurfaceArea(); + cumulativeLower[i] = lower.SurfaceArea(); + cumulativeUpper[numFaces - i - 1] = upper.SurfaceArea(); } - double invTotalSA = 1.0f / cumulativeUpper[0]; + double invTotalSA = double(1.0) / cumulativeUpper[0]; // test all split positions for (uint32_t i = 0; i < numFaces - 1; ++i) @@ -4967,7 +4576,7 @@ uint32_t AABBTreeImpl::PartitionSAH(Node& n, double pBelow = cumulativeLower[i] * invTotalSA; double pAbove = cumulativeUpper[i] * invTotalSA; - double cost = 0.125f + (pBelow * i + pAbove * (numFaces - i)); + double cost = double(0.125) + (pBelow * i + pAbove * (numFaces - i)); if (cost <= bestCost) { bestCost = cost; @@ -4988,16 +4597,49 @@ uint32_t AABBTreeImpl::PartitionSAH(Node& n, return bestIndex + 1; } -void AABBTreeImpl::BuildRecursive(uint32_t nodeIndex, - uint32_t* faces, - uint32_t numFaces) +void AABBTree::Build() +{ + const uint32_t numFaces = uint32_t(m_indices->size()); + + // build initial list of faces + m_faces.reserve(numFaces); + + // calculate bounds of each face and store + m_faceBounds.reserve(numFaces); + + std::vector stack; + for (uint32_t i = 0; i < numFaces; ++i) + { + VHACD::BoundsAABB top = CalculateFaceBounds(&i, + 1); + + m_faces.push_back(i); + m_faceBounds.push_back(top); + } + + m_nodes.reserve(uint32_t(numFaces * double(1.5))); + + // allocate space for all the nodes + m_freeNode = 1; + + // start building + BuildRecursive(0, + m_faces.data(), + numFaces); + + assert(s_depth == 0); +} + +void AABBTree::BuildRecursive(uint32_t nodeIndex, + uint32_t* faces, + uint32_t numFaces) { const uint32_t kMaxFacesPerLeaf = 6; // if we've run out of nodes allocate some more if (nodeIndex >= m_nodes.size()) { - uint32_t s = std::max(uint32_t(1.5f * m_nodes.size()), 512U); + uint32_t s = std::max(uint32_t(double(1.5) * m_nodes.size()), 512U); m_nodes.resize(s); } @@ -5008,10 +4650,8 @@ void AABBTreeImpl::BuildRecursive(uint32_t nodeIndex, ++s_depth; m_treeDepth = std::max(m_treeDepth, s_depth); - CalculateFaceBounds(faces, - numFaces, - n.m_minExtents, - n.m_maxExtents); + n.m_extents = CalculateFaceBounds(faces, + numFaces); // calculate bounds of faces and add node if (numFaces <= kMaxFacesPerLeaf) @@ -5044,37 +4684,15 @@ void AABBTreeImpl::BuildRecursive(uint32_t nodeIndex, --s_depth; } -bool AABBTreeImpl::TraceRay(const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& dir, - double& outT, - double& u, - double& v, - double& w, - double& faceSign, - uint32_t& faceIndex) const -{ - outT = FLT_MAX; - TraceRecursive(0, - start, - dir, - outT, - u, - v, - w, - faceSign, - faceIndex); - return (outT != FLT_MAX); -} - -void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, - const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& dir, - double& outT, - double& outU, - double& outV, - double& outW, - double& faceSign, - uint32_t& faceIndex) const +void AABBTree::TraceRecursive(uint32_t nodeIndex, + const VHACD::Vect3& start, + const VHACD::Vect3& dir, + double& outT, + double& outU, + double& outV, + double& outW, + double& faceSign, + uint32_t& faceIndex) const { const Node& node = m_nodes[nodeIndex]; @@ -5088,16 +4706,12 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, IntersectRayAABB(start, dir, - leftChild.m_minExtents, - leftChild.m_maxExtents, - dist[0], - nullptr); + leftChild.m_extents, + dist[0]); IntersectRayAABB(start, dir, - rightChild.m_minExtents, - rightChild.m_maxExtents, - dist[1], - nullptr); + rightChild.m_extents, + dist[1]); uint32_t closest = 0; uint32_t furthest = 1; @@ -5109,6 +4723,7 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, } if (dist[closest] < outT) + { TraceRecursive(node.m_children + closest, start, dir, @@ -5118,8 +4733,10 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, outW, faceSign, faceIndex); + } if (dist[furthest] < outT) + { TraceRecursive(node.m_children + furthest, start, dir, @@ -5129,6 +4746,7 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, outW, faceSign, faceIndex); + } } else { @@ -5138,9 +4756,9 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, { uint32_t indexStart = node.m_faces[i]; - const nd::VHACD::Vect3& a = (*m_vertices)[(*m_indices)[indexStart].mI0]; - const nd::VHACD::Vect3& b = (*m_vertices)[(*m_indices)[indexStart].mI1]; - const nd::VHACD::Vect3& c = (*m_vertices)[(*m_indices)[indexStart].mI2]; + const VHACD::Vect3& a = (*m_vertices)[(*m_indices)[indexStart].mI0]; + const VHACD::Vect3& b = (*m_vertices)[(*m_indices)[indexStart].mI1]; + const VHACD::Vect3& c = (*m_vertices)[(*m_indices)[indexStart].mI2]; if (IntersectRayTriTwoSided(start, dir, a, b, c, t, u, v, w, s, NULL)) { if (t < outT) @@ -5157,13 +4775,13 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, } } -bool AABBTreeImpl::GetClosestPointWithinDistance(const nd::VHACD::Vect3& point, - const double maxDis, - double& dis, - double& v, - double& w, - uint32_t& faceIndex, - nd::VHACD::Vect3& closest) const +bool AABBTree::GetClosestPointWithinDistance(const VHACD::Vect3& point, + const double maxDis, + double& dis, + double& v, + double& w, + uint32_t& faceIndex, + VHACD::Vect3& closest) const { dis = maxDis; faceIndex = uint32_t(~0); @@ -5181,13 +4799,13 @@ bool AABBTreeImpl::GetClosestPointWithinDistance(const nd::VHACD::Vect3& return (faceIndex < (~(static_cast(0)))); } -void AABBTreeImpl::GetClosestPointWithinDistanceSqRecursive(uint32_t nodeIndex, - const nd::VHACD::Vect3& point, - double& outDisSq, - double& outV, - double& outW, - uint32_t& outFaceIndex, - nd::VHACD::Vect3& closestPoint) const +void AABBTree::GetClosestPointWithinDistanceSqRecursive(uint32_t nodeIndex, + const VHACD::Vect3& point, + double& outDisSq, + double& outV, + double& outW, + uint32_t& outFaceIndex, + VHACD::Vect3& closestPoint) const { const Node& node = m_nodes[nodeIndex]; @@ -5198,12 +4816,8 @@ void AABBTreeImpl::GetClosestPointWithinDistanceSqRecursive(uint32_t nodeIndex, const Node& rightChild = m_nodes[node.m_children + 1]; // double dist[2] = { FLT_MAX, FLT_MAX }; - nd::VHACD::Vect3 lp = ClosestPointToAABB(point, - leftChild.m_minExtents, - leftChild.m_maxExtents); - nd::VHACD::Vect3 rp = ClosestPointToAABB(point, - rightChild.m_minExtents, - rightChild.m_maxExtents); + VHACD::Vect3 lp = leftChild.m_extents.ClosestPoint(point); + VHACD::Vect3 rp = rightChild.m_extents.ClosestPoint(point); uint32_t closest = 0; @@ -5247,11 +4861,11 @@ void AABBTreeImpl::GetClosestPointWithinDistanceSqRecursive(uint32_t nodeIndex, { uint32_t indexStart = node.m_faces[i]; - const nd::VHACD::Vect3& a = (*m_vertices)[(*m_indices)[indexStart].mI0]; - const nd::VHACD::Vect3& b = (*m_vertices)[(*m_indices)[indexStart].mI1]; - const nd::VHACD::Vect3& c = (*m_vertices)[(*m_indices)[indexStart].mI2]; + const VHACD::Vect3& a = (*m_vertices)[(*m_indices)[indexStart].mI0]; + const VHACD::Vect3& b = (*m_vertices)[(*m_indices)[indexStart].mI1]; + const VHACD::Vect3& c = (*m_vertices)[(*m_indices)[indexStart].mI2]; - nd::VHACD::Vect3 cp = ClosestPointOnTriangle(a, b, c, point, v, w); + VHACD::Vect3 cp = ClosestPointOnTriangle(a, b, c, point, v, w); double disSq = (cp - point).GetNormSquared(); if (disSq < outDisSq) @@ -5266,100 +4880,32 @@ void AABBTreeImpl::GetClosestPointWithinDistanceSqRecursive(uint32_t nodeIndex, } } -} // namespace aabbtree - -//****************************************************************************************** -// Implementation of the RaycastMesh class -//****************************************************************************************** -namespace VHACD -{ - -class MyRaycastMesh +VHACD::BoundsAABB AABBTree::CalculateFaceBounds(uint32_t* faces, + uint32_t numFaces) { -public: - MyRaycastMesh() = default; - MyRaycastMesh(MyRaycastMesh&& rhs) = default; - MyRaycastMesh& operator=(MyRaycastMesh&& rhs) = default; - - MyRaycastMesh(const std::vector& vertices, - const std::vector& indices) - : mAABBTree(vertices, indices) - , mVertices(&vertices) - , mIndices(&indices) - { - } - - // Uses high speed AABB raycasting - bool raycast(const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& dir, - double& outT, - double& u, - double& v, - double& w, - double& faceSign, - uint32_t& faceIndex) const - { - return mAABBTree.raycast(start, - dir, - outT, - u, - v, - w, - faceSign, - faceIndex); - } - - bool raycast(const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& to, - double& outT, - double& faceSign, - nd::VHACD::Vect3& hitLocation) const - { - nd::VHACD::Vect3 dir = to - start; - double distance = dir.Normalize(); - double u, v, w; - uint32_t faceIndex; - bool hit = mAABBTree.raycast(start, - dir, - outT, - u, - v, - w, - faceSign, - faceIndex); - if (hit) - { - hitLocation = start + dir * outT; - } - if (hit && outT > distance) - { - hit = false; - } - return hit; - } + VHACD::Vect3 minExtents( FLT_MAX); + VHACD::Vect3 maxExtents(-FLT_MAX); - bool getClosestPointWithinDistance(const nd::VHACD::Vect3 point, - double maxDistance, - nd::VHACD::Vect3& closestPoint) + // calculate face bounds + for (uint32_t i = 0; i < numFaces; ++i) { - return mAABBTree.getClosestPointWithinDistance(point, - maxDistance, - closestPoint); - } + VHACD::Vect3 a = (*m_vertices)[(*m_indices)[faces[i]].mI0]; + VHACD::Vect3 b = (*m_vertices)[(*m_indices)[faces[i]].mI1]; + VHACD::Vect3 c = (*m_vertices)[(*m_indices)[faces[i]].mI2]; - VHACD::AABBTreeImpl mAABBTree; - const std::vector* mVertices = nullptr; - const std::vector* mIndices = nullptr; -}; + minExtents = a.CWiseMin(minExtents); + maxExtents = a.CWiseMax(maxExtents); -} // namespace VHACD + minExtents = b.CWiseMin(minExtents); + maxExtents = b.CWiseMax(maxExtents); -//************************************************************************************************************* -// Definition of the Volume class -//************************************************************************************************************* + minExtents = c.CWiseMin(minExtents); + maxExtents = c.CWiseMax(maxExtents); + } -namespace VHACD -{ + return VHACD::BoundsAABB(minExtents, + maxExtents); +} enum class VoxelValue : uint8_t { @@ -5377,77 +4923,39 @@ class Volume const std::vector& triangles, const size_t dim, FillMode fillMode, - MyRaycastMesh* raycastMesh); - - void raycastFill(MyRaycastMesh* raycastMesh); + const AABBTree& aabbTree); - void SetVoxel(const size_t i, const size_t j, const size_t k, VoxelValue value) - { - assert(i < m_dim[0] || i >= 0); - assert(j < m_dim[1] || j >= 0); - assert(k < m_dim[2] || k >= 0); - - m_data[k + j * m_dim[2] + i * m_dim[1] * m_dim[2]] = value; - } - - VoxelValue& GetVoxel(const size_t i, const size_t j, const size_t k) - { - assert(i < m_dim[0] || i >= 0); - assert(j < m_dim[1] || j >= 0); - assert(k < m_dim[2] || k >= 0); - return m_data[k + j * m_dim[2] + i * m_dim[1] * m_dim[2]]; - } - - const VoxelValue& GetVoxel(const size_t i, const size_t j, const size_t k) const - { - assert(i < m_dim[0] || i >= 0); - assert(j < m_dim[1] || j >= 0); - assert(k < m_dim[2] || k >= 0); - return m_data[k + j * m_dim[2] + i * m_dim[1] * m_dim[2]]; - } - - size_t GetNPrimitivesOnSurf() const - { - return m_numVoxelsOnSurface; - } + void RaycastFill(const AABBTree& aabbTree); - size_t GetNPrimitivesInsideSurf() const - { - return m_numVoxelsInsideSurface; - } + void SetVoxel(const size_t i, + const size_t j, + const size_t k, + VoxelValue value); - const VHACD::VoxelVector& getSurfaceVoxels() const - { - return mSurfaceVoxels; - } + VoxelValue& GetVoxel(const size_t i, + const size_t j, + const size_t k); - const VHACD::VoxelVector& getInteriorVoxels() const - { - return mInteriorVoxels; - } + const VoxelValue& GetVoxel(const size_t i, + const size_t j, + const size_t k) const; - void addSurfaceVoxel(int32_t x, int32_t y, int32_t z) - { - VHACD::Voxel v(x, y, z); - mSurfaceVoxels.push_back(v); - } + const std::vector& GetSurfaceVoxels() const; + const std::vector& GetInteriorVoxels() const; - void addInteriorVoxel(int32_t x, int32_t y, int32_t z) - { - VHACD::Voxel v(x, y, z); - mInteriorVoxels.push_back(v); - } + double GetScale() const; + const VHACD::BoundsAABB& GetBounds() const; + const VHACD::Vector3& GetDimensions() const; - nd::VHACD::Vect3 m_minBB{ 0.0 }; - nd::VHACD::Vect3 m_maxBB{ 1.0 }; + VHACD::BoundsAABB m_bounds; double m_scale{ 1.0 }; - nd::VHACD::Vect3 m_dim{ 0 }; + VHACD::Vector3 m_dim{ 0 }; size_t m_numVoxelsOnSurface{ 0 }; size_t m_numVoxelsInsideSurface{ 0 }; size_t m_numVoxelsOutsideSurface{ 0 }; std::vector m_data; - private: + void MarkOutsideSurface(const size_t i0, const size_t j0, const size_t k0, @@ -5458,52 +4966,163 @@ class Volume void FillInsideSurface(); - void ComputeBB(const std::vector& points); - - void Allocate(); - - std::vector mSurfaceVoxels; - std::vector mInteriorVoxels; + std::vector m_surfaceVoxels; + std::vector m_interiorVoxels; }; -int32_t TriBoxOverlap(const nd::VHACD::Vect3& boxcenter, - const nd::VHACD::Vect3& boxhalfsize, - const nd::VHACD::Vect3& triver0, - const nd::VHACD::Vect3& triver1, - const nd::VHACD::Vect3& triver2); - -inline void Volume::ComputeBB(const std::vector& points) +bool PlaneBoxOverlap(const VHACD::Vect3& normal, + const VHACD::Vect3& vert, + const VHACD::Vect3& maxbox) { - nd::VHACD::Vect3 pt = points[0]; - m_maxBB = pt; - m_minBB = pt; - for (uint32_t v = 1; v < points.size(); ++v) + int32_t q; + VHACD::Vect3 vmin; + VHACD::Vect3 vmax; + double v; + for (q = 0; q < 3; q++) { - pt = points[v]; - for (int32_t i = 0; i < 3; ++i) + v = vert[q]; + if (normal[q] > double(0.0)) + { + vmin[q] = -maxbox[q] - v; + vmax[q] = maxbox[q] - v; + } + else { - if (pt[i] < m_minBB[i]) - m_minBB[i] = pt[i]; - else if (pt[i] > m_maxBB[i]) - m_maxBB[i] = pt[i]; + vmin[q] = maxbox[q] - v; + vmax[q] = -maxbox[q] - v; } } + if (normal.Dot(vmin) > double(0.0)) + return false; + if (normal.Dot(vmax) >= double(0.0)) + return true; + return false; +} + +bool AxisTest(double a, double b, double fa, double fb, + double v0, double v1, double v2, double v3, + double boxHalfSize1, double boxHalfSize2) +{ + double p0 = a * v0 + b * v1; + double p1 = a * v2 + b * v3; + + double min = std::min(p0, p1); + double max = std::max(p0, p1); + + double rad = fa * boxHalfSize1 + fb * boxHalfSize2; + if (min > rad || max < -rad) + { + return false; + } + + return true; +} + +bool TriBoxOverlap(const VHACD::Vect3& boxCenter, + const VHACD::Vect3& boxHalfSize, + const VHACD::Vect3& triVer0, + const VHACD::Vect3& triVer1, + const VHACD::Vect3& triVer2) +{ + /* use separating axis theorem to test overlap between triangle and box */ + /* need to test for overlap in these directions: */ + /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */ + /* we do not even need to test these) */ + /* 2) normal of the triangle */ + /* 3) crossproduct(edge from tri, {x,y,z}-directin) */ + /* this gives 3x3=9 more tests */ + + VHACD::Vect3 v0 = triVer0 - boxCenter; + VHACD::Vect3 v1 = triVer1 - boxCenter; + VHACD::Vect3 v2 = triVer2 - boxCenter; + VHACD::Vect3 e0 = v1 - v0; + VHACD::Vect3 e1 = v2 - v1; + VHACD::Vect3 e2 = v0 - v2; + + /* This is the fastest branch on Sun */ + /* move everything so that the boxcenter is in (0,0,0) */ + + /* Bullet 3: */ + /* test the 9 tests first (this was faster) */ + double fex = fabs(e0[0]); + double fey = fabs(e0[1]); + double fez = fabs(e0[2]); + + /* + * These should use Get*() instead of subscript for consistency, but the function calls are long enough already + */ + if (!AxisTest( e0[2], -e0[1], fez, fey, v0[1], v0[2], v2[1], v2[2], boxHalfSize[1], boxHalfSize[2])) return 0; // X01 + if (!AxisTest(-e0[2], e0[0], fez, fex, v0[0], v0[2], v2[0], v2[2], boxHalfSize[0], boxHalfSize[2])) return 0; // Y02 + if (!AxisTest( e0[1], -e0[0], fey, fex, v1[0], v1[1], v2[0], v2[1], boxHalfSize[0], boxHalfSize[1])) return 0; // Z12 + + fex = fabs(e1[0]); + fey = fabs(e1[1]); + fez = fabs(e1[2]); + + if (!AxisTest( e1[2], -e1[1], fez, fey, v0[1], v0[2], v2[1], v2[2], boxHalfSize[1], boxHalfSize[2])) return 0; // X01 + if (!AxisTest(-e1[2], e1[0], fez, fex, v0[0], v0[2], v2[0], v2[2], boxHalfSize[0], boxHalfSize[2])) return 0; // Y02 + if (!AxisTest( e1[1], -e1[0], fey, fex, v0[0], v0[1], v1[0], v1[1], boxHalfSize[0], boxHalfSize[2])) return 0; // Z0 + + fex = fabs(e2[0]); + fey = fabs(e2[1]); + fez = fabs(e2[2]); + + if (!AxisTest( e2[2], -e2[1], fez, fey, v0[1], v0[2], v1[1], v1[2], boxHalfSize[1], boxHalfSize[2])) return 0; // X2 + if (!AxisTest(-e2[2], e2[0], fez, fex, v0[0], v0[2], v1[0], v1[2], boxHalfSize[0], boxHalfSize[2])) return 0; // Y1 + if (!AxisTest( e2[1], -e2[0], fey, fex, v1[0], v1[1], v2[0], v2[1], boxHalfSize[0], boxHalfSize[1])) return 0; // Z12 + + /* Bullet 1: */ + /* first test overlap in the {x,y,z}-directions */ + /* find min, max of the triangle each direction, and test for overlap in */ + /* that direction -- this is equivalent to testing a minimal AABB around */ + /* the triangle against the AABB */ + + /* test in 0-direction */ + double min = std::min({v0.GetX(), v1.GetX(), v2.GetX()}); + double max = std::max({v0.GetX(), v1.GetX(), v2.GetX()}); + if (min > boxHalfSize[0] || max < -boxHalfSize[0]) + return false; + + /* test in 1-direction */ + min = std::min({v0.GetY(), v1.GetY(), v2.GetY()}); + max = std::max({v0.GetY(), v1.GetY(), v2.GetY()}); + if (min > boxHalfSize[1] || max < -boxHalfSize[1]) + return false; + + /* test in getZ-direction */ + min = std::min({v0.GetZ(), v1.GetZ(), v2.GetZ()}); + max = std::max({v0.GetZ(), v1.GetZ(), v2.GetZ()}); + if (min > boxHalfSize[2] || max < -boxHalfSize[2]) + return false; + + /* Bullet 2: */ + /* test if the box intersects the plane of the triangle */ + /* compute plane equation of triangle: normal*x+d=0 */ + VHACD::Vect3 normal = e0.Cross(e1); + + if (!PlaneBoxOverlap(normal, v0, boxHalfSize)) + return false; + return true; /* box and triangle overlaps */ } -inline void Volume::Voxelize(const std::vector& points, - const std::vector& indices, - const size_t dim, - FillMode fillMode, - VHACD::MyRaycastMesh* raycastMesh) +void Volume::Voxelize(const std::vector& points, + const std::vector& indices, + const size_t dimensions, + FillMode fillMode, + const AABBTree& aabbTree) { + double a = std::pow(dimensions, 0.33); + size_t dim = a * double(1.5); + dim = std::max(dim, size_t(32)); + if (points.size() == 0) { return; } - ComputeBB(points); + m_bounds = BoundsAABB(points); - nd::VHACD::Vect3 d = m_maxBB - m_minBB; + VHACD::Vect3 d = m_bounds.GetSize(); double r; // Equal comparison is important here to avoid taking the last branch when d[0] == d[1] with d[2] being the smallest // dimension. That would lead to dimensions in i and j to be a lot bigger than expected and make the amount of @@ -5533,32 +5152,30 @@ inline void Volume::Voxelize(const std::vector& points, m_scale = r / (dim - 1); double invScale = (dim - 1) / r; - Allocate(); + m_data = std::vector(m_dim[0] * m_dim[1] * m_dim[2], + VoxelValue::PRIMITIVE_UNDEFINED); m_numVoxelsOnSurface = 0; m_numVoxelsInsideSurface = 0; m_numVoxelsOutsideSurface = 0; - nd::VHACD::Vect3 p[3]; - size_t i, j, k; - size_t i0, j0, k0; - size_t i1, j1, k1; - nd::VHACD::Vect3 boxcenter; - nd::VHACD::Vect3 pt; - const nd::VHACD::Vect3 boxhalfsize(0.5, 0.5, 0.5); + VHACD::Vect3 p[3]; + VHACD::Vect3 boxcenter; + VHACD::Vect3 pt; + const VHACD::Vect3 boxhalfsize(double(0.5)); for (size_t t = 0; t < indices.size(); ++t) { - nd::VHACD::Vect3 tri = indices[t]; + size_t i0, j0, k0; + size_t i1, j1, k1; + VHACD::Vector3 tri = indices[t]; for (int32_t c = 0; c < 3; ++c) { pt = points[tri[c]]; - p[c][0] = (pt[0] - m_minBB[0]) * invScale; - p[c][1] = (pt[1] - m_minBB[1]) * invScale; - p[c][2] = (pt[2] - m_minBB[2]) * invScale; + p[c] = (pt - m_bounds.GetMin()) * invScale; - i = static_cast(p[c][0] + 0.5); - j = static_cast(p[c][1] + 0.5); - k = static_cast(p[c][2] + 0.5); + size_t i = static_cast(p[c][0] + double(0.5)); + size_t j = static_cast(p[c][1] + double(0.5)); + size_t k = static_cast(p[c][2] + double(0.5)); assert(i < m_dim[0] && i >= 0 && j < m_dim[1] && j >= 0 && k < m_dim[2] && k >= 0); @@ -5570,12 +5187,13 @@ inline void Volume::Voxelize(const std::vector& points, } else { - i0 = Min(i0, i); - j0 = Min(j0, j); - k0 = Min(k0, k); - i1 = Max(i1, i); - j1 = Max(j1, j); - k1 = Max(k1, k); + i0 = std::min(i0, i); + j0 = std::min(j0, j); + k0 = std::min(k0, k); + + i1 = std::max(i1, i); + j1 = std::max(j1, j); + k1 = std::max(k1, k); } } if (i0 > 0) @@ -5599,194 +5217,203 @@ inline void Volume::Voxelize(const std::vector& points, for (size_t k_id = k0; k_id < k1; ++k_id) { boxcenter[2] = uint32_t(k_id); - int32_t res = TriBoxOverlap(boxcenter, boxhalfsize, p[0], p[1], p[2]); - VoxelValue& value = GetVoxel(i_id, j_id, k_id); - if (res == 1 && value == VoxelValue::PRIMITIVE_UNDEFINED) + bool res = TriBoxOverlap(boxcenter, + boxhalfsize, + p[0], + p[1], + p[2]); + VoxelValue& value = GetVoxel(i_id, + j_id, + k_id); + if ( res + && value == VoxelValue::PRIMITIVE_UNDEFINED) { value = VoxelValue::PRIMITIVE_ON_SURFACE; ++m_numVoxelsOnSurface; - addSurfaceVoxel(int32_t(i_id),int32_t(j_id),int32_t(k_id)); + m_surfaceVoxels.emplace_back(uint32_t(i_id), + uint32_t(j_id), + uint32_t(k_id)); + } + } + } + } + } + + if (fillMode == FillMode::SURFACE_ONLY) + { + const size_t i0_local = m_dim[0]; + const size_t j0_local = m_dim[1]; + const size_t k0_local = m_dim[2]; + for (size_t i_id = 0; i_id < i0_local; ++i_id) + { + for (size_t j_id = 0; j_id < j0_local; ++j_id) + { + for (size_t k_id = 0; k_id < k0_local; ++k_id) + { + const VoxelValue& voxel = GetVoxel(i_id, + j_id, + k_id); + if (voxel != VoxelValue::PRIMITIVE_ON_SURFACE) + { + SetVoxel(i_id, + j_id, + k_id, + VoxelValue::PRIMITIVE_OUTSIDE_SURFACE); + } + } + } + } + } + else if (fillMode == FillMode::FLOOD_FILL) + { + /* + * Marking the outside edges of the voxel cube to be outside surfaces to walk + */ + MarkOutsideSurface(0, 0, 0, m_dim[0], m_dim[1], 1); + MarkOutsideSurface(0, 0, m_dim[2] - 1, m_dim[0], m_dim[1], m_dim[2]); + MarkOutsideSurface(0, 0, 0, m_dim[0], 1, m_dim[2]); + MarkOutsideSurface(0, m_dim[1] - 1, 0, m_dim[0], m_dim[1], m_dim[2]); + MarkOutsideSurface(0, 0, 0, 1, m_dim[1], m_dim[2]); + MarkOutsideSurface(m_dim[0] - 1, 0, 0, m_dim[0], m_dim[1], m_dim[2]); + FillOutsideSurface(); + FillInsideSurface(); + } + else if (fillMode == FillMode::RAYCAST_FILL) + { + RaycastFill(aabbTree); + } +} + +void Volume::RaycastFill(const AABBTree& aabbTree) +{ + const uint32_t i0 = m_dim[0]; + const uint32_t j0 = m_dim[1]; + const uint32_t k0 = m_dim[2]; + + size_t maxSize = i0 * j0 * k0; + + std::vector temp; + temp.reserve(maxSize); + uint32_t count{ 0 }; + m_numVoxelsInsideSurface = 0; + for (uint32_t i = 0; i < i0; ++i) + { + for (uint32_t j = 0; j < j0; ++j) + { + for (uint32_t k = 0; k < k0; ++k) + { + VoxelValue& voxel = GetVoxel(i, j, k); + if (voxel != VoxelValue::PRIMITIVE_ON_SURFACE) + { + VHACD::Vect3 start = VHACD::Vect3(i, j, k) * m_scale + m_bounds.GetMin(); + + uint32_t insideCount = 0; + uint32_t outsideCount = 0; + + VHACD::Vect3 directions[6] = { + VHACD::Vect3( 1, 0, 0), + VHACD::Vect3(-1, 0, 0), // this was 1, 0, 0 in the original code, but looks wrong + VHACD::Vect3( 0, 1, 0), + VHACD::Vect3( 0, -1, 0), + VHACD::Vect3( 0, 0, 1), + VHACD::Vect3( 0, 0, -1) + }; + + for (uint32_t r = 0; r < 6; r++) + { + aabbTree.TraceRay(start, + directions[r], + insideCount, + outsideCount); + // Early out if we hit the outside of the mesh + if (outsideCount) + { + break; + } + // Early out if we accumulated 3 inside hits + if (insideCount >= 3) + { + break; + } + } + + if (outsideCount == 0 && insideCount >= 3) + { + voxel = VoxelValue::PRIMITIVE_INSIDE_SURFACE; + temp.emplace_back(i, j, k); + count++; + m_numVoxelsInsideSurface++; } - } - } - } - } - if (fillMode == FillMode::SURFACE_ONLY) - { - const size_t i0_local = m_dim[0]; - const size_t j0_local = m_dim[1]; - const size_t k0_local = m_dim[2]; - for (size_t i_id = 0; i_id < i0_local; ++i_id) - { - for (size_t j_id = 0; j_id < j0_local; ++j_id) - { - for (size_t k_id = 0; k_id < k0_local; ++k_id) - { - const VoxelValue& voxel = GetVoxel(i_id, j_id, k_id); - if (voxel != VoxelValue::PRIMITIVE_ON_SURFACE) + else { - SetVoxel(i_id, j_id, k_id, VoxelValue::PRIMITIVE_OUTSIDE_SURFACE); + voxel = VoxelValue::PRIMITIVE_OUTSIDE_SURFACE; } } } } } - else if (fillMode == FillMode::FLOOD_FILL) - { - MarkOutsideSurface(0, 0, 0, m_dim[0], m_dim[1], 1); - MarkOutsideSurface(0, 0, m_dim[2] - 1, m_dim[0], m_dim[1], m_dim[2]); - MarkOutsideSurface(0, 0, 0, m_dim[0], 1, m_dim[2]); - MarkOutsideSurface(0, m_dim[1] - 1, 0, m_dim[0], m_dim[1], m_dim[2]); - MarkOutsideSurface(0, 0, 0, 1, m_dim[1], m_dim[2]); - MarkOutsideSurface(m_dim[0] - 1, 0, 0, m_dim[0], m_dim[1], m_dim[2]); - FillOutsideSurface(); - FillInsideSurface(); - } - else if (fillMode == FillMode::RAYCAST_FILL) + + if (count) { - raycastFill(raycastMesh); + m_interiorVoxels = std::move(temp); } - } -} // namespace VHACD +void Volume::SetVoxel(const size_t i, + const size_t j, + const size_t k, + VoxelValue value) +{ + assert(i < m_dim[0] || i >= 0); + assert(j < m_dim[1] || j >= 0); + assert(k < m_dim[2] || k >= 0); -//************************************************************************************************************* -// Implementation of the Volume class -//************************************************************************************************************* + m_data[k + j * m_dim[2] + i * m_dim[1] * m_dim[2]] = value; +} -namespace VHACD +VoxelValue& Volume::GetVoxel(const size_t i, + const size_t j, + const size_t k) { + assert(i < m_dim[0] || i >= 0); + assert(j < m_dim[1] || j >= 0); + assert(k < m_dim[2] || k >= 0); + return m_data[k + j * m_dim[2] + i * m_dim[1] * m_dim[2]]; +} -int32_t PlaneBoxOverlap(const nd::VHACD::Vect3& normal, - const nd::VHACD::Vect3& vert, - const nd::VHACD::Vect3& maxbox) +const VoxelValue& Volume::GetVoxel(const size_t i, + const size_t j, + const size_t k) const { - int32_t q; - nd::VHACD::Vect3 vmin; - nd::VHACD::Vect3 vmax; - double v; - for (q = 0; q <= 2; q++) - { - v = vert[q]; - if (normal[q] > 0.0) - { - vmin[q] = -maxbox[q] - v; - vmax[q] = maxbox[q] - v; - } - else - { - vmin[q] = maxbox[q] - v; - vmax[q] = -maxbox[q] - v; - } - } - if (normal.Dot(vmin) > 0.0) - return 0; - if (normal.Dot(vmax) >= 0.0) - return 1; - return 0; + assert(i < m_dim[0] || i >= 0); + assert(j < m_dim[1] || j >= 0); + assert(k < m_dim[2] || k >= 0); + return m_data[k + j * m_dim[2] + i * m_dim[1] * m_dim[2]]; } -int32_t TriBoxOverlap(const nd::VHACD::Vect3& boxcenter, - const nd::VHACD::Vect3& boxhalfsize, - const nd::VHACD::Vect3& triver0, - const nd::VHACD::Vect3& triver1, - const nd::VHACD::Vect3& triver2) +const std::vector& Volume::GetSurfaceVoxels() const { - /* use separating axis theorem to test overlap between triangle and box */ - /* need to test for overlap in these directions: */ - /* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */ - /* we do not even need to test these) */ - /* 2) normal of the triangle */ - /* 3) crossproduct(edge from tri, {x,y,z}-directin) */ - /* this gives 3x3=9 more tests */ - - nd::VHACD::Vect3 v0; - nd::VHACD::Vect3 v1; - nd::VHACD::Vect3 v2; - double min, max, p0, p1, p2, rad, fex, fey, fez; // -NJMP- "d" local variable removed - nd::VHACD::Vect3 normal; - nd::VHACD::Vect3 e0; - nd::VHACD::Vect3 e1; - nd::VHACD::Vect3 e2; - - /* This is the fastest branch on Sun */ - /* move everything so that the boxcenter is in (0,0,0) */ - - v0 = triver0 - boxcenter; - v1 = triver1 - boxcenter; - v2 = triver2 - boxcenter; - - /* compute triangle edges */ - e0 = v1 - v0; /* tri edge 0 */ - e1 = v2 - v1; /* tri edge 1 */ - e2 = v0 - v2; /* tri edge 2 */ - - /* Bullet 3: */ - /* test the 9 tests first (this was faster) */ - fex = fabs(e0[0]); - fey = fabs(e0[1]); - fez = fabs(e0[2]); - - VHACD_AXISTEST_X01(e0[2], e0[1], fez, fey); - VHACD_AXISTEST_Y02(e0[2], e0[0], fez, fex); - VHACD_AXISTEST_Z12(e0[1], e0[0], fey, fex); - - fex = fabs(e1[0]); - fey = fabs(e1[1]); - fez = fabs(e1[2]); - - VHACD_AXISTEST_X01(e1[2], e1[1], fez, fey); - VHACD_AXISTEST_Y02(e1[2], e1[0], fez, fex); - VHACD_AXISTEST_Z0(e1[1], e1[0], fey, fex); - - fex = fabs(e2[0]); - fey = fabs(e2[1]); - fez = fabs(e2[2]); - - VHACD_AXISTEST_X2(e2[2], e2[1], fez, fey); - VHACD_AXISTEST_Y1(e2[2], e2[0], fez, fex); - VHACD_AXISTEST_Z12(e2[1], e2[0], fey, fex); - - /* Bullet 1: */ - /* first test overlap in the {x,y,z}-directions */ - /* find min, max of the triangle each direction, and test for overlap in */ - /* that direction -- this is equivalent to testing a minimal AABB around */ - /* the triangle against the AABB */ - - /* test in 0-direction */ - min = std::min({v0.getX(), v1.getX(), v2.getX()}); - max = std::max({v0.getX(), v1.getX(), v2.getX()}); - if (min > boxhalfsize[0] || max < -boxhalfsize[0]) - return 0; - - /* test in 1-direction */ - min = std::min({v0.getY(), v1.getY(), v2.getY()}); - max = std::max({v0.getY(), v1.getY(), v2.getY()}); - if (min > boxhalfsize[1] || max < -boxhalfsize[1]) - return 0; + return m_surfaceVoxels; +} - /* test in getZ-direction */ - min = std::min({v0.getZ(), v1.getZ(), v2.getZ()}); - max = std::max({v0.getZ(), v1.getZ(), v2.getZ()}); - if (min > boxhalfsize[2] || max < -boxhalfsize[2]) - return 0; +const std::vector& Volume::GetInteriorVoxels() const +{ + return m_interiorVoxels; +} - /* Bullet 2: */ - /* test if the box intersects the plane of the triangle */ - /* compute plane equation of triangle: normal*x+d=0 */ - normal = e0.Cross(e1); +double Volume::GetScale() const +{ + return m_scale; +} - if (!PlaneBoxOverlap(normal, v0, boxhalfsize)) - return 0; - return 1; /* box and triangle overlaps */ +const VHACD::BoundsAABB& Volume::GetBounds() const +{ + return m_bounds; } -void Volume::Allocate() +const VHACD::Vector3& Volume::GetDimensions() const { - size_t size = m_dim[0] * m_dim[1] * m_dim[2]; - m_data = std::vector(size, - VoxelValue::PRIMITIVE_UNDEFINED); + return m_dim; } void Volume::MarkOutsideSurface(const size_t i0, @@ -5812,18 +5439,28 @@ void Volume::MarkOutsideSurface(const size_t i0, } } -inline void WalkForward(int64_t start, int64_t end, VoxelValue* ptr, int64_t stride, int64_t maxDistance) +inline void WalkForward(int64_t start, + int64_t end, + VoxelValue* ptr, + int64_t stride, + int64_t maxDistance) { - for (int64_t i = start, count = 0; count < maxDistance && i < end && *ptr == VoxelValue::PRIMITIVE_UNDEFINED; + for (int64_t i = start, count = 0; + count < maxDistance && i < end && *ptr == VoxelValue::PRIMITIVE_UNDEFINED; ++i, ptr += stride, ++count) { *ptr = VoxelValue::PRIMITIVE_OUTSIDE_SURFACE_TOWALK; } } -inline void WalkBackward(int64_t start, int64_t end, VoxelValue* ptr, int64_t stride, int64_t maxDistance) +inline void WalkBackward(int64_t start, + int64_t end, + VoxelValue* ptr, + int64_t stride, + int64_t maxDistance) { - for (int64_t i = start, count = 0; count < maxDistance && i >= end && *ptr == VoxelValue::PRIMITIVE_UNDEFINED; + for (int64_t i = start, count = 0; + count < maxDistance && i >= end && *ptr == VoxelValue::PRIMITIVE_UNDEFINED; --i, ptr -= stride, ++count) { *ptr = VoxelValue::PRIMITIVE_OUTSIDE_SURFACE_TOWALK; @@ -5890,17 +5527,15 @@ void Volume::FillOutsideSurface() } while (voxelsWalked != 0); } -#pragma warning(disable:4244) - void Volume::FillInsideSurface() { const uint32_t i0 = uint32_t(m_dim[0]); const uint32_t j0 = uint32_t(m_dim[1]); const uint32_t k0 = uint32_t(m_dim[2]); - size_t maxSize = i0*j0*k0; + size_t maxSize = i0 * j0 * k0; - std::vector temp; + std::vector temp; temp.reserve(maxSize); uint32_t count{ 0 }; @@ -5914,8 +5549,7 @@ void Volume::FillInsideSurface() if (v == VoxelValue::PRIMITIVE_UNDEFINED) { v = VoxelValue::PRIMITIVE_INSIDE_SURFACE; - uint32_t index = (i << VHACD_VOXEL_BITS2) | (j << VHACD_VOXEL_BITS) | k; - temp.push_back(index); + temp.emplace_back(i, j, k); count++; ++m_numVoxelsInsideSurface; } @@ -5925,239 +5559,10 @@ void Volume::FillInsideSurface() if ( count ) { - mInteriorVoxels.resize(count); - std::copy(temp.begin(), - temp.end(), - mInteriorVoxels.begin()); - } -} - -void traceRay(VHACD::MyRaycastMesh* raycastMesh, - const nd::VHACD::Vect3& start, - const nd::VHACD::Vect3& dir, - uint32_t& insideCount, - uint32_t& outsideCount) -{ - double outT, u, v, w, faceSign; - uint32_t faceIndex; - bool hit = raycastMesh->raycast(start, - dir, - outT, - u, - v, - w, - faceSign, - faceIndex); - if (hit) - { - if (faceSign >= 0) - { - insideCount++; - } - else - { - outsideCount++; - } - } -} - -void Volume::raycastFill(VHACD::MyRaycastMesh* raycastMesh) -{ - if (!raycastMesh) - { - return; - } - - const uint32_t i0 = m_dim[0]; - const uint32_t j0 = m_dim[1]; - const uint32_t k0 = m_dim[2]; - - size_t maxSize = i0 * j0*k0; - - std::vector temp; - temp.reserve(maxSize); - uint32_t count{ 0 }; - m_numVoxelsInsideSurface = 0; - for (uint32_t i = 0; i < i0; ++i) - { - for (uint32_t j = 0; j < j0; ++j) - { - for (uint32_t k = 0; k < k0; ++k) - { - VoxelValue& voxel = GetVoxel(i, j, k); - if (voxel != VoxelValue::PRIMITIVE_ON_SURFACE) - { - nd::VHACD::Vect3 start; - - start[0] = float(i) * m_scale + m_minBB[0]; - start[1] = float(j) * m_scale + m_minBB[1]; - start[2] = float(k) * m_scale + m_minBB[2]; - - uint32_t insideCount = 0; - uint32_t outsideCount = 0; - - nd::VHACD::Vect3 directions[6] = { - nd::VHACD::Vect3( 1, 0, 0), - nd::VHACD::Vect3(-1, 0, 0), // this was 1, 0, 0 in the original code, but looks wrong - nd::VHACD::Vect3( 0, 1, 0), - nd::VHACD::Vect3( 0, -1, 0), - nd::VHACD::Vect3( 0, 0, 1), - nd::VHACD::Vect3( 0, 0, -1) - }; - - for (uint32_t r = 0; r < 6; r++) - { - traceRay(raycastMesh, start, directions[r], insideCount, outsideCount); - // Early out if we hit the outside of the mesh - if (outsideCount) - { - break; - } - // Early out if we accumulated 3 inside hits - if (insideCount >= 3) - { - break; - } - } - - if (outsideCount == 0 && insideCount >= 3) - { - voxel = VoxelValue::PRIMITIVE_INSIDE_SURFACE; - uint32_t index = (i << VHACD_VOXEL_BITS2) | (j << VHACD_VOXEL_BITS) | k; - temp.push_back(index); - count++; - m_numVoxelsInsideSurface++; - } - else - { - voxel = VoxelValue::PRIMITIVE_OUTSIDE_SURFACE; - } - } - } - } - } - - if (count) - { - mInteriorVoxels.resize(count); - std::copy(temp.begin(), - temp.end(), - mInteriorVoxels.begin()); + m_interiorVoxels = std::move(temp); } } -} // namespace VHACD - - -//************************************************************************************************************* -// Implementation of the Voxelize class -//************************************************************************************************************* -namespace VHACD -{ - -class VoxelizeImpl -{ -public: - uint32_t voxelize(VHACD::MyRaycastMesh& raycastMesh, - const std::vector& vertices, - const std::vector& indices, - const uint32_t resolution, - FillMode fillMode) - { - double a = pow(resolution, 0.33); - mDimensions = (a*1.5); - // Minimum voxel resolution is 32x32x32 - mDimensions = Max(mDimensions, uint32_t(32)); - - mVolume = VHACD::Volume(); - mVolume.Voxelize(vertices, - indices, - mDimensions, - fillMode, - &raycastMesh); - - return mDimensions; - } - - double getScale() const - { - return mVolume.m_scale;; - } - - nd::VHACD::Vect3 getBoundsMin() const - { - return mVolume.m_minBB; - } - - nd::VHACD::Vect3 getBoundsMax() const - { - return mVolume.m_maxBB; - } - - nd::VHACD::Vect3 getDimensions() const - { - return mVolume.m_dim; - } - - VoxelValue getVoxel(uint32_t x, - uint32_t y, - uint32_t z) const - { - return mVolume.GetVoxel(x,y,z);; - } - - void setVoxel(uint32_t x, - uint32_t y, - uint32_t z, - VoxelValue value) - { - mVolume.SetVoxel(x,y,z,value); - } - - bool getVoxel(const nd::VHACD::Vect3& pos, - uint32_t &x, - uint32_t &y, - uint32_t &z) const - { - bool ret = false; - - nd::VHACD::Vect3 bmin = getBoundsMin(); - nd::VHACD::Vect3 bmax = getBoundsMax(); - - if ( pos[0] >= bmin[0] && pos[0] < bmax[0] && - pos[1] >= bmin[1] && pos[1] < bmax[1] && - pos[2] >= bmin[2] && pos[2] < bmax[2] ) - { - double recipScale = 1.0 / getScale(); - x = uint32_t( (pos[0] - bmin[0]) * recipScale); - y = uint32_t( (pos[1] - bmin[1]) * recipScale); - z = uint32_t( (pos[2] - bmin[2]) * recipScale); - ret = true; - } - return ret; - } - - bool getSurfaceVoxels(VoxelVector &surfaceVoxels) - { - surfaceVoxels.clear(); - surfaceVoxels = mVolume.getSurfaceVoxels(); - - return !surfaceVoxels.empty(); - } - - bool getInteriorVoxels(VoxelVector &interiorVoxels) - { - interiorVoxels.clear(); - interiorVoxels = mVolume.getInteriorVoxels(); - return !interiorVoxels.empty(); - } - - uint32_t mDimensions{32}; - VHACD::Volume mVolume; -}; - -} // namespace VHACD - //****************************************************************************************** // ShrinkWrap helper class //****************************************************************************************** @@ -6182,110 +5587,100 @@ class VoxelizeImpl //*********************************************************************************************** // QuickHull implementation //*********************************************************************************************** -namespace VHACD -{ ////////////////////////////////////////////////////////////////////////// // Quickhull base class holding the hull during construction -class QuickHullImpl +////////////////////////////////////////////////////////////////////////// +class QuickHull { public: - uint32_t computeConvexHull(const std::vector& vertices, - uint32_t maxHullVertices) - { - mIndices.clear(); + uint32_t ComputeConvexHull(const std::vector& vertices, + uint32_t maxHullVertices); - nd::VHACD::ConvexHull ch(vertices, - 0.0001, - maxHullVertices); + const std::vector& GetVertices() const; + const std::vector& GetIndices() const; - auto& vlist = ch.GetVertexPool(); - if ( !vlist.empty() ) - { - size_t vcount = vlist.size(); - mVertices.resize(vcount); - std::copy(vlist.begin(), - vlist.end(), - mVertices.begin()); - } - - for (nd::VHACD::ConvexHull::ndNode* node = ch.GetFirst(); node; node = node->GetNext()) - { - nd::VHACD::ConvexHullFace* const face = &node->GetInfo(); - mIndices.emplace_back(face->m_index[0], - face->m_index[1], - face->m_index[2]); - } +private: + std::vector m_vertices; + std::vector m_indices; +}; - return uint32_t(mIndices.size()); - } +uint32_t QuickHull::ComputeConvexHull(const std::vector& vertices, + uint32_t maxHullVertices) +{ + m_indices.clear(); - const std::vector& getVertices() const + VHACD::ConvexHull ch(vertices, + double(0.0001), + maxHullVertices); + + auto& vlist = ch.GetVertexPool(); + if ( !vlist.empty() ) { - return mVertices; + size_t vcount = vlist.size(); + m_vertices.resize(vcount); + std::copy(vlist.begin(), + vlist.end(), + m_vertices.begin()); } - const std::vector& getIndices() const + for (std::list::const_iterator node = ch.GetList().begin(); node != ch.GetList().end(); ++node) { - return mIndices; + const VHACD::ConvexHullFace& face = *node; + m_indices.emplace_back(face.m_index[0], + face.m_index[1], + face.m_index[2]); } -private: - std::vector mVertices; - std::vector mIndices; -}; + return uint32_t(m_indices.size()); +} -} // end of VHACD namespace +const std::vector& QuickHull::GetVertices() const +{ + return m_vertices; +} + +const std::vector& QuickHull::GetIndices() const +{ + return m_indices; +} //****************************************************************************************** -// Implementation of the ShrinkWrap class +// Implementation of the ShrinkWrap function //****************************************************************************************** -#ifdef _MSC_VER -#pragma warning(disable:4100) -#endif - -namespace VHACD +void ShrinkWrap(SimpleMesh& sourceConvexHull, + const AABBTree& aabbTree, + uint32_t maxHullVertexCount, + double distanceThreshold, + bool doShrinkWrap) { - -class ShrinkWrapImpl -{ -public: - void shrinkWrap(SimpleMesh &sourceConvexHull, - VHACD::MyRaycastMesh &raycastMesh, - uint32_t maxHullVertexCount, - double distanceThreshold, - bool doShrinkWrap) - { - std::vector verts; // New verts for the new convex hull - verts.reserve(sourceConvexHull.mVertices.size()); - // Examine each vertex and see if it is within the voxel distance. - // If it is, then replace the point with the shrinkwrapped / projected point - for (uint32_t j = 0; j < sourceConvexHull.mVertices.size(); j++) + std::vector verts; // New verts for the new convex hull + verts.reserve(sourceConvexHull.m_vertices.size()); + // Examine each vertex and see if it is within the voxel distance. + // If it is, then replace the point with the shrinkwrapped / projected point + for (uint32_t j = 0; j < sourceConvexHull.m_vertices.size(); j++) + { + VHACD::Vertex& p = sourceConvexHull.m_vertices[j]; + if (doShrinkWrap) { - VHACD::Vertex& p = sourceConvexHull.mVertices[j]; - if (doShrinkWrap) + VHACD::Vect3 closest; + if (aabbTree.GetClosestPointWithinDistance(p, distanceThreshold, closest)) { - nd::VHACD::Vect3 closest; - if (raycastMesh.getClosestPointWithinDistance(p, distanceThreshold, closest)) - { - p = closest; - } + p = closest; } - verts.emplace_back(p); - } - // Final step is to recompute the convex hull - VHACD::QuickHullImpl qh; - uint32_t tcount = qh.computeConvexHull(verts, - maxHullVertexCount); - if (tcount) - { - sourceConvexHull.mVertices = qh.getVertices(); - sourceConvexHull.mIndices = qh.getIndices(); } + verts.emplace_back(p); + } + // Final step is to recompute the convex hull + VHACD::QuickHull qh; + uint32_t tcount = qh.ComputeConvexHull(verts, + maxHullVertexCount); + if (tcount) + { + sourceConvexHull.m_vertices = qh.GetVertices(); + sourceConvexHull.m_indices = qh.GetIndices(); } -}; - } //******************************************************************************************************************** @@ -6296,12 +5691,9 @@ class ShrinkWrapImpl // Definition of the ThreadPool //******************************************************************************************************************** -namespace VHACD -{ - class ThreadPool { public: - ThreadPool() : ThreadPool(1) {}; + ThreadPool(); ThreadPool(int worker); ~ThreadPool(); template @@ -6320,14 +5712,21 @@ class ThreadPool { int count; }; -ThreadPool::ThreadPool(int worker) : closed(false), count(0) +ThreadPool::ThreadPool() + : ThreadPool(1) +{ +} + +ThreadPool::ThreadPool(int worker) + : closed(false) + , count(0) { workers.reserve(worker); for(int i=0; i lock(this->task_mutex); while(true) { @@ -6350,8 +5749,6 @@ ThreadPool::ThreadPool(int worker) : closed(false), count(0) } } - - template auto ThreadPool::enqueue(F&& f, Args&& ... args) #ifndef __cpp_lib_is_invocable @@ -6397,20 +5794,8 @@ ThreadPool::~ThreadPool() { worker.join(); } } - - -} #endif - -namespace VHACD -{ - - -using AABBTreeVector = std::vector< VHACD::AABBTreeImpl* >; -using VoxelVector = std::vector< VHACD::Voxel >; -using ConvexHullVector = std::vector< IVHACD::ConvexHull *>; - enum class Stages { COMPUTE_BOUNDS_OF_INPUT_MESH, @@ -6429,8 +5814,10 @@ enum class Stages class VHACDCallbacks { public: - virtual void progressUpdate(Stages stage,double stageProgress,const char *operation) = 0; - virtual bool isCanceled(void) const = 0; + virtual void ProgressUpdate(Stages stage, + double stageProgress, + const char *operation) = 0; + virtual bool IsCanceled() const = 0; virtual ~VHACDCallbacks() = default; }; @@ -6445,9 +5832,6 @@ enum class SplitAxis Z_AXIS_POSITIVE, }; -// Maps from a voxel index to a vertex position index -using VoxelIndexMap = std::unordered_map< uint32_t, uint32_t >; - // This class represents a collection of voxels, the convex hull // which surrounds them, and a triangle mesh representation of those voxels class VoxelHull @@ -6456,339 +5840,44 @@ class VoxelHull // This method constructs a new VoxelHull based on a plane split of the parent // convex hull - VoxelHull(const VoxelHull &parent, + VoxelHull(const VoxelHull& parent, SplitAxis axis, - uint32_t splitLoc) : mAxis(axis) - { - mDepth = parent.mDepth+1; - mVoxelHullCount++; // Each VoxelHull is given a unique index number to distinguish it from the rest. Primarily used for debugging - mIndex = mVoxelHullCount; - mVoxels = parent.mVoxels; // Copy the parent's voxels pointer - mParams = parent.mParams; // Copy the parent's parameters - mVoxelScale = mVoxels->getScale(); // Get the scale of a single voxel - mVoxelBmin = mVoxels->getBoundsMin(); // Get the 3d bounding volume minimum for this voxel space - mVoxelBmax = mVoxels->getBoundsMax(); // Get the 3d bounding volume maximum for this voxel space - mVoxelScaleHalf = mVoxelScale * 0.5; // Compute half of the voxel size - - mVoxelAdjust = mVoxelBmin - mVoxelScaleHalf; // The adjustment to move from voxel coordinate to a 3d position - - // Default copy the voxel region from the parent, but values will - // be adjusted next based on the split axis and location - m1 = parent.m1; - m2 = parent.m2; - - switch ( mAxis ) - { - case SplitAxis::X_AXIS_NEGATIVE: - m2.getX() = splitLoc; - break; - case SplitAxis::X_AXIS_POSITIVE: - m1.getX() = splitLoc + 1; - break; - case SplitAxis::Y_AXIS_NEGATIVE: - m2.getY() = splitLoc; - break; - case SplitAxis::Y_AXIS_POSITIVE: - m1.getY() = splitLoc + 1; - break; - case SplitAxis::Z_AXIS_NEGATIVE: - m2.getZ() = splitLoc; - break; - case SplitAxis::Z_AXIS_POSITIVE: - m1.getZ() = splitLoc + 1; - break; - } - // First, we copy all of the interior voxels from our parent - // which intersect our region - for (auto &i : parent.mInteriorVoxels) - { - uint32_t x, y, z; - i.getVoxel(x, y, z); - if ( x >= m1.getX() && x <= m2.getX() && - y >= m1.getY() && y <= m2.getY() && - z >= m1.getZ() && z <= m2.getZ() ) - { - bool newSurface = false; - switch ( mAxis ) - { - case SplitAxis::X_AXIS_NEGATIVE: - if ( x == splitLoc ) - { - newSurface = true; - } - break; - case SplitAxis::X_AXIS_POSITIVE: - if ( x == m1.getX() ) - { - newSurface = true; - } - break; - case SplitAxis::Y_AXIS_NEGATIVE: - if ( y == splitLoc ) - { - newSurface = true; - } - break; - case SplitAxis::Y_AXIS_POSITIVE: - if ( y == m1.getY() ) - { - newSurface = true; - } - break; - case SplitAxis::Z_AXIS_NEGATIVE: - if ( z == splitLoc ) - { - newSurface = true; - } - break; - case SplitAxis::Z_AXIS_POSITIVE: - if ( z == m1.getZ() ) - { - newSurface = true; - } - break; - } - // If his interior voxels lie directly on the split plane then - // these become new surface voxels for our patch - if ( newSurface ) - { - mNewSurfaceVoxels.push_back(i); - } - else - { - mInteriorVoxels.push_back(i); - } - } - } - // Next we copy all of the surface voxels which intersect our region - for (auto &i : parent.mSurfaceVoxels) - { - uint32_t x, y, z; - i.getVoxel(x, y, z); - if ( x >= m1.getX() && x <= m2.getX() && - y >= m1.getY() && y <= m2.getY() && - z >= m1.getZ() && z <= m2.getZ() ) - { - mSurfaceVoxels.push_back(i); - } - } - // Our parent's new surface voxels become our new surface voxels so long as they intersect our region - for (auto &i : parent.mNewSurfaceVoxels) - { - uint32_t x, y, z; - i.getVoxel(x, y, z); - if ( x >= m1.getX() && x <= m2.getX() && - y >= m1.getY() && y <= m2.getY() && - z >= m1.getZ() && z <= m2.getZ() ) - { - mNewSurfaceVoxels.push_back(i); - } - } - - // Recompute the min-max bounding box which would be different after the split occurs - m1 = nd::VHACD::Vect3(0x7FFFFFFF); - m2 = nd::VHACD::Vect3(0); - for (auto &i : mSurfaceVoxels) - { - minMaxVoxelRegion(i); - } - for (auto &i : mNewSurfaceVoxels) - { - minMaxVoxelRegion(i); - } - for (auto &i : mInteriorVoxels) - { - minMaxVoxelRegion(i); - } - - buildVoxelMesh(); - buildRaycastMesh(); // build a raycast mesh of the voxel mesh - computeConvexHull(); - } - - // Helper method to refresh the min/max voxel bounding region - void minMaxVoxelRegion(const Voxel &v) - { - uint32_t x, y, z; - v.getVoxel(x, y, z); - m1.getX() = Min(m1.getX(), x); - m2.getX() = Max(m2.getX(), x); - m1.getY() = Min(m1.getY(), y); - m2.getY() = Max(m2.getY(), y); - m1.getZ() = Min(m1.getZ(), z); - m2.getZ() = Max(m2.getZ(), z); - } - + uint32_t splitLoc); // Here we construct the intitial convex hull around the // entire voxel set - VoxelHull(VoxelizeImpl& voxels, - const SimpleMesh &inputMesh, + VoxelHull(Volume& voxels, const IVHACD::Parameters ¶ms, - VHACDCallbacks *callbacks) - : mVoxels(&voxels) - , mParams(params) - , mCallbacks(callbacks) - { - mVoxelHullCount++; - mIndex = mVoxelHullCount; - nd::VHACD::Vect3 dimensions = mVoxels->getDimensions(); + VHACDCallbacks *callbacks); - m2 = dimensions - 1; - - mVoxelScale = mVoxels->getScale(); - mVoxelBmin = mVoxels->getBoundsMin(); - mVoxelBmax = mVoxels->getBoundsMax(); - mVoxelScaleHalf = mVoxelScale * 0.5; - - mVoxelAdjust = mVoxelBmin - mVoxelScaleHalf; - - // Here we get a copy of all voxels which lie on the surface mesh - mVoxels->getSurfaceVoxels(mSurfaceVoxels); - // Now we get a copy of all voxels which are considered part of the 'interior' of the source mesh - mVoxels->getInteriorVoxels(mInteriorVoxels); - buildVoxelMesh(); - buildRaycastMesh(); // build a raycast mesh of the voxel mesh -// saveVoxelMesh(inputMesh,true,false); - computeConvexHull(); - } + ~VoxelHull() = default; - ~VoxelHull(void) - { - if ( mConvexHull ) - { - delete mConvexHull; - mConvexHull = nullptr; - } - delete mHullA; - delete mHullB; - } + // Helper method to refresh the min/max voxel bounding region + void MinMaxVoxelRegion(const Voxel &v); - void buildRaycastMesh(void) - { - // Create a raycast mesh representation of the voxelized surface mesh - if ( !mIndices.empty() ) - { - mRaycastMesh = MyRaycastMesh(mVertices, - mIndices); - } - } + void BuildRaycastMesh(); // We now compute the convex hull relative to a triangle mesh generated // from the voxels - void computeConvexHull(void) - { - if ( !mVertices.empty() ) - { - // we compute the convex hull as follows... - VHACD::QuickHullImpl qh; - uint32_t tcount = qh.computeConvexHull(mVertices, - uint32_t(mVertices.size())); - if ( tcount ) - { - mConvexHull = new IVHACD::ConvexHull; - - const std::vector& vertices = qh.getVertices(); - mConvexHull->m_points.resize(vertices.size()); - std::copy(vertices.begin(), - vertices.end(), - mConvexHull->m_points.begin()); - - const std::vector& indices = qh.getIndices(); - mConvexHull->m_triangles.resize(indices.size()); - std::copy(indices.begin(), - indices.end(), - mConvexHull->m_triangles.begin()); - - VHACD::fm_computeCentroid(mConvexHull->m_points, - mConvexHull->m_triangles, - mConvexHull->m_center); - mConvexHull->m_volume = VHACD::fm_computeMeshVolume(mConvexHull->m_points, - mConvexHull->m_triangles); - } - } - if ( mConvexHull ) - { - mHullVolume = mConvexHull->m_volume; - } - // This is the volume of a single voxel - double singleVoxelVolume = mVoxelScale * mVoxelScale * mVoxelScale; - size_t voxelCount = mInteriorVoxels.size() + mNewSurfaceVoxels.size() + mSurfaceVoxels.size(); - mVoxelVolume = singleVoxelVolume * double(voxelCount); - double diff = fabs(mHullVolume - mVoxelVolume); - mVolumeError = (diff * 100) / mVoxelVolume; - } + void ComputeConvexHull(); // Returns true if this convex hull should be considered done - bool isComplete(void) - { - bool ret = false; - if ( mConvexHull == nullptr ) - { - ret = true; - } - else if ( mVolumeError < mParams.m_minimumVolumePercentErrorAllowed ) - { - ret = true; - } - else if ( mDepth > mParams.m_maxRecursionDepth ) - { - ret = true; - } - else - { - // We compute the voxel width on all 3 axes and see if they are below the min threshold size - nd::VHACD::Vect3 d = m2 - m1; - if ( d.getX() <= mParams.m_minEdgeLength && - d.getY() <= mParams.m_minEdgeLength && - d.getZ() <= mParams.m_minEdgeLength ) - { - ret = true; - } - } - return ret; - } + bool IsComplete(); // Convert a voxel position into it's correct double precision location - inline nd::VHACD::Vect3 getPoint(const int32_t x, - const int32_t y, - const int32_t z, - const double scale, - const nd::VHACD::Vect3& bmin) const - { - return nd::VHACD::Vect3(x * scale + bmin.getX(), - y * scale + bmin.getY(), - z * scale + bmin.getZ()); - } + VHACD::Vect3 GetPoint(const int32_t x, + const int32_t y, + const int32_t z, + const double scale, + const VHACD::Vect3& bmin) const; // Sees if we have already got an index for this voxel position. // If the voxel position has already been indexed, we just return // that index value. // If not, then we convert it into the floating point position and // add it to the index map - inline uint32_t getVertexIndex(const nd::VHACD::Vect3& p) - { - uint32_t ret = 0; - uint32_t address = (p.getX() << 20) | (p.getY() << 10) | p.getZ(); - VoxelIndexMap::iterator found = mVoxelIndexMap.find(address); - if ( found != mVoxelIndexMap.end() ) - { - ret = (*found).second; - } - else - { - nd::VHACD::Vect3 vertex = getPoint(p.getX(), - p.getY(), - p.getZ(), - mVoxelScale, - mVoxelAdjust); - ret = uint32_t(mVoxelIndexMap.size()); - mVoxelIndexMap[address] = ret; - mVertices.emplace_back(vertex); - } - return ret; - } + uint32_t GetVertexIndex(const VHACD::Vector3& p); // This method will convert the voxels into an actual indexed triangle mesh of boxes // This serves two purposes. @@ -6798,807 +5887,858 @@ class VoxelHull // The second reason we convert it into a triangle mesh is so that we can do raycasting against it // to search for the best splitting plane fairly quickly. That algorithm will be discussed in the // method which computes the best splitting plane. - void buildVoxelMesh(void) - { - // When we build the triangle mesh we do *not* need the interior voxels, only the ones - // which lie upon the logical surface of the mesh. - // Each time we perform a plane split, voxels which are along the splitting plane become - // 'new surface voxels'. - - for (auto &i:mSurfaceVoxels) - { - addVoxelBox(i); - } - for (auto &i:mNewSurfaceVoxels) - { - addVoxelBox(i); - } - } + void BuildVoxelMesh(); // Convert a single voxel position into an actual 3d box mesh comprised // of 12 triangles - inline void addVoxelBox(const Voxel &v) - { - // The voxel position of the upper left corner of the box - nd::VHACD::Vect3 bmin(v.getX(), - v.getY(), - v.getZ()); - // The voxel position of the lower right corner of the box - nd::VHACD::Vect3 bmax(bmin.getX() + 1, - bmin.getY() + 1, - bmin.getZ() + 1); - - // Build the set of 8 voxel positions representing - // the coordinates of the box - nd::VHACD::Vect3 box[8]; - - box[0] = nd::VHACD::Vect3(bmin.getX(), bmin.getY(), bmin.getZ()); - box[1] = nd::VHACD::Vect3(bmax.getX(), bmin.getY(), bmin.getZ()); - box[2] = nd::VHACD::Vect3(bmax.getX(), bmax.getY(), bmin.getZ()); - box[3] = nd::VHACD::Vect3(bmin.getX(), bmax.getY(), bmin.getZ()); - box[4] = nd::VHACD::Vect3(bmin.getX(), bmin.getY(), bmax.getZ()); - box[5] = nd::VHACD::Vect3(bmax.getX(), bmin.getY(), bmax.getZ()); - box[6] = nd::VHACD::Vect3(bmax.getX(), bmax.getY(), bmax.getZ()); - box[7] = nd::VHACD::Vect3(bmin.getX(), bmax.getY(), bmax.getZ()); + void AddVoxelBox(const Voxel &v); + + // Add the triangle represented by these 3 indices into the 'box' set of vertices + // to the output mesh + void AddTri(const std::array, 8>& box, + uint32_t i1, + uint32_t i2, + uint32_t i3); - // Now add the 12 triangles comprising the 3d box - addTri(box, 2, 1, 0); - addTri(box, 3, 2, 0); + // Here we convert from voxel space to a 3d position, index it, and add + // the triangle positions and indices for the output mesh + void AddTriangle(const VHACD::Vector3& p1, + const VHACD::Vector3& p2, + const VHACD::Vector3& p3); - addTri(box, 7, 2, 3); - addTri(box, 7, 6, 2); + // When computing the split plane, we start by simply + // taking the midpoint of the longest side. However, + // we can also search the surface and look for the greatest + // spot of concavity and use that as the split location. + // This will make the convex decomposition more efficient + // as it will tend to cut across the greatest point of + // concavity on the surface. + SplitAxis ComputeSplitPlane(uint32_t& location); - addTri(box, 5, 1, 2); - addTri(box, 5, 2, 6); + VHACD::Vect3 GetPosition(const VHACD::Vector3& ip) const; - addTri(box, 5, 4, 1); - addTri(box, 4, 0, 1); + double Raycast(const VHACD::Vector3& p1, + const VHACD::Vector3& p2) const; - addTri(box, 4, 6, 7); - addTri(box, 4, 5, 6); + bool FindConcavity(uint32_t idx, + uint32_t& splitLoc); - addTri(box, 4, 7, 0); - addTri(box, 7, 3, 0); - } + // Finding the greatest area of concavity.. + bool FindConcavityX(uint32_t& splitLoc); - - // Add the triangle represented by these 3 indices into the 'box' set of vertices - // to the output mesh - inline void addTri(const nd::VHACD::Vect3* box, - uint32_t i1, - uint32_t i2, - uint32_t i3) - { - addTriangle(box[i1], box[i2], box[i3]); - } + // Finding the greatest area of concavity.. + bool FindConcavityY(uint32_t& splitLoc); - // Here we convert from voxel space to a 3d position, index it, and add - // the triangle positions and indices for the output mesh - inline void addTriangle(const nd::VHACD::Vect3& p1, - const nd::VHACD::Vect3& p2, - const nd::VHACD::Vect3& p3) - { - uint32_t i1 = getVertexIndex(p1); - uint32_t i2 = getVertexIndex(p2); - uint32_t i3 = getVertexIndex(p3); + // Finding the greatest area of concavity.. + bool FindConcavityZ(uint32_t& splitLoc); - mIndices.emplace_back(i1, i2, i3); - } + // This operation is performed in a background thread. + // It splits the voxels by a plane + void PerformPlaneSplit(); // Used only for debugging. Saves the voxelized mesh to disk // Optionally saves the original source mesh as well for comparison - void saveVoxelMesh(const SimpleMesh &inputMesh, + void SaveVoxelMesh(const SimpleMesh& inputMesh, bool saveVoxelMesh, - bool saveSourceMesh) + bool saveSourceMesh); + + void SaveOBJ(const char* fname, + const VoxelHull* h); + + void SaveOBJ(const char* fname); + +private: + void WriteOBJ(FILE* fph, + const std::vector& vertices, + const std::vector& indices, + uint32_t baseIndex); +public: + + SplitAxis m_axis{ SplitAxis::X_AXIS_NEGATIVE }; + Volume* m_voxels{ nullptr }; // The voxelized data set + double m_voxelScale{ 0 }; // Size of a single voxel + double m_voxelScaleHalf{ 0 }; // 1/2 of the size of a single voxel + VHACD::BoundsAABB m_voxelBounds; + VHACD::Vect3 m_voxelAdjust; // Minimum coordinates of the voxel space, with adjustment + uint32_t m_depth{ 0 }; // How deep in the recursion of the binary tree this hull is + uint32_t m_index{ 0 }; // Each convex hull is given a unique id to distinguish it from the others + double m_volumeError{ 0 }; // The percentage error from the convex hull volume vs. the voxel volume + double m_voxelVolume{ 0 }; // The volume of the voxels + double m_hullVolume{ 0 }; // The volume of the enclosing convex hull + + std::unique_ptr m_convexHull{ nullptr }; // The convex hull which encloses this set of voxels. + std::vector m_surfaceVoxels; // The voxels which are on the surface of the source mesh. + std::vector m_newSurfaceVoxels; // Voxels which are on the surface as a result of a plane split + std::vector m_interiorVoxels; // Voxels which are part of the interior of the hull + + std::unique_ptr m_hullA{ nullptr }; // hull resulting from one side of the plane split + std::unique_ptr m_hullB{ nullptr }; // hull resulting from the other side of the plane split + + // Defines the coordinates this convex hull comprises within the voxel volume + // of the entire source + VHACD::Vector3 m_1{ 0 }; + VHACD::Vector3 m_2{ 0 }; + AABBTree m_AABBTree; + std::unordered_map m_voxelIndexMap; // Maps from a voxel coordinate space into a vertex index space + std::vector m_vertices; + std::vector m_indices; + static uint32_t m_voxelHullCount; + IVHACD::Parameters m_params; + VHACDCallbacks* m_callbacks{ nullptr }; +}; + +uint32_t VoxelHull::m_voxelHullCount = 0; + +VoxelHull::VoxelHull(const VoxelHull& parent, + SplitAxis axis, + uint32_t splitLoc) + : m_axis(axis) + , m_voxels(parent.m_voxels) + , m_voxelScale(m_voxels->GetScale()) + , m_voxelScaleHalf(m_voxelScale * double(0.5)) + , m_voxelBounds(m_voxels->GetBounds()) + , m_voxelAdjust(m_voxelBounds.GetMin() - m_voxelScaleHalf) + , m_depth(parent.m_depth + 1) + , m_index(++m_voxelHullCount) + , m_1(parent.m_1) + , m_2(parent.m_2) + , m_params(parent.m_params) +{ + // Default copy the voxel region from the parent, but values will + // be adjusted next based on the split axis and location + switch ( m_axis ) { - char scratch[512]; - snprintf(scratch,sizeof(scratch),"voxel-mesh-%03d.obj", mIndex); - FILE *fph = fopen(scratch,"wb"); - if ( fph ) - { - uint32_t baseIndex = 1; - if ( saveVoxelMesh ) - { - for (size_t i = 0; i < mVertices.size(); i++) - { - const VHACD::Vertex& p = mVertices[i]; - fprintf(fph, - "v %0.9f %0.9f %0.9f\n", - p.mX, - p.mY, - p.mZ); - baseIndex++; - } - for (size_t i = 0; i < mIndices.size(); i++) - { - const VHACD::Triangle& t = mIndices[i]; - fprintf(fph, - "f %d %d %d\n", - t.mI0 + 1, - t.mI1 + 1, - t.mI2 + 1); - } - } - if ( saveSourceMesh ) - { - for (uint32_t i = 0; i < inputMesh.mVertices.size(); i++) - { - const VHACD::Vertex& p = inputMesh.mVertices[i]; - fprintf(fph, - "v %0.9f %0.9f %0.9f\n", - p.mX, - p.mY, - p.mZ); - } - for (uint32_t i = 0; i < inputMesh.mIndices.size(); i++) - { - const VHACD::Triangle& idx = inputMesh.mIndices[i]; - fprintf(fph, - "f %d %d %d\n", - idx.mI0 + baseIndex, - idx.mI1 + baseIndex, - idx.mI2 + baseIndex); - } - } - fclose(fph); - } + case SplitAxis::X_AXIS_NEGATIVE: + m_2.GetX() = splitLoc; + break; + case SplitAxis::X_AXIS_POSITIVE: + m_1.GetX() = splitLoc + 1; + break; + case SplitAxis::Y_AXIS_NEGATIVE: + m_2.GetY() = splitLoc; + break; + case SplitAxis::Y_AXIS_POSITIVE: + m_1.GetY() = splitLoc + 1; + break; + case SplitAxis::Z_AXIS_NEGATIVE: + m_2.GetZ() = splitLoc; + break; + case SplitAxis::Z_AXIS_POSITIVE: + m_1.GetZ() = splitLoc + 1; + break; } - // When computing the split plane, we start by simply - // taking the midpoint of the longest side. However, - // we can also search the surface and look for the greatest - // spot of concavity and use that as the split location. - // This will make the convex decomposition more efficient - // as it will tend to cut across the greatest point of - // concavity on the surface. - SplitAxis computeSplitPlane(uint32_t &location) + // First, we copy all of the interior voxels from our parent + // which intersect our region + for (auto& i : parent.m_interiorVoxels) { - SplitAxis ret = SplitAxis::X_AXIS_NEGATIVE; - - nd::VHACD::Vect3 d = m2 - m1; - - if ( d.getX() >= d.getY() && d.getX() >= d.getZ() ) + VHACD::Vector3 v = i.GetVoxel(); + if (v.CWiseAllGE(m_1) && v.CWiseAllLE(m_2)) { - ret = SplitAxis::X_AXIS_NEGATIVE; - location = (m2.getX() + 1 + m1.getX()) / 2; - uint32_t edgeLoc; - if ( mParams.m_findBestPlane && findConcavityX(edgeLoc) ) + bool newSurface = false; + switch ( m_axis ) { - location = edgeLoc; + case SplitAxis::X_AXIS_NEGATIVE: + if ( v.GetX() == splitLoc ) + { + newSurface = true; + } + break; + case SplitAxis::X_AXIS_POSITIVE: + if ( v.GetX() == m_1.GetX() ) + { + newSurface = true; + } + break; + case SplitAxis::Y_AXIS_NEGATIVE: + if ( v.GetY() == splitLoc ) + { + newSurface = true; + } + break; + case SplitAxis::Y_AXIS_POSITIVE: + if ( v.GetY() == m_1.GetY() ) + { + newSurface = true; + } + break; + case SplitAxis::Z_AXIS_NEGATIVE: + if ( v.GetZ() == splitLoc ) + { + newSurface = true; + } + break; + case SplitAxis::Z_AXIS_POSITIVE: + if ( v.GetZ() == m_1.GetZ() ) + { + newSurface = true; + } + break; } - } - else if ( d.getY() >= d.getX() && d.getY() >= d.getZ() ) - { - ret = SplitAxis::Y_AXIS_NEGATIVE; - location = (m2.getY() + 1 + m1.getY()) / 2; - uint32_t edgeLoc; - if ( mParams.m_findBestPlane && findConcavityY(edgeLoc) ) + // If his interior voxels lie directly on the split plane then + // these become new surface voxels for our patch + if ( newSurface ) { - location = edgeLoc; + m_newSurfaceVoxels.push_back(i); } - } - else - { - ret = SplitAxis::Z_AXIS_NEGATIVE; - location = (m2.getZ() + 1 + m1.getZ()) / 2; - uint32_t edgeLoc; - if ( mParams.m_findBestPlane && findConcavityZ(edgeLoc) ) + else { - location = edgeLoc; + m_interiorVoxels.push_back(i); } } - - return ret; } - - inline nd::VHACD::Vect3 getPosition(const nd::VHACD::Vect3& ip) const - { - return getPoint(ip.getX(), - ip.getY(), - ip.getZ(), - mVoxelScale, - mVoxelAdjust); - } - - double raycast(const nd::VHACD::Vect3& p1, - const nd::VHACD::Vect3& p2) const + // Next we copy all of the surface voxels which intersect our region + for (auto& i : parent.m_surfaceVoxels) { - double ret; - nd::VHACD::Vect3 from = getPosition(p1); - nd::VHACD::Vect3 to = getPosition(p2); - - double outT; - double faceSign; - nd::VHACD::Vect3 hitLocation; - if ( mRaycastMesh.raycast(from, to, outT, faceSign, hitLocation) ) + VHACD::Vector3 v = i.GetVoxel(); + if (v.CWiseAllGE(m_1) && v.CWiseAllLE(m_2)) { - ret = (from - hitLocation).GetNorm(); + m_surfaceVoxels.push_back(i); } - else + } + // Our parent's new surface voxels become our new surface voxels so long as they intersect our region + for (auto& i : parent.m_newSurfaceVoxels) + { + VHACD::Vector3 v = i.GetVoxel(); + if (v.CWiseAllGE(m_1) && v.CWiseAllLE(m_2)) { - ret = 0; // if it doesn't hit anything, just assign it to zero. + m_newSurfaceVoxels.push_back(i); } - - return ret; } -// bool findConcavity(uint32_t idx, -// uint32_t &splitLoc) -// { -// bool ret = false; -// -// int32_t d = (m2[idx] - m1[idx]) + 1; // The length of the getX axis in voxel space -// -// uint32_t idx1; -// uint32_t idx2; -// uint32_t idx3; -// switch (idx) -// { -// case 0: // X -// idx1 = 0; -// idx2 = 1; -// idx3 = 2; -// break; -// case 1: // Y -// idx1 = 1; -// idx2 = 0; -// idx3 = 2; -// break; -// case 2: -// idx1 = 2; -// idx2 = 1; -// idx3 = 0; -// break; -// } -// -// // We will compute the edge error on the XY plane and the XZ plane -// // searching for the greatest location of concavity -// std::vector edgeError1 = std::vector(d); -// std::vector edgeError2 = std::vector(d); -// -// // Counter of number of voxel samples on the XY plane we have accumulated -// uint32_t indexZ = 0; -// -// // Compute Edge Error on the XY plane -// for (int32_t x=(int32_t)m1[idx1]; x<=(int32_t)m2[idx1]; x++) -// { -// double errorTotal = 0; -// // We now perform a raycast from the sides inward on the XY plane to -// // determine the total error (distance of the surface from the sides) -// // along this getX position. -// for (int32_t y=(int32_t)m1[idx2]; y<=(int32_t)m2[idx2]; y++) -// { -// IVec3 p1(x, y, m1[idx3] - 2); -// IVec3 p2(x, y, m2[idx3] + 2); -// -// double e1 = raycast(p1, p2); -// double e2 = raycast(p2, p1); -// -// errorTotal = errorTotal+e1+e2; -// } -// // The total amount of edge error along this voxel location -// edgeError1[indexZ] = errorTotal; -// indexZ++; -// } -// -// // Compute edge error along the XZ plane -// uint32_t indexY = 0; -// -// for (int32_t x=(int32_t)m1[idx1]; x<=(int32_t)m2[idx1]; x++) -// { -// double errorTotal = 0; -// -// for (int32_t z=(int32_t)m1[idx3]; z<=(int32_t)m2[idx3]; z++) -// { -// IVec3 p1(x, m1[idx2] - 2, z); -// IVec3 p2(x, m2[idx2] + 2, z); -// -// double e1 = raycast(p1, p2); // raycast from one side to the interior -// double e2 = raycast(p2, p1); // raycast from the other side to the interior -// -// errorTotal = errorTotal+e1+e2; -// } -// edgeError2[indexY] = errorTotal; -// indexY++; -// } -// -// -// // we now compute the first derivitave to find the greatest spot of concavity on the XY plane -// double maxDiff = 0; -// uint32_t maxC = 0; -// int32_t wid = (m2[idx1] - m1[idx1]) / 2 + 1; -// for (uint32_t x=1; x 0 && edgeError1[x-1] > 0 ) -// { -// double diff = abs(edgeError1[x] - edgeError1[x-1]); -// if ( diff > maxDiff ) -// { -// maxDiff = diff; -// maxC = x-1; -// } -// } -// } -// -// // Now see if there is a greater concavity on the XZ plane -// for (uint32_t x=1; x 0 && edgeError2[x-1] > 0 ) -// { -// double diff = abs(edgeError2[x] - edgeError2[x-1]); -// if ( diff > maxDiff ) -// { -// maxDiff = diff; -// maxC = x-1; -// } -// } -// } -// -// splitLoc = maxC + m1[idx1]; -// -// // we do not allow an edge split if it is too close to the ends -// if ( splitLoc > (m1[idx1] + 4) && splitLoc < (m2[idx1] - 4) ) -// { -// ret = true; -// } -// -// -// return ret; -// } - - // Finding the greatest area of concavity.. - bool findConcavityX(uint32_t &splitLoc) + // Recompute the min-max bounding box which would be different after the split occurs + m_1 = VHACD::Vector3(0x7FFFFFFF); + m_2 = VHACD::Vector3(0); + for (auto& i : m_surfaceVoxels) { -// return findConcavity(0, splitLoc); - bool ret = false; - - uint32_t dx = (m2.getX() - m1.getX()) + 1; // The length of the getX axis in voxel space - - // We will compute the edge error on the XY plane and the XZ plane - // searching for the greatest location of concavity - - std::vector edgeErrorZ = std::vector(dx); - std::vector edgeErrorY = std::vector(dx); - - // Counter of number of voxel samples on the XY plane we have accumulated - uint32_t indexZ = 0; + MinMaxVoxelRegion(i); + } + for (auto& i : m_newSurfaceVoxels) + { + MinMaxVoxelRegion(i); + } + for (auto& i : m_interiorVoxels) + { + MinMaxVoxelRegion(i); + } - // Compute Edge Error on the XY plane - for (uint32_t x = m1.getX(); x <= m2.getX(); x++) - { - double errorTotal = 0; - // We now perform a raycast from the sides inward on the XY plane to - // determine the total error (distance of the surface from the sides) - // along this getX position. - for (uint32_t y = m1.getY(); y <= m2.getY(); y++) - { - nd::VHACD::Vect3 p1(x, y, m1.getZ() - 2); - nd::VHACD::Vect3 p2(x, y, m2.getZ() + 2); + BuildVoxelMesh(); + BuildRaycastMesh(); // build a raycast mesh of the voxel mesh + ComputeConvexHull(); +} - double e1 = raycast(p1, p2); - double e2 = raycast(p2, p1); +VoxelHull::VoxelHull(Volume& voxels, + const IVHACD::Parameters& params, + VHACDCallbacks* callbacks) + : m_voxels(&voxels) + , m_voxelScale(m_voxels->GetScale()) + , m_voxelScaleHalf(m_voxelScale * double(0.5)) + , m_voxelBounds(m_voxels->GetBounds()) + , m_voxelAdjust(m_voxelBounds.GetMin() - m_voxelScaleHalf) + , m_index(++m_voxelHullCount) + // Here we get a copy of all voxels which lie on the surface mesh + , m_surfaceVoxels(m_voxels->GetSurfaceVoxels()) + // Now we get a copy of all voxels which are considered part of the 'interior' of the source mesh + , m_interiorVoxels(m_voxels->GetInteriorVoxels()) + , m_2(m_voxels->GetDimensions() - 1) + , m_params(params) + , m_callbacks(callbacks) +{ + BuildVoxelMesh(); + BuildRaycastMesh(); // build a raycast mesh of the voxel mesh + ComputeConvexHull(); +} - errorTotal = errorTotal + e1 + e2; - } - // The total amount of edge error along this voxel location - edgeErrorZ[indexZ] = errorTotal; - indexZ++; - } +void VoxelHull::MinMaxVoxelRegion(const Voxel& v) +{ + VHACD::Vector3 x = v.GetVoxel(); + m_1 = m_1.CWiseMin(x); + m_2 = m_2.CWiseMax(x); +} - // Compute edge error along the XZ plane - uint32_t indexY = 0; +void VoxelHull::BuildRaycastMesh() +{ + // Create a raycast mesh representation of the voxelized surface mesh + if ( !m_indices.empty() ) + { + m_AABBTree = AABBTree(m_vertices, + m_indices); + } +} - for (uint32_t x = m1.getX(); x <= m2.getX(); x++) +void VoxelHull::ComputeConvexHull() +{ + if ( !m_vertices.empty() ) + { + // we compute the convex hull as follows... + VHACD::QuickHull qh; + uint32_t tcount = qh.ComputeConvexHull(m_vertices, + uint32_t(m_vertices.size())); + if ( tcount ) { - double errorTotal = 0; - - for (uint32_t z = m1.getZ(); z <= m2.getZ(); z++) - { - nd::VHACD::Vect3 p1(x, m1.getY() - 2, z); - nd::VHACD::Vect3 p2(x, m2.getY() + 2, z); - - double e1 = raycast(p1, p2); // raycast from one side to the interior - double e2 = raycast(p2, p1); // raycast from the other side to the interior - - errorTotal = errorTotal + e1 + e2; - } - edgeErrorY[indexY] = errorTotal; - indexY++; - } + m_convexHull = std::unique_ptr(new IVHACD::ConvexHull); + m_convexHull->m_points = qh.GetVertices(); + m_convexHull->m_triangles = qh.GetIndices(); - // we now compute the first derivitave to find the greatest spot of concavity on the XY plane - double maxDiff = 0; - uint32_t maxC = 0; - for (uint32_t x = 1; x < indexZ; x++) - { - if ( edgeErrorZ[x] > 0 && edgeErrorZ[x - 1] > 0 ) - { - double diff = abs(edgeErrorZ[x] - edgeErrorZ[x - 1]); - if ( diff > maxDiff ) - { - maxDiff = diff; - maxC = x-1; - } - } - } - // Now see if there is a greater concavity on the XZ plane - for (uint32_t x = 1; x < indexY; x++) - { - if ( edgeErrorY[x] > 0 && edgeErrorY[x - 1] > 0 ) - { - double diff = abs(edgeErrorY[x] - edgeErrorY[x - 1]); - if ( diff > maxDiff ) - { - maxDiff = diff; - maxC = x-1; - } - } + VHACD::ComputeCentroid(m_convexHull->m_points, + m_convexHull->m_triangles, + m_convexHull->m_center); + m_convexHull->m_volume = VHACD::ComputeMeshVolume(m_convexHull->m_points, + m_convexHull->m_triangles); } + } + if ( m_convexHull ) + { + m_hullVolume = m_convexHull->m_volume; + } + // This is the volume of a single voxel + double singleVoxelVolume = m_voxelScale * m_voxelScale * m_voxelScale; + size_t voxelCount = m_interiorVoxels.size() + m_newSurfaceVoxels.size() + m_surfaceVoxels.size(); + m_voxelVolume = singleVoxelVolume * double(voxelCount); + double diff = fabs(m_hullVolume - m_voxelVolume); + m_volumeError = (diff * 100) / m_voxelVolume; +} - splitLoc = maxC + m1.getX(); - - // we do not allow an edge split if it is too close to the ends - if ( splitLoc > (m1.getX() + 4) && splitLoc < (m2.getX() - 4) ) +bool VoxelHull::IsComplete() +{ + bool ret = false; + if ( m_convexHull == nullptr ) + { + ret = true; + } + else if ( m_volumeError < m_params.m_minimumVolumePercentErrorAllowed ) + { + ret = true; + } + else if ( m_depth > m_params.m_maxRecursionDepth ) + { + ret = true; + } + else + { + // We compute the voxel width on all 3 axes and see if they are below the min threshold size + VHACD::Vector3 d = m_2 - m_1; + if ( d.GetX() <= m_params.m_minEdgeLength && + d.GetY() <= m_params.m_minEdgeLength && + d.GetZ() <= m_params.m_minEdgeLength ) { ret = true; } - - return ret; } + return ret; +} +VHACD::Vect3 VoxelHull::GetPoint(const int32_t x, + const int32_t y, + const int32_t z, + const double scale, + const VHACD::Vect3& bmin) const +{ + return VHACD::Vect3(x * scale + bmin.GetX(), + y * scale + bmin.GetY(), + z * scale + bmin.GetZ()); +} - // Finding the greatest area of concavity.. - bool findConcavityY(uint32_t &splitLoc) +uint32_t VoxelHull::GetVertexIndex(const VHACD::Vector3& p) +{ + uint32_t ret = 0; + uint32_t address = (p.GetX() << 20) | (p.GetY() << 10) | p.GetZ(); + auto found = m_voxelIndexMap.find(address); + if ( found != m_voxelIndexMap.end() ) { -// return findConcavity(1, splitLoc); - bool ret = false; + ret = found->second; + } + else + { + VHACD::Vect3 vertex = GetPoint(p.GetX(), + p.GetY(), + p.GetZ(), + m_voxelScale, + m_voxelAdjust); + ret = uint32_t(m_voxelIndexMap.size()); + m_voxelIndexMap[address] = ret; + m_vertices.emplace_back(vertex); + } + return ret; +} - uint32_t dy = (m2.getY() - m1.getY()) + 1; // The length of the getX axis in voxel space +void VoxelHull::BuildVoxelMesh() +{ + // When we build the triangle mesh we do *not* need the interior voxels, only the ones + // which lie upon the logical surface of the mesh. + // Each time we perform a plane split, voxels which are along the splitting plane become + // 'new surface voxels'. - // We will compute the edge error on the XY plane and the XZ plane - // searching for the greatest location of concavity + for (auto& i : m_surfaceVoxels) + { + AddVoxelBox(i); + } + for (auto& i : m_newSurfaceVoxels) + { + AddVoxelBox(i); + } +} - std::vector edgeErrorZ = std::vector(dy); - std::vector edgeErrorX = std::vector(dy); +void VoxelHull::AddVoxelBox(const Voxel &v) +{ + // The voxel position of the upper left corner of the box + VHACD::Vector3 bmin(v.GetX(), + v.GetY(), + v.GetZ()); + // The voxel position of the lower right corner of the box + VHACD::Vector3 bmax(bmin.GetX() + 1, + bmin.GetY() + 1, + bmin.GetZ() + 1); + + // Build the set of 8 voxel positions representing + // the coordinates of the box + std::array, 8> box{{ + { bmin.GetX(), bmin.GetY(), bmin.GetZ() }, + { bmax.GetX(), bmin.GetY(), bmin.GetZ() }, + { bmax.GetX(), bmax.GetY(), bmin.GetZ() }, + { bmin.GetX(), bmax.GetY(), bmin.GetZ() }, + { bmin.GetX(), bmin.GetY(), bmax.GetZ() }, + { bmax.GetX(), bmin.GetY(), bmax.GetZ() }, + { bmax.GetX(), bmax.GetY(), bmax.GetZ() }, + { bmin.GetX(), bmax.GetY(), bmax.GetZ() } + }}; + + // Now add the 12 triangles comprising the 3d box + AddTri(box, 2, 1, 0); + AddTri(box, 3, 2, 0); + + AddTri(box, 7, 2, 3); + AddTri(box, 7, 6, 2); + + AddTri(box, 5, 1, 2); + AddTri(box, 5, 2, 6); + + AddTri(box, 5, 4, 1); + AddTri(box, 4, 0, 1); + + AddTri(box, 4, 6, 7); + AddTri(box, 4, 5, 6); + + AddTri(box, 4, 7, 0); + AddTri(box, 7, 3, 0); +} - // Counter of number of voxel samples on the XY plane we have accumulated - uint32_t indexZ = 0; +void VoxelHull::AddTri(const std::array, 8>& box, + uint32_t i1, + uint32_t i2, + uint32_t i3) +{ + AddTriangle(box[i1], box[i2], box[i3]); +} - // Compute Edge Error on the XY plane - for (uint32_t y = m1.getY(); y <= m2.getY(); y++) - { - double errorTotal = 0; - // We now perform a raycast from the sides inward on the XY plane to - // determine the total error (distance of the surface from the sides) - // along this getX position. - for (uint32_t x = m1.getX(); x <= m2.getX(); x++) - { - nd::VHACD::Vect3 p1(x, y, m1.getZ() - 2); - nd::VHACD::Vect3 p2(x, y, m2.getZ() + 2); +void VoxelHull::AddTriangle(const VHACD::Vector3& p1, + const VHACD::Vector3& p2, + const VHACD::Vector3& p3) +{ + uint32_t i1 = GetVertexIndex(p1); + uint32_t i2 = GetVertexIndex(p2); + uint32_t i3 = GetVertexIndex(p3); - double e1 = raycast(p1, p2); - double e2 = raycast(p2, p1); + m_indices.emplace_back(i1, i2, i3); +} - errorTotal = errorTotal + e1 + e2; - } - // The total amount of edge error along this voxel location - edgeErrorZ[indexZ] = errorTotal; - indexZ++; - } +SplitAxis VoxelHull::ComputeSplitPlane(uint32_t& location) +{ + SplitAxis ret = SplitAxis::X_AXIS_NEGATIVE; - // Compute edge error along the XZ plane - uint32_t indexX = 0; + VHACD::Vector3 d = m_2 - m_1; - for (uint32_t y = m1.getY(); y <= m2.getY(); y++) + if ( d.GetX() >= d.GetY() && d.GetX() >= d.GetZ() ) + { + ret = SplitAxis::X_AXIS_NEGATIVE; + location = (m_2.GetX() + 1 + m_1.GetX()) / 2; + uint32_t edgeLoc; + if ( m_params.m_findBestPlane && FindConcavityX(edgeLoc) ) { - double errorTotal = 0; - - for (uint32_t z = m1.getZ(); z <= m2.getZ(); z++) - { - nd::VHACD::Vect3 p1(m1.getX() - 2, y, z); - nd::VHACD::Vect3 p2(m2.getX() + 2, y, z); - - double e1 = raycast(p1, p2); // raycast from one side to the interior - double e2 = raycast(p2, p1); // raycast from the other side to the interior - - errorTotal = errorTotal + e1 + e2; - } - edgeErrorX[indexX] = errorTotal; - indexX++; + location = edgeLoc; } - - // we now compute the first derivitave to find the greatest spot of concavity on the XY plane - double maxDiff = 0; - uint32_t maxC = 0; - for (uint32_t y = 1; y < indexZ; y++) + } + else if ( d.GetY() >= d.GetX() && d.GetY() >= d.GetZ() ) + { + ret = SplitAxis::Y_AXIS_NEGATIVE; + location = (m_2.GetY() + 1 + m_1.GetY()) / 2; + uint32_t edgeLoc; + if ( m_params.m_findBestPlane && FindConcavityY(edgeLoc) ) { - if ( edgeErrorZ[y] > 0 && edgeErrorZ[y - 1] > 0 ) - { - double diff = abs(edgeErrorZ[y] - edgeErrorZ[y - 1]); - if ( diff > maxDiff ) - { - maxDiff = diff; - maxC = y-1; - } - } + location = edgeLoc; } - // Now see if there is a greater concavity on the XZ plane - for (uint32_t y = 1; y < indexX; y++) + } + else + { + ret = SplitAxis::Z_AXIS_NEGATIVE; + location = (m_2.GetZ() + 1 + m_1.GetZ()) / 2; + uint32_t edgeLoc; + if ( m_params.m_findBestPlane && FindConcavityZ(edgeLoc) ) { - if ( edgeErrorX[y] >0 && edgeErrorX[y - 1] > 0 ) - { - double diff = abs(edgeErrorX[y] - edgeErrorX[y - 1]); - if ( diff > maxDiff ) - { - maxDiff = diff; - maxC = y - 1; - } - } + location = edgeLoc; } + } + + return ret; +} - splitLoc = maxC + m1.getY(); +VHACD::Vect3 VoxelHull::GetPosition(const VHACD::Vector3& ip) const +{ + return GetPoint(ip.GetX(), + ip.GetY(), + ip.GetZ(), + m_voxelScale, + m_voxelAdjust); +} - // we do not allow an edge split if it is too close to the ends - if ( splitLoc > (m1.getY() + 4) && splitLoc < (m2.getY() - 4) ) - { - ret = true; - } +double VoxelHull::Raycast(const VHACD::Vector3& p1, + const VHACD::Vector3& p2) const +{ + double ret; + VHACD::Vect3 from = GetPosition(p1); + VHACD::Vect3 to = GetPosition(p2); - return ret; + double outT; + double faceSign; + VHACD::Vect3 hitLocation; + if (m_AABBTree.TraceRay(from, to, outT, faceSign, hitLocation)) + { + ret = (from - hitLocation).GetNorm(); } - - // Finding the greatest area of concavity.. - bool findConcavityZ(uint32_t &splitLoc) + else { -// return findConcavity(2, splitLoc); - bool ret = false; - - int32_t dz = (m2.getZ() - m1.getZ()) + 1; // The length of the getX axis in voxel space + ret = 0; // if it doesn't hit anything, just assign it to zero. + } - // We will compute the edge error on the XY plane and the XZ plane - // searching for the greatest location of concavity - std::vector edgeErrorX = std::vector(dz); - std::vector edgeErrorY = std::vector(dz); + return ret; +} - // Counter of number of voxel samples on the XY plane we have accumulated - uint32_t indexX = 0; +bool VoxelHull::FindConcavity(uint32_t idx, + uint32_t& splitLoc) +{ + bool ret = false; - // Compute Edge Error on the XY plane - for (uint32_t z = m1.getZ(); z <= m2.getZ(); z++) - { - double errorTotal = 0; - // We now perform a raycast from the sides inward on the XY plane to - // determine the total error (distance of the surface from the sides) - // along this getX position. - for (uint32_t y = m1.getY(); y <= m2.getY(); y++) - { - nd::VHACD::Vect3 p1(m1.getX() - 2, y, z); - nd::VHACD::Vect3 p2(m2.getX() + 1, y, z); + int32_t d = (m_2[idx] - m_1[idx]) + 1; // The length of the getX axis in voxel space - double e1 = raycast(p1, p2); - double e2 = raycast(p2, p1); + uint32_t idx1; + uint32_t idx2; + uint32_t idx3; + switch (idx) + { + case 0: // X + idx1 = 0; + idx2 = 1; + idx3 = 2; + break; + case 1: // Y + idx1 = 1; + idx2 = 0; + idx3 = 2; + break; + case 2: + idx1 = 2; + idx2 = 1; + idx3 = 0; + break; + default: + /* + * To silence uninitialized variable warnings + */ + idx1 = 0; + idx2 = 0; + idx3 = 0; + assert(0 && "findConcavity::idx must be 0, 1, or 2"); + break; + } - errorTotal = errorTotal + e1 + e2; - } - // The total amount of edge error along this voxel location - edgeErrorX[indexX] = errorTotal; - indexX++; - } + // We will compute the edge error on the XY plane and the XZ plane + // searching for the greatest location of concavity + std::vector edgeError1 = std::vector(d); + std::vector edgeError2 = std::vector(d); - // Compute edge error along the XZ plane - uint32_t indexY = 0; + // Counter of number of voxel samples on the XY plane we have accumulated + uint32_t index1 = 0; - for (uint32_t z = m1.getZ(); z <= m2.getZ(); z++) + // Compute Edge Error on the XY plane + for (uint32_t i0 = m_1[idx1]; i0 <= m_2[idx1]; i0++) + { + double errorTotal = 0; + // We now perform a raycast from the sides inward on the XY plane to + // determine the total error (distance of the surface from the sides) + // along this getX position. + for (uint32_t i1 = m_1[idx2]; i1 <= m_2[idx2]; i1++) { - double errorTotal = 0; - - for (uint32_t x = m1.getX(); x <= m2.getX(); x++) + VHACD::Vector3 p1; + VHACD::Vector3 p2; + switch (idx) { - nd::VHACD::Vect3 p1(x, m1.getY() - 2, z); - nd::VHACD::Vect3 p2(x, m2.getY() + 2, z); + case 0: + { + p1 = VHACD::Vector3(i0, i1, m_1.GetZ() - 2); + p2 = VHACD::Vector3(i0, i1, m_2.GetZ() + 2); + break; + } + case 1: + { + p1 = VHACD::Vector3(i1, i0, m_1.GetZ() - 2); + p2 = VHACD::Vector3(i1, i0, m_2.GetZ() + 2); + break; + } + case 2: + { + p1 = VHACD::Vector3(m_1.GetX() - 2, i1, i0); + p2 = VHACD::Vector3(m_2.GetX() + 2, i1, i0); + break; + } + } - double e1 = raycast(p1, p2); // raycast from one side to the interior - double e2 = raycast(p2, p1); // raycast from the other side to the interior + double e1 = Raycast(p1, p2); + double e2 = Raycast(p2, p1); - errorTotal = errorTotal + e1 + e2; - } - edgeErrorY[indexY] = errorTotal; - indexY++; + errorTotal = errorTotal + e1 + e2; } + // The total amount of edge error along this voxel location + edgeError1[index1] = errorTotal; + index1++; + } + + // Compute edge error along the XZ plane + uint32_t index2 = 0; + for (uint32_t i0 = m_1[idx1]; i0 <= m_2[idx1]; i0++) + { + double errorTotal = 0; - // we now compute the first derivitave to find the greatest spot of concavity on the XY plane - double maxDiff = 0; - uint32_t maxC = 0; - for (uint32_t z = 1; z < indexX; z++) + for (uint32_t i1 = m_1[idx3]; i1 <= m_2[idx3]; i1++) { - if ( edgeErrorX[z] > 0 && edgeErrorX[z - 1] > 0 ) + VHACD::Vector3 p1; + VHACD::Vector3 p2; + switch (idx) { - double diff = abs(edgeErrorX[z] - edgeErrorX[z - 1]); - if ( diff > maxDiff ) + case 0: { - maxDiff = diff; - maxC = z - 1; + p1 = VHACD::Vector3(i0, m_1.GetY() - 2, i1); + p2 = VHACD::Vector3(i0, m_2.GetY() + 2, i1); + break; } - } - } - // Now see if there is a greater concavity on the XZ plane - for (uint32_t z=1; z 0 && edgeErrorY[z - 1] > 0 ) - { - double diff = abs(edgeErrorY[z] - edgeErrorY[z - 1]); - if ( diff > maxDiff ) + case 1: + { + p1 = VHACD::Vector3(m_1.GetX() - 2, i0, i1); + p2 = VHACD::Vector3(m_2.GetX() + 2, i0, i1); + break; + } + case 2: { - maxDiff = diff; - maxC = z-1; + p1 = VHACD::Vector3(i1, m_1.GetY() - 2, i0); + p2 = VHACD::Vector3(i1, m_2.GetY() + 2, i0); + break; } } - } - splitLoc = maxC + m1.getX(); + double e1 = Raycast(p1, p2); // raycast from one side to the interior + double e2 = Raycast(p2, p1); // raycast from the other side to the interior - // we do not allow an edge split if it is too close to the ends - if ( splitLoc > (m1.getZ() + 4) && splitLoc < (m2.getZ() - 4) ) - { - ret = true; + errorTotal = errorTotal + e1 + e2; } - - return ret; + edgeError2[index2] = errorTotal; + index2++; } - // This operation is performed in a background thread. - //It splits the voxels by a plane - void performPlaneSplit(void) + + // we now compute the first derivitave to find the greatest spot of concavity on the XY plane + double maxDiff = 0; + uint32_t maxC = 0; + for (uint32_t x = 1; x < index1; x++) { - if ( isComplete() ) - { - } - else + if ( edgeError1[x] > 0 && edgeError1[x - 1] > 0 ) { - uint32_t splitLoc; - SplitAxis axis = computeSplitPlane(splitLoc); - switch ( axis ) + double diff = abs(edgeError1[x] - edgeError1[x - 1]); + if ( diff > maxDiff ) { - case SplitAxis::X_AXIS_NEGATIVE: - case SplitAxis::X_AXIS_POSITIVE: - // Split on the getX axis at this split location - mHullA = new VoxelHull(*this,SplitAxis::X_AXIS_NEGATIVE,splitLoc); - mHullB = new VoxelHull(*this,SplitAxis::X_AXIS_POSITIVE,splitLoc); - break; - case SplitAxis::Y_AXIS_NEGATIVE: - case SplitAxis::Y_AXIS_POSITIVE: - // Split on the 1 axis at this split location - mHullA = new VoxelHull(*this,SplitAxis::Y_AXIS_NEGATIVE,splitLoc); - mHullB = new VoxelHull(*this,SplitAxis::Y_AXIS_POSITIVE,splitLoc); - break; - case SplitAxis::Z_AXIS_NEGATIVE: - case SplitAxis::Z_AXIS_POSITIVE: - // Split on the getZ axis at this split location - mHullA = new VoxelHull(*this,SplitAxis::Z_AXIS_NEGATIVE,splitLoc); - mHullB = new VoxelHull(*this,SplitAxis::Z_AXIS_POSITIVE,splitLoc); - break; + maxDiff = diff; + maxC = x-1; } } } - void saveOBJ(const char *fname, - const VoxelHull *h) + // Now see if there is a greater concavity on the XZ plane + for (uint32_t x = 1; x < index2; x++) { - FILE *fph = fopen(fname,"wb"); - if ( fph ) + if ( edgeError2[x] > 0 && edgeError2[x - 1] > 0 ) { - uint32_t baseIndex = 1; - for (size_t i = 0; i < mVertices.size(); ++i) + double diff = abs(edgeError2[x] - edgeError2[x - 1]); + if ( diff > maxDiff ) { - const VHACD::Vertex& v = mVertices[i]; - fprintf(fph, "v %0.9f %0.9f %0.9f\n", - v.mX, - v.mY, - v.mZ); + maxDiff = diff; + maxC = x - 1; } + } + } - for (size_t i = 0; i < mIndices.size(); ++i) - { - const VHACD::Triangle& t = mIndices[i]; - fprintf(fph, "f %d %d %d\n", - t.mI0 + baseIndex, - t.mI1 + baseIndex, - t.mI2 + baseIndex); - } + splitLoc = maxC + m_1[idx1]; + + // we do not allow an edge split if it is too close to the ends + if ( splitLoc > (m_1[idx1] + 4) + && splitLoc < (m_2[idx1] - 4) ) + { + ret = true; + } - baseIndex += uint32_t(mVertices.size()); + return ret; +} - for (size_t i = 0; i < h->mVertices.size(); ++i) - { - const VHACD::Vertex& v = h->mVertices[i]; - fprintf(fph, "v %0.9f %0.9f %0.9f\n", - v.mX, - v.mY + 0.1, - v.mZ); - } +// Finding the greatest area of concavity.. +bool VoxelHull::FindConcavityX(uint32_t& splitLoc) +{ + return FindConcavity(0, splitLoc); +} - for (size_t i = 0; i < h->mIndices.size(); ++i) - { - const VHACD::Triangle& t = h->mIndices[i]; - fprintf(fph, "f %d %d %d\n", - t.mI0 + baseIndex, - t.mI1 + baseIndex, - t.mI2 + baseIndex); - } - fclose(fph); +// Finding the greatest area of concavity.. +bool VoxelHull::FindConcavityY(uint32_t& splitLoc) +{ + return FindConcavity(1, splitLoc); +} + +// Finding the greatest area of concavity.. +bool VoxelHull::FindConcavityZ(uint32_t &splitLoc) +{ + return FindConcavity(2, splitLoc); +} + +void VoxelHull::PerformPlaneSplit() +{ + if ( IsComplete() ) + { + } + else + { + uint32_t splitLoc; + SplitAxis axis = ComputeSplitPlane(splitLoc); + switch ( axis ) + { + case SplitAxis::X_AXIS_NEGATIVE: + case SplitAxis::X_AXIS_POSITIVE: + // Split on the getX axis at this split location + m_hullA = std::unique_ptr(new VoxelHull(*this, SplitAxis::X_AXIS_NEGATIVE, splitLoc)); + m_hullB = std::unique_ptr(new VoxelHull(*this, SplitAxis::X_AXIS_POSITIVE, splitLoc)); + break; + case SplitAxis::Y_AXIS_NEGATIVE: + case SplitAxis::Y_AXIS_POSITIVE: + // Split on the 1 axis at this split location + m_hullA = std::unique_ptr(new VoxelHull(*this, SplitAxis::Y_AXIS_NEGATIVE, splitLoc)); + m_hullB = std::unique_ptr(new VoxelHull(*this, SplitAxis::Y_AXIS_POSITIVE, splitLoc)); + break; + case SplitAxis::Z_AXIS_NEGATIVE: + case SplitAxis::Z_AXIS_POSITIVE: + // Split on the getZ axis at this split location + m_hullA = std::unique_ptr(new VoxelHull(*this, SplitAxis::Z_AXIS_NEGATIVE, splitLoc)); + m_hullB = std::unique_ptr(new VoxelHull(*this, SplitAxis::Z_AXIS_POSITIVE, splitLoc)); + break; } } +} - void saveOBJ(const char *fname) +void VoxelHull::SaveVoxelMesh(const SimpleMesh &inputMesh, + bool saveVoxelMesh, + bool saveSourceMesh) +{ + char scratch[512]; + snprintf(scratch, + sizeof(scratch), + "voxel-mesh-%03d.obj", + m_index); + FILE *fph = fopen(scratch, + "wb"); + if ( fph ) { - FILE *fph = fopen(fname, "wb"); - if ( fph ) + uint32_t baseIndex = 1; + if ( saveVoxelMesh ) { - printf("Saving '%s' with %d vertices and %d triangles\n", - fname, - uint32_t(mVertices.size()), - uint32_t(mIndices.size())); - for (size_t i = 0; i < mVertices.size(); ++i) - { - const VHACD::Vertex& v = mVertices[i]; - fprintf(fph, "v %0.9f %0.9f %0.9f\n", - v.mX, - v.mY, - v.mZ); - } - - for (size_t i = 0; i < mIndices.size(); ++i) - { - const VHACD::Triangle& t = mIndices[i]; - fprintf(fph, "f %d %d %d\n", - t.mI0 + 1, - t.mI1 + 1, - t.mI2 + 1); - } - fclose(fph); + WriteOBJ(fph, + m_vertices, + m_indices, + baseIndex); + baseIndex += uint32_t(m_vertices.size()); + } + if ( saveSourceMesh ) + { + WriteOBJ(fph, + inputMesh.m_vertices, + inputMesh.m_indices, + baseIndex); } + fclose(fph); } +} - SplitAxis mAxis{SplitAxis::X_AXIS_NEGATIVE}; - VoxelizeImpl *mVoxels{nullptr}; // The voxelized data set - double mVoxelScale{0}; // Size of a single voxel - double mVoxelScaleHalf{0}; // 1/2 of the size of a single voxel - nd::VHACD::Vect3 mVoxelAdjust; // Minimum coordinates of the voxel space, with adjustment - nd::VHACD::Vect3 mVoxelBmin; // Minimum coordinates of the voxel space - nd::VHACD::Vect3 mVoxelBmax; // Maximum coordinates of the voxel space - uint32_t mDepth{0}; // How deep in the recursion of the binary tree this hull is - uint32_t mIndex{0}; // Each convex hull is given a unique id to distinguish it from the others - double mVolumeError{0}; // The percentage error from the convex hull volume vs. the voxel volume - double mVoxelVolume{0}; // The volume of the voxels - double mHullVolume{0}; // The volume of the enclosing convex hull +void VoxelHull::SaveOBJ(const char* fname, + const VoxelHull* h) +{ + FILE *fph = fopen(fname,"wb"); + if ( fph ) + { + uint32_t baseIndex = 1; + WriteOBJ(fph, + m_vertices, + m_indices, + baseIndex); - IVHACD::ConvexHull *mConvexHull{nullptr}; // The convex hull which encloses this set of voxels. - VoxelVector mSurfaceVoxels; // The voxels which are on the surface of the source mesh. - VoxelVector mNewSurfaceVoxels; // Voxels which are on the surface as a result of a plane split - VoxelVector mInteriorVoxels; // Voxels which are part of the interior of the hull + baseIndex += uint32_t(m_vertices.size()); - VoxelHull *mHullA{nullptr}; // hull resulting from one side of the plane split - VoxelHull *mHullB{nullptr}; // hull resulting from the other side of the plane split + WriteOBJ(fph, + h->m_vertices, + h->m_indices, + baseIndex); + fclose(fph); + } +} - // Defines the coordinates this convex hull comprises within the voxel volume - // of the entire source - nd::VHACD::Vect3 m1{0}; - nd::VHACD::Vect3 m2{0}; - MyRaycastMesh mRaycastMesh; - VoxelIndexMap mVoxelIndexMap; // Maps from a voxel coordinate space into a vertex index space - std::vector mVertices; - std::vector mIndices; - static uint32_t mVoxelHullCount; - IVHACD::Parameters mParams; - VHACDCallbacks *mCallbacks{nullptr}; -}; +void VoxelHull::SaveOBJ(const char *fname) +{ + FILE *fph = fopen(fname, "wb"); + if ( fph ) + { + printf("Saving '%s' with %d vertices and %d triangles\n", + fname, + uint32_t(m_vertices.size()), + uint32_t(m_indices.size())); + WriteOBJ(fph, + m_vertices, + m_indices, + 1); + fclose(fph); + } +} + +void VoxelHull::WriteOBJ(FILE* fph, + const std::vector& vertices, + const std::vector& indices, + uint32_t baseIndex) +{ + if (!fph) + { + return; + } -uint32_t VoxelHull::mVoxelHullCount=0; + for (size_t i = 0; i < vertices.size(); ++i) + { + const VHACD::Vertex& v = vertices[i]; + fprintf(fph, "v %0.9f %0.9f %0.9f\n", + v.mX, + v.mY, + v.mZ); + } -using VoxelHullVector = std::vector< VoxelHull * >; -using VoxelHullQueue = std::queue< VoxelHull * >; + for (size_t i = 0; i < indices.size(); ++i) + { + const VHACD::Triangle& t = indices[i]; + fprintf(fph, "f %d %d %d\n", + t.mI0 + baseIndex, + t.mI1 + baseIndex, + t.mI2 + baseIndex); + } +} class VHACDImpl; @@ -7607,1218 +6747,1248 @@ class VHACDImpl; class CostTask { public: - VHACDImpl *mThis{nullptr}; - IVHACD::ConvexHull *mHullA{nullptr}; - IVHACD::ConvexHull *mHullB{nullptr}; - double mConcavity{0}; // concavity of the two combineds - std::future mFuture; + VHACDImpl* m_this{ nullptr }; + IVHACD::ConvexHull* m_hullA{ nullptr }; + IVHACD::ConvexHull* m_hullB{ nullptr }; + double m_concavity{ 0 }; // concavity of the two combineds + std::future m_future; }; -void computeMergeCostTask(void *ptr); - class HullPair { public: - HullPair(void) { }; - HullPair(uint32_t hullA,uint32_t hullB,double concavity) : mHullA(hullA),mHullB(hullB),mConcavity(concavity) - { - } + HullPair() = default; + HullPair(uint32_t hullA, + uint32_t hullB, + double concavity); - bool operator<(const HullPair &h) const - { - return mConcavity > h.mConcavity ? true : false; - } + bool operator<(const HullPair &h) const; - uint32_t mHullA{0}; - uint32_t mHullB{0}; - double mConcavity{0}; + uint32_t m_hullA{ 0 }; + uint32_t m_hullB{ 0 }; + double m_concavity{ 0 }; }; -using HullPairQueue = std::priority_queue< HullPair >; -using HullMap = std::unordered_map< uint32_t, IVHACD::ConvexHull * >; +HullPair::HullPair(uint32_t hullA, + uint32_t hullB, + double concavity) + : m_hullA(hullA) + , m_hullB(hullB) + , m_concavity(concavity) +{ +} -void jobCallback(void *userPtr); +bool HullPair::operator<(const HullPair &h) const +{ + return m_concavity > h.m_concavity ? true : false; +} -// Don't consider more than 100,000 convex hulls. -#define VHACD_MAX_CONVEX_HULL_FRAGMENTS 100000 +// void jobCallback(void* userPtr); class VHACDImpl : public IVHACD, public VHACDCallbacks { + // Don't consider more than 100,000 convex hulls. + static constexpr uint32_t MaxConvexHullFragments{ 100000 }; public: VHACDImpl() = default; + /* + * Overrides VHACD::IVHACD + */ ~VHACDImpl() override { Clean(); } - void Cancel() override final - { - mCanceled = true; - } + void Cancel() override final; bool Compute(const float* const points, const uint32_t countPoints, const uint32_t* const triangles, const uint32_t countTriangles, - const Parameters& params) override final - { - std::vector v; - v.reserve(countPoints); - for (uint32_t i = 0; i < countPoints; ++i) - { - v.emplace_back(points[i * 3 + 0], - points[i * 3 + 1], - points[i * 3 + 2]); - } - - std::vector t; - t.reserve(countTriangles); - for (uint32_t i = 0; i < countTriangles; ++i) - { - t.emplace_back(triangles[i * 3 + 0], - triangles[i * 3 + 1], - triangles[i * 3 + 2]); - } - - return Compute(v, t, params); - } + const Parameters& params) override final; bool Compute(const double* const points, const uint32_t countPoints, const uint32_t* const triangles, const uint32_t countTriangles, - const Parameters& params) override final - { - std::vector v; - v.reserve(countPoints); - for (uint32_t i = 0; i < countPoints; ++i) - { - v.emplace_back(points[i * 3 + 0], - points[i * 3 + 1], - points[i * 3 + 2]); - } + const Parameters& params) override final; - std::vector t; - t.reserve(countTriangles); - for (uint32_t i = 0; i < countTriangles; ++i) - { - t.emplace_back(triangles[i * 3 + 0], - triangles[i * 3 + 1], - triangles[i * 3 + 2]); - } + uint32_t GetNConvexHulls() const override final; - return Compute(v, t, params); - } + bool GetConvexHull(const uint32_t index, + ConvexHull& ch) const override final; + + void Clean() override final; // release internally allocated memory + + void Release() override final; + + // Will compute the center of mass of the convex hull decomposition results and return it + // in 'centerOfMass'. Returns false if the center of mass could not be computed. + bool ComputeCenterOfMass(double centerOfMass[3]) const override final; + + // In synchronous mode (non-multi-threaded) the state is always 'ready' + // In asynchronous mode, this returns true if the background thread is not still actively computing + // a new solution. In an asynchronous config the 'IsReady' call will report any update or log + // messages in the caller's current thread. + bool IsReady(void) const override final; + + /** + * At the request of LegionFu : out_look@foxmail.com + * This method will return which convex hull is closest to the source position. + * You can use this method to figure out, for example, which vertices in the original + * source mesh are best associated with which convex hull. + * + * @param pos : The input 3d position to test against + * + * @return : Returns which convex hull this position is closest to. + */ + uint32_t findNearestConvexHull(const double pos[3], + double& distanceToHull) override final; +// private: bool Compute(const std::vector& points, const std::vector& triangles, - const Parameters& params) - { - bool ret = false; + const Parameters& params); - mParams = params; - mCanceled = false; + // Take the source position, normalize it, and then convert it into an index position + uint32_t GetIndex(VHACD::VertexIndex& vi, + const VHACD::Vertex& p); - Clean(); // release any previous results -#if !VHACD_DISABLE_THREADING - if ( mParams.m_asyncACD ) - { - mThreadPool = new ThreadPool(8); - } -#endif - copyInputMesh(points, - triangles); - if ( !mCanceled ) - { - // We now recursively perform convex decomposition until complete - performConvexDecomposition(); - } + // This copies the input mesh while scaling the input positions + // to fit into a normalized unit cube. It also re-indexes all of the + // vertex positions in case they weren't clean coming in. + void CopyInputMesh(const std::vector& points, + const std::vector& triangles); - if ( mCanceled ) - { - Clean(); - ret = false; - if ( mParams.m_logger ) - { - mParams.m_logger->Log("VHACD operation canceled before it was complete."); - } - } - else - { - ret = true; - } + void ScaleOutputConvexHull(ConvexHull &ch); + + void AddCostToPriorityQueue(CostTask& task); + + void ReleaseConvexHull(ConvexHull* ch); + + void PerformConvexDecomposition(); + + double ComputeConvexHullVolume(const ConvexHull& sm); + + double ComputeVolume4(const VHACD::Vect3& a, + const VHACD::Vect3& b, + const VHACD::Vect3& c, + const VHACD::Vect3& d); + + double ComputeConcavity(double volumeSeparate, + double volumeCombined, + double volumeMesh); + + // See if we can compute the cost without having to actually merge convex hulls. + // If the axis aligned bounding boxes (slightly inflated) of the two convex hulls + // do not intersect, then we don't need to actually compute the merged convex hull + // volume. + bool DoFastCost(CostTask& mt); + + void PerformMergeCostTask(CostTask& mt); + + ConvexHull* ComputeReducedConvexHull(const ConvexHull& ch, + uint32_t maxVerts, + bool projectHullVertices); + + // Take the points in convex hull A and the points in convex hull B and generate + // a new convex hull on the combined set of points. + // Once completed, we create a SimpleMesh instance to hold the triangle mesh + // and we compute an inflated AABB for it. + ConvexHull* ComputeCombinedConvexHull(const ConvexHull& sm1, + const ConvexHull& sm2); + + + ConvexHull* GetHull(uint32_t index); + + bool RemoveHull(uint32_t index); + + ConvexHull* CopyConvexHull(const ConvexHull& source); + + const char* GetStageName(Stages stage) const; + + /* + * Overrides VHACD::VHACDCallbacks + */ + void ProgressUpdate(Stages stage, + double stageProgress, + const char* operation) override final; + + bool IsCanceled() const override final; + + std::atomic m_canceled{ false }; + Parameters m_params; // Convex decomposition parameters + + std::vector m_convexHulls; // Finalized convex hulls + std::vector> m_voxelHulls; // completed voxel hulls + std::vector> m_pendingHulls; + + std::vector> m_trees; + VHACD::AABBTree m_AABBTree; + VHACD::Volume m_voxelize; + VHACD::Vect3 m_center; + double m_scale{ double(1.0) }; + double m_recipScale{ double(1.0) }; + SimpleMesh m_inputMesh; // re-indexed and normalized input mesh + std::vector m_vertices; + std::vector m_indices; + + double m_overallHullVolume{ double(0.0) }; + double m_voxelScale{ double(0.0) }; + double m_voxelHalfScale{ double(0.0) }; + VHACD::Vect3 m_voxelBmin; + VHACD::Vect3 m_voxelBmax; + uint32_t m_meshId{ 0 }; + std::priority_queue m_hullPairQueue; #if !VHACD_DISABLE_THREADING - delete mThreadPool; - mThreadPool = nullptr; + std::unique_ptr m_threadPool{ nullptr }; #endif - return ret; - } + std::unordered_map m_hulls; + + double m_overallProgress{ double(0.0) }; + double m_stageProgress{ double(0.0) }; + double m_operationProgress{ double(0.0) }; +}; + +void VHACDImpl::Cancel() +{ + m_canceled = true; +} - uint32_t GetNConvexHulls() const override final +bool VHACDImpl::Compute(const float* const points, + const uint32_t countPoints, + const uint32_t* const triangles, + const uint32_t countTriangles, + const Parameters& params) +{ + std::vector v; + v.reserve(countPoints); + for (uint32_t i = 0; i < countPoints; ++i) { - return uint32_t(mConvexHulls.size()); + v.emplace_back(points[i * 3 + 0], + points[i * 3 + 1], + points[i * 3 + 2]); } - bool GetConvexHull(const uint32_t index, ConvexHull& ch) const override final + std::vector t; + t.reserve(countTriangles); + for (uint32_t i = 0; i < countTriangles; ++i) { - bool ret = false; + t.emplace_back(triangles[i * 3 + 0], + triangles[i * 3 + 1], + triangles[i * 3 + 2]); + } - if ( index < uint32_t(mConvexHulls.size() )) - { - ch = *mConvexHulls[index]; - ret = true; - } + return Compute(v, t, params); +} - return ret; +bool VHACDImpl::Compute(const double* const points, + const uint32_t countPoints, + const uint32_t* const triangles, + const uint32_t countTriangles, + const Parameters& params) +{ + std::vector v; + v.reserve(countPoints); + for (uint32_t i = 0; i < countPoints; ++i) + { + v.emplace_back(points[i * 3 + 0], + points[i * 3 + 1], + points[i * 3 + 2]); } - void Clean() override final // release internally allocated memory + std::vector t; + t.reserve(countTriangles); + for (uint32_t i = 0; i < countTriangles; ++i) { -#if !VHACD_DISABLE_THREADING - delete mThreadPool; - mThreadPool = nullptr; -#endif + t.emplace_back(triangles[i * 3 + 0], + triangles[i * 3 + 1], + triangles[i * 3 + 2]); + } - mTrees.clear(); + return Compute(v, t, params); +} - for (auto &ch:mConvexHulls) - { - releaseConvexHull(ch); - } - mConvexHulls.clear(); +uint32_t VHACDImpl::GetNConvexHulls() const +{ + return uint32_t(m_convexHulls.size()); +} - for (auto &ch:mHulls) - { - releaseConvexHull(ch.second); - } - mHulls.clear(); +bool VHACDImpl::GetConvexHull(const uint32_t index, + ConvexHull& ch) const +{ + bool ret = false; - for (auto &ch:mVoxelHulls) - { - delete ch; - } - mVoxelHulls.clear(); + if ( index < uint32_t(m_convexHulls.size() )) + { + ch = *m_convexHulls[index]; + ret = true; + } - for (auto &ch:mPendingHulls) - { - delete ch; - } - mPendingHulls.clear(); + return ret; +} - mVertices.clear(); - mIndices.clear(); - } +void VHACDImpl::Clean() +{ +#if !VHACD_DISABLE_THREADING + m_threadPool = nullptr; +#endif - void Release(void) override final - { - delete this; - } + m_trees.clear(); - // Will compute the center of mass of the convex hull decomposition results and return it - // in 'centerOfMass'. Returns false if the center of mass could not be computed. - bool ComputeCenterOfMass(double centerOfMass[3]) const override final + for (auto& ch : m_convexHulls) { - bool ret = false; - - return ret; + ReleaseConvexHull(ch); } + m_convexHulls.clear(); - // In synchronous mode (non-multi-threaded) the state is always 'ready' - // In asynchronous mode, this returns true if the background thread is not still actively computing - // a new solution. In an asynchronous config the 'IsReady' call will report any update or log - // messages in the caller's current thread. - bool IsReady(void) const override final + for (auto& ch : m_hulls) { - return true; + ReleaseConvexHull(ch.second); } + m_hulls.clear(); - /** - * At the request of LegionFu : out_look@foxmail.com - * This method will return which convex hull is closest to the source position. - * You can use this method to figure out, for example, which vertices in the original - * source mesh are best associated with which convex hull. - * - * @param pos : The input 3d position to test against - * - * @return : Returns which convex hull this position is closest to. - */ - uint32_t findNearestConvexHull(const double pos[3], - double &distanceToHull) override final - { - uint32_t ret = 0; // The default return code is zero + m_voxelHulls.clear(); + + m_pendingHulls.clear(); + + m_vertices.clear(); + m_indices.clear(); +} + +void VHACDImpl::Release() +{ + delete this; +} + +bool VHACDImpl::ComputeCenterOfMass(double centerOfMass[3]) const +{ + bool ret = false; + + return ret; +} + +bool VHACDImpl::IsReady() const +{ + return true; +} + +uint32_t VHACDImpl::findNearestConvexHull(const double pos[3], + double& distanceToHull) +{ + uint32_t ret = 0; // The default return code is zero - uint32_t hullCount = GetNConvexHulls(); - distanceToHull = 0; - // First, make sure that we have valid and completed results - if ( hullCount ) + uint32_t hullCount = GetNConvexHulls(); + distanceToHull = 0; + // First, make sure that we have valid and completed results + if ( hullCount ) + { + // See if we already have AABB trees created for each convex hull + if ( m_trees.empty() ) { - // See if we already have AABB trees created for each convex hull - if ( mTrees.empty() ) + // For each convex hull, we generate an AABB tree for fast closest point queries + for (uint32_t i = 0; i < hullCount; i++) { - // For each convex hull, we generate an AABB tree for fast closest point queries - for (uint32_t i=0; i& t = m_trees[i]; + if ( t ) { - AABBTreeImpl *t = mTrees[i]; - if ( t ) + VHACD::Vect3 closestPoint; + VHACD::Vect3 position(pos[0], + pos[1], + pos[2]); + if ( t->GetClosestPointWithinDistance(position, 1e99, closestPoint)) { - nd::VHACD::Vect3 closestPoint; - if ( t->getClosestPointWithinDistance(*(const nd::VHACD::Vect3*)pos,1e99,closestPoint)) + VHACD::Vect3 d = position - closestPoint; + double distanceSquared = d.GetNormSquared(); + if ( distanceSquared < closest ) { - double dx = pos[0] - closestPoint[0]; - double dy = pos[1] - closestPoint[1]; - double dz = pos[2] - closestPoint[2]; - double distanceSquared = dx*dx + dy*dy + dz*dz; - if ( distanceSquared < closest ) - { - closest = distanceSquared; - ret = i; - } + closest = distanceSquared; + ret = i; } } } - distanceToHull = sqrt(closest); // compute the distance to the nearest convex hull } + distanceToHull = sqrt(closest); // compute the distance to the nearest convex hull + } - return ret; + return ret; +} + +bool VHACDImpl::Compute(const std::vector& points, + const std::vector& triangles, + const Parameters& params) +{ + bool ret = false; + + m_params = params; + m_canceled = false; + Clean(); // release any previous results +#if !VHACD_DISABLE_THREADING + if ( m_params.m_asyncACD ) + { + m_threadPool = std::unique_ptr(new ThreadPool(8)); + } +#endif + CopyInputMesh(points, + triangles); + if ( !m_canceled ) + { + // We now recursively perform convex decomposition until complete + PerformConvexDecomposition(); } - // Take the source position, normalize it, and then convert it into an index position - uint32_t getIndex(VHACD::MyVertexIndex& vi, - const VHACD::Vertex& p) - { - nd::VHACD::Vect3 pos; - pos[0] = (p.mX - mCenter[0]) * mRecipScale; - pos[1] = (p.mY - mCenter[1]) * mRecipScale; - pos[2] = (p.mZ - mCenter[2]) * mRecipScale; - bool newPos; - uint32_t ret = vi.getIndex(pos,newPos); - return ret; + if ( m_canceled ) + { + Clean(); + ret = false; + if ( m_params.m_logger ) + { + m_params.m_logger->Log("VHACD operation canceled before it was complete."); + } + } + else + { + ret = true; } +#if !VHACD_DISABLE_THREADING + m_threadPool = nullptr; +#endif + return ret; +} +uint32_t VHACDImpl::GetIndex(VHACD::VertexIndex& vi, + const VHACD::Vertex& p) +{ + VHACD::Vect3 pos = (VHACD::Vect3(p) - m_center) * m_recipScale; + bool newPos; + uint32_t ret = vi.GetIndex(pos, + newPos); + return ret; +} - // This copies the input mesh while scaling the input positions - // to fit into a normalized unit cube. It also re-indexes all of the - // vertex positions in case they weren't clean coming in. - void copyInputMesh(const std::vector& points, - const std::vector& triangles) +void VHACDImpl::CopyInputMesh(const std::vector& points, + const std::vector& triangles) +{ + m_vertices.clear(); + m_indices.clear(); + m_indices.reserve(triangles.size()); + + // First we must find the bounding box of this input vertices and normalize them into a unit-cube + VHACD::Vect3 bmin( FLT_MAX); + VHACD::Vect3 bmax(-FLT_MAX); + ProgressUpdate(Stages::COMPUTE_BOUNDS_OF_INPUT_MESH, + 0, + "ComputingBounds"); + for (uint32_t i = 0; i < points.size(); i++) { - mVertices.clear(); - mIndices.clear(); - mIndices.reserve(triangles.size()); + const VHACD::Vertex& p = points[i]; + + bmin = bmin.CWiseMin(p); + bmax = bmax.CWiseMax(p); + } + ProgressUpdate(Stages::COMPUTE_BOUNDS_OF_INPUT_MESH, + 100, + "ComputingBounds"); - // First we must find the bounding box of this input vertices and normalize them into a unit-cube - nd::VHACD::Vect3 bmin(FLT_MAX); - nd::VHACD::Vect3 bmax(-FLT_MAX); - progressUpdate(Stages::COMPUTE_BOUNDS_OF_INPUT_MESH, - 0, - "ComputingBounds"); - for (uint32_t i = 0; i < points.size(); i++) - { - const VHACD::Vertex& p = points[i]; + m_center = (bmax + bmin) * double(0.5); - bmin = bmin.CWiseMin(p); - bmax = bmax.CWiseMax(p); - } - progressUpdate(Stages::COMPUTE_BOUNDS_OF_INPUT_MESH, - 100, - "ComputingBounds"); + VHACD::Vect3 scale = bmax - bmin; + m_scale = scale.MaxCoeff(); - mCenter = (bmax + bmin) * 0.5; + m_recipScale = m_scale > double(0.0) ? double(1.0) / m_scale : double(0.0); - nd::VHACD::Vect3 scale = bmax - bmin; - mScale = Max(Max(scale.getX(), scale.getY()), scale.getZ()); + { + VHACD::VertexIndex vi = VHACD::VertexIndex(double(0.001), false); - mRecipScale = mScale > 0 ? 1.0 / mScale : 0; + uint32_t dcount = 0; + for (uint32_t i = 0; i < triangles.size() && !m_canceled; ++i) { - VHACD::MyVertexIndex vi = VHACD::MyVertexIndex(0.001, false); + const VHACD::Triangle& t = triangles[i]; + const VHACD::Vertex& p1 = points[t.mI0]; + const VHACD::Vertex& p2 = points[t.mI1]; + const VHACD::Vertex& p3 = points[t.mI2]; - uint32_t dcount=0; + uint32_t i1 = GetIndex(vi, p1); + uint32_t i2 = GetIndex(vi, p2); + uint32_t i3 = GetIndex(vi, p3); - for (uint32_t i = 0; i < triangles.size() && !mCanceled; ++i) + if ( i1 == i2 || i1 == i3 || i2 == i3 ) { - const VHACD::Triangle& t = triangles[i]; - const VHACD::Vertex& p1 = points[t.mI0]; - const VHACD::Vertex& p2 = points[t.mI1]; - const VHACD::Vertex& p3 = points[t.mI2]; - - uint32_t i1 = getIndex(vi, p1); - uint32_t i2 = getIndex(vi, p2); - uint32_t i3 = getIndex(vi, p3); - - if ( i1 == i2 || i1 == i3 || i2 == i3 ) - { - dcount++; - } - else - { - mIndices.emplace_back(i1, i2, i3); - } + dcount++; } - if ( dcount ) + else { - if ( mParams.m_logger ) - { - char scratch[512]; - snprintf(scratch, - sizeof(scratch), - "Skipped %d degenerate triangles", dcount); - mParams.m_logger->Log(scratch); - } + m_indices.emplace_back(i1, i2, i3); } - - mVertices = vi.takeVertices(); - } - - // Create the raycast mesh - if ( !mCanceled ) - { - progressUpdate(Stages::CREATE_RAYCAST_MESH, - 0, - "Building RaycastMesh"); - mRaycastMesh = VHACD::MyRaycastMesh(mVertices, - mIndices); - progressUpdate(Stages::CREATE_RAYCAST_MESH, - 100, - "RaycastMesh completed"); - } - if ( !mCanceled ) - { - progressUpdate(Stages::VOXELIZING_INPUT_MESH, - 0, - "Voxelizing Input Mesh"); - mVoxelize = VHACD::VoxelizeImpl(); - mVoxelize.voxelize(mRaycastMesh, - mVertices, - mIndices, - mParams.m_resolution, - mParams.m_fillMode); - mVoxelScale = mVoxelize.getScale(); - mVoxelHalfScale = mVoxelScale * 0.5; - mVoxelBmin = mVoxelize.getBoundsMin(); - mVoxelBmax = mVoxelize.getBoundsMax(); - progressUpdate(Stages::VOXELIZING_INPUT_MESH, - 100, - "Voxelization complete"); } - mInputMesh.mVertices = mVertices; - mInputMesh.mIndices = mIndices; - if ( !mCanceled ) + if ( dcount ) { - progressUpdate(Stages::BUILD_INITIAL_CONVEX_HULL, - 0, - "Build initial ConvexHull"); - VoxelHull *vh = new VoxelHull(mVoxelize, - mInputMesh, - mParams, - this); - if ( vh->mConvexHull ) + if ( m_params.m_logger ) { - mOverallHullVolume = vh->mConvexHull->m_volume; + char scratch[512]; + snprintf(scratch, + sizeof(scratch), + "Skipped %d degenerate triangles", dcount); + m_params.m_logger->Log(scratch); } - mPendingHulls.push_back(vh); - progressUpdate(Stages::BUILD_INITIAL_CONVEX_HULL, - 100, - "Initial ConvexHull complete"); } - } - void scaleOutputConvexHull(ConvexHull &ch) - { - for (uint32_t i = 0; i < ch.m_points.size(); i++) - { - VHACD::Vertex& p = ch.m_points[i]; - p.mX = (p.mX * mScale) + mCenter[0]; - p.mY = (p.mY * mScale) + mCenter[1]; - p.mZ = (p.mZ * mScale) + mCenter[2]; - } - ch.m_volume = computeConvexHullVolume(ch); // get the combined volume - fm_getAABB(ch.m_points, - ch.mBmin, - ch.mBmax); - fm_computeCentroid(ch.m_points, - ch.m_triangles, - ch.m_center); + m_vertices = vi.TakeVertices(); } - void addCostToPriorityQueue(CostTask *task) + // Create the raycast mesh + if ( !m_canceled ) { - HullPair hp(task->mHullA->m_meshId, - task->mHullB->m_meshId, - task->mConcavity); - mHullPairQueue.push(hp); + ProgressUpdate(Stages::CREATE_RAYCAST_MESH, + 0, + "Building RaycastMesh"); + m_AABBTree = VHACD::AABBTree(m_vertices, + m_indices); + ProgressUpdate(Stages::CREATE_RAYCAST_MESH, + 100, + "RaycastMesh completed"); + } + if ( !m_canceled ) + { + ProgressUpdate(Stages::VOXELIZING_INPUT_MESH, + 0, + "Voxelizing Input Mesh"); + m_voxelize = VHACD::Volume(); + m_voxelize.Voxelize(m_vertices, + m_indices, + m_params.m_resolution, + m_params.m_fillMode, + m_AABBTree); + m_voxelScale = m_voxelize.GetScale(); + m_voxelHalfScale = m_voxelScale * double(0.5); + m_voxelBmin = m_voxelize.GetBounds().GetMin(); + m_voxelBmax = m_voxelize.GetBounds().GetMax(); + ProgressUpdate(Stages::VOXELIZING_INPUT_MESH, + 100, + "Voxelization complete"); } - void releaseConvexHull(ConvexHull *ch) + m_inputMesh.m_vertices = m_vertices; + m_inputMesh.m_indices = m_indices; + if ( !m_canceled ) { - if ( ch ) + ProgressUpdate(Stages::BUILD_INITIAL_CONVEX_HULL, + 0, + "Build initial ConvexHull"); + std::unique_ptr vh = std::unique_ptr(new VoxelHull(m_voxelize, + m_params, + this)); + if ( vh->m_convexHull ) { - delete ch; + m_overallHullVolume = vh->m_convexHull->m_volume; } + m_pendingHulls.push_back(std::move(vh)); + ProgressUpdate(Stages::BUILD_INITIAL_CONVEX_HULL, + 100, + "Initial ConvexHull complete"); + } +} + +void VHACDImpl::ScaleOutputConvexHull(ConvexHull& ch) +{ + for (uint32_t i = 0; i < ch.m_points.size(); i++) + { + VHACD::Vect3 p = ch.m_points[i]; + p = (p * m_scale) + m_center; + ch.m_points[i] = p; + } + ch.m_volume = ComputeConvexHullVolume(ch); // get the combined volume + VHACD::BoundsAABB b(ch.m_points); + ch.mBmin = b.GetMin(); + ch.mBmax = b.GetMax(); + ComputeCentroid(ch.m_points, + ch.m_triangles, + ch.m_center); +} + +void VHACDImpl::AddCostToPriorityQueue(CostTask& task) +{ + HullPair hp(task.m_hullA->m_meshId, + task.m_hullB->m_meshId, + task.m_concavity); + m_hullPairQueue.push(hp); +} + +void VHACDImpl::ReleaseConvexHull(ConvexHull* ch) +{ + if ( ch ) + { + delete ch; } +} - void performConvexDecomposition(void) +void jobCallback(std::unique_ptr& userPtr) +{ + userPtr->PerformPlaneSplit(); +} + +void computeMergeCostTask(CostTask& ptr) +{ + ptr.m_this->PerformMergeCostTask(ptr); +} + +void VHACDImpl::PerformConvexDecomposition() +{ { + ScopedTime st("Convex Decompostion", + m_params.m_logger); + double maxHulls = pow(2, m_params.m_maxRecursionDepth); + // We recursively split convex hulls until we can + // no longer recurse further. + Timer t; + + while ( !m_pendingHulls.empty() && !m_canceled ) { - ScopedTime st("Convex Decompostion", - mParams.m_logger); - double maxHulls = pow(2, mParams.m_maxRecursionDepth); - // We recursively split convex hulls until we can - // no longer recurse further. - Timer t; - - while ( !mPendingHulls.empty() && !mCanceled ) + size_t count = m_pendingHulls.size() + m_voxelHulls.size(); + double e = t.PeekElapsedSeconds(); + if ( e >= double(0.1) ) { - size_t count = mPendingHulls.size() + mVoxelHulls.size(); - double e = t.peekElapsedSeconds(); - if ( e >= 0.1 ) + t.Reset(); + double stageProgress = (double(count) * double(100.0)) / maxHulls; + ProgressUpdate(Stages::PERFORMING_DECOMPOSITION, + stageProgress, + "Performing recursive decomposition of convex hulls"); + } + // First we make a copy of the hulls we are processing + std::vector> oldList = std::move(m_pendingHulls); + // For each hull we want to split, we either + // immediately perform the plane split or we post it as + // a job to be performed in a background thread + std::vector> futures(oldList.size()); + uint32_t futureCount = 0; + for (auto& i : oldList) + { + if ( i->IsComplete() || count > MaxConvexHullFragments ) { - t.reset(); - double stageProgress = (double(count) * 100.0) / maxHulls; - progressUpdate(Stages::PERFORMING_DECOMPOSITION, - stageProgress, - "Performing recursive decomposition of convex hulls"); } - // First we make a copy of the hulls we are processing - VoxelHullVector oldList = mPendingHulls; - // For each hull we want to split, we either - // immediately perform the plane split or we post it as - // a job to be performed in a background thread - std::future *futures = new std::future[mPendingHulls.size()]; - uint32_t futureCount = 0; - for (auto &i : mPendingHulls) + else { - if ( i->isComplete() || count > VHACD_MAX_CONVEX_HULL_FRAGMENTS ) +#if !VHACD_DISABLE_THREADING + if ( m_threadPool ) { + futures[futureCount] = m_threadPool->enqueue([&i] + { + jobCallback(i); + }); + futureCount++; } else - { -#if !VHACD_DISABLE_THREADING - if ( mThreadPool ) - { - futures[futureCount] = mThreadPool->enqueue([i] - { - jobCallback(i); - }); - futureCount++; - } - else #endif - { - i->performPlaneSplit(); - } + { + i->PerformPlaneSplit(); } } - // Wait for any outstanding jobs to complete in the background threads - if ( futureCount ) + } + // Wait for any outstanding jobs to complete in the background threads + if ( futureCount ) + { + for (uint32_t i = 0; i < futureCount; i++) + { + futures[i].get(); + } + } + // Now, we rebuild the pending convex hulls list by + // adding the two children to the output list if + // we need to recurse them further + for (auto& vh : oldList) + { + if ( vh->IsComplete() || count > MaxConvexHullFragments ) { - for (uint32_t i = 0; i < futureCount; i++) + if ( vh->m_convexHull ) { - futures[i].get(); + m_voxelHulls.push_back(std::move(vh)); } } - delete []futures; - // Now, we rebuild the pending convex hulls list by - // adding the two children to the output list if - // we need to recurse them further - mPendingHulls.clear(); - for (auto &vh : oldList) + else { - if ( vh->isComplete() || count > VHACD_MAX_CONVEX_HULL_FRAGMENTS ) + if ( vh->m_hullA ) { - if ( vh->mConvexHull ) - { - mVoxelHulls.push_back(vh); - } - else - { - delete vh; - } + m_pendingHulls.push_back(std::move(vh->m_hullA)); } - else + if ( vh->m_hullB ) { - if ( vh->mHullA ) - { - mPendingHulls.push_back(vh->mHullA); - } - if ( vh->mHullB ) - { - mPendingHulls.push_back(vh->mHullB); - } - vh->mHullA = nullptr; - vh->mHullB = nullptr; - delete vh; + m_pendingHulls.push_back(std::move(vh->m_hullB)); } } } } + } - if ( !mCanceled ) - { - // Give each convex hull a unique guid - mMeshId = 0; - mHulls.clear(); - - // Build the convex hull id map - std::vector< ConvexHull *> hulls; + if ( !m_canceled ) + { + // Give each convex hull a unique guid + m_meshId = 0; + m_hulls.clear(); - progressUpdate(Stages::INITIALIZING_CONVEX_HULLS_FOR_MERGING, - 0, - "Initializing ConvexHulls"); + // Build the convex hull id map + std::vector hulls; - for (auto &vh:mVoxelHulls) + ProgressUpdate(Stages::INITIALIZING_CONVEX_HULLS_FOR_MERGING, + 0, + "Initializing ConvexHulls"); + for (auto& vh : m_voxelHulls) + { + if ( m_canceled ) { - if ( mCanceled ) - { - break; - } - ConvexHull *ch = copyConvexHull(*vh->mConvexHull); - mMeshId++; - ch->m_meshId = mMeshId; - mHulls[mMeshId] = ch; - // Compute the volume of the convex hull - ch->m_volume = computeConvexHullVolume(*ch); - // Compute the AABB of the convex hull - fm_getAABB(ch->m_points, - ch->mBmin, - ch->mBmax); - // Inflate the AABB by 10% - fm_inflateMinMax(ch->mBmin, - ch->mBmax, - 0.1); - - fm_computeCentroid(ch->m_points, - ch->m_triangles, - ch->m_center); - - delete vh; - hulls.push_back(ch); + break; } - progressUpdate(Stages::INITIALIZING_CONVEX_HULLS_FOR_MERGING, - 100, - "ConvexHull initialization complete"); - - mVoxelHulls.clear(); - - // here we merge convex hulls as needed until the match the - // desired maximum hull count. - size_t hullCount = hulls.size(); - - if ( hullCount > mParams.m_maxConvexHulls && !mCanceled) + ConvexHull* ch = CopyConvexHull(*vh->m_convexHull); + m_meshId++; + ch->m_meshId = m_meshId; + m_hulls[m_meshId] = ch; + // Compute the volume of the convex hull + ch->m_volume = ComputeConvexHullVolume(*ch); + // Compute the AABB of the convex hull + VHACD::BoundsAABB b = VHACD::BoundsAABB(ch->m_points).Inflate(double(0.1)); + ch->mBmin = b.GetMin(); + ch->mBmax = b.GetMax(); + + ComputeCentroid(ch->m_points, + ch->m_triangles, + ch->m_center); + + hulls.push_back(ch); + } + ProgressUpdate(Stages::INITIALIZING_CONVEX_HULLS_FOR_MERGING, + 100, + "ConvexHull initialization complete"); + + m_voxelHulls.clear(); + + // here we merge convex hulls as needed until the match the + // desired maximum hull count. + size_t hullCount = hulls.size(); + + if ( hullCount > m_params.m_maxConvexHulls && !m_canceled) + { + size_t costMatrixSize = ((hullCount * hullCount) - hullCount) >> 1; + std::vector tasks; + tasks.reserve(costMatrixSize); + + ScopedTime st("Computing the Cost Matrix", + m_params.m_logger); + // First thing we need to do is compute the cost matrix + // This is computed as the volume error of any two convex hulls + // combined + ProgressUpdate(Stages::COMPUTING_COST_MATRIX, + 0, + "Computing Hull Merge Cost Matrix"); + for (size_t i = 1; i < hullCount && !m_canceled; i++) { - size_t costMatrixSize = ((hullCount*hullCount)-hullCount)>>1; - CostTask *tasks = new CostTask[costMatrixSize]; - CostTask *task = tasks; - ScopedTime st("Computing the Cost Matrix", - mParams.m_logger); - // First thing we need to do is compute the cost matrix - // This is computed as the volume error of any two convex hulls - // combined - progressUpdate(Stages::COMPUTING_COST_MATRIX, - 0, - "Computing Hull Merge Cost Matrix"); - for (size_t i = 1; i < hullCount && !mCanceled; i++) - { - ConvexHull *chA = hulls[i]; + ConvexHull* chA = hulls[i]; - for (size_t j = 0; j < i && !mCanceled; j++) - { - ConvexHull *chB = hulls[j]; + for (size_t j = 0; j < i && !m_canceled; j++) + { + ConvexHull* chB = hulls[j]; - task->mHullA = chA; - task->mHullB = chB; - task->mThis = this; + CostTask t; + t.m_hullA = chA; + t.m_hullB = chB; + t.m_this = this; - if ( doFastCost(task) ) - { - } - else - { + if ( DoFastCost(t) ) + { + } + else + { + tasks.push_back(std::move(t)); + std::vector::reverse_iterator task = tasks.rbegin(); #if !VHACD_DISABLE_THREADING - if ( mThreadPool ) + if ( m_threadPool ) + { + task->m_future = m_threadPool->enqueue([task] { - task->mFuture = mThreadPool->enqueue([task] - { - computeMergeCostTask(task); - }); - } -#endif - task++; + computeMergeCostTask(*task); + }); } +#endif } } - if ( !mCanceled ) - { - size_t taskCount = task - tasks; + } + + if ( !m_canceled ) + { #if !VHACD_DISABLE_THREADING - if ( mThreadPool ) + if ( m_threadPool ) + { + for (CostTask& task : tasks) { - if ( taskCount ) - { - for (uint32_t i = 0; i < taskCount; i++) - { - tasks[i].mFuture.get(); - } - } - for (size_t i = 0; i < taskCount; i++) - { - addCostToPriorityQueue(&tasks[i]); - } + task.m_future.get(); } - else + + for (CostTask& task : tasks) + { + AddCostToPriorityQueue(task); + } + } + else #endif + { + for (CostTask& task : tasks) { - task = tasks; - for (size_t i = 0; i < taskCount; i++) - { - performMergeCostTask(task); - addCostToPriorityQueue(task); - task++; - } + PerformMergeCostTask(task); + AddCostToPriorityQueue(task); } - progressUpdate(Stages::COMPUTING_COST_MATRIX, - 100, - "Finished cost matrix"); } - if ( !mCanceled ) + ProgressUpdate(Stages::COMPUTING_COST_MATRIX, + 100, + "Finished cost matrix"); + } + + if ( !m_canceled ) + { + ScopedTime stMerging("Merging Convex Hulls", + m_params.m_logger); + Timer t; + // Now that we know the cost to merge each hull, we can begin merging them. + bool cancel = false; + + uint32_t maxMergeCount = uint32_t(m_hulls.size()) - m_params.m_maxConvexHulls; + uint32_t startCount = uint32_t(m_hulls.size()); + + while ( !cancel + && m_hulls.size() > m_params.m_maxConvexHulls + && !m_hullPairQueue.empty() + && !m_canceled) { - ScopedTime stMerging("Merging Convex Hulls", - mParams.m_logger); - Timer t; - // Now that we know the cost to merge each hull, we can begin merging them. - bool cancel = false; - - uint32_t maxMergeCount = uint32_t(mHulls.size()) - mParams.m_maxConvexHulls; - uint32_t startCount = uint32_t(mHulls.size()); - - while ( !cancel - && mHulls.size() > mParams.m_maxConvexHulls - && !mHullPairQueue.empty() - && !mCanceled) + double e = t.PeekElapsedSeconds(); + if ( e >= double(0.1) ) { - double e = t.peekElapsedSeconds(); - if ( e >= 0.1 ) - { - t.reset(); - uint32_t hullsProcessed = startCount - uint32_t(mHulls.size() ); - double stageProgess = double(hullsProcessed * 100) / double(maxMergeCount); - progressUpdate(Stages::MERGING_CONVEX_HULLS, - stageProgess, - "Merging Convex Hulls"); - } + t.Reset(); + uint32_t hullsProcessed = startCount - uint32_t(m_hulls.size() ); + double stageProgess = double(hullsProcessed * 100) / double(maxMergeCount); + ProgressUpdate(Stages::MERGING_CONVEX_HULLS, + stageProgess, + "Merging Convex Hulls"); + } - HullPair hp = mHullPairQueue.top(); - mHullPairQueue.pop(); + HullPair hp = m_hullPairQueue.top(); + m_hullPairQueue.pop(); - // It is entirely possible that the hull pair queue can - // have references to convex hulls that are no longer valid - // because they were previously merged. So we check for this - // and if either hull referenced in this pair no longer - // exists, then we skip it. + // It is entirely possible that the hull pair queue can + // have references to convex hulls that are no longer valid + // because they were previously merged. So we check for this + // and if either hull referenced in this pair no longer + // exists, then we skip it. - // Look up this pair of hulls by ID - ConvexHull *ch1 = getHull(hp.mHullA); - ConvexHull *ch2 = getHull(hp.mHullB); + // Look up this pair of hulls by ID + ConvexHull* ch1 = GetHull(hp.m_hullA); + ConvexHull* ch2 = GetHull(hp.m_hullB); - // If both hulls are still valid, then we merge them, delete the old - // two hulls and recompute the cost matrix for the new combined hull - // we have created - if ( ch1 && ch2 ) + // If both hulls are still valid, then we merge them, delete the old + // two hulls and recompute the cost matrix for the new combined hull + // we have created + if ( ch1 && ch2 ) + { + // This is the convex hull which results from combining the + // vertices in the two source hulls + ConvexHull* combinedHull = ComputeCombinedConvexHull(*ch1, + *ch2); + // The two old convex hulls are going to get removed + RemoveHull(hp.m_hullA); + RemoveHull(hp.m_hullB); + + m_meshId++; + combinedHull->m_meshId = m_meshId; + CostTask* taskCost = tasks.data(); + + // Compute the cost between this new merged hull + // and all existing convex hulls and then + // add that to the priority queue + for (auto& i : m_hulls) { - // This is the convex hull which results from combining the - // vertices in the two source hulls - ConvexHull *combinedHull = computeCombinedConvexHull(*ch1, - *ch2); - // The two old convex hulls are going to get removed - removeHull(hp.mHullA); - removeHull(hp.mHullB); - - mMeshId++; - combinedHull->m_meshId = mMeshId; - CostTask *taskCost = tasks; - - // Compute the cost between this new merged hull - // and all existing convex hulls and then - // add that to the priority queue - for (auto &i : mHulls) + if ( m_canceled ) { - if ( mCanceled ) - { - break; - } - ConvexHull *secondHull = i.second; - taskCost->mHullA = combinedHull; - taskCost->mHullB = secondHull; - taskCost->mThis = this; - if ( doFastCost(taskCost) ) - { - } - else - { - taskCost++; - } + break; } - mHulls[combinedHull->m_meshId] = combinedHull; - // See how many merge cost tasks were posted - // If there are 8 or more and we are running asynchronously, then do them that way. - size_t tcount = taskCost - tasks; -#if !VHACD_DISABLE_THREADING - if ( mThreadPool && tcount >= 2 ) + ConvexHull* secondHull = i.second; + taskCost->m_hullA = combinedHull; + taskCost->m_hullB = secondHull; + taskCost->m_this = this; + if ( DoFastCost(*taskCost) ) { - taskCost = tasks; - for (uint32_t i = 0; i < tcount; i++) - { - taskCost->mFuture = mThreadPool->enqueue([taskCost] - { - computeMergeCostTask(taskCost); - }); - taskCost++; - } - for (uint32_t i=0; im_meshId] = combinedHull; + // See how many merge cost tasks were posted + // If there are 8 or more and we are running asynchronously, then do them that way. + size_t tcount = taskCost - tasks.data(); +#if !VHACD_DISABLE_THREADING + if ( m_threadPool && tcount >= 2 ) + { + taskCost = tasks.data(); + for (uint32_t i = 0; i < tcount; i++) + { + taskCost->m_future = m_threadPool->enqueue([taskCost] { - performMergeCostTask(taskCost); - taskCost++; - } + computeMergeCostTask(*taskCost); + }); + taskCost++; } - for (size_t i = 0; i < tcount; i++) + for (uint32_t i = 0; i < tcount; i++) { - addCostToPriorityQueue(&tasks[i]); + tasks[i].m_future.get(); } } - } - // Ok...once we are done, we copy the results! - mMeshId -= 0; - progressUpdate(Stages::FINALIZING_RESULTS, - 0, - "Finalizing results"); - for (auto &i:mHulls) - { - if ( mCanceled ) + else +#endif { - break; + taskCost = tasks.data(); + for (size_t i = 0; i < tcount; i++) + { + PerformMergeCostTask(*taskCost); + taskCost++; + } } - ConvexHull *ch = i.second; - // We now must reduce the convex hull - if ( ch->m_points.size() > mParams.m_maxNumVerticesPerCH || mParams.m_shrinkWrap) + for (size_t i = 0; i < tcount; i++) { - ConvexHull *reduce = computeReducedConvexHull(*ch, - mParams.m_maxNumVerticesPerCH, - mParams.m_shrinkWrap); - releaseConvexHull(ch); - ch = reduce; + AddCostToPriorityQueue(tasks[i]); } - scaleOutputConvexHull(*ch); - ch->m_meshId = mMeshId; - mMeshId++; - mConvexHulls.push_back(ch); } - mHulls.clear(); // since the hulls were moved into the output list, we don't need to delete them from this container - progressUpdate(Stages::FINALIZING_RESULTS, - 100, - "Finalized results complete"); } - delete []tasks; - - } - else - { - progressUpdate(Stages::FINALIZING_RESULTS, + // Ok...once we are done, we copy the results! + m_meshId -= 0; + ProgressUpdate(Stages::FINALIZING_RESULTS, 0, "Finalizing results"); - mMeshId = 0; - for (auto &ch:hulls) + for (auto& i : m_hulls) { - // We now must reduce the convex hull - if ( ch->m_points.size() > mParams.m_maxNumVerticesPerCH || mParams.m_shrinkWrap ) + if ( m_canceled ) + { + break; + } + ConvexHull* ch = i.second; + // We now must reduce the convex hull + if ( ch->m_points.size() > m_params.m_maxNumVerticesPerCH || m_params.m_shrinkWrap) { - ConvexHull *reduce = computeReducedConvexHull(*ch, - mParams.m_maxNumVerticesPerCH, - mParams.m_shrinkWrap); - releaseConvexHull(ch); + ConvexHull* reduce = ComputeReducedConvexHull(*ch, + m_params.m_maxNumVerticesPerCH, + m_params.m_shrinkWrap); + ReleaseConvexHull(ch); ch = reduce; } - scaleOutputConvexHull(*ch); - ch->m_meshId = mMeshId; - mMeshId++; - mConvexHulls.push_back(ch); + ScaleOutputConvexHull(*ch); + ch->m_meshId = m_meshId; + m_meshId++; + m_convexHulls.push_back(ch); } - mHulls.clear(); - progressUpdate(Stages::FINALIZING_RESULTS, + m_hulls.clear(); // since the hulls were moved into the output list, we don't need to delete them from this container + ProgressUpdate(Stages::FINALIZING_RESULTS, 100, - "Finalized results"); + "Finalized results complete"); } - - } - } - - double computeConvexHullVolume(const ConvexHull &sm) - { - double totalVolume = 0; - nd::VHACD::Vect3 bary(0, 0, 0); - for (uint32_t i = 0; i < sm.m_points.size(); i++) - { - nd::VHACD::Vect3 p(sm.m_points[i]); - bary += p; - } - bary /= double(sm.m_points.size()); - - for (uint32_t i = 0; i < sm.m_triangles.size(); i++) - { - uint32_t i1 = sm.m_triangles[i].mI0; - uint32_t i2 = sm.m_triangles[i].mI1; - uint32_t i3 = sm.m_triangles[i].mI2; - - nd::VHACD::Vect3 ver0(sm.m_points[i1]); - nd::VHACD::Vect3 ver1(sm.m_points[i2]); - nd::VHACD::Vect3 ver2(sm.m_points[i3]); - - totalVolume += computeVolume4(ver0, - ver1, - ver2, - bary); - } - totalVolume = totalVolume / 6.0; - return totalVolume; - } - - double computeVolume4(const nd::VHACD::Vect3& a, - const nd::VHACD::Vect3& b, - const nd::VHACD::Vect3& c, - const nd::VHACD::Vect3& d) - { - nd::VHACD::Vect3 ad = a - d; - nd::VHACD::Vect3 bd = b - d; - nd::VHACD::Vect3 cd = c - d; - nd::VHACD::Vect3 bcd = bd.Cross(cd); - double dot = ad.Dot(bcd); - return dot; - } - - double computeConcavity(double volumeSeparate,double volumeCombined,double volumeMesh) - { - return fabs(volumeSeparate - volumeCombined) / volumeMesh; - } - - // See if we can compute the cost without having to actually merge convex hulls. - // If the axis aligned bounding boxes (slightly inflated) of the two convex hulls - // do not intersect, then we don't need to actually compute the merged convex hull - // volume. - inline bool doFastCost(CostTask *mt) - { - bool ret = false; - - ConvexHull *ch1 = mt->mHullA; - ConvexHull *ch2 = mt->mHullB; - - double volume1 = ch1->m_volume; - double volume2 = ch2->m_volume; - double concavity = FLT_MAX; - - if ( !fm_intersectAABB(ch1->mBmin, ch1->mBmax, ch2->mBmin, ch2->mBmax)) + else { - nd::VHACD::Vect3 bmin; - nd::VHACD::Vect3 bmax; - fm_combineAABB(ch1->mBmin, - ch1->mBmax, - ch2->mBmin, - ch2->mBmax, - bmin, - bmax); - double combinedVolume = fm_volumeAABB(bmin, - bmax); - concavity = computeConcavity(volume1 + volume2, - combinedVolume, - mOverallHullVolume); - HullPair hp(ch1->m_meshId, - ch2->m_meshId, - concavity); - mHullPairQueue.push(hp); - ret = true; + ProgressUpdate(Stages::FINALIZING_RESULTS, + 0, + "Finalizing results"); + m_meshId = 0; + for (auto& ch : hulls) + { + // We now must reduce the convex hull + if ( ch->m_points.size() > m_params.m_maxNumVerticesPerCH || m_params.m_shrinkWrap ) + { + ConvexHull* reduce = ComputeReducedConvexHull(*ch, + m_params.m_maxNumVerticesPerCH, + m_params.m_shrinkWrap); + ReleaseConvexHull(ch); + ch = reduce; + } + ScaleOutputConvexHull(*ch); + ch->m_meshId = m_meshId; + m_meshId++; + m_convexHulls.push_back(ch); + } + m_hulls.clear(); + ProgressUpdate(Stages::FINALIZING_RESULTS, + 100, + "Finalized results"); } - return ret; } +} - void performMergeCostTask(CostTask *mt) +double VHACDImpl::ComputeConvexHullVolume(const ConvexHull& sm) +{ + double totalVolume = 0; + VHACD::Vect3 bary(0, 0, 0); + for (uint32_t i = 0; i < sm.m_points.size(); i++) { - ConvexHull *ch1 = mt->mHullA; - ConvexHull *ch2 = mt->mHullB; - - double volume1 = ch1->m_volume; - double volume2 = ch2->m_volume; - - ConvexHull *combined = computeCombinedConvexHull(*ch1, - *ch2); // Build the combined convex hull - double combinedVolume = computeConvexHullVolume(*combined); // get the combined volume - mt->mConcavity = computeConcavity(volume1 + volume2, - combinedVolume, - mOverallHullVolume); - releaseConvexHull(combined); + VHACD::Vect3 p(sm.m_points[i]); + bary += p; } + bary /= double(sm.m_points.size()); - ConvexHull *computeReducedConvexHull(const ConvexHull &ch, - uint32_t maxVerts, - bool projectHullVertices) + for (uint32_t i = 0; i < sm.m_triangles.size(); i++) { - SimpleMesh sourceConvexHull; - - sourceConvexHull.mVertices = ch.m_points; - sourceConvexHull.mIndices = ch.m_triangles; + uint32_t i1 = sm.m_triangles[i].mI0; + uint32_t i2 = sm.m_triangles[i].mI1; + uint32_t i3 = sm.m_triangles[i].mI2; - ShrinkWrapImpl sw; - sw.shrinkWrap(sourceConvexHull, - mRaycastMesh, - maxVerts, - mVoxelScale * 4, - projectHullVertices); + VHACD::Vect3 ver0(sm.m_points[i1]); + VHACD::Vect3 ver1(sm.m_points[i2]); + VHACD::Vect3 ver2(sm.m_points[i3]); - ConvexHull *ret = new ConvexHull; + totalVolume += ComputeVolume4(ver0, + ver1, + ver2, + bary); - ret->m_points = sourceConvexHull.mVertices; - ret->m_triangles = sourceConvexHull.mIndices; - - fm_getAABB(ret->m_points, - ret->mBmin, - ret->mBmax); - fm_inflateMinMax(ret->mBmin, - ret->mBmax, - 0.1); - fm_computeCentroid(ret->m_points, - ret->m_triangles, - ret->m_center); - - ret->m_volume = computeConvexHullVolume(*ret); - - // Return the convex hull - return ret; - } - - // Take the points in convex hull A and the points in convex hull B and generate - // a new convex hull on the combined set of points. - // Once completed, we create a SimpleMesh instance to hold the triangle mesh - // and we compute an inflated AABB for it. - ConvexHull* computeCombinedConvexHull(const ConvexHull& sm1, - const ConvexHull& sm2) - { - uint32_t vcount = uint32_t(sm1.m_points.size() + sm2.m_points.size()); // Total vertices from both hulls - std::vector vertices(vcount); - auto it = std::copy(sm1.m_points.begin(), - sm1.m_points.end(), - vertices.begin()); - std::copy(sm2.m_points.begin(), - sm2.m_points.end(), - it); - - VHACD::QuickHullImpl qh; - qh.computeConvexHull(vertices, - vcount); - - const std::vector& hvertices = qh.getVertices(); - uint32_t hvcount = uint32_t(hvertices.size()); - const std::vector& hindices = qh.getIndices(); - uint32_t htcount = uint32_t(hindices.size()); - - ConvexHull *ret = new ConvexHull; - ret->m_points.resize(hvcount); - std::copy(hvertices.begin(), - hvertices.end(), - ret->m_points.begin()); - - ret->m_triangles.resize(htcount); - std::copy(hindices.begin(), - hindices.end(), - ret->m_triangles.begin()); - - ret->m_volume = computeConvexHullVolume(*ret); - - fm_getAABB(hvertices, - ret->mBmin, - ret->mBmax); - fm_inflateMinMax(ret->mBmin, - ret->mBmax, - 0.1); - fm_computeCentroid(ret->m_points, - ret->m_triangles, - ret->m_center); - - // Return the convex hull - return ret; } + totalVolume = totalVolume / double(6.0); + return totalVolume; +} +double VHACDImpl::ComputeVolume4(const VHACD::Vect3& a, + const VHACD::Vect3& b, + const VHACD::Vect3& c, + const VHACD::Vect3& d) +{ + VHACD::Vect3 ad = a - d; + VHACD::Vect3 bd = b - d; + VHACD::Vect3 cd = c - d; + VHACD::Vect3 bcd = bd.Cross(cd); + double dot = ad.Dot(bcd); + return dot; +} - ConvexHull *getHull(uint32_t index) - { - ConvexHull *ret = nullptr; +double VHACDImpl::ComputeConcavity(double volumeSeparate, + double volumeCombined, + double volumeMesh) +{ + return fabs(volumeSeparate - volumeCombined) / volumeMesh; +} - HullMap::iterator found = mHulls.find(index); - if ( found != mHulls.end() ) - { - ret = (*found).second; - } +bool VHACDImpl::DoFastCost(CostTask& mt) +{ + bool ret = false; - return ret; + ConvexHull* ch1 = mt.m_hullA; + ConvexHull* ch2 = mt.m_hullB; + + VHACD::BoundsAABB ch1b(ch1->mBmin, + ch1->mBmax); + VHACD::BoundsAABB ch2b(ch2->mBmin, + ch2->mBmax); + if (!ch1b.Intersects(ch2b)) + { + VHACD::BoundsAABB b = ch1b.Union(ch2b); + + double combinedVolume = b.Volume(); + double concavity = ComputeConcavity(ch1->m_volume + ch2->m_volume, + combinedVolume, + m_overallHullVolume); + HullPair hp(ch1->m_meshId, + ch2->m_meshId, + concavity); + m_hullPairQueue.push(hp); + ret = true; } + return ret; +} - bool removeHull(uint32_t index) - { - bool ret = false; - HullMap::iterator found = mHulls.find(index); - if ( found != mHulls.end() ) - { - ret = true; - releaseConvexHull((*found).second); - mHulls.erase(found); - } - return ret; - } +void VHACDImpl::PerformMergeCostTask(CostTask& mt) +{ + ConvexHull* ch1 = mt.m_hullA; + ConvexHull* ch2 = mt.m_hullB; + + double volume1 = ch1->m_volume; + double volume2 = ch2->m_volume; + + ConvexHull* combined = ComputeCombinedConvexHull(*ch1, + *ch2); // Build the combined convex hull + double combinedVolume = ComputeConvexHullVolume(*combined); // get the combined volume + mt.m_concavity = ComputeConcavity(volume1 + volume2, + combinedVolume, + m_overallHullVolume); + ReleaseConvexHull(combined); +} - ConvexHull *copyConvexHull(const ConvexHull &source) - { - ConvexHull *ch = new ConvexHull; +IVHACD::ConvexHull* VHACDImpl::ComputeReducedConvexHull(const ConvexHull& ch, + uint32_t maxVerts, + bool projectHullVertices) +{ + SimpleMesh sourceConvexHull; - ch->mBmin = source.mBmin; - ch->mBmax = source.mBmax; + sourceConvexHull.m_vertices = ch.m_points; + sourceConvexHull.m_indices = ch.m_triangles; - ch->m_center = source.m_center; + ShrinkWrap(sourceConvexHull, + m_AABBTree, + maxVerts, + m_voxelScale * 4, + projectHullVertices); - ch->m_meshId = source.m_meshId; + ConvexHull *ret = new ConvexHull; - ch->m_points = source.m_points; + ret->m_points = sourceConvexHull.m_vertices; + ret->m_triangles = sourceConvexHull.m_indices; - ch->m_triangles = source.m_triangles; + VHACD::BoundsAABB b = VHACD::BoundsAABB(ret->m_points).Inflate(double(0.1)); + ret->mBmin = b.GetMin(); + ret->mBmax = b.GetMax(); + ComputeCentroid(ret->m_points, + ret->m_triangles, + ret->m_center); - ch->m_volume = source.m_volume; + ret->m_volume = ComputeConvexHullVolume(*ret); - return ch; - } + // Return the convex hull + return ret; +} - void progressUpdate(Stages stage,double stageProgress, - const char *operation) - { - if ( mParams.m_callback ) - { - double overallProgress = (double(stage) * 100) / double(Stages::NUM_STAGES); - const char *s = getStageName(stage); - mParams.m_callback->Update(overallProgress, - stageProgress, - s, - operation); - } - } +IVHACD::ConvexHull* VHACDImpl::ComputeCombinedConvexHull(const ConvexHull& sm1, + const ConvexHull& sm2) +{ + uint32_t vcount = uint32_t(sm1.m_points.size() + sm2.m_points.size()); // Total vertices from both hulls + std::vector vertices(vcount); + auto it = std::copy(sm1.m_points.begin(), + sm1.m_points.end(), + vertices.begin()); + std::copy(sm2.m_points.begin(), + sm2.m_points.end(), + it); + + VHACD::QuickHull qh; + qh.ComputeConvexHull(vertices, + vcount); + + ConvexHull* ret = new ConvexHull; + ret->m_points = qh.GetVertices(); + ret->m_triangles = qh.GetIndices(); + + ret->m_volume = ComputeConvexHullVolume(*ret); + + VHACD::BoundsAABB b = VHACD::BoundsAABB(qh.GetVertices()).Inflate(double(0.1)); + ret->mBmin = b.GetMin(); + ret->mBmax = b.GetMax(); + ComputeCentroid(ret->m_points, + ret->m_triangles, + ret->m_center); + + // Return the convex hull + return ret; +} - const char *getStageName(Stages stage) const - { - const char *ret = "unknown"; - switch ( stage ) - { - case Stages::COMPUTE_BOUNDS_OF_INPUT_MESH: - ret = "COMPUTE_BOUNDS_OF_INPUT_MESH"; - break; - case Stages::REINDEXING_INPUT_MESH: - ret = "REINDEXING_INPUT_MESH"; - break; - case Stages::CREATE_RAYCAST_MESH: - ret = "CREATE_RAYCAST_MESH"; - break; - case Stages::VOXELIZING_INPUT_MESH: - ret = "VOXELIZING_INPUT_MESH"; - break; - case Stages::BUILD_INITIAL_CONVEX_HULL: - ret = "BUILD_INITIAL_CONVEX_HULL"; - break; - case Stages::PERFORMING_DECOMPOSITION: - ret = "PERFORMING_DECOMPOSITION"; - break; - case Stages::INITIALIZING_CONVEX_HULLS_FOR_MERGING: - ret = "INITIALIZING_CONVEX_HULLS_FOR_MERGING"; - break; - case Stages::COMPUTING_COST_MATRIX: - ret = "COMPUTING_COST_MATRIX"; - break; - case Stages::MERGING_CONVEX_HULLS: - ret = "MERGING_CONVEX_HULLS"; - break; - case Stages::FINALIZING_RESULTS: - ret = "FINALIZING_RESULTS"; - break; - } - return ret; - } +IVHACD::ConvexHull* VHACDImpl::GetHull(uint32_t index) +{ + ConvexHull* ret = nullptr; - bool isCanceled(void) const override final + auto found = m_hulls.find(index); + if ( found != m_hulls.end() ) { - return mCanceled; + ret = found->second; } - std::atomic mCanceled{false}; - Parameters mParams; // Convex decomposition parameters + return ret; +} - ConvexHullVector mConvexHulls; // Finalized convex hulls - VoxelHullVector mVoxelHulls; // completed voxel hulls - VoxelHullVector mPendingHulls; +bool VHACDImpl::RemoveHull(uint32_t index) +{ + bool ret = false; + auto found = m_hulls.find(index); + if ( found != m_hulls.end() ) + { + ret = true; + ReleaseConvexHull(found->second); + m_hulls.erase(found); + } + return ret; +} - AABBTreeVector mTrees; - VHACD::MyRaycastMesh mRaycastMesh; - VHACD::VoxelizeImpl mVoxelize; - nd::VHACD::Vect3 mCenter; - double mScale{1}; - double mRecipScale{1}; - SimpleMesh mInputMesh; // re-indexed and normalized input mesh - std::vector mVertices; - std::vector mIndices; +IVHACD::ConvexHull* VHACDImpl::CopyConvexHull(const ConvexHull& source) +{ + ConvexHull *ch = new ConvexHull; + *ch = source; - double mOverallHullVolume{0}; - double mVoxelScale{0}; - double mVoxelHalfScale{0}; - nd::VHACD::Vect3 mVoxelBmin; - nd::VHACD::Vect3 mVoxelBmax; - uint32_t mMeshId{0}; - HullPairQueue mHullPairQueue; -#if !VHACD_DISABLE_THREADING - ThreadPool *mThreadPool{nullptr}; -#endif - HullMap mHulls; + return ch; +} - double mOverallProgress{0}; - double mStageProgress{0}; - double mOperationProgress{0}; -}; +const char* VHACDImpl::GetStageName(Stages stage) const +{ + const char *ret = "unknown"; + switch ( stage ) + { + case Stages::COMPUTE_BOUNDS_OF_INPUT_MESH: + ret = "COMPUTE_BOUNDS_OF_INPUT_MESH"; + break; + case Stages::REINDEXING_INPUT_MESH: + ret = "REINDEXING_INPUT_MESH"; + break; + case Stages::CREATE_RAYCAST_MESH: + ret = "CREATE_RAYCAST_MESH"; + break; + case Stages::VOXELIZING_INPUT_MESH: + ret = "VOXELIZING_INPUT_MESH"; + break; + case Stages::BUILD_INITIAL_CONVEX_HULL: + ret = "BUILD_INITIAL_CONVEX_HULL"; + break; + case Stages::PERFORMING_DECOMPOSITION: + ret = "PERFORMING_DECOMPOSITION"; + break; + case Stages::INITIALIZING_CONVEX_HULLS_FOR_MERGING: + ret = "INITIALIZING_CONVEX_HULLS_FOR_MERGING"; + break; + case Stages::COMPUTING_COST_MATRIX: + ret = "COMPUTING_COST_MATRIX"; + break; + case Stages::MERGING_CONVEX_HULLS: + ret = "MERGING_CONVEX_HULLS"; + break; + case Stages::FINALIZING_RESULTS: + ret = "FINALIZING_RESULTS"; + break; + case Stages::NUM_STAGES: + // Should be unreachable, here to silence enumeration value not handled in switch warnings + // GCC/Clang's -Wswitch + break; + } + return ret; +} -void jobCallback(void *userPtr) +void VHACDImpl::ProgressUpdate(Stages stage, + double stageProgress, + const char* operation) { - VoxelHull *vh = static_cast(userPtr); - vh->performPlaneSplit(); + if ( m_params.m_callback ) + { + double overallProgress = (double(stage) * 100) / double(Stages::NUM_STAGES); + const char *s = GetStageName(stage); + m_params.m_callback->Update(overallProgress, + stageProgress, + s, + operation); + } } -void computeMergeCostTask(void *ptr) +bool VHACDImpl::IsCanceled() const { - CostTask *ct = static_cast(ptr); - ct->mThis->performMergeCostTask(ct); + return m_canceled; } IVHACD* CreateVHACD(void) @@ -8834,346 +8004,370 @@ IVHACD* CreateVHACD(void); class LogMessage { public: - double mOverallProgress{-1}; - double mStageProgress{-1}; + double mOverallProgress{ double(-1.0) }; + double mStageProgress{ double(-1.0) }; std::string mStage; std::string mOperation; }; -using LogMessageVector = std::vector< LogMessage >; - -class MyHACD_API : public VHACD::IVHACD, - public VHACD::IVHACD::IUserCallback, - VHACD::IVHACD::IUserLogger, - VHACD::IVHACD::IUserTaskRunner +class VHACDAsyncImpl : public VHACD::IVHACD, + public VHACD::IVHACD::IUserCallback, + VHACD::IVHACD::IUserLogger, + VHACD::IVHACD::IUserTaskRunner { public: - MyHACD_API() - { - mVHACD = VHACD::CreateVHACD(); - } - - ~MyHACD_API() override - { - Cancel(); - if ( mVHACD ) - { - mVHACD->Release(); - } - } + VHACDAsyncImpl() = default; - void* StartTask(std::function func) override - { - return new std::thread(func); - } + ~VHACDAsyncImpl() override; - void JoinTask(void* Task) override - { - std::thread* t = static_cast(Task); - t->join(); - delete t; - } + void Cancel() override final; bool Compute(const float* const points, const uint32_t countPoints, const uint32_t* const triangles, const uint32_t countTriangles, - const Parameters& params) override final - { - mVertices.reserve(countPoints * 3); - for (uint32_t i = 0; i < countPoints; ++i) - { - mVertices.push_back(points[i * 3 + 0]); - mVertices.push_back(points[i * 3 + 1]); - mVertices.push_back(points[i * 3 + 2]); - } - - mIndices.reserve(countTriangles * 3); - for (uint32_t i = 0; i < countTriangles; ++i) - { - mIndices.push_back(triangles[i * 3 + 0]); - mIndices.push_back(triangles[i * 3 + 1]); - mIndices.push_back(triangles[i * 3 + 2]); - } + const Parameters& params) override final; - return Compute(params); - } - - bool Compute(const double* const _points, + bool Compute(const double* const points, const uint32_t countPoints, - const uint32_t* const _triangles, + const uint32_t* const triangles, const uint32_t countTriangles, - const Parameters& _desc) override final - { - // We need to copy the input vertices and triangles into our own buffers so we can operate - // on them safely from the background thread. - mVertices.reserve(countPoints * 3); - for (uint32_t i = 0; i < countPoints; ++i) - { - mVertices.push_back(_points[i * 3 + 0]); - mVertices.push_back(_points[i * 3 + 1]); - mVertices.push_back(_points[i * 3 + 2]); - } + const Parameters& params) override final; - mIndices.reserve(countTriangles * 3); - for (uint32_t i = 0; i < countTriangles; ++i) - { - mIndices.push_back(_triangles[i * 3 + 0]); - mIndices.push_back(_triangles[i * 3 + 1]); - mIndices.push_back(_triangles[i * 3 + 2]); - } + bool GetConvexHull(const uint32_t index, + VHACD::IVHACD::ConvexHull& ch) const override final; - return Compute(_desc); - } + uint32_t GetNConvexHulls() const override final; - bool Compute(const Parameters& _desc) - { - Cancel(); // if we previously had a solution running; cancel it. + void Clean() override final; // release internally allocated memory - Parameters desc = _desc; - mTaskRunner = _desc.m_taskRunner ? _desc.m_taskRunner : this; - desc.m_taskRunner = mTaskRunner; + void Release() override final; // release IVHACD - mRunning = true; - mTask = mTaskRunner->StartTask([this, desc]() { - ComputeNow(mVertices, mIndices, desc); - // If we have a user provided callback and the user did *not* call 'cancel' we notify him that the - // task is completed. However..if the user selected 'cancel' we do not send a completed notification event. - if (desc.m_callback && !mCancel) - { - desc.m_callback->NotifyVHACDComplete(); - } - mRunning = false; - }); - return true; - } + // Will compute the center of mass of the convex hull decomposition results and return it + // in 'centerOfMass'. Returns false if the center of mass could not be computed. + bool ComputeCenterOfMass(double centerOfMass[3]) const override; - bool ComputeNow(const std::vector& points, - const std::vector& triangles, - const Parameters& _desc) - { - uint32_t ret = 0; + bool IsReady() const override final; - Parameters desc; - mCallback = _desc.m_callback; - mLogger = _desc.m_logger; + /** + * At the request of LegionFu : out_look@foxmail.com + * This method will return which convex hull is closest to the source position. + * You can use this method to figure out, for example, which vertices in the original + * source mesh are best associated with which convex hull. + * + * @param pos : The input 3d position to test against + * + * @return : Returns which convex hull this position is closest to. + */ + uint32_t findNearestConvexHull(const double pos[3], + double& distanceToHull) override final; - desc = _desc; - // Set our intercepting callback interfaces if non-null - desc.m_callback = _desc.m_callback ? this : nullptr; - desc.m_logger = _desc.m_logger ? this : nullptr; + void Update(const double overallProgress, + const double stageProgress, + const char* const stage, + const char *operation) override final; - // If not task runner provided, then use the default one - if (desc.m_taskRunner == nullptr) - { - desc.m_taskRunner = this; - } + void Log(const char* const msg) override final; - if (mVHACD) - { - bool ok = mVHACD->Compute(points.data(), - uint32_t(points.size() / 3), - triangles.data(), - uint32_t(triangles.size() / 3), - desc); - if (ok) - { - ret = mVHACD->GetNConvexHulls(); - } - } + void* StartTask(std::function func) override; - return ret ? true : false; - } + void JoinTask(void* Task) override; - bool GetConvexHull(const uint32_t index, - VHACD::IVHACD::ConvexHull& ch) const override final + bool Compute(const Parameters params); + + bool ComputeNow(const std::vector& points, + const std::vector& triangles, + const Parameters& _desc); + + // As a convenience for the calling application we only send it update and log messages from it's own main + // thread. This reduces the complexity burden on the caller by making sure it only has to deal with log + // messages in it's main application thread. + void ProcessPendingMessages() const; + +private: + VHACD::VHACDImpl m_VHACD; + std::vector m_vertices; + std::vector m_indices; + VHACD::IVHACD::IUserCallback* m_callback{ nullptr }; + VHACD::IVHACD::IUserLogger* m_logger{ nullptr }; + VHACD::IVHACD::IUserTaskRunner* m_taskRunner{ nullptr }; + void* m_task{ nullptr }; + std::atomic m_running{ false }; + std::atomic m_cancel{ false }; + + // Thread safe caching mechanism for messages and update status. + // This is so that caller always gets messages in his own thread + // Member variables are marked as 'mutable' since the message dispatch function + // is called from const query methods. + mutable std::mutex m_messageMutex; + mutable std::vector m_messages; + mutable std::atomic m_haveMessages{ false }; +}; + +VHACDAsyncImpl::~VHACDAsyncImpl() +{ + Cancel(); +} + +void VHACDAsyncImpl::Cancel() +{ + m_cancel = true; + m_VHACD.Cancel(); + + if (m_task) { - return mVHACD->GetConvexHull(index,ch); + m_taskRunner->JoinTask(m_task); // Wait for the thread to fully exit before we delete the instance + m_task = nullptr; } + m_cancel = false; // clear the cancel semaphore +} - void release() // release the HACD_API interface +bool VHACDAsyncImpl::Compute(const float* const points, + const uint32_t countPoints, + const uint32_t* const triangles, + const uint32_t countTriangles, + const Parameters& params) +{ + m_vertices.reserve(countPoints); + for (uint32_t i = 0; i < countPoints; ++i) { - delete this; + m_vertices.emplace_back(points[i * 3 + 0], + points[i * 3 + 1], + points[i * 3 + 2]); } - uint32_t getHullCount() + m_indices.reserve(countTriangles); + for (uint32_t i = 0; i < countTriangles; ++i) { - return mVHACD->GetNConvexHulls(); + m_indices.emplace_back(triangles[i * 3 + 0], + triangles[i * 3 + 1], + triangles[i * 3 + 2]); } - void Cancel() override final + return Compute(params); +} + +bool VHACDAsyncImpl::Compute(const double* const points, + const uint32_t countPoints, + const uint32_t* const triangles, + const uint32_t countTriangles, + const Parameters& params) +{ + // We need to copy the input vertices and triangles into our own buffers so we can operate + // on them safely from the background thread. + // Can't be local variables due to being asynchronous + m_vertices.reserve(countPoints); + for (uint32_t i = 0; i < countPoints; ++i) { - // printf("Entered Cancel\n"); - mCancel = true; - if (mVHACD) - { - // printf("Setting cancel on the V-HACD instance\n"); - mVHACD->Cancel(); // Set the cancel signal to the base VHACD - } - if (mTask) - { - // printf("JoinTask waiting for task to complete.\n"); - mTaskRunner->JoinTask(mTask); // Wait for the thread to fully exit before we delete the instance - mTask = nullptr; - } - // printf("Leaving Cancel routine\n"); - mCancel = false; // clear the cancel semaphore + m_vertices.emplace_back(points[i * 3 + 0], + points[i * 3 + 1], + points[i * 3 + 2]); } - uint32_t GetNConvexHulls() const override final + m_indices.reserve(countTriangles); + for (uint32_t i = 0; i < countTriangles; ++i) { - processPendingMessages(); - return mVHACD->GetNConvexHulls(); + m_indices.emplace_back(triangles[i * 3 + 0], + triangles[i * 3 + 1], + triangles[i * 3 + 2]); } - void Clean() override final // release internally allocated memory + return Compute(params); +} + +bool VHACDAsyncImpl::GetConvexHull(const uint32_t index, + VHACD::IVHACD::ConvexHull& ch) const +{ + return m_VHACD.GetConvexHull(index, + ch); +} + +uint32_t VHACDAsyncImpl::GetNConvexHulls() const +{ + ProcessPendingMessages(); + return m_VHACD.GetNConvexHulls(); +} + +void VHACDAsyncImpl::Clean() +{ + Cancel(); + m_VHACD.Clean(); +} + +void VHACDAsyncImpl::Release() +{ + delete this; +} + +bool VHACDAsyncImpl::ComputeCenterOfMass(double centerOfMass[3]) const +{ + bool ret = false; + + centerOfMass[0] = 0; + centerOfMass[1] = 0; + centerOfMass[2] = 0; + + if (IsReady()) { - Cancel(); - mVHACD->Clean(); + ret = m_VHACD.ComputeCenterOfMass(centerOfMass); } + return ret; +} + +bool VHACDAsyncImpl::IsReady() const +{ + ProcessPendingMessages(); + return !m_running; +} + +uint32_t VHACDAsyncImpl::findNearestConvexHull(const double pos[3], + double& distanceToHull) +{ + uint32_t ret = 0; // The default return code is zero - void Release() override final // release IVHACD + distanceToHull = 0; + // First, make sure that we have valid and completed results + if (IsReady() ) { - delete this; + ret = m_VHACD.findNearestConvexHull(pos,distanceToHull); } - void Update(const double overallProgress, - const double stageProgress, - const char* const stage, - const char *operation) override final + return ret; +} + +void VHACDAsyncImpl::Update(const double overallProgress, + const double stageProgress, + const char* const stage, + const char* operation) +{ + m_messageMutex.lock(); + LogMessage m; + m.mOperation = std::string(operation); + m.mOverallProgress = overallProgress; + m.mStageProgress = stageProgress; + m.mStage = std::string(stage); + m_messages.push_back(m); + m_haveMessages = true; + m_messageMutex.unlock(); +} + +void VHACDAsyncImpl::Log(const char* const msg) +{ + m_messageMutex.lock(); + LogMessage m; + m.mOperation = std::string(msg); + m_haveMessages = true; + m_messages.push_back(m); + m_messageMutex.unlock(); +} + +void* VHACDAsyncImpl::StartTask(std::function func) +{ + return new std::thread(func); +} + +void VHACDAsyncImpl::JoinTask(void* Task) +{ + std::thread* t = static_cast(Task); + t->join(); + delete t; +} + +bool VHACDAsyncImpl::Compute(Parameters params) +{ + Cancel(); // if we previously had a solution running; cancel it. + + m_taskRunner = params.m_taskRunner ? params.m_taskRunner : this; + params.m_taskRunner = m_taskRunner; + + m_running = true; + m_task = m_taskRunner->StartTask([this, params]() { + ComputeNow(m_vertices, + m_indices, + params); + // If we have a user provided callback and the user did *not* call 'cancel' we notify him that the + // task is completed. However..if the user selected 'cancel' we do not send a completed notification event. + if (params.m_callback && !m_cancel) + { + params.m_callback->NotifyVHACDComplete(); + } + m_running = false; + }); + return true; +} + +bool VHACDAsyncImpl::ComputeNow(const std::vector& points, + const std::vector& triangles, + const Parameters& _desc) +{ + uint32_t ret = 0; + + Parameters desc; + m_callback = _desc.m_callback; + m_logger = _desc.m_logger; + + desc = _desc; + // Set our intercepting callback interfaces if non-null + desc.m_callback = _desc.m_callback ? this : nullptr; + desc.m_logger = _desc.m_logger ? this : nullptr; + + // If not task runner provided, then use the default one + if (desc.m_taskRunner == nullptr) { - mMessageMutex.lock(); - LogMessage m; - m.mOperation = std::string(operation); - m.mOverallProgress = overallProgress; - m.mStageProgress = stageProgress; - m.mStage = std::string(stage); - mMessages.push_back(m); - mHaveMessages = true; - mMessageMutex.unlock(); + desc.m_taskRunner = this; } - void Log(const char* const msg) override final + bool ok = m_VHACD.Compute(points, + triangles, + desc); + if (ok) { - mMessageMutex.lock(); - LogMessage m; - m.mOperation = std::string(msg); - mHaveMessages = true; - mMessages.push_back(m); - mMessageMutex.unlock(); + ret = m_VHACD.GetNConvexHulls(); } - bool IsReady() const override final + return ret ? true : false; +} + +void VHACDAsyncImpl::ProcessPendingMessages() const +{ + if (m_cancel) { - processPendingMessages(); - return !mRunning; + return; } - - // As a convenience for the calling application we only send it update and log messages from it's own main - // thread. This reduces the complexity burden on the caller by making sure it only has to deal with log - // messages in it's main application thread. - void processPendingMessages() const + if ( m_haveMessages ) { - if (mCancel) - { - return; - } - if ( mHaveMessages ) + m_messageMutex.lock(); + for (auto& i : m_messages) { - mMessageMutex.lock(); - for (auto &i:mMessages) + if ( i.mOverallProgress == -1 ) { - if ( i.mOverallProgress == -1 ) - { - if ( mLogger ) - { - mLogger->Log(i.mOperation.c_str()); - } - } - else if ( mCallback ) + if ( m_logger ) { - mCallback->Update(i.mOverallProgress, - i.mStageProgress, - i.mStage.c_str(), - i.mOperation.c_str()); + m_logger->Log(i.mOperation.c_str()); } } - mMessages.clear(); - mHaveMessages = false; - mMessageMutex.unlock(); - } - } - - // Will compute the center of mass of the convex hull decomposition results and return it - // in 'centerOfMass'. Returns false if the center of mass could not be computed. - bool ComputeCenterOfMass(double centerOfMass[3]) const override - { - bool ret = false; - - centerOfMass[0] = 0; - centerOfMass[1] = 0; - centerOfMass[2] = 0; - - if (mVHACD && IsReady()) - { - ret = mVHACD->ComputeCenterOfMass(centerOfMass); + else if ( m_callback ) + { + m_callback->Update(i.mOverallProgress, + i.mStageProgress, + i.mStage.c_str(), + i.mOperation.c_str()); + } } - return ret; + m_messages.clear(); + m_haveMessages = false; + m_messageMutex.unlock(); } +} - /** - * At the request of LegionFu : out_look@foxmail.com - * This method will return which convex hull is closest to the source position. - * You can use this method to figure out, for example, which vertices in the original - * source mesh are best associated with which convex hull. - * - * @param pos : The input 3d position to test against - * - * @return : Returns which convex hull this position is closest to. - */ - uint32_t findNearestConvexHull(const double pos[3], - double &distanceToHull) override final - { - uint32_t ret = 0; // The default return code is zero - - distanceToHull = 0; - // First, make sure that we have valid and completed results - if (mVHACD && IsReady() ) - { - ret = mVHACD->findNearestConvexHull(pos,distanceToHull); - } - - return ret; - } - -private: - std::vector mVertices; - std::vector mIndices; - VHACD::IVHACD::IUserCallback* mCallback{ nullptr }; - VHACD::IVHACD::IUserLogger* mLogger{ nullptr }; - VHACD::IVHACD* mVHACD{ nullptr }; - VHACD::IVHACD::IUserTaskRunner* mTaskRunner{ nullptr }; - void* mTask{ nullptr }; - std::atomic mRunning{ false }; - std::atomic mCancel{ false }; - - // Thread safe caching mechanism for messages and update status. - // This is so that caller always gets messages in his own thread - // Member variables are marked as 'mutable' since the message dispatch function - // is called from const query methods. - mutable std::mutex mMessageMutex; - mutable LogMessageVector mMessages; - mutable std::atomic mHaveMessages{false}; -}; - -IVHACD* CreateVHACD_ASYNC(void) +IVHACD* CreateVHACD_ASYNC() { - MyHACD_API* m = new MyHACD_API; + VHACDAsyncImpl* m = new VHACDAsyncImpl; return static_cast(m); } #endif -}; +} // namespace VHACD #ifdef _MSC_VER #pragma warning(pop) From 36308e37033bd73b0cf491027a5c57ab54e9bfb2 Mon Sep 17 00:00:00 2001 From: Evangel Date: Sat, 30 Jul 2022 19:23:27 +1000 Subject: [PATCH 2/3] Fixing issues with MSVC --- include/VHACD.h | 80 +++++++++++++++++++++++-------------------------- 1 file changed, 38 insertions(+), 42 deletions(-) diff --git a/include/VHACD.h b/include/VHACD.h index a15744e0..33cff517 100644 --- a/include/VHACD.h +++ b/include/VHACD.h @@ -378,9 +378,6 @@ class IVHACD double m_volume{ 0 }; // The volume of the convex hull VHACD::Vect3 m_center{ 0, 0, 0 }; // The centroid of the convex hull uint32_t m_meshId{ 0 }; // A unique id for this convex hull -// VHACD::Vect3 m_bMin; // Should be these to keep consistent naming style, but don't want to break API -// VHACD::Vect3 m_bMax; // at least not yet, or any more than currently -// VHACD::BoundsAABB m_bounds; // Should be this to properly reflect what it is VHACD::Vect3 mBmin; // Bounding box minimum of the AABB VHACD::Vect3 mBmax; // Bounding box maximum of the AABB }; @@ -1797,7 +1794,7 @@ void Googol::ToString(char* const string) const void Googol::NegateMantissa(std::array& mantissa) const { uint64_t carrier = 1; - for (int i = mantissa.size() - 1; i >= 0; i--) + for (size_t i = mantissa.size() - 1; i >= 0 && i < mantissa.size(); i--) { uint64_t a = ~mantissa[i] + carrier; if (a) @@ -3384,7 +3381,7 @@ void ConvexHull::CalculateConvexHull3D(ConvexHullAABBTreeNode* vertexTree, if (!face1.m_mark && (face1.Evalue(m_points, p) < double(0.0))) { #ifdef _DEBUG - for (const auto* node : deleteList) + for (const auto node : deleteList) { assert(node != node1); } @@ -7512,7 +7509,7 @@ void VHACDImpl::PerformConvexDecomposition() else { tasks.push_back(std::move(t)); - std::vector::reverse_iterator task = tasks.rbegin(); + CostTask* task = &tasks.back(); #if !VHACD_DISABLE_THREADING if ( m_threadPool ) { @@ -7610,7 +7607,8 @@ void VHACDImpl::PerformConvexDecomposition() m_meshId++; combinedHull->m_meshId = m_meshId; - CostTask* taskCost = tasks.data(); + tasks.clear(); + tasks.reserve(m_hulls.size()); // Compute the cost between this new merged hull // and all existing convex hulls and then @@ -7622,51 +7620,49 @@ void VHACDImpl::PerformConvexDecomposition() break; } ConvexHull* secondHull = i.second; - taskCost->m_hullA = combinedHull; - taskCost->m_hullB = secondHull; - taskCost->m_this = this; - if ( DoFastCost(*taskCost) ) + CostTask t; + t.m_hullA = combinedHull; + t.m_hullB = secondHull; + t.m_this = this; + if ( DoFastCost(t) ) { } else { - taskCost++; + tasks.push_back(std::move(t)); } } m_hulls[combinedHull->m_meshId] = combinedHull; // See how many merge cost tasks were posted // If there are 8 or more and we are running asynchronously, then do them that way. - size_t tcount = taskCost - tasks.data(); #if !VHACD_DISABLE_THREADING - if ( m_threadPool && tcount >= 2 ) + if ( m_threadPool && tasks.size() >= 2) { - taskCost = tasks.data(); - for (uint32_t i = 0; i < tcount; i++) + for (CostTask& task : tasks) { - taskCost->m_future = m_threadPool->enqueue([taskCost] + task.m_future = m_threadPool->enqueue([&task] { - computeMergeCostTask(*taskCost); + computeMergeCostTask(task); }); - taskCost++; } - for (uint32_t i = 0; i < tcount; i++) + + for (CostTask& task : tasks) { - tasks[i].m_future.get(); + task.m_future.get(); } } else #endif { - taskCost = tasks.data(); - for (size_t i = 0; i < tcount; i++) + for (CostTask& task : tasks) { - PerformMergeCostTask(*taskCost); - taskCost++; + PerformMergeCostTask(task); } } - for (size_t i = 0; i < tcount; i++) + + for (CostTask& task : tasks) { - AddCostToPriorityQueue(tasks[i]); + AddCostToPriorityQueue(task); } } } @@ -8004,10 +8000,10 @@ IVHACD* CreateVHACD(void); class LogMessage { public: - double mOverallProgress{ double(-1.0) }; - double mStageProgress{ double(-1.0) }; - std::string mStage; - std::string mOperation; + double m_overallProgress{ double(-1.0) }; + double m_stageProgress{ double(-1.0) }; + std::string m_stage; + std::string m_operation; }; class VHACDAsyncImpl : public VHACD::IVHACD, @@ -8242,10 +8238,10 @@ void VHACDAsyncImpl::Update(const double overallProgress, { m_messageMutex.lock(); LogMessage m; - m.mOperation = std::string(operation); - m.mOverallProgress = overallProgress; - m.mStageProgress = stageProgress; - m.mStage = std::string(stage); + m.m_operation = std::string(operation); + m.m_overallProgress = overallProgress; + m.m_stageProgress = stageProgress; + m.m_stage = std::string(stage); m_messages.push_back(m); m_haveMessages = true; m_messageMutex.unlock(); @@ -8255,7 +8251,7 @@ void VHACDAsyncImpl::Log(const char* const msg) { m_messageMutex.lock(); LogMessage m; - m.mOperation = std::string(msg); + m.m_operation = std::string(msg); m_haveMessages = true; m_messages.push_back(m); m_messageMutex.unlock(); @@ -8339,19 +8335,19 @@ void VHACDAsyncImpl::ProcessPendingMessages() const m_messageMutex.lock(); for (auto& i : m_messages) { - if ( i.mOverallProgress == -1 ) + if ( i.m_overallProgress == -1 ) { if ( m_logger ) { - m_logger->Log(i.mOperation.c_str()); + m_logger->Log(i.m_operation.c_str()); } } else if ( m_callback ) { - m_callback->Update(i.mOverallProgress, - i.mStageProgress, - i.mStage.c_str(), - i.mOperation.c_str()); + m_callback->Update(i.m_overallProgress, + i.m_stageProgress, + i.m_stage.c_str(), + i.m_operation.c_str()); } } m_messages.clear(); From 63e5d9adcb6627201d59fd0210dba1f36dda2c6d Mon Sep 17 00:00:00 2001 From: Evangel Date: Sat, 30 Jul 2022 19:32:12 +1000 Subject: [PATCH 3/3] Removed some temporary stuff. --- include/VHACD.h | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/include/VHACD.h b/include/VHACD.h index 33cff517..39780808 100644 --- a/include/VHACD.h +++ b/include/VHACD.h @@ -115,19 +115,6 @@ namespace VHACD { -// #ifdef VHACD_SCALAR -// using VhScalar = VHACD_SCALAR; -// #else -// using VhScalar = double; -// #endif -// -// #ifdef VHACD_VECTOR -// using VhVector = VHACD_VECTOR; -// #else -// template > -// using VhVector = std::vector; -// #endif - struct Vertex { double mX;