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..39780808 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. @@ -146,74 +146,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 + * Comparison operators + */ + bool operator<(const Vector3& rhs) const; + bool operator>(const Vector3& rhs) const; + + /* + * 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 +222,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 +318,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 +359,14 @@ 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 mBmin; // Bounding box minimum of the AABB + VHACD::Vect3 mBmax; // Bounding box maximum of the AABB }; /** @@ -348,20 +378,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 +447,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 +462,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 +478,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 +508,7 @@ IVHACD* CreateVHACD_ASYNC(void); // Create an asynchronous (non-blocking) imp #include #include #include +#include #include #include #include @@ -488,3662 +520,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(); - } - return getZ(); - } +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 (size_t i = mantissa.size() - 1; i >= 0 && i < mantissa.size(); 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 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); + } + + 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); + } + } +} + +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); - template - inline T Vect3::MinCoeff(uint32_t& idx) const - { - if (getX() < getY() && getX() < getZ()) + if (i0 == 0) { - idx = 0; - return getX(); + i0 = count / 2; } - if (getY() < getZ()) + if (i0 >= (count - 1)) { - idx = 1; - return getY(); + i0 = count / 2; } - idx = 2; - return getZ(); - } - template - inline T Vect3::MaxCoeff(uint32_t& idx) const - { - if (getX() > getY() && getX() < getZ()) - { - idx = 0; - return getX(); - } - if (getY() > getZ()) - { - idx = 1; - return getY(); - } - idx = 2; - return getZ(); + 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; /* - * Constructors + * WARNING: Changing the compiler conversion of 1.0e-3f changes the results of the convex decomposition + * Inflate the tree's bounding box slightly */ - 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 + 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) { - 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; + } + return BuildTreeRecurse(nullptr, + points.data(), + count, + 0, + memoryPool); +} +ConvexHullAABBTreeNode* ConvexHull::BuildTreeNew(std::vector& points, + std::vector& memoryPool) const +{ + class dCluster + { public: - int m_index[3]; + VHACD::Vect3 m_sum{ double(0.0) }; + VHACD::Vect3 m_sum2{ double(0.0) }; + int m_start{ 0 }; + int m_count{ 0 }; + }; - private: - int m_mark; - List::ndNode* m_twin[3]; + dCluster firstCluster; + firstCluster.m_count = int(points.size()); - friend class ConvexHull; - }; + 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; - class ConvexHull : public List + if (firstCluster.m_count > clusterSize) { - class ndNormalMap; + dCluster spliteStack[128]; + spliteStack[0] = firstCluster; + size_t stack = 1; - public: - ConvexHull(const ConvexHull& source); - ConvexHull(const std::vector<::VHACD::Vertex>& vertexCloud, - double distTol, - int maxVertexCount = 0x7fffffff); - ~ConvexHull() = default; + while (stack) + { + stack--; + dCluster cluster (spliteStack[stack]); - const std::vector>& GetVertexPool() const; + 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(); - 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 ( (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; + } + } + } -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); - } - } -} + 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]; -//*********************************************************************************************** -// End of ConvexHull generation code by Julio Jerez -//*********************************************************************************************** + int i0 = 0; + int i1 = cluster.m_count - 1; -// VertexIndex support -namespace VERTEX_INDEX -{ + const int start = cluster.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 < cluster.m_count)) + { + ++i0; + }; -class KdTreeFindNode -{ -public: - KdTreeFindNode(void) + #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++; + } + } + } + + points.resize(baseCount); + if (baseCount < 4) { - mNode = 0; - mDistance = 0; + return nullptr; } - KdTreeNode* mNode; - double mDistance; -}; -class KdTreeNode; -class KdTreeNodeBundle; + VHACD::Vect3 sum(0); + VHACD::Vect3 sum2(0); + VHACD::Vect3 minP(double( 1.0e15)); + VHACD::Vect3 maxP(double(-1.0e15)); + class dTreeBox + { + public: + 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; + }; -class KdTree -{ -public: - KdTree() = default; + 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); + } - ~KdTree() { reset(); } + 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; - const VHACD::Vertex& getPosition(uint32_t index) const; + int stack = 1; + ConvexHullAABBTreeNode* root = nullptr; + while (stack) + { + 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(); - uint32_t search(const nd::VHACD::Vect3& pos, - double radius, - uint32_t maxObjects, - KdTreeFindNode* found) const; + 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; - void reset(); + if (box.m_child) + { + *box.m_child = &clump; + } - uint32_t add(const VHACD::Vertex& v); + 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)); - KdTreeNode* getNewNode(uint32_t index); + 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]; - uint32_t getNearest(const nd::VHACD::Vect3& pos, - double radius, - bool& _found) const; // returns the nearest possible neighbor's index. + int i0 = 0; + int i1 = box.m_count - 1; - const std::vector& getVertices() const; - std::vector&& takeVertices(); + const int start = box.m_start; + while (i0 < i1) + { + while ((points[start + i0][firstSortAxis] <= axisVal) && (i0 < i1)) + { + ++i0; + }; - uint32_t getVcount(void) const; + while ((points[start + i1][firstSortAxis] > axisVal) && (i0 < i1)) + { + --i1; + } -private: - KdTreeNode* mRoot = nullptr; - KdTreeNodeBundle* mBundle = nullptr; - KdTreeNodeBundle* mBundleHead = nullptr; + assert(i0 <= i1); + if (i0 < i1) + { + std::swap(points[start + i0], + points[start + i1]); + ++i0; + --i1; + } + } - std::vector mVertices; -}; + while ((points[start + i0][firstSortAxis] <= axisVal) && (i0 < box.m_count)) + { + ++i0; + }; -class KdTreeNode -{ -public: - KdTreeNode() = default; + #ifdef _DEBUG + for (int i = 0; i < i0; ++i) + { + assert(points[start + i][firstSortAxis] <= axisVal); + } - KdTreeNode(uint32_t index) : mIndex(index) {} + for (int i = i0; i < box.m_count; ++i) + { + assert(points[start + i][firstSortAxis] > axisVal); + } + #endif - ~KdTreeNode() = default; + assert(memoryPool.size() != memoryPool.capacity() + && "memoryPool is going to be reallocated, pointers will be invalid"); + memoryPool.emplace_back(); + ConvexHullAABBTreeNode& node = memoryPool.back(); - void add(KdTreeNode* node, - Axes dim, - const KdTree* iface) - { - 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; - } + 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& nodePosition = iface->getPosition(node->mIndex); - const VHACD::Vertex& position = iface->getPosition(mIndex); - if (nodePosition[idx] <= position[idx]) - { - if (mLeft) - mLeft->add(node, axis, iface); - else - mLeft = node; + if (!root) + { + root = &node; + } + + { + 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); + } + + 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++; + } + + { + 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); + } + + 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++; + } } - else - { - if (mRight) - mRight->add(node, axis, iface); + } + + return root; +} + +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 - mRight = node; + { + 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--; + } + } + + 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; + } + } + } + } } } - uint32_t getIndex() const - { - return mIndex; - }; + assert(index != -1); + return index; +} - void search(Axes axis, - const nd::VHACD::Vect3& pos, - double radius, - uint32_t& count, - uint32_t maxObjects, - KdTreeFindNode* found, - const KdTree* iface) +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)); +} + +int ConvexHull::InitVertexArray(std::vector& points, + NodeBundle& memoryPool) +// std::vector& memoryPool) +{ +#if 1 + ConvexHullAABBTreeNode* tree = BuildTreeOld(points, + memoryPool); +#else + ConvexHullAABBTreeNode* tree = BuildTreeNew(points, (char**)&memoryPool, maxMemSize); +#endif + int count = int(points.size()); + if (count < 4) { - const nd::VHACD::Vect3 position = iface->getPosition(mIndex); + m_points.resize(0); + return 0; + } + + m_points.resize(count); + m_aabbP0 = tree->m_box[0]; + m_aabbP1 = tree->m_box[1]; - const nd::VHACD::Vect3 d = pos - position; + VHACD::Vect3 boxSize(tree->m_box[1] - tree->m_box[0]); + m_diag = boxSize.GetNorm(); + const ndNormalMap& normalMap = ndNormalMap::GetNormalMap(); - KdTreeNode* search1 = 0; - KdTreeNode* search2 = 0; + int index0 = SupportVertex(&tree, + points, + normalMap.m_normal[0]); + m_points[0] = points[index0]; + points[index0].m_mark = 1; - uint32_t idx = 0; - switch (axis) + bool validTetrahedrum = false; + VHACD::Vect3 e1(double(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-4) * m_diag * m_diag)) { - case X_AXIS: - idx = 0; - axis = Y_AXIS; + m_points[1] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; break; - case Y_AXIS: - idx = 1; - axis = Z_AXIS; - break; - case Z_AXIS: - idx = 2; - axis = X_AXIS; + } + } + if (!validTetrahedrum) + { + m_points.resize(0); + assert(0); + return count; + } + + 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)) + { + m_points[2] = points[index]; + points[index].m_mark = 1; + validTetrahedrum = true; break; } + } + + if (!validTetrahedrum) + { + m_points.resize(0); + assert(0); + return count; + } - if (d[idx] <= 0) // JWR if we are to the left + // 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)) { - 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; + // 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; } - else + } + if (!validTetrahedrum) + { + for (int i = 3; i < normalMap.m_count; ++i) { - 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; + 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-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) + { + // 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.0)) + { + 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; +} + +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; + + std::list::iterator node = m_list.emplace(m_list.end(), face); + return node; +} + +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); + + 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; - double r2 = radius * radius; - double m = d.GetNormSquared(); + f2.m_twin[0] = f0Node; + f2.m_twin[1] = f3Node; + f2.m_twin[2] = f1Node; - if (m < r2) - { - switch (count) + 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()) { - case 0: - found[count].mNode = this; - found[count].mDistance = m; - break; - case 1: - if (m < found[0].mDistance) + std::list::iterator node1 = stack.back(); + ConvexHullFace& face1 = *node1; + + stack.pop_back(); + + if (!face1.m_mark && (face1.Evalue(m_points, p) < double(0.0))) { - if (maxObjects == 1) + #ifdef _DEBUG + for (const auto node : deleteList) { - found[0].mNode = this; - found[0].mDistance = m; + assert(node != node1); } - else + #endif + + deleteList.push_back(node1); + face1.m_mark = 1; + for (std::list::iterator& twinNode : face1.m_twin) { - found[1] = found[0]; - found[0].mNode = this; - found[0].mDistance = m; + ConvexHullFace& twinFace = *twinNode; + if (!twinFace.m_mark) + { + stack.push_back(twinNode); + } } } - else if (maxObjects > 1) - { - found[1].mNode = this; - found[1].mDistance = m; - } - break; - default: - { - bool inserted = false; + } + + m_points[currentIndex] = points[index]; + points[index].m_mark = 1; - for (uint32_t i = 0; i < count; i++) + coneList.clear(); + for (std::list::iterator node1 : deleteList) + { + ConvexHullFace& face1 = *node1; + assert(face1.m_mark == 1); + for (std::size_t j0 = 0; j0 < face1.m_twin.size(); ++j0) { - if (m < found[i].mDistance) // if this one is closer than a pre-existing one... + std::list::iterator twinNode = face1.m_twin[j0]; + ConvexHullFace& twinFace = *twinNode; + if (!twinFace.m_mark) { - // insertion sort... - uint32_t scan = count; - if (scan >= maxObjects) - scan = maxObjects - 1; - for (uint32_t j = scan; j > i; j--) + 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) { - found[j] = found[j - 1]; + if (twinFace.m_twin[k] == node1) + { + twinFace.m_twin[k] = newNode; + } } - found[i].mNode = this; - found[i].mDistance = m; - inserted = true; + coneList.push_back(newNode); + } + } + } + + 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++) + { + 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; } } - if (!inserted && count < maxObjects) + for (std::size_t j = i + 1; j < coneList.size(); j++) { - found[count].mNode = this; - found[count].mDistance = m; + std::list::iterator nodeB = coneList[j]; + ConvexHullFace& faceB = *nodeB; + 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; + } } } - break; + + for (std::list::iterator node : deleteList) + { + auto it = std::find(boundaryFaces.begin(), + boundaryFaces.end(), + node); + if (it != boundaryFaces.end()) + { + boundaryFaces.erase(it); + } + m_list.erase(node); } - 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); + + KdTreeNode& GetNewNode(uint32_t index); + + 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; -# define MAX_BUNDLE_SIZE \ - 1024 // 1024 nodes at a time, to minimize memory allocation and guarantee that pointers are persistent. +private: + KdTreeNode* m_root{ nullptr }; + NodeBundle m_bundle; + + std::vector m_vertices; +}; -class KdTreeNodeBundle +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. +KdTreeNode::KdTreeNode(uint32_t index) + : m_index(index) +{ +} -Output, double TRIANGLE_AREA_3D, the area of the triangle. -*/ +void KdTreeNode::Add(KdTreeNode& node, + Axes dim, + const KdTree& tree) { - /* - 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) - { + 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; + } - height = 0.0; + const VHACD::Vertex& nodePosition = tree.GetPosition(node.m_index); + const VHACD::Vertex& position = tree.GetPosition(m_index); + if (nodePosition[idx] <= position[idx]) + { + if (m_left) + m_left->Add(node, axis, tree); + else + m_left = &node; } 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); + if (m_right) + m_right->Add(node, axis, tree); + else + m_right = &node; } - - return 0.5f * base * height; } -double fm_computeArea(const nd::VHACD::Vect3& p1, - const nd::VHACD::Vect3& p2, - const nd::VHACD::Vect3& p3) +uint32_t KdTreeNode::GetIndex() const { - return triangle_area_3d(p1, - p2, - p3); + return m_index; } -bool fm_computeCentroid(const std::vector& points, - const std::vector& indices, - nd::VHACD::Vect3& center) - +void KdTreeNode::Search(Axes axis, + const VHACD::Vect3& pos, + double radius, + uint32_t& count, + uint32_t maxObjects, + KdTreeFindNode* found, + const KdTree& iface) { - bool ret = false; - if (points.size()) - { - 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; + const VHACD::Vect3 position = iface.GetPosition(m_index); - // Compute the area of this triangle - double area = fm_computeArea(p1, - p2, - p3); + const VHACD::Vect3 d = pos - position; - numerator += (sum * area); + KdTreeNode* search1 = 0; + KdTreeNode* search2 = 0; - denominator += area; - } - double recip = 1 / denominator; - center = numerator * recip; - ret = true; + 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; } - 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++) + if (d[idx] <= 0) // JWR if we are to the left { - 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. + 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; } - - 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) -{ - bmin = bmin1.CWiseMin(bmin2); - bmax = bmax1.CWiseMax(bmax2); -} - -double fm_volumeAABB(const nd::VHACD::Vect3& bmin, - const nd::VHACD::Vect3& bmax) -{ - double dx = bmax.getX() - bmin.getX(); - double dy = bmax.getY() - bmin.getY(); - double dz = bmax.getZ() - bmin.getZ(); - return dx*dy*dz; -} - -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; -} - -void fm_getAABB(const std::vector& points, - nd::VHACD::Vect3& bmin, - nd::VHACD::Vect3& bmax) -{ - bmin = points[0]; - bmax = points[0]; - - for (uint32_t i = 1; i < points.size(); i++) + else { - const VHACD::Vertex& p = points[i]; - bmin = bmin.CWiseMin(p); - bmax = bmax.CWiseMax(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; } -} -class MyVertexIndex -{ -public: - MyVertexIndex(double granularity, - bool snapToGrid) - : mSnapToGrid(snapToGrid) - , mGranularity(granularity) + double r2 = radius * radius; + double m = d.GetNormSquared(); + + if (m < r2) { - } + switch (count) + { + case 0: + { + found[count].m_node = this; + found[count].m_distance = m; + break; + } + 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: + { + 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; + } - double snapToGrid(double p) - { - double m = fmod(p, mGranularity); - p -= m; - return p; + count++; + + if (count > maxObjects) + { + count = maxObjects; + } } - uint32_t getIndex(const nd::VHACD::Vect3& _p, - bool& newPos) // get index for a vector double - { - uint32_t ret; - newPos = false; + if (search1) + search1->Search(axis, pos, radius, count, maxObjects, found, iface); - nd::VHACD::Vect3 p; + if (search2) + search2->Search(axis, pos, radius, count, maxObjects, found, iface); +} - if (mSnapToGrid) - { - p[0] = snapToGrid(_p[0]); - p[1] = snapToGrid(_p[1]); - p[2] = snapToGrid(_p[2]); - } - else - { - p[0] = _p[0]; - p[1] = _p[1]; - p[2] = _p[2]; - } +class VertexIndex +{ +public: + VertexIndex(double granularity, + bool snapToGrid); - bool found; - ret = mKdTree.getNearest(p, mGranularity, found); - if (!found) - { - newPos = true; - ret = mKdTree.add(VHACD::Vertex(p.getX(), p.getY(), p.getZ())); - } + VHACD::Vect3 SnapToGrid(VHACD::Vect3 p); - return ret; - } + uint32_t GetIndex(VHACD::Vect3 p, + bool& newPos); - const std::vector& getVertices() const - { - return mKdTree.getVertices(); - } + const std::vector& GetVertices() const; - std::vector&& takeVertices() - { - return std::move(mKdTree.takeVertices()); - } + 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 +3847,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 +3873,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; +} + +uint32_t VertexIndex::GetIndex(VHACD::Vect3 p, + bool& newPos) { + uint32_t ret; -#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 +4051,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 +4060,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 +4091,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 +4249,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 +4267,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 +4300,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 +4310,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 +4518,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 +4540,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 +4560,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 +4581,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 +4634,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 +4668,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 +4690,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 +4707,7 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, } if (dist[closest] < outT) + { TraceRecursive(node.m_children + closest, start, dir, @@ -5118,8 +4717,10 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, outW, faceSign, faceIndex); + } if (dist[furthest] < outT) + { TraceRecursive(node.m_children + furthest, start, dir, @@ -5129,6 +4730,7 @@ void AABBTreeImpl::TraceRecursive(uint32_t nodeIndex, outW, faceSign, faceIndex); + } } else { @@ -5138,9 +4740,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 +4759,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 +4783,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 +4800,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 +4845,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 +4864,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 +4907,39 @@ class Volume const std::vector& triangles, const size_t dim, FillMode fillMode, - MyRaycastMesh* raycastMesh); - - void raycastFill(MyRaycastMesh* raycastMesh); - - 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; - } + const AABBTree& aabbTree); - 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 +4950,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 +5136,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 +5171,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,18 +5201,28 @@ 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]; @@ -5622,171 +5234,170 @@ inline void Volume::Voxelize(const std::vector& points, { for (size_t k_id = 0; k_id < k0_local; ++k_id) { - const VoxelValue& voxel = GetVoxel(i_id, j_id, 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); + 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++; + } + else + { + 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 +5423,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 +5511,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 +5533,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 +5543,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 +5571,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 +5675,9 @@ class ShrinkWrapImpl // Definition of the ThreadPool //******************************************************************************************************************** -namespace VHACD -{ - class ThreadPool { public: - ThreadPool() : ThreadPool(1) {}; + ThreadPool(); ThreadPool(int worker); ~ThreadPool(); template @@ -6320,14 +5696,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 +5733,6 @@ ThreadPool::ThreadPool(int worker) : closed(false), count(0) } } - - template auto ThreadPool::enqueue(F&& f, Args&& ... args) #ifndef __cpp_lib_is_invocable @@ -6397,20 +5778,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 +5798,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 +5816,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 +5824,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; + ~VoxelHull() = default; - 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(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 +5871,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 ) + { + 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; + } + + // First, we copy all of the interior voxels from our parent + // which intersect our region + for (auto& i : parent.m_interiorVoxels) { - char scratch[512]; - snprintf(scratch,sizeof(scratch),"voxel-mesh-%03d.obj", mIndex); - FILE *fph = fopen(scratch,"wb"); - if ( fph ) + VHACD::Vector3 v = i.GetVoxel(); + if (v.CWiseAllGE(m_1) && v.CWiseAllLE(m_2)) { - uint32_t baseIndex = 1; - if ( saveVoxelMesh ) + bool newSurface = false; + switch ( m_axis ) { - 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); - } + 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; } - if ( saveSourceMesh ) + // If his interior voxels lie directly on the split plane then + // these become new surface voxels for our patch + if ( newSurface ) { - 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); - } + m_newSurfaceVoxels.push_back(i); } - fclose(fph); - } - } - - // 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) - { - SplitAxis ret = SplitAxis::X_AXIS_NEGATIVE; - - nd::VHACD::Vect3 d = m2 - m1; - - if ( d.getX() >= d.getY() && d.getX() >= d.getZ() ) - { - ret = SplitAxis::X_AXIS_NEGATIVE; - location = (m2.getX() + 1 + m1.getX()) / 2; - uint32_t edgeLoc; - if ( mParams.m_findBestPlane && findConcavityX(edgeLoc) ) + else { - location = edgeLoc; + m_interiorVoxels.push_back(i); } } - else if ( d.getY() >= d.getX() && d.getY() >= d.getZ() ) + } + // Next we copy all of the surface voxels which intersect our region + for (auto& i : parent.m_surfaceVoxels) + { + VHACD::Vector3 v = i.GetVoxel(); + if (v.CWiseAllGE(m_1) && v.CWiseAllLE(m_2)) { - ret = SplitAxis::Y_AXIS_NEGATIVE; - location = (m2.getY() + 1 + m1.getY()) / 2; - uint32_t edgeLoc; - if ( mParams.m_findBestPlane && findConcavityY(edgeLoc) ) - { - location = edgeLoc; - } + 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 = SplitAxis::Z_AXIS_NEGATIVE; - location = (m2.getZ() + 1 + m1.getZ()) / 2; - uint32_t edgeLoc; - if ( mParams.m_findBestPlane && findConcavityZ(edgeLoc) ) - { - location = edgeLoc; - } + m_newSurfaceVoxels.push_back(i); } + } - return ret; + // 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) + { + MinMaxVoxelRegion(i); + } + for (auto& i : m_newSurfaceVoxels) + { + MinMaxVoxelRegion(i); + } + for (auto& i : m_interiorVoxels) + { + MinMaxVoxelRegion(i); } - inline nd::VHACD::Vect3 getPosition(const nd::VHACD::Vect3& ip) const + BuildVoxelMesh(); + BuildRaycastMesh(); // build a raycast mesh of the voxel mesh + ComputeConvexHull(); +} + +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(); +} + +void VoxelHull::MinMaxVoxelRegion(const Voxel& v) +{ + VHACD::Vector3 x = v.GetVoxel(); + m_1 = m_1.CWiseMin(x); + m_2 = m_2.CWiseMax(x); +} + +void VoxelHull::BuildRaycastMesh() +{ + // Create a raycast mesh representation of the voxelized surface mesh + if ( !m_indices.empty() ) { - return getPoint(ip.getX(), - ip.getY(), - ip.getZ(), - mVoxelScale, - mVoxelAdjust); + m_AABBTree = AABBTree(m_vertices, + m_indices); } +} - double raycast(const nd::VHACD::Vect3& p1, - const nd::VHACD::Vect3& p2) const +void VoxelHull::ComputeConvexHull() +{ + if ( !m_vertices.empty() ) { - 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) ) + // we compute the convex hull as follows... + VHACD::QuickHull qh; + uint32_t tcount = qh.ComputeConvexHull(m_vertices, + uint32_t(m_vertices.size())); + if ( tcount ) { - ret = (from - hitLocation).GetNorm(); + m_convexHull = std::unique_ptr(new IVHACD::ConvexHull); + + m_convexHull->m_points = qh.GetVertices(); + m_convexHull->m_triangles = qh.GetIndices(); + + 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); } - else + } + 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; +} + +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 = 0; // if it doesn't hit anything, just assign it to zero. + ret = true; } - - return ret; } + 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; -// } +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 findConcavityX(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(0, 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 dx = (m2.getX() - m1.getX()) + 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(dx); - std::vector edgeErrorY = std::vector(dx); +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 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); +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 indexY = 0; + VHACD::Vector3 d = m_2 - m_1; - for (uint32_t x = m1.getX(); x <= m2.getX(); x++) + 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(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++; + 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 x = 1; x < indexZ; x++) + } + 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[x] > 0 && edgeErrorZ[x - 1] > 0 ) - { - double diff = abs(edgeErrorZ[x] - edgeErrorZ[x - 1]); - if ( diff > maxDiff ) - { - maxDiff = diff; - maxC = x-1; - } - } + location = edgeLoc; } - // Now see if there is a greater concavity on the XZ plane - for (uint32_t x = 1; x < indexY; x++) + } + 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 ( edgeErrorY[x] > 0 && edgeErrorY[x - 1] > 0 ) - { - double diff = abs(edgeErrorY[x] - edgeErrorY[x - 1]); - if ( diff > maxDiff ) - { - maxDiff = diff; - maxC = x-1; - } - } + location = edgeLoc; } + } - splitLoc = maxC + m1.getX(); + return ret; +} - // we do not allow an edge split if it is too close to the ends - if ( splitLoc > (m1.getX() + 4) && splitLoc < (m2.getX() - 4) ) - { - ret = true; - } +VHACD::Vect3 VoxelHull::GetPosition(const VHACD::Vector3& ip) const +{ + return GetPoint(ip.GetX(), + ip.GetY(), + ip.GetZ(), + m_voxelScale, + m_voxelAdjust); +} - return ret; +double VoxelHull::Raycast(const VHACD::Vector3& p1, + const VHACD::Vector3& p2) const +{ + double ret; + VHACD::Vect3 from = GetPosition(p1); + VHACD::Vect3 to = GetPosition(p2); + + double outT; + double faceSign; + VHACD::Vect3 hitLocation; + if (m_AABBTree.TraceRay(from, to, outT, faceSign, hitLocation)) + { + ret = (from - hitLocation).GetNorm(); + } + else + { + ret = 0; // if it doesn't hit anything, just assign it to zero. } + return ret; +} - // Finding the greatest area of concavity.. - bool findConcavityY(uint32_t &splitLoc) - { -// return findConcavity(1, splitLoc); - bool ret = false; +bool VoxelHull::FindConcavity(uint32_t idx, + uint32_t& splitLoc) +{ + bool ret = false; - uint32_t dy = (m2.getY() - m1.getY()) + 1; // The length of the getX axis in voxel space + int32_t d = (m_2[idx] - m_1[idx]) + 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 + 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; + } - std::vector edgeErrorZ = std::vector(dy); - std::vector edgeErrorX = std::vector(dy); + // 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; + // Counter of number of voxel samples on the XY plane we have accumulated + uint32_t index1 = 0; - // Compute Edge Error on the XY plane - for (uint32_t y = m1.getY(); y <= m2.getY(); y++) + // 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; - // 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++) + VHACD::Vector3 p1; + VHACD::Vector3 p2; + switch (idx) { - nd::VHACD::Vect3 p1(x, y, m1.getZ() - 2); - nd::VHACD::Vect3 p2(x, y, m2.getZ() + 2); - - double e1 = raycast(p1, p2); - double e2 = raycast(p2, p1); - - errorTotal = errorTotal + e1 + e2; + 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; + } } - // The total amount of edge error along this voxel location - edgeErrorZ[indexZ] = errorTotal; - indexZ++; - } - // Compute edge error along the XZ plane - uint32_t indexX = 0; + double e1 = Raycast(p1, p2); + double e2 = Raycast(p2, p1); - for (uint32_t y = m1.getY(); y <= m2.getY(); y++) - { - 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); + errorTotal = errorTotal + e1 + e2; + } + // The total amount of edge error along this voxel location + edgeError1[index1] = errorTotal; + index1++; + } - 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 + // Compute edge error along the XZ plane + uint32_t index2 = 0; - errorTotal = errorTotal + e1 + e2; - } - edgeErrorX[indexX] = errorTotal; - indexX++; - } + 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 y = 1; y < indexZ; y++) + for (uint32_t i1 = m_1[idx3]; i1 <= m_2[idx3]; i1++) { - if ( edgeErrorZ[y] > 0 && edgeErrorZ[y - 1] > 0 ) + VHACD::Vector3 p1; + VHACD::Vector3 p2; + switch (idx) { - double diff = abs(edgeErrorZ[y] - edgeErrorZ[y - 1]); - if ( diff > maxDiff ) + case 0: { - maxDiff = diff; - maxC = y-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 y = 1; y < indexX; y++) - { - if ( edgeErrorX[y] >0 && edgeErrorX[y - 1] > 0 ) - { - double diff = abs(edgeErrorX[y] - edgeErrorX[y - 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 = y - 1; + p1 = VHACD::Vector3(i1, m_1.GetY() - 2, i0); + p2 = VHACD::Vector3(i1, m_2.GetY() + 2, i0); + break; } } - } - splitLoc = maxC + m1.getY(); + 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.getY() + 4) && splitLoc < (m2.getY() - 4) ) - { - ret = true; + errorTotal = errorTotal + e1 + e2; } - - return ret; + edgeError2[index2] = errorTotal; + index2++; } - // Finding the greatest area of concavity.. - bool findConcavityZ(uint32_t &splitLoc) - { -// return findConcavity(2, splitLoc); - bool ret = false; - - int32_t dz = (m2.getZ() - m1.getZ()) + 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 edgeErrorX = std::vector(dz); - std::vector edgeErrorY = std::vector(dz); - - // Counter of number of voxel samples on the XY plane we have accumulated - uint32_t indexX = 0; - // Compute Edge Error on the XY plane - for (uint32_t z = m1.getZ(); z <= m2.getZ(); z++) + // 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 ( edgeError1[x] > 0 && edgeError1[x - 1] > 0 ) { - 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++) + double diff = abs(edgeError1[x] - edgeError1[x - 1]); + if ( diff > maxDiff ) { - nd::VHACD::Vect3 p1(m1.getX() - 2, y, z); - nd::VHACD::Vect3 p2(m2.getX() + 1, y, z); - - double e1 = raycast(p1, p2); - double e2 = raycast(p2, p1); - - errorTotal = errorTotal + e1 + e2; + maxDiff = diff; + maxC = x-1; } - // The total amount of edge error along this voxel location - edgeErrorX[indexX] = errorTotal; - indexX++; } + } - // Compute edge error along the XZ plane - uint32_t indexY = 0; - - for (uint32_t z = m1.getZ(); z <= m2.getZ(); z++) + // Now see if there is a greater concavity on the XZ plane + for (uint32_t x = 1; x < index2; x++) + { + if ( edgeError2[x] > 0 && edgeError2[x - 1] > 0 ) { - double errorTotal = 0; - - for (uint32_t x = m1.getX(); x <= m2.getX(); x++) + double diff = abs(edgeError2[x] - edgeError2[x - 1]); + if ( diff > maxDiff ) { - 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; + maxDiff = diff; + maxC = x - 1; } - edgeErrorY[indexY] = errorTotal; - indexY++; } + } + splitLoc = maxC + m_1[idx1]; - // 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++) - { - if ( edgeErrorX[z] > 0 && edgeErrorX[z - 1] > 0 ) - { - double diff = abs(edgeErrorX[z] - edgeErrorX[z - 1]); - if ( diff > maxDiff ) - { - maxDiff = diff; - maxC = z - 1; - } - } - } - // 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 ) - { - maxDiff = diff; - maxC = z-1; - } - } - } + // 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; + } - splitLoc = maxC + m1.getX(); + return ret; +} - // 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; - } +// Finding the greatest area of concavity.. +bool VoxelHull::FindConcavityX(uint32_t& splitLoc) +{ + return FindConcavity(0, splitLoc); +} - return ret; - } +// Finding the greatest area of concavity.. +bool VoxelHull::FindConcavityY(uint32_t& splitLoc) +{ + return FindConcavity(1, splitLoc); +} - // This operation is performed in a background thread. - //It splits the voxels by a plane - void performPlaneSplit(void) +// Finding the greatest area of concavity.. +bool VoxelHull::FindConcavityZ(uint32_t &splitLoc) +{ + return FindConcavity(2, splitLoc); +} + +void VoxelHull::PerformPlaneSplit() +{ + if ( IsComplete() ) { - if ( isComplete() ) - { - } - else + } + else + { + uint32_t splitLoc; + SplitAxis axis = ComputeSplitPlane(splitLoc); + switch ( axis ) { - 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 - 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; - } + 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, - const VoxelHull *h) +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 ) { - uint32_t baseIndex = 1; - 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 + baseIndex, - t.mI1 + baseIndex, - t.mI2 + baseIndex); - } - - baseIndex += uint32_t(mVertices.size()); - - 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); - } - - 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); + 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); } +} - void saveOBJ(const char *fname) +void VoxelHull::SaveOBJ(const char* fname, + const VoxelHull* h) +{ + FILE *fph = fopen(fname,"wb"); + if ( fph ) { - FILE *fph = fopen(fname, "wb"); - if ( fph ) - { - 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); - } - } + uint32_t baseIndex = 1; + WriteOBJ(fph, + m_vertices, + m_indices, + baseIndex); - 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 + baseIndex += uint32_t(m_vertices.size()); - 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 + WriteOBJ(fph, + h->m_vertices, + h->m_indices, + baseIndex); + fclose(fph); + } +} - 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 +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); + } +} - // 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::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 +6731,1247 @@ 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(); - uint32_t hullCount = GetNConvexHulls(); - distanceToHull = 0; - // First, make sure that we have valid and completed results - if ( hullCount ) + 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 ) + { + // 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]; - // 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]; + bmin = bmin.CWiseMin(p); + bmax = bmax.CWiseMax(p); + } + ProgressUpdate(Stages::COMPUTE_BOUNDS_OF_INPUT_MESH, + 100, + "ComputingBounds"); - bmin = bmin.CWiseMin(p); - bmax = bmax.CWiseMax(p); - } - progressUpdate(Stages::COMPUTE_BOUNDS_OF_INPUT_MESH, - 100, - "ComputingBounds"); + m_center = (bmax + bmin) * double(0.5); - mCenter = (bmax + bmin) * 0.5; + VHACD::Vect3 scale = bmax - bmin; + m_scale = scale.MaxCoeff(); - nd::VHACD::Vect3 scale = bmax - bmin; - mScale = Max(Max(scale.getX(), scale.getY()), scale.getZ()); + m_recipScale = m_scale > double(0.0) ? double(1.0) / m_scale : double(0.0); + + { + 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 jobCallback(std::unique_ptr& userPtr) +{ + userPtr->PerformPlaneSplit(); +} + +void computeMergeCostTask(CostTask& ptr) +{ + ptr.m_this->PerformMergeCostTask(ptr); +} - void performConvexDecomposition(void) +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) ) + { + 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) { - size_t count = mPendingHulls.size() + mVoxelHulls.size(); - double e = t.peekElapsedSeconds(); - if ( e >= 0.1 ) + 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)); + CostTask* task = &tasks.back(); #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; + tasks.clear(); + tasks.reserve(m_hulls.size()); + + // 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; + CostTask t; + t.m_hullA = combinedHull; + t.m_hullB = secondHull; + t.m_this = this; + if ( DoFastCost(t) ) { - 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. +#if !VHACD_DISABLE_THREADING + if ( m_threadPool && tasks.size() >= 2) + { + for (CostTask& task : tasks) + { + task.m_future = m_threadPool->enqueue([&task] { - performMergeCostTask(taskCost); - taskCost++; - } + computeMergeCostTask(task); + }); } - for (size_t i = 0; i < tcount; i++) + + for (CostTask& task : tasks) { - addCostToPriorityQueue(&tasks[i]); + task.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; + for (CostTask& task : tasks) + { + PerformMergeCostTask(task); + } } - ConvexHull *ch = i.second; - // We now must reduce the convex hull - if ( ch->m_points.size() > mParams.m_maxNumVerticesPerCH || mParams.m_shrinkWrap) + + for (CostTask& task : tasks) { - ConvexHull *reduce = computeReducedConvexHull(*ch, - mParams.m_maxNumVerticesPerCH, - mParams.m_shrinkWrap); - releaseConvexHull(ch); - ch = reduce; + AddCostToPriorityQueue(task); } - 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 +7987,370 @@ IVHACD* CreateVHACD(void); class LogMessage { public: - double mOverallProgress{-1}; - double mStageProgress{-1}; - 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; }; -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.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(); +} + +void VHACDAsyncImpl::Log(const char* const msg) +{ + m_messageMutex.lock(); + LogMessage m; + m.m_operation = 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.m_overallProgress == -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.m_operation.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.m_overallProgress, + i.m_stageProgress, + i.m_stage.c_str(), + i.m_operation.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)