diff --git a/CMakeLists.txt b/CMakeLists.txt index acdfebf04dc8f..b79ad04c46e17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -141,19 +141,17 @@ if(NOT CMAKE_BUILD_TYPE) endif() string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE) -set(BUILD_TYPE_DEBUG FALSE) -set(BUILD_TYPE_RELEASE FALSE) -set(BUILD_TYPE_RELWITHDEBINFO FALSE) -set(BUILD_TYPE_MINSIZEREL FALSE) +set(DART_BUILD_MODE_DEBUG FALSE) +set(DART_BUILD_MODE_RELEASE FALSE) if("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "DEBUG") - set(BUILD_TYPE_DEBUG TRUE) + set(DART_BUILD_MODE_DEBUG TRUE) elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELEASE") - set(BUILD_TYPE_RELEASE TRUE) + set(DART_BUILD_MODE_RELEASE TRUE) elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELWITHDEBINFO") - set(BUILD_TYPE_RELWITHDEBINFO TRUE) + set(DART_BUILD_MODE_RELEASE TRUE) elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "MINSIZEREL") - set(BUILD_TYPE_MINSIZEREL TRUE) + set(DART_BUILD_MODE_RELEASE TRUE) else() message(WARNING "CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} unknown. Valid options are: Debug | Release | RelWithDebInfo | MinSizeRel") endif() @@ -167,14 +165,14 @@ endif() # - ERROR: To enable log with DART_ERROR() and below # - FATAL: To enable log with DART_FATAL() # - OFF: To turn off all the logs -if(BUILD_TYPE_DEBUG) +if(DART_BUILD_MODE_DEBUG) set(DART_ACTIVE_LOG_LEVEL "DEBUG" CACHE STRING "Compile time active log level to enable") else() set(DART_ACTIVE_LOG_LEVEL "INFO" CACHE STRING "Compile time active log level to enable") endif() set_property(CACHE DART_ACTIVE_LOG_LEVEL PROPERTY STRINGS TRACE DEBUG INFO WARN ERROR FATAL OFF) -if(BUILD_TYPE_DEBUG) +if(DART_BUILD_MODE_DEBUG) option(DART_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) else() option(DART_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" ON) diff --git a/dart/collision/Option.hpp b/dart/collision/Option.hpp index 467ce0e3b4829..dbde8de16b961 100644 --- a/dart/collision/Option.hpp +++ b/dart/collision/Option.hpp @@ -35,7 +35,7 @@ #pragma message( \ "This header has been deprecated in DART 6.1. " \ - "Please include CollisionOption.hpp intead.") + "Please include CollisionOption.hpp instead.") #include "dart/collision/CollisionOption.hpp" diff --git a/dart/collision/Result.hpp b/dart/collision/Result.hpp index 0925818102b34..08c7aed9e8af2 100644 --- a/dart/collision/Result.hpp +++ b/dart/collision/Result.hpp @@ -35,7 +35,7 @@ #pragma message( \ "This header has been deprecated in DART 6.1. " \ - "Please include CollisionResult.hpp intead.") + "Please include CollisionResult.hpp instead.") #include "dart/collision/CollisionResult.hpp" diff --git a/dart/common/FreeListAllocator.cpp b/dart/common/FreeListAllocator.cpp index 1dea221672b53..f5d6d5001643a 100644 --- a/dart/common/FreeListAllocator.cpp +++ b/dart/common/FreeListAllocator.cpp @@ -395,7 +395,7 @@ void FreeListAllocator::MemoryBlockHeader::merge(MemoryBlockHeader* other) } //============================================================================== -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG bool FreeListAllocator::MemoryBlockHeader::isValid() const { if (mPrev != nullptr && mPrev->mNext != this) diff --git a/dart/common/FreeListAllocator.hpp b/dart/common/FreeListAllocator.hpp index c2f91e95e9243..228b29bcc4f48 100644 --- a/dart/common/FreeListAllocator.hpp +++ b/dart/common/FreeListAllocator.hpp @@ -126,7 +126,7 @@ class FreeListAllocator : public MemoryAllocator /// Merges this memory block with the given memory block void merge(MemoryBlockHeader* other); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG /// [Debug only] Returns whether this memory block is valid bool isValid() const; #endif diff --git a/dart/common/Macros.hpp b/dart/common/Macros.hpp index 5c90e3ebcb539..205922b4675f3 100644 --- a/dart/common/Macros.hpp +++ b/dart/common/Macros.hpp @@ -36,6 +36,7 @@ #include #include "dart/common/Logging.hpp" +#include "dart/config.hpp" // DART_NUM_ARGS( [, [, ...]]) #define DETAIL_DART_NUM_ARGS(z, a, b, c, d, e, f, cnt, ...) cnt @@ -68,7 +69,7 @@ #define DETAIL_DART_ASSERT_2(condition, message) assert((condition) && #message) #define DART_ASSERT(...) \ DART_CONCAT(DETAIL_DART_ASSERT_, DART_NUM_ARGS(__VA_ARGS__)) \ - (__VA_ARGS__) + (__VA_ARGS__) // Macro to mark the function is not implemented #define DART_NOT_IMPLEMENTED \ diff --git a/dart/common/MemoryAllocator.hpp b/dart/common/MemoryAllocator.hpp index 4644f1df0fe4e..3bcf228d4fe20 100644 --- a/dart/common/MemoryAllocator.hpp +++ b/dart/common/MemoryAllocator.hpp @@ -38,6 +38,7 @@ #include #include "dart/common/Castable.hpp" +#include "dart/config.hpp" namespace dart::common { diff --git a/dart/common/MemoryManager.cpp b/dart/common/MemoryManager.cpp index 9252cd725ec89..bc4a4ccb0e2b5 100644 --- a/dart/common/MemoryManager.cpp +++ b/dart/common/MemoryManager.cpp @@ -32,7 +32,7 @@ #include "dart/common/MemoryManager.hpp" -#ifndef NDEBUG // debug +#if DART_BUILD_MODE_DEBUG #include "dart/common/Logging.hpp" #endif @@ -49,7 +49,7 @@ MemoryManager& MemoryManager::GetDefault() MemoryManager::MemoryManager(MemoryAllocator& baseAllocator) : mBaseAllocator(baseAllocator), mFreeListAllocator(mBaseAllocator), -#ifdef NDEBUG +#if DART_BUILD_MODE_RELEASE mPoolAllocator(mFreeListAllocator) #else mPoolAllocator(mFreeListAllocator.getInternalAllocator()) @@ -73,7 +73,7 @@ MemoryAllocator& MemoryManager::getBaseAllocator() //============================================================================== FreeListAllocator& MemoryManager::getFreeListAllocator() { -#ifdef NDEBUG +#if DART_BUILD_MODE_RELEASE return mFreeListAllocator; #else return mFreeListAllocator.getInternalAllocator(); @@ -83,7 +83,7 @@ FreeListAllocator& MemoryManager::getFreeListAllocator() //============================================================================== PoolAllocator& MemoryManager::getPoolAllocator() { -#ifdef NDEBUG +#if DART_BUILD_MODE_RELEASE return mPoolAllocator; #else return mPoolAllocator.getInternalAllocator(); @@ -146,7 +146,7 @@ void MemoryManager::deallocateUsingPool(void* pointer, size_t bytes) deallocate(Type::Pool, pointer, bytes); } -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG //============================================================================== bool MemoryManager::hasAllocated(void* pointer, size_t size) const noexcept { diff --git a/dart/common/MemoryManager.hpp b/dart/common/MemoryManager.hpp index 1e61c15b4ced5..46a82bdc4d499 100644 --- a/dart/common/MemoryManager.hpp +++ b/dart/common/MemoryManager.hpp @@ -33,7 +33,7 @@ #ifndef DART_COMMON_MEMORYMANAGER_HPP_ #define DART_COMMON_MEMORYMANAGER_HPP_ -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG #include #endif #include @@ -151,7 +151,7 @@ class MemoryManager final template void destroyUsingPool(T* pointer) noexcept; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG /// Returns true if a pointer is allocated by the internal allocator. [[nodiscard]] bool hasAllocated(void* pointer, size_t size) const noexcept; #endif @@ -164,10 +164,10 @@ class MemoryManager final std::ostream& os, const MemoryManager& memoryManager); private: - /// The base allocator to allocate memory chunck. + /// The base allocator to allocate memory chunk. MemoryAllocator& mBaseAllocator; -#ifdef NDEBUG +#if DART_BUILD_MODE_RELEASE /// The free list allocator. FreeListAllocator mFreeListAllocator; diff --git a/dart/config.hpp.in b/dart/config.hpp.in index 20e4e1eb424bd..7e157c7a29c4c 100644 --- a/dart/config.hpp.in +++ b/dart/config.hpp.in @@ -44,10 +44,9 @@ #define DART_COMPILER_MSVC #endif -#cmakedefine01 BUILD_TYPE_DEBUG -#cmakedefine01 BUILD_TYPE_RELEASE -#cmakedefine01 BUILD_TYPE_RELWITHDEBINFO -#cmakedefine01 BUILD_TYPE_MINSIZEREL +// Indicates the build mode used for compiling +#cmakedefine01 DART_BUILD_MODE_DEBUG +#cmakedefine01 DART_BUILD_MODE_RELEASE #cmakedefine01 HAVE_NLOPT #cmakedefine01 HAVE_IPOPT diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 85600a3719841..312c3d626a8f1 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -33,7 +33,7 @@ #include "dart/constraint/BoxedLcpConstraintSolver.hpp" #include -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG #include #include #endif @@ -155,7 +155,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) return; const int nSkip = dPAD(n); -#ifdef NDEBUG // release +#if DART_BUILD_MODE_RELEASE mA.resize(n, nSkip); #else // debug mA.setZero(n, nSkip); @@ -310,7 +310,7 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) } //============================================================================== -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG bool BoxedLcpConstraintSolver::isSymmetric(std::size_t n, double* A) { std::size_t nSkip = dPAD(n); diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index c32cb699e32a0..00b9b0af10dce 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -154,12 +154,12 @@ class BoxedLcpConstraintSolver : public ConstraintSolver /// Cache data for boxed LCP formulation Eigen::VectorXi mOffset; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG private: /// Return true if the matrix is symmetric bool isSymmetric(std::size_t n, double* A); - /// Return true if the diagonla block of matrix is symmetric + /// Return true if the diagonal block of matrix is symmetric bool isSymmetric( std::size_t n, double* A, std::size_t begin, std::size_t end); diff --git a/dart/constraint/BoxedLcpSolver.hpp b/dart/constraint/BoxedLcpSolver.hpp index e79822ee7944d..d3e37e9003e33 100644 --- a/dart/constraint/BoxedLcpSolver.hpp +++ b/dart/constraint/BoxedLcpSolver.hpp @@ -88,7 +88,7 @@ class BoxedLcpSolver : public common::Castable bool earlyTermination = false) = 0; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG virtual bool canSolve(int n, const double* A) = 0; #endif }; diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index 4b8aced3fd9cf..a0fbc1bc7c337 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -101,7 +101,7 @@ void ConstrainedGroup::removeAllConstraints() } //============================================================================== -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG bool ConstrainedGroup::containConstraint( const ConstConstraintBasePtr& _constraint) const { diff --git a/dart/constraint/ConstrainedGroup.hpp b/dart/constraint/ConstrainedGroup.hpp index dafbcecd2cca4..43ef1c113e87f 100644 --- a/dart/constraint/ConstrainedGroup.hpp +++ b/dart/constraint/ConstrainedGroup.hpp @@ -38,6 +38,7 @@ #include +#include "dart/config.hpp" #include "dart/constraint/SmartPointer.hpp" namespace dart { @@ -100,7 +101,7 @@ class ConstrainedGroup friend class ConstraintSolver; private: -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG /// Return true if _constraint is contained bool containConstraint(const ConstConstraintBasePtr& _constraint) const; #endif diff --git a/dart/constraint/DantzigBoxedLcpSolver.cpp b/dart/constraint/DantzigBoxedLcpSolver.cpp index 9c9e7826598b9..0399ee03c6d30 100644 --- a/dart/constraint/DantzigBoxedLcpSolver.cpp +++ b/dart/constraint/DantzigBoxedLcpSolver.cpp @@ -66,7 +66,7 @@ bool DantzigBoxedLcpSolver::solve( n, A, x, b, nullptr, 0, lo, hi, findex, earlyTermination); } -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG //============================================================================== bool DantzigBoxedLcpSolver::canSolve(int /*n*/, const double* /*A*/) { diff --git a/dart/constraint/DantzigBoxedLcpSolver.hpp b/dart/constraint/DantzigBoxedLcpSolver.hpp index 5fdc4bb5587cc..9c67617a40cff 100644 --- a/dart/constraint/DantzigBoxedLcpSolver.hpp +++ b/dart/constraint/DantzigBoxedLcpSolver.hpp @@ -59,7 +59,7 @@ class DantzigBoxedLcpSolver : public BoxedLcpSolver int* findex, bool earlyTermination) override; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG // Documentation inherited. bool canSolve(int n, const double* A) override; #endif diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp index 9dbad391e5b90..fcc10acd654a3 100644 --- a/dart/constraint/DantzigLCPSolver.cpp +++ b/dart/constraint/DantzigLCPSolver.cpp @@ -32,7 +32,7 @@ #include "dart/constraint/DantzigLCPSolver.hpp" -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG #include #include #endif @@ -79,7 +79,7 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group) int* findex = new int[n]; // Set w to 0 and findex to -1 -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG std::memset(A, 0.0, n * nSkip * sizeof(double)); #endif std::memset(w, 0.0, n * sizeof(double)); @@ -189,7 +189,7 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group) } //============================================================================== -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG bool DantzigLCPSolver::isSymmetric(std::size_t _n, double* _A) { std::size_t nSkip = dPAD(_n); diff --git a/dart/constraint/DantzigLCPSolver.hpp b/dart/constraint/DantzigLCPSolver.hpp index 1a6c23016c7f4..252f613ab7041 100644 --- a/dart/constraint/DantzigLCPSolver.hpp +++ b/dart/constraint/DantzigLCPSolver.hpp @@ -42,7 +42,7 @@ namespace dart { namespace constraint { /// \deprecated This header has been deprecated in DART 6.7. Please include -/// DantzigBoxedLcpSolver.hpp intead. +/// DantzigBoxedLcpSolver.hpp instead. /// /// DantzigLCPSolver is a LCP solver that uses ODE's implementation of Dantzig /// algorithm @@ -58,12 +58,12 @@ class DantzigLCPSolver : public LCPSolver // Documentation inherited void solve(ConstrainedGroup* _group) override; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG private: /// Return true if the matrix is symmetric bool isSymmetric(std::size_t _n, double* _A); - /// Return true if the diagonla block of matrix is symmetric + /// Return true if the diagonal block of matrix is symmetric bool isSymmetric( std::size_t _n, double* _A, std::size_t _begin, std::size_t _end); diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp index bf032f0ffb2a1..b9c8f40546530 100644 --- a/dart/constraint/JointConstraint.cpp +++ b/dart/constraint/JointConstraint.cpp @@ -403,7 +403,7 @@ void JointConstraint::getInformation(ConstraintInfo* lcp) if (!mActive[i]) continue; -#ifndef NDEBUG // debug +#if DART_BUILD_MODE_DEBUG if (std::abs(lcp->w[index]) > 1e-6) { dterr << "Invalid " << index @@ -418,7 +418,7 @@ void JointConstraint::getInformation(ConstraintInfo* lcp) lcp->lo[index] = mImpulseLowerBound[i]; lcp->hi[index] = mImpulseUpperBound[i]; -#ifndef NDEBUG // debug +#if DART_BUILD_MODE_DEBUG if (lcp->findex[index] != -1) { dterr << "Invalid " << index diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp index 24fd90ffb6219..fb4fe3900f8a9 100644 --- a/dart/constraint/JointLimitConstraint.cpp +++ b/dart/constraint/JointLimitConstraint.cpp @@ -312,7 +312,7 @@ void JointLimitConstraint::getInformation(ConstraintInfo* lcp) lcp->lo[index] = mLowerBound[i]; lcp->hi[index] = mUpperBound[i]; -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG if (lcp->lo[index] > lcp->hi[index]) { std::cout << "dim: " << mDim << std::endl; diff --git a/dart/constraint/PGSLCPSolver.cpp b/dart/constraint/PGSLCPSolver.cpp index cb2018597b96b..dad00c51174f6 100644 --- a/dart/constraint/PGSLCPSolver.cpp +++ b/dart/constraint/PGSLCPSolver.cpp @@ -32,7 +32,7 @@ #include "dart/constraint/PGSLCPSolver.hpp" -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG #include #include #endif @@ -72,7 +72,7 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group) int* findex = new int[n]; // Set w to 0 and findex to -1 -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG std::memset(A, 0.0, n * nSkip * sizeof(double)); #endif std::memset(w, 0.0, n * sizeof(double)); @@ -185,7 +185,7 @@ void PGSLCPSolver::solve(ConstrainedGroup* _group) } //============================================================================== -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG bool PGSLCPSolver::isSymmetric(std::size_t _n, double* _A) { std::size_t nSkip = dPAD(_n); diff --git a/dart/constraint/PGSLCPSolver.hpp b/dart/constraint/PGSLCPSolver.hpp index b9990c343c83c..4b6d4d74eed2a 100644 --- a/dart/constraint/PGSLCPSolver.hpp +++ b/dart/constraint/PGSLCPSolver.hpp @@ -42,7 +42,7 @@ namespace dart { namespace constraint { /// \deprecated This header has been deprecated in DART 6.7. Please include -/// PgsBoxedLcpSolver.hpp intead. +/// PgsBoxedLcpSolver.hpp instead. /// /// PGSLCPSolver class PGSLCPSolver : public LCPSolver @@ -57,12 +57,12 @@ class PGSLCPSolver : public LCPSolver // Documentation inherited void solve(ConstrainedGroup* _group) override; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG private: /// Return true if the matrix is symmetric bool isSymmetric(std::size_t _n, double* _A); - /// Return true if the diagonla block of matrix is symmetric + /// Return true if the diagonal block of matrix is symmetric bool isSymmetric( std::size_t _n, double* _A, std::size_t _begin, std::size_t _end); diff --git a/dart/constraint/PgsBoxedLcpSolver.cpp b/dart/constraint/PgsBoxedLcpSolver.cpp index 2df0cb48b6261..c8fea155f6ec6 100644 --- a/dart/constraint/PgsBoxedLcpSolver.cpp +++ b/dart/constraint/PgsBoxedLcpSolver.cpp @@ -246,7 +246,7 @@ bool PgsBoxedLcpSolver::solve( return possibleToTerminate; } -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG //============================================================================== bool PgsBoxedLcpSolver::canSolve(int n, const double* A) { diff --git a/dart/constraint/PgsBoxedLcpSolver.hpp b/dart/constraint/PgsBoxedLcpSolver.hpp index b54846662e044..141c781564992 100644 --- a/dart/constraint/PgsBoxedLcpSolver.hpp +++ b/dart/constraint/PgsBoxedLcpSolver.hpp @@ -78,7 +78,7 @@ class PgsBoxedLcpSolver : public BoxedLcpSolver int* findex, bool earlyTermination) override; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG // Documentation inherited. bool canSolve(int n, const double* A) override; #endif diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index b905f37873661..274bd035592e8 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1410,7 +1410,7 @@ void BodyNode::init(const SkeletonPtr& _skeleton) mConstDependentDofs.push_back(_skeleton->getDof(index)); } -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG // Check whether there is duplicated indices. std::size_t nDepGenCoordIndices = mDependentGenCoordIndices.size(); for (std::size_t i = 0; i < nDepGenCoordIndices; ++i) @@ -1422,7 +1422,7 @@ void BodyNode::init(const SkeletonPtr& _skeleton) && "Duplicated index is found in mDependentGenCoordIndices."); } } -#endif // NDEBUG +#endif //-------------------------------------------------------------------------- // Set dimensions of dynamics matrices and vectors. diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 2eb6829800e35..20ca339027dae 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -211,7 +211,7 @@ Eigen::Matrix EulerJoint::getRelativeJacobianStatic( J1 << s2, c2, 0.0, 0.0, 0.0, 0.0; J2 << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG if (std::abs(getPositionsStatic()[1]) == math::constantsd::pi() * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << Joint::mAspectProperties.mName << "]. (" << _positions[0] @@ -234,7 +234,7 @@ Eigen::Matrix EulerJoint::getRelativeJacobianStatic( J1 << 0.0, c2, -s2, 0.0, 0.0, 0.0; J2 << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG if (std::abs(_positions[1]) == math::constantsd::pi() * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << Joint::mAspectProperties.mName << "]. (" << _positions[0] @@ -256,7 +256,7 @@ Eigen::Matrix EulerJoint::getRelativeJacobianStatic( assert(!math::isNan(J)); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG Eigen::MatrixXd JTJ = J.transpose() * J; Eigen::FullPivLU luJTJ(JTJ); // Eigen::FullPivLU luS(mS); diff --git a/dart/dynamics/MultiSphereShape.hpp b/dart/dynamics/MultiSphereShape.hpp index bfeeccc5c089d..9e82ef08cc71f 100644 --- a/dart/dynamics/MultiSphereShape.hpp +++ b/dart/dynamics/MultiSphereShape.hpp @@ -35,7 +35,7 @@ #pragma message( \ "This header has been deprecated in DART 6.2. " \ - "Please include MultiSphereConvexHullShape.hpp intead.") + "Please include MultiSphereConvexHullShape.hpp instead.") #include "dart/dynamics/MultiSphereConvexHullShape.hpp" diff --git a/dart/dynamics/Node.cpp b/dart/dynamics/Node.cpp index e68670a1fb05d..f42a59f795ca9 100644 --- a/dart/dynamics/Node.cpp +++ b/dart/dynamics/Node.cpp @@ -193,7 +193,7 @@ void Node::attach() // If we are in release mode, and the Node believes it is attached, then we // can shortcut this procedure -#ifdef NDEBUG +#if DART_BUILD_MODE_RELEASE if (mAmAttached) return; #endif @@ -247,7 +247,7 @@ void Node::stageForRemoval() // If we are in release mode, and the Node believes it is detached, then we // can shortcut this procedure. -#ifdef NDEBUG +#if DART_BUILD_MODE_RELEASE if (!mAmAttached) return; #endif diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index aafea5798b91d..04b3cfdcb5290 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -2170,7 +2170,7 @@ void Skeleton::constructNewTree() //============================================================================== void Skeleton::registerBodyNode(BodyNode* _newBodyNode) { -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::vector::iterator repeat = std::find( mSkelCache.mBodyNodes.begin(), mSkelCache.mBodyNodes.end(), _newBodyNode); if (repeat != mSkelCache.mBodyNodes.end()) @@ -2224,7 +2224,7 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode) updateTotalMass(); updateCacheDimensions(_newBodyNode->mTreeIndex); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) { if (mSkelCache.mBodyNodes[i]->mIndexInSkeleton != i) @@ -3839,7 +3839,7 @@ void Skeleton::updateBiasImpulse(BodyNode* _bodyNode) // This skeleton should contain _bodyNode assert(_bodyNode->getSkeleton().get() == this); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG // All the constraint impulse should be zero for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) assert( @@ -3872,7 +3872,7 @@ void Skeleton::updateBiasImpulse( // This skeleton should contain _bodyNode assert(_bodyNode->getSkeleton().get() == this); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG // All the constraint impulse should be zero for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) assert( @@ -3922,7 +3922,7 @@ void Skeleton::updateBiasImpulse( assert(_bodyNode1->getSkeleton().get() == this); assert(_bodyNode2->getSkeleton().get() == this); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG // All the constraint impulse should be zero for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) assert( @@ -3963,7 +3963,7 @@ void Skeleton::updateBiasImpulse( std::find(mSoftBodyNodes.begin(), mSoftBodyNodes.end(), _softBodyNode) != mSoftBodyNodes.end()); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG // All the constraint impulse should be zero for (std::size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) assert( diff --git a/dart/external/convhull_3d/convhull_3d.h b/dart/external/convhull_3d/convhull_3d.h index 28ad779fc6b8c..df497bb099323 100644 --- a/dart/external/convhull_3d/convhull_3d.h +++ b/dart/external/convhull_3d/convhull_3d.h @@ -1,16 +1,16 @@ /* Copyright (c) 2017-2021 Leo McCormack - + Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: - + The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. - + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -63,110 +63,174 @@ typedef float CH_FLOAT; #else typedef double CH_FLOAT; #endif -typedef struct _ch_vertex { - union { - CH_FLOAT v[3]; - struct{ - CH_FLOAT x, y, z; - }; +typedef struct _ch_vertex +{ + union + { + CH_FLOAT v[3]; + struct + { + CH_FLOAT x, y, z; }; + }; } ch_vertex; typedef ch_vertex ch_vec3; /* builds the 3-D convexhull */ -void convhull_3d_build(/* input arguments */ - ch_vertex* const in_vertices, /* vector of input vertices; nVert x 1 */ - const int nVert, /* number of vertices */ +void convhull_3d_build( /* input arguments */ + ch_vertex* const in_vertices, /* vector of input + vertices; nVert x 1 */ + const int nVert, /* number of vertices */ /* output arguments */ - int** out_faces, /* & of empty int*, output face indices; flat: nOut_faces x 3 */ - int* nOut_faces); /* & of int, number of output face indices */ - -/* exports the vertices, face indices, and face normals, as an 'obj' file, ready for GPU (for 3d convexhulls only) */ -void convhull_3d_export_obj(/* input arguments */ - ch_vertex* const vertices, /* vector of input vertices; nVert x 1 */ - const int nVert, /* number of vertices */ - int* const faces, /* face indices; flat: nFaces x 3 */ - const int nFaces, /* number of faces in hull */ - const int keepOnlyUsedVerticesFLAG, /* 0: exports in_vertices, 1: exports only used vertices */ - char* const obj_filename); /* obj filename, WITHOUT extension */ - -/* exports the vertices, face indices, and face normals, as an 'm' file, for MatLab verification (for 3d convexhulls only) */ -void convhull_3d_export_m(/* input arguments */ - ch_vertex* const vertices, /* vector of input vertices; nVert x 1 */ - const int nVert, /* number of vertices */ - int* const faces, /* face indices; flat: nFaces x 3 */ - const int nFaces, /* number of faces in hull */ - char* const m_filename); /* m filename, WITHOUT extension */ - -/* reads an 'obj' file and extracts only the vertices (for 3d convexhulls only) */ + int** out_faces, /* & of empty int*, output face indices; + flat: nOut_faces x 3 */ + int* nOut_faces); /* & of int, number of output face + indices */ + +/* exports the vertices, face indices, and face normals, as an 'obj' file, ready + * for GPU (for 3d convexhulls only) */ +void convhull_3d_export_obj( /* input arguments */ + ch_vertex* const vertices, /* vector of input + vertices; nVert x 1 */ + const int nVert, /* number of vertices */ + int* const + faces, /* face indices; flat: nFaces x 3 */ + const int nFaces, /* number of faces in hull */ + const int + keepOnlyUsedVerticesFLAG, /* 0: exports + in_vertices, 1: + exports only used + vertices */ + char* const obj_filename); /* obj filename, WITHOUT + extension */ + +/* exports the vertices, face indices, and face normals, as an 'm' file, for + * MatLab verification (for 3d convexhulls only) */ +void convhull_3d_export_m( /* input arguments */ + ch_vertex* const vertices, /* vector of input + vertices; nVert x 1 */ + const int nVert, /* number of vertices */ + int* const faces, /* face indices; flat: nFaces x 3 */ + const int nFaces, /* number of faces in hull */ + char* const + m_filename); /* m filename, WITHOUT extension */ + +/* reads an 'obj' file and extracts only the vertices (for 3d convexhulls only) + */ void extract_vertices_from_obj_file(/* input arguments */ - char* const obj_filename, /* obj filename, WITHOUT extension */ + char* const + obj_filename, /* obj filename, WITHOUT + extension */ /* output arguments */ - ch_vertex** out_vertices, /* & of empty ch_vertex*, output vertices; out_nVert x 1 */ - int* out_nVert); /* & of int, number of vertices */ + ch_vertex** + out_vertices, /* & of empty ch_vertex*, + output vertices; + out_nVert x 1 */ + int* out_nVert); /* & of int, number of + vertices */ /**** NEW! ****/ /* builds the N-Dimensional convexhull of a grid of points */ void convhull_nd_build(/* input arguments */ - CH_FLOAT* const in_points, /* Matrix of points in 'd' dimensions; FLAT: nPoints x d */ - const int nPoints, /* number of points */ - const int d, /* Number of dimensions */ + CH_FLOAT* const + in_points, /* Matrix of points in 'd' dimensions; + FLAT: nPoints x d */ + const int nPoints, /* number of points */ + const int d, /* Number of dimensions */ /* output arguments */ - int** out_faces, /* (&) output face indices; FLAT: nOut_faces x d */ - CH_FLOAT** out_cf, /* (&) contains the coefficients of the planes (set to NULL if not wanted); FLAT: nOut_faces x d */ - CH_FLOAT** out_df, /* (&) contains the constant terms of the planes (set to NULL if not wanted); nOut_faces x 1 */ - int* nOut_faces); /* (&) number of output face indices */ - -/* Computes the Delaunay triangulation (mesh) of an arrangement of points in N-dimensional space */ + int** out_faces, /* (&) output face indices; FLAT: + nOut_faces x d */ + CH_FLOAT** out_cf, /* (&) contains the coefficients of + the planes (set to NULL if not + wanted); FLAT: nOut_faces x d */ + CH_FLOAT** out_df, /* (&) contains the constant terms of + the planes (set to NULL if not + wanted); nOut_faces x 1 */ + int* nOut_faces); /* (&) number of output face indices */ + +/* Computes the Delaunay triangulation (mesh) of an arrangement of points in + * N-dimensional space */ void delaunay_nd_mesh(/* input Arguments */ - const float* points, /* The input points; FLAT: nPoints x nd */ - const int nPoints, /* Number of points */ - const int nd, /* The number of dimensions */ + const float* + points, /* The input points; FLAT: nPoints x nd */ + const int nPoints, /* Number of points */ + const int nd, /* The number of dimensions */ /* output Arguments */ - int** Mesh, /* (&) the indices defining the Delaunay triangulation of the points; FLAT: nMesh x (nd+1) */ - int* nMesh); /* (&) Number of triangulations */ + int** Mesh, /* (&) the indices defining the Delaunay + triangulation of the points; FLAT: nMesh x + (nd+1) */ + int* nMesh); /* (&) Number of triangulations */ /**** CUSTOM ALLOCATOR VERSIONS ****/ /* builds the 3-D convexhull */ void convhull_3d_build_alloc(/* input arguments */ - ch_vertex* const in_vertices, /* vector of input vertices; nVert x 1 */ - const int nVert, /* number of vertices */ + ch_vertex* const + in_vertices, /* vector of input vertices; nVert + x 1 */ + const int nVert, /* number of vertices */ /* output arguments */ - int** out_faces, /* & of empty int*, output face indices; flat: nOut_faces x 3 */ - int* nOut_faces, /* & of int, number of output face indices */ - void* allocator); /* & of an allocator */ + int** + out_faces, /* & of empty int*, output face + indices; flat: nOut_faces x 3 */ + int* nOut_faces, /* & of int, number of output face + indices */ + void* allocator); /* & of an allocator */ /* builds the N-Dimensional convexhull of a grid of points */ void convhull_nd_build_alloc(/* input arguments */ - CH_FLOAT* const in_points, /* Matrix of points in 'd' dimensions; FLAT: nPoints x d */ - const int nPoints, /* number of points */ - const int d, /* Number of dimensions */ + CH_FLOAT* const + in_points, /* Matrix of points in 'd' + dimensions; FLAT: nPoints x d */ + const int nPoints, /* number of points */ + const int d, /* Number of dimensions */ /* output arguments */ - int** out_faces, /* (&) output face indices; FLAT: nOut_faces x d */ - CH_FLOAT** out_cf, /* (&) contains the coefficients of the planes (set to NULL if not wanted); FLAT: nOut_faces x d */ - CH_FLOAT** out_df, /* (&) contains the constant terms of the planes (set to NULL if not wanted); nOut_faces x 1 */ - int* nOut_faces, /* (&) number of output face indices */ - void* allocator); /* & of an allocator */ - -/* Computes the Delaunay triangulation (mesh) of an arrangement of points in N-dimensional space */ -void delaunay_nd_mesh_alloc(/* input Arguments */ - const float* points, /* The input points; FLAT: nPoints x nd */ - const int nPoints, /* Number of points */ - const int nd, /* The number of dimensions */ + int** out_faces, /* (&) output face indices; FLAT: + nOut_faces x d */ + CH_FLOAT** + out_cf, /* (&) contains the coefficients of the + planes (set to NULL if not wanted); + FLAT: nOut_faces x d */ + CH_FLOAT** + out_df, /* (&) contains the constant terms of + the planes (set to NULL if not + wanted); nOut_faces x 1 */ + int* nOut_faces, /* (&) number of output face + indices */ + void* allocator); /* & of an allocator */ + +/* Computes the Delaunay triangulation (mesh) of an arrangement of points in + * N-dimensional space */ +void delaunay_nd_mesh_alloc( /* input Arguments */ + const float* points, /* The input points; FLAT: + nPoints x nd */ + const int nPoints, /* Number of points */ + const int nd, /* The number of dimensions */ /* output Arguments */ - int** Mesh, /* (&) the indices defining the Delaunay triangulation of the points; FLAT: nMesh x (nd+1) */ - int* nMesh, /* (&) Number of triangulations */ - void* allocator); /* & of an allocator */ + int** Mesh, /* (&) the indices defining the Delaunay + triangulation of the points; FLAT: + nMesh x (nd+1) */ + int* nMesh, /* (&) Number of triangulations */ + void* allocator); /* & of an allocator */ -/* reads an 'obj' file and extracts only the vertices (for 3d convexhulls only) */ +/* reads an 'obj' file and extracts only the vertices (for 3d convexhulls only) + */ void extract_vertices_from_obj_file_alloc(/* input arguments */ - char* const obj_filename, /* obj filename, WITHOUT extension */ + char* const + obj_filename, /* obj filename, + WITHOUT extension + */ /* output arguments */ - ch_vertex** out_vertices, /* & of empty ch_vertex*, output vertices; out_nVert x 1 */ - int* out_nVert, /* & of int, number of vertices */ - void* allocator); /* & of an allocator */ + ch_vertex** + out_vertices, /* & of empty + ch_vertex*, + output vertices; + out_nVert x 1 */ + int* out_nVert, /* & of int, number of + vertices */ + void* allocator); /* & of an allocator + */ #ifdef __cplusplus } /*extern "C"*/ @@ -174,28 +238,26 @@ void extract_vertices_from_obj_file_alloc(/* input arguments */ #endif /* CONVHULL_3D_INCLUDED */ - /************ * INTERNAL: ***********/ #ifdef CONVHULL_3D_ENABLE -#include -#include -#include -#include -#include +#include #include +#include +#include +#include +#include +#include #include -#include -#include #if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) - #define CV_STRNCPY(a,b,c) strncpy_s(a,c+1,b,c); - #define CV_STRCAT(a,b) strcat_s(a,sizeof(b),b); + #define CV_STRNCPY(a, b, c) strncpy_s(a, c + 1, b, c); + #define CV_STRCAT(a, b) strcat_s(a, sizeof(b), b); #else - #define CV_STRNCPY(a,b,c) strncpy(a,b,c); - #define CV_STRCAT(a,b) strcat(a,b); + #define CV_STRNCPY(a, b, c) strncpy(a, b, c); + #define CV_STRCAT(a, b) strcat(a, b); #endif #ifdef CONVHULL_3D_USE_SINGLE_PRECISION #define CH_FLT_MIN FLT_MIN @@ -211,10 +273,10 @@ void extract_vertices_from_obj_file_alloc(/* input arguments */ #define ch_sqrt sqrt #endif #ifndef MIN - #define MIN(a,b) (( (a) < (b) ) ? (a) : (b) ) + #define MIN(a, b) (((a) < (b)) ? (a) : (b)) #endif #ifndef MAX - #define MAX(a,b) (( (a) > (b) ) ? (a) : (b) ) + #define MAX(a, b) (((a) > (b)) ? (a) : (b)) #endif #ifndef ch_malloc #define ch_malloc malloc @@ -229,20 +291,21 @@ void extract_vertices_from_obj_file_alloc(/* input arguments */ #define ch_free free #endif #ifndef ch_stateful_malloc - #define ch_stateful_malloc(allocator, size) ch_malloc(size) + #define ch_stateful_malloc(allocator, size) ch_malloc(size) #endif #ifndef ch_stateful_calloc - #define ch_stateful_calloc(allocator, num, size) ch_calloc(num, size) + #define ch_stateful_calloc(allocator, num, size) ch_calloc(num, size) #endif #ifndef ch_stateful_realloc - #define ch_stateful_realloc(allocator, ptr, size) ch_realloc(ptr, size) + #define ch_stateful_realloc(allocator, ptr, size) ch_realloc(ptr, size) #endif #ifndef ch_stateful_free - #define ch_stateful_free(allocator, ptr) ch_free(ptr) + #define ch_stateful_free(allocator, ptr) ch_free(ptr) #endif #ifndef ch_stateful_resize - #define ch_stateful_resize(allocator, ptr, size) default_memory_resize(allocator, ptr, size) - #define CONVHULL_CREATE_DEFAULT_RESIZE 1 + #define ch_stateful_resize(allocator, ptr, size) \ + default_memory_resize(allocator, ptr, size) + #define CONVHULL_CREATE_DEFAULT_RESIZE 1 #endif #define CH_MAX_NUM_FACES 50000 @@ -250,10 +313,11 @@ void extract_vertices_from_obj_file_alloc(/* input arguments */ #define CONVHULL_ND_MAX_DIMENSIONS 5 /* structs for qsort */ -typedef struct float_w_idx { - CH_FLOAT val; - int idx; -}float_w_idx; +typedef struct float_w_idx +{ + CH_FLOAT val; + int idx; +} float_w_idx; /* internal functions prototypes: */ static int cmp_asc_float(const void*, const void*); @@ -270,287 +334,296 @@ static void ismember(int*, int*, int*, int, int); #ifdef CONVHULL_CREATE_DEFAULT_RESIZE static void* default_memory_resize(void* allocator, void* ptr, size_t size) { - if (ptr) - ch_stateful_free(allocator, ptr); - return ch_stateful_malloc(allocator, size); + if (ptr) + ch_stateful_free(allocator, ptr); + return ch_stateful_malloc(allocator, size); } #endif -static int cmp_asc_float(const void *a,const void *b) { - struct float_w_idx *a1 = (struct float_w_idx*)a; - struct float_w_idx *a2 = (struct float_w_idx*)b; - if((*a1).val<(*a2).val)return -1; - else if((*a1).val>(*a2).val)return 1; - else return 0; +static int cmp_asc_float(const void* a, const void* b) +{ + struct float_w_idx* a1 = (struct float_w_idx*)a; + struct float_w_idx* a2 = (struct float_w_idx*)b; + if ((*a1).val < (*a2).val) + return -1; + else if ((*a1).val > (*a2).val) + return 1; + else + return 0; } -static int cmp_desc_float(const void *a,const void *b) { - struct float_w_idx *a1 = (struct float_w_idx*)a; - struct float_w_idx *a2 = (struct float_w_idx*)b; - if((*a1).val>(*a2).val)return -1; - else if((*a1).val<(*a2).val)return 1; - else return 0; +static int cmp_desc_float(const void* a, const void* b) +{ + struct float_w_idx* a1 = (struct float_w_idx*)a; + struct float_w_idx* a2 = (struct float_w_idx*)b; + if ((*a1).val > (*a2).val) + return -1; + else if ((*a1).val < (*a2).val) + return 1; + else + return 0; } -static int cmp_asc_int(const void *a,const void *b) { - int *a1 = (int*)a; - int *a2 = (int*)b; - if((*a1)<(*a2))return -1; - else if((*a1)>(*a2))return 1; - else return 0; +static int cmp_asc_int(const void* a, const void* b) +{ + int* a1 = (int*)a; + int* a2 = (int*)b; + if ((*a1) < (*a2)) + return -1; + else if ((*a1) > (*a2)) + return 1; + else + return 0; } -static void sort_float -( +static void sort_float( CH_FLOAT* in_vec, /* vector[len] to be sorted */ CH_FLOAT* out_vec, /* if NULL, then in_vec is sorted "in-place" */ int* new_idices, /* set to NULL if you don't need them */ - int len, /* number of elements in vectors, must be consistent with the input data */ - int descendFLAG, /* !1:ascending, 1:descending */ - void* allocator /* (stateful) allocator */ + int len, /* number of elements in vectors, must be consistent with the input + data */ + int descendFLAG, /* !1:ascending, 1:descending */ + void* allocator /* (stateful) allocator */ ) { - int i; - struct float_w_idx *data; - - data = (float_w_idx*)ch_stateful_malloc(allocator, len*sizeof(float_w_idx)); - for(i=0;iy * v2->z - v1->z * v2->y; - cross.y = v1->z * v2->x - v1->x * v2->z; - cross.z = v1->x * v2->y - v1->y * v2->x; - return cross; + ch_vec3 cross; + cross.x = v1->y * v2->z - v1->z * v2->y; + cross.y = v1->z * v2->x - v1->x * v2->z; + cross.z = v1->x * v2->y - v1->y * v2->x; + return cross; } /* calculates the determinent of a 4x4 matrix */ -static CH_FLOAT det_4x4(CH_FLOAT* m) { - return - m[3] * m[6] * m[9] * m[12] - m[2] * m[7] * m[9] * m[12] - - m[3] * m[5] * m[10] * m[12] + m[1] * m[7] * m[10] * m[12] + - m[2] * m[5] * m[11] * m[12] - m[1] * m[6] * m[11] * m[12] - - m[3] * m[6] * m[8] * m[13] + m[2] * m[7] * m[8] * m[13] + - m[3] * m[4] * m[10] * m[13] - m[0] * m[7] * m[10] * m[13] - - m[2] * m[4] * m[11] * m[13] + m[0] * m[6] * m[11] * m[13] + - m[3] * m[5] * m[8] * m[14] - m[1] * m[7] * m[8] * m[14] - - m[3] * m[4] * m[9] * m[14] + m[0] * m[7] * m[9] * m[14] + - m[1] * m[4] * m[11] * m[14] - m[0] * m[5] * m[11] * m[14] - - m[2] * m[5] * m[8] * m[15] + m[1] * m[6] * m[8] * m[15] + - m[2] * m[4] * m[9] * m[15] - m[0] * m[6] * m[9] * m[15] - - m[1] * m[4] * m[10] * m[15] + m[0] * m[5] * m[10] * m[15]; +static CH_FLOAT det_4x4(CH_FLOAT* m) +{ + return m[3] * m[6] * m[9] * m[12] - m[2] * m[7] * m[9] * m[12] + - m[3] * m[5] * m[10] * m[12] + m[1] * m[7] * m[10] * m[12] + + m[2] * m[5] * m[11] * m[12] - m[1] * m[6] * m[11] * m[12] + - m[3] * m[6] * m[8] * m[13] + m[2] * m[7] * m[8] * m[13] + + m[3] * m[4] * m[10] * m[13] - m[0] * m[7] * m[10] * m[13] + - m[2] * m[4] * m[11] * m[13] + m[0] * m[6] * m[11] * m[13] + + m[3] * m[5] * m[8] * m[14] - m[1] * m[7] * m[8] * m[14] + - m[3] * m[4] * m[9] * m[14] + m[0] * m[7] * m[9] * m[14] + + m[1] * m[4] * m[11] * m[14] - m[0] * m[5] * m[11] * m[14] + - m[2] * m[5] * m[8] * m[15] + m[1] * m[6] * m[8] * m[15] + + m[2] * m[4] * m[9] * m[15] - m[0] * m[6] * m[9] * m[15] + - m[1] * m[4] * m[10] * m[15] + m[0] * m[5] * m[10] * m[15]; } /* Helper function for det_NxN() */ -static void createSubMatrix -( - CH_FLOAT* m, - int N, - int i, - CH_FLOAT* sub_m -) +static void createSubMatrix(CH_FLOAT* m, int N, int i, CH_FLOAT* sub_m) { - int j, k; - for(j = N, k=0; j < N * N; j++){ - if(j % N != i){ /* i is the index to remove */ - sub_m[k] = m[j]; - k++; - } + int j, k; + for (j = N, k = 0; j < N * N; j++) + { + if (j % N != i) + { /* i is the index to remove */ + sub_m[k] = m[j]; + k++; } + } } -static CH_FLOAT det_NxN -( - CH_FLOAT* m, - int d -) +static CH_FLOAT det_NxN(CH_FLOAT* m, int d) { - CH_FLOAT sum; - CH_FLOAT sub_m[CONVHULL_ND_MAX_DIMENSIONS*CONVHULL_ND_MAX_DIMENSIONS]; - int sign; - - if (d == 0) - return 1.0; - sum = 0.0; - sign = 1; - for (int i = 0; i < d; i++) { - createSubMatrix(m, d, i, sub_m); - sum += sign * m[i] * det_NxN(sub_m, d - 1); - sign *= -1; - } - return sum; + CH_FLOAT sum; + CH_FLOAT sub_m[CONVHULL_ND_MAX_DIMENSIONS * CONVHULL_ND_MAX_DIMENSIONS]; + int sign; + + if (d == 0) + return 1.0; + sum = 0.0; + sign = 1; + for (int i = 0; i < d; i++) + { + createSubMatrix(m, d, i, sub_m); + sum += sign * m[i] * det_NxN(sub_m, d - 1); + sign *= -1; + } + return sum; } /* Calculates the coefficients of the equation of a PLANE in 3D. * Original Copyright (c) 2014, George Papazafeiropoulos * Distributed under the BSD (2-clause) license */ -static void plane_3d -( - CH_FLOAT* p, - CH_FLOAT* c, - CH_FLOAT* d -) +static void plane_3d(CH_FLOAT* p, CH_FLOAT* c, CH_FLOAT* d) { - int i, j, k, l; - int r[3]; - CH_FLOAT sign, det, norm_c; - CH_FLOAT pdiff[2][3], pdiff_s[2][2]; - - for(i=0; i<2; i++) - for(j=0; j<3; j++) - pdiff[i][j] = p[(i+1)*3+j] - p[i*3+j]; - memset(c, 0, 3*sizeof(CH_FLOAT)); - sign = 1.0; - for(i=0; i<3; i++) - r[i] = i; - for(i=0; i<3; i++){ - for(j=0; j<2; j++){ - for(k=0, l=0; k<3; k++){ - if(r[k]!=i){ - pdiff_s[j][l] = pdiff[j][k]; - l++; - } - } + int i, j, k, l; + int r[3]; + CH_FLOAT sign, det, norm_c; + CH_FLOAT pdiff[2][3], pdiff_s[2][2]; + + for (i = 0; i < 2; i++) + for (j = 0; j < 3; j++) + pdiff[i][j] = p[(i + 1) * 3 + j] - p[i * 3 + j]; + memset(c, 0, 3 * sizeof(CH_FLOAT)); + sign = 1.0; + for (i = 0; i < 3; i++) + r[i] = i; + for (i = 0; i < 3; i++) + { + for (j = 0; j < 2; j++) + { + for (k = 0, l = 0; k < 3; k++) + { + if (r[k] != i) + { + pdiff_s[j][l] = pdiff[j][k]; + l++; } - det = pdiff_s[0][0]*pdiff_s[1][1] - pdiff_s[1][0]*pdiff_s[0][1]; - c[i] = sign * det; - sign *= -1.0; + } } - norm_c = (CH_FLOAT)0.0; - for(i=0; i<3; i++) - norm_c += (ch_pow(c[i], (CH_FLOAT)2.0)); - norm_c = ch_sqrt(norm_c); - for(i=0; i<3; i++) - c[i] /= norm_c; - (*d) = (CH_FLOAT)0.0; - for(i=0; i<3; i++) - (*d) += -p[i] * c[i]; + det = pdiff_s[0][0] * pdiff_s[1][1] - pdiff_s[1][0] * pdiff_s[0][1]; + c[i] = sign * det; + sign *= -1.0; + } + norm_c = (CH_FLOAT)0.0; + for (i = 0; i < 3; i++) + norm_c += (ch_pow(c[i], (CH_FLOAT)2.0)); + norm_c = ch_sqrt(norm_c); + for (i = 0; i < 3; i++) + c[i] /= norm_c; + (*d) = (CH_FLOAT)0.0; + for (i = 0; i < 3; i++) + (*d) += -p[i] * c[i]; } /* Calculates the coefficients of the equation of a PLANE in ND. * Original Copyright (c) 2014, George Papazafeiropoulos * Distributed under the BSD (2-clause) license */ -static void plane_nd -( - const int Nd, - CH_FLOAT* p, - CH_FLOAT* c, - CH_FLOAT* d -) +static void plane_nd(const int Nd, CH_FLOAT* p, CH_FLOAT* c, CH_FLOAT* d) { - int i, j, k, l; - int r[CONVHULL_ND_MAX_DIMENSIONS]; - CH_FLOAT sign, det, norm_c; - CH_FLOAT pdiff[CONVHULL_ND_MAX_DIMENSIONS-1][CONVHULL_ND_MAX_DIMENSIONS], pdiff_s[(CONVHULL_ND_MAX_DIMENSIONS-1)*(CONVHULL_ND_MAX_DIMENSIONS-1)]; - - if(Nd==3){ - plane_3d(p,c,d); - return; - } - - for(i=0; i=2 dimensions, but "plane_3d" and "det_4x4" are hardcoded for 3, - * so would need to be rewritten */ - d = 3; - - /* Add noise to the points */ - points = (CH_FLOAT*)ch_stateful_malloc(allocator, nVert*(d+1)*sizeof(CH_FLOAT)); - for(i=0; i=2 dimensions, but + * "plane_3d" and "det_4x4" are hardcoded for 3, so would need to be rewritten + */ + d = 3; + + /* Add noise to the points */ + points = (CH_FLOAT*)ch_stateful_malloc( + allocator, nVert * (d + 1) * sizeof(CH_FLOAT)); + for (i = 0; i < nVert; i++) + { + for (j = 0; j < d; j++) + points[i * (d + 1) + j] + = in_vertices[i].v[j] + + CH_NOISE_VAL * rnd(i, j); /* noise mitigates duplicates */ + points[i * (d + 1) + d] = 1.0f; /* add a last column of ones. Used only for + determinant calculation */ + } + + /* Find the span */ + for (j = 0; j < d; j++) + { + max_p = (CH_FLOAT)-2.23e+13; + min_p = (CH_FLOAT)2.23e+13; + for (i = 0; i < nVert; i++) + { + max_p = MAX(max_p, points[i * (d + 1) + j]); + min_p = MIN(min_p, points[i * (d + 1) + j]); } - - /* Find the span */ - for(j=0; j0.000000001); + /* If you hit this assertion error, then the input vertices do not span all + * 3 dimensions. Therefore the convex hull could be built in less + * dimensions. In these cases, consider reducing the dimensionality of the + * points and calling convhull_nd_build() instead with d<3 You can turn this + * assert off using CONVHULL_ALLOW_BUILD_IN_HIGHER_DIM if you still wish to + * build in a higher number of dimensions. */ + assert(span[j] > 0.000000001); #endif + } + + /* The initial convex hull is a simplex with (d+1) facets, where d is the + * number of dimensions */ + nFaces = (d + 1); + faces = (int*)ch_stateful_calloc(allocator, nFaces * d, sizeof(int)); + aVec = (int*)ch_stateful_malloc(allocator, nFaces * sizeof(int)); + for (i = 0; i < nFaces; i++) + aVec[i] = i; + + /* Each column of cf contains the coefficients of a plane */ + cf = (CH_FLOAT*)ch_stateful_malloc(allocator, nFaces * d * sizeof(CH_FLOAT)); + df = (CH_FLOAT*)ch_stateful_malloc(allocator, nFaces * sizeof(CH_FLOAT)); + for (i = 0; i < nFaces; i++) + { + /* Set the indices of the points defining the face */ + for (j = 0, k = 0; j < (d + 1); j++) + { + if (aVec[j] != i) + { + faces[i * d + k] = aVec[j]; + k++; + } } - - /* The initial convex hull is a simplex with (d+1) facets, where d is the number of dimensions */ - nFaces = (d+1); - faces = (int*)ch_stateful_calloc(allocator, nFaces*d, sizeof(int)); - aVec = (int*)ch_stateful_malloc(allocator, nFaces*sizeof(int)); - for(i=0; i0) ){ - /* i is the first point of the points left */ - i = pleft[0]; - - /* Delete the point selected */ - for(j=0; j 0)) + { + /* i is the first point of the points left */ + i = pleft[0]; + + /* Delete the point selected */ + for (j = 0; j < num_pleft - 1; j++) + pleft[j] = pleft[j + 1]; + num_pleft--; + if (num_pleft == 0) + ch_stateful_free(allocator, pleft); + else + pleft = (int*)ch_stateful_realloc( + allocator, pleft, num_pleft * sizeof(int)); + + /* Update point selection counter */ + cnt++; + + /* find visible faces */ + for (j = 0; j < d; j++) + points_s[j] = points[i * (d + 1) + j]; + points_cf = (CH_FLOAT*)ch_stateful_realloc( + allocator, points_cf, nFaces * sizeof(CH_FLOAT)); + visible_ind = (int*)ch_stateful_realloc( + allocator, visible_ind, nFaces * sizeof(int)); #ifdef CONVHULL_3D_USE_CBLAS #ifdef CONVHULL_3D_USE_SINGLE_PRECISION - cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0f, - points_s, d, - cf, d, 0.0f, - points_cf, nFaces); + cblas_sgemm( + CblasRowMajor, + CblasNoTrans, + CblasTrans, + 1, + nFaces, + d, + 1.0f, + points_s, + d, + cf, + d, + 0.0f, + points_cf, + nFaces); #else - cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0, - points_s, d, - cf, d, 0.0, - points_cf, nFaces); + cblas_dgemm( + CblasRowMajor, + CblasNoTrans, + CblasTrans, + 1, + nFaces, + d, + 1.0, + points_s, + d, + cf, + d, + 0.0, + points_cf, + nFaces); #endif #else - for (j = 0; j < nFaces; j++) { - points_cf[j] = 0; - for (k = 0; k < d; k++) - points_cf[j] += points_s[k]*cf[j*d+k]; - } + for (j = 0; j < nFaces; j++) + { + points_cf[j] = 0; + for (k = 0; k < d; k++) + points_cf[j] += points_s[k] * cf[j * d + k]; + } #endif - num_visible_ind = 0; - for(j=0; j 0.0){ - num_visible_ind++; /* will sum to 0 if none are visible */ - visible_ind[j] = 1; - } + num_visible_ind = 0; + for (j = 0; j < nFaces; j++) + { + if (points_cf[j] + df[j] > 0.0) + { + num_visible_ind++; /* will sum to 0 if none are visible */ + visible_ind[j] = 1; + } + else + visible_ind[j] = 0; + } + num_nonvisible_faces = nFaces - num_visible_ind; + + /* proceed if there are any visible faces */ + if (num_visible_ind != 0) + { + /* Find visible face indices */ + visible = (int*)ch_stateful_resize( + allocator, visible, num_visible_ind * sizeof(int)); + for (j = 0, k = 0; j < nFaces; j++) + { + if (visible_ind[j] == 1) + { + visible[k] = j; + k++; + } + } + + /* Find nonvisible faces */ + nonvisible_faces = (int*)ch_stateful_resize( + allocator, nonvisible_faces, num_nonvisible_faces * d * sizeof(int)); + f0 = (int*)ch_stateful_resize( + allocator, f0, num_nonvisible_faces * d * sizeof(int)); + for (j = 0, k = 0; j < nFaces; j++) + { + if (visible_ind[j] == 0) + { + for (l = 0; l < d; l++) + nonvisible_faces[k * d + l] = faces[j * d + l]; + k++; + } + } + + /* Create horizon (count is the number of the edges of the horizon) */ + count = 0; + for (j = 0; j < num_visible_ind; j++) + { + /* visible face */ + vis = visible[j]; + for (k = 0; k < d; k++) + face_s[k] = faces[vis * d + k]; + sort_int(face_s, d); + ismember(nonvisible_faces, face_s, f0, num_nonvisible_faces * d, d); + u_len = 0; + + /* u are the nonvisible faces connected to the face v, if any */ + for (k = 0; k < num_nonvisible_faces; k++) + { + f0_sum = 0; + for (l = 0; l < d; l++) + f0_sum += f0[k * d + l]; + if (f0_sum == d - 1) + { + u_len++; + if (u_len == 1) + u = (int*)ch_stateful_resize(allocator, u, u_len * sizeof(int)); else - visible_ind[j] = 0; + u = (int*)ch_stateful_realloc(allocator, u, u_len * sizeof(int)); + u[u_len - 1] = k; + } } - num_nonvisible_faces = nFaces - num_visible_ind; - - /* proceed if there are any visible faces */ - if(num_visible_ind!=0){ - /* Find visible face indices */ - visible = (int*)ch_stateful_resize(allocator, visible, num_visible_ind*sizeof(int)); - for(j=0, k=0; j CH_MAX_NUM_FACES) - n_realloc_faces = CH_MAX_NUM_FACES+1; - faces = (int*)ch_stateful_realloc(allocator, faces, n_realloc_faces*d*sizeof(int)); - cf = (CH_FLOAT*)ch_stateful_realloc(allocator, cf, n_realloc_faces*d*sizeof(CH_FLOAT)); - df = (CH_FLOAT*)ch_stateful_realloc(allocator, df, n_realloc_faces*sizeof(CH_FLOAT)); - - for(j=0; j CH_MAX_NUM_FACES){ - FUCKED = 1; - nFaces = 0; - break; - } - } - - /* Orient each new face properly */ - hVec = (int*)ch_stateful_resize(allocator, hVec, nFaces*sizeof(int)); - hVec_mem_face = (int*)ch_stateful_resize(allocator, hVec_mem_face, nFaces*sizeof(int)); - for(j=0; j0.0); -#endif - } + for (k = 0; k < u_len; k++) + { + /* The boundary between the visible face v and the k(th) nonvisible + * face connected to the face v forms part of the horizon */ + count++; + if (count == 1) + horizon = (int*)ch_stateful_resize( + allocator, horizon, count * (d - 1) * sizeof(int)); + else + horizon = (int*)ch_stateful_realloc( + allocator, horizon, count * (d - 1) * sizeof(int)); + for (l = 0; l < d; l++) + gVec[l] = nonvisible_faces[u[k] * d + l]; + for (l = 0, h = 0; l < d; l++) + { + if (f0[u[k] * d + l]) + { + horizon[(count - 1) * (d - 1) + h] = gVec[l]; + h++; } + } } - if(FUCKED){ - break; + } + horizon_size1 = count; + for (j = 0, l = 0; j < nFaces; j++) + { + if (!visible_ind[j]) + { + /* Delete visible faces */ + for (k = 0; k < d; k++) + faces[l * d + k] = faces[j * d + k]; + + /* Delete the corresponding plane coefficients of the faces */ + for (k = 0; k < d; k++) + cf[l * d + k] = cf[j * d + k]; + df[l] = df[j]; + l++; } + } + + /* Update the number of faces */ + nFaces = nFaces - num_visible_ind; + + /* start is the first row of the new faces */ + start = nFaces; + + /* Add faces connecting horizon to the new point */ + n_newfaces = horizon_size1; + n_realloc_faces = nFaces + n_newfaces; + if (n_realloc_faces > CH_MAX_NUM_FACES) + n_realloc_faces = CH_MAX_NUM_FACES + 1; + faces = (int*)ch_stateful_realloc( + allocator, faces, n_realloc_faces * d * sizeof(int)); + cf = (CH_FLOAT*)ch_stateful_realloc( + allocator, cf, n_realloc_faces * d * sizeof(CH_FLOAT)); + df = (CH_FLOAT*)ch_stateful_realloc( + allocator, df, n_realloc_faces * sizeof(CH_FLOAT)); + + for (j = 0; j < n_newfaces; j++) + { + nFaces++; + for (k = 0; k < d - 1; k++) + faces[(nFaces - 1) * d + k] = horizon[j * (d - 1) + k]; + faces[(nFaces - 1) * d + (d - 1)] = i; + + /* Calculate and store appropriately the plane coefficients of the faces + */ + for (k = 0; k < d; k++) + for (l = 0; l < d; l++) + p_s[k * d + l] + = points[(faces[(nFaces - 1) * d + k]) * (d + 1) + l]; + plane_3d(p_s, cfi, &dfi); + for (k = 0; k < d; k++) + cf[(nFaces - 1) * d + k] = cfi[k]; + df[(nFaces - 1)] = dfi; + if (nFaces > CH_MAX_NUM_FACES) + { + FUCKED = 1; + nFaces = 0; + break; + } + } + + /* Orient each new face properly */ + hVec = (int*)ch_stateful_resize(allocator, hVec, nFaces * sizeof(int)); + hVec_mem_face = (int*)ch_stateful_resize( + allocator, hVec_mem_face, nFaces * sizeof(int)); + for (j = 0; j < nFaces; j++) + hVec[j] = j; + for (k = start; k < nFaces; k++) + { + for (j = 0; j < d; j++) + face_s[j] = faces[k * d + j]; + sort_int(face_s, d); + ismember(hVec, face_s, hVec_mem_face, nFaces, d); + num_p = 0; + for (j = 0; j < nFaces; j++) + if (!hVec_mem_face[j]) + num_p++; + pp = (int*)ch_stateful_resize(allocator, pp, num_p * sizeof(int)); + for (j = 0, l = 0; j < nFaces; j++) + { + if (!hVec_mem_face[j]) + { + pp[l] = hVec[j]; + l++; + } + } + index = 0; + detA = 0.0; + + /* While new point is coplanar, choose another point */ + while (detA == 0.0) + { + for (j = 0; j < d; j++) + for (l = 0; l < d + 1; l++) + A[j * (d + 1) + l] = points[(faces[k * d + j]) * (d + 1) + l]; + for (; j < d + 1; j++) + for (l = 0; l < d + 1; l++) + A[j * (d + 1) + l] = points[pp[index] * (d + 1) + l]; + index++; + detA = det_4x4(A); + } + + /* Orient faces so that each point on the original simplex can't see the + * opposite face */ + if (detA < 0.0) + { + /* If orientation is improper, reverse the order to change the volume + * sign */ + for (j = 0; j < 2; j++) + face_tmp[j] = faces[k * d + d - j - 1]; + for (j = 0; j < 2; j++) + faces[k * d + d - j - 1] = face_tmp[1 - j]; + + /* Modify the plane coefficients of the properly oriented faces */ + for (j = 0; j < d; j++) + cf[k * d + j] = -cf[k * d + j]; + df[k] = -df[k]; + for (l = 0; l < d; l++) + for (j = 0; j < d + 1; j++) + A[l * (d + 1) + j] = points[(faces[k * d + l]) * (d + 1) + j]; + for (; l < d + 1; l++) + for (j = 0; j < d + 1; j++) + A[l * (d + 1) + j] = points[pp[index] * (d + 1) + j]; +#if DART_BUILD_MODE_DEBUG + /* Check */ + detA = det_4x4(A); + /* If you hit this assertion error, then the face cannot be properly + * orientated */ + assert(detA > 0.0); +#endif + } + } } - - /* output */ - if(FUCKED){ - (*out_faces) = NULL; - (*nOut_faces) = 0; - } - else{ - (*out_faces) = (int*)ch_stateful_malloc(allocator, nFaces*d*sizeof(int)); - memcpy((*out_faces),faces, nFaces*d*sizeof(int)); - (*nOut_faces) = nFaces; + if (FUCKED) + { + break; } - - /* clean-up */ - ch_stateful_free(allocator, u); - ch_stateful_free(allocator, pp); - ch_stateful_free(allocator, horizon); - ch_stateful_free(allocator, f0); - ch_stateful_free(allocator, nonvisible_faces); - ch_stateful_free(allocator, visible); - ch_stateful_free(allocator, hVec); - ch_stateful_free(allocator, hVec_mem_face); - ch_stateful_free(allocator, visible_ind); - ch_stateful_free(allocator, points_cf); - ch_stateful_free(allocator, absdist); - ch_stateful_free(allocator, reldist); - ch_stateful_free(allocator, desReldist); - ch_stateful_free(allocator, ind); - ch_stateful_free(allocator, points); - ch_stateful_free(allocator, faces); - ch_stateful_free(allocator, aVec); - ch_stateful_free(allocator, cf); - ch_stateful_free(allocator, df); + } + + /* output */ + if (FUCKED) + { + (*out_faces) = NULL; + (*nOut_faces) = 0; + } + else + { + (*out_faces) + = (int*)ch_stateful_malloc(allocator, nFaces * d * sizeof(int)); + memcpy((*out_faces), faces, nFaces * d * sizeof(int)); + (*nOut_faces) = nFaces; + } + + /* clean-up */ + ch_stateful_free(allocator, u); + ch_stateful_free(allocator, pp); + ch_stateful_free(allocator, horizon); + ch_stateful_free(allocator, f0); + ch_stateful_free(allocator, nonvisible_faces); + ch_stateful_free(allocator, visible); + ch_stateful_free(allocator, hVec); + ch_stateful_free(allocator, hVec_mem_face); + ch_stateful_free(allocator, visible_ind); + ch_stateful_free(allocator, points_cf); + ch_stateful_free(allocator, absdist); + ch_stateful_free(allocator, reldist); + ch_stateful_free(allocator, desReldist); + ch_stateful_free(allocator, ind); + ch_stateful_free(allocator, points); + ch_stateful_free(allocator, faces); + ch_stateful_free(allocator, aVec); + ch_stateful_free(allocator, cf); + ch_stateful_free(allocator, df); } -inline void convhull_3d_export_obj -( +inline void convhull_3d_export_obj( ch_vertex* const vertices, const int nVert, int* const faces, const int nFaces, const int keepOnlyUsedVerticesFLAG, - char* const obj_filename -) + char* const obj_filename) { - int i, j; - char path[256] = "\0"; - CV_STRNCPY(path, obj_filename, strlen(obj_filename)); - FILE* obj_file; + int i, j; + char path[256] = "\0"; + CV_STRNCPY(path, obj_filename, strlen(obj_filename)); + FILE* obj_file; #if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) - CV_STRCAT(path, ".obj"); - fopen_s(&obj_file, path, "wt"); + CV_STRCAT(path, ".obj"); + fopen_s(&obj_file, path, "wt"); #else - errno = 0; - obj_file = fopen(strcat(path, ".obj"), "wt"); + errno = 0; + obj_file = fopen(strcat(path, ".obj"), "wt"); #endif - if (obj_file==NULL) { - printf("Error %d \n", errno); - printf("It's null"); - } - fprintf(obj_file, "o\n"); - CH_FLOAT scale; - ch_vec3 v1, v2, normal; - - /* export vertices */ - if(keepOnlyUsedVerticesFLAG){ - for (i = 0; i < nFaces; i++) - for(j=0; j<3; j++) - fprintf(obj_file, "v %f %f %f\n", vertices[faces[i*3+j]].x, - vertices[faces[i*3+j]].y, vertices[faces[i*3+j]].z); - } - else { - for (i = 0; i < nVert; i++) - fprintf(obj_file, "v %f %f %f\n", vertices[i].x, - vertices[i].y, vertices[i].z); + if (obj_file == NULL) + { + printf("Error %d \n", errno); + printf("It's null"); + } + fprintf(obj_file, "o\n"); + CH_FLOAT scale; + ch_vec3 v1, v2, normal; + + /* export vertices */ + if (keepOnlyUsedVerticesFLAG) + { + for (i = 0; i < nFaces; i++) + for (j = 0; j < 3; j++) + fprintf( + obj_file, + "v %f %f %f\n", + vertices[faces[i * 3 + j]].x, + vertices[faces[i * 3 + j]].y, + vertices[faces[i * 3 + j]].z); + } + else + { + for (i = 0; i < nVert; i++) + fprintf( + obj_file, + "v %f %f %f\n", + vertices[i].x, + vertices[i].y, + vertices[i].z); + } + + /* export the face normals */ + for (i = 0; i < nFaces; i++) + { + /* calculate cross product between v1-v0 and v2-v0 */ + v1 = vertices[faces[i * 3 + 1]]; + v2 = vertices[faces[i * 3 + 2]]; + v1.x -= vertices[faces[i * 3]].x; + v1.y -= vertices[faces[i * 3]].y; + v1.z -= vertices[faces[i * 3]].z; + v2.x -= vertices[faces[i * 3]].x; + v2.y -= vertices[faces[i * 3]].y; + v2.z -= vertices[faces[i * 3]].z; + normal = cross(&v1, &v2); + + /* normalise to unit length */ + scale = ((CH_FLOAT)1.0) + / (ch_sqrt( + ch_pow(normal.x, (CH_FLOAT)2.0) + + ch_pow(normal.y, (CH_FLOAT)2.0) + + ch_pow(normal.z, (CH_FLOAT)2.0)) + + (CH_FLOAT)2.23e-9); + normal.x *= scale; + normal.y *= scale; + normal.z *= scale; + fprintf(obj_file, "vn %f %f %f\n", normal.x, normal.y, normal.z); + } + + /* export the face indices */ + if (keepOnlyUsedVerticesFLAG) + { + for (i = 0; i < nFaces; i++) + { + /* vertices are in same order as the faces, and normals are in order */ + fprintf( + obj_file, + "f %u//%u %u//%u %u//%u\n", + i * 3 + 1, + i + 1, + i * 3 + 1 + 1, + i + 1, + i * 3 + 2 + 1, + i + 1); } - - /* export the face normals */ - for (i = 0; i < nFaces; i++){ - /* calculate cross product between v1-v0 and v2-v0 */ - v1 = vertices[faces[i*3+1]]; - v2 = vertices[faces[i*3+2]]; - v1.x -= vertices[faces[i*3]].x; - v1.y -= vertices[faces[i*3]].y; - v1.z -= vertices[faces[i*3]].z; - v2.x -= vertices[faces[i*3]].x; - v2.y -= vertices[faces[i*3]].y; - v2.z -= vertices[faces[i*3]].z; - normal = cross(&v1, &v2); - - /* normalise to unit length */ - scale = ((CH_FLOAT)1.0)/(ch_sqrt(ch_pow(normal.x, (CH_FLOAT)2.0)+ch_pow(normal.y, (CH_FLOAT)2.0)+ch_pow(normal.z, (CH_FLOAT)2.0))+(CH_FLOAT)2.23e-9); - normal.x *= scale; - normal.y *= scale; - normal.z *= scale; - fprintf(obj_file, "vn %f %f %f\n", normal.x, normal.y, normal.z); + } + else + { + /* just normals are in order */ + for (i = 0; i < nFaces; i++) + { + fprintf( + obj_file, + "f %u//%u %u//%u %u//%u\n", + faces[i * 3] + 1, + i + 1, + faces[i * 3 + 1] + 1, + i + 1, + faces[i * 3 + 2] + 1, + i + 1); } - - /* export the face indices */ - if(keepOnlyUsedVerticesFLAG){ - for (i = 0; i < nFaces; i++){ - /* vertices are in same order as the faces, and normals are in order */ - fprintf(obj_file, "f %u//%u %u//%u %u//%u\n", - i*3 + 1, i + 1, - i*3+1 + 1, i + 1, - i*3+2 + 1, i + 1); - } - } - else { - /* just normals are in order */ - for (i = 0; i < nFaces; i++){ - fprintf(obj_file, "f %u//%u %u//%u %u//%u\n", - faces[i*3] + 1, i + 1, - faces[i*3+1] + 1, i + 1, - faces[i*3+2] + 1, i + 1); - } - } - fclose(obj_file); + } + fclose(obj_file); } -inline void convhull_3d_export_m -( +inline void convhull_3d_export_m( ch_vertex* const vertices, const int nVert, int* const faces, const int nFaces, - char* const m_filename -) + char* const m_filename) { - int i; - char path[256] = { "\0" }; - memcpy(path, m_filename, strlen(m_filename)); - FILE* m_file; + int i; + char path[256] = {"\0"}; + memcpy(path, m_filename, strlen(m_filename)); + FILE* m_file; #if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) - CV_STRCAT(path, ".m"); - fopen_s(&m_file, path, "wt"); + CV_STRCAT(path, ".m"); + fopen_s(&m_file, path, "wt"); #else - m_file = fopen(strcat(path, ".m"), "wt"); + m_file = fopen(strcat(path, ".m"), "wt"); #endif - - /* save face indices and vertices for verification in matlab: */ - fprintf(m_file, "vertices = [\n"); - for (i = 0; i < nVert; i++) - fprintf(m_file, "%f, %f, %f;\n", vertices[i].x, vertices[i].y, vertices[i].z); - fprintf(m_file, "];\n\n\n"); - fprintf(m_file, "faces = [\n"); - for (i = 0; i < nFaces; i++) { - fprintf(m_file, " %u, %u, %u;\n", - faces[3*i+0]+1, - faces[3*i+1]+1, - faces[3*i+2]+1); - } - fprintf(m_file, "];\n\n\n"); - fclose(m_file); + + /* save face indices and vertices for verification in matlab: */ + fprintf(m_file, "vertices = [\n"); + for (i = 0; i < nVert; i++) + fprintf( + m_file, "%f, %f, %f;\n", vertices[i].x, vertices[i].y, vertices[i].z); + fprintf(m_file, "];\n\n\n"); + fprintf(m_file, "faces = [\n"); + for (i = 0; i < nFaces; i++) + { + fprintf( + m_file, + " %u, %u, %u;\n", + faces[3 * i + 0] + 1, + faces[3 * i + 1] + 1, + faces[3 * i + 2] + 1); + } + fprintf(m_file, "];\n\n\n"); + fclose(m_file); } -inline void extract_vertices_from_obj_file -( - char* const obj_filename, - ch_vertex** out_vertices, - int* out_nVert) +inline void extract_vertices_from_obj_file( + char* const obj_filename, ch_vertex** out_vertices, int* out_nVert) { - extract_vertices_from_obj_file_alloc(obj_filename, out_vertices, out_nVert, NULL); + extract_vertices_from_obj_file_alloc( + obj_filename, out_vertices, out_nVert, NULL); } -inline void extract_vertices_from_obj_file_alloc -( +inline void extract_vertices_from_obj_file_alloc( char* const obj_filename, ch_vertex** out_vertices, int* out_nVert, - void* allocator -) + void* allocator) { - FILE* obj_file; + FILE* obj_file; #if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) - CV_STRCAT(obj_filename, ".obj"); - fopen_s(&obj_file, obj_filename, "r"); + CV_STRCAT(obj_filename, ".obj"); + fopen_s(&obj_file, obj_filename, "r"); #else - obj_file = fopen(strcat(obj_filename, ".obj"), "r"); -#endif - - /* determine number of vertices */ - unsigned int nVert = 0; - char line[256]; - while (fgets(line, sizeof(line), obj_file)) { - char* vexists = strstr(line, "v "); - if(vexists!=NULL) - nVert++; - } - (*out_nVert) = nVert; - (*out_vertices) = (ch_vertex*)ch_stateful_malloc(allocator, nVert*sizeof(ch_vertex)); - - /* extract the vertices */ - rewind(obj_file); - int i=0; - int vertID, prev_char_isDigit, current_char_isDigit; - char vert_char[256] = { 0 }; - while (fgets(line, sizeof(line), obj_file)) { - char* vexists = strstr(line, "v "); - if(vexists!=NULL){ - prev_char_isDigit = 0; - vertID = -1; - for(size_t j=0; j4){ - /* not a valid file */ - ch_stateful_free(allocator, (*out_vertices)); - (*out_vertices) = NULL; - (*out_nVert) = 0; - return; - } - (*out_vertices)[i].v[vertID] = (CH_FLOAT)atof(vert_char); - memset(vert_char, 0, 256 * sizeof(char)); - } - prev_char_isDigit = current_char_isDigit; - } - i++; + obj_file = fopen(strcat(obj_filename, ".obj"), "r"); +#endif + + /* determine number of vertices */ + unsigned int nVert = 0; + char line[256]; + while (fgets(line, sizeof(line), obj_file)) + { + char* vexists = strstr(line, "v "); + if (vexists != NULL) + nVert++; + } + (*out_nVert) = nVert; + (*out_vertices) + = (ch_vertex*)ch_stateful_malloc(allocator, nVert * sizeof(ch_vertex)); + + /* extract the vertices */ + rewind(obj_file); + int i = 0; + int vertID, prev_char_isDigit, current_char_isDigit; + char vert_char[256] = {0}; + while (fgets(line, sizeof(line), obj_file)) + { + char* vexists = strstr(line, "v "); + if (vexists != NULL) + { + prev_char_isDigit = 0; + vertID = -1; + for (size_t j = 0; j < strlen(line) - 1; j++) + { + if (isdigit(line[j]) || line[j] == '.' || line[j] == '-' + || line[j] == '+' || line[j] == 'E' || line[j] == 'e') + { + vert_char[strlen(vert_char)] = line[j]; + current_char_isDigit = 1; } + else + current_char_isDigit = 0; + if ((prev_char_isDigit && !current_char_isDigit) + || j == strlen(line) - 2) + { + vertID++; + if (vertID > 4) + { + /* not a valid file */ + ch_stateful_free(allocator, (*out_vertices)); + (*out_vertices) = NULL; + (*out_nVert) = 0; + return; + } + (*out_vertices)[i].v[vertID] = (CH_FLOAT)atof(vert_char); + memset(vert_char, 0, 256 * sizeof(char)); + } + prev_char_isDigit = current_char_isDigit; + } + i++; } + } } - - /**** NEW! ****/ /* A C version of the ND quickhull matlab implementation from here: @@ -1232,25 +1434,24 @@ inline void extract_vertices_from_obj_file_alloc * (*out_faces) is returned as NULL, if triangulation fails * * Original Copyright (c) 2014, George Papazafeiropoulos * Distributed under the BSD (2-clause) license - * Reference: "The Quickhull Algorithm for Convex Hull, C. Bradford Barber, David P. Dobkin - * and Hannu Huhdanpaa, Geometry Center Technical Report GCG53, July 30, 1993" + * Reference: "The Quickhull Algorithm for Convex Hull, C. Bradford Barber, + * David P. Dobkin and Hannu Huhdanpaa, Geometry Center Technical Report GCG53, + * July 30, 1993" */ -inline void convhull_nd_build -( +inline void convhull_nd_build( CH_FLOAT* const in_vertices, const int nVert, const int d, int** out_faces, CH_FLOAT** out_cf, CH_FLOAT** out_df, - int* nOut_faces -) + int* nOut_faces) { - convhull_nd_build_alloc(in_vertices, nVert, d, out_faces, out_cf, out_df, nOut_faces, NULL); + convhull_nd_build_alloc( + in_vertices, nVert, d, out_faces, out_cf, out_df, nOut_faces, NULL); } -inline void convhull_nd_build_alloc -( +inline void convhull_nd_build_alloc( CH_FLOAT* const in_vertices, const int nVert, const int d, @@ -1258,621 +1459,759 @@ inline void convhull_nd_build_alloc CH_FLOAT** out_cf, CH_FLOAT** out_df, int* nOut_faces, - void* allocator -) + void* allocator) { - int i, j, k, l, h; - int nFaces, p; - int* aVec, *faces; - CH_FLOAT dfi, v, max_p, min_p; - CH_FLOAT span[CONVHULL_ND_MAX_DIMENSIONS]; - CH_FLOAT cfi[CONVHULL_ND_MAX_DIMENSIONS]; - CH_FLOAT p_s[CONVHULL_ND_MAX_DIMENSIONS*CONVHULL_ND_MAX_DIMENSIONS]; - CH_FLOAT* points, *cf, *df; - - assert(d<=CONVHULL_ND_MAX_DIMENSIONS); - - /* Solution not possible... */ - if(d>CONVHULL_ND_MAX_DIMENSIONS || nVert<=d || in_vertices==NULL){ - (*out_faces) = NULL; - (*nOut_faces) = 0; - if(out_cf!=NULL) - (*out_cf) = NULL; - if(out_df!=NULL) - (*out_df) = NULL; - return; - } - - /* Add noise to the points */ - points = (CH_FLOAT*)ch_stateful_malloc(allocator, nVert*(d+1)*sizeof(CH_FLOAT)); - for(i=0; i CONVHULL_ND_MAX_DIMENSIONS || nVert <= d || in_vertices == NULL) + { + (*out_faces) = NULL; + (*nOut_faces) = 0; + if (out_cf != NULL) + (*out_cf) = NULL; + if (out_df != NULL) + (*out_df) = NULL; + return; + } + + /* Add noise to the points */ + points = (CH_FLOAT*)ch_stateful_malloc( + allocator, nVert * (d + 1) * sizeof(CH_FLOAT)); + for (i = 0; i < nVert; i++) + { + for (j = 0; j < d; j++) + points[i * (d + 1) + j] + = in_vertices[i * d + j] + CH_NOISE_VAL * rnd(i, j); + points[i * (d + 1) + d] = 1.0; /* add a last column of ones. Used only for + determinant calculation */ + } + + /* Find the span */ + for (j = 0; j < d; j++) + { + max_p = (CH_FLOAT)-2.23e+13; + min_p = (CH_FLOAT)2.23e+13; + for (i = 0; i < nVert; i++) + { + max_p = MAX(max_p, points[i * (d + 1) + j]); + min_p = MIN(min_p, points[i * (d + 1) + j]); } - - /* Find the span */ - for(j=0; j0.000000001); + /* If you hit this assertion error, then the input vertices do not span all + * 'd' dimensions. Therefore the convex hull could be built in less + * dimensions. In these cases, consider reducing the dimensionality of the + * points and calling convhull_nd_build() with a smaller d You can turn this + * assert off using CONVHULL_ALLOW_BUILD_IN_HIGHER_DIM if you still wish to + * build in a higher number of dimensions. */ + assert(span[j] > 0.000000001); #endif + } + + /* The initial convex hull is a simplex with (d+1) facets, where d is the + * number of dimensions */ + nFaces = (d + 1); + faces = (int*)ch_stateful_calloc(allocator, nFaces * d, sizeof(int)); + aVec = (int*)ch_stateful_malloc(allocator, nFaces * sizeof(int)); + for (i = 0; i < nFaces; i++) + aVec[i] = i; + + /* Each column of cf contains the coefficients of a plane */ + cf = (CH_FLOAT*)ch_stateful_malloc(allocator, nFaces * d * sizeof(CH_FLOAT)); + df = (CH_FLOAT*)ch_stateful_malloc(allocator, nFaces * sizeof(CH_FLOAT)); + for (i = 0; i < nFaces; i++) + { + /* Set the indices of the points defining the face */ + for (j = 0, k = 0; j < (d + 1); j++) + { + if (aVec[j] != i) + { + faces[i * d + k] = aVec[j]; + k++; + } } - /* The initial convex hull is a simplex with (d+1) facets, where d is the number of dimensions */ - nFaces = (d+1); - faces = (int*)ch_stateful_calloc(allocator, nFaces*d, sizeof(int)); - aVec = (int*)ch_stateful_malloc(allocator, nFaces*sizeof(int)); - for(i=0; i0) ){ - /* i is the first point of the points left */ - i = pleft[0]; - - /* Delete the point selected */ - for(j=0; j 0)) + { + /* i is the first point of the points left */ + i = pleft[0]; + + /* Delete the point selected */ + for (j = 0; j < num_pleft - 1; j++) + pleft[j] = pleft[j + 1]; + num_pleft--; + if (num_pleft == 0) + ch_stateful_free(allocator, pleft); + else + pleft = (int*)ch_stateful_realloc( + allocator, pleft, num_pleft * sizeof(int)); + + /* Update point selection counter */ + cnt++; + + /* find visible faces */ + for (j = 0; j < d; j++) + points_s[j] = points[i * (d + 1) + j]; + points_cf = (CH_FLOAT*)ch_stateful_realloc( + allocator, points_cf, nFaces * sizeof(CH_FLOAT)); + visible_ind = (int*)ch_stateful_realloc( + allocator, visible_ind, nFaces * sizeof(int)); #ifdef CONVHULL_3D_USE_CBLAS #ifdef CONVHULL_3D_USE_SINGLE_PRECISION - cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0f, - points_s, d, - cf, d, 0.0f, - points_cf, nFaces); + cblas_sgemm( + CblasRowMajor, + CblasNoTrans, + CblasTrans, + 1, + nFaces, + d, + 1.0f, + points_s, + d, + cf, + d, + 0.0f, + points_cf, + nFaces); #else - cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, nFaces, d, 1.0, - points_s, d, - cf, d, 0.0, - points_cf, nFaces); + cblas_dgemm( + CblasRowMajor, + CblasNoTrans, + CblasTrans, + 1, + nFaces, + d, + 1.0, + points_s, + d, + cf, + d, + 0.0, + points_cf, + nFaces); #endif #else - for (j = 0; j < nFaces; j++) { - points_cf[j] = 0; - for (k = 0; k < d; k++) - points_cf[j] += points_s[k]*cf[j*d+k]; - } + for (j = 0; j < nFaces; j++) + { + points_cf[j] = 0; + for (k = 0; k < d; k++) + points_cf[j] += points_s[k] * cf[j * d + k]; + } #endif - num_visible_ind = 0; - for(j=0; j 0.0){ - num_visible_ind++; /* will sum to 0 if none are visible */ - visible_ind[j] = 1; - } + num_visible_ind = 0; + for (j = 0; j < nFaces; j++) + { + if (points_cf[j] + df[j] > 0.0) + { + num_visible_ind++; /* will sum to 0 if none are visible */ + visible_ind[j] = 1; + } + else + visible_ind[j] = 0; + } + num_nonvisible_faces = nFaces - num_visible_ind; + + /* proceed if there are any visible faces */ + if (num_visible_ind != 0) + { + /* Find visible face indices */ + visible = (int*)ch_stateful_resize( + allocator, visible, num_visible_ind * sizeof(int)); + for (j = 0, k = 0; j < nFaces; j++) + { + if (visible_ind[j] == 1) + { + visible[k] = j; + k++; + } + } + + /* Find nonvisible faces */ + nonvisible_faces = (int*)ch_stateful_resize( + allocator, nonvisible_faces, num_nonvisible_faces * d * sizeof(int)); + f0 = (int*)ch_stateful_resize( + allocator, f0, num_nonvisible_faces * d * sizeof(int)); + for (j = 0, k = 0; j < nFaces; j++) + { + if (visible_ind[j] == 0) + { + for (l = 0; l < d; l++) + nonvisible_faces[k * d + l] = faces[j * d + l]; + k++; + } + } + + /* Create horizon (count is the number of the edges of the horizon) */ + count = 0; + for (j = 0; j < num_visible_ind; j++) + { + /* visible face */ + vis = visible[j]; + for (k = 0; k < d; k++) + face_s[k] = faces[vis * d + k]; + sort_int(face_s, d); + ismember(nonvisible_faces, face_s, f0, num_nonvisible_faces * d, d); + u_len = 0; + + /* u are the nonvisible faces connected to the face v, if any */ + for (k = 0; k < num_nonvisible_faces; k++) + { + f0_sum = 0; + for (l = 0; l < d; l++) + f0_sum += f0[k * d + l]; + if (f0_sum == d - 1) + { + u_len++; + if (u_len == 1) + u = (int*)ch_stateful_resize(allocator, u, u_len * sizeof(int)); else - visible_ind[j] = 0; + u = (int*)ch_stateful_realloc(allocator, u, u_len * sizeof(int)); + u[u_len - 1] = k; + } } - num_nonvisible_faces = nFaces - num_visible_ind; - - /* proceed if there are any visible faces */ - if(num_visible_ind!=0){ - /* Find visible face indices */ - visible = (int*)ch_stateful_resize(allocator, visible, num_visible_ind*sizeof(int)); - for(j=0, k=0; j CH_MAX_NUM_FACES) - n_realloc_faces = CH_MAX_NUM_FACES+1; - faces = (int*)ch_stateful_realloc(allocator, faces, (nFaces+n_newfaces)*d*sizeof(int)); - cf = (CH_FLOAT*)ch_stateful_realloc(allocator, cf, (nFaces+n_newfaces)*d*sizeof(CH_FLOAT)); - df = (CH_FLOAT*)ch_stateful_realloc(allocator, df, (nFaces+n_newfaces)*sizeof(CH_FLOAT)); - - for(j=0; j CH_MAX_NUM_FACES){ - FUCKED = 1; - nFaces = 0; - break; - } + for (k = 0; k < u_len; k++) + { + /* The boundary between the visible face v and the k(th) nonvisible + * face connected to the face v forms part of the horizon */ + count++; + if (count == 1) + horizon = (int*)ch_stateful_resize( + allocator, horizon, count * (d - 1) * sizeof(int)); + else + horizon = (int*)ch_stateful_realloc( + allocator, horizon, count * (d - 1) * sizeof(int)); + for (l = 0; l < d; l++) + gVec[l] = nonvisible_faces[u[k] * d + l]; + for (l = 0, h = 0; l < d; l++) + { + if (f0[u[k] * d + l]) + { + horizon[(count - 1) * (d - 1) + h] = gVec[l]; + h++; } + } + } + } + horizon_size1 = count; + for (j = 0, l = 0; j < nFaces; j++) + { + if (!visible_ind[j]) + { + /* Delete visible faces */ + for (k = 0; k < d; k++) + faces[l * d + k] = faces[j * d + k]; + + /* Delete the corresponding plane coefficients of the faces */ + for (k = 0; k < d; k++) + cf[l * d + k] = cf[j * d + k]; + df[l] = df[j]; + l++; + } + } + + /* Update the number of faces */ + nFaces = nFaces - num_visible_ind; + + /* start is the first row of the new faces */ + start = nFaces; + + /* Add faces connecting horizon to the new point */ + n_newfaces = horizon_size1; + n_realloc_faces = nFaces + n_newfaces; + if (n_realloc_faces > CH_MAX_NUM_FACES) + n_realloc_faces = CH_MAX_NUM_FACES + 1; + faces = (int*)ch_stateful_realloc( + allocator, faces, (nFaces + n_newfaces) * d * sizeof(int)); + cf = (CH_FLOAT*)ch_stateful_realloc( + allocator, cf, (nFaces + n_newfaces) * d * sizeof(CH_FLOAT)); + df = (CH_FLOAT*)ch_stateful_realloc( + allocator, df, (nFaces + n_newfaces) * sizeof(CH_FLOAT)); + + for (j = 0; j < n_newfaces; j++) + { + nFaces++; + for (k = 0; k < d - 1; k++) + faces[(nFaces - 1) * d + k] = horizon[j * (d - 1) + k]; + faces[(nFaces - 1) * d + (d - 1)] = i; + + /* Calculate and store appropriately the plane coefficients of the faces + */ + for (k = 0; k < d; k++) + for (l = 0; l < d; l++) + p_s[k * d + l] + = points[(faces[(nFaces - 1) * d + k]) * (d + 1) + l]; + plane_nd(d, p_s, cfi, &dfi); + for (k = 0; k < d; k++) + cf[(nFaces - 1) * d + k] = cfi[k]; + df[(nFaces - 1)] = dfi; + if (nFaces > CH_MAX_NUM_FACES) + { + FUCKED = 1; + nFaces = 0; + break; + } + } + + /* Orient each new face properly */ + hVec = (int*)ch_stateful_resize(allocator, hVec, nFaces * sizeof(int)); + hVec_mem_face = (int*)ch_stateful_resize( + allocator, hVec_mem_face, nFaces * sizeof(int)); + for (j = 0; j < nFaces; j++) + hVec[j] = j; + for (k = start; k < nFaces; k++) + { + for (j = 0; j < d; j++) + face_s[j] = faces[k * d + j]; + sort_int(face_s, d); + ismember(hVec, face_s, hVec_mem_face, nFaces, d); + num_p = 0; + for (j = 0; j < nFaces; j++) + if (!hVec_mem_face[j]) + num_p++; + pp = (int*)ch_stateful_resize(allocator, pp, num_p * sizeof(int)); + for (j = 0, l = 0; j < nFaces; j++) + { + if (!hVec_mem_face[j]) + { + pp[l] = hVec[j]; + l++; + } + } + index = 0; + detA = 0.0; + + /* While new point is coplanar, choose another point */ + while (detA == 0.0) + { + for (j = 0; j < d; j++) + for (l = 0; l < d + 1; l++) + A[j * (d + 1) + l] = points[(faces[k * d + j]) * (d + 1) + l]; + for (; j < d + 1; j++) + for (l = 0; l < d + 1; l++) + A[j * (d + 1) + l] = points[pp[index] * (d + 1) + l]; + index++; + if (d == 3) + detA = det_4x4(A); + else + detA = det_NxN((CH_FLOAT*)A, d + 1); + } - /* Orient each new face properly */ - hVec = (int*)ch_stateful_resize(allocator, hVec, nFaces*sizeof(int)); - hVec_mem_face = (int*)ch_stateful_resize(allocator, hVec_mem_face, nFaces*sizeof(int)); - for(j=0; j0.0); + /* Orient faces so that each point on the original simplex can't see the + * opposite face */ + if (detA < 0.0) + { + /* If orientation is improper, reverse the order to change the volume + * sign */ + for (j = 0; j < 2; j++) + face_tmp[j] = faces[k * d + d - j - 1]; + for (j = 0; j < 2; j++) + faces[k * d + d - j - 1] = face_tmp[1 - j]; + + /* Modify the plane coefficients of the properly oriented faces */ + for (j = 0; j < d; j++) + cf[k * d + j] = -cf[k * d + j]; + df[k] = -df[k]; + for (l = 0; l < d; l++) + for (j = 0; j < d + 1; j++) + A[l * (d + 1) + j] = points[(faces[k * d + l]) * (d + 1) + j]; + for (; l < d + 1; l++) + for (j = 0; j < d + 1; j++) + A[l * (d + 1) + j] = points[pp[index] * (d + 1) + j]; +#if DART_BUILD_MODE_DEBUG + /* Check */ + if (d == 3) + detA = det_4x4(A); + else + detA = det_NxN((CH_FLOAT*)A, d + 1); + /* If you hit this assertion error, then the face cannot be properly + * orientated and building the convex hull is likely impossible */ + assert(detA > 0.0); #endif - } - } - } - if(FUCKED){ - break; } + } } - - /* output */ - if(FUCKED){ - (*out_faces) = NULL; - if(out_cf!=NULL) - (*out_cf) = NULL; - if(out_df!=NULL) - (*out_df) = NULL; - (*nOut_faces) = 0; + if (FUCKED) + { + break; } - else{ - (*out_faces) = (int*)ch_stateful_malloc(allocator, nFaces*d*sizeof(int)); - memcpy((*out_faces),faces, nFaces*d*sizeof(int)); - (*nOut_faces) = nFaces; - if(out_cf!=NULL){ - (*out_cf) = (CH_FLOAT*)ch_stateful_malloc(allocator, nFaces*d*sizeof(CH_FLOAT)); - memcpy((*out_cf), cf, nFaces*d*sizeof(CH_FLOAT)); - } - if(out_df!=NULL){ - (*out_df) = (CH_FLOAT*)ch_stateful_malloc(allocator, nFaces*sizeof(CH_FLOAT)); - memcpy((*out_df), df, nFaces*sizeof(CH_FLOAT)); - } + } + + /* output */ + if (FUCKED) + { + (*out_faces) = NULL; + if (out_cf != NULL) + (*out_cf) = NULL; + if (out_df != NULL) + (*out_df) = NULL; + (*nOut_faces) = 0; + } + else + { + (*out_faces) + = (int*)ch_stateful_malloc(allocator, nFaces * d * sizeof(int)); + memcpy((*out_faces), faces, nFaces * d * sizeof(int)); + (*nOut_faces) = nFaces; + if (out_cf != NULL) + { + (*out_cf) = (CH_FLOAT*)ch_stateful_malloc( + allocator, nFaces * d * sizeof(CH_FLOAT)); + memcpy((*out_cf), cf, nFaces * d * sizeof(CH_FLOAT)); } - - /* clean-up */ - ch_stateful_free(allocator, u); - ch_stateful_free(allocator, pp); - ch_stateful_free(allocator, horizon); - ch_stateful_free(allocator, f0); - ch_stateful_free(allocator, nonvisible_faces); - ch_stateful_free(allocator, visible); - ch_stateful_free(allocator, hVec); - ch_stateful_free(allocator, hVec_mem_face); - ch_stateful_free(allocator, visible_ind); - ch_stateful_free(allocator, points_cf); - ch_stateful_free(allocator, absdist); - ch_stateful_free(allocator, reldist); - ch_stateful_free(allocator, desReldist); - ch_stateful_free(allocator, ind); - ch_stateful_free(allocator, points); - ch_stateful_free(allocator, faces); - ch_stateful_free(allocator, aVec); - ch_stateful_free(allocator, cf); - ch_stateful_free(allocator, df); + if (out_df != NULL) + { + (*out_df) + = (CH_FLOAT*)ch_stateful_malloc(allocator, nFaces * sizeof(CH_FLOAT)); + memcpy((*out_df), df, nFaces * sizeof(CH_FLOAT)); + } + } + + /* clean-up */ + ch_stateful_free(allocator, u); + ch_stateful_free(allocator, pp); + ch_stateful_free(allocator, horizon); + ch_stateful_free(allocator, f0); + ch_stateful_free(allocator, nonvisible_faces); + ch_stateful_free(allocator, visible); + ch_stateful_free(allocator, hVec); + ch_stateful_free(allocator, hVec_mem_face); + ch_stateful_free(allocator, visible_ind); + ch_stateful_free(allocator, points_cf); + ch_stateful_free(allocator, absdist); + ch_stateful_free(allocator, reldist); + ch_stateful_free(allocator, desReldist); + ch_stateful_free(allocator, ind); + ch_stateful_free(allocator, points); + ch_stateful_free(allocator, faces); + ch_stateful_free(allocator, aVec); + ch_stateful_free(allocator, cf); + ch_stateful_free(allocator, df); } -inline void delaunay_nd_mesh -( +inline void delaunay_nd_mesh( const float* points, const int nPoints, const int nd, int** Mesh, - int* nMesh -) + int* nMesh) { - delaunay_nd_mesh_alloc(points, nPoints, nd, Mesh, nMesh, NULL); + delaunay_nd_mesh_alloc(points, nPoints, nd, Mesh, nMesh, NULL); } -inline void delaunay_nd_mesh_alloc -( +inline void delaunay_nd_mesh_alloc( const float* points, const int nPoints, const int nd, int** Mesh, int* nMesh, - void* allocator -) + void* allocator) { - int i, j, k, nHullFaces, maxW_idx, nVisible; - int* hullfaces; - CH_FLOAT w0, w_optimal, w_optimal2; - CH_FLOAT* projpoints, *cf, *df, *p0, *p, *visible; - - /* Project the N-dimensional points onto a N+1-dimensional paraboloid */ - projpoints = (CH_FLOAT*)ch_stateful_malloc(allocator, nPoints*(nd+1)*sizeof(CH_FLOAT)); - for(i = 0; i < nPoints; i++) { - projpoints[i*(nd+1)+nd] = 0.0; - for(j=0; j CONVHULL_ND_MAX_DIMENSIONS || !hullfaces || !nHullFaces) + { + (*Mesh) = NULL; + (*nMesh) = 0; + ch_stateful_free(allocator, projpoints); + return; + } - /* The N-dimensional delaunay triangulation requires first computing the convex hull of this N+1-dimensional paraboloid */ - hullfaces = NULL; - cf = df = NULL; - convhull_nd_build(projpoints, nPoints, nd+1, &hullfaces, &cf, &df, &nHullFaces); - - /* Solution not possible... */ - if(nd>CONVHULL_ND_MAX_DIMENSIONS || !hullfaces || !nHullFaces){ - (*Mesh) = NULL; - (*nMesh) = 0; - ch_stateful_free(allocator, projpoints); - return; - } - - /* Find the coordinates of the point with the maximum (N+1 dimension) coordinate (i.e. the w vector) */ + /* Find the coordinates of the point with the maximum (N+1 dimension) + * coordinate (i.e. the w vector) */ #ifdef CONVHULL_3D_USE_CBLAS - if(sizeof(CH_FLOAT)==sizeof(double)) - maxW_idx = (int)cblas_idamax(nPoints, (double*)&projpoints[nd], nd+1); - else - maxW_idx = (int)cblas_isamax(nPoints, (float*)&projpoints[nd], nd+1); + if (sizeof(CH_FLOAT) == sizeof(double)) + maxW_idx = (int)cblas_idamax(nPoints, (double*)&projpoints[nd], nd + 1); + else + maxW_idx = (int)cblas_isamax(nPoints, (float*)&projpoints[nd], nd + 1); #else - CH_FLOAT maxVal; - maxVal = (CH_FLOAT)-2.23e13; - maxW_idx = -1; - for(i=0; imaxVal){ - maxVal = projpoints[i*(nd+1)+nd]; - maxW_idx = i; - } + CH_FLOAT maxVal; + maxVal = (CH_FLOAT)-2.23e13; + maxW_idx = -1; + for (i = 0; i < nPoints; i++) + { + if (projpoints[i * (nd + 1) + nd] > maxVal) + { + maxVal = projpoints[i * (nd + 1) + nd]; + maxW_idx = i; } - assert(maxW_idx!=-1); + } + assert(maxW_idx != -1); #endif - w0 = projpoints[maxW_idx*(nd+1)+nd]; - p0 = (CH_FLOAT*)ch_stateful_malloc(allocator, nd*sizeof(CH_FLOAT)); - for(j=0; j0.0) - nVisible++; - } - - /* Output */ - (*nMesh) = nVisible; - if(nVisible>0){ - (*Mesh) = (int*)ch_stateful_malloc(allocator, nVisible*(nd+1)*sizeof(int)); - for(i=0, j=0; i0.0){ - for(k=0; k 0.0) + nVisible++; + } + + /* Output */ + (*nMesh) = nVisible; + if (nVisible > 0) + { + (*Mesh) = (int*)ch_stateful_malloc( + allocator, nVisible * (nd + 1) * sizeof(int)); + for (i = 0, j = 0; i < nHullFaces; i++) + { + if (visible[i] > 0.0) + { + for (k = 0; k < nd + 1; k++) + (*Mesh)[j * (nd + 1) + k] = hullfaces[i * (nd + 1) + k]; + j++; + } } - - /* clean up */ - ch_stateful_free(allocator, projpoints); - ch_stateful_free(allocator, hullfaces); - ch_stateful_free(allocator, cf); - ch_stateful_free(allocator, df); - ch_stateful_free(allocator, p0); - ch_stateful_free(allocator, p); - ch_stateful_free(allocator, visible); + assert(j == nVisible); + } + + /* clean up */ + ch_stateful_free(allocator, projpoints); + ch_stateful_free(allocator, hullfaces); + ch_stateful_free(allocator, cf); + ch_stateful_free(allocator, df); + ch_stateful_free(allocator, p0); + ch_stateful_free(allocator, p); + ch_stateful_free(allocator, visible); } - #endif /* CONVHULL_3D_ENABLE */ diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index 005d7692a1d24..eaabe16b7e4f6 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -1523,7 +1523,7 @@ bool verifyTransform(const Eigen::Isometry3d& _T) Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG if (std::abs(_m(0, 0)) > DART_EPSILON || std::abs(_m(1, 1)) > DART_EPSILON || std::abs(_m(2, 2)) > DART_EPSILON) { diff --git a/dart/utils/urdf/URDFTypes.hpp b/dart/utils/urdf/URDFTypes.hpp index 44120a7590820..2e08e08b4f0a2 100644 --- a/dart/utils/urdf/URDFTypes.hpp +++ b/dart/utils/urdf/URDFTypes.hpp @@ -35,7 +35,7 @@ #pragma message( \ "This header has been deprecated in DART 6.2. " \ - "Please include dart/utils/urdf/BackwardCompatibility.hpp intead.") + "Please include dart/utils/urdf/BackwardCompatibility.hpp instead.") #include "dart/utils/urdf/BackwardCompatibility.hpp" diff --git a/examples/wam_ikfast/Helpers.cpp b/examples/wam_ikfast/Helpers.cpp index c5a6b8c74d3b1..726280c2eaa4a 100644 --- a/examples/wam_ikfast/Helpers.cpp +++ b/examples/wam_ikfast/Helpers.cpp @@ -109,7 +109,7 @@ void setupEndEffectors(const dart::dynamics::SkeletonPtr& wam) std::stringstream ss; ss << DART_SHARED_LIB_PREFIX << "wamIk"; -#if (DART_OS_LINUX || DART_OS_MACOS) && !NDEBUG +#if (DART_OS_LINUX || DART_OS_MACOS) && DART_BUILD_MODE_DEBUG ss << "d"; #endif ss << "." << DART_SHARED_LIB_EXTENSION; diff --git a/python/dartpy/eigen_geometry_pybind.cpp b/python/dartpy/eigen_geometry_pybind.cpp index 6d93dbfd5eeec..c6a04b0ea97ef 100644 --- a/python/dartpy/eigen_geometry_pybind.cpp +++ b/python/dartpy/eigen_geometry_pybind.cpp @@ -32,15 +32,15 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // -#include +#include "eigen_geometry_pybind.h" + #include +#include #include #include "pybind11/pybind11.h" -#include "eigen_geometry_pybind.h" - using std::fabs; namespace dart { @@ -60,60 +60,61 @@ namespace { // N.B. Use a loose tolerance, so that we don't have to be super strict with // C++. -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG const double kCheckTolerance = 1e-5; #endif template -void CheckRotMat(const Eigen::Matrix& R) { +void CheckRotMat(const Eigen::Matrix& R) +{ // See `ExpectRotMat`. - const T identity_error = - (R * R.transpose() - Eigen::Matrix::Identity()) - .array().abs().maxCoeff(); + const T identity_error + = (R * R.transpose() - Eigen::Matrix::Identity()) + .array() + .abs() + .maxCoeff(); DART_UNUSED(identity_error); assert( - identity_error < kCheckTolerance && - "Rotation matrix is not orthonormal"); + identity_error < kCheckTolerance && "Rotation matrix is not orthonormal"); const T det_error = fabs(R.determinant() - 1); DART_UNUSED(det_error); assert( - det_error < kCheckTolerance && - "Rotation matrix violates right-hand rule"); + det_error < kCheckTolerance + && "Rotation matrix violates right-hand rule"); } template -void CheckIsometry(const Eigen::Transform& X) { +void CheckIsometry(const Eigen::Transform& X) +{ CheckRotMat(X.linear()); Eigen::Matrix bottom_expected; bottom_expected << 0, 0, 0, 1; - const T bottom_error = - (X.matrix().bottomRows(1) - bottom_expected).array().abs().maxCoeff(); + const T bottom_error + = (X.matrix().bottomRows(1) - bottom_expected).array().abs().maxCoeff(); DART_UNUSED(bottom_error); assert( - bottom_error < kCheckTolerance && - "Homogeneous matrix is improperly scaled."); + bottom_error < kCheckTolerance + && "Homogeneous matrix is improperly scaled."); } template -void CheckQuaternion(const Eigen::Quaternion& q) { +void CheckQuaternion(const Eigen::Quaternion& q) +{ const T norm_error = fabs(q.coeffs().norm() - 1); DART_UNUSED(norm_error); - assert( - norm_error < kCheckTolerance && - "Quaternion is not normalized"); + assert(norm_error < kCheckTolerance && "Quaternion is not normalized"); } template -void CheckAngleAxis(const Eigen::AngleAxis& value) { +void CheckAngleAxis(const Eigen::AngleAxis& value) +{ const T norm_error = fabs(value.axis().norm() - 1); DART_UNUSED(norm_error); - assert( - norm_error < kCheckTolerance && - "Axis is not normalized"); + assert(norm_error < kCheckTolerance && "Axis is not normalized"); } -} // namespace +} // namespace // PYBIND11_MODULE(eigen_geometry, m) { void eigen_geometry(pybind11::module& parent_m) @@ -132,98 +133,125 @@ void eigen_geometry(pybind11::module& parent_m) { using Class = Eigen::Transform; ::pybind11::class_ py_class(m, "Isometry3"); - py_class - .def(::pybind11::init([]() { - return Class::Identity(); - })) - .def_static("Identity", []() { - return Class::Identity(); - }) - .def(::pybind11::init([](const Eigen::Matrix& matrix) { - Class out(matrix); - CheckIsometry(out); - return out; - }), ::pybind11::arg("matrix")) - .def(::pybind11::init([]( - const Eigen::Matrix& rotation, - const Eigen::Matrix& translation) { - CheckRotMat(rotation); - Class out = Class::Identity(); - out.linear() = rotation; - out.translation() = translation; - return out; - }), ::pybind11::arg("rotation"), ::pybind11::arg("translation")) - .def(::pybind11::init([]( - const Eigen::Quaternion& q, - const Eigen::Matrix& translation) { - CheckQuaternion(q); - Class out = Class::Identity(); - out.linear() = q.toRotationMatrix(); - out.translation() = translation; - return out; - }), ::pybind11::arg("quaternion"), ::pybind11::arg("translation")) - .def(::pybind11::init([](const Class& other) { - CheckIsometry(other); - return other; - }), ::pybind11::arg("other")) - .def("matrix", [](const Class* self) -> Eigen::Matrix { - return self->matrix(); - }) - .def("set_identity", [](Class* self) { - self->setIdentity(); - }) - .def("set_matrix", [](Class* self, const Eigen::Matrix& matrix) { - Class update(matrix); - CheckIsometry(update); - *self = update; - }) - .def("translation", [](const Class* self) -> Eigen::Matrix { - return self->translation(); - }) - .def("set_translation", [](Class* self, const Eigen::Matrix& translation) { - self->translation() = translation; - }) - .def("rotation", [](const Class* self) -> Eigen::Matrix { - return self->linear(); - }) - .def("set_rotation", [](Class* self, const Eigen::Matrix& rotation) { - CheckRotMat(rotation); - self->linear() = rotation; - }) - .def("quaternion", [](const Class* self) { - return Eigen::Quaternion(self->linear()); - }) - .def("set_quaternion", [](Class* self, const Eigen::Quaternion& q) { - CheckQuaternion(q); - self->linear() = q.toRotationMatrix(); - }) - .def("__str__", [](::pybind11::object self) { - return ::pybind11::str(self.attr("matrix")()); - }) - // Do not define operator `__mul__` until we have the Python3 `@` - // operator so that operations are similar to those of arrays. - .def("multiply", [](const Class& self, const Class& other) { - return self * other; - }, ::pybind11::arg("other")) - .def("multiply", [](const Class& self, const Eigen::Matrix& position) { - return self * position; - }, ::pybind11::arg("position")) - .def("inverse", [](const Class* self) { - return self->inverse(); - }) - //======================== - // Begin: added by dartpy - //======================== - .def("translate", [](Class* self, const Eigen::Matrix& other) { - self->translate(other); - }, ::pybind11::arg("other")) - .def("pretranslate", [](Class* self, const Eigen::Matrix& other) { - self->pretranslate(other); - }, ::pybind11::arg("other")) - //======================== - // End: added by dartpy - //======================== - ; + py_class.def(::pybind11::init([]() { return Class::Identity(); })) + .def_static("Identity", []() { return Class::Identity(); }) + .def( + ::pybind11::init([](const Eigen::Matrix& matrix) { + Class out(matrix); + CheckIsometry(out); + return out; + }), + ::pybind11::arg("matrix")) + .def( + ::pybind11::init([](const Eigen::Matrix& rotation, + const Eigen::Matrix& translation) { + CheckRotMat(rotation); + Class out = Class::Identity(); + out.linear() = rotation; + out.translation() = translation; + return out; + }), + ::pybind11::arg("rotation"), + ::pybind11::arg("translation")) + .def( + ::pybind11::init([](const Eigen::Quaternion& q, + const Eigen::Matrix& translation) { + CheckQuaternion(q); + Class out = Class::Identity(); + out.linear() = q.toRotationMatrix(); + out.translation() = translation; + return out; + }), + ::pybind11::arg("quaternion"), + ::pybind11::arg("translation")) + .def( + ::pybind11::init([](const Class& other) { + CheckIsometry(other); + return other; + }), + ::pybind11::arg("other")) + .def( + "matrix", + [](const Class* self) -> Eigen::Matrix { + return self->matrix(); + }) + .def("set_identity", [](Class* self) { self->setIdentity(); }) + .def( + "set_matrix", + [](Class* self, const Eigen::Matrix& matrix) { + Class update(matrix); + CheckIsometry(update); + *self = update; + }) + .def( + "translation", + [](const Class* self) -> Eigen::Matrix { + return self->translation(); + }) + .def( + "set_translation", + [](Class* self, const Eigen::Matrix& translation) { + self->translation() = translation; + }) + .def( + "rotation", + [](const Class* self) -> Eigen::Matrix { + return self->linear(); + }) + .def( + "set_rotation", + [](Class* self, const Eigen::Matrix& rotation) { + CheckRotMat(rotation); + self->linear() = rotation; + }) + .def( + "quaternion", + [](const Class* self) { + return Eigen::Quaternion(self->linear()); + }) + .def( + "set_quaternion", + [](Class* self, const Eigen::Quaternion& q) { + CheckQuaternion(q); + self->linear() = q.toRotationMatrix(); + }) + .def( + "__str__", + [](::pybind11::object self) { + return ::pybind11::str(self.attr("matrix")()); + }) + // Do not define operator `__mul__` until we have the Python3 `@` + // operator so that operations are similar to those of arrays. + .def( + "multiply", + [](const Class& self, const Class& other) { return self * other; }, + ::pybind11::arg("other")) + .def( + "multiply", + [](const Class& self, const Eigen::Matrix& position) { + return self * position; + }, + ::pybind11::arg("position")) + .def("inverse", [](const Class* self) { return self->inverse(); }) + //======================== + // Begin: added by dartpy + //======================== + .def( + "translate", + [](Class* self, const Eigen::Matrix& other) { + self->translate(other); + }, + ::pybind11::arg("other")) + .def( + "pretranslate", + [](Class* self, const Eigen::Matrix& other) { + self->pretranslate(other); + }, + ::pybind11::arg("other")) + //======================== + // End: added by dartpy + //======================== + ; ::pybind11::implicitly_convertible, Class>(); } @@ -234,180 +262,225 @@ void eigen_geometry(pybind11::module& parent_m) { using Class = Eigen::Quaternion; ::pybind11::class_ py_class(m, "Quaternion"); - py_class.attr("__doc__") = - "Provides a unit quaternion binding of Eigen::Quaternion<>."; + py_class.attr("__doc__") + = "Provides a unit quaternion binding of Eigen::Quaternion<>."; ::pybind11::object py_class_obj = py_class; - py_class - .def(::pybind11::init([]() { - return Class::Identity(); - })) - .def_static("Identity", []() { - return Class::Identity(); - }) - .def(::pybind11::init([](const Eigen::Matrix& wxyz) { - Class out(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); - CheckQuaternion(out); - return out; - }), ::pybind11::arg("wxyz")) - .def(::pybind11::init([](T w, T x, T y, T z) { - Class out(w, x, y, z); - CheckQuaternion(out); - return out; - }), ::pybind11::arg("w"), ::pybind11::arg("x"), ::pybind11::arg("y"), ::pybind11::arg("z")) - .def(::pybind11::init([](const Eigen::Matrix& rotation) { - Class out(rotation); - CheckQuaternion(out); - return out; - }), ::pybind11::arg("rotation")) - .def(::pybind11::init([](const Class& other) { - CheckQuaternion(other); - return other; - }), ::pybind11::arg("other")) - .def("w", [](const Class* self) { return self->w(); }) - .def("x", [](const Class* self) { return self->x(); }) - .def("y", [](const Class* self) { return self->y(); }) - .def("z", [](const Class* self) { return self->z(); }) - .def("xyz", [](const Class* self) { return self->vec(); }) - .def("wxyz", [](Class* self) { - Eigen::Matrix wxyz; - wxyz << self->w(), self->vec(); - return wxyz; - }) - .def("set_wxyz", [](Class* self, const Eigen::Matrix& wxyz) { - Class update; - update.w() = wxyz(0); - update.vec() = wxyz.tail(3); - CheckQuaternion(update); - *self = update; - }, ::pybind11::arg("wxyz")) - .def("set_wxyz", [](Class* self, T w, T x, T y, T z) { - Class update(w, x, y, z); - CheckQuaternion(update); - *self = update; - }, ::pybind11::arg("w"), ::pybind11::arg("x"), ::pybind11::arg("y"), ::pybind11::arg("z")) - .def("rotation", [](const Class* self) { - return self->toRotationMatrix(); - }) - .def("set_rotation", [](Class* self, const Eigen::Matrix& rotation) { - Class update(rotation); - CheckQuaternion(update); - *self = update; - }) - .def("__str__", [py_class_obj](const Class* self) { - return ::pybind11::str("{}(w={}, x={}, y={}, z={})").format( - py_class_obj.attr("__name__"), - self->w(), self->x(), self->y(), self->z()); - }) - // Do not define operator `__mul__` until we have the Python3 `@` - // operator so that operations are similar to those of arrays. - .def("multiply", [](const Class& self, const Class& other) { - return self * other; - }) - .def("multiply", [](const Class& self, const Eigen::Matrix& position) { - return self * position; - }, ::pybind11::arg("position")) - .def("inverse", [](const Class* self) { - return self->inverse(); - }) - .def("conjugate", [](const Class* self) { - return self->conjugate(); - }) - //======================== - // Begin: added by dartpy - //======================== - .def("to_rotation_matrix", [](Class* self) -> Eigen::Matrix { - return self->toRotationMatrix(); - }) - //======================== - // End: added by dartpy - //======================== - ; + py_class.def(::pybind11::init([]() { return Class::Identity(); })) + .def_static("Identity", []() { return Class::Identity(); }) + .def( + ::pybind11::init([](const Eigen::Matrix& wxyz) { + Class out(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + CheckQuaternion(out); + return out; + }), + ::pybind11::arg("wxyz")) + .def( + ::pybind11::init([](T w, T x, T y, T z) { + Class out(w, x, y, z); + CheckQuaternion(out); + return out; + }), + ::pybind11::arg("w"), + ::pybind11::arg("x"), + ::pybind11::arg("y"), + ::pybind11::arg("z")) + .def( + ::pybind11::init([](const Eigen::Matrix& rotation) { + Class out(rotation); + CheckQuaternion(out); + return out; + }), + ::pybind11::arg("rotation")) + .def( + ::pybind11::init([](const Class& other) { + CheckQuaternion(other); + return other; + }), + ::pybind11::arg("other")) + .def("w", [](const Class* self) { return self->w(); }) + .def("x", [](const Class* self) { return self->x(); }) + .def("y", [](const Class* self) { return self->y(); }) + .def("z", [](const Class* self) { return self->z(); }) + .def("xyz", [](const Class* self) { return self->vec(); }) + .def( + "wxyz", + [](Class* self) { + Eigen::Matrix wxyz; + wxyz << self->w(), self->vec(); + return wxyz; + }) + .def( + "set_wxyz", + [](Class* self, const Eigen::Matrix& wxyz) { + Class update; + update.w() = wxyz(0); + update.vec() = wxyz.tail(3); + CheckQuaternion(update); + *self = update; + }, + ::pybind11::arg("wxyz")) + .def( + "set_wxyz", + [](Class* self, T w, T x, T y, T z) { + Class update(w, x, y, z); + CheckQuaternion(update); + *self = update; + }, + ::pybind11::arg("w"), + ::pybind11::arg("x"), + ::pybind11::arg("y"), + ::pybind11::arg("z")) + .def( + "rotation", + [](const Class* self) { return self->toRotationMatrix(); }) + .def( + "set_rotation", + [](Class* self, const Eigen::Matrix& rotation) { + Class update(rotation); + CheckQuaternion(update); + *self = update; + }) + .def( + "__str__", + [py_class_obj](const Class* self) { + return ::pybind11::str("{}(w={}, x={}, y={}, z={})") + .format( + py_class_obj.attr("__name__"), + self->w(), + self->x(), + self->y(), + self->z()); + }) + // Do not define operator `__mul__` until we have the Python3 `@` + // operator so that operations are similar to those of arrays. + .def( + "multiply", + [](const Class& self, const Class& other) { return self * other; }) + .def( + "multiply", + [](const Class& self, const Eigen::Matrix& position) { + return self * position; + }, + ::pybind11::arg("position")) + .def("inverse", [](const Class* self) { return self->inverse(); }) + .def("conjugate", [](const Class* self) { return self->conjugate(); }) + //======================== + // Begin: added by dartpy + //======================== + .def( + "to_rotation_matrix", + [](Class* self) -> Eigen::Matrix { + return self->toRotationMatrix(); + }) + //======================== + // End: added by dartpy + //======================== + ; } // Angle-axis. { using Class = Eigen::AngleAxis; ::pybind11::class_ py_class(m, "AngleAxis"); - py_class.attr("__doc__") = - "Bindings for Eigen::AngleAxis<>."; + py_class.attr("__doc__") = "Bindings for Eigen::AngleAxis<>."; ::pybind11::object py_class_obj = py_class; - py_class - .def(::pybind11::init([]() { - return Class::Identity(); - })) - .def_static("Identity", []() { - return Class::Identity(); - }) - .def(::pybind11::init([](const T& angle, const Eigen::Matrix& axis) { - Class out(angle, axis); - CheckAngleAxis(out); - return out; - }), ::pybind11::arg("angle"), ::pybind11::arg("axis")) - .def(::pybind11::init([](const Eigen::Quaternion& q) { - Class out(q); - CheckAngleAxis(out); - return out; - }), ::pybind11::arg("quaternion")) - .def(::pybind11::init([](const Eigen::Matrix& rotation) { - Class out(rotation); - CheckAngleAxis(out); - return out; - }), ::pybind11::arg("rotation")) - .def(::pybind11::init([](const Class& other) { - CheckAngleAxis(other); - return other; - }), ::pybind11::arg("other")) - .def("angle", [](const Class* self) { return self->angle(); }) - .def("axis", [](const Class* self) { return self->axis(); }) - .def("set_angle", [](Class* self, const T& angle) { - // N.B. Since `axis` should already be valid, do not need to check. - self->angle() = angle; - }, ::pybind11::arg("angle")) - .def("set_axis", [](Class* self, const Eigen::Matrix& axis) { - Class update(self->angle(), axis); - CheckAngleAxis(update); - *self = update; - }, ::pybind11::arg("axis")) - .def("rotation", [](const Class* self) { - return self->toRotationMatrix(); - }) - .def("set_rotation", [](Class* self, const Eigen::Matrix& rotation) { - Class update(rotation); - CheckAngleAxis(update); - *self = update; - }) - .def("quaternion", [](const Class* self) { - return Eigen::Quaternion(*self); - }) - .def("set_quaternion", [](Class* self, const Eigen::Quaternion& q) { - CheckQuaternion(q); - Class update(q); - CheckAngleAxis(update); - *self = update; - }) - .def("__str__", [py_class_obj](const Class* self) { - return ::pybind11::str("{}(angle={}, axis={})").format( - py_class_obj.attr("__name__"), - self->angle(), self->axis()); - }) - // Do not define operator `__mul__` until we have the Python3 `@` - // operator so that operations are similar to those of arrays. - .def("multiply", [](const Class& self, const Class& other) { - return self * other; - }) - .def("inverse", [](const Class* self) { - return self->inverse(); - }) - //======================== - // Begin: added by dartpy - //======================== - .def("to_rotation_matrix", [](Class* self) -> Eigen::Matrix { - return self->toRotationMatrix(); - }) - //======================== - // End: added by dartpy - //======================== - ; + py_class.def(::pybind11::init([]() { return Class::Identity(); })) + .def_static("Identity", []() { return Class::Identity(); }) + .def( + ::pybind11::init( + [](const T& angle, const Eigen::Matrix& axis) { + Class out(angle, axis); + CheckAngleAxis(out); + return out; + }), + ::pybind11::arg("angle"), + ::pybind11::arg("axis")) + .def( + ::pybind11::init([](const Eigen::Quaternion& q) { + Class out(q); + CheckAngleAxis(out); + return out; + }), + ::pybind11::arg("quaternion")) + .def( + ::pybind11::init([](const Eigen::Matrix& rotation) { + Class out(rotation); + CheckAngleAxis(out); + return out; + }), + ::pybind11::arg("rotation")) + .def( + ::pybind11::init([](const Class& other) { + CheckAngleAxis(other); + return other; + }), + ::pybind11::arg("other")) + .def("angle", [](const Class* self) { return self->angle(); }) + .def("axis", [](const Class* self) { return self->axis(); }) + .def( + "set_angle", + [](Class* self, const T& angle) { + // N.B. Since `axis` should already be valid, do not need to + // check. + self->angle() = angle; + }, + ::pybind11::arg("angle")) + .def( + "set_axis", + [](Class* self, const Eigen::Matrix& axis) { + Class update(self->angle(), axis); + CheckAngleAxis(update); + *self = update; + }, + ::pybind11::arg("axis")) + .def( + "rotation", + [](const Class* self) { return self->toRotationMatrix(); }) + .def( + "set_rotation", + [](Class* self, const Eigen::Matrix& rotation) { + Class update(rotation); + CheckAngleAxis(update); + *self = update; + }) + .def( + "quaternion", + [](const Class* self) { return Eigen::Quaternion(*self); }) + .def( + "set_quaternion", + [](Class* self, const Eigen::Quaternion& q) { + CheckQuaternion(q); + Class update(q); + CheckAngleAxis(update); + *self = update; + }) + .def( + "__str__", + [py_class_obj](const Class* self) { + return ::pybind11::str("{}(angle={}, axis={})") + .format( + py_class_obj.attr("__name__"), + self->angle(), + self->axis()); + }) + // Do not define operator `__mul__` until we have the Python3 `@` + // operator so that operations are similar to those of arrays. + .def( + "multiply", + [](const Class& self, const Class& other) { return self * other; }) + .def("inverse", [](const Class* self) { return self->inverse(); }) + //======================== + // Begin: added by dartpy + //======================== + .def( + "to_rotation_matrix", + [](Class* self) -> Eigen::Matrix { + return self->toRotationMatrix(); + }) + //======================== + // End: added by dartpy + //======================== + ; } } diff --git a/unittests/gtest/include/gtest/gtest-death-test.h b/unittests/gtest/include/gtest/gtest-death-test.h index 20c54d869519f..7450d1c63f551 100644 --- a/unittests/gtest/include/gtest/gtest-death-test.h +++ b/unittests/gtest/include/gtest/gtest-death-test.h @@ -60,219 +60,221 @@ namespace internal { // implementation of death tests. User code MUST NOT use it. GTEST_API_ bool InDeathTestChild(); -} // namespace internal +} // namespace internal -// The following macros are useful for writing death tests. + // The following macros are useful for writing death tests. -// Here's what happens when an ASSERT_DEATH* or EXPECT_DEATH* is -// executed: -// -// 1. It generates a warning if there is more than one active -// thread. This is because it's safe to fork() or clone() only -// when there is a single thread. -// -// 2. The parent process clone()s a sub-process and runs the death -// test in it; the sub-process exits with code 0 at the end of the -// death test, if it hasn't exited already. -// -// 3. The parent process waits for the sub-process to terminate. -// -// 4. The parent process checks the exit code and error message of -// the sub-process. -// -// Examples: -// -// ASSERT_DEATH(server.SendMessage(56, "Hello"), "Invalid port number"); -// for (int i = 0; i < 5; i++) { -// EXPECT_DEATH(server.ProcessRequest(i), -// "Invalid request .* in ProcessRequest()") -// << "Failed to die on request " << i; -// } -// -// ASSERT_EXIT(server.ExitNow(), ::testing::ExitedWithCode(0), "Exiting"); -// -// bool KilledBySIGHUP(int exit_code) { -// return WIFSIGNALED(exit_code) && WTERMSIG(exit_code) == SIGHUP; -// } -// -// ASSERT_EXIT(client.HangUpServer(), KilledBySIGHUP, "Hanging up!"); -// -// On the regular expressions used in death tests: -// -// GOOGLETEST_CM0005 DO NOT DELETE -// On POSIX-compliant systems (*nix), we use the library, -// which uses the POSIX extended regex syntax. -// -// On other platforms (e.g. Windows or Mac), we only support a simple regex -// syntax implemented as part of Google Test. This limited -// implementation should be enough most of the time when writing -// death tests; though it lacks many features you can find in PCRE -// or POSIX extended regex syntax. For example, we don't support -// union ("x|y"), grouping ("(xy)"), brackets ("[xy]"), and -// repetition count ("x{5,7}"), among others. -// -// Below is the syntax that we do support. We chose it to be a -// subset of both PCRE and POSIX extended regex, so it's easy to -// learn wherever you come from. In the following: 'A' denotes a -// literal character, period (.), or a single \\ escape sequence; -// 'x' and 'y' denote regular expressions; 'm' and 'n' are for -// natural numbers. -// -// c matches any literal character c -// \\d matches any decimal digit -// \\D matches any character that's not a decimal digit -// \\f matches \f -// \\n matches \n -// \\r matches \r -// \\s matches any ASCII whitespace, including \n -// \\S matches any character that's not a whitespace -// \\t matches \t -// \\v matches \v -// \\w matches any letter, _, or decimal digit -// \\W matches any character that \\w doesn't match -// \\c matches any literal character c, which must be a punctuation -// . matches any single character except \n -// A? matches 0 or 1 occurrences of A -// A* matches 0 or many occurrences of A -// A+ matches 1 or many occurrences of A -// ^ matches the beginning of a string (not that of each line) -// $ matches the end of a string (not that of each line) -// xy matches x followed by y -// -// If you accidentally use PCRE or POSIX extended regex features -// not implemented by us, you will get a run-time failure. In that -// case, please try to rewrite your regular expression within the -// above syntax. -// -// This implementation is *not* meant to be as highly tuned or robust -// as a compiled regex library, but should perform well enough for a -// death test, which already incurs significant overhead by launching -// a child process. -// -// Known caveats: -// -// A "threadsafe" style death test obtains the path to the test -// program from argv[0] and re-executes it in the sub-process. For -// simplicity, the current implementation doesn't search the PATH -// when launching the sub-process. This means that the user must -// invoke the test program via a path that contains at least one -// path separator (e.g. path/to/foo_test and -// /absolute/path/to/bar_test are fine, but foo_test is not). This -// is rarely a problem as people usually don't put the test binary -// directory in PATH. -// -// FIXME: make thread-safe death tests search the PATH. + // Here's what happens when an ASSERT_DEATH* or EXPECT_DEATH* is + // executed: + // + // 1. It generates a warning if there is more than one active + // thread. This is because it's safe to fork() or clone() only + // when there is a single thread. + // + // 2. The parent process clone()s a sub-process and runs the death + // test in it; the sub-process exits with code 0 at the end of the + // death test, if it hasn't exited already. + // + // 3. The parent process waits for the sub-process to terminate. + // + // 4. The parent process checks the exit code and error message of + // the sub-process. + // + // Examples: + // + // ASSERT_DEATH(server.SendMessage(56, "Hello"), "Invalid port number"); + // for (int i = 0; i < 5; i++) { + // EXPECT_DEATH(server.ProcessRequest(i), + // "Invalid request .* in ProcessRequest()") + // << "Failed to die on request " << i; + // } + // + // ASSERT_EXIT(server.ExitNow(), ::testing::ExitedWithCode(0), "Exiting"); + // + // bool KilledBySIGHUP(int exit_code) { + // return WIFSIGNALED(exit_code) && WTERMSIG(exit_code) == SIGHUP; + // } + // + // ASSERT_EXIT(client.HangUpServer(), KilledBySIGHUP, "Hanging up!"); + // + // On the regular expressions used in death tests: + // + // GOOGLETEST_CM0005 DO NOT DELETE + // On POSIX-compliant systems (*nix), we use the library, + // which uses the POSIX extended regex syntax. + // + // On other platforms (e.g. Windows or Mac), we only support a simple regex + // syntax implemented as part of Google Test. This limited + // implementation should be enough most of the time when writing + // death tests; though it lacks many features you can find in PCRE + // or POSIX extended regex syntax. For example, we don't support + // union ("x|y"), grouping ("(xy)"), brackets ("[xy]"), and + // repetition count ("x{5,7}"), among others. + // + // Below is the syntax that we do support. We chose it to be a + // subset of both PCRE and POSIX extended regex, so it's easy to + // learn wherever you come from. In the following: 'A' denotes a + // literal character, period (.), or a single \\ escape sequence; + // 'x' and 'y' denote regular expressions; 'm' and 'n' are for + // natural numbers. + // + // c matches any literal character c + // \\d matches any decimal digit + // \\D matches any character that's not a decimal digit + // \\f matches \f + // \\n matches \n + // \\r matches \r + // \\s matches any ASCII whitespace, including \n + // \\S matches any character that's not a whitespace + // \\t matches \t + // \\v matches \v + // \\w matches any letter, _, or decimal digit + // \\W matches any character that \\w doesn't match + // \\c matches any literal character c, which must be a punctuation + // . matches any single character except \n + // A? matches 0 or 1 occurrences of A + // A* matches 0 or many occurrences of A + // A+ matches 1 or many occurrences of A + // ^ matches the beginning of a string (not that of each line) + // $ matches the end of a string (not that of each line) + // xy matches x followed by y + // + // If you accidentally use PCRE or POSIX extended regex features + // not implemented by us, you will get a run-time failure. In that + // case, please try to rewrite your regular expression within the + // above syntax. + // + // This implementation is *not* meant to be as highly tuned or robust + // as a compiled regex library, but should perform well enough for a + // death test, which already incurs significant overhead by launching + // a child process. + // + // Known caveats: + // + // A "threadsafe" style death test obtains the path to the test + // program from argv[0] and re-executes it in the sub-process. For + // simplicity, the current implementation doesn't search the PATH + // when launching the sub-process. This means that the user must + // invoke the test program via a path that contains at least one + // path separator (e.g. path/to/foo_test and + // /absolute/path/to/bar_test are fine, but foo_test is not). This + // is rarely a problem as people usually don't put the test binary + // directory in PATH. + // + // FIXME: make thread-safe death tests search the PATH. -// Asserts that a given statement causes the program to exit, with an -// integer exit status that satisfies predicate, and emitting error output -// that matches regex. -# define ASSERT_EXIT(statement, predicate, regex) \ + // Asserts that a given statement causes the program to exit, with an + // integer exit status that satisfies predicate, and emitting error output + // that matches regex. + #define ASSERT_EXIT(statement, predicate, regex) \ GTEST_DEATH_TEST_(statement, predicate, regex, GTEST_FATAL_FAILURE_) -// Like ASSERT_EXIT, but continues on to successive tests in the -// test case, if any: -# define EXPECT_EXIT(statement, predicate, regex) \ + // Like ASSERT_EXIT, but continues on to successive tests in the + // test case, if any: + #define EXPECT_EXIT(statement, predicate, regex) \ GTEST_DEATH_TEST_(statement, predicate, regex, GTEST_NONFATAL_FAILURE_) -// Asserts that a given statement causes the program to exit, either by -// explicitly exiting with a nonzero exit code or being killed by a -// signal, and emitting error output that matches regex. -# define ASSERT_DEATH(statement, regex) \ + // Asserts that a given statement causes the program to exit, either by + // explicitly exiting with a nonzero exit code or being killed by a + // signal, and emitting error output that matches regex. + #define ASSERT_DEATH(statement, regex) \ ASSERT_EXIT(statement, ::testing::internal::ExitedUnsuccessfully, regex) -// Like ASSERT_DEATH, but continues on to successive tests in the -// test case, if any: -# define EXPECT_DEATH(statement, regex) \ + // Like ASSERT_DEATH, but continues on to successive tests in the + // test case, if any: + #define EXPECT_DEATH(statement, regex) \ EXPECT_EXIT(statement, ::testing::internal::ExitedUnsuccessfully, regex) // Two predicate classes that can be used in {ASSERT,EXPECT}_EXIT*: // Tests that an exit code describes a normal exit with a given exit code. -class GTEST_API_ ExitedWithCode { - public: +class GTEST_API_ ExitedWithCode +{ +public: explicit ExitedWithCode(int exit_code); bool operator()(int exit_status) const; - private: + +private: // No implementation - assignment is unsupported. void operator=(const ExitedWithCode& other); const int exit_code_; }; -# if !GTEST_OS_WINDOWS && !GTEST_OS_FUCHSIA + #if !GTEST_OS_WINDOWS && !GTEST_OS_FUCHSIA // Tests that an exit code describes an exit due to termination by a // given signal. // GOOGLETEST_CM0006 DO NOT DELETE -class GTEST_API_ KilledBySignal { - public: +class GTEST_API_ KilledBySignal +{ +public: explicit KilledBySignal(int signum); bool operator()(int exit_status) const; - private: + +private: const int signum_; }; -# endif // !GTEST_OS_WINDOWS + #endif // !GTEST_OS_WINDOWS -// EXPECT_DEBUG_DEATH asserts that the given statements die in debug mode. -// The death testing framework causes this to have interesting semantics, -// since the sideeffects of the call are only visible in opt mode, and not -// in debug mode. -// -// In practice, this can be used to test functions that utilize the -// LOG(DFATAL) macro using the following style: -// -// int DieInDebugOr12(int* sideeffect) { -// if (sideeffect) { -// *sideeffect = 12; -// } -// LOG(DFATAL) << "death"; -// return 12; -// } -// -// TEST(TestCase, TestDieOr12WorksInDgbAndOpt) { -// int sideeffect = 0; -// // Only asserts in dbg. -// EXPECT_DEBUG_DEATH(DieInDebugOr12(&sideeffect), "death"); -// -// #ifdef NDEBUG -// // opt-mode has sideeffect visible. -// EXPECT_EQ(12, sideeffect); -// #else -// // dbg-mode no visible sideeffect. -// EXPECT_EQ(0, sideeffect); -// #endif -// } -// -// This will assert that DieInDebugReturn12InOpt() crashes in debug -// mode, usually due to a DCHECK or LOG(DFATAL), but returns the -// appropriate fallback value (12 in this case) in opt mode. If you -// need to test that a function has appropriate side-effects in opt -// mode, include assertions against the side-effects. A general -// pattern for this is: -// -// EXPECT_DEBUG_DEATH({ -// // Side-effects here will have an effect after this statement in -// // opt mode, but none in debug mode. -// EXPECT_EQ(12, DieInDebugOr12(&sideeffect)); -// }, "death"); -// -# ifdef NDEBUG + // EXPECT_DEBUG_DEATH asserts that the given statements die in debug mode. + // The death testing framework causes this to have interesting semantics, + // since the sideeffects of the call are only visible in opt mode, and not + // in debug mode. + // + // In practice, this can be used to test functions that utilize the + // LOG(DFATAL) macro using the following style: + // + // int DieInDebugOr12(int* sideeffect) { + // if (sideeffect) { + // *sideeffect = 12; + // } + // LOG(DFATAL) << "death"; + // return 12; + // } + // + // TEST(TestCase, TestDieOr12WorksInDgbAndOpt) { + // int sideeffect = 0; + // // Only asserts in dbg. + // EXPECT_DEBUG_DEATH(DieInDebugOr12(&sideeffect), "death"); + // + // #if DART_BUILD_MODE_RELEASE + // // opt-mode has sideeffect visible. + // EXPECT_EQ(12, sideeffect); + // #else + // // dbg-mode no visible sideeffect. + // EXPECT_EQ(0, sideeffect); + // #endif + // } + // + // This will assert that DieInDebugReturn12InOpt() crashes in debug + // mode, usually due to a DCHECK or LOG(DFATAL), but returns the + // appropriate fallback value (12 in this case) in opt mode. If you + // need to test that a function has appropriate side-effects in opt + // mode, include assertions against the side-effects. A general + // pattern for this is: + // + // EXPECT_DEBUG_DEATH({ + // // Side-effects here will have an effect after this statement in + // // opt mode, but none in debug mode. + // EXPECT_EQ(12, DieInDebugOr12(&sideeffect)); + // }, "death"); + // + #ifdef NDEBUG -# define EXPECT_DEBUG_DEATH(statement, regex) \ - GTEST_EXECUTE_STATEMENT_(statement, regex) + #define EXPECT_DEBUG_DEATH(statement, regex) \ + GTEST_EXECUTE_STATEMENT_(statement, regex) -# define ASSERT_DEBUG_DEATH(statement, regex) \ - GTEST_EXECUTE_STATEMENT_(statement, regex) + #define ASSERT_DEBUG_DEATH(statement, regex) \ + GTEST_EXECUTE_STATEMENT_(statement, regex) -# else + #else -# define EXPECT_DEBUG_DEATH(statement, regex) \ - EXPECT_DEATH(statement, regex) + #define EXPECT_DEBUG_DEATH(statement, regex) EXPECT_DEATH(statement, regex) -# define ASSERT_DEBUG_DEATH(statement, regex) \ - ASSERT_DEATH(statement, regex) + #define ASSERT_DEBUG_DEATH(statement, regex) ASSERT_DEATH(statement, regex) -# endif // NDEBUG for EXPECT_DEBUG_DEATH -#endif // GTEST_HAS_DEATH_TEST + #endif // NDEBUG for EXPECT_DEBUG_DEATH +#endif // GTEST_HAS_DEATH_TEST // This macro is used for implementing macros such as // EXPECT_DEATH_IF_SUPPORTED and ASSERT_DEATH_IF_SUPPORTED on systems where @@ -309,18 +311,21 @@ class GTEST_API_ KilledBySignal { // statement unconditionally returns or throws. The Message constructor at // the end allows the syntax of streaming additional messages into the // macro, for compilational compatibility with EXPECT_DEATH/ASSERT_DEATH. -# define GTEST_UNSUPPORTED_DEATH_TEST(statement, regex, terminator) \ - GTEST_AMBIGUOUS_ELSE_BLOCKER_ \ - if (::testing::internal::AlwaysTrue()) { \ - GTEST_LOG_(WARNING) \ - << "Death tests are not supported on this platform.\n" \ - << "Statement '" #statement "' cannot be verified."; \ - } else if (::testing::internal::AlwaysFalse()) { \ - ::testing::internal::RE::PartialMatch(".*", (regex)); \ - GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \ - terminator; \ - } else \ - ::testing::Message() +#define GTEST_UNSUPPORTED_DEATH_TEST(statement, regex, terminator) \ + GTEST_AMBIGUOUS_ELSE_BLOCKER_ \ + if (::testing::internal::AlwaysTrue()) \ + { \ + GTEST_LOG_(WARNING) << "Death tests are not supported on this platform.\n" \ + << "Statement '" #statement "' cannot be verified."; \ + } \ + else if (::testing::internal::AlwaysFalse()) \ + { \ + ::testing::internal::RE::PartialMatch(".*", (regex)); \ + GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \ + terminator; \ + } \ + else \ + ::testing::Message() // EXPECT_DEATH_IF_SUPPORTED(statement, regex) and // ASSERT_DEATH_IF_SUPPORTED(statement, regex) expand to real death tests if @@ -328,17 +333,17 @@ class GTEST_API_ KilledBySignal { // useful when you are combining death test assertions with normal test // assertions in one test. #if GTEST_HAS_DEATH_TEST -# define EXPECT_DEATH_IF_SUPPORTED(statement, regex) \ + #define EXPECT_DEATH_IF_SUPPORTED(statement, regex) \ EXPECT_DEATH(statement, regex) -# define ASSERT_DEATH_IF_SUPPORTED(statement, regex) \ + #define ASSERT_DEATH_IF_SUPPORTED(statement, regex) \ ASSERT_DEATH(statement, regex) #else -# define EXPECT_DEATH_IF_SUPPORTED(statement, regex) \ + #define EXPECT_DEATH_IF_SUPPORTED(statement, regex) \ GTEST_UNSUPPORTED_DEATH_TEST(statement, regex, ) -# define ASSERT_DEATH_IF_SUPPORTED(statement, regex) \ + #define ASSERT_DEATH_IF_SUPPORTED(statement, regex) \ GTEST_UNSUPPORTED_DEATH_TEST(statement, regex, return) #endif -} // namespace testing +} // namespace testing -#endif // GTEST_INCLUDE_GTEST_GTEST_DEATH_TEST_H_ +#endif // GTEST_INCLUDE_GTEST_GTEST_DEATH_TEST_H_ diff --git a/unittests/integration/test_Constraint.cpp b/unittests/integration/test_Constraint.cpp index 39e5b424b7262..4e8b0ed29cc5c 100644 --- a/unittests/integration/test_Constraint.cpp +++ b/unittests/integration/test_Constraint.cpp @@ -113,7 +113,7 @@ void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/) // Settings //---------------------------------------------------------------------------- // Number of random state tests for each skeletons -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG // std::size_t testCount = 1; #else // std::size_t testCount = 1; @@ -226,9 +226,9 @@ TEST_F(ConstraintTest, SingleContactTest) { // for (int i = 0; i < getList().size(); ++i) // { - //#ifndef NDEBUG + // #if DART_BUILD_MODE_DEBUG // dtdbg << getList()[i] << std::endl; - //#endif + // #endif // SingleContactTest(getList()[i]); // } diff --git a/unittests/integration/test_Dynamics.cpp b/unittests/integration/test_Dynamics.cpp index 942633ba3d9b9..d160b72b65ac4 100644 --- a/unittests/integration/test_Dynamics.cpp +++ b/unittests/integration/test_Dynamics.cpp @@ -876,7 +876,7 @@ void DynamicsTest::testJacobians(const common::Uri& uri) //----------------------------- Settings ------------------------------------- const double TOLERANCE = 1.0e-6; -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG int nTestItr = 2; #else int nTestItr = 5; @@ -979,7 +979,7 @@ void DynamicsTest::testJacobians(const common::Uri& uri) compareBodyNodeFkToJacobianRelative(bn, bn, Frame::World(), TOLERANCE); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG if (skeleton->getNumBodyNodes() == 0u) continue; @@ -1056,7 +1056,7 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates( using namespace utils; //----------------------------- Settings ------------------------------------- -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG int nRandomItr = 2; #else int nRandomItr = 10; @@ -1146,7 +1146,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeVelocity(const common::Uri& uri) using namespace utils; //----------------------------- Settings ------------------------------------- -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG int nRandomItr = 2; std::size_t numSteps = 1e+1; #else @@ -1242,7 +1242,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( //----------------------------- Settings ------------------------------------- const double TOLERANCE = 1.0e-2; -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG int nRandomItr = 2; #else int nRandomItr = 10; @@ -1385,7 +1385,7 @@ void DynamicsTest::testFiniteDifferenceBodyNodeAcceleration( //============================================================================== void testForwardKinematicsSkeleton(const dynamics::SkeletonPtr& skel) { -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t nRandomItr = 1e+1; std::size_t numSteps = 1e+1; #else @@ -1525,7 +1525,7 @@ void DynamicsTest::testInverseDynamics(const common::Uri& uri) { //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeleton -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG const std::size_t nRandomItr = 2; #else const std::size_t nRandomItr = 100; @@ -1611,7 +1611,7 @@ void DynamicsTest::compareEquationsOfMotion(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 100; @@ -1903,7 +1903,7 @@ void DynamicsTest::testCenterOfMass(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 100; @@ -2067,7 +2067,7 @@ void DynamicsTest::testCenterOfMassFreeFall(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t nRandomItr = 2; #else std::size_t nRandomItr = 10; @@ -2106,7 +2106,7 @@ void DynamicsTest::testCenterOfMassFreeFall(const common::Uri& uri) if (nullptr == rootFreeJoint || !skel->isMobile() || 0 == dof) { -#if BUILD_TYPE_DEBUG +#if DART_BUILD_MODE_DEBUG dtmsg << "Skipping COM free fall test for Skeleton [" << skel->getName() << "] since the Skeleton doesn't have FreeJoint at the root body " << " or immobile." << endl; @@ -2182,7 +2182,7 @@ void DynamicsTest::testConstraintImpulse(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t nRandomItr = 1; #else std::size_t nRandomItr = 1; @@ -2275,7 +2275,7 @@ void DynamicsTest::testImpulseBasedDynamics(const common::Uri& uri) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t nRandomItr = 1; #else std::size_t nRandomItr = 100; @@ -2365,7 +2365,7 @@ TEST_F(DynamicsTest, testJacobians) { for (std::size_t i = 0; i < getList().size(); ++i) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testJacobians(getList()[i]); @@ -2377,7 +2377,7 @@ TEST_F(DynamicsTest, testFiniteDifference) { for (std::size_t i = 0; i < getList().size(); ++i) { -#if BUILD_TYPE_DEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testFiniteDifferenceGeneralizedCoordinates(getList()[i]); @@ -2391,7 +2391,7 @@ TEST_F(DynamicsTest, testForwardKinematics) { for (std::size_t i = 0; i < getList().size(); ++i) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testForwardKinematics(getList()[i]); @@ -2403,7 +2403,7 @@ TEST_F(DynamicsTest, testInverseDynamics) { for (std::size_t i = 0; i < getList().size(); ++i) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testInverseDynamics(getList()[i]); @@ -2430,7 +2430,7 @@ TEST_F(DynamicsTest, compareEquationsOfMotion) } //////////////////////////////////////////////////////////////////////////// -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif compareEquationsOfMotion(getList()[i]); @@ -2442,7 +2442,7 @@ TEST_F(DynamicsTest, testCenterOfMass) { for (std::size_t i = 0; i < getList().size(); ++i) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testCenterOfMass(getList()[i]); @@ -2454,7 +2454,7 @@ TEST_F(DynamicsTest, testCenterOfMassFreeFall) { for (std::size_t i = 0; i < getList().size(); ++i) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testCenterOfMassFreeFall(getList()[i]); @@ -2466,7 +2466,7 @@ TEST_F(DynamicsTest, testConstraintImpulse) { for (std::size_t i = 0; i < getList().size(); ++i) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testConstraintImpulse(getList()[i]); @@ -2478,7 +2478,7 @@ TEST_F(DynamicsTest, testImpulseBasedDynamics) { for (std::size_t i = 0; i < getList().size(); ++i) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG dtdbg << getList()[i].toString() << std::endl; #endif testImpulseBasedDynamics(getList()[i]); @@ -2490,7 +2490,7 @@ TEST_F(DynamicsTest, HybridDynamics) { const double tol = 1e-8; const double timeStep = 1e-3; -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG // Debug mode const std::size_t numFrames = 50; // 0.05 secs #else const std::size_t numFrames = 5e+3; // 5 secs diff --git a/unittests/integration/test_IkFast.cpp b/unittests/integration/test_IkFast.cpp index 943d760432d64..852f3f3a09ed5 100644 --- a/unittests/integration/test_IkFast.cpp +++ b/unittests/integration/test_IkFast.cpp @@ -132,7 +132,7 @@ TEST(IkFast, LoadWamArmIk) ik->setHierarchyLevel(1); std::stringstream ss; ss << DART_SHARED_LIB_PREFIX << "GeneratedWamIkFast"; -#if (DART_OS_LINUX || DART_OS_MACOS) && !NDEBUG +#if (DART_OS_LINUX || DART_OS_MACOS) && DART_BUILD_MODE_DEBUG ss << "d"; #endif ss << "." << DART_SHARED_LIB_EXTENSION; diff --git a/unittests/integration/test_Joints.cpp b/unittests/integration/test_Joints.cpp index ac0ae1b81ec8b..b78c16925575d 100644 --- a/unittests/integration/test_Joints.cpp +++ b/unittests/integration/test_Joints.cpp @@ -466,7 +466,7 @@ TEST_F(JOINTS, POSITION_LIMIT) joint1->setPositionLowerLimit(0, -limit1); joint1->setPositionUpperLimit(0, limit1); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -548,7 +548,7 @@ TEST_F(JOINTS, POSITION_AND_VELOCITY_LIMIT) joint1->setVelocityLowerLimit(0, -velLimit1); joint1->setVelocityUpperLimit(0, velLimit1); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -709,7 +709,7 @@ void testJointCoulombFrictionForce(double _timeStep) EXPECT_EQ(joint0->getCoulombFriction(0), frictionForce); EXPECT_EQ(joint1->getCoulombFriction(0), frictionForce); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -974,7 +974,7 @@ void testServoMotor() for (auto pendulum : pendulums) world->addSkeleton(pendulum); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -1100,7 +1100,7 @@ void testMimicJoint() world->addSkeleton(pendulum); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG double simTime = 0.2; #else double simTime = 2.0; @@ -1189,7 +1189,7 @@ TEST_F(JOINTS, JOINT_COULOMB_FRICTION_AND_POSITION_LIMIT) EXPECT_EQ(joint0->getCoulombFriction(0), frictionForce); EXPECT_EQ(joint1->getCoulombFriction(0), frictionForce); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG double simTime = 0.2; #else double simTime = 2.0; diff --git a/unittests/integration/test_MetaSkeleton.cpp b/unittests/integration/test_MetaSkeleton.cpp index 63279587b80ed..e232f5671f398 100644 --- a/unittests/integration/test_MetaSkeleton.cpp +++ b/unittests/integration/test_MetaSkeleton.cpp @@ -107,7 +107,7 @@ TEST(MetaSkeleton, Referential) { std::vector skeletons = getSkeletons(); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t numIterations = 1; #else // Release mode std::size_t numIterations = 20; diff --git a/unittests/integration/test_MultiObjectiveOptimization.cpp b/unittests/integration/test_MultiObjectiveOptimization.cpp index 22225cef4b6bc..53a74c4abcfe2 100644 --- a/unittests/integration/test_MultiObjectiveOptimization.cpp +++ b/unittests/integration/test_MultiObjectiveOptimization.cpp @@ -1,4 +1,5 @@ #include + #include #include #include @@ -7,7 +8,7 @@ #include #include #if HAVE_PAGMO -# include + #include #endif using namespace dart; @@ -97,12 +98,12 @@ class Func2 : public Function //============================================================================== void testZDT1(MultiObjectiveSolver& solver) { -#ifdef NDEBUG // release mode +#if DART_BUILD_MODE_RELEASE std::size_t numSolutions = 50; #else std::size_t numSolutions = 10; #endif -#ifdef NDEBUG // release mode +#if DART_BUILD_MODE_RELEASE std::size_t iterationNum = 1000; #else std::size_t iterationNum = 200; @@ -133,12 +134,12 @@ void testZDT1Generic(MultiObjectiveSolver& solver) pFuncs.push_back(pFunc1); pFuncs.push_back(pFunc2); -#ifdef NDEBUG // release mode +#if DART_BUILD_MODE_RELEASE std::size_t numSolutions = 50; #else std::size_t numSolutions = 10; #endif -#ifdef NDEBUG // release mode +#if DART_BUILD_MODE_RELEASE std::size_t iterationNum = 1000; #else std::size_t iterationNum = 200; diff --git a/unittests/integration/test_Skeleton.cpp b/unittests/integration/test_Skeleton.cpp index 858ac75663c17..11dff7685fa8a 100644 --- a/unittests/integration/test_Skeleton.cpp +++ b/unittests/integration/test_Skeleton.cpp @@ -111,7 +111,7 @@ TEST(Skeleton, Restructuring) { std::vector skeletons = getSkeletons(); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG std::size_t numIterations = 10; #else std::size_t numIterations = 2 * skeletons.size(); @@ -653,7 +653,7 @@ TEST(Skeleton, NodePersistence) // The Node has been removed, so no reference to it will exist in the // Skeleton -#ifdef NDEBUG // Release Mode +#if DART_BUILD_MODE_RELEASE EXPECT_NE(skel->getEndEffector("manip"), manip); EXPECT_EQ(skel->getEndEffector("manip"), nullptr); @@ -661,8 +661,8 @@ TEST(Skeleton, NodePersistence) EXPECT_EQ(skel->getEndEffector(0), nullptr); #endif // Release Mode -#ifdef NDEBUG // Release Mode - // But it will not remain in the BodyNode's indexing. +#if DART_BUILD_MODE_RELEASE + // But it will not remain in the BodyNode's indexing. // Note: We should only run this test in release mode, because otherwise it // will trigger an assertion. // EXPECT_NE(skel->getBodyNode(0)->getEndEffector(0), manip); @@ -725,7 +725,7 @@ TEST(Skeleton, NodePersistence) // The Node has been removed, so no reference to it will exist in the // Skeleton -#ifdef NDEBUG // Release Mode +#if DART_BUILD_MODE_RELEASE EXPECT_NE(skel->getNode("node"), node); EXPECT_EQ(skel->getNode("node"), nullptr); @@ -733,7 +733,7 @@ TEST(Skeleton, NodePersistence) EXPECT_EQ(skel->getNode(0), nullptr); #endif // Release Mode -#ifdef NDEBUG // Release Mode +#if DART_BUILD_MODE_RELEASE // But it will not remain in the BodyNode's indexing. // Note: We should only run this test in release mode, because otherwise it // will trigger an assertion. diff --git a/unittests/integration/test_SoftDynamics.cpp b/unittests/integration/test_SoftDynamics.cpp index 29b7cac4862a5..05f698751bd8c 100644 --- a/unittests/integration/test_SoftDynamics.cpp +++ b/unittests/integration/test_SoftDynamics.cpp @@ -243,7 +243,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t nRandomItr = 1; #else std::size_t nRandomItr = 1; @@ -453,9 +453,9 @@ TEST_F(SoftDynamicsTest, compareEquationsOfMotion) // for (int i = 0; i < getList().size(); ++i) // { - //#ifndef NDEBUG + // #if DART_BUILD_MODE_DEBUG // dtdbg << getList()[i] << std::endl; - //#endif + // #endif // compareEquationsOfMotion(getList()[i]); // } } diff --git a/unittests/integration/test_World.cpp b/unittests/integration/test_World.cpp index 3d79cfe5b279e..cff77edae8e8a 100644 --- a/unittests/integration/test_World.cpp +++ b/unittests/integration/test_World.cpp @@ -225,7 +225,7 @@ TEST(World, Cloning) for (std::size_t j = 1; j < 5; ++j) clones.push_back(clones[j - 1]->clone()); -#ifndef NDEBUG // Debug mode +#if DART_BUILD_MODE_DEBUG std::size_t numIterations = 3; #else std::size_t numIterations = 500; diff --git a/unittests/regression/test_Issue1184.cpp b/unittests/regression/test_Issue1184.cpp index 199a269949b6c..5eac9cf30eecf 100644 --- a/unittests/regression/test_Issue1184.cpp +++ b/unittests/regression/test_Issue1184.cpp @@ -76,7 +76,7 @@ TEST(Issue1184, Accuracy) return std::make_shared(s); }; -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG const auto groundInfoFunctions = {makePlaneGround}; const auto objectShapeFunctions = {makeSphereObject}; const auto halfsizes = {10.0}; diff --git a/unittests/regression/test_Issue1596.cpp b/unittests/regression/test_Issue1596.cpp index b0c24ca55cab6..076379b6302fd 100644 --- a/unittests/regression/test_Issue1596.cpp +++ b/unittests/regression/test_Issue1596.cpp @@ -38,7 +38,7 @@ //======================================================================================== TEST(Issue1596, ServoJointWithPositionLimits) { -#if NDEBUG // release +#if DART_BUILD_MODE_RELEASE const auto num_steps = 50000; #else const auto num_steps = 1000; diff --git a/unittests/unit/common/test_MemoryManager.cpp b/unittests/unit/common/test_MemoryManager.cpp index 76e116100d843..d623f1299bbb3 100644 --- a/unittests/unit/common/test_MemoryManager.cpp +++ b/unittests/unit/common/test_MemoryManager.cpp @@ -62,7 +62,7 @@ TEST(MemoryManagerTest, Allocate) // Allocate 1 byte using FreeListAllocator auto ptr1 = mm.allocateUsingFree(1); EXPECT_NE(ptr1, nullptr); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG EXPECT_TRUE(mm.hasAllocated(ptr1, 1)); EXPECT_FALSE(mm.hasAllocated(nullptr, 1)); EXPECT_FALSE(mm.hasAllocated(ptr1, 1 * 2)); @@ -71,7 +71,7 @@ TEST(MemoryManagerTest, Allocate) // Allocate 1 byte using PoolAllocator auto ptr2 = mm.allocateUsingPool(1); EXPECT_NE(ptr2, nullptr); -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG EXPECT_TRUE(mm.hasAllocated(ptr2, 1)); EXPECT_FALSE(mm.hasAllocated(nullptr, 1)); EXPECT_FALSE(mm.hasAllocated(ptr2, 1 * 2)); diff --git a/unittests/unit/math/test_Math.cpp b/unittests/unit/math/test_Math.cpp index 3306b0a11088a..a0291b96159d2 100644 --- a/unittests/unit/math/test_Math.cpp +++ b/unittests/unit/math/test_Math.cpp @@ -1354,7 +1354,7 @@ typename Derived::PlainObject AdTJac3( //============================================================================== TEST(MATH, PerformanceComparisonOfAdTJac) { -#ifndef NDEBUG +#if DART_BUILD_MODE_DEBUG int testCount = 1e+2; #else int testCount = 1e+6;