diff options
Diffstat (limited to 'thirdparty')
142 files changed, 23551 insertions, 11789 deletions
diff --git a/thirdparty/README.md b/thirdparty/README.md index d011832210..73a62458c3 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -40,11 +40,9 @@ Files extracted from upstream source: ## bullet - Upstream: https://github.com/bulletphysics/bullet3 -- Version: git pre-2.90 (cd8cf7521cbb8b7808126a6adebd47bb83ea166a, 2020) +- Version: 3.07 (e32fc59c88a3908876949c6f2665e8d091d987fa, 2020) - License: zlib -Important: Synced with a pre-release version of bullet 2.90 from the master branch. - Files extracted from upstream source: - src/* apart from CMakeLists.txt and premake4.lua files @@ -341,7 +339,7 @@ changes are marked with `// -- GODOT --` comments. ## mbedtls - Upstream: https://tls.mbed.org/ -- Version: 2.16.8 (2020) +- Version: 2.16.9 (2020) - License: Apache 2.0 File extracted from upstream release tarball: @@ -358,6 +356,21 @@ File extracted from upstream release tarball: for light bundling with core. +## meshoptimizer + +- Upstream: https://github.com/zeux/meshoptimizer +- Version: 0.15 (2020) +- License: MIT + +File extracted from upstream release tarball: + +- All files in `src/`. + +Important: Some files have Godot-made changes. +They can be applied with the patch in the `patches` folder, but are meant to be superseded +by upstream API changes. + + ## miniupnpc - Upstream: https://github.com/miniupnp/miniupnp/tree/master/miniupnpc @@ -413,7 +426,7 @@ Collection of single-file libraries used in Godot components. * License: Apache 2.0 - `open-simplex-noise.{c,h}` * Upstream: https://github.com/smcameron/open-simplex-noise-in-c - * Version: git (0fef0dbedd76f767da7e3c894822729d0f07e54d, 2020) + custom changes + * Version: git (826f1dd1724e6fb3ff45f58e48c0fbae864c3403, 2020) + custom changes * License: Unlicense - `pcg.{cpp,h}` * Upstream: http://www.pcg-random.org @@ -671,7 +684,7 @@ File extracted from upstream release tarball: ## xatlas - Upstream: https://github.com/jpcy/xatlas -- Version: git (470576d3516f7e6d8b4554e7c941194a935969fd, 2020) +- Version: git (5571fc7ef0d06832947c0a935ccdcf083f7a9264, 2020) - License: MIT Files extracted from upstream source: diff --git a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp index 6f2c5251a0..4938fa17af 100644 --- a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp @@ -285,7 +285,6 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart); curNodeSubPart = nodeSubPart; - b3Assert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, @@ -293,7 +292,13 @@ void b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface, int for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: b3Assert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp b/thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp index 145de62db3..f6c779a919 100644 --- a/thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp +++ b/thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp @@ -851,12 +851,12 @@ void bFile::swapData(char *data, short type, int arraySize, bool ignoreEndianFla void bFile::safeSwapPtr(char *dst, const char *src) { + if (!src || !dst) + return; + int ptrFile = mFileDNA->getPointerSize(); int ptrMem = mMemoryDNA->getPointerSize(); - if (!src && !dst) - return; - if (ptrFile == ptrMem) { memcpy(dst, src, ptrMem); diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 4954e773e2..19f1737b73 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -346,8 +346,6 @@ void btQuantizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallb } } -int maxIterations = 0; - void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, const btVector3& aabbMin, const btVector3& aabbMax) const { btAssert(!m_useQuantization); @@ -387,8 +385,6 @@ void btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback, cons curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } /* @@ -529,8 +525,6 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex, int endNodeIndex) const @@ -654,8 +648,6 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback, unsigned short int* quantizedQueryAabbMin, unsigned short int* quantizedQueryAabbMax, int startNodeIndex, int endNodeIndex) const @@ -718,8 +710,6 @@ void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb curIndex += escapeIndex; } } - if (maxIterations < walkIterations) - maxIterations = walkIterations; } //This traversal can be called from Playstation 3 SPU diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h index 85dc488c8c..e085c40892 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -127,6 +127,7 @@ public: enum CollisionFlags { + CF_DYNAMIC_OBJECT = 0, CF_STATIC_OBJECT = 1, CF_KINEMATIC_OBJECT = 2, CF_NO_CONTACT_RESPONSE = 4, @@ -251,6 +252,16 @@ public: m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0; } + int getNumObjectsWithoutCollision() const + { + return m_objectsWithoutCollisionCheck.size(); + } + + const btCollisionObject* getObjectWithoutCollision(int index) + { + return m_objectsWithoutCollisionCheck[index]; + } + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index a4252c296a..a71700f58a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -361,7 +361,13 @@ void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangle for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index d663b3d6d6..c66ce58e3e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -124,12 +124,17 @@ void btBvhTriangleMeshShape::performRaycast(btTriangleCallback* callback, const nodeSubPart); unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { @@ -193,12 +198,17 @@ void btBvhTriangleMeshShape::performConvexcast(btTriangleCallback* callback, con nodeSubPart); unsigned int* gfxbase = (unsigned int*)(indexbase + nodeTriangleIndex * indexstride); - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); const btVector3& meshScaling = m_meshInterface->getScaling(); for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h index c80e105a4d..16f9e0c77a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h @@ -30,11 +30,12 @@ protected: int m_shapeType; void* m_userPointer; int m_userIndex; + int m_userIndex2; public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1) + btCollisionShape() : m_shapeType(INVALID_SHAPE_PROXYTYPE), m_userPointer(0), m_userIndex(-1), m_userIndex2(-1) { } @@ -137,6 +138,16 @@ public: return m_userIndex; } + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + + int getUserIndex2() const + { + return m_userIndex2; + } + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 34e7926f17..cab6980b65 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -21,8 +21,7 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape( int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, PHY_ScalarType hdt, bool flipQuadEdges) - :m_userIndex2(-1), - m_userValue3(0), + :m_userValue3(0), m_triangleInfoMap(0) { initialize(heightStickWidth, heightStickLength, heightfieldData, @@ -31,8 +30,7 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape( } btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges) - :m_userIndex2(-1), - m_userValue3(0), + : m_userValue3(0), m_triangleInfoMap(0) { // legacy constructor: support only float or unsigned char, diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 8dea98fc6b..2cf3c00721 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -114,7 +114,7 @@ protected: int m_vboundsGridLength; int m_vboundsChunkSize; - int m_userIndex2; + btScalar m_userValue3; struct btTriangleInfoMap* m_triangleInfoMap; @@ -192,14 +192,6 @@ public: virtual const char* getName() const { return "HEIGHTFIELD"; } - void setUserIndex2(int index) - { - m_userIndex2 = index; - } - int getUserIndex2() const - { - return m_userIndex2; - } void setUserValue3(btScalar value) { m_userValue3 = value; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 687399e0a9..863ea6d6ac 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -286,7 +286,6 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, nodeSubPart); curNodeSubPart = nodeSubPart; - btAssert(indicestype == PHY_INTEGER || indicestype == PHY_SHORT); } //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, @@ -294,7 +293,13 @@ void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface, int for (int j = 2; j >= 0; j--) { - int graphicsindex = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp index 4a95dbea4f..23c95ad3ff 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp @@ -2,8 +2,11 @@ #include "btMiniSDF.h" #include "LinearMath/btAabbUtil2.h" -struct btSdfCollisionShapeInternalData +ATTRIBUTE_ALIGNED16(struct) +btSdfCollisionShapeInternalData { + BT_DECLARE_ALIGNED_ALLOCATOR(); + btVector3 m_localScaling; btScalar m_margin; btMiniSDF m_sdf; diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h index 5b85e87041..cc91079579 100644 --- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h +++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h @@ -623,13 +623,21 @@ public: i1 = s_indices[1]; i2 = s_indices[2]; } - else + else if (indicestype == PHY_INTEGER) { unsigned int* i_indices = (unsigned int*)(indexbase + face_index * indexstride); i0 = i_indices[0]; i1 = i_indices[1]; i2 = i_indices[2]; } + else + { + btAssert(indicestype == PHY_UCHAR); + unsigned char* i_indices = (unsigned char*)(indexbase + face_index * indexstride); + i0 = i_indices[0]; + i1 = i_indices[1]; + i2 = i_indices[2]; + } } SIMD_FORCE_INLINE void get_vertex(unsigned int vertex_index, btVector3& vertex) const diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 45d1817135..7d53f8624a 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -1049,7 +1049,8 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, const btScalar length = delta.length(); results.normal = delta / length; results.witnesses[0] += results.normal * margin; - return (length - margin); + results.distance = length - margin; + return results.distance; } else { diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp index 27f76b8425..0f5ed1c2ce 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp @@ -852,7 +852,7 @@ static void setupSpatialGridBatchesMt( memHelper.addChunk((void**)&constraintRowBatchIds, sizeof(int) * numConstraintRows); size_t scratchSize = memHelper.getSizeToAllocate(); // if we need to reallocate - if (scratchMemory->capacity() < scratchSize) + if (static_cast<size_t>(scratchMemory->capacity()) < scratchSize) { // allocate 6.25% extra to avoid repeated reallocs scratchMemory->reserve(scratchSize + scratchSize / 16); diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 4356c12abf..3316403a87 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -47,6 +47,8 @@ struct btContactSolverInfoData btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints btScalar m_deformable_erp; //error reduction for deformable constraints + btScalar m_deformable_cfm; //constraint force mixing for deformable constraints + btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -83,7 +85,9 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.1); + m_deformable_erp = btScalar(0.06); + m_deformable_cfm = btScalar(0.01); + m_deformable_maxErrorReduction = btScalar(0.1); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h index 943d724cce..7442dd1e6a 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h @@ -356,12 +356,12 @@ public: } } - btVector3 getPushVelocity() + btVector3 getPushVelocity() const { return m_pushVelocity; } - btVector3 getTurnVelocity() + btVector3 getTurnVelocity() const { return m_turnVelocity; } @@ -465,6 +465,12 @@ public: //for kinematic objects, we could also use use: // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; } + + btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const + { + //we also calculate lin/ang velocity for kinematic objects + return m_pushVelocity + m_turnVelocity.cross(rel_pos); + } void translate(const btVector3& v) { diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp index a1d5bb9ca8..bec8c6530d 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp @@ -344,6 +344,8 @@ void btMultiBody::finalizeMultiDof() { m_deltaV.resize(0); m_deltaV.resize(6 + m_dofCount); + m_splitV.resize(0); + m_splitV.resize(6 + m_dofCount); m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) m_matrixBuf.resize(m_links.size() + 1); @@ -671,6 +673,30 @@ btScalar *btMultiBody::getJointTorqueMultiDof(int i) return &m_links[i].m_jointTorque[0]; } +bool btMultiBody::hasFixedBase() const +{ + return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticObject()); +} + +bool btMultiBody::isBaseStaticOrKinematic() const +{ + return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticOrKinematicObject()); +} + +bool btMultiBody::isBaseKinematic() const +{ + return getBaseCollider() && getBaseCollider()->isKinematicObject(); +} + +void btMultiBody::setBaseDynamicType(int dynamicType) +{ + if(getBaseCollider()) { + int oldFlags = getBaseCollider()->getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + getBaseCollider()->setCollisionFlags(oldFlags | dynamicType); + } +} + inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? { btVector3 row0 = btVector3( @@ -796,7 +822,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { zeroAccSpatFrc[0].setZero(); } @@ -872,31 +898,53 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // calculate zhat_i^A // - //external forces - btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; - btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; + if (isLinkAndAllAncestorsKinematic(i)) + { + zeroAccSpatFrc[i].setZero(); + } + else{ + //external forces + btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; + btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; - zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce)); + zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce)); #if 0 - { + { - b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", - i+1, - zeroAccSpatFrc[i+1].m_topVec[0], - zeroAccSpatFrc[i+1].m_topVec[1], - zeroAccSpatFrc[i+1].m_topVec[2], + b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", + i+1, + zeroAccSpatFrc[i+1].m_topVec[0], + zeroAccSpatFrc[i+1].m_topVec[1], + zeroAccSpatFrc[i+1].m_topVec[2], - zeroAccSpatFrc[i+1].m_bottomVec[0], - zeroAccSpatFrc[i+1].m_bottomVec[1], - zeroAccSpatFrc[i+1].m_bottomVec[2]); - } + zeroAccSpatFrc[i+1].m_bottomVec[0], + zeroAccSpatFrc[i+1].m_bottomVec[1], + zeroAccSpatFrc[i+1].m_bottomVec[2]); + } #endif - // - //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()), - linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm())); + // + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 1.; + zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()), + linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm())); + //p += vhat x Ihat vhat - done in a simpler way + if (m_useGyroTerm) + zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular())); + // + zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear())); + // + //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + ////clamp parent's omega + //btScalar parOmegaMod = temp.length(); + //btScalar parOmegaModMax = 1000; + //if(parOmegaMod > parOmegaModMax) + // temp *= parOmegaModMax / parOmegaMod; + //zeroAccSpatFrc[i+1].addLinear(temp); + //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); + //temp = spatCoriolisAcc[i].getLinear(); + //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); + } // calculate Ihat_i^A //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) @@ -909,22 +957,6 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0, 0, m_links[i].m_inertiaLocal[1], 0, 0, 0, m_links[i].m_inertiaLocal[2])); - // - //p += vhat x Ihat vhat - done in a simpler way - if (m_useGyroTerm) - zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular())); - // - zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear())); - //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); - ////clamp parent's omega - //btScalar parOmegaMod = temp.length(); - //btScalar parOmegaModMax = 1000; - //if(parOmegaMod > parOmegaModMax) - // temp *= parOmegaModMax / parOmegaMod; - //zeroAccSpatFrc[i+1].addLinear(temp); - //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); - //temp = spatCoriolisAcc[i].getLinear(); - //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); @@ -935,6 +967,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { + if(isLinkAndAllAncestorsKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1047,7 +1081,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1081,21 +1115,23 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]); - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + if(!isLinkAndAllAncestorsKinematic(i)) { - const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof); - } - - btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; - //D^{-1} * (Y - h^{T}*apar) - mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof); + } + btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset]; + //D^{-1} * (Y - h^{T}*apar) + mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); - spatAcc[i + 1] += spatCoriolisAcc[i]; + spatAcc[i + 1] += spatCoriolisAcc[i]; - for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + for (int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + } if (m_links[i].m_jointFeedback) { @@ -1432,7 +1468,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Fill in zero_acc // -- set to force/torque on the base, zero otherwise - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { zeroAccSpatFrc[0].setZero(); } @@ -1451,6 +1487,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // (part of TreeForwardDynamics in Mirtich.) for (int i = num_links - 1; i >= 0; --i) { + if(isLinkAndAllAncestorsKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1494,7 +1532,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1507,6 +1545,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // now do the loop over the m_links for (int i = 0; i < num_links; ++i) { + if(isLinkAndAllAncestorsKinematic(i)) + continue; const int parent = m_links[i].m_parent; fromParent.m_rotMat = rot_from_parent[i + 1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; @@ -1550,23 +1590,26 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar void btMultiBody::predictPositionsMultiDof(btScalar dt) { int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos; - btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - // reset to current position - for (int i = 0; i < 3; ++i) - { - m_basePos_interpolate[i] = m_basePos[i]; - } - pBasePos = m_basePos_interpolate; + if(!isBaseKinematic()) + { + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos; + btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; + // reset to current position + for (int i = 0; i < 3; ++i) + { + m_basePos_interpolate[i] = m_basePos[i]; + } + pBasePos = m_basePos_interpolate; + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + } /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -1617,26 +1660,29 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat; - - // reset to current orientation - for (int i = 0; i < 4; ++i) - { - m_baseQuat_interpolate[i] = m_baseQuat[i]; - } - pBaseQuat = m_baseQuat_interpolate; + if(!isBaseKinematic()) + { + btScalar *pBaseQuat; - btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); + // reset to current orientation + for (int i = 0; i < 4; ++i) + { + m_baseQuat_interpolate[i] = m_baseQuat[i]; + } + pBaseQuat = m_baseQuat_interpolate; + + btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + } // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) @@ -1644,55 +1690,88 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) btScalar *pJointPos; pJointPos = &m_links[i].m_jointPos_interpolate[0]; - btScalar *pJointVel = getJointVelMultiDof(i); - - switch (m_links[i].m_jointType) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - //reset to current pos - pJointPos[0] = m_links[i].m_jointPos[0]; - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - //reset to current pos - - for (int j = 0; j < 4; ++j) + if (m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()) + { + switch (m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: { - pJointPos[j] = m_links[i].m_jointPos[j]; + pJointPos[0] = m_links[i].m_jointPos[0]; + break; } - - btVector3 jointVel; - jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; - jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); - pJointPos[1] = jointOri.y(); - pJointPos[2] = jointOri.z(); - pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: - { - for (int j = 0; j < 3; ++j) + case btMultibodyLink::eSpherical: { - pJointPos[j] = m_links[i].m_jointPos[j]; + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; } - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; - - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - break; + case btMultibodyLink::ePlanar: + { + for (int j = 0; j < 3; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; + } + default: + break; } - default: + } + else + { + btScalar *pJointVel = getJointVelMultiDof(i); + + switch (m_links[i].m_jointType) { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + //reset to current pos + pJointPos[0] = m_links[i].m_jointPos[0]; + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + //reset to current pos + + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + + btVector3 jointVel; + jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; + jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + for (int j = 0; j < 3; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + break; + } + default: + { + } } } @@ -1703,16 +1782,19 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) { int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; + if(!isBaseKinematic()) + { + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + } /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -1763,22 +1845,25 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; - btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); - - //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); - //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); - //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + if(!isBaseKinematic()) + { + btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + + //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); + //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); + //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + } if (pq) pq += 7; @@ -1788,48 +1873,51 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) { - btScalar *pJointPos; - pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); - - btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - - switch (m_links[i].m_jointType) + if (!(m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic())) { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - //reset to current pos - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - //reset to current pos - btVector3 jointVel; - jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - btQuaternion jointOri; - jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); - pJointPos[1] = jointOri.y(); - pJointPos[2] = jointOri.z(); - pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: + btScalar *pJointPos; + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); + + switch (m_links[i].m_jointType) { - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + //reset to current pos + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + //reset to current pos + btVector3 jointVel; + jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; + jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - break; - } - default: - { + break; + } + default: + { + } } } @@ -2135,8 +2223,15 @@ void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObj world_to_local.resize(getNumLinks() + 1); local_origin.resize(getNumLinks() + 1); - world_to_local[0] = getInterpolateWorldToBaseRot(); - local_origin[0] = getInterpolateBasePos(); + if(isBaseKinematic()){ + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + } + else + { + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + } if (getBaseCollider()) { @@ -2282,3 +2377,81 @@ const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *seriali return btMultiBodyDataName; } + +void btMultiBody::saveKinematicState(btScalar timeStep) +{ + //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities + if (timeStep != btScalar(0.)) + { + btVector3 linearVelocity, angularVelocity; + btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity); + setBaseVel(linearVelocity); + setBaseOmega(angularVelocity); + setInterpolateBaseWorldTransform(getBaseWorldTransform()); + } +} + +void btMultiBody::setLinkDynamicType(const int i, int type) +{ + if (i == -1) + { + setBaseDynamicType(type); + } + else if (i >= 0 && i < getNumLinks()) + { + if (m_links[i].m_collider) + { + m_links[i].m_collider->setDynamicType(type); + } + } +} + +bool btMultiBody::isLinkStaticOrKinematic(const int i) const +{ + if (i == -1) + { + return isBaseStaticOrKinematic(); + } + else + { + if (m_links[i].m_collider) + return m_links[i].m_collider->isStaticOrKinematic(); + } + return false; +} + +bool btMultiBody::isLinkKinematic(const int i) const +{ + if (i == -1) + { + return isBaseKinematic(); + } + else + { + if (m_links[i].m_collider) + return m_links[i].m_collider->isKinematic(); + } + return false; +} + +bool btMultiBody::isLinkAndAllAncestorsStaticOrKinematic(const int i) const +{ + int link = i; + while (link != -1) { + if (!isLinkStaticOrKinematic(link)) + return false; + link = m_links[link].m_parent; + } + return isBaseStaticOrKinematic(); +} + +bool btMultiBody::isLinkAndAllAncestorsKinematic(const int i) const +{ + int link = i; + while (link != -1) { + if (!isLinkKinematic(link)) + return false; + link = m_links[link].m_parent; + } + return isBaseKinematic(); +} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h index be795633fd..25112a6805 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h @@ -210,7 +210,13 @@ public: void setBasePos(const btVector3 &pos) { m_basePos = pos; - m_basePos_interpolate = pos; + if(!isBaseKinematic()) + m_basePos_interpolate = pos; + } + + void setInterpolateBasePos(const btVector3 &pos) + { + m_basePos_interpolate = pos; } void setBaseWorldTransform(const btTransform &tr) @@ -227,17 +233,39 @@ public: return tr; } + void setInterpolateBaseWorldTransform(const btTransform &tr) + { + setInterpolateBasePos(tr.getOrigin()); + setInterpolateWorldToBaseRot(tr.getRotation().inverse()); + } + + btTransform getInterpolateBaseWorldTransform() const + { + btTransform tr; + tr.setOrigin(getInterpolateBasePos()); + tr.setRotation(getInterpolateWorldToBaseRot().inverse()); + return tr; + } + void setBaseVel(const btVector3 &vel) { m_realBuf[3] = vel[0]; m_realBuf[4] = vel[1]; m_realBuf[5] = vel[2]; } + void setWorldToBaseRot(const btQuaternion &rot) { m_baseQuat = rot; //m_baseQuat asumed to ba alias!? - m_baseQuat_interpolate = rot; + if(!isBaseKinematic()) + m_baseQuat_interpolate = rot; + } + + void setInterpolateWorldToBaseRot(const btQuaternion &rot) + { + m_baseQuat_interpolate = rot; } + void setBaseOmega(const btVector3 &omega) { m_realBuf[0] = omega[0]; @@ -245,6 +273,8 @@ public: m_realBuf[2] = omega[2]; } + void saveKinematicState(btScalar timeStep); + // // get/set pos/vel for child m_links (i = 0 to num_links-1) // @@ -278,6 +308,11 @@ public: { return &m_deltaV[0]; } + + const btScalar *getSplitVelocityVector() const + { + return &m_splitV[0]; + } /* btScalar * getVelocityVector() { return &real_buf[0]; @@ -397,6 +432,26 @@ public: m_deltaV[dof] += delta_vee[dof] * multiplier; } } + void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier) + { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] += delta_vee[dof] * multiplier; + } + } + void addSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], 1); + } + void substractSplitV() + { + applyDeltaVeeMultiDof(&m_splitV[0], -1); + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_splitV[dof] = 0.f; + } + } void processDeltaVeeMultiDof2() { applyDeltaVeeMultiDof(&m_deltaV[0], 1); @@ -495,14 +550,22 @@ public: void goToSleep(); void checkMotionAndSleepIfRequired(btScalar timestep); - bool hasFixedBase() const - { - return m_fixedBase; - } + bool hasFixedBase() const; + + bool isBaseKinematic() const; + + bool isBaseStaticOrKinematic() const; + + // set the dynamic type in the base's collision flags. + void setBaseDynamicType(int dynamicType); void setFixedBase(bool fixedBase) { m_fixedBase = fixedBase; + if(m_fixedBase) + setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT); + else + setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); } int getCompanionId() const @@ -653,7 +716,15 @@ public: btVector3 &top_out, // top part of output vector btVector3 &bottom_out); // bottom part of output vector + void setLinkDynamicType(const int i, int type); + + bool isLinkStaticOrKinematic(const int i) const; + + bool isLinkKinematic(const int i) const; + + bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const; + bool isLinkAndAllAncestorsKinematic(const int i) const; private: btMultiBody(const btMultiBody &); // not implemented @@ -711,6 +782,7 @@ private: // offset size array // 0 num_links+1 rot_from_parent // + btAlignedObjectArray<btScalar> m_splitV; btAlignedObjectArray<btScalar> m_deltaV; btAlignedObjectArray<btScalar> m_realBuf; btAlignedObjectArray<btVector3> m_vectorBuf; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d7ed05ce57..1ba5861145 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -2,11 +2,12 @@ #include "BulletDynamics/Dynamics/btRigidBody.h" #include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral) +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type) : m_bodyA(bodyA), m_bodyB(bodyB), m_linkA(linkA), m_linkB(linkB), + m_type(type), m_numRows(numRows), m_jacSizeA(0), m_jacSizeBoth(0), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 5c15f3e851..4a6007ee3e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -20,6 +20,21 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" #include "btMultiBody.h" + +//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility +enum btTypedMultiBodyConstraintType +{ + MULTIBODY_CONSTRAINT_LIMIT=3, + MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR, + MULTIBODY_CONSTRAINT_GEAR, + MULTIBODY_CONSTRAINT_POINT_TO_POINT, + MULTIBODY_CONSTRAINT_SLIDER, + MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR, + MULTIBODY_CONSTRAINT_FIXED, + + MAX_MULTIBODY_CONSTRAINT_TYPE, +}; + class btMultiBody; struct btSolverInfo; @@ -46,6 +61,8 @@ protected: int m_linkA; int m_linkB; + int m_type; //btTypedMultiBodyConstraintType + int m_numRows; int m_jacSizeA; int m_jacSizeBoth; @@ -82,12 +99,16 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral); + btMultiBodyConstraint(btMultiBody * bodyA, btMultiBody * bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type); virtual ~btMultiBodyConstraint(); void updateJacobianSizes(); void allocateJacobiansMultiDof(); + int getConstraintType() const + { + return m_type; + } //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. virtual void setFrameInB(const btMatrix3x3& frameInB) {} virtual void setPivotInB(const btVector3& pivotInB) {} diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index cd1bad089e..fef95f0c4e 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -592,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) if (!isSleeping) { + bod->addSplitV(); int nLinks = bod->getNumLinks(); ///base + num m_links @@ -610,6 +611,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->substractSplitV(); } else { @@ -867,6 +869,18 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } } + +void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep) +{ + btDiscreteDynamicsWorld::saveKinematicState(timeStep); + for(int i = 0; i < m_multiBodies.size(); i++) + { + btMultiBody* body = m_multiBodies[i]; + if(body->isBaseKinematic()) + body->saveKinematicState(timeStep); + } +} + // //void btMultiBodyDynamicsWorld::setSplitIslands(bool split) //{ diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 9ac46f4b64..d2d76c8b92 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -120,5 +120,7 @@ public: virtual void solveExternalForces(btContactSolverInfo& solverInfo); virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); void buildIslands(); + + virtual void saveKinematicState(btScalar timeStep); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp index 5ef9444c2f..df2abbe97a 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -24,7 +24,7 @@ subject to the following restrictions: #define BTMBFIXEDCONSTRAINT_DIM 6 btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -36,7 +36,7 @@ btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int li } btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_FIXED), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index bf6b811d26..ee02cf9b07 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false, MULTIBODY_CONSTRAINT_GEAR), m_gearRatio(1), m_gearAuxLink(-1), m_erp(0), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 8791ad2868..94b36ac108 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) //:btMultiBodyConstraint(body,0,link,-1,2,true), - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT), m_lowerBound(lower), m_upperBound(upper) { diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index 6716ba490f..b810692b4c 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -42,6 +42,22 @@ public: { //todo(erwincoumans) } + btScalar getLowerBound() const + { + return m_lowerBound; + } + btScalar getUpperBound() const + { + return m_upperBound; + } + void setLowerBound(btScalar lower) + { + m_lowerBound = lower; + } + void setUpperBound(btScalar upper) + { + m_upperBound = upper; + } }; #endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 5c816c4987..fec9b03213 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), @@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof() btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) //:btMultiBodyConstraint(body,0,link,-1,1,true), - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true, MULTIBODY_CONSTRAINT_1DOF_JOINT_MOTOR), m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h index 01d5583c2f..5a1429340f 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -295,6 +295,9 @@ struct btMultibodyLink } } } + + + }; #endif //BT_MULTIBODY_LINK_H diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index bc909990c2..3dc35a5814 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -130,6 +130,23 @@ public: return true; } + bool isStaticOrKinematic() const + { + return isStaticOrKinematicObject(); + } + + bool isKinematic() const + { + return isKinematicObject(); + } + + void setDynamicType(int dynamicType) + { + int oldFlags = getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + setCollisionFlags(oldFlags | dynamicType); + } + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 37d3aede37..f51e69deb1 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: #endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -37,7 +37,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp index e025302ce6..48ec1d5af2 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -25,7 +25,7 @@ subject to the following restrictions: #define EPSILON 0.000001 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false), + : btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -38,7 +38,7 @@ btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int } btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) - : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false), + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_SLIDER), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp index 3e5aa30f28..25ddd539bf 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -23,7 +23,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse) - : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true), + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR), m_desiredVelocity(0, 0, 0), m_desiredPosition(0,0,0,1), m_kd(1.), diff --git a/thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h b/thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h index 7b225701f6..01c7e93a1b 100644 --- a/thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h +++ b/thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h @@ -13,13 +13,12 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver btDeformableMultiBodyConstraintSolver* m_deformableSolver; DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) + btDispatcher* dispatcher) + : MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver) { } - - virtual void processConstraints(int islandId=-1) + virtual void processConstraints(int islandId = -1) { btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0; @@ -30,7 +29,7 @@ struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolver //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics & 1)) { m_deformableSolver->m_analyticsData.m_islandId = islandId; m_islandAnalyticsData.push_back(m_solver->m_analyticsData); diff --git a/thirdparty/bullet/BulletSoftBody/btCGProjection.h b/thirdparty/bullet/BulletSoftBody/btCGProjection.h index d047e6d3d9..e05970664c 100644 --- a/thirdparty/bullet/BulletSoftBody/btCGProjection.h +++ b/thirdparty/bullet/BulletSoftBody/btCGProjection.h @@ -22,85 +22,83 @@ struct DeformableContactConstraint { - const btSoftBody::Node* m_node; - btAlignedObjectArray<const btSoftBody::RContact*> m_contact; - btAlignedObjectArray<btVector3> m_total_normal_dv; - btAlignedObjectArray<btVector3> m_total_tangent_dv; - btAlignedObjectArray<bool> m_static; - btAlignedObjectArray<bool> m_can_be_dynamic; - - DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node) - { - append(rcontact); - } - - DeformableContactConstraint(): m_node(NULL) - { - m_contact.push_back(NULL); - } - - void append(const btSoftBody::RContact& rcontact) - { - m_contact.push_back(&rcontact); - m_total_normal_dv.push_back(btVector3(0,0,0)); - m_total_tangent_dv.push_back(btVector3(0,0,0)); - m_static.push_back(false); - m_can_be_dynamic.push_back(true); - } - - void replace(const btSoftBody::RContact& rcontact) - { - m_contact.clear(); - m_total_normal_dv.clear(); - m_total_tangent_dv.clear(); - m_static.clear(); - m_can_be_dynamic.clear(); - append(rcontact); - } - - ~DeformableContactConstraint() - { - } + const btSoftBody::Node* m_node; + btAlignedObjectArray<const btSoftBody::RContact*> m_contact; + btAlignedObjectArray<btVector3> m_total_normal_dv; + btAlignedObjectArray<btVector3> m_total_tangent_dv; + btAlignedObjectArray<bool> m_static; + btAlignedObjectArray<bool> m_can_be_dynamic; + + DeformableContactConstraint(const btSoftBody::RContact& rcontact) : m_node(rcontact.m_node) + { + append(rcontact); + } + + DeformableContactConstraint() : m_node(NULL) + { + m_contact.push_back(NULL); + } + + void append(const btSoftBody::RContact& rcontact) + { + m_contact.push_back(&rcontact); + m_total_normal_dv.push_back(btVector3(0, 0, 0)); + m_total_tangent_dv.push_back(btVector3(0, 0, 0)); + m_static.push_back(false); + m_can_be_dynamic.push_back(true); + } + + void replace(const btSoftBody::RContact& rcontact) + { + m_contact.clear(); + m_total_normal_dv.clear(); + m_total_tangent_dv.clear(); + m_static.clear(); + m_can_be_dynamic.clear(); + append(rcontact); + } + + ~DeformableContactConstraint() + { + } }; class btCGProjection { public: - typedef btAlignedObjectArray<btVector3> TVStack; - typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; - typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - const btScalar& m_dt; - // map from node indices to node pointers - const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; - - btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) - : m_softBodies(softBodies) - , m_dt(dt) - { - } - - virtual ~btCGProjection() - { - } - - // apply the constraints - virtual void project(TVStack& x) = 0; - - virtual void setConstraints() = 0; - - // update the constraints - virtual btScalar update() = 0; - - virtual void reinitialize(bool nodeUpdated) - { - } - - virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) - { - m_nodes = nodes; - } -}; + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack; + typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + const btScalar& m_dt; + // map from node indices to node pointers + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btCGProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const btScalar& dt) + : m_softBodies(softBodies), m_dt(dt) + { + } + virtual ~btCGProjection() + { + } + + // apply the constraints + virtual void project(TVStack& x) = 0; + + virtual void setConstraints() = 0; + + // update the constraints + virtual btScalar update() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } +}; #endif /* btCGProjection_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btConjugateGradient.h b/thirdparty/bullet/BulletSoftBody/btConjugateGradient.h index bd51e584b9..bcd5e6b519 100644 --- a/thirdparty/bullet/BulletSoftBody/btConjugateGradient.h +++ b/thirdparty/bullet/BulletSoftBody/btConjugateGradient.h @@ -15,144 +15,103 @@ #ifndef BT_CONJUGATE_GRADIENT_H #define BT_CONJUGATE_GRADIENT_H -#include <iostream> -#include <cmath> -#include <limits> -#include <LinearMath/btAlignedObjectArray.h> -#include <LinearMath/btVector3.h> -#include "LinearMath/btQuickprof.h" +#include "btKrylovSolver.h" template <class MatrixX> -class btConjugateGradient +class btConjugateGradient : public btKrylovSolver<MatrixX> { - typedef btAlignedObjectArray<btVector3> TVStack; - TVStack r,p,z,temp; - int max_iterations; - btScalar tolerance_squared; + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btKrylovSolver<MatrixX> Base; + TVStack r, p, z, temp; + public: - btConjugateGradient(const int max_it_in) - : max_iterations(max_it_in) - { - tolerance_squared = 1e-5; - } - - virtual ~btConjugateGradient(){} - - // return the number of iterations taken - int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) - { - BT_PROFILE("CGSolve"); - btAssert(x.size() == b.size()); - reinitialize(b); - // r = b - A * x --with assigned dof zeroed out - A.multiply(x, temp); - r = sub(b, temp); - A.project(r); - // z = M^(-1) * r - A.precondition(r, z); - A.project(z); - btScalar r_dot_z = dot(z,r); - if (r_dot_z <= tolerance_squared) { - if (verbose) - { - std::cout << "Iteration = 0" << std::endl; - std::cout << "Two norm of the residual = " << r_dot_z << std::endl; - } - return 0; - } - p = z; - btScalar r_dot_z_new = r_dot_z; - for (int k = 1; k <= max_iterations; k++) { - // temp = A*p - A.multiply(p, temp); - A.project(temp); - if (dot(p,temp) < SIMD_EPSILON) - { - if (verbose) - std::cout << "Encountered negative direction in CG!" << std::endl; - if (k == 1) - { - x = b; - } - return k; - } - // alpha = r^T * z / (p^T * A * p) - btScalar alpha = r_dot_z_new / dot(p, temp); - // x += alpha * p; - multAndAddTo(alpha, p, x); - // r -= alpha * temp; - multAndAddTo(-alpha, temp, r); - // z = M^(-1) * r - A.precondition(r, z); - r_dot_z = r_dot_z_new; - r_dot_z_new = dot(r,z); - if (r_dot_z_new < tolerance_squared) { - if (verbose) - { - std::cout << "ConjugateGradient iterations " << k << std::endl; - } - return k; - } + btConjugateGradient(const int max_it_in) + : btKrylovSolver<MatrixX>(max_it_in, SIMD_EPSILON) + { + } + + virtual ~btConjugateGradient() {} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) + { + BT_PROFILE("CGSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + temp = b; + A.project(temp); + p = temp; + A.precondition(p, z); + btScalar d0 = this->dot(z, temp); + d0 = btMin(btScalar(1), d0); + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp); + r = this->sub(b, temp); + A.project(r); + // z = M^(-1) * r + A.precondition(r, z); + A.project(z); + btScalar r_dot_z = this->dot(z, r); + if (r_dot_z <= Base::m_tolerance * d0) + { + if (verbose) + { + std::cout << "Iteration = 0" << std::endl; + std::cout << "Two norm of the residual = " << r_dot_z << std::endl; + } + return 0; + } + p = z; + btScalar r_dot_z_new = r_dot_z; + for (int k = 1; k <= Base::m_maxIterations; k++) + { + // temp = A*p + A.multiply(p, temp); + A.project(temp); + if (this->dot(p, temp) < 0) + { + if (verbose) + std::cout << "Encountered negative direction in CG!" << std::endl; + if (k == 1) + { + x = b; + } + return k; + } + // alpha = r^T * z / (p^T * A * p) + btScalar alpha = r_dot_z_new / this->dot(p, temp); + // x += alpha * p; + this->multAndAddTo(alpha, p, x); + // r -= alpha * temp; + this->multAndAddTo(-alpha, temp, r); + // z = M^(-1) * r + A.precondition(r, z); + r_dot_z = r_dot_z_new; + r_dot_z_new = this->dot(r, z); + if (r_dot_z_new < Base::m_tolerance * d0) + { + if (verbose) + { + std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl; + } + return k; + } + + btScalar beta = r_dot_z_new / r_dot_z; + p = this->multAndAdd(beta, p, z); + } + if (verbose) + { + std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl; + } + return Base::m_maxIterations; + } - btScalar beta = r_dot_z_new/r_dot_z; - p = multAndAdd(beta, p, z); - } - if (verbose) - { - std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl; - } - return max_iterations; - } - - void reinitialize(const TVStack& b) - { - r.resize(b.size()); - p.resize(b.size()); - z.resize(b.size()); - temp.resize(b.size()); - } - - TVStack sub(const TVStack& a, const TVStack& b) - { - // c = a-b - btAssert(a.size() == b.size()); - TVStack c; - c.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - { - c[i] = a[i] - b[i]; - } - return c; - } - - btScalar squaredNorm(const TVStack& a) - { - return dot(a,a); - } - - btScalar dot(const TVStack& a, const TVStack& b) - { - btScalar ans(0); - for (int i = 0; i < a.size(); ++i) - ans += a[i].dot(b[i]); - return ans; - } - - void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) - { -// result += s*a - btAssert(a.size() == result.size()); - for (int i = 0; i < a.size(); ++i) - result[i] += s * a[i]; - } - - TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) - { - // result = a*s + b - TVStack result; - result.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - result[i] = s * a[i] + b[i]; - return result; - } + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp.resize(b.size()); + } }; #endif /* btConjugateGradient_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h b/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h index 7b211c4172..6146120365 100644 --- a/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h +++ b/thirdparty/bullet/BulletSoftBody/btConjugateResidual.h @@ -15,174 +15,98 @@ #ifndef BT_CONJUGATE_RESIDUAL_H #define BT_CONJUGATE_RESIDUAL_H -#include <iostream> -#include <cmath> -#include <limits> -#include <LinearMath/btAlignedObjectArray.h> -#include <LinearMath/btVector3.h> -#include <LinearMath/btScalar.h> -#include "LinearMath/btQuickprof.h" +#include "btKrylovSolver.h" + template <class MatrixX> -class btConjugateResidual +class btConjugateResidual : public btKrylovSolver<MatrixX> { - typedef btAlignedObjectArray<btVector3> TVStack; - TVStack r,p,z,temp_p, temp_r, best_x; - // temp_r = A*r - // temp_p = A*p - // z = M^(-1) * temp_p = M^(-1) * A * p - int max_iterations; - btScalar tolerance_squared, best_r; + typedef btAlignedObjectArray<btVector3> TVStack; + typedef btKrylovSolver<MatrixX> Base; + TVStack r, p, z, temp_p, temp_r, best_x; + // temp_r = A*r + // temp_p = A*p + // z = M^(-1) * temp_p = M^(-1) * A * p + btScalar best_r; + public: - btConjugateResidual(const int max_it_in) - : max_iterations(max_it_in) - { - tolerance_squared = 1e-2; - } - - virtual ~btConjugateResidual(){} - - // return the number of iterations taken - int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) - { - BT_PROFILE("CRSolve"); - btAssert(x.size() == b.size()); - reinitialize(b); - // r = b - A * x --with assigned dof zeroed out - A.multiply(x, temp_r); // borrow temp_r here to store A*x - r = sub(b, temp_r); - // z = M^(-1) * r - A.precondition(r, z); // borrow z to store preconditioned r - r = z; - btScalar residual_norm = norm(r); - if (residual_norm <= tolerance_squared) { - if (verbose) - { - std::cout << "Iteration = 0" << std::endl; - std::cout << "Two norm of the residual = " << residual_norm << std::endl; - } - return 0; - } - p = r; - btScalar r_dot_Ar, r_dot_Ar_new; - // temp_p = A*p - A.multiply(p, temp_p); - // temp_r = A*r - temp_r = temp_p; - r_dot_Ar = dot(r, temp_r); - for (int k = 1; k <= max_iterations; k++) { - // z = M^(-1) * Ap - A.precondition(temp_p, z); - // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) - btScalar alpha = r_dot_Ar / dot(temp_p, z); - // x += alpha * p; - multAndAddTo(alpha, p, x); - // r -= alpha * z; - multAndAddTo(-alpha, z, r); - btScalar norm_r = norm(r); - if (norm_r < best_r) - { - best_x = x; - best_r = norm_r; - if (norm_r < tolerance_squared) { - if (verbose) - { - std::cout << "ConjugateResidual iterations " << k << std::endl; - } - return k; - } - else - { - if (verbose) - { - std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl; - } - } - } - // temp_r = A * r; - A.multiply(r, temp_r); - r_dot_Ar_new = dot(r, temp_r); - btScalar beta = r_dot_Ar_new/r_dot_Ar; - r_dot_Ar = r_dot_Ar_new; - // p = beta*p + r; - p = multAndAdd(beta, p, r); - // temp_p = beta*temp_p + temp_r; - temp_p = multAndAdd(beta, temp_p, temp_r); - } - if (verbose) - { - std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl; - } - x = best_x; - return max_iterations; - } - - void reinitialize(const TVStack& b) - { - r.resize(b.size()); - p.resize(b.size()); - z.resize(b.size()); - temp_p.resize(b.size()); - temp_r.resize(b.size()); - best_x.resize(b.size()); - best_r = SIMD_INFINITY; - } - - TVStack sub(const TVStack& a, const TVStack& b) - { - // c = a-b - btAssert(a.size() == b.size()); - TVStack c; - c.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - { - c[i] = a[i] - b[i]; - } - return c; - } - - btScalar squaredNorm(const TVStack& a) - { - return dot(a,a); - } - - btScalar norm(const TVStack& a) - { - btScalar ret = 0; - for (int i = 0; i < a.size(); ++i) - { - for (int d = 0; d < 3; ++d) - { - ret = btMax(ret, btFabs(a[i][d])); - } - } - return ret; - } - - btScalar dot(const TVStack& a, const TVStack& b) - { - btScalar ans(0); - for (int i = 0; i < a.size(); ++i) - ans += a[i].dot(b[i]); - return ans; - } - - void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) - { - // result += s*a - btAssert(a.size() == result.size()); - for (int i = 0; i < a.size(); ++i) - result[i] += s * a[i]; - } - - TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) - { - // result = a*s + b - TVStack result; - result.resize(a.size()); - for (int i = 0; i < a.size(); ++i) - result[i] = s * a[i] + b[i]; - return result; - } + btConjugateResidual(const int max_it_in) + : Base(max_it_in, 1e-8) + { + } + + virtual ~btConjugateResidual() {} + + // return the number of iterations taken + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) + { + BT_PROFILE("CRSolve"); + btAssert(x.size() == b.size()); + reinitialize(b); + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp_r); // borrow temp_r here to store A*x + r = this->sub(b, temp_r); + // z = M^(-1) * r + A.precondition(r, z); // borrow z to store preconditioned r + r = z; + btScalar residual_norm = this->norm(r); + if (residual_norm <= Base::m_tolerance) + { + return 0; + } + p = r; + btScalar r_dot_Ar, r_dot_Ar_new; + // temp_p = A*p + A.multiply(p, temp_p); + // temp_r = A*r + temp_r = temp_p; + r_dot_Ar = this->dot(r, temp_r); + for (int k = 1; k <= Base::m_maxIterations; k++) + { + // z = M^(-1) * Ap + A.precondition(temp_p, z); + // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) + btScalar alpha = r_dot_Ar / this->dot(temp_p, z); + // x += alpha * p; + this->multAndAddTo(alpha, p, x); + // r -= alpha * z; + this->multAndAddTo(-alpha, z, r); + btScalar norm_r = this->norm(r); + if (norm_r < best_r) + { + best_x = x; + best_r = norm_r; + if (norm_r < Base::m_tolerance) + { + return k; + } + } + // temp_r = A * r; + A.multiply(r, temp_r); + r_dot_Ar_new = this->dot(r, temp_r); + btScalar beta = r_dot_Ar_new / r_dot_Ar; + r_dot_Ar = r_dot_Ar_new; + // p = beta*p + r; + p = this->multAndAdd(beta, p, r); + // temp_p = beta*temp_p + temp_r; + temp_p = this->multAndAdd(beta, temp_p, temp_r); + } + if (verbose) + { + std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl; + } + x = best_x; + return Base::m_maxIterations; + } + + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp_p.resize(b.size()); + temp_r.resize(b.size()); + best_x.resize(b.size()); + best_r = SIMD_INFINITY; + } }; #endif /* btConjugateResidual_h */ - diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 5381ee6265..2455ed2138 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -17,211 +17,283 @@ #include "btPreconditioner.h" #include "LinearMath/btQuickprof.h" -btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v) -: m_softBodies(softBodies) -, m_projection(softBodies) -, m_backupVelocity(backup_v) -, m_implicit(false) +btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v) + : m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false) { - m_massPreconditioner = new MassPreconditioner(m_softBodies); - m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); - m_preconditioner = m_KKTPreconditioner; + m_massPreconditioner = new MassPreconditioner(m_softBodies); + m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); + m_preconditioner = m_KKTPreconditioner; } btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() { - delete m_KKTPreconditioner; - delete m_massPreconditioner; + delete m_KKTPreconditioner; + delete m_massPreconditioner; } void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt) { - BT_PROFILE("reinitialize"); - if (dt > 0) - { - setDt(dt); - } - if(nodeUpdated) - { - updateId(); - } - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->reinitialize(nodeUpdated); - } - m_projection.reinitialize(nodeUpdated); -// m_preconditioner->reinitialize(nodeUpdated); + BT_PROFILE("reinitialize"); + if (dt > 0) + { + setDt(dt); + } + if (nodeUpdated) + { + updateId(); + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->reinitialize(nodeUpdated); + } + btMatrix3x3 I; + I.setIdentity(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im); + } + } + m_projection.reinitialize(nodeUpdated); + // m_preconditioner->reinitialize(nodeUpdated); } void btDeformableBackwardEulerObjective::setDt(btScalar dt) { - m_dt = dt; + m_dt = dt; } void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const { - BT_PROFILE("multiply"); - // add in the mass term - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im; - ++counter; - } - } - - for (int i = 0; i < m_lf.size(); ++i) - { - // add damping matrix - m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); - if (m_implicit) - { - m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); - } - } - int offset = m_nodes.size(); - for (int i = offset; i < b.size(); ++i) - { - b[i].setZero(); - } - // add in the lagrange multiplier terms - - for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c) - { - // C^T * lambda - const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c]; - for (int i = 0; i < lm.m_num_nodes; ++i) - { - for (int j = 0; j < lm.m_num_constraints; ++j) - { - b[lm.m_indices[i]] += x[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; - } - } - // C * x - for (int d = 0; d < lm.m_num_constraints; ++d) - { - for (int i = 0; i < lm.m_num_nodes; ++i) - { - b[offset+c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]); - } - } - } + BT_PROFILE("multiply"); + // add in the mass term + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + b[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : x[counter] / node.m_im; + ++counter; + } + } + + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) + { + m_lf[i]->addScaledElasticForceDifferential(-m_dt * m_dt, x, b); + } + } + int offset = m_nodes.size(); + for (int i = offset; i < b.size(); ++i) + { + b[i].setZero(); + } + // add in the lagrange multiplier terms + + for (int c = 0; c < m_projection.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += x[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset + c][d] += lm.m_weights[i] * x[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } } void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - node.m_v = m_backupVelocity[node.index] + dv[node.index]; - } - } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + node.m_v = m_backupVelocity[node.index] + dv[node.index]; + } + } } void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero) { - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; - psb->m_nodes[j].m_v += one_over_mass * force[counter++]; - } - } - if (setZero) - { - for (int i = 0; i < force.size(); ++i) - force[i].setZero(); - } + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + if (m_implicit) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im != 0) + { + psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; + } + } + } + else + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + } + } + } + if (setZero) + { + for (int i = 0; i < force.size(); ++i) + force[i].setZero(); + } } -void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) +void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack& residual) { - BT_PROFILE("computeResidual"); - // add implicit force - for (int i = 0; i < m_lf.size(); ++i) - { - if (m_implicit) - { - m_lf[i]->addScaledForces(dt, residual); - } - else - { - m_lf[i]->addScaledDampingForce(dt, residual); - } - } -// m_projection.project(residual); + BT_PROFILE("computeResidual"); + // add implicit force + for (int i = 0; i < m_lf.size(); ++i) + { + // Always integrate picking force implicitly for stability. + if (m_implicit || m_lf[i]->getForceType() == BT_MOUSE_PICKING_FORCE) + { + m_lf[i]->addScaledForces(dt, residual); + } + else + { + m_lf[i]->addScaledDampingForce(dt, residual); + } + } + // m_projection.project(residual); } btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const { - btScalar mag = 0; - for (int i = 0; i < residual.size(); ++i) - { - mag += residual[i].length2(); - } - return std::sqrt(mag); + btScalar mag = 0; + for (int i = 0; i < residual.size(); ++i) + { + mag += residual[i].length2(); + } + return std::sqrt(mag); } btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt) { - btScalar e = 0; - for (int i = 0; i < m_lf.size(); ++i) - { - e += m_lf[i]->totalEnergy(dt); - } - return e; + btScalar e = 0; + for (int i = 0; i < m_lf.size(); ++i) + { + e += m_lf[i]->totalEnergy(dt); + } + return e; } void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) { - for (int i = 0; i < m_softBodies.size(); ++i) - { - m_softBodies[i]->advanceDeformation(); - } - - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->addScaledExplicitForce(m_dt, force); - } - applyForce(force, true); + for (int i = 0; i < m_softBodies.size(); ++i) + { + m_softBodies[i]->advanceDeformation(); + } + if (m_implicit) + { + // apply forces except gravity force + btVector3 gravity; + for (int i = 0; i < m_lf.size(); ++i) + { + if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE) + { + gravity = static_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity; + } + else + { + m_lf[i]->addScaledForces(m_dt, force); + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledHessian(m_dt); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // add gravity explicitly + psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity; + } + } + } + } + else + { + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledExplicitForce(m_dt, force); + } + } + // calculate inverse mass matrix for all nodes + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + { + psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse(); + } + } + } + } + applyForce(force, true); } void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) { - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - dv[counter] = psb->m_nodes[j].m_im * residual[counter]; - ++counter; - } - } + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + dv[counter] = psb->m_nodes[j].m_im * residual[counter]; + ++counter; + } + } } //set constraints as projections void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal) { - m_projection.setConstraints(infoGlobal); + m_projection.setConstraints(infoGlobal); } void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) { - m_projection.applyDynamicFriction(r); + m_projection.applyDynamicFriction(r); } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h index 86579e71ac..eb05b9f010 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -31,143 +31,168 @@ class btDeformableBackwardEulerObjective { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_dt; - btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - Preconditioner* m_preconditioner; - btDeformableContactProjection m_projection; - const TVStack& m_backupVelocity; - btAlignedObjectArray<btSoftBody::Node* > m_nodes; - bool m_implicit; - MassPreconditioner* m_massPreconditioner; - KKTPreconditioner* m_KKTPreconditioner; - - btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v); - - virtual ~btDeformableBackwardEulerObjective(); - - void initialize(){} - - // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual - void computeResidual(btScalar dt, TVStack& residual); - - // add explicit force to the velocity - void applyExplicitForce(TVStack& force); - - // apply force to velocity and optionally reset the force to zero - void applyForce(TVStack& force, bool setZero); - - // compute the norm of the residual - btScalar computeNorm(const TVStack& residual) const; - - // compute one step of the solve (there is only one solve if the system is linear) - void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); - - // perform A*x = b - void multiply(const TVStack& x, TVStack& b) const; - - // set initial guess for CG solve - void initialGuess(TVStack& dv, const TVStack& residual); - - // reset data structure and reset dt - void reinitialize(bool nodeUpdated, btScalar dt); - - void setDt(btScalar dt); - - // add friction force to residual - void applyDynamicFriction(TVStack& r); - - // add dv to velocity - void updateVelocity(const TVStack& dv); - - //set constraints as projections - void setConstraints(const btContactSolverInfo& infoGlobal); - - // update the projections and project the residual - void project(TVStack& r) - { - BT_PROFILE("project"); - m_projection.project(r); - } - - // perform precondition M^(-1) x = b - void precondition(const TVStack& x, TVStack& b) - { - m_preconditioner->operator()(x,b); - } - - // reindex all the vertices - virtual void updateId() - { - size_t node_id = 0; - size_t face_id = 0; - m_nodes.clear(); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].index = node_id; - m_nodes.push_back(&psb->m_nodes[j]); - ++node_id; - } - for (int j = 0; j < psb->m_faces.size(); ++j) - { - psb->m_faces[j].m_index = face_id; - ++face_id; - } - } - } - - const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const - { - return &m_nodes; - } - - void setImplicit(bool implicit) - { - m_implicit = implicit; - } - - // Calculate the total potential energy in the system - btScalar totalEnergy(btScalar dt); - - void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec) - { - extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size()); - for (int i = 0; i < vec.size(); ++i) - { - extended_vec[i] = vec[i]; - } - int offset = vec.size(); - for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) - { - extended_vec[offset + i].setZero(); - } - } - - void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual) - { - extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size()); - for (int i = 0; i < residual.size(); ++i) - { - extended_residual[i] = residual[i]; - } - int offset = residual.size(); - for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) - { - const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i]; - extended_residual[offset + i].setZero(); - for (int d = 0; d < lm.m_num_constraints; ++d) - { - for (int n = 0; n < lm.m_num_nodes; ++n) - { - extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]); - } - } - } - } + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_dt; + btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + Preconditioner* m_preconditioner; + btDeformableContactProjection m_projection; + const TVStack& m_backupVelocity; + btAlignedObjectArray<btSoftBody::Node*> m_nodes; + bool m_implicit; + MassPreconditioner* m_massPreconditioner; + KKTPreconditioner* m_KKTPreconditioner; + + btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v); + + virtual ~btDeformableBackwardEulerObjective(); + + void initialize() {} + + // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual + void computeResidual(btScalar dt, TVStack& residual); + + // add explicit force to the velocity + void applyExplicitForce(TVStack& force); + + // apply force to velocity and optionally reset the force to zero + void applyForce(TVStack& force, bool setZero); + + // compute the norm of the residual + btScalar computeNorm(const TVStack& residual) const; + + // compute one step of the solve (there is only one solve if the system is linear) + void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); + + // perform A*x = b + void multiply(const TVStack& x, TVStack& b) const; + + // set initial guess for CG solve + void initialGuess(TVStack& dv, const TVStack& residual); + + // reset data structure and reset dt + void reinitialize(bool nodeUpdated, btScalar dt); + + void setDt(btScalar dt); + + // add friction force to residual + void applyDynamicFriction(TVStack& r); + + // add dv to velocity + void updateVelocity(const TVStack& dv); + + //set constraints as projections + void setConstraints(const btContactSolverInfo& infoGlobal); + + // update the projections and project the residual + void project(TVStack& r) + { + BT_PROFILE("project"); + m_projection.project(r); + } + + // perform precondition M^(-1) x = b + void precondition(const TVStack& x, TVStack& b) + { + m_preconditioner->operator()(x, b); + } + + // reindex all the vertices + virtual void updateId() + { + size_t node_id = 0; + size_t face_id = 0; + m_nodes.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].index = node_id; + m_nodes.push_back(&psb->m_nodes[j]); + ++node_id; + } + for (int j = 0; j < psb->m_faces.size(); ++j) + { + psb->m_faces[j].m_index = face_id; + ++face_id; + } + } + } + + const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const + { + return &m_nodes; + } + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } + + // Calculate the total potential energy in the system + btScalar totalEnergy(btScalar dt); + + void addLagrangeMultiplier(const TVStack& vec, TVStack& extended_vec) + { + extended_vec.resize(vec.size() + m_projection.m_lagrangeMultipliers.size()); + for (int i = 0; i < vec.size(); ++i) + { + extended_vec[i] = vec[i]; + } + int offset = vec.size(); + for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) + { + extended_vec[offset + i].setZero(); + } + } + + void addLagrangeMultiplierRHS(const TVStack& residual, const TVStack& m_dv, TVStack& extended_residual) + { + extended_residual.resize(residual.size() + m_projection.m_lagrangeMultipliers.size()); + for (int i = 0; i < residual.size(); ++i) + { + extended_residual[i] = residual[i]; + } + int offset = residual.size(); + for (int i = 0; i < m_projection.m_lagrangeMultipliers.size(); ++i) + { + const LagrangeMultiplier& lm = m_projection.m_lagrangeMultipliers[i]; + extended_residual[offset + i].setZero(); + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int n = 0; n < lm.m_num_nodes; ++n) + { + extended_residual[offset + i][d] += lm.m_weights[n] * m_dv[lm.m_indices[n]].dot(lm.m_dirs[d]); + } + } + } + } + + void calculateContactForce(const TVStack& dv, const TVStack& rhs, TVStack& f) + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + f[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : dv[counter] / node.m_im; + ++counter; + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, dv, f); + } + counter = 0; + for (; counter < f.size(); ++counter) + { + f[counter] = rhs[counter] - f[counter]; + } + } }; #endif /* btBackwardEulerObjective_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp index 132699c54f..4b11fccecb 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp @@ -18,468 +18,489 @@ #include "btDeformableBodySolver.h" #include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" -static const int kMaxConjugateGradientIterations = 50; +static const int kMaxConjugateGradientIterations = 300; btDeformableBodySolver::btDeformableBodySolver() -: m_numNodes(0) -, m_cg(kMaxConjugateGradientIterations) -, m_cr(kMaxConjugateGradientIterations) -, m_maxNewtonIterations(5) -, m_newtonTolerance(1e-4) -, m_lineSearch(false) -, m_useProjection(false) + : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) { - m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); + m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); } btDeformableBodySolver::~btDeformableBodySolver() { - delete m_objective; + delete m_objective; } void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { - BT_PROFILE("solveDeformableConstraints"); - if (!m_implicit) - { - m_objective->computeResidual(solverdt, m_residual); - m_objective->applyDynamicFriction(m_residual); - if (m_useProjection) - { - computeStep(m_dv, m_residual); - } - else - { - TVStack rhs, x; - m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs); - m_objective->addLagrangeMultiplier(m_dv, x); - m_objective->m_preconditioner->reinitialize(true); - computeStep(x, rhs); - for (int i = 0; i<m_dv.size(); ++i) - { - m_dv[i] = x[i]; - } - } - updateVelocity(); - } - else - { - for (int i = 0; i < m_maxNewtonIterations; ++i) - { - updateState(); - // add the inertia term in the residual - int counter = 0; - for (int k = 0; k < m_softBodies.size(); ++k) - { - btSoftBody* psb = m_softBodies[k]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (psb->m_nodes[j].m_im > 0) - { - m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter]; - } - ++counter; - } - } - - m_objective->computeResidual(solverdt, m_residual); - if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) - { - break; - } - // todo xuchenhan@: this really only needs to be calculated once - m_objective->applyDynamicFriction(m_residual); - if (m_lineSearch) - { - btScalar inner_product = computeDescentStep(m_ddv,m_residual); - btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 - btScalar scale = 2; - btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2; - backupDv(); - do { - scale *= beta; - if (scale < 1e-8) { - return; - } - updateEnergy(scale); - f1 = m_objective->totalEnergy(solverdt)+kineticEnergy(); - f2 = f0 - alpha * scale * inner_product; - } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues - revertDv(); - updateDv(scale); - } - else - { - computeStep(m_ddv, m_residual); - updateDv(); - } - for (int j = 0; j < m_numNodes; ++j) - { - m_ddv[j].setZero(); - m_residual[j].setZero(); - } - } - updateVelocity(); - } + BT_PROFILE("solveDeformableConstraints"); + if (!m_implicit) + { + m_objective->computeResidual(solverdt, m_residual); + m_objective->applyDynamicFriction(m_residual); + if (m_useProjection) + { + computeStep(m_dv, m_residual); + } + else + { + TVStack rhs, x; + m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs); + m_objective->addLagrangeMultiplier(m_dv, x); + m_objective->m_preconditioner->reinitialize(true); + computeStep(x, rhs); + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i] = x[i]; + } + } + updateVelocity(); + } + else + { + for (int i = 0; i < m_maxNewtonIterations; ++i) + { + updateState(); + // add the inertia term in the residual + int counter = 0; + for (int k = 0; k < m_softBodies.size(); ++k) + { + btSoftBody* psb = m_softBodies[k]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + { + m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter]; + } + ++counter; + } + } + + m_objective->computeResidual(solverdt, m_residual); + if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) + { + break; + } + // todo xuchenhan@: this really only needs to be calculated once + m_objective->applyDynamicFriction(m_residual); + if (m_lineSearch) + { + btScalar inner_product = computeDescentStep(m_ddv, m_residual); + btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 + btScalar scale = 2; + btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2; + backupDv(); + do + { + scale *= beta; + if (scale < 1e-8) + { + return; + } + updateEnergy(scale); + f1 = m_objective->totalEnergy(solverdt) + kineticEnergy(); + f2 = f0 - alpha * scale * inner_product; + } while (!(f1 < f2 + SIMD_EPSILON)); // if anything here is nan then the search continues + revertDv(); + updateDv(scale); + } + else + { + computeStep(m_ddv, m_residual); + updateDv(); + } + for (int j = 0; j < m_numNodes; ++j) + { + m_ddv[j].setZero(); + m_residual[j].setZero(); + } + } + updateVelocity(); + } } btScalar btDeformableBodySolver::kineticEnergy() { - btScalar ke = 0; - for (int i = 0; i < m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size();++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - if (node.m_im > 0) - { - ke += m_dv[node.index].length2() * 0.5 / node.m_im; - } - } - } - return ke; + btScalar ke = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + ke += m_dv[node.index].length2() * 0.5 / node.m_im; + } + } + } + return ke; } void btDeformableBodySolver::backupDv() { - m_backup_dv.resize(m_dv.size()); - for (int i = 0; i<m_backup_dv.size(); ++i) - { - m_backup_dv[i] = m_dv[i]; - } + m_backup_dv.resize(m_dv.size()); + for (int i = 0; i < m_backup_dv.size(); ++i) + { + m_backup_dv[i] = m_dv[i]; + } } void btDeformableBodySolver::revertDv() { - for (int i = 0; i<m_backup_dv.size(); ++i) - { - m_dv[i] = m_backup_dv[i]; - } + for (int i = 0; i < m_backup_dv.size(); ++i) + { + m_dv[i] = m_backup_dv[i]; + } } void btDeformableBodySolver::updateEnergy(btScalar scale) { - for (int i = 0; i<m_dv.size(); ++i) - { - m_dv[i] = m_backup_dv[i] + scale * m_ddv[i]; - } - updateState(); + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i] = m_backup_dv[i] + scale * m_ddv[i]; + } + updateState(); } - btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose) { - m_cg.solve(*m_objective, ddv, residual, false); - btScalar inner_product = m_cg.dot(residual, m_ddv); - btScalar res_norm = m_objective->computeNorm(residual); - btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); - if (inner_product < -tol) - { - if (verbose) - { - std::cout << "Looking backwards!" << std::endl; - } - for (int i = 0; i < m_ddv.size();++i) - { - m_ddv[i] = -m_ddv[i]; - } - inner_product = -inner_product; - } - else if (std::abs(inner_product) < tol) - { - if (verbose) - { - std::cout << "Gradient Descent!" << std::endl; - } - btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; - for (int i = 0; i < m_ddv.size();++i) - { - m_ddv[i] = scale * residual[i]; - } - inner_product = scale * res_norm * res_norm; - } - return inner_product; + m_cg.solve(*m_objective, ddv, residual, false); + btScalar inner_product = m_cg.dot(residual, m_ddv); + btScalar res_norm = m_objective->computeNorm(residual); + btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); + if (inner_product < -tol) + { + if (verbose) + { + std::cout << "Looking backwards!" << std::endl; + } + for (int i = 0; i < m_ddv.size(); ++i) + { + m_ddv[i] = -m_ddv[i]; + } + inner_product = -inner_product; + } + else if (std::abs(inner_product) < tol) + { + if (verbose) + { + std::cout << "Gradient Descent!" << std::endl; + } + btScalar scale = m_objective->computeNorm(m_ddv) / res_norm; + for (int i = 0; i < m_ddv.size(); ++i) + { + m_ddv[i] = scale * residual[i]; + } + inner_product = scale * res_norm * res_norm; + } + return inner_product; } void btDeformableBodySolver::updateState() { - updateVelocity(); - updateTempPosition(); + updateVelocity(); + updateTempPosition(); } void btDeformableBodySolver::updateDv(btScalar scale) { - for (int i = 0; i < m_numNodes; ++i) - { - m_dv[i] += scale * m_ddv[i]; - } + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i] += scale * m_ddv[i]; + } } void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - if (m_useProjection) - m_cg.solve(*m_objective, ddv, residual, false); - else - m_cr.solve(*m_objective, ddv, residual, false); + if (m_useProjection) + m_cg.solve(*m_objective, ddv, residual, false); + else + m_cr.solve(*m_objective, ddv, residual, false); } -void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt) +void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt) { - m_softBodies.copyFromArray(softBodies); - bool nodeUpdated = updateNodes(); - - if (nodeUpdated) - { - m_dv.resize(m_numNodes, btVector3(0,0,0)); - m_ddv.resize(m_numNodes, btVector3(0,0,0)); - m_residual.resize(m_numNodes, btVector3(0,0,0)); - m_backupVelocity.resize(m_numNodes, btVector3(0,0,0)); - } - - // need to setZero here as resize only set value for newly allocated items - for (int i = 0; i < m_numNodes; ++i) - { - m_dv[i].setZero(); - m_ddv[i].setZero(); - m_residual[i].setZero(); - } - - m_dt = dt; - m_objective->reinitialize(nodeUpdated, dt); - updateSoftBodies(); -} + m_softBodies.copyFromArray(softBodies); + bool nodeUpdated = updateNodes(); -void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) -{ - BT_PROFILE("setConstraint"); - m_objective->setConstraints(infoGlobal); + if (nodeUpdated) + { + m_dv.resize(m_numNodes, btVector3(0, 0, 0)); + m_ddv.resize(m_numNodes, btVector3(0, 0, 0)); + m_residual.resize(m_numNodes, btVector3(0, 0, 0)); + m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0)); + } + + // need to setZero here as resize only set value for newly allocated items + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i].setZero(); + m_ddv[i].setZero(); + m_residual[i].setZero(); + } + + if (dt > 0) + { + m_dt = dt; + } + m_objective->reinitialize(nodeUpdated, dt); + updateSoftBodies(); } -btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) +void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) { - BT_PROFILE("solveContactConstraints"); - btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal); - return maxSquaredResidual; + BT_PROFILE("setConstraint"); + m_objective->setConstraints(infoGlobal); } -void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { - m_objective->m_projection.splitImpulseSetup(infoGlobal); + BT_PROFILE("solveContactConstraints"); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal); + return maxSquaredResidual; } void btDeformableBodySolver::updateVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - psb->m_maxSpeedSquared = 0; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - // set NaN to zero; - if (m_dv[counter] != m_dv[counter]) - { - m_dv[counter].setZero(); - } - psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; - psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); - ++counter; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + psb->m_maxSpeedSquared = 0; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // set NaN to zero; + if (m_dv[counter] != m_dv[counter]) + { + m_dv[counter].setZero(); + } + if (m_implicit) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter]; + } + else + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; + } + psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); + ++counter; + } + } } void btDeformableBodySolver::updateTempPosition() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v; - ++counter; - } - psb->updateDeformation(); - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv); + ++counter; + } + psb->updateDeformation(); + } } void btDeformableBodySolver::backupVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - m_backupVelocity[counter++] = psb->m_nodes[j].m_v; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_backupVelocity[counter++] = psb->m_nodes[j].m_v; + } + } } void btDeformableBodySolver::setupDeformableSolve(bool implicit) { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - counter += psb->m_nodes.size(); - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (implicit) - { - if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON) - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; - else - m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; - m_backupVelocity[counter] = psb->m_nodes[j].m_vn; - } - else - { - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; - } - psb->m_nodes[j].m_v = m_backupVelocity[counter]; - ++counter; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + counter += psb->m_nodes.size(); + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (implicit) + { + // setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint. + if (psb->m_nodes[j].m_v == m_backupVelocity[counter]) + m_dv[counter].setZero(); + else + m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; + m_backupVelocity[counter] = psb->m_nodes[j].m_vn; + } + else + { + m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter]; + } + psb->m_nodes[j].m_v = m_backupVelocity[counter]; + ++counter; + } + } } void btDeformableBodySolver::revertVelocity() { - int counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_v = m_backupVelocity[counter++]; - } - } + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter++]; + } + } } bool btDeformableBodySolver::updateNodes() { - int numNodes = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - numNodes += m_softBodies[i]->m_nodes.size(); - if (numNodes != m_numNodes) - { - m_numNodes = numNodes; - return true; - } - return false; + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + numNodes += m_softBodies[i]->m_nodes.size(); + if (numNodes != m_numNodes) + { + m_numNodes = numNodes; + return true; + } + return false; } - void btDeformableBodySolver::predictMotion(btScalar solverdt) { - // apply explicit forces to velocity - m_objective->applyExplicitForce(m_residual); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody *psb = m_softBodies[i]; - - if (psb->isActive()) - { - // predict motion for collision detection - predictDeformableMotion(psb, solverdt); - } - } + // apply explicit forces to velocity + if (m_implicit) + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt; + } + } + } + } + m_objective->applyExplicitForce(m_residual); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + + if (psb->isActive()) + { + // predict motion for collision detection + predictDeformableMotion(psb, solverdt); + } + } } void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt) { - BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); - int i, ni; - - /* Update */ - if (psb->m_bUpdateRtCst) - { - psb->m_bUpdateRtCst = false; - psb->updateConstants(); - psb->m_fdbvt.clear(); - if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD) - { - psb->initializeFaceTree(); - } - } - - /* Prepare */ - psb->m_sst.sdt = dt * psb->m_cfg.timescale; - psb->m_sst.isdt = 1 / psb->m_sst.sdt; - psb->m_sst.velmrg = psb->m_sst.sdt * 3; - psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); - psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; - /* Bounds */ - psb->updateBounds(); - - /* Integrate */ - // do not allow particles to move more than the bounding box size - btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt; - for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) - { - btSoftBody::Node& n = psb->m_nodes[i]; - // apply drag - n.m_v *= (1 - psb->m_cfg.drag); - // scale velocity back - if (n.m_v.norm() > max_v) - { - n.m_v.safeNormalize(); - n.m_v *= max_v; - } - n.m_q = n.m_x + n.m_v * dt; - n.m_penetration = 0; - } - - /* Nodes */ - psb->updateNodeTree(true, true); - if (!psb->m_fdbvt.empty()) - { - psb->updateFaceTree(true, true); - } - /* Clear contacts */ - psb->m_nodeRigidContacts.resize(0); - psb->m_faceRigidContacts.resize(0); - psb->m_faceNodeContacts.resize(0); - /* Optimize dbvt's */ -// psb->m_ndbvt.optimizeIncremental(1); -// psb->m_fdbvt.optimizeIncremental(1); -} + BT_PROFILE("btDeformableBodySolver::predictDeformableMotion"); + int i, ni; + + /* Update */ + if (psb->m_bUpdateRtCst) + { + psb->m_bUpdateRtCst = false; + psb->updateConstants(); + psb->m_fdbvt.clear(); + if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD) + { + psb->initializeFaceTree(); + } + } + /* Prepare */ + psb->m_sst.sdt = dt * psb->m_cfg.timescale; + psb->m_sst.isdt = 1 / psb->m_sst.sdt; + psb->m_sst.velmrg = psb->m_sst.sdt * 3; + psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); + psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; + /* Bounds */ + psb->updateBounds(); + + /* Integrate */ + // do not allow particles to move more than the bounding box size + btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt; + for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + // apply drag + n.m_v *= (1 - psb->m_cfg.drag); + // scale velocity back + if (m_implicit) + { + n.m_q = n.m_x; + } + else + { + if (n.m_v.norm() > max_v) + { + n.m_v.safeNormalize(); + n.m_v *= max_v; + } + n.m_q = n.m_x + n.m_v * dt; + } + n.m_splitv.setZero(); + n.m_constrained = false; + } + + /* Nodes */ + psb->updateNodeTree(true, true); + if (!psb->m_fdbvt.empty()) + { + psb->updateFaceTree(true, true); + } + /* Clear contacts */ + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); + psb->m_faceNodeContacts.resize(0); + /* Optimize dbvt's */ + // psb->m_ndbvt.optimizeIncremental(1); + // psb->m_fdbvt.optimizeIncremental(1); +} void btDeformableBodySolver::updateSoftBodies() { - BT_PROFILE("updateSoftBodies"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody *psb = (btSoftBody *)m_softBodies[i]; - if (psb->isActive()) - { - psb->updateNormals(); - } - } + BT_PROFILE("updateSoftBodies"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; + if (psb->isActive()) + { + psb->updateNormals(); + } + } } void btDeformableBodySolver::setImplicit(bool implicit) { - m_implicit = implicit; - m_objective->setImplicit(implicit); + m_implicit = implicit; + m_objective->setImplicit(implicit); } void btDeformableBodySolver::setLineSearch(bool lineSearch) { - m_lineSearch = lineSearch; + m_lineSearch = lineSearch; } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h index d4e5f4c603..ae674d6e89 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h @@ -16,7 +16,6 @@ #ifndef BT_DEFORMABLE_BODY_SOLVERS_H #define BT_DEFORMABLE_BODY_SOLVERS_H - #include "btSoftBodySolvers.h" #include "btDeformableBackwardEulerObjective.h" #include "btDeformableMultiBodyDynamicsWorld.h" @@ -30,133 +29,132 @@ class btDeformableMultiBodyDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { - typedef btAlignedObjectArray<btVector3> TVStack; + typedef btAlignedObjectArray<btVector3> TVStack; + protected: - int m_numNodes; // total number of deformable body nodes - TVStack m_dv; // v_{n+1} - v_n - TVStack m_backup_dv; // backed up dv - TVStack m_ddv; // incremental dv - TVStack m_residual; // rhs of the linear solve - btAlignedObjectArray<btSoftBody *> m_softBodies; // all deformable bodies - TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit - btScalar m_dt; // dt - btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver - btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver - bool m_implicit; // use implicit scheme if true, explicit scheme if false - int m_maxNewtonIterations; // max number of newton iterations - btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance - bool m_lineSearch; // If true, use newton's method with line search under implicit scheme + int m_numNodes; // total number of deformable body nodes + TVStack m_dv; // v_{n+1} - v_n + TVStack m_backup_dv; // backed up dv + TVStack m_ddv; // incremental dv + TVStack m_residual; // rhs of the linear solve + btAlignedObjectArray<btSoftBody*> m_softBodies; // all deformable bodies + TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit + btScalar m_dt; // dt + btConjugateGradient<btDeformableBackwardEulerObjective> m_cg; // CG solver + btConjugateResidual<btDeformableBackwardEulerObjective> m_cr; // CR solver + bool m_implicit; // use implicit scheme if true, explicit scheme if false + int m_maxNewtonIterations; // max number of newton iterations + btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance + bool m_lineSearch; // If true, use newton's method with line search under implicit scheme public: - // handles data related to objective function - btDeformableBackwardEulerObjective* m_objective; - bool m_useProjection; - - btDeformableBodySolver(); - - virtual ~btDeformableBodySolver(); - - virtual SolverTypes getSolverType() const - { - return DEFORMABLE_SOLVER; - } - - // update soft body normals - virtual void updateSoftBodies(); - - virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); - - // solve the momentum equation - virtual void solveDeformableConstraints(btScalar solverdt); - - // set up the position error in split impulse - void splitImpulseSetup(const btContactSolverInfo& infoGlobal); - - // resize/clear data structures - void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt); - - // set up contact constraints - void setConstraints(const btContactSolverInfo& infoGlobal); - - // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion - virtual void predictMotion(btScalar solverdt); - - // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* - // x_{n+1}^* is stored in m_q - void predictDeformableMotion(btSoftBody* psb, btScalar dt); - - // save the current velocity to m_backupVelocity - void backupVelocity(); - - // set m_dv and m_backupVelocity to desired value to prepare for momentum solve - void setupDeformableSolve(bool implicit); - - // set the current velocity to that backed up in m_backupVelocity - void revertVelocity(); - - // set velocity to m_dv + m_backupVelocity - void updateVelocity(); - - // update the node count - bool updateNodes(); - - // calculate the change in dv resulting from the momentum solve - void computeStep(TVStack& ddv, const TVStack& residual); - - // calculate the change in dv resulting from the momentum solve when line search is turned on - btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose=false); - - virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} - - // process collision between deformable and rigid - virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap) - { - softBody->defaultCollisionHandler(collisionObjectWrap); - } - - // process collision between deformable and deformable - virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { - softBody->defaultCollisionHandler(otherSoftBody); - } - - // If true, implicit time stepping scheme is used. - // Otherwise, explicit time stepping scheme is used - void setImplicit(bool implicit); - - // If true, newton's method with line search is used when implicit time stepping scheme is turned on - void setLineSearch(bool lineSearch); - - // set temporary position x^* = x_n + dt * v - // update the deformation gradient at position x^* - void updateState(); - - // set dv = dv + scale * ddv - void updateDv(btScalar scale = 1); - - // set temporary position x^* = x_n + dt * v^* - void updateTempPosition(); - - // save the current dv to m_backup_dv; - void backupDv(); - - // set dv to the backed-up value - void revertDv(); - - // set dv = dv + scale * ddv - // set v^* = v_n + dv - // set temporary position x^* = x_n + dt * v^* - // update the deformation gradient at position x^* - void updateEnergy(btScalar scale); - - // calculates the appropriately scaled kinetic energy in the system, which is - // 1/2 * dv^T * M * dv - // used in line search - btScalar kineticEnergy(); - - // unused functions - virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){} - virtual void solveConstraints(btScalar dt){} - virtual bool checkInitialized(){return true;} - virtual void copyBackToSoftBodies(bool bMove = true) {} + // handles data related to objective function + btDeformableBackwardEulerObjective* m_objective; + bool m_useProjection; + + btDeformableBodySolver(); + + virtual ~btDeformableBodySolver(); + + virtual SolverTypes getSolverType() const + { + return DEFORMABLE_SOLVER; + } + + // update soft body normals + virtual void updateSoftBodies(); + + virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // solve the momentum equation + virtual void solveDeformableConstraints(btScalar solverdt); + + // resize/clear data structures + void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt); + + // set up contact constraints + void setConstraints(const btContactSolverInfo& infoGlobal); + + // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion + virtual void predictMotion(btScalar solverdt); + + // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* + // x_{n+1}^* is stored in m_q + void predictDeformableMotion(btSoftBody* psb, btScalar dt); + + // save the current velocity to m_backupVelocity + void backupVelocity(); + + // set m_dv and m_backupVelocity to desired value to prepare for momentum solve + void setupDeformableSolve(bool implicit); + + // set the current velocity to that backed up in m_backupVelocity + void revertVelocity(); + + // set velocity to m_dv + m_backupVelocity + void updateVelocity(); + + // update the node count + bool updateNodes(); + + // calculate the change in dv resulting from the momentum solve + void computeStep(TVStack& ddv, const TVStack& residual); + + // calculate the change in dv resulting from the momentum solve when line search is turned on + btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose = false); + + virtual void copySoftBodyToVertexBuffer(const btSoftBody* const softBody, btVertexBufferDescriptor* vertexBuffer) {} + + // process collision between deformable and rigid + virtual void processCollision(btSoftBody* softBody, const btCollisionObjectWrapper* collisionObjectWrap) + { + softBody->defaultCollisionHandler(collisionObjectWrap); + } + + // process collision between deformable and deformable + virtual void processCollision(btSoftBody* softBody, btSoftBody* otherSoftBody) + { + softBody->defaultCollisionHandler(otherSoftBody); + } + + // If true, implicit time stepping scheme is used. + // Otherwise, explicit time stepping scheme is used + void setImplicit(bool implicit); + + // If true, newton's method with line search is used when implicit time stepping scheme is turned on + void setLineSearch(bool lineSearch); + + // set temporary position x^* = x_n + dt * v + // update the deformation gradient at position x^* + void updateState(); + + // set dv = dv + scale * ddv + void updateDv(btScalar scale = 1); + + // set temporary position x^* = x_n + dt * v^* + void updateTempPosition(); + + // save the current dv to m_backup_dv; + void backupDv(); + + // set dv to the backed-up value + void revertDv(); + + // set dv = dv + scale * ddv + // set v^* = v_n + dv + // set temporary position x^* = x_n + dt * v^* + // update the deformation gradient at position x^* + void updateEnergy(btScalar scale); + + // calculates the appropriately scaled kinetic energy in the system, which is + // 1/2 * dv^T * M * dv + // used in line search + btScalar kineticEnergy(); + + // unused functions + virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {} + virtual void solveConstraints(btScalar dt) {} + virtual bool checkInitialized() { return true; } + virtual void copyBackToSoftBodies(bool bMove = true) {} }; #endif /* btDeformableBodySolver_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp index 2864446de6..09398d79a5 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp @@ -16,387 +16,503 @@ #include "btDeformableContactConstraint.h" /* ================ Deformable Node Anchor =================== */ btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal) -: m_anchor(&a) -, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) + : m_anchor(&a), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) { } btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other) -: m_anchor(other.m_anchor) -, btDeformableContactConstraint(other) + : m_anchor(other.m_anchor), btDeformableContactConstraint(other) { } btVector3 btDeformableNodeAnchorConstraint::getVa() const { - const btSoftBody::sCti& cti = m_anchor->m_cti; - btVector3 va(0, 0, 0); - if (cti.m_colObj->hasContactResponse()) - { - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; - const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_n[k]; - } - va = cti.m_normal * vel; - // add in the tangential components of the va - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t1[k]; - } - va += m_anchor->t1 * vel; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t2[k]; - } - va += m_anchor->t2 * vel; - } - } - } - return va; + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t1[k]; + } + va += m_anchor->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t2[k]; + } + va += m_anchor->t2 * vel; + } + } + } + return va; } btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - const btSoftBody::sCti& cti = m_anchor->m_cti; - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = (vb - va); - // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 - const btScalar dn = btDot(vr, vr); - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_anchor->m_c0 * vr; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - - // apply impulse to the rigid/multibodies involved and change their velocities - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - btRigidBody* rigidCol = 0; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - if (rigidCol) - { - rigidCol->applyImpulse(impulse, m_anchor->m_c1); - } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = 0; - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); - const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); - } - } - return residualSquare; + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = (vb - va); + // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 + const btScalar dn = btDot(vr, vr); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn * dn; + btVector3 impulse = m_anchor->m_c0 * vr; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_anchor->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); + const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); + } + } + return residualSquare; } btVector3 btDeformableNodeAnchorConstraint::getVb() const { - return m_anchor->m_node->m_v; + return m_anchor->m_node->m_v; } void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) { - btVector3 dv = impulse * m_anchor->m_c2; - m_anchor->m_node->m_v -= dv; + btVector3 dv = impulse * m_anchor->m_c2; + m_anchor->m_node->m_v -= dv; } /* ================ Deformable vs. Rigid =================== */ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal) -: m_contact(&c) -, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) + : m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) { - m_total_normal_dv.setZero(); - m_total_tangent_dv.setZero(); - // The magnitude of penetration is the depth of penetration. - m_penetration = c.m_cti.m_offset; -// m_penetration = btMin(btScalar(0),c.m_cti.m_offset); + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); + // The magnitude of penetration is the depth of penetration. + m_penetration = c.m_cti.m_offset; + m_total_split_impulse = 0; + m_binding = false; } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) -: m_contact(other.m_contact) -, btDeformableContactConstraint(other) -, m_penetration(other.m_penetration) + : m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding) { - m_total_normal_dv = other.m_total_normal_dv; - m_total_tangent_dv = other.m_total_tangent_dv; + m_total_normal_dv = other.m_total_normal_dv; + m_total_tangent_dv = other.m_total_tangent_dv; } - btVector3 btDeformableRigidContactConstraint::getVa() const { - const btSoftBody::sCti& cti = m_contact->m_cti; - btVector3 va(0, 0, 0); - if (cti.m_colObj->hasContactResponse()) - { - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; - const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_n[k]; - } - va = cti.m_normal * vel; - // add in the tangential components of the va - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t1[k]; - } - va += m_contact->t1 * vel; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[k]) * J_t2[k]; - } - va += m_contact->t2 * vel; - } - } - } - return va; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k] + local_dv[k]) * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; +} + +btVector3 btDeformableRigidContactConstraint::getSplitVa() const +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_split_v[k] * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; } btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - const btSoftBody::sCti& cti = m_contact->m_cti; - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = vb - va; - btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ; - const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - btVector3 old_total_tangent_dv = m_total_tangent_dv; - // m_c2 is the inverse mass of the deformable node/face - m_total_normal_dv -= impulse_normal * m_contact->m_c2; - m_total_tangent_dv -= impulse_tangent * m_contact->m_c2; - - if (m_total_normal_dv.dot(cti.m_normal) < 0) - { - // separating in the normal direction - m_static = false; - m_total_tangent_dv = btVector3(0,0,0); - impulse_tangent.setZero(); - } - else - { - if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) - { - // dynamic friction - // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. - m_static = false; - if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) - { - m_total_tangent_dv = btVector3(0,0,0); - } - else - { - m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; - } - impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); - } - else - { - // static friction - m_static = true; - } - } - impulse = impulse_normal + impulse_tangent; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - if (residualSquare < 1e-7) - return residualSquare; - // apply impulse to the rigid/multibodies involved and change their velocities - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - btRigidBody* rigidCol = 0; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - if (rigidCol) - { - rigidCol->applyImpulse(impulse, m_contact->m_c1); - } - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = 0; - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - if (impulse_tangent.norm() > SIMD_EPSILON) - { - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); - const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); - } - } - } -// va = getVa(); -// vb = getVb(); -// vr = vb - va; -// btScalar dn1 = btDot(vr, cti.m_normal) / 150; -// m_penetration += dn1; - return residualSquare; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm; + if (m_penetration > 0) + { + dn += m_penetration / infoGlobal.m_timeStep; + } + if (!infoGlobal.m_splitImpulse) + { + dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + } + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0))); + if (!infoGlobal.m_splitImpulse) + { + impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); + } + btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + if (dn > 0) + { + return 0; + } + m_binding = true; + btScalar residualSquare = dn * dn; + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c5 is the inverse mass of the deformable node/face + m_total_normal_dv -= m_contact->m_c5 * impulse_normal; + m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent; + + if (m_total_normal_dv.dot(cti.m_normal) < 0) + { + // separating in the normal direction + m_binding = false; + m_static = false; + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0, 0, 0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; + } + // impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); + impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + if (impulse_tangent.norm() > SIMD_EPSILON) + { + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); + const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); + } + } + } + return residualSquare; +} + +btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +{ + btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction; + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 vb = getSplitVb(); + btVector3 va = getSplitVa(); + btScalar p = m_penetration; + if (p > 0) + { + return 0; + } + btVector3 vr = vb - va; + btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + if (dn > 0) + { + return 0; + } + if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION) + { + dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION) + { + dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + m_total_split_impulse += dn; + + btScalar residualSquare = dn * dn; + const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn); + applySplitImpulse(impulse); + + // apply split impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyPushImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); + } + } + return residualSquare; } /* ================ Node vs. Rigid =================== */ btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal) - : m_node(contact.m_node) - , btDeformableRigidContactConstraint(contact, infoGlobal) - { - } + : m_node(contact.m_node), btDeformableRigidContactConstraint(contact, infoGlobal) +{ +} btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other) -: m_node(other.m_node) -, btDeformableRigidContactConstraint(other) + : m_node(other.m_node), btDeformableRigidContactConstraint(other) { } btVector3 btDeformableNodeRigidContactConstraint::getVb() const { - return m_node->m_v; + return m_node->m_v; } +btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const +{ + return m_node->m_splitv; +} btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const { - return m_total_normal_dv + m_total_tangent_dv; + return m_total_normal_dv + m_total_tangent_dv; } void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableNodeRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - contact->m_node->m_v -= dv; + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = contact->m_c5 * impulse; + contact->m_node->m_v -= dv; +} + +void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = contact->m_c5 * impulse; + contact->m_node->m_splitv -= dv; } /* ================ Face vs. Rigid =================== */ btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting) -: m_face(contact.m_face) -, m_useStrainLimiting(useStrainLimiting) -, btDeformableRigidContactConstraint(contact, infoGlobal) + : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal) { } btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other) -: m_face(other.m_face) -, m_useStrainLimiting(other.m_useStrainLimiting) -, btDeformableRigidContactConstraint(other) + : m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other) { } btVector3 btDeformableFaceRigidContactConstraint::getVb() const { - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; - return vb; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; } - btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const { - btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - if (m_face->m_n[0] == node) - { - return face_dv * contact->m_weights[0]; - } - if (m_face->m_n[1] == node) - { - return face_dv * contact->m_weights[1]; - } - btAssert(node == m_face->m_n[2]); - return face_dv * contact->m_weights[2]; + btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + if (m_face->m_n[0] == node) + { + return face_dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == node) + { + return face_dv * contact->m_weights[1]; + } + btAssert(node == m_face->m_n[2]); + return face_dv * contact->m_weights[2]; } void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - btSoftBody::Face* face = contact->m_face; - - btVector3& v0 = face->m_n[0]->m_v; - btVector3& v1 = face->m_n[1]->m_v; - btVector3& v2 = face->m_n[2]->m_v; - const btScalar& im0 = face->m_n[0]->m_im; - const btScalar& im1 = face->m_n[1]->m_im; - const btScalar& im2 = face->m_n[2]->m_im; - if (im0 > 0) - v0 -= dv * contact->m_weights[0]; - if (im1 > 0) - v1 -= dv * contact->m_weights[1]; - if (im2 > 0) - v2 -= dv * contact->m_weights[2]; + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + v0 -= dv * contact->m_weights[0]; + if (im1 > 0) + v1 -= dv * contact->m_weights[1]; + if (im2 > 0) + v2 -= dv * contact->m_weights[2]; if (m_useStrainLimiting) { - btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations); - btScalar m01 = (relaxation/(im0 + im1)); - btScalar m02 = (relaxation/(im0 + im2)); - btScalar m12 = (relaxation/(im1 + im2)); - #ifdef USE_STRAIN_RATE_LIMITING + btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations); + btScalar m01 = (relaxation / (im0 + im1)); + btScalar m02 = (relaxation / (im0 + im2)); + btScalar m12 = (relaxation / (im1 + im2)); +#ifdef USE_STRAIN_RATE_LIMITING // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. btScalar p = 0.01; btVector3& x0 = face->m_n[0]->m_x; btVector3& x1 = face->m_n[1]->m_x; btVector3& x2 = face->m_n[2]->m_x; - const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1}; - const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1}; + const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1}; + const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1}; btVector3 u[3]; btScalar x_diff_dot_u, dn[3]; btScalar dt = m_infoGlobal->m_timeStep; @@ -404,172 +520,201 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul { btScalar x_diff_norm = x_diff[i].safeNorm(); btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); - btScalar strainRate = x_diff_norm_new/x_diff_norm; + btScalar strainRate = x_diff_norm_new / x_diff_norm; u[i] = v_diff[i]; u[i].safeNormalize(); - if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p)) + if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p)) { dn[i] = 0; continue; } x_diff_dot_u = btDot(x_diff[i], u[i]); btScalar s; - if (1-p > strainRate) + if (1 - p > strainRate) { - s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm)); + s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm)); } else { - s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm)); + s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm)); } // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); // strainRate = x_diff_norm_new/x_diff_norm; dn[i] = s - v_diff[i].safeNorm(); } - btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1])); - btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2])); - btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1])); - #else + btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1])); + btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2])); + btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1])); +#else // apply strain limiting to prevent undamped modes - btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); - btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); - btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); - #endif + btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0)); + btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1)); + btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2)); +#endif v0 += dv0; v1 += dv1; v2 += dv2; } } +btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2]; + return vb; +} + +void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_splitv; + btVector3& v1 = face->m_n[1]->m_splitv; + btVector3& v2 = face->m_n[2]->m_splitv; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dv * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dv * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dv * contact->m_weights[2]; + } +} + /* ================ Face vs. Node =================== */ btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal) -: m_node(contact.m_node) -, m_face(contact.m_face) -, m_contact(&contact) -, btDeformableContactConstraint(contact.m_normal, infoGlobal) + : m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal) { - m_total_normal_dv.setZero(); - m_total_tangent_dv.setZero(); + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); } btVector3 btDeformableFaceNodeContactConstraint::getVa() const { - return m_node->m_v; + return m_node->m_v; } btVector3 btDeformableFaceNodeContactConstraint::getVb() const { - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; - return vb; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; } btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const { - btVector3 dv = m_total_normal_dv + m_total_tangent_dv; - if (n == m_node) - return dv; - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - if (m_face->m_n[0] == n) - { - return dv * contact->m_weights[0]; - } - if (m_face->m_n[1] == n) - { - return dv * contact->m_weights[1]; - } - btAssert(n == m_face->m_n[2]); - return dv * contact->m_weights[2]; + btVector3 dv = m_total_normal_dv + m_total_tangent_dv; + if (n == m_node) + return dv; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + if (m_face->m_n[0] == n) + { + return dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == n) + { + return dv * contact->m_weights[1]; + } + btAssert(n == m_face->m_n[2]); + return dv * contact->m_weights[2]; } btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { - btVector3 va = getVa(); - btVector3 vb = getVb(); - btVector3 vr = vb - va; - const btScalar dn = btDot(vr, m_contact->m_normal); - // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btScalar residualSquare = dn*dn; - btVector3 impulse = m_contact->m_c0 * vr; - const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - - btVector3 old_total_tangent_dv = m_total_tangent_dv; - // m_c2 is the inverse mass of the deformable node/face - if (m_node->m_im > 0) - { - m_total_normal_dv -= impulse_normal * m_node->m_im; - m_total_tangent_dv -= impulse_tangent * m_node->m_im; - } - else - { - m_total_normal_dv -= impulse_normal * m_contact->m_imf; - m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; - } - - if (m_total_normal_dv.dot(m_contact->m_normal) > 0) - { - // separating in the normal direction - m_static = false; - m_total_tangent_dv = btVector3(0,0,0); - impulse_tangent.setZero(); - } - else - { - if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm()) - { - // dynamic friction - // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. - m_static = false; - if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) - { - m_total_tangent_dv = btVector3(0,0,0); - } - else - { - m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction; - } - impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); - } - else - { - // static friction - m_static = true; - } - } - impulse = impulse_normal + impulse_tangent; - // apply impulse to deformable nodes involved and change their velocities - applyImpulse(impulse); - return residualSquare; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + const btScalar dn = btDot(vr, m_contact->m_normal); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn * dn; + btVector3 impulse = m_contact->m_c0 * vr; + const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c2 is the inverse mass of the deformable node/face + if (m_node->m_im > 0) + { + m_total_normal_dv -= impulse_normal * m_node->m_im; + m_total_tangent_dv -= impulse_tangent * m_node->m_im; + } + else + { + m_total_normal_dv -= impulse_normal * m_contact->m_imf; + m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; + } + + if (m_total_normal_dv.dot(m_contact->m_normal) > 0) + { + // separating in the normal direction + m_static = false; + m_total_tangent_dv = btVector3(0, 0, 0); + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm()) + { + // dynamic friction + // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations. + m_static = false; + if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0, 0, 0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction; + } + impulse_tangent = -btScalar(1) / m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + return residualSquare; } void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse) { - const btSoftBody::DeformableFaceNodeContact* contact = getContact(); - btVector3 dva = impulse * contact->m_node->m_im; - btVector3 dvb = impulse * contact->m_imf; - if (contact->m_node->m_im > 0) - { - contact->m_node->m_v += dva; - } - - btSoftBody::Face* face = contact->m_face; - btVector3& v0 = face->m_n[0]->m_v; - btVector3& v1 = face->m_n[1]->m_v; - btVector3& v2 = face->m_n[2]->m_v; - const btScalar& im0 = face->m_n[0]->m_im; - const btScalar& im1 = face->m_n[1]->m_im; - const btScalar& im2 = face->m_n[2]->m_im; - if (im0 > 0) - { - v0 -= dvb * contact->m_weights[0]; - } - if (im1 > 0) - { - v1 -= dvb * contact->m_weights[1]; - } - if (im2 > 0) - { - v2 -= dvb * contact->m_weights[2]; - } + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 dva = impulse * contact->m_node->m_im; + btVector3 dvb = impulse * contact->m_imf; + if (contact->m_node->m_im > 0) + { + contact->m_node->m_v += dva; + } + + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dvb * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dvb * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dvb * contact->m_weights[2]; + } } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h index 9f9d5bf0a3..1e2c9f5bce 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h @@ -21,51 +21,49 @@ class btDeformableContactConstraint { public: - // True if the friction is static - // False if the friction is dynamic - bool m_static; + // True if the friction is static + // False if the friction is dynamic + bool m_static; const btContactSolverInfo* m_infoGlobal; // normal of the contact btVector3 m_normal; - btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) + btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) { } - btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) + btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) { } - - btDeformableContactConstraint(){} + + btDeformableContactConstraint() {} btDeformableContactConstraint(const btDeformableContactConstraint& other) - : m_static(other.m_static) - , m_normal(other.m_normal) - , m_infoGlobal(other.m_infoGlobal) + : m_static(other.m_static), m_normal(other.m_normal), m_infoGlobal(other.m_infoGlobal) { } - virtual ~btDeformableContactConstraint(){} - - // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence - // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; - - // get the velocity of the object A in the contact - virtual btVector3 getVa() const = 0; - - // get the velocity of the object B in the contact - virtual btVector3 getVb() const = 0; - - // get the velocity change of the soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const = 0; - - // apply impulse to the soft body node and/or face involved - virtual void applyImpulse(const btVector3& impulse) = 0; - - // scale the penetration depth by erp - virtual void setPenetrationScale(btScalar scale) = 0; + virtual ~btDeformableContactConstraint() {} + + // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence + // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const = 0; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const = 0; + + // get the velocity change of the soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const = 0; + + // apply impulse to the soft body node and/or face involved + virtual void applyImpulse(const btVector3& impulse) = 0; + + // scale the penetration depth by erp + virtual void setPenetrationScale(btScalar scale) = 0; }; // @@ -73,42 +71,41 @@ public: class btDeformableStaticConstraint : public btDeformableContactConstraint { public: - btSoftBody::Node* m_node; - - btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal) - { - } - btDeformableStaticConstraint(){} - btDeformableStaticConstraint(const btDeformableStaticConstraint& other) - : m_node(other.m_node) - , btDeformableContactConstraint(other) - { - } - - virtual ~btDeformableStaticConstraint(){} - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) - { - return 0; - } - - virtual btVector3 getVa() const - { - return btVector3(0,0,0); - } - - virtual btVector3 getVb() const - { - return btVector3(0,0,0); - } - - virtual btVector3 getDv(const btSoftBody::Node* n) const - { - return btVector3(0,0,0); - } - - virtual void applyImpulse(const btVector3& impulse){} - virtual void setPenetrationScale(btScalar scale){} + btSoftBody::Node* m_node; + + btDeformableStaticConstraint(btSoftBody::Node* node, const btContactSolverInfo& infoGlobal) : m_node(node), btDeformableContactConstraint(false, btVector3(0, 0, 0), infoGlobal) + { + } + btDeformableStaticConstraint() {} + btDeformableStaticConstraint(const btDeformableStaticConstraint& other) + : m_node(other.m_node), btDeformableContactConstraint(other) + { + } + + virtual ~btDeformableStaticConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) + { + return 0; + } + + virtual btVector3 getVa() const + { + return btVector3(0, 0, 0); + } + + virtual btVector3 getVb() const + { + return btVector3(0, 0, 0); + } + + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0, 0, 0); + } + + virtual void applyImpulse(const btVector3& impulse) {} + virtual void setPenetrationScale(btScalar scale) {} }; // @@ -116,56 +113,67 @@ public: class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint { public: - const btSoftBody::DeformableNodeRigidAnchor* m_anchor; - - btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); - btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); - btDeformableNodeAnchorConstraint(){} - virtual ~btDeformableNodeAnchorConstraint() - { - } - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - // object A is the rigid/multi body, and object B is the deformable node/face - virtual btVector3 getVa() const; - // get the velocity of the deformable node in contact - virtual btVector3 getVb() const; - virtual btVector3 getDv(const btSoftBody::Node* n) const - { - return btVector3(0,0,0); - } - virtual void applyImpulse(const btVector3& impulse); - - virtual void setPenetrationScale(btScalar scale){} -}; + const btSoftBody::DeformableNodeRigidAnchor* m_anchor; + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); + btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + btDeformableNodeAnchorConstraint() {} + virtual ~btDeformableNodeAnchorConstraint() + { + } + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0, 0, 0); + } + virtual void applyImpulse(const btVector3& impulse); + + virtual void setPenetrationScale(btScalar scale) {} +}; // // Constraint between rigid/multi body and deformable objects class btDeformableRigidContactConstraint : public btDeformableContactConstraint { public: - btVector3 m_total_normal_dv; - btVector3 m_total_tangent_dv; - btScalar m_penetration; - const btSoftBody::DeformableRigidContact* m_contact; - - btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); - btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); - btDeformableRigidContactConstraint(){} - virtual ~btDeformableRigidContactConstraint() - { - } - - // object A is the rigid/multi body, and object B is the deformable node/face - virtual btVector3 getVa() const; - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - virtual void setPenetrationScale(btScalar scale) - { - m_penetration *= scale; - } + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + btScalar m_penetration; + btScalar m_total_split_impulse; + bool m_binding; + const btSoftBody::DeformableRigidContact* m_contact; + + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); + btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + btDeformableRigidContactConstraint() {} + virtual ~btDeformableRigidContactConstraint() + { + } + + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const = 0; + + // get the split impulse velocity of the rigid/multibdoy at the contaft + virtual btVector3 getSplitVa() const; + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + virtual void setPenetrationScale(btScalar scale) + { + m_penetration *= scale; + } + + btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); + + virtual void applySplitImpulse(const btVector3& impulse) = 0; }; // @@ -173,29 +181,34 @@ public: class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint { public: - // the deformable node in contact - btSoftBody::Node* m_node; - - btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); - btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); - btDeformableNodeRigidContactConstraint(){} - virtual ~btDeformableNodeRigidContactConstraint() - { - } - - // get the velocity of the deformable node in contact - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableNodeRigidContact* getContact() const - { - return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); + // the deformable node in contact + btSoftBody::Node* m_node; + + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); + btDeformableNodeRigidContactConstraint() {} + virtual ~btDeformableNodeRigidContactConstraint() + { + } + + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableNodeRigidContact* getContact() const + { + return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -203,28 +216,33 @@ public: class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint { public: - const btSoftBody::Face* m_face; - bool m_useStrainLimiting; - btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); - btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); - btDeformableFaceRigidContactConstraint(): m_useStrainLimiting(false) {} - virtual ~btDeformableFaceRigidContactConstraint() - { - } - - // get the velocity of the deformable face at the contact point - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableFaceRigidContact* getContact() const - { - return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); + btSoftBody::Face* m_face; + bool m_useStrainLimiting; + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); + btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); + btDeformableFaceRigidContactConstraint() : m_useStrainLimiting(false) {} + virtual ~btDeformableFaceRigidContactConstraint() + { + } + + // get the velocity of the deformable face at the contact point + virtual btVector3 getVb() const; + + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceRigidContact* getContact() const + { + return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -232,35 +250,35 @@ public: class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint { public: - btSoftBody::Node* m_node; - btSoftBody::Face* m_face; - const btSoftBody::DeformableFaceNodeContact* m_contact; - btVector3 m_total_normal_dv; - btVector3 m_total_tangent_dv; - - btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); - btDeformableFaceNodeContactConstraint(){} - virtual ~btDeformableFaceNodeContactConstraint(){} - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - // get the velocity of the object A in the contact - virtual btVector3 getVa() const; - - // get the velocity of the object B in the contact - virtual btVector3 getVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableFaceNodeContact* getContact() const - { - return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact); - } - - virtual void applyImpulse(const btVector3& impulse); - - virtual void setPenetrationScale(btScalar scale){} + btSoftBody::Node* m_node; + btSoftBody::Face* m_face; + const btSoftBody::DeformableFaceNodeContact* m_contact; + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableFaceNodeContactConstraint() {} + virtual ~btDeformableFaceNodeContactConstraint() {} + + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const; + + // get the velocity change of the input soft body node in the constraint + virtual btVector3 getDv(const btSoftBody::Node*) const; + + // cast the contact to the desired type + const btSoftBody::DeformableFaceNodeContact* getContact() const + { + return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); + + virtual void setPenetrationScale(btScalar scale) {} }; #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp index 22ca8bf582..7f67260ce6 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include <algorithm> #include <cmath> -btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; for (int i = 0; i < numDeformableBodies; ++i) @@ -58,27 +58,37 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod return residualSquare; } -void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < m_softBodies.size(); ++i) + btScalar residualSquare = 0; + for (int i = 0; i < numDeformableBodies; ++i) { - // node constraints - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); - } - // face constraints - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + for (int j = 0; j < m_softBodies.size(); ++j) { - btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); + btCollisionObject* psb = m_softBodies[j]; + if (psb != deformableBodies[i]) + { + continue; + } + for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } } } + return residualSquare; } void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) -{ +{ BT_PROFILE("setConstraints"); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -97,7 +107,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in m_staticConstraints[i].push_back(static_constraint); } } - + // set up deformable anchors for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) { @@ -111,7 +121,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); m_nodeAnchorConstraints[i].push_back(constraint); } - + // set Deformable Node vs. Rigid constraint for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { @@ -122,17 +132,9 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_nodeRigidConstraints[i].push_back(constraint); - } + m_nodeRigidConstraints[i].push_back(constraint); } - + // set Deformable Face vs. Rigid constraint for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) { @@ -143,15 +145,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_faceRigidConstraints[i].push_back(constraint); - } + m_faceRigidConstraints[i].push_back(constraint); } } } @@ -159,267 +153,269 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in void btDeformableContactProjection::project(TVStack& x) { #ifndef USE_MGS - const int dim = 3; - for (int index = 0; index < m_projectionsDict.size(); ++index) - { - btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); - size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); - if (projectionDirs.size() >= dim) - { - // static node - x[i].setZero(); - continue; - } - else if (projectionDirs.size() == 2) - { - btVector3 dir0 = projectionDirs[0]; - btVector3 dir1 = projectionDirs[1]; - btVector3 free_dir = btCross(dir0, dir1); - if (free_dir.safeNorm() < SIMD_EPSILON) - { - x[i] -= x[i].dot(dir0) * dir0; - x[i] -= x[i].dot(dir1) * dir1; - } - else - { - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; - } - } - else - { - btAssert(projectionDirs.size() == 1); - btVector3 dir0 = projectionDirs[0]; - x[i] -= x[i].dot(dir0) * dir0; - } - } + const int dim = 3; + for (int index = 0; index < m_projectionsDict.size(); ++index) + { + btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); + size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); + if (projectionDirs.size() >= dim) + { + // static node + x[i].setZero(); + continue; + } + else if (projectionDirs.size() == 2) + { + btVector3 dir0 = projectionDirs[0]; + btVector3 dir1 = projectionDirs[1]; + btVector3 free_dir = btCross(dir0, dir1); + if (free_dir.safeNorm() < SIMD_EPSILON) + { + x[i] -= x[i].dot(dir0) * dir0; + } + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + } + else + { + btAssert(projectionDirs.size() == 1); + btVector3 dir0 = projectionDirs[0]; + x[i] -= x[i].dot(dir0) * dir0; + } + } #else - btReducedVector p(x.size()); - for (int i = 0; i < m_projections.size(); ++i) - { - p += (m_projections[i].dot(x) * m_projections[i]); - } - for (int i = 0; i < p.m_indices.size(); ++i) - { - x[p.m_indices[i]] -= p.m_vecs[i]; - } + btReducedVector p(x.size()); + for (int i = 0; i < m_projections.size(); ++i) + { + p += (m_projections[i].dot(x) * m_projections[i]); + } + for (int i = 0; i < p.m_indices.size(); ++i) + { + x[p.m_indices[i]] -= p.m_vecs[i]; + } #endif } void btDeformableContactProjection::setProjection() { #ifndef USE_MGS - BT_PROFILE("btDeformableContactProjection::setProjection"); - btAlignedObjectArray<btVector3> units; - units.push_back(btVector3(1,0,0)); - units.push_back(btVector3(0,1,0)); - units.push_back(btVector3(0,0,1)); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) - { - int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - if (m_nodeRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray<btVector3> projections; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - } - } - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; - for (int k = 0; k < 3; ++k) - { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); - } - for (int k = 0; k < 3; ++k) - { - btSoftBody::Node* node = face->m_n[k]; - node->m_penetration = true; - int index = node->index; - if (m_faceRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray<btVector3> projections; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); - } - } - } - } - } + BT_PROFILE("btDeformableContactProjection::setProjection"); + btAlignedObjectArray<btVector3> units; + units.push_back(btVector3(1, 0, 0)); + units.push_back(btVector3(0, 1, 0)); + units.push_back(btVector3(0, 0, 1)); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_constrained = true; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + if (m_nodeRigidConstraints[i][j].m_binding) + { + if (m_nodeRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray<btVector3> projections; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + } + } + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + if (m_faceRigidConstraints[i][j].m_binding) + { + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_constrained = true; + } + } + for (int k = 0; k < 3; ++k) + { + btSoftBody::Node* node = face->m_n[k]; + int index = node->index; + if (m_faceRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int l = 0; l < 3; ++l) + { + projections.push_back(units[l]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray<btVector3> projections; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + } + } + } + } + } #else - int dof = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - dof += m_softBodies[i]->m_nodes.size(); - } - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; - btAlignedObjectArray<int> indices; - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - indices.push_back(index); - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - - for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) - { - int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; - btAlignedObjectArray<int> indices; - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - indices.push_back(index); - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - btAlignedObjectArray<int> indices; - indices.push_back(index); - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - if (m_nodeRigidConstraints[i][j].m_static) - { - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - else - { - vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - } - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + int dof = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + dof += m_softBodies[i]->m_nodes.size(); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; + btAlignedObjectArray<int> indices; + indices.push_back(index); + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + if (m_nodeRigidConstraints[i][j].m_static) + { + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + else + { + vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; - for (int k = 0; k < 3; ++k) - { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); - } + btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + } if (m_faceRigidConstraints[i][j].m_static) { for (int l = 0; l < 3; ++l) { - btReducedVector rv(dof); for (int k = 0; k < 3; ++k) { rv.m_indices.push_back(face->m_n[k]->index); - btVector3 v(0,0,0); + btVector3 v(0, 0, 0); v[l] = bary[k]; rv.m_vecs.push_back(v); - rv.sort(); + rv.sort(); } m_projections.push_back(rv); } @@ -431,121 +427,134 @@ void btDeformableContactProjection::setProjection() { rv.m_indices.push_back(face->m_n[k]->index); rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal); - rv.sort(); + rv.sort(); } m_projections.push_back(rv); } } - } - btModifiedGramSchmidt<btReducedVector> mgs(m_projections); - mgs.solve(); - m_projections = mgs.m_out; + } + btModifiedGramSchmidt<btReducedVector> mgs(m_projections); + mgs.solve(); + m_projections = mgs.m_out; #endif } void btDeformableContactProjection::checkConstraints(const TVStack& x) { - for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) - { - btVector3 d(0,0,0); - const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; - for (int j = 0; j < lm.m_num_constraints; ++j) - { - for (int k = 0; k < lm.m_num_nodes; ++k) - { - d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); - } - } - printf("d = %f, %f, %f\n",d[0],d[1],d[2]); - } + for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) + { + btVector3 d(0, 0, 0); + const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int k = 0; k < lm.m_num_nodes; ++k) + { + d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); + } + } + // printf("d = %f, %f, %f\n", d[0], d[1], d[2]); + // printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]); + } } void btDeformableContactProjection::setLagrangeMultiplier() { - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - m_lagrangeMultipliers.push_back(lm); - } - for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) - { - int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - m_lagrangeMultipliers.push_back(lm); - } - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - if (m_nodeRigidConstraints[i][j].m_static) - { - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - } - else - { - lm.m_num_constraints = 1; - lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; - } - m_lagrangeMultipliers.push_back(lm); - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; - - btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + if (!m_nodeRigidConstraints[i][j].m_binding) + { + continue; + } + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + if (m_nodeRigidConstraints[i][j].m_static) + { + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + } + else + { + lm.m_num_constraints = 1; + lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; + } + m_lagrangeMultipliers.push_back(lm); + } + + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + if (!m_faceRigidConstraints[i][j].m_binding) + { + continue; + } + btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + + btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; LagrangeMultiplier lm; lm.m_num_nodes = 3; - for (int k = 0; k<3; ++k) + + for (int k = 0; k < 3; ++k) { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + face->m_n[k]->m_constrained = true; lm.m_indices[k] = face->m_n[k]->index; lm.m_weights[k] = bary[k]; } - if (m_faceRigidConstraints[i][j].m_static) - { + if (m_faceRigidConstraints[i][j].m_static) + { + face->m_pcontact[3] = 1; lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); } else { + face->m_pcontact[3] = 0; lm.m_num_constraints = 1; lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; } - m_lagrangeMultipliers.push_back(lm); + m_lagrangeMultipliers.push_back(lm); } } } @@ -562,7 +571,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) @@ -575,7 +584,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } } @@ -587,7 +596,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } for (int k = 0; k < 3; ++k) { @@ -595,7 +604,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } } @@ -612,9 +621,8 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_nodeRigidConstraints.resize(N); m_faceRigidConstraints.resize(N); m_deformableConstraints.resize(N); - } - for (int i = 0 ; i < N; ++i) + for (int i = 0; i < N; ++i) { m_staticConstraints[i].clear(); m_nodeAnchorConstraints[i].clear(); @@ -623,12 +631,9 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_deformableConstraints[i].clear(); } #ifndef USE_MGS - m_projectionsDict.clear(); + m_projectionsDict.clear(); #else - m_projections.clear(); + m_projections.clear(); #endif - m_lagrangeMultipliers.clear(); + m_lagrangeMultipliers.clear(); } - - - diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h index 8d7e94d4fb..4964eaf990 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h @@ -27,31 +27,30 @@ struct LagrangeMultiplier { - int m_num_constraints; // Number of constraints - int m_num_nodes; // Number of nodes in these constraints - btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes - btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; - int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; + int m_num_constraints; // Number of constraints + int m_num_nodes; // Number of nodes in these constraints + btScalar m_weights[3]; // weights of the nodes involved, same size as m_num_nodes + btVector3 m_dirs[3]; // Constraint directions, same size of m_num_constraints; + int m_indices[3]; // indices of the nodes involved, same size as m_num_nodes; }; - class btDeformableContactProjection { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btAlignedObjectArray<btSoftBody *>& m_softBodies; - - // all constraints involving face - btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody*>& m_softBodies; + + // all constraints involving face + btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; #ifndef USE_MGS - // map from node index to projection directions - btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; + // map from node index to projection directions + btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; #else - btAlignedObjectArray<btReducedVector> m_projections; + btAlignedObjectArray<btReducedVector> m_projections; #endif - - btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers; - + + btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers; + // map from node index to static constraint btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints; // map from node index to node rigid constraint @@ -62,39 +61,39 @@ public: btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints; // map from node index to node anchor constraint btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints; - - bool m_useStrainLimiting; - - btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies) - : m_softBodies(softBodies) - { - } - - virtual ~btDeformableContactProjection() - { - } - - // apply the constraints to the rhs of the linear solve - virtual void project(TVStack& x); - - // add friction force to the rhs of the linear solve - virtual void applyDynamicFriction(TVStack& f); - - // update and solve the constraints - virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); - - // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. - virtual void setConstraints(const btContactSolverInfo& infoGlobal); - - // Set up projections for each vertex by adding the projection direction to - virtual void setProjection(); - - virtual void reinitialize(bool nodeUpdated); - - virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal); - - virtual void setLagrangeMultiplier(); - - void checkConstraints(const TVStack& x); + + bool m_useStrainLimiting; + + btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies) + : m_softBodies(softBodies) + { + } + + virtual ~btDeformableContactProjection() + { + } + + // apply the constraints to the rhs of the linear solve + virtual void project(TVStack& x); + + // add friction force to the rhs of the linear solve + virtual void applyDynamicFriction(TVStack& f); + + // update and solve the constraints + virtual btScalar update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. + virtual void setConstraints(const btContactSolverInfo& infoGlobal); + + // Set up projections for each vertex by adding the projection direction to + virtual void setProjection(); + + virtual void reinitialize(bool nodeUpdated); + + btScalar solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); + + virtual void setLagrangeMultiplier(); + + void checkConstraints(const TVStack& x); }; #endif /* btDeformableContactProjection_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h index 2d042df729..dfd85523bc 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h @@ -21,107 +21,104 @@ static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) { - static const btPolarDecomposition polar; - return polar.decompose(m, q, s); + static const btPolarDecomposition polar; + return polar.decompose(m, q, s); } class btDeformableCorotatedForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; - btDeformableCorotatedForce(): m_mu(1), m_lambda(1) - { - - } - - btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(tetra.m_F,P); - btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - // explicit elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) - { - // btMatrix3x3 JFinvT = F.adjoint(); - btScalar J = F.determinant(); - P = F.adjoint().transpose() * (m_lambda * (J-1)); - if (m_mu > SIMD_EPSILON) - { - btMatrix3x3 R,S; - if (J < 1024 * SIMD_EPSILON) - R.setIdentity(); - else - PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd - /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ - P += (F-R) * 2 * m_mu; - } - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_COROTATED_FORCE; - } - -}; + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; + btDeformableCorotatedForce() : m_mu(1), m_lambda(1) + { + } + + btDeformableCorotatedForce(btScalar mu, btScalar lambda) : m_mu(mu), m_lambda(lambda) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(tetra.m_F, P); + btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + // elastic force + // explicit elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) + { + // btMatrix3x3 JFinvT = F.adjoint(); + btScalar J = F.determinant(); + P = F.adjoint().transpose() * (m_lambda * (J - 1)); + if (m_mu > SIMD_EPSILON) + { + btMatrix3x3 R, S; + if (J < 1024 * SIMD_EPSILON) + R.setIdentity(); + else + PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd + /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ + P += (F - R) * 2 * m_mu; + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_COROTATED_FORCE; + } +}; #endif /* btCorotated_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h index 13ee3eacb6..d91867f457 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h @@ -21,87 +21,85 @@ class btDeformableGravityForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btVector3 m_gravity; - - btDeformableGravityForce(const btVector3& g) : m_gravity(g) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledGravityForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledGravityForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} - - virtual void addScaledGravityForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& n = psb->m_nodes[j]; - size_t id = n.index; - btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; - btVector3 scaled_force = scale * m_gravity * mass; - force[id] += scaled_force; - } - } - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_GRAVITY_FORCE; - } + typedef btAlignedObjectArray<btVector3> TVStack; + btVector3 m_gravity; - // the gravitational potential energy - virtual double totalEnergy(btScalar dt) - { - double e = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - if (node.m_im > 0) - { - e -= m_gravity.dot(node.m_q)/node.m_im; - } - } - } - return e; - } - - + btDeformableGravityForce(const btVector3& g) : m_gravity(g) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual void addScaledGravityForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& n = psb->m_nodes[j]; + size_t id = n.index; + btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; + btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor; + force[id] += scaled_force; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_GRAVITY_FORCE; + } + + // the gravitational potential energy + virtual double totalEnergy(btScalar dt) + { + double e = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + e -= m_gravity.dot(node.m_q) / node.m_im; + } + } + } + return e; + } }; #endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h index 0b6447442d..d58d825d1c 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h @@ -22,352 +22,351 @@ enum btDeformableLagrangianForceType { - BT_GRAVITY_FORCE = 1, - BT_MASSSPRING_FORCE = 2, - BT_COROTATED_FORCE = 3, - BT_NEOHOOKEAN_FORCE = 4, - BT_LINEAR_ELASTICITY_FORCE = 5, - BT_MOUSE_PICKING_FORCE = 6 + BT_GRAVITY_FORCE = 1, + BT_MASSSPRING_FORCE = 2, + BT_COROTATED_FORCE = 3, + BT_NEOHOOKEAN_FORCE = 4, + BT_LINEAR_ELASTICITY_FORCE = 5, + BT_MOUSE_PICKING_FORCE = 6 }; static inline double randomDouble(double low, double high) { - return low + static_cast<double>(rand()) / RAND_MAX * (high - low); + return low + static_cast<double>(rand()) / RAND_MAX * (high - low); } class btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btAlignedObjectArray<btSoftBody *> m_softBodies; - const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; - - btDeformableLagrangianForce() - { - } - - virtual ~btDeformableLagrangianForce(){} - - // add all forces - virtual void addScaledForces(btScalar scale, TVStack& force) = 0; - - // add damping df - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; - - // build diagonal of A matrix - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0; - - // add elastic df - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; - - // add all forces that are explicit in explicit solve - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; - - // add all damping forces - virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; - - virtual btDeformableLagrangianForceType getForceType() = 0; - - virtual void reinitialize(bool nodeUpdated) - { - } - - // get number of nodes that have the force - virtual int getNumNodes() - { - int numNodes = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - numNodes += m_softBodies[i]->m_nodes.size(); - } - return numNodes; - } - - // add a soft body to be affected by the particular lagrangian force - virtual void addSoftBody(btSoftBody* psb) - { - m_softBodies.push_back(psb); - } - - virtual void removeSoftBody(btSoftBody* psb) - { - m_softBodies.remove(psb); - } - - virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) - { - m_nodes = nodes; - } - - // Calculate the incremental deformable generated from the input dx - virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) - { - btVector3 c1 = dx[id1] - dx[id0]; - btVector3 c2 = dx[id2] - dx[id0]; - btVector3 c3 = dx[id3] - dx[id0]; - return btMatrix3x3(c1,c2,c3).transpose(); - } - - // Calculate the incremental deformable generated from the current velocity - virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) - { - btVector3 c1 = n1->m_v - n0->m_v; - btVector3 c2 = n2->m_v - n0->m_v; - btVector3 c3 = n3->m_v - n0->m_v; - return btMatrix3x3(c1,c2,c3).transpose(); - } - - // test for addScaledElasticForce function - virtual void testDerivative() - { - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); - } - psb->updateDeformation(); - } - - TVStack dx; - dx.resize(getNumNodes()); - TVStack dphi_dx; - dphi_dx.resize(dx.size()); - for (int i =0; i < dphi_dx.size();++i) - { - dphi_dx[i].setZero(); - } - addScaledForces(-1, dphi_dx); - - // write down the current position - TVStack x; - x.resize(dx.size()); - int counter = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - x[counter] = psb->m_nodes[j].m_q; - counter++; - } - } - counter = 0; - - // populate dx with random vectors - for (int i = 0; i < dx.size(); ++i) - { - dx[i].setX(randomDouble(-1, 1)); - dx[i].setY(randomDouble(-1, 1)); - dx[i].setZ(randomDouble(-1, 1)); - } - - btAlignedObjectArray<double> errors; - for (int it = 0; it < 10; ++it) - { - for (int i = 0; i < dx.size(); ++i) - { - dx[i] *= 0.5; - } - - // get dphi/dx * dx - double dphi = 0; - for (int i = 0; i < dx.size(); ++i) - { - dphi += dphi_dx[i].dot(dx[i]); - } - - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] + dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double f1 = totalElasticEnergy(0); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] - dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - double f2 = totalElasticEnergy(0); - - //restore m_q - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double error = f1-f2-2*dphi; - errors.push_back(error); - std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl; - } - for (int i = 1; i < errors.size(); ++i) - { - std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; - } - } - - // test for addScaledElasticForce function - virtual void testHessian() - { - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); - } - psb->updateDeformation(); - } - - - TVStack dx; - dx.resize(getNumNodes()); - TVStack df; - df.resize(dx.size()); - TVStack f1; - f1.resize(dx.size()); - TVStack f2; - f2.resize(dx.size()); - - - // write down the current position - TVStack x; - x.resize(dx.size()); - int counter = 0; - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - x[counter] = psb->m_nodes[j].m_q; - counter++; - } - } - counter = 0; - - // populate dx with random vectors - for (int i = 0; i < dx.size(); ++i) - { - dx[i].setX(randomDouble(-1, 1)); - dx[i].setY(randomDouble(-1, 1)); - dx[i].setZ(randomDouble(-1, 1)); - } - - btAlignedObjectArray<double> errors; - for (int it = 0; it < 10; ++it) - { - for (int i = 0; i < dx.size(); ++i) - { - dx[i] *= 0.5; - } - - // get df - for (int i =0; i < df.size();++i) - { - df[i].setZero(); - f1[i].setZero(); - f2[i].setZero(); - } - - //set df - addScaledElasticForceDifferential(-1, dx, df); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] + dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - //set f1 - addScaledForces(-1, f1); - - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter] - dx[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - - //set f2 - addScaledForces(-1, f2); - - //restore m_q - for (int i = 0; i<m_softBodies.size();++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_q = x[counter]; - counter++; - } - psb->updateDeformation(); - } - counter = 0; - double error = 0; - for (int i = 0; i < df.size();++i) - { - btVector3 error_vector = f1[i]-f2[i]-2*df[i]; - error += error_vector.length2(); - } - error = btSqrt(error); - errors.push_back(error); - std::cout << "Iteration = " << it << ", error = " << error << std::endl; - } - for (int i = 1; i < errors.size(); ++i) - { - std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; - } - } - - // - virtual double totalElasticEnergy(btScalar dt) - { - return 0; - } - - // - virtual double totalDampingEnergy(btScalar dt) - { - return 0; - } - - // total Energy takes dt as input because certain energies depend on dt - virtual double totalEnergy(btScalar dt) - { - return totalElasticEnergy(dt) + totalDampingEnergy(dt); - } + typedef btAlignedObjectArray<btVector3> TVStack; + btAlignedObjectArray<btSoftBody*> m_softBodies; + const btAlignedObjectArray<btSoftBody::Node*>* m_nodes; + + btDeformableLagrangianForce() + { + } + + virtual ~btDeformableLagrangianForce() {} + + // add all forces + virtual void addScaledForces(btScalar scale, TVStack& force) = 0; + + // add damping df + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + + // build diagonal of A matrix + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) = 0; + + // add elastic df + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; + + // add all forces that are explicit in explicit solve + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + + // add all damping forces + virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; + + virtual void addScaledHessian(btScalar scale) {} + + virtual btDeformableLagrangianForceType getForceType() = 0; + + virtual void reinitialize(bool nodeUpdated) + { + } + + // get number of nodes that have the force + virtual int getNumNodes() + { + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + numNodes += m_softBodies[i]->m_nodes.size(); + } + return numNodes; + } + + // add a soft body to be affected by the particular lagrangian force + virtual void addSoftBody(btSoftBody* psb) + { + m_softBodies.push_back(psb); + } + + virtual void removeSoftBody(btSoftBody* psb) + { + m_softBodies.remove(psb); + } + + virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes) + { + m_nodes = nodes; + } + + // Calculate the incremental deformable generated from the input dx + virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) + { + btVector3 c1 = dx[id1] - dx[id0]; + btVector3 c2 = dx[id2] - dx[id0]; + btVector3 c3 = dx[id3] - dx[id0]; + return btMatrix3x3(c1, c2, c3).transpose(); + } + + // Calculate the incremental deformable generated from the current velocity + virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) + { + btVector3 c1 = n1->m_v - n0->m_v; + btVector3 c2 = n2->m_v - n0->m_v; + btVector3 c3 = n3->m_v - n0->m_v; + return btMatrix3x3(c1, c2, c3).transpose(); + } + + // test for addScaledElasticForce function + virtual void testDerivative() + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + TVStack dx; + dx.resize(getNumNodes()); + TVStack dphi_dx; + dphi_dx.resize(dx.size()); + for (int i = 0; i < dphi_dx.size(); ++i) + { + dphi_dx[i].setZero(); + } + addScaledForces(-1, dphi_dx); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray<double> errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get dphi/dx * dx + double dphi = 0; + for (int i = 0; i < dx.size(); ++i) + { + dphi += dphi_dx[i].dot(dx[i]); + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double f1 = totalElasticEnergy(0); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + double f2 = totalElasticEnergy(0); + + //restore m_q + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = f1 - f2 - 2 * dphi; + errors.push_back(error); + std::cout << "Iteration = " << it << ", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl; + } + } + + // test for addScaledElasticForce function + virtual void testHessian() + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + TVStack dx; + dx.resize(getNumNodes()); + TVStack df; + df.resize(dx.size()); + TVStack f1; + f1.resize(dx.size()); + TVStack f2; + f2.resize(dx.size()); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray<double> errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get df + for (int i = 0; i < df.size(); ++i) + { + df[i].setZero(); + f1[i].setZero(); + f2[i].setZero(); + } + + //set df + addScaledElasticForceDifferential(-1, dx, df); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f1 + addScaledForces(-1, f1); + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f2 + addScaledForces(-1, f2); + + //restore m_q + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = 0; + for (int i = 0; i < df.size(); ++i) + { + btVector3 error_vector = f1[i] - f2[i] - 2 * df[i]; + error += error_vector.length2(); + } + error = btSqrt(error); + errors.push_back(error); + std::cout << "Iteration = " << it << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i - 1] / errors[i] << std::endl; + } + } + + // + virtual double totalElasticEnergy(btScalar dt) + { + return 0; + } + + // + virtual double totalDampingEnergy(btScalar dt) + { + return 0; + } + + // total Energy takes dt as input because certain energies depend on dt + virtual double totalEnergy(btScalar dt) + { + return totalElasticEnergy(dt) + totalDampingEnergy(dt); + } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h index 106dc10ad6..971192050b 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h @@ -18,323 +18,445 @@ #include "btDeformableLagrangianForce.h" #include "LinearMath/btQuickprof.h" +#include "btSoftBodyInternals.h" +#define TETRA_FLAT_THRESHOLD 0.01 class btDeformableLinearElasticityForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; - btScalar m_mu_damp, m_lambda_damp; - btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1) - { - btScalar damping = 0.05; - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; - } - - btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) - { - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; - // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * df_on_node0; - force[id1] -= scale1 * df_on_node123.getColumn(0); - force[id2] -= scale1 * df_on_node123.getColumn(1); - force[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetraScratches.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; - energy += tetra.m_element_measure * elasticEnergyDensity(s); - } - } - return energy; - } - - // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - double elasticEnergyDensity(const btSoftBody::TetraScratch& s) - { - double density = 0; - btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); - btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; - density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); - density += m_lambda * trace * trace * 0.5; - return density; - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar max_p = psb->m_cfg.m_maxStress; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(psb->m_tetraScratches[j],P); + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_damping_alpha, m_damping_beta; + btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1), m_damping_alpha(0.01), m_damping_beta(0.01) + { + updateYoungsModulusAndPoissonRatio(); + } + + btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta) + { + updateYoungsModulusAndPoissonRatio(); + } + + void updateYoungsModulusAndPoissonRatio() + { + // conversion from Lame Parameters to Young's modulus and Poisson ratio + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); + m_nu = m_lambda * 0.5 / (m_mu + m_lambda); + } + + void updateLameParameters() + { + // conversion from Young's modulus and Poisson ratio to Lame Parameters + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_mu = m_E * 0.5 / (1 + m_nu); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); + } + + void setYoungsModulus(btScalar E) + { + m_E = E; + updateLameParameters(); + } + + void setPoissonRatio(btScalar nu) + { + m_nu = nu; + updateLameParameters(); + } + + void setDamping(btScalar damping_alpha, btScalar damping_beta) + { + m_damping_alpha = damping_alpha; + m_damping_beta = damping_beta; + } + + void setLameParameters(btScalar mu, btScalar lambda) + { + m_mu = mu; + m_lambda = lambda; + updateYoungsModulusAndPoissonRatio(); + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_damping_alpha == 0 && m_damping_beta == 0) + return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + force[id] -= scale * node.m_v / node.m_im * m_damping_alpha; + } + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); + density += m_lambda * trace * trace * 0.5; + return density; + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); #if USE_SVD - if (max_p > 0) - { - // since we want to clamp the principal stress to max_p, we only need to - // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p - btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); - if (trPTP > max_p * max_p) - { - btMatrix3x3 U, V; - btVector3 sigma; - singularValueDecomposition(P, U, sigma, V); - sigma[0] = btMin(sigma[0], max_p); - sigma[1] = btMin(sigma[1], max_p); - sigma[2] = btMin(sigma[2], max_p); - sigma[0] = btMax(sigma[0], -max_p); - sigma[1] = btMax(sigma[1], -max_p); - sigma[2] = btMax(sigma[2], -max_p); - btMatrix3x3 Sigma; - Sigma.setIdentity(); - Sigma[0][0] = sigma[0]; - Sigma[1][1] = sigma[1]; - Sigma[2][2] = sigma[2]; - P = U * Sigma * V.transpose(); - } - } + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } #endif - // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; - // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; - btMatrix3x3 dP; - firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); - // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // elastic force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) - { - btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); - btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; - P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; - } - - // Let P be the first piola stress. - // This function calculates the dP = dP/dF * dF - void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); - dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; - } - - // Let Q be the damping stress. - // This function calculates the dP = dQ/dF * dF - void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); - dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace; - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_LINEAR_ELASTICITY_FORCE; - } - + // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_damping_alpha == 0 && m_damping_beta == 0) + return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + df[id] -= scale * dv[id] / node.m_im * m_damping_alpha; + } + } + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F; + + btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace; + } + + virtual void addScaledHessian(btScalar scale) + { + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); // make sure scratch is evaluated at x_n + dt * vn + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure; // stiff and stiffness-damping terms; + node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1; + node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1; + node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1; + node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + btMatrix3x3 I; + I.setIdentity(); + node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha); + } + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_LINEAR_ELASTICITY_FORCE; + } }; #endif /* BT_LINEAR_ELASTICITY_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h index b128df92cc..8c97bd1ba8 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h @@ -20,282 +20,282 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce { - // If true, the damping force will be in the direction of the spring - // If false, the damping force will be in the direction of the velocity - bool m_momentum_conserving; - btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + bool m_momentum_conserving; + btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; + public: - typedef btAlignedObjectArray<btVector3> TVStack; - btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) - { - } - btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) - { - if (m_bendingStiffness < btScalar(0)) - { - m_bendingStiffness = m_elasticStiffness; - } - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - - // damping force - btVector3 v_diff = (node2->m_v - node1->m_v); - btVector3 scaled_force = scale * m_dampingStiffness * v_diff; - if (m_momentum_conserving) - { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) - { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); - scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; - } - } - force[id1] += scaled_force; - force[id2] -= scaled_force; - } - } - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - btScalar r = link.m_rl; - size_t id1 = node1->index; - size_t id2 = node2->index; - - // elastic force - btVector3 dir = (node2->m_q - node1->m_q); - btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); - btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); - btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r); - force[id1] += scaled_force; - force[id2] -= scaled_force; - } - } - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - // implicit damping force differential - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar scaled_k_damp = m_dampingStiffness * scale; - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) + { + } + btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) + { + if (m_bendingStiffness < btScalar(0)) + { + m_bendingStiffness = m_elasticStiffness; + } + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + } + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + size_t id1 = node1->index; + size_t id2 = node2->index; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r); + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + + btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; + } + } + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + if (m_momentum_conserving) + { + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + { + btVector3 dir = (node2->m_x - node1->m_x).normalized(); + for (int d = 0; d < 3; ++d) + { + if (node1->m_im > 0) + diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d]; + if (node2->m_im > 0) + diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d]; + } + } + } + else + { + for (int d = 0; d < 3; ++d) + { + if (node1->m_im > 0) + diagA[id1][d] -= scaled_k_damp; + if (node2->m_im > 0) + diagA[id2][d] -= scaled_k_damp; + } + } + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r); + } + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + btScalar r = link.m_rl; - btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); - if (m_momentum_conserving) - { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) - { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); - local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; - } - } - df[id1] += local_scaled_df; - df[id2] -= local_scaled_df; - } - } - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) - { - // implicit damping force differential - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar scaled_k_damp = m_dampingStiffness * scale; - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - if (m_momentum_conserving) - { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) - { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); - for (int d = 0; d < 3; ++d) - { - if (node1->m_im > 0) - diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d]; - if (node2->m_im > 0) - diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d]; - } - } - } - else - { - for (int d = 0; d < 3; ++d) - { - if (node1->m_im > 0) - diagA[id1][d] -= scaled_k_damp; - if (node2->m_im > 0) - diagA[id2][d] -= scaled_k_damp; - } - } - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - btScalar r = link.m_rl; + btVector3 dir = (node1->m_q - node2->m_q); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + btVector3 dx_diff = dx[id1] - dx[id2]; + btVector3 scaled_df = btVector3(0, 0, 0); + btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff; + } - // elastic force - btVector3 dir = (node2->m_q - node1->m_q); - energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r); - } - } - return energy; - } - - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - // implicit damping force differential - for (int i = 0; i < m_softBodies.size(); ++i) - { - const btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_links.size(); ++j) - { - const btSoftBody::Link& link = psb->m_links[j]; - btSoftBody::Node* node1 = link.m_n[0]; - btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = node1->index; - size_t id2 = node2->index; - btScalar r = link.m_rl; + df[id1] += scaled_df; + df[id2] -= scaled_df; + } + } + } - btVector3 dir = (node1->m_q - node2->m_q); - btScalar dir_norm = dir.norm(); - btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); - btVector3 dx_diff = dx[id1] - dx[id2]; - btVector3 scaled_df = btVector3(0,0,0); - btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); - if (dir_norm > SIMD_EPSILON) - { - scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; - scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized; - scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff; - } - - df[id1] += scaled_df; - df[id2] -= scaled_df; - } - } - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_MASSSPRING_FORCE; - } - + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MASSSPRING_FORCE; + } }; #endif /* btMassSpring_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h index 07c10935f4..d218d96214 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h @@ -20,126 +20,143 @@ class btDeformableMousePickingForce : public btDeformableLagrangianForce { - // If true, the damping force will be in the direction of the spring - // If false, the damping force will be in the direction of the velocity - btScalar m_elasticStiffness, m_dampingStiffness; - const btSoftBody::Face& m_face; - btVector3 m_mouse_pos; - btScalar m_maxForce; + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + btScalar m_elasticStiffness, m_dampingStiffness; + const btSoftBody::Face& m_face; + btVector3 m_mouse_pos; + btScalar m_maxForce; + public: - typedef btAlignedObjectArray<btVector3> TVStack; - btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) - { - } - - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - for (int i = 0; i < 3; ++i) - { - btVector3 v_diff = m_face.m_n[i]->m_v; - btVector3 scaled_force = scale * m_dampingStiffness * v_diff; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; - } - force[m_face.m_n[i]->index] -= scaled_force; - } - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - btScalar scaled_stiffness = scale * m_elasticStiffness; - for (int i = 0; i < 3; ++i) - { - btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); - btVector3 scaled_force = scaled_stiffness * dir; - if (scaled_force.safeNorm() > m_maxForce) - { - scaled_force.safeNormalize(); - scaled_force *= m_maxForce; - } - force[m_face.m_n[i]->index] -= scaled_force; - } - } - - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - btScalar scaled_k_damp = m_dampingStiffness * scale; - for (int i = 0; i < 3; ++i) - { - btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; - } - df[m_face.m_n[i]->index] -= local_scaled_df; - } - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < 3; ++i) - { - btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); - btVector3 scaled_force = m_elasticStiffness * dir; - if (scaled_force.safeNorm() > m_maxForce) - { - scaled_force.safeNormalize(); - scaled_force *= m_maxForce; - } - energy += 0.5 * scaled_force.dot(dir); - } - return energy; - } - - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < 3; ++i) - { - btVector3 v_diff = m_face.m_n[i]->m_v; - btVector3 scaled_force = m_dampingStiffness * v_diff; - if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) - { - btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); - scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; - } - energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; - } - return energy; - } - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - //TODO - } - - void setMousePos(const btVector3& p) - { - m_mouse_pos = p; - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_MOUSE_PICKING_FORCE; - } - + typedef btAlignedObjectArray<btVector3> TVStack; + btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = scaled_stiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int i = 0; i < 3; ++i) + { + btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; + } + df[m_face.m_n[i]->index] -= local_scaled_df; + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = m_elasticStiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + energy += 0.5 * scaled_force.dot(dir); + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; + } + energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + int id = m_face.m_n[i]->index; + btVector3 dx_diff = dx[id]; + btScalar r = 0; // rest length is 0 for picking spring + btVector3 scaled_df = btVector3(0, 0, 0); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff; + } + df[id] += scaled_df; + } + } + + void setMousePos(const btVector3& p) + { + m_mouse_pos = p; + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MOUSE_PICKING_FORCE; + } }; #endif /* btMassSpring_h */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index c8cc47923e..631fd5fbed 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -13,131 +13,132 @@ 3. This notice may not be removed or altered from any source distribution. */ - #include "btDeformableMultiBodyConstraintSolver.h" #include <iostream> // override the iterations method to include deformable/multibody contact -btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - { - ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + { + ///this is a special step to resolve penetrations (just for contacts) + solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + for (int iteration = 0; iteration < maxIterations; iteration++) + { + // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body - int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - for (int iteration = 0; iteration < maxIterations; iteration++) - { - // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body - - // solve rigid/rigid in solver body - m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - // solver body velocity -> rigid body velocity - solverBodyWriteBack(infoGlobal); - btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal); - // update rigid body velocity in rigid/deformable contact - m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); - // solver body velocity <- rigid body velocity - writeToSolverBody(bodies, numBodies, infoGlobal); - - if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) - { + // solve rigid/rigid in solver body + m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + // solver body velocity -> rigid body velocity + solverBodyWriteBack(infoGlobal); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal); + // update rigid body velocity in rigid/deformable contact + m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); + // solver body velocity <- rigid body velocity + writeToSolverBody(bodies, numBodies, infoGlobal); + + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) + { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); + if (iteration >= (maxIterations - 1)) + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif - m_analyticsData.m_numSolverCalls++; - m_analyticsData.m_numIterationsUsed = iteration+1; - m_analyticsData.m_islandId = -2; - if (numBodies>0) - m_analyticsData.m_islandId = bodies[0]->getCompanionId(); - m_analyticsData.m_numBodies = numBodies; - m_analyticsData.m_numContactManifolds = numManifolds; - m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; - break; - } - } - } - return 0.f; + m_analyticsData.m_numSolverCalls++; + m_analyticsData.m_numIterationsUsed = iteration + 1; + m_analyticsData.m_islandId = -2; + if (numBodies > 0) + m_analyticsData.m_islandId = bodies[0]->getCompanionId(); + m_analyticsData.m_numBodies = numBodies; + m_analyticsData.m_numContactManifolds = numManifolds; + m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; + break; + } + } + } + return 0.f; } -void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) { - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // overriden - solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlyFinish(bodies, numBodies, info); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; + m_tmpMultiBodyConstraints = multiBodyConstraints; + m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // overriden + solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlyFinish(bodies, numBodies, info); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; } void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < numBodies; i++) - { - int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + for (int i = 0; i < numBodies; i++) + { + int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body && body->getInvMass()) - { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; - solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; - } - } + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; + solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; + } + } } void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) - { - btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; - if (body) - { - m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity); - } - } + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) + { + btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; + if (body) + { + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity); + } + } } -void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); - int iteration; - if (infoGlobal.m_splitImpulse) - { - { -// m_deformableSolver->splitImpulseSetup(infoGlobal); - for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) - { - btScalar leastSquaresResidual = 0.f; - { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j = 0; j < numPoolConstraints; j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - - btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); - leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); - } - // solve the position correction between deformable and rigid/multibody -// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); -// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); - } - if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) - { + BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); + int iteration; + if (infoGlobal.m_splitImpulse) + { + { + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + { + btScalar leastSquaresResidual = 0.f; + { + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + for (j = 0; j < numPoolConstraints; j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + + btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + // solve the position correction between deformable and rigid/multibody + // btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); + btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + } + if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) + { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration); + if (iteration >= (infoGlobal.m_numIterations - 1)) + printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration); #endif - break; - } - } - } - } + break; + } + } + } + } } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 0c7cc26a83..94aabce838 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -13,7 +13,6 @@ 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H #define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H @@ -32,30 +31,31 @@ class btDeformableBodySolver; ATTRIBUTE_ALIGNED16(class) btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver { - btDeformableBodySolver* m_deformableSolver; - + btDeformableBodySolver* m_deformableSolver; + protected: - // override the iterations method to include deformable/multibody contact -// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - - // write the velocity of the the solver body to the underlying rigid body - void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); - - // write the velocity of the underlying rigid body to the the the solver body - void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - - virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - - virtual btScalar solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + // override the iterations method to include deformable/multibody contact + // virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + // write the velocity of the the solver body to the underlying rigid body + void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); + + // write the velocity of the underlying rigid body to the the the solver body + void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); + + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + + virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - void setDeformableSolver(btDeformableBodySolver* deformableSolver) - { - m_deformableSolver = deformableSolver; - } - - virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + BT_DECLARE_ALIGNED_ALLOCATOR(); + + void setDeformableSolver(btDeformableBodySolver * deformableSolver) + { + m_deformableSolver = deformableSolver; + } + + virtual void solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); }; #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 6b742978ef..983e622b5f 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -40,8 +40,9 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed #include "LinearMath/btQuickprof.h" #include "btSoftBodyInternals.h" btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver) -: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), -m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) + : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), + m_deformableBodySolver(deformableBodySolver), + m_solverCallback(0) { m_drawFlags = fDrawFlags::Std; m_drawNodeTree = true; @@ -52,7 +53,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_sbi.m_sparsesdf.Initialize(); m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005); m_sbi.m_sparsesdf.Reset(); - + m_sbi.air_density = (btScalar)1.2; m_sbi.water_density = 0; m_sbi.water_offset = 0; @@ -61,57 +62,57 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_internalTime = 0.0; m_implicit = false; m_lineSearch = false; - m_useProjection = true; + m_useProjection = false; m_ccdIterations = 5; m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); } btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld() { - delete m_solverDeformableBodyIslandCallback; + delete m_solverDeformableBodyIslandCallback; } void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - BT_PROFILE("internalSingleStepSimulation"); - if (0 != m_internalPreTickCallback) - { - (*m_internalPreTickCallback)(this, timeStep); - } - reinitialize(timeStep); - - // add gravity to velocity of rigid and multi bodys - applyRigidBodyGravity(timeStep); - - ///apply gravity and explicit force to velocity, predict motion - predictUnconstraintMotion(timeStep); - - ///perform collision detection that involves rigid/multi bodies - btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); - - btMultiBodyDynamicsWorld::calculateSimulationIslands(); - - beforeSolverCallbacks(timeStep); - - ///solve contact constraints and then deformable bodies momemtum equation - solveConstraints(timeStep); - - afterSolverCallbacks(timeStep); + BT_PROFILE("internalSingleStepSimulation"); + if (0 != m_internalPreTickCallback) + { + (*m_internalPreTickCallback)(this, timeStep); + } + reinitialize(timeStep); + + // add gravity to velocity of rigid and multi bodys + applyRigidBodyGravity(timeStep); + + ///apply gravity and explicit force to velocity, predict motion + predictUnconstraintMotion(timeStep); + + ///perform collision detection that involves rigid/multi bodies + btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + + btMultiBodyDynamicsWorld::calculateSimulationIslands(); + + beforeSolverCallbacks(timeStep); + + ///solve contact constraints and then deformable bodies momemtum equation + solveConstraints(timeStep); + + afterSolverCallbacks(timeStep); performDeformableCollisionDetection(); - applyRepulsionForce(timeStep); + applyRepulsionForce(timeStep); + + performGeometricCollisions(timeStep); + + integrateTransforms(timeStep); - performGeometricCollisions(timeStep); + ///update vehicle simulation + btMultiBodyDynamicsWorld::updateActions(timeStep); - integrateTransforms(timeStep); - - ///update vehicle simulation - btMultiBodyDynamicsWorld::updateActions(timeStep); - - updateActivationState(timeStep); - // End solver-wise simulation step - // /////////////////////////////// + updateActivationState(timeStep); + // End solver-wise simulation step + // /////////////////////////////// } void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() @@ -120,7 +121,7 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() { m_softBodies[i]->m_softSoftCollision = true; } - + for (int i = 0; i < m_softBodies.size(); ++i) { for (int j = i; j < m_softBodies.size(); ++j) @@ -128,7 +129,7 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]); } } - + for (int i = 0; i < m_softBodies.size(); ++i) { m_softBodies[i]->m_softSoftCollision = false; @@ -137,45 +138,45 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection() void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) { - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - psb->updateDeactivation(timeStep); - if (psb->wantsSleeping()) - { - if (psb->getActivationState() == ACTIVE_TAG) - psb->setActivationState(WANTS_DEACTIVATION); - if (psb->getActivationState() == ISLAND_SLEEPING) - { - psb->setZeroVelocity(); - } - } - else - { - if (psb->getActivationState() != DISABLE_DEACTIVATION) - psb->setActivationState(ACTIVE_TAG); - } - } - btMultiBodyDynamicsWorld::updateActivationState(timeStep); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + psb->updateDeactivation(timeStep); + if (psb->wantsSleeping()) + { + if (psb->getActivationState() == ACTIVE_TAG) + psb->setActivationState(WANTS_DEACTIVATION); + if (psb->getActivationState() == ISLAND_SLEEPING) + { + psb->setZeroVelocity(); + } + } + else + { + if (psb->getActivationState() != DISABLE_DEACTIVATION) + psb->setActivationState(ACTIVE_TAG); + } + } + btMultiBodyDynamicsWorld::updateActivationState(timeStep); } void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep) { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - if (psb->isActive()) - { + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { psb->applyRepulsionForce(timeStep, true); - } - } + } + } } void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep) { BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions"); - // refit the BVH tree for CCD + // refit the BVH tree for CCD for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -214,7 +215,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep; } } - } + } // apply CCD to register new contact points for (int i = 0; i < m_softBodies.size(); ++i) @@ -228,7 +229,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]); } } - } + } int penetration_count = 0; for (int i = 0; i < m_softBodies.size(); ++i) @@ -258,294 +259,292 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision"); - for (int i = 0; i < m_softBodies.size(); i++) - { - btSoftBody* psb = m_softBodies[i]; - if (psb->isActive()) - { - psb->defaultCollisionHandler(psb); - } - } + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision"); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + psb->defaultCollisionHandler(psb); + } + } } void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep) { - // correct the position of rigid bodies with temporary velocity generated from split impulse - btContactSolverInfo infoGlobal; - btVector3 zero(0,0,0); - for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) - { - btRigidBody* rb = m_nonStaticRigidBodies[i]; - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btVector3 pushVelocity = rb->getPushVelocity(); - btVector3 turnVelocity = rb->getTurnVelocity(); - if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0) - { - btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform); - rb->setWorldTransform(newTransform); - rb->setPushVelocity(zero); - rb->setTurnVelocity(zero); - } - } + // correct the position of rigid bodies with temporary velocity generated from split impulse + btContactSolverInfo infoGlobal; + btVector3 zero(0, 0, 0); + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btVector3 pushVelocity = rb->getPushVelocity(); + btVector3 turnVelocity = rb->getTurnVelocity(); + if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0) + { + btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform); + rb->setWorldTransform(newTransform); + rb->setPushVelocity(zero); + rb->setTurnVelocity(zero); + } + } } void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { - BT_PROFILE("integrateTransforms"); - positionCorrection(timeStep); - btMultiBodyDynamicsWorld::integrateTransforms(timeStep); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / timeStep; - for (int c = 0; c < 3; c++) - { - if (node.m_v[c] > clampDeltaV) - { - node.m_v[c] = clampDeltaV; - } - if (node.m_v[c] < -clampDeltaV) - { - node.m_v[c] = -clampDeltaV; - } - } - node.m_x = node.m_x + timeStep * node.m_v; - node.m_q = node.m_x; - node.m_vn = node.m_v; - } - // enforce anchor constraints - for (int j = 0; j < psb->m_deformableAnchors.size();++j) - { - btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; - btSoftBody::Node* n = a.m_node; - n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; - - // update multibody anchor info - if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 nrm; - const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); - const btTransform& wtr = multibodyLinkCol->getWorldTransform(); - psb->m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(n->m_x), - shp, - nrm, - 0); - a.m_cti.m_normal = wtr.getBasis() * nrm; - btVector3 normal = a.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - a.m_c0 = rot.transpose() * local_impulse_matrix * rot; - a.jacobianData_normal = jacobianData_normal; - a.jacobianData_t1 = jacobianData_t1; - a.jacobianData_t2 = jacobianData_t2; - a.t1 = t1; - a.t2 = t2; - } - } - } - psb->interpolateRenderMesh(); - } + BT_PROFILE("integrateTransforms"); + positionCorrection(timeStep); + btMultiBodyDynamicsWorld::integrateTransforms(timeStep); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / timeStep; + for (int c = 0; c < 3; c++) + { + if (node.m_v[c] > clampDeltaV) + { + node.m_v[c] = clampDeltaV; + } + if (node.m_v[c] < -clampDeltaV) + { + node.m_v[c] = -clampDeltaV; + } + } + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); + node.m_q = node.m_x; + node.m_vn = node.m_v; + } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + + // update multibody anchor info + if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 nrm; + const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); + const btTransform& wtr = multibodyLinkCol->getWorldTransform(); + psb->m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(n->m_x), + shp, + nrm, + 0); + a.m_cti.m_normal = wtr.getBasis() * nrm; + btVector3 normal = a.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + a.m_c0 = rot.transpose() * local_impulse_matrix * rot; + a.jacobianData_normal = jacobianData_normal; + a.jacobianData_t1 = jacobianData_t1; + a.jacobianData_t2 = jacobianData_t2; + a.t1 = t1; + a.t2 = t2; + } + } + } + psb->interpolateRenderMesh(); + } } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); - - // set up constraints among multibodies and between multibodies and deformable bodies - setupConstraints(); - - // solve contact constraints - solveContactConstraints(); - - // set up the directions in which the velocity does not change in the momentum solve - if (m_useProjection) - m_deformableBodySolver->m_objective->m_projection.setProjection(); - else - m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); - - // for explicit scheme, m_backupVelocity = v_{n+1}^* - // for implicit scheme, m_backupVelocity = v_n - // Here, set dv = v_{n+1} - v_n for nodes in contact - m_deformableBodySolver->setupDeformableSolve(m_implicit); - - // At this point, dv should be golden for nodes in contact - // proceed to solve deformable momentum equation - m_deformableBodySolver->solveDeformableConstraints(timeStep); + BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints"); + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); + + // set up constraints among multibodies and between multibodies and deformable bodies + setupConstraints(); + + // solve contact constraints + solveContactConstraints(); + + // set up the directions in which the velocity does not change in the momentum solve + if (m_useProjection) + m_deformableBodySolver->m_objective->m_projection.setProjection(); + else + m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); + + // for explicit scheme, m_backupVelocity = v_{n+1}^* + // for implicit scheme, m_backupVelocity = v_n + // Here, set dv = v_{n+1} - v_n for nodes in contact + m_deformableBodySolver->setupDeformableSolve(m_implicit); + + // At this point, dv should be golden for nodes in contact + // proceed to solve deformable momentum equation + m_deformableBodySolver->solveDeformableConstraints(timeStep); } void btDeformableMultiBodyDynamicsWorld::setupConstraints() { - // set up constraints between multibody and deformable bodies - m_deformableBodySolver->setConstraints(m_solverInfo); - - // set up constraints among multibodies - { - sortConstraints(); - // setup the solver callback - btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - - // build islands - m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); - } + // set up constraints between multibody and deformable bodies + m_deformableBodySolver->setConstraints(m_solverInfo); + + // set up constraints among multibodies + { + sortConstraints(); + // setup the solver callback + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + + // build islands + m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); + } } void btDeformableMultiBodyDynamicsWorld::sortConstraints() { - m_sortedConstraints.resize(m_constraints.size()); - int i; - for (i = 0; i < getNumConstraints(); i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i = 0; i < m_multiBodyConstraints.size(); i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); } - - + void btDeformableMultiBodyDynamicsWorld::solveContactConstraints() { - // process constraints on each island - m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback); - - // process deferred - m_solverDeformableBodyIslandCallback->processConstraints(); - m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); - - // write joint feedback - { - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - if (bod->internalNeedsJointFeedback()) - { - if (!bod->isUsingRK4Integration()) - { - if (bod->internalNeedsJointFeedback()) - { - bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } - } - } - } - - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->processDeltaVeeMultiDof2(); - } + // process constraints on each island + m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback); + + // process deferred + m_solverDeformableBodyIslandCallback->processConstraints(); + m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); + + // write joint feedback + { + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + if (bod->internalNeedsJointFeedback()) + { + if (!bod->isUsingRK4Integration()) + { + if (bod->internalNeedsJointFeedback()) + { + bool isConstraintPass = true; + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + } + } + } + + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); + } } void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) { - m_softBodies.push_back(body); - - // Set the soft body solver that will deal with this body - // to be the world's solver - body->setSoftBodySolver(m_deformableBodySolver); - - btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); + m_softBodies.push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver(m_deformableBodySolver); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); } void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - BT_PROFILE("predictUnconstraintMotion"); - btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); - m_deformableBodySolver->predictMotion(timeStep); + BT_PROFILE("predictUnconstraintMotion"); + btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); + m_deformableBodySolver->predictMotion(timeStep); } void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) { - m_internalTime += timeStep; - m_deformableBodySolver->setImplicit(m_implicit); - m_deformableBodySolver->setLineSearch(m_lineSearch); - m_deformableBodySolver->reinitialize(m_softBodies, timeStep); - btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); - btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; - if (m_useProjection) - { - m_deformableBodySolver->m_useProjection = true; -// m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; - m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; - } - else - { - m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; - } - + m_internalTime += timeStep; + m_deformableBodySolver->setImplicit(m_implicit); + m_deformableBodySolver->setLineSearch(m_lineSearch); + m_deformableBodySolver->reinitialize(m_softBodies, timeStep); + btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); + btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; + if (m_useProjection) + { + m_deformableBodySolver->m_useProjection = true; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; + } + else + { + m_deformableBodySolver->m_useProjection = false; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; + } } - void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() { - btMultiBodyDynamicsWorld::debugDrawWorld(); for (int i = 0; i < getSoftBodyArray().size(); i++) @@ -556,253 +555,260 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags()); } } - - } void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) { - // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again - // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep - // when there are multiple substeps - btMultiBodyDynamicsWorld::applyGravity(); - // integrate rigid body gravity - for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) - { - btRigidBody* rb = m_nonStaticRigidBodies[i]; - rb->integrateVelocities(timeStep); - } - - // integrate multibody gravity - { - forwardKinematics(); - clearMultiBodyConstraintForces(); - { - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - m_scratch_r.resize(bod->getNumLinks() + 1); - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - bool isConstraintPass = false; - { - if (!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, - m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - else - { - btAssert(" RK4Integration is not supported" ); - } - } - } - } - } - } - clearGravity(); + // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again + // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep + // when there are multiple substeps + btMultiBodyDynamicsWorld::applyGravity(); + // integrate rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + rb->integrateVelocities(timeStep); + } + + // integrate multibody gravity + { + forwardKinematics(); + clearMultiBodyConstraintForces(); + { + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + m_scratch_r.resize(bod->getNumLinks() + 1); + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool isConstraintPass = false; + { + if (!bod->isUsingRK4Integration()) + { + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + else + { + btAssert(" RK4Integration is not supported"); + } + } + } + } + } + } + clearGravity(); } void btDeformableMultiBodyDynamicsWorld::clearGravity() { - BT_PROFILE("btMultiBody clearGravity"); - // clear rigid body gravity - for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive()) - { - body->clearGravity(); - } - } - // clear multibody gravity - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - bod->addBaseForce(-m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j)); - } - } - } + BT_PROFILE("btMultiBody clearGravity"); + // clear rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) + { + body->clearGravity(); + } + } + // clear multibody gravity + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + bod->addBaseForce(-m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j)); + } + } + } } void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) { - if (0 != m_internalTickCallback) - { - (*m_internalTickCallback)(this, timeStep); - } - - if (0 != m_solverCallback) - { - (*m_solverCallback)(m_internalTime, this); - } + if (0 != m_internalTickCallback) + { + (*m_internalTickCallback)(this, timeStep); + } + + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } } void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep) { - if (0 != m_solverCallback) - { - (*m_solverCallback)(m_internalTime, this); - } + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } } void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; - bool added = false; - for (int i = 0; i < forces.size(); ++i) - { - if (forces[i]->getForceType() == force->getForceType()) - { - forces[i]->addSoftBody(psb); - added = true; - break; - } - } - if (!added) - { - force->addSoftBody(psb); - force->setIndices(m_deformableBodySolver->m_objective->getIndices()); - forces.push_back(force); - } + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + bool added = false; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->addSoftBody(psb); + added = true; + break; + } + } + if (!added) + { + force->addSoftBody(psb); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + forces.push_back(force); + } } void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; - int removed_index = -1; - for (int i = 0; i < forces.size(); ++i) - { - if (forces[i]->getForceType() == force->getForceType()) - { - forces[i]->removeSoftBody(psb); - if (forces[i]->m_softBodies.size() == 0) - removed_index = i; - break; - } - } - if (removed_index >= 0) - forces.removeAtIndex(removed_index); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + int removed_index = -1; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->removeSoftBody(psb); + if (forces[i]->m_softBodies.size() == 0) + removed_index = i; + break; + } + } + if (removed_index >= 0) + forces.removeAtIndex(removed_index); +} + +void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb) +{ + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + for (int i = 0; i < forces.size(); ++i) + { + forces[i]->removeSoftBody(psb); + } } void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) { - m_softBodies.remove(body); - btCollisionWorld::removeCollisionObject(body); - // force a reinitialize so that node indices get updated. - m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1)); + removeSoftBodyForce(body); + m_softBodies.remove(body); + btCollisionWorld::removeCollisionObject(body); + // force a reinitialize so that node indices get updated. + m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1)); } void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { - btSoftBody* body = btSoftBody::upcast(collisionObject); - if (body) - removeSoftBody(body); - else - btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); } - int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { - startProfiling(timeStep); - - int numSimulationSubSteps = 0; - - if (maxSubSteps) - { - //fixed timestep with interpolation - m_fixedTimeStep = fixedTimeStep; - m_localTime += timeStep; - if (m_localTime >= fixedTimeStep) - { - numSimulationSubSteps = int(m_localTime / fixedTimeStep); - m_localTime -= numSimulationSubSteps * fixedTimeStep; - } - } - else - { - //variable timestep - fixedTimeStep = timeStep; - m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; - m_fixedTimeStep = 0; - if (btFuzzyZero(timeStep)) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } - else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (getDebugDrawer()) - { - btIDebugDraw* debugDrawer = getDebugDrawer(); - gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; - } - if (numSimulationSubSteps) - { - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; - - saveKinematicState(fixedTimeStep * clampedSimulationSteps); - - for (int i = 0; i < clampedSimulationSteps; i++) - { - internalSingleStepSimulation(fixedTimeStep); - synchronizeMotionStates(); - } - } - else - { - synchronizeMotionStates(); - } - - clearForces(); - + startProfiling(timeStep); + + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_fixedTimeStep = fixedTimeStep; + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int(m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } + else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; + m_fixedTimeStep = 0; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } + else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (getDebugDrawer()) + { + btIDebugDraw* debugDrawer = getDebugDrawer(); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (numSimulationSubSteps) + { + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; + + saveKinematicState(fixedTimeStep * clampedSimulationSteps); + + for (int i = 0; i < clampedSimulationSteps; i++) + { + internalSingleStepSimulation(fixedTimeStep); + synchronizeMotionStates(); + } + } + else + { + synchronizeMotionStates(); + } + + clearForces(); + #ifndef BT_NO_PROFILE - CProfileManager::Increment_Frame_Counter(); + CProfileManager::Increment_Frame_Counter(); #endif //BT_NO_PROFILE - - return numSimulationSubSteps; + + return numSimulationSubSteps; } diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 76b58a0378..4b7069aac7 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -36,185 +36,192 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld { - typedef btAlignedObjectArray<btVector3> TVStack; - ///Solver classes that encapsulate multiple deformable bodies for solving - btDeformableBodySolver* m_deformableBodySolver; - btSoftBodyArray m_softBodies; - int m_drawFlags; - bool m_drawNodeTree; - bool m_drawFaceTree; - bool m_drawClusterTree; - btSoftBodyWorldInfo m_sbi; - btScalar m_internalTime; - int m_ccdIterations; - bool m_implicit; - bool m_lineSearch; - bool m_useProjection; - DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; - - typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); - btSolverCallback m_solverCallback; - + typedef btAlignedObjectArray<btVector3> TVStack; + ///Solver classes that encapsulate multiple deformable bodies for solving + btDeformableBodySolver* m_deformableBodySolver; + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + btScalar m_internalTime; + int m_ccdIterations; + bool m_implicit; + bool m_lineSearch; + bool m_useProjection; + DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; + + typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); + btSolverCallback m_solverCallback; + protected: - virtual void internalSingleStepSimulation(btScalar timeStep); - - virtual void integrateTransforms(btScalar timeStep); - - void positionCorrection(btScalar timeStep); - - void solveConstraints(btScalar timeStep); - - void updateActivationState(btScalar timeStep); - - void clearGravity(); - + virtual void internalSingleStepSimulation(btScalar timeStep); + + virtual void integrateTransforms(btScalar timeStep); + + void positionCorrection(btScalar timeStep); + + void solveConstraints(btScalar timeStep); + + void updateActivationState(btScalar timeStep); + + void clearGravity(); + public: btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0); - virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); virtual void debugDrawWorld(); - void setSolverCallback(btSolverCallback cb) - { - m_solverCallback = cb; - } - - virtual ~btDeformableMultiBodyDynamicsWorld(); - - virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() - { - return (btMultiBodyDynamicsWorld*)(this); - } - - virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const - { - return (const btMultiBodyDynamicsWorld*)(this); - } - - virtual btDynamicsWorldType getWorldType() const - { - return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; - } - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); - - btSoftBodyArray& getSoftBodyArray() - { - return m_softBodies; - } - - const btSoftBodyArray& getSoftBodyArray() const - { - return m_softBodies; - } - - btSoftBodyWorldInfo& getWorldInfo() - { - return m_sbi; - } - - const btSoftBodyWorldInfo& getWorldInfo() const - { - return m_sbi; - } - - void reinitialize(btScalar timeStep); - - void applyRigidBodyGravity(btScalar timeStep); - - void beforeSolverCallbacks(btScalar timeStep); - - void afterSolverCallbacks(btScalar timeStep); - - void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); - - void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); - - void removeSoftBody(btSoftBody* body); - - void removeCollisionObject(btCollisionObject* collisionObject); - - int getDrawFlags() const { return (m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags = f; } - - void setupConstraints(); - - void performDeformableCollisionDetection(); - - void solveMultiBodyConstraints(); - - void solveContactConstraints(); - - void sortConstraints(); - - void softBodySelfCollision(); - - void setImplicit(bool implicit) - { - m_implicit = implicit; - } - - void setLineSearch(bool lineSearch) - { - m_lineSearch = lineSearch; - } - - void applyRepulsionForce(btScalar timeStep); - - void performGeometricCollisions(btScalar timeStep); - - struct btDeformableSingleRayCallback : public btBroadphaseRayCallback - { - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btDeformableMultiBodyDynamicsWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) - : m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) - { - m_rayFromTrans.setIdentity(); - m_rayFromTrans.setOrigin(m_rayFromWorld); - m_rayToTrans.setIdentity(); - m_rayToTrans.setOrigin(m_rayToWorld); - - btVector3 rayDir = (rayToWorld - rayFromWorld); - - rayDir.normalize(); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; - m_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); - } - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further ray tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + void setSolverCallback(btSolverCallback cb) + { + m_solverCallback = cb; + } + + virtual ~btDeformableMultiBodyDynamicsWorld(); + + virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() + { + return (btMultiBodyDynamicsWorld*)(this); + } + + virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const + { + return (const btMultiBodyDynamicsWorld*)(this); + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; + } + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + void reinitialize(btScalar timeStep); + + void applyRigidBodyGravity(btScalar timeStep); + + void beforeSolverCallbacks(btScalar timeStep); + + void afterSolverCallbacks(btScalar timeStep); + + void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); + + void removeSoftBodyForce(btSoftBody* psb); + + void removeSoftBody(btSoftBody* body); + + void removeCollisionObject(btCollisionObject* collisionObject); + + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } + + void setupConstraints(); + + void performDeformableCollisionDetection(); + + void solveMultiBodyConstraints(); + + void solveContactConstraints(); + + void sortConstraints(); + + void softBodySelfCollision(); + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } + + void setLineSearch(bool lineSearch) + { + m_lineSearch = lineSearch; + } + + void setUseProjection(bool useProjection) + { + m_useProjection = useProjection; + } + + void applyRepulsionForce(btScalar timeStep); + + void performGeometricCollisions(btScalar timeStep); + + struct btDeformableSingleRayCallback : public btBroadphaseRayCallback + { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btDeformableMultiBodyDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) + { + m_rayFromTrans.setIdentity(); + m_rayFromTrans.setOrigin(m_rayFromWorld); + m_rayToTrans.setIdentity(); + m_rayToTrans.setOrigin(m_rayToWorld); + + btVector3 rayDir = (rayToWorld - rayFromWorld); + + rayDir.normalize(); + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; #if 0 #ifdef RECALCULATE_AABB btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -225,87 +232,85 @@ public: const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; #endif #endif - //btScalar hitLambda = m_resultCallback.m_closestHitFraction; - //culling already done by broadphase - //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) - { - m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); - } - } - return true; - } - }; - - - - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const - { - BT_PROFILE("rayTest"); - /// use the broadphase to accelerate the search for objects, based on their aabb - /// and for each object with ray-aabb overlap, perform an exact ray test - btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); - + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) + { + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); + } + } + return true; + } + }; + + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const + { + BT_PROFILE("rayTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); + #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i = 0; i < this->getNumCollisionObjects(); i++) - { - rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } + for (int i = 0; i < this->getNumCollisionObjects(); i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } #endif //USE_BRUTEFORCE_RAYBROADPHASE - } - - void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) const - { - if (collisionShape->isSoftBody()) - { - btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) - { - btSoftBody::sRayCast softResult; - if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) - { - if (softResult.fraction <= resultCallback.m_closestHitFraction) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = 0; - shapeInfo.m_triangleIndex = softResult.index; - // get the normal - btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); - btVector3 normal = -rayDir; - normal.normalize(); - { - normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) - { - // normal always point toward origin of the ray - normal = -normal; - } - } - - btCollisionWorld::LocalRayResult rayResult(collisionObject, - &shapeInfo, - normal, - softResult.fraction); - bool normalInWorldSpace = true; - resultCallback.addSingleResult(rayResult, normalInWorldSpace); - } - } - } - } - else - { - btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); - } - } + } + + void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) const + { + if (collisionShape->isSoftBody()) + { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) + { + btSoftBody::sRayCast softResult; + if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + { + if (softResult.fraction <= resultCallback.m_closestHitFraction) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = 0; + shapeInfo.m_triangleIndex = softResult.index; + // get the normal + btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); + btVector3 normal = -rayDir; + normal.normalize(); + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) + { + // normal always point toward origin of the ray + normal = -normal; + } + } + + btCollisionWorld::LocalRayResult rayResult(collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult, normalInWorldSpace); + } + } + } + } + else + { + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); + } + } }; #endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h b/thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h index d89bc4aca4..60798c5bcd 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -23,30 +23,30 @@ subject to the following restrictions: class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { public: - typedef btAlignedObjectArray<btVector3> TVStack; - btScalar m_mu, m_lambda; // Lame Parameters - btScalar m_E, m_nu; // Young's modulus and Poisson ratio - btScalar m_mu_damp, m_lambda_damp; - btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) - { - btScalar damping = 0.05; - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; + typedef btAlignedObjectArray<btVector3> TVStack; + btScalar m_mu, m_lambda; // Lame Parameters + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_mu_damp, m_lambda_damp; + btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1) + { + btScalar damping = 0.05; + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; updateYoungsModulusAndPoissonRatio(); - } - - btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) - { - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; + } + + btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda) + { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; updateYoungsModulusAndPoissonRatio(); - } + } void updateYoungsModulusAndPoissonRatio() { // conversion from Lame Parameters to Young's modulus and Poisson ratio // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters - m_E = m_mu * (3*m_lambda + 2*m_mu)/(m_lambda + m_mu); + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); m_nu = m_lambda * 0.5 / (m_mu + m_lambda); } @@ -55,21 +55,21 @@ public: // conversion from Young's modulus and Poisson ratio to Lame Parameters // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters m_mu = m_E * 0.5 / (1 + m_nu); - m_lambda = m_E * m_nu / ((1 + m_nu) * (1- 2*m_nu)); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); } - void setYoungsModulus(btScalar E) - { + void setYoungsModulus(btScalar E) + { m_E = E; updateLameParameters(); - } + } void setPoissonRatio(btScalar nu) { m_nu = nu; updateLameParameters(); } - + void setDamping(btScalar damping) { m_mu_damp = damping * m_mu; @@ -83,339 +83,338 @@ public: updateYoungsModulusAndPoissonRatio(); } - virtual void addScaledForces(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - addScaledElasticForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForce(btScalar scale, TVStack& force) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; -// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz + 1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + density += m_mu * 0.5 * (s.m_trace - 3.); + density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda); + density -= m_mu * 0.5 * log(s.m_trace + 1); + return density; + } - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * df_on_node0; - force[id1] -= scale1 * df_on_node123.getColumn(0); - force[id2] -= scale1 * df_on_node123.getColumn(1); - force[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual double totalElasticEnergy(btScalar dt) - { - double energy = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetraScratches.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; - energy += tetra.m_element_measure * elasticEnergyDensity(s); - } - } - return energy; - } - - // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual double totalDampingEnergy(btScalar dt) - { - double energy = 0; - int sz = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - sz = btMax(sz, psb->m_nodes[j].index); - } - } - TVStack dampingForce; - dampingForce.resize(sz+1); - for (int i = 0; i < dampingForce.size(); ++i) - dampingForce[i].setZero(); - addScaledDampingForce(0.5, dampingForce); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - energy -= dampingForce[node.index].dot(node.m_v) / dt; - } - } - return energy; - } - - double elasticEnergyDensity(const btSoftBody::TetraScratch& s) - { - double density = 0; - density += m_mu * 0.5 * (s.m_trace - 3.); - density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda); - density -= m_mu * 0.5 * log(s.m_trace+1); - return density; - } - - virtual void addScaledElasticForce(btScalar scale, TVStack& force) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= force.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - btScalar max_p = psb->m_cfg.m_maxStress; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btMatrix3x3 P; - firstPiola(psb->m_tetraScratches[j],P); + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); #ifdef USE_SVD - if (max_p > 0) - { - // since we want to clamp the principal stress to max_p, we only need to - // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p - btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); - if (trPTP > max_p * max_p) - { - btMatrix3x3 U, V; - btVector3 sigma; - singularValueDecomposition(P, U, sigma, V); - sigma[0] = btMin(sigma[0], max_p); - sigma[1] = btMin(sigma[1], max_p); - sigma[2] = btMin(sigma[2], max_p); - sigma[0] = btMax(sigma[0], -max_p); - sigma[1] = btMax(sigma[1], -max_p); - sigma[2] = btMax(sigma[2], -max_p); - btMatrix3x3 Sigma; - Sigma.setIdentity(); - Sigma[0][0] = sigma[0]; - Sigma[1][1] = sigma[1]; - Sigma[2][2] = sigma[2]; - P = U * Sigma * V.transpose(); - } - } + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } #endif -// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); - btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; - - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - - // elastic force - btScalar scale1 = scale * tetra.m_element_measure; - force[id0] -= scale1 * force_on_node0; - force[id1] -= scale1 * force_on_node123.getColumn(0); - force[id2] -= scale1 * force_on_node123.getColumn(1); - force[id3] -= scale1 * force_on_node123.getColumn(2); - } - } - } - - // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search - virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { - if (m_mu_damp == 0 && m_lambda_damp == 0) - return; - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; - btMatrix3x3 I; - I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; -// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); -// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.))); + btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); + P = s.m_F * c1 + s.m_cofF * c2; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.)); + btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))); + btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP); + dP += s.m_cofF * c3; + } - // damping force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA){} - - virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) - { - int numNodes = getNumNodes(); - btAssert(numNodes <= df.size()); - btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - btSoftBody::Tetra& tetra = psb->m_tetras[j]; - btSoftBody::Node* node0 = tetra.m_n[0]; - btSoftBody::Node* node1 = tetra.m_n[1]; - btSoftBody::Node* node2 = tetra.m_n[2]; - btSoftBody::Node* node3 = tetra.m_n[3]; - size_t id0 = node0->index; - size_t id1 = node1->index; - size_t id2 = node2->index; - size_t id3 = node3->index; - btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; - btMatrix3x3 dP; - firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); -// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; - - // elastic force differential - btScalar scale1 = scale * tetra.m_element_measure; - df[id0] -= scale1 * df_on_node0; - df[id1] -= scale1 * df_on_node123.getColumn(0); - df[id2] -= scale1 * df_on_node123.getColumn(1); - df[id3] -= scale1 * df_on_node123.getColumn(2); - } - } - } - - void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) - { - btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.))); - btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); - P = s.m_F * c1 + s.m_cofF * c2; - } - - // Let P be the first piola stress. - // This function calculates the dP = dP/dF * dF - void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.)); - btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); - btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); - dP = dF * c1 + s.m_F * c2; - addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP); - dP += s.m_cofF * c3; - } - - // Let Q be the damping stress. - // This function calculates the dP = dQ/dF * dF - void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) - { - btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.))); - btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace)))); - btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); - dP = dF * c1 + s.m_F * c2; - addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP); - dP += s.m_cofF * c3; - } - - btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) - { - btScalar ans = 0; - for (int i = 0; i < 3; ++i) - { - ans += A[i].dot(B[i]); - } - return ans; - } - - // Let C(A) be the cofactor of the matrix A - // Let H = the derivative of C(A) with respect to A evaluated at F = A - // This function calculates H*dF - void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) - { - M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); - M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); - M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); - M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); - M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); - M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); - M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); - M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); - M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); - } - - virtual btDeformableLagrangianForceType getForceType() - { - return BT_NEOHOOKEAN_FORCE; - } - + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.))); + btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)))); + btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; + addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP); + dP += s.m_cofF * c3; + } + + btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) + { + btScalar ans = 0; + for (int i = 0; i < 3; ++i) + { + ans += A[i].dot(B[i]); + } + return ans; + } + + // Let C(A) be the cofactor of the matrix A + // Let H = the derivative of C(A) with respect to A evaluated at F = A + // This function calculates H*dF + void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) + { + M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); + M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); + M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); + M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); + M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); + M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); + M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); + M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); + M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_NEOHOOKEAN_FORCE; + } }; #endif /* BT_NEOHOOKEAN_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btKrylovSolver.h b/thirdparty/bullet/BulletSoftBody/btKrylovSolver.h new file mode 100644 index 0000000000..59126b47ae --- /dev/null +++ b/thirdparty/bullet/BulletSoftBody/btKrylovSolver.h @@ -0,0 +1,107 @@ +/* + Written by Xuchen Han <xuchenhan2015@u.northwestern.edu> + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_KRYLOV_SOLVER_H +#define BT_KRYLOV_SOLVER_H +#include <iostream> +#include <cmath> +#include <limits> +#include <LinearMath/btAlignedObjectArray.h> +#include <LinearMath/btVector3.h> +#include <LinearMath/btScalar.h> +#include "LinearMath/btQuickprof.h" + +template <class MatrixX> +class btKrylovSolver +{ + typedef btAlignedObjectArray<btVector3> TVStack; + +public: + int m_maxIterations; + btScalar m_tolerance; + btKrylovSolver(int maxIterations, btScalar tolerance) + : m_maxIterations(maxIterations), m_tolerance(tolerance) + { + } + + virtual ~btKrylovSolver() {} + + virtual int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) = 0; + + virtual void reinitialize(const TVStack& b) = 0; + + virtual SIMD_FORCE_INLINE TVStack sub(const TVStack& a, const TVStack& b) + { + // c = a-b + btAssert(a.size() == b.size()); + TVStack c; + c.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + { + c[i] = a[i] - b[i]; + } + return c; + } + + virtual SIMD_FORCE_INLINE btScalar squaredNorm(const TVStack& a) + { + return dot(a, a); + } + + virtual SIMD_FORCE_INLINE btScalar norm(const TVStack& a) + { + btScalar ret = 0; + for (int i = 0; i < a.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + ret = btMax(ret, btFabs(a[i][d])); + } + } + return ret; + } + + virtual SIMD_FORCE_INLINE btScalar dot(const TVStack& a, const TVStack& b) + { + btScalar ans(0); + for (int i = 0; i < a.size(); ++i) + ans += a[i].dot(b[i]); + return ans; + } + + virtual SIMD_FORCE_INLINE void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) + { + // result += s*a + btAssert(a.size() == result.size()); + for (int i = 0; i < a.size(); ++i) + result[i] += s * a[i]; + } + + virtual SIMD_FORCE_INLINE TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) + { + // result = a*s + b + TVStack result; + result.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + result[i] = s * a[i] + b[i]; + return result; + } + + virtual SIMD_FORCE_INLINE void setTolerance(btScalar tolerance) + { + m_tolerance = tolerance; + } +}; +#endif /* BT_KRYLOV_SOLVER_H */ diff --git a/thirdparty/bullet/BulletSoftBody/btPreconditioner.h b/thirdparty/bullet/BulletSoftBody/btPreconditioner.h index c2db448ef8..21c1106a42 100644 --- a/thirdparty/bullet/BulletSoftBody/btPreconditioner.h +++ b/thirdparty/bullet/BulletSoftBody/btPreconditioner.h @@ -19,269 +19,266 @@ class Preconditioner { public: - typedef btAlignedObjectArray<btVector3> TVStack; - virtual void operator()(const TVStack& x, TVStack& b) = 0; - virtual void reinitialize(bool nodeUpdated) = 0; - virtual ~Preconditioner(){} + typedef btAlignedObjectArray<btVector3> TVStack; + virtual void operator()(const TVStack& x, TVStack& b) = 0; + virtual void reinitialize(bool nodeUpdated) = 0; + virtual ~Preconditioner() {} }; class DefaultPreconditioner : public Preconditioner { public: - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - for (int i = 0; i < b.size(); ++i) - b[i] = x[i]; - } - virtual void reinitialize(bool nodeUpdated) - { - } - - virtual ~DefaultPreconditioner(){} + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i]; + } + virtual void reinitialize(bool nodeUpdated) + { + } + + virtual ~DefaultPreconditioner() {} }; class MassPreconditioner : public Preconditioner { - btAlignedObjectArray<btScalar> m_inv_mass; - const btAlignedObjectArray<btSoftBody *>& m_softBodies; + btAlignedObjectArray<btScalar> m_inv_mass; + const btAlignedObjectArray<btSoftBody*>& m_softBodies; + public: - MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies) - : m_softBodies(softBodies) - { - } - - virtual void reinitialize(bool nodeUpdated) - { - if (nodeUpdated) - { - m_inv_mass.clear(); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - m_inv_mass.push_back(psb->m_nodes[j].m_im); - } - } - } - - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - btAssert(m_inv_mass.size() <= x.size()); - for (int i = 0; i < m_inv_mass.size(); ++i) - { - b[i] = x[i] * m_inv_mass[i]; - } - for (int i = m_inv_mass.size(); i < b.size(); ++i) - { - b[i] = x[i]; - } - } -}; + MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies) + : m_softBodies(softBodies) + { + } + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + m_inv_mass.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + m_inv_mass.push_back(psb->m_nodes[j].m_im); + } + } + } + + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + btAssert(m_inv_mass.size() <= x.size()); + for (int i = 0; i < m_inv_mass.size(); ++i) + { + b[i] = x[i] * m_inv_mass[i]; + } + for (int i = m_inv_mass.size(); i < b.size(); ++i) + { + b[i] = x[i]; + } + } +}; class KKTPreconditioner : public Preconditioner { - const btAlignedObjectArray<btSoftBody *>& m_softBodies; - const btDeformableContactProjection& m_projections; - const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf; - TVStack m_inv_A, m_inv_S; - const btScalar& m_dt; - const bool& m_implicit; + const btAlignedObjectArray<btSoftBody*>& m_softBodies; + const btDeformableContactProjection& m_projections; + const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf; + TVStack m_inv_A, m_inv_S; + const btScalar& m_dt; + const bool& m_implicit; + public: - KKTPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit) - : m_softBodies(softBodies) - , m_projections(projections) - , m_lf(lf) - , m_dt(dt) - , m_implicit(implicit) - { - } - - virtual void reinitialize(bool nodeUpdated) - { - if (nodeUpdated) - { - int num_nodes = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - num_nodes += psb->m_nodes.size(); - } - m_inv_A.resize(num_nodes); - } - buildDiagonalA(m_inv_A); - for (int i = 0; i < m_inv_A.size(); ++i) - { -// printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]); - for (int d = 0; d < 3; ++d) - { - m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0/ m_inv_A[i][d]; - } - } - m_inv_S.resize(m_projections.m_lagrangeMultipliers.size()); -// printf("S.size() = %d \n", m_inv_S.size()); - buildDiagonalS(m_inv_A, m_inv_S); - for (int i = 0; i < m_inv_S.size(); ++i) - { -// printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]); - for (int d = 0; d < 3; ++d) - { - m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0/ m_inv_S[i][d]; - } - } - } - - void buildDiagonalA(TVStack& diagA) const - { - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - const btSoftBody::Node& node = psb->m_nodes[j]; - diagA[counter] = (node.m_im == 0) ? btVector3(0,0,0) : btVector3(1.0/node.m_im, 1.0 / node.m_im, 1.0 / node.m_im); - ++counter; - } - } - if (m_implicit) - { - printf("implicit not implemented\n"); - btAssert(false); - } - for (int i = 0; i < m_lf.size(); ++i) - { - // add damping matrix - m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA); - } - } - - void buildDiagonalS(const TVStack& inv_A, TVStack& diagS) - { - for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) - { - // S[k,k] = e_k^T * C A_d^-1 C^T * e_k - const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; - btVector3& t = diagS[c]; - t.setZero(); - for (int j = 0; j < lm.m_num_constraints; ++j) - { - for (int i = 0; i < lm.m_num_nodes; ++i) - { - for (int d = 0; d < 3; ++d) - { - t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i]; - } - } - } - } - } -#define USE_FULL_PRECONDITIONER + KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit) + : m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit) + { + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + int num_nodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + num_nodes += psb->m_nodes.size(); + } + m_inv_A.resize(num_nodes); + } + buildDiagonalA(m_inv_A); + for (int i = 0; i < m_inv_A.size(); ++i) + { + // printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]); + for (int d = 0; d < 3; ++d) + { + m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d]; + } + } + m_inv_S.resize(m_projections.m_lagrangeMultipliers.size()); + // printf("S.size() = %d \n", m_inv_S.size()); + buildDiagonalS(m_inv_A, m_inv_S); + for (int i = 0; i < m_inv_S.size(); ++i) + { + // printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]); + for (int d = 0; d < 3; ++d) + { + m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d]; + } + } + } + + void buildDiagonalA(TVStack& diagA) const + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im); + ++counter; + } + } + if (m_implicit) + { + printf("implicit not implemented\n"); + btAssert(false); + } + for (int i = 0; i < m_lf.size(); ++i) + { + // add damping matrix + m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA); + } + } + + void buildDiagonalS(const TVStack& inv_A, TVStack& diagS) + { + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // S[k,k] = e_k^T * C A_d^-1 C^T * e_k + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + btVector3& t = diagS[c]; + t.setZero(); + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int d = 0; d < 3; ++d) + { + t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i]; + } + } + } + } + } +//#define USE_FULL_PRECONDITIONER #ifndef USE_FULL_PRECONDITIONER - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i] = x[i] * m_inv_A[i]; - } - int offset = m_inv_A.size(); - for (int i = 0; i < m_inv_S.size(); ++i) - { - b[i+offset] = x[i+offset] * m_inv_S[i]; - } - } + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } + int offset = m_inv_A.size(); + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] = x[i + offset] * m_inv_S[i]; + } + } #else - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - int offset = m_inv_A.size(); + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + int offset = m_inv_A.size(); - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i] = x[i] * m_inv_A[i]; - } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = x[i] * m_inv_A[i]; + } - for (int i = 0; i < m_inv_S.size(); ++i) - { - b[i+offset].setZero(); - } + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset].setZero(); + } - for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) - { - const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; - // C * x - for (int d = 0; d < lm.m_num_constraints; ++d) - { - for (int i = 0; i < lm.m_num_nodes; ++i) - { - b[offset+c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]); - } - } - } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + // C * x + for (int d = 0; d < lm.m_num_constraints; ++d) + { + for (int i = 0; i < lm.m_num_nodes; ++i) + { + b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]); + } + } + } - for (int i = 0; i < m_inv_S.size(); ++i) - { - b[i+offset] = b[i+offset] * m_inv_S[i]; - } + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] = b[i + offset] * m_inv_S[i]; + } - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i].setZero(); - } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i].setZero(); + } - for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) - { - // C^T * lambda - const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; - for (int i = 0; i < lm.m_num_nodes; ++i) - { - for (int j = 0; j < lm.m_num_constraints; ++j) - { - b[lm.m_indices[i]] += b[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; - } - } - } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i] = (x[i] - b[i]) * m_inv_A[i]; - } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] = (x[i] - b[i]) * m_inv_A[i]; + } - TVStack t; - t.resize(b.size()); - for (int i = 0; i < m_inv_S.size(); ++i) - { - t[i+offset] = x[i+offset] * m_inv_S[i]; - } - for (int i = 0; i < m_inv_A.size(); ++i) - { - t[i].setZero(); - } - for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) - { - // C^T * lambda - const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; - for (int i = 0; i < lm.m_num_nodes; ++i) - { - for (int j = 0; j < lm.m_num_constraints; ++j) - { - t[lm.m_indices[i]] += t[offset+c][j] * lm.m_weights[i] * lm.m_dirs[j]; - } - } - } - for (int i = 0; i < m_inv_A.size(); ++i) - { - b[i] += t[i] * m_inv_A[i]; - } + TVStack t; + t.resize(b.size()); + for (int i = 0; i < m_inv_S.size(); ++i) + { + t[i + offset] = x[i + offset] * m_inv_S[i]; + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + t[i].setZero(); + } + for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c) + { + // C^T * lambda + const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c]; + for (int i = 0; i < lm.m_num_nodes; ++i) + { + for (int j = 0; j < lm.m_num_constraints; ++j) + { + t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j]; + } + } + } + for (int i = 0; i < m_inv_A.size(); ++i) + { + b[i] += t[i] * m_inv_A[i]; + } - for (int i = 0; i < m_inv_S.size(); ++i) - { - b[i+offset] -= x[i+offset] * m_inv_S[i]; - } - } + for (int i = 0; i < m_inv_S.size(); ++i) + { + b[i + offset] -= x[i + offset] * m_inv_S[i]; + } + } #endif }; diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp index 81b846d7f8..d1980ea6c5 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp @@ -37,12 +37,12 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l { btAlignedObjectArray<bool> marked; btAlignedObjectArray<btDbvtNode*> newLeafNodes; - btAlignedObjectArray<std::pair<int,int> > childIds; + btAlignedObjectArray<std::pair<int, int> > childIds; btAlignedObjectArray<btAlignedObjectArray<int> > newAdj; marked.resize(N); for (int i = 0; i < N; ++i) marked[i] = false; - + // pair adjacent nodes into new(parent) node for (int i = 0; i < N; ++i) { @@ -61,7 +61,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l leafNodes[i]->parent = node; leafNodes[n]->parent = node; newLeafNodes.push_back(node); - childIds.push_back(std::make_pair(i,n)); + childIds.push_back(std::make_pair(i, n)); merged = true; marked[n] = true; break; @@ -70,7 +70,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l if (!merged) { newLeafNodes.push_back(leafNodes[i]); - childIds.push_back(std::make_pair(i,-1)); + childIds.push_back(std::make_pair(i, -1)); } marked[i] = true; } @@ -78,7 +78,7 @@ static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& l newAdj.resize(newLeafNodes.size()); for (int i = 0; i < newLeafNodes.size(); ++i) { - for (int j = i+1; j < newLeafNodes.size(); ++j) + for (int j = i + 1; j < newLeafNodes.size(); ++j) { bool neighbor = false; const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first]; @@ -143,7 +143,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV /* Nodes */ const btScalar margin = getCollisionShape()->getMargin(); m_nodes.resize(node_count); - m_X.resize(node_count); + m_X.resize(node_count); for (int i = 0, ni = node_count; i < ni; ++i) { Node& n = m_nodes[i]; @@ -154,7 +154,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV n.m_im = n.m_im > 0 ? 1 / n.m_im : 0; n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n); n.m_material = pm; - m_X[i] = n.m_x; + m_X[i] = n.m_x; } updateBounds(); setCollisionQuadrature(3); @@ -195,8 +195,8 @@ void btSoftBody::initDefaults() m_cfg.piterations = 1; m_cfg.diterations = 0; m_cfg.citerations = 4; - m_cfg.drag = 0; - m_cfg.m_maxStress = 0; + m_cfg.drag = 0; + m_cfg.m_maxStress = 0; m_cfg.collisions = fCollision::Default; m_pose.m_bvolume = false; m_pose.m_bframe = false; @@ -222,12 +222,14 @@ void btSoftBody::initDefaults() m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); m_dampingCoefficient = 1.0; - m_sleepingThreshold = .4; + m_sleepingThreshold = .04; m_useSelfCollision = false; m_collisionFlags = 0; m_softSoftCollision = false; m_maxSpeedSquared = 0; m_repulsionStiffness = 0.5; + m_gravityFactor = 1; + m_cacheBarycenter = false; m_fdbvnt = 0; } @@ -436,7 +438,7 @@ void btSoftBody::appendFace(int model, Material* mat) ZeroInitialize(f); f.m_material = mat ? mat : m_materials[0]; } - m_faces.push_back(f); + m_faces.push_back(f); } // @@ -525,94 +527,111 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca // void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) { - DeformableNodeRigidAnchor c; - btSoftBody::Node& n = m_nodes[node]; - const btScalar ima = n.m_im; - const btScalar imb = body->getInvMass(); - btVector3 nrm; - const btCollisionShape* shp = body->getCollisionShape(); - const btTransform& wtr = body->getWorldTransform(); - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(m_nodes[node].m_x), - shp, - nrm, - 0); - - c.m_cti.m_colObj = body; - c.m_cti.m_normal = wtr.getBasis() * nrm; - c.m_cti.m_offset = dst; - c.m_node = &m_nodes[node]; - const btScalar fc = m_cfg.kDF * body->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); - const btVector3 ra = n.m_x - wtr.getOrigin(); - - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; - c.m_node->m_battach = 1; - m_deformableAnchors.push_back(c); + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + const btScalar imb = body->getInvMass(); + btVector3 nrm; + const btCollisionShape* shp = body->getCollisionShape(); + const btTransform& wtr = body->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + + c.m_cti.m_colObj = body; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * body->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); +} + +void btSoftBody::removeAnchor(int node) +{ + const btSoftBody::Node& n = m_nodes[node]; + for (int i = 0; i < m_deformableAnchors.size();) + { + const DeformableNodeRigidAnchor& c = m_deformableAnchors[i]; + if (c.m_node == &n) + { + m_deformableAnchors.removeAtIndex(i); + } + else + { + i++; + } + } } // void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link) { - DeformableNodeRigidAnchor c; - btSoftBody::Node& n = m_nodes[node]; - const btScalar ima = n.m_im; - btVector3 nrm; - const btCollisionShape* shp = link->getCollisionShape(); - const btTransform& wtr = link->getWorldTransform(); - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(m_nodes[node].m_x), - shp, - nrm, - 0); - c.m_cti.m_colObj = link; - c.m_cti.m_normal = wtr.getBasis() * nrm; - c.m_cti.m_offset = dst; - c.m_node = &m_nodes[node]; - const btScalar fc = m_cfg.kDF * link->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; - btVector3 normal = c.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); - findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); - findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = link->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - const btVector3 ra = n.m_x - wtr.getOrigin(); - c.m_c1 = ra; - c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; - c.m_node->m_battach = 1; - m_deformableAnchors.push_back(c); + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + btVector3 nrm; + const btCollisionShape* shp = link->getCollisionShape(); + const btTransform& wtr = link->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + c.m_cti.m_colObj = link; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * link->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + btVector3 normal = c.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = link->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + const btVector3 ra = n.m_x - wtr.getOrigin(); + c.m_c1 = ra; + c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); } // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) @@ -731,7 +750,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack - // cos(10º) = 0.98480 + // cos(10º) = 0.98480 if (0 < n_dot_v && n_dot_v < 0.98480f) fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); @@ -817,7 +836,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack - // cos(10º) = 0.98480 + // cos(10º) = 0.98480 if (0 < n_dot_v && n_dot_v < 0.98480f) fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); @@ -882,6 +901,7 @@ void btSoftBody::setVelocity(const btVector3& velocity) if (n.m_im > 0) { n.m_v = velocity; + n.m_vn = velocity; } } } @@ -1010,66 +1030,70 @@ void btSoftBody::setVolumeDensity(btScalar density) // btVector3 btSoftBody::getLinearVelocity() { - btVector3 total_momentum = btVector3(0,0,0); - for (int i = 0; i < m_nodes.size(); ++i) - { - btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0/m_nodes[i].m_im; - total_momentum += mass * m_nodes[i].m_v; - } - btScalar total_mass = getTotalMass(); - return total_mass == 0 ? total_momentum : total_momentum / total_mass; + btVector3 total_momentum = btVector3(0, 0, 0); + for (int i = 0; i < m_nodes.size(); ++i) + { + btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0 / m_nodes[i].m_im; + total_momentum += mass * m_nodes[i].m_v; + } + btScalar total_mass = getTotalMass(); + return total_mass == 0 ? total_momentum : total_momentum / total_mass; } // void btSoftBody::setLinearVelocity(const btVector3& linVel) { - btVector3 old_vel = getLinearVelocity(); - btVector3 diff = linVel - old_vel; - for (int i = 0; i < m_nodes.size(); ++i) - m_nodes[i].m_v += diff; + btVector3 old_vel = getLinearVelocity(); + btVector3 diff = linVel - old_vel; + for (int i = 0; i < m_nodes.size(); ++i) + m_nodes[i].m_v += diff; } // void btSoftBody::setAngularVelocity(const btVector3& angVel) { - btVector3 old_vel = getLinearVelocity(); - btVector3 com = getCenterOfMass(); - for (int i = 0; i < m_nodes.size(); ++i) - { - m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel; - } + btVector3 old_vel = getLinearVelocity(); + btVector3 com = getCenterOfMass(); + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel; + } } // btTransform btSoftBody::getRigidTransform() { - btVector3 t = getCenterOfMass(); - btMatrix3x3 S; - S.setZero(); - // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || - for (int i = 0; i < m_nodes.size(); ++i) - { - S += OuterProduct(m_X[i], t-m_nodes[i].m_x); - } - btVector3 sigma; - btMatrix3x3 U,V; - singularValueDecomposition(S,U,sigma,V); - btMatrix3x3 R = V * U.transpose(); - btTransform trs; - trs.setIdentity(); - trs.setOrigin(t); - trs.setBasis(R); - return trs; + btVector3 t = getCenterOfMass(); + btMatrix3x3 S; + S.setZero(); + // Get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i || + // It's important to make sure that S has the correct signs. + // SVD is only unique up to the ordering of singular values. + // SVD will manipulate U and V to ensure the ordering of singular values. If all three singular + // vaues are negative, SVD will permute colums of U to make two of them positive. + for (int i = 0; i < m_nodes.size(); ++i) + { + S -= OuterProduct(m_X[i], t - m_nodes[i].m_x); + } + btVector3 sigma; + btMatrix3x3 U, V; + singularValueDecomposition(S, U, sigma, V); + btMatrix3x3 R = V * U.transpose(); + btTransform trs; + trs.setIdentity(); + trs.setOrigin(t); + trs.setBasis(R); + return trs; } // void btSoftBody::transformTo(const btTransform& trs) { - // get the current best rigid fit - btTransform current_transform = getRigidTransform(); - // apply transform in material space - btTransform new_transform = trs * current_transform.inverse(); - transform(new_transform); + // get the current best rigid fit + btTransform current_transform = getRigidTransform(); + // apply transform in material space + btTransform new_transform = trs * current_transform.inverse(); + transform(new_transform); } // @@ -1130,7 +1154,7 @@ void btSoftBody::scale(const btVector3& scl) updateNormals(); updateBounds(); updateConstants(); - initializeDmInverse(); + initializeDmInverse(); } // @@ -2010,22 +2034,22 @@ bool btSoftBody::rayTest(const btVector3& rayFrom, } bool btSoftBody::rayFaceTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) + const btVector3& rayTo, + sRayCast& results) { if (m_faces.size() == 0) return false; else { - if (m_fdbvt.empty()) - initializeFaceTree(); + if (m_fdbvt.empty()) + initializeFaceTree(); } - - results.body = this; - results.fraction = 1.f; - results.index = -1; - - return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); + + results.body = this; + results.fraction = 1.f; + results.index = -1; + + return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); } // @@ -2056,112 +2080,111 @@ void btSoftBody::setSolver(eSolverPresets::_ preset) void btSoftBody::predictMotion(btScalar dt) { - int i, ni; - - /* Update */ - if (m_bUpdateRtCst) - { - m_bUpdateRtCst = false; - updateConstants(); - m_fdbvt.clear(); - if (m_cfg.collisions & fCollision::VF_SS) - { - initializeFaceTree(); - } - } - - /* Prepare */ - m_sst.sdt = dt * m_cfg.timescale; - m_sst.isdt = 1 / m_sst.sdt; - m_sst.velmrg = m_sst.sdt * 3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; - /* Forces */ - addVelocity(m_worldInfo->m_gravity * m_sst.sdt); - applyForces(); - /* Integrate */ - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - n.m_q = n.m_x; - btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; - { - btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / m_sst.sdt; - for (int c = 0; c < 3; c++) - { - if (deltaV[c] > clampDeltaV) - { - deltaV[c] = clampDeltaV; - } - if (deltaV[c] < -clampDeltaV) - { - deltaV[c] = -clampDeltaV; - } - } - } - n.m_v += deltaV; - n.m_x += n.m_v * m_sst.sdt; - n.m_f = btVector3(0, 0, 0); - } - /* Clusters */ - updateClusters(); - /* Bounds */ - updateBounds(); - /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) - vol; - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); - m_ndbvt.update(n.m_leaf, - vol, - n.m_v * m_sst.velmrg, - m_sst.updmrg); - } - /* Faces */ - if (!m_fdbvt.empty()) - { - for (int i = 0; i < m_faces.size(); ++i) - { - Face& f = m_faces[i]; - const btVector3 v = (f.m_n[0]->m_v + - f.m_n[1]->m_v + - f.m_n[2]->m_v) / - 3; - vol = VolumeOf(f, m_sst.radmrg); - m_fdbvt.update(f.m_leaf, - vol, - v * m_sst.velmrg, - m_sst.updmrg); - } - } - /* Pose */ - updatePose(); - /* Match */ - if (m_pose.m_bframe && (m_cfg.kMT > 0)) - { - const btMatrix3x3 posetrs = m_pose.m_rot; - for (int i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - if (n.m_im > 0) - { - const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; - n.m_x = Lerp(n.m_x, x, m_cfg.kMT); - } - } - } - /* Clear contacts */ - m_rcontacts.resize(0); - m_scontacts.resize(0); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); -} + int i, ni; + /* Update */ + if (m_bUpdateRtCst) + { + m_bUpdateRtCst = false; + updateConstants(); + m_fdbvt.clear(); + if (m_cfg.collisions & fCollision::VF_SS) + { + initializeFaceTree(); + } + } + + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); + applyForces(); + /* Integrate */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + n.m_q = n.m_x; + btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; + { + btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) + { + if (deltaV[c] > clampDeltaV) + { + deltaV[c] = clampDeltaV; + } + if (deltaV[c] < -clampDeltaV) + { + deltaV[c] = -clampDeltaV; + } + } + } + n.m_v += deltaV; + n.m_x += n.m_v * m_sst.sdt; + n.m_f = btVector3(0, 0, 0); + } + /* Clusters */ + updateClusters(); + /* Bounds */ + updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); + m_ndbvt.update(n.m_leaf, + vol, + n.m_v * m_sst.velmrg, + m_sst.updmrg); + } + /* Faces */ + if (!m_fdbvt.empty()) + { + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + vol, + v * m_sst.velmrg, + m_sst.updmrg); + } + } + /* Pose */ + updatePose(); + /* Match */ + if (m_pose.m_bframe && (m_cfg.kMT > 0)) + { + const btMatrix3x3 posetrs = m_pose.m_rot; + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + if (n.m_im > 0) + { + const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; + n.m_x = Lerp(n.m_x, x, m_cfg.kMT); + } + } + } + /* Clear contacts */ + m_rcontacts.resize(0); + m_scontacts.resize(0); + /* Optimize dbvt's */ + m_ndbvt.optimizeIncremental(1); + m_fdbvt.optimizeIncremental(1); + m_cdbvt.optimizeIncremental(1); +} // void btSoftBody::solveConstraints() @@ -2534,12 +2557,12 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, } int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, - btScalar& mint, int& index) const + btScalar& mint, int& index) const { int cnt = 0; { /* Use dbvt */ RayFromToCaster collider(rayFrom, rayTo, mint); - + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); if (collider.m_face) { @@ -2551,7 +2574,6 @@ int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, return (cnt); } - // static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) { @@ -2580,7 +2602,7 @@ static inline void calculateNormalCone(btDbvntNode* root) } else { - btVector3 n0(0,0,0), n1(0,0,0); + btVector3 n0(0, 0, 0), n1(0, 0, 0); btScalar a0 = 0, a1 = 0; if (root->childs[0]) { @@ -2594,8 +2616,8 @@ static inline void calculateNormalCone(btDbvntNode* root) n1 = root->childs[1]->normal; a1 = root->childs[1]->angle; } - root->normal = (n0+n1).safeNormalize(); - root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5; + root->normal = (n0 + n1).safeNormalize(); + root->angle = btMax(a0, a1) + btAngle(n0, n1) * 0.5; } } @@ -2609,7 +2631,8 @@ void btSoftBody::initializeFaceTree() for (int i = 0; i < m_faces.size(); ++i) { Face& f = m_faces[i]; - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = VolumeOf(f, 0); btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); node->parent = NULL; node->data = &f; @@ -2623,7 +2646,7 @@ void btSoftBody::initializeFaceTree() // construct the adjacency list for triangles for (int i = 0; i < adj.size(); ++i) { - for (int j = i+1; j < adj.size(); ++j) + for (int j = i + 1; j < adj.size(); ++j) { int dup = 0; for (int k = 0; k < 3; ++k) @@ -2661,7 +2684,8 @@ void btSoftBody::rebuildNodeTree() for (int i = 0; i < m_nodes.size(); ++i) { Node& n = m_nodes[i]; - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol = btDbvtVolume::FromCR(n.m_x, 0); btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); node->parent = NULL; node->data = &n; @@ -2704,61 +2728,61 @@ btVector3 btSoftBody::evaluateCom() const } bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti) const -{ - btVector3 nrm; - const btCollisionShape* shp = colObjWrap->getCollisionShape(); - // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); - //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform& wtr = colObjWrap->getWorldTransform(); - //todo: check which transform is needed here - - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(x), - shp, - nrm, - margin); - if (dst < 0) - { - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); - return (true); - } - return (false); + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); + //todo: check which transform is needed here + + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); + if (dst < 0) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); + } + return (false); } // bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, - const btVector3& x, - btScalar margin, - btSoftBody::sCti& cti, bool predict) const + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); - // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect - // but resolve contact at x_n - btTransform wtr = (predict) ? - (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) - : colObjWrap->getWorldTransform(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), shp, nrm, margin); + if (!predict) { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = dst; + cti.m_offset = dst; } - if (dst < 0) - return true; + if (dst < 0) + return true; return (false); } @@ -2767,175 +2791,131 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr // point p with respect to triangle (a, b, c) static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary) { - btVector3 v0 = b - a, v1 = c - a, v2 = p - a; - btScalar d00 = v0.dot(v0); - btScalar d01 = v0.dot(v1); - btScalar d11 = v1.dot(v1); - btScalar d20 = v2.dot(v0); - btScalar d21 = v2.dot(v1); - btScalar denom = d00 * d11 - d01 * d01; - bary.setY((d11 * d20 - d01 * d21) / denom); - bary.setZ((d00 * d21 - d01 * d20) / denom); - bary.setX(btScalar(1) - bary.getY() - bary.getZ()); + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = v0.dot(v0); + btScalar d01 = v0.dot(v1); + btScalar d11 = v1.dot(v1); + btScalar d20 = v2.dot(v0); + btScalar d21 = v2.dot(v1); + btScalar denom = d00 * d11 - d01 * d01; + bary.setY((d11 * d20 - d01 * d21) / denom); + bary.setZ((d00 * d21 - d01 * d20) / denom); + bary.setX(btScalar(1) - bary.getY() - bary.getZ()); } // bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, - Face& f, - btVector3& contact_point, - btVector3& bary, - btScalar margin, - btSoftBody::sCti& cti, bool predict) const -{ - btVector3 nrm; - const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); - // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect - // but resolve contact at x_n - btTransform wtr = (predict) ? - (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) - : colObjWrap->getWorldTransform(); - btScalar dst; - -//#define USE_QUADRATURE 1 -//#define CACHE_PREV_COLLISION - - // use the contact position of the previous collision -#ifdef CACHE_PREV_COLLISION - if (f.m_pcontact[3] != 0) - { - for (int i = 0; i < 3; ++i) - bary[i] = f.m_pcontact[i]; - contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - dst = m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(contact_point), - shp, - nrm, - margin); - nrm = wtr.getBasis() * nrm; - cti.m_colObj = colObjWrap->getCollisionObject(); - // use cached contact point - } - else - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_x); - btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); - btVector3 guess(0,0,0); - const btConvexShape* csh = static_cast<const btConvexShape*>(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance - margin; - contact_point = results.witnesses[0]; - getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - nrm = results.normal; - cti.m_colObj = colObjWrap->getCollisionObject(); - for (int i = 0; i < 3; ++i) - f.m_pcontact[i] = bary[i]; - } - return (dst < 0); -#endif + Face& f, + btVector3& contact_point, + btVector3& bary, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + btScalar dst; + btGjkEpaSolver2::sResults results; + +// #define USE_QUADRATURE 1 - // use collision quadrature point + // use collision quadrature point #ifdef USE_QUADRATURE - { - dst = SIMD_INFINITY; - btVector3 local_nrm; - for (int q = 0; q < m_quads.size(); ++q) - { - btVector3 p; - if (predict) - p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); - else - p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); - btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(p), - shp, - local_nrm, - margin); - if (local_dst < dst) - { - if (local_dst < 0 && predict) - return true; - dst = local_dst; - contact_point = p; - bary = m_quads[q]; - nrm = local_nrm; - } - if (!predict) - { - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = dst; - } - } - return (dst < 0); - } + { + dst = SIMD_INFINITY; + btVector3 local_nrm; + for (int q = 0; q < m_quads.size(); ++q) + { + btVector3 p; + if (predict) + p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]); + else + p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]); + btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(p), + shp, + local_nrm, + margin); + if (local_dst < dst) + { + if (local_dst < 0 && predict) + return true; + dst = local_dst; + contact_point = p; + bary = m_quads[q]; + nrm = local_nrm; + } + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = dst; + } + } + return (dst < 0); + } #endif - -// // regular face contact -// { -// btGjkEpaSolver2::sResults results; -// btTransform triangle_transform; -// triangle_transform.setIdentity(); -// triangle_transform.setOrigin(f.m_n[0]->m_x); -// btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); -// btVector3 guess(0,0,0); -// if (predict) -// { -// triangle_transform.setOrigin(f.m_n[0]->m_q); -// triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); -// } -// const btConvexShape* csh = static_cast<const btConvexShape*>(shp); -//// btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); -//// dst = results.distance - margin; -//// contact_point = results.witnesses[0]; -// btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results); -// if (results.status == btGjkEpaSolver2::sResults::Separated) -// return false; -// dst = results.distance - margin; -// contact_point = results.witnesses[1]; -// getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); -// nrm = results.normal; -// for (int i = 0; i < 3; ++i) -// f.m_pcontact[i] = bary[i]; -// } -// -// if (!predict) -// { -// cti.m_colObj = colObjWrap->getCollisionObject(); -// cti.m_normal = nrm; -// cti.m_offset = dst; -// } -// - - // regular face contact - { - btGjkEpaSolver2::sResults results; - btTransform triangle_transform; - triangle_transform.setIdentity(); - triangle_transform.setOrigin(f.m_n[0]->m_q); - btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q); - btVector3 guess(0,0,0); - const btConvexShape* csh = static_cast<const btConvexShape*>(shp); - btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance-csh->getMargin(); - dst -= margin; - if (dst >= 0) - return false; - contact_point = results.witnesses[0]; - getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary); - btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - nrm = results.normal; - cti.m_colObj = colObjWrap->getCollisionObject(); - cti.m_normal = nrm; - cti.m_offset = dst + (curr - contact_point).dot(nrm); - } - return (dst < 0); + + // collision detection using x* + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_q); + btTriangleShape triangle(btVector3(0, 0, 0), f.m_n[1]->m_q - f.m_n[0]->m_q, f.m_n[2]->m_q - f.m_n[0]->m_q); + btVector3 guess(0, 0, 0); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + dst = results.distance - 2.0 * csh->getMargin() - margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable + if (dst >= 0) + return false; + + // Use consistent barycenter to recalculate distance. + if (this->m_cacheBarycenter) + { + if (f.m_pcontact[3] != 0) + { + for (int i = 0; i < 3; ++i) + bary[i] = f.m_pcontact[i]; + contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + const btConvexShape* csh = static_cast<const btConvexShape*>(shp); + btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results); + cti.m_colObj = colObjWrap->getCollisionObject(); + dst = results.distance; + cti.m_normal = results.normal; + cti.m_offset = dst; + + //point-convex CD + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + + dst = results.distance - csh->getMargin() - margin; + return true; + } + } + + // Use triangle-convex CD. + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + + for (int i = 0; i < 3; ++i) + f.m_pcontact[i] = bary[i]; + + dst = results.distance - csh->getMargin() - margin; + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = results.normal; + cti.m_offset = dst; + return true; } -// void btSoftBody::updateNormals() { const btVector3 zv(0, 0, 0); @@ -2979,63 +2959,63 @@ void btSoftBody::updateBounds() m_bounds[1] = btVector3(1000, 1000, 1000); } else {*/ -// if (m_ndbvt.m_root) -// { -// const btVector3& mins = m_ndbvt.m_root->volume.Mins(); -// const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); -// const btScalar csm = getCollisionShape()->getMargin(); -// const btVector3 mrg = btVector3(csm, -// csm, -// csm) * -// 1; // ??? to investigate... -// m_bounds[0] = mins - mrg; -// m_bounds[1] = maxs + mrg; -// if (0 != getBroadphaseHandle()) -// { -// m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), -// m_bounds[0], -// m_bounds[1], -// m_worldInfo->m_dispatcher); -// } -// } -// else -// { -// m_bounds[0] = -// m_bounds[1] = btVector3(0, 0, 0); -// } - if (m_nodes.size()) - { - btVector3 mins = m_nodes[0].m_x; - btVector3 maxs = m_nodes[0].m_x; - for (int i = 1; i < m_nodes.size(); ++i) - { - for (int d = 0; d < 3; ++d) - { - if (m_nodes[i].m_x[d] > maxs[d]) - maxs[d] = m_nodes[i].m_x[d]; - if (m_nodes[i].m_x[d] < mins[d]) - mins[d] = m_nodes[i].m_x[d]; - } - } - const btScalar csm = getCollisionShape()->getMargin(); - const btVector3 mrg = btVector3(csm, - csm, - csm); - m_bounds[0] = mins - mrg; - m_bounds[1] = maxs + mrg; - if (0 != getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); - } - } - else - { - m_bounds[0] = - m_bounds[1] = btVector3(0, 0, 0); - } + // if (m_ndbvt.m_root) + // { + // const btVector3& mins = m_ndbvt.m_root->volume.Mins(); + // const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); + // const btScalar csm = getCollisionShape()->getMargin(); + // const btVector3 mrg = btVector3(csm, + // csm, + // csm) * + // 1; // ??? to investigate... + // m_bounds[0] = mins - mrg; + // m_bounds[1] = maxs + mrg; + // if (0 != getBroadphaseHandle()) + // { + // m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + // m_bounds[0], + // m_bounds[1], + // m_worldInfo->m_dispatcher); + // } + // } + // else + // { + // m_bounds[0] = + // m_bounds[1] = btVector3(0, 0, 0); + // } + if (m_nodes.size()) + { + btVector3 mins = m_nodes[0].m_x; + btVector3 maxs = m_nodes[0].m_x; + for (int i = 1; i < m_nodes.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + if (m_nodes[i].m_x[d] > maxs[d]) + maxs[d] = m_nodes[i].m_x[d]; + if (m_nodes[i].m_x[d] < mins[d]) + mins[d] = m_nodes[i].m_x[d]; + } + } + const btScalar csm = getCollisionShape()->getMargin(); + const btVector3 mrg = btVector3(csm, + csm, + csm); + m_bounds[0] = mins - mrg; + m_bounds[1] = maxs + mrg; + if (0 != getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } + } + else + { + m_bounds[0] = + m_bounds[1] = btVector3(0, 0, 0); + } } // @@ -3454,60 +3434,120 @@ void btSoftBody::dampClusters() void btSoftBody::setSpringStiffness(btScalar k) { - for (int i = 0; i < m_links.size(); ++i) - { - m_links[i].Feature::m_material->m_kLST = k; - } - m_repulsionStiffness = k; + for (int i = 0; i < m_links.size(); ++i) + { + m_links[i].Feature::m_material->m_kLST = k; + } + m_repulsionStiffness = k; +} + +void btSoftBody::setGravityFactor(btScalar gravFactor) +{ + m_gravityFactor = gravFactor; +} + +void btSoftBody::setCacheBarycenter(bool cacheBarycenter) +{ + m_cacheBarycenter = cacheBarycenter; } void btSoftBody::initializeDmInverse() { - btScalar unit_simplex_measure = 1./6.; - - for (int i = 0; i < m_tetras.size(); ++i) - { - Tetra &t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; - btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; - btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; - btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - t.m_element_measure = Dm.determinant() * unit_simplex_measure; - t.m_Dm_inverse = Dm.inverse(); - } + btScalar unit_simplex_measure = 1. / 6.; + + for (int i = 0; i < m_tetras.size(); ++i) + { + Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; + btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; + btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; + btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_element_measure = Dm.determinant() * unit_simplex_measure; + t.m_Dm_inverse = Dm.inverse(); + + // calculate the first three columns of P^{-1} + btVector3 a = t.m_n[0]->m_x; + btVector3 b = t.m_n[1]->m_x; + btVector3 c = t.m_n[2]->m_x; + btVector3 d = t.m_n[3]->m_x; + + btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]); + + btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2]; + btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2]; + btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1]; + btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2]; + btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2]; + btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1]; + btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2]; + btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2]; + btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1]; + btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2]; + btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2]; + btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1]; + + btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det); + btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det); + btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det); + + t.m_P_inv[0] = p1; + t.m_P_inv[1] = p2; + t.m_P_inv[2] = p3; + } +} + +static btScalar Dot4(const btVector4& a, const btVector4& b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; } void btSoftBody::updateDeformation() { - for (int i = 0; i < m_tetras.size(); ++i) - { - btSoftBody::Tetra& t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; - btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; - btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; - btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - t.m_F = Ds * t.m_Dm_inverse; - - btSoftBody::TetraScratch& s = m_tetraScratches[i]; - s.m_F = t.m_F; - s.m_J = t.m_F.determinant(); - btMatrix3x3 C = t.m_F.transpose()*t.m_F; - s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); - s.m_cofF = t.m_F.adjoint().transpose(); - } + btQuaternion q; + for (int i = 0; i < m_tetras.size(); ++i) + { + btSoftBody::Tetra& t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; + btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; + btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_F = Ds * t.m_Dm_inverse; + + btSoftBody::TetraScratch& s = m_tetraScratches[i]; + s.m_F = t.m_F; + s.m_J = t.m_F.determinant(); + btMatrix3x3 C = t.m_F.transpose() * t.m_F; + s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); + s.m_cofF = t.m_F.adjoint().transpose(); + + btVector3 a = t.m_n[0]->m_q; + btVector3 b = t.m_n[1]->m_q; + btVector3 c = t.m_n[2]->m_q; + btVector3 d = t.m_n[3]->m_q; + btVector4 q1(a[0], b[0], c[0], d[0]); + btVector4 q2(a[1], b[1], c[1], d[1]); + btVector4 q3(a[2], b[2], c[2], d[2]); + btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]), + Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]), + Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2])); + q.setRotation(btVector3(0, 0, 1), 0); + B.extractRotation(q, 0.01); // precision of the rotation is not very important for visual correctness. + btMatrix3x3 Q(q); + s.m_corotation = Q; + } } void btSoftBody::advanceDeformation() { - updateDeformation(); - for (int i = 0; i < m_tetras.size(); ++i) - { - m_tetraScratchesTn[i] = m_tetraScratches[i]; - } + updateDeformation(); + for (int i = 0; i < m_tetras.size(); ++i) + { + m_tetraScratchesTn[i] = m_tetraScratches[i]; + } } // void btSoftBody::Joint::Prepare(btScalar dt, int) @@ -3750,7 +3790,7 @@ void btSoftBody::applyForces() // void btSoftBody::setMaxStress(btScalar maxStress) { - m_cfg.m_maxStress = maxStress; + m_cfg.m_maxStress = maxStress; } // @@ -3765,7 +3805,7 @@ void btSoftBody::interpolateRenderMesh() const Node* p2 = m_renderNodesParents[i][2]; btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x); btVector3 unit_normal = normal.normalized(); - Node& n = m_renderNodes[i]; + RenderNode& n = m_renderNodes[i]; n.m_x.setZero(); for (int j = 0; j < 3; ++j) { @@ -3778,7 +3818,7 @@ void btSoftBody::interpolateRenderMesh() { for (int i = 0; i < m_renderNodes.size(); ++i) { - Node& n = m_renderNodes[i]; + RenderNode& n = m_renderNodes[i]; n.m_x.setZero(); for (int j = 0; j < 4; ++j) { @@ -3793,13 +3833,13 @@ void btSoftBody::interpolateRenderMesh() void btSoftBody::setCollisionQuadrature(int N) { - for (int i = 0; i <= N; ++i) - { - for (int j = 0; i+j <= N; ++j) - { - m_quads.push_back(btVector3(btScalar(i)/btScalar(N), btScalar(j)/btScalar(N), btScalar(N-i-j)/btScalar(N))); - } - } + for (int i = 0; i <= N; ++i) + { + for (int j = 0; i + j <= N; ++j) + { + m_quads.push_back(btVector3(btScalar(i) / btScalar(N), btScalar(j) / btScalar(N), btScalar(N - i - j) / btScalar(N))); + } + } } // @@ -4006,12 +4046,12 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) void btSoftBody::setSelfCollision(bool useSelfCollision) { - m_useSelfCollision = useSelfCollision; + m_useSelfCollision = useSelfCollision; } bool btSoftBody::useSelfCollision() { - return m_useSelfCollision; + return m_useSelfCollision; } // @@ -4052,23 +4092,23 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap collider.ProcessColObj(this, pcoWrap); } break; - case fCollision::SDF_RD: - { - btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); - if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) - { - const btTransform wtr = pcoWrap->getWorldTransform(); - const btScalar timemargin = 0; - const btScalar basemargin = getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) - volume; - pcoWrap->getCollisionShape()->getAabb(wtr, - mins, - maxs); - volume = btDbvtVolume::FromMM(mins, maxs); - volume.Expand(btVector3(basemargin, basemargin, basemargin)); + case fCollision::SDF_RD: + { + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) + { + const btTransform wtr = pcoWrap->getWorldTransform(); + const btScalar timemargin = 0; + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(wtr, + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); if (m_cfg.collisions & fCollision::SDF_RDN) { btSoftColliders::CollideSDF_RD docollideNode; @@ -4080,26 +4120,26 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); } - if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF))) - { - btSoftColliders::CollideSDF_RDF docollideFace; - docollideFace.psb = this; - docollideFace.m_colObj1Wrap = pcoWrap; - docollideFace.m_rigidBody = prb1; + if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF))) + { + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; docollideFace.dynmargin = basemargin + timemargin; docollideFace.stamargin = basemargin; - m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); - } - } - } - break; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + } + } + } + break; } } // void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { - BT_PROFILE("Deformable Collision"); + BT_PROFILE("Deformable Collision"); const int cf = m_cfg.collisions & psb->m_cfg.collisions; switch (cf & fCollision::SVSmask) { @@ -4137,60 +4177,60 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } } break; - case fCollision::VF_DD: - { - if (!psb->m_softSoftCollision) - return; - if (psb->isActive() || this->isActive()) - { - if (this != psb) - { - btSoftColliders::CollideVF_DD docollide; - /* common */ - docollide.mrg = getCollisionShape()->getMargin() + - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - if (psb->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - docollide.psb[0] = this; - docollide.psb[1] = psb; - docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - - /* psb1 nodes vs psb0 faces */ - if (this->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - docollide.psb[0] = psb; - docollide.psb[1] = this; - docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - } - else - { - if (psb->useSelfCollision()) - { - btSoftColliders::CollideFF_DD docollide; - docollide.mrg = 2*getCollisionShape()->getMargin(); - docollide.psb[0] = this; - docollide.psb[1] = psb; - if (this->m_tetras.size() > 0) - docollide.useFaceNormal = true; - else - docollide.useFaceNormal = false; - /* psb0 faces vs psb0 faces */ - calculateNormalCone(this->m_fdbvnt); - this->m_fdbvt.selfCollideT(m_fdbvnt,docollide); - } - } - } - } - break; + case fCollision::VF_DD: + { + if (!psb->m_softSoftCollision) + return; + if (psb->isActive() || this->isActive()) + { + if (this != psb) + { + btSoftColliders::CollideVF_DD docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + if (psb->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + + /* psb1 nodes vs psb0 faces */ + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = 2 * getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + if (this->m_tetras.size() > 0) + docollide.useFaceNormal = true; + else + docollide.useFaceNormal = false; + /* psb0 faces vs psb0 faces */ + calculateNormalCone(this->m_fdbvnt); + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); + } + } + } + } + break; default: { } @@ -4205,7 +4245,7 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) { btSoftColliders::CollideCCD docollide; /* common */ - docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin + docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin docollide.dt = psb->m_sst.sdt; /* psb0 nodes vs psb1 faces */ if (psb->m_tetras.size() > 0) @@ -4215,8 +4255,8 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.psb[0] = this; docollide.psb[1] = psb; docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + docollide.psb[1]->m_fdbvt.m_root, + docollide); /* psb1 nodes vs psb0 faces */ if (this->m_tetras.size() > 0) docollide.useFaceNormal = true; @@ -4225,8 +4265,8 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.psb[0] = psb; docollide.psb[1] = this; docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + docollide.psb[1]->m_fdbvt.m_root, + docollide); } else { @@ -4236,14 +4276,14 @@ void btSoftBody::geometricCollisionHandler(btSoftBody* psb) docollide.mrg = SAFE_EPSILON; docollide.psb[0] = this; docollide.psb[1] = psb; - docollide.dt = psb->m_sst.sdt; + docollide.dt = psb->m_sst.sdt; if (this->m_tetras.size() > 0) docollide.useFaceNormal = true; else docollide.useFaceNormal = false; /* psb0 faces vs psb0 faces */ calculateNormalCone(this->m_fdbvnt); // should compute this outside of this scope - this->m_fdbvt.selfCollideT(m_fdbvnt,docollide); + this->m_fdbvt.selfCollideT(m_fdbvnt, docollide); } } } @@ -4648,44 +4688,43 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ void btSoftBody::updateDeactivation(btScalar timeStep) { - if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) - return; + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + return; - if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) - { - m_deactivationTime += timeStep; - } - else - { - m_deactivationTime = btScalar(0.); - setActivationState(0); - } + if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) + { + m_deactivationTime += timeStep; + } + else + { + m_deactivationTime = btScalar(0.); + setActivationState(0); + } } - void btSoftBody::setZeroVelocity() { - for (int i = 0; i < m_nodes.size(); ++i) - { - m_nodes[i].m_v.setZero(); - } + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v.setZero(); + } } bool btSoftBody::wantsSleeping() { - if (getActivationState() == DISABLE_DEACTIVATION) - return false; + if (getActivationState() == DISABLE_DEACTIVATION) + return false; - //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) - return false; + //disable deactivation + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) + return false; - if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) - return true; + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + return true; - if (m_deactivationTime > gDeactivationTime) - { - return true; - } - return false; + if (m_deactivationTime > gDeactivationTime) + { + return true; + } + return false; } diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.h b/thirdparty/bullet/BulletSoftBody/btSoftBody.h index 6a55eccbd2..f578487b8c 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBody.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.h @@ -35,7 +35,7 @@ subject to the following restrictions: //#else #define btSoftBodyData btSoftBodyFloatData #define btSoftBodyDataName "btSoftBodyFloatData" -static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; +static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1; static unsigned long seed = 243703; //#endif //BT_USE_DOUBLE_PRECISION @@ -171,10 +171,10 @@ public: CL_SELF = 0x0040, ///Cluster soft body self collision VF_DD = 0x0080, ///Vertex vs face soft vs soft handling - RVDFmask = 0x0f00, /// Rigid versus deformable face mask - SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face - SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face - SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node + RVDFmask = 0x0f00, /// Rigid versus deformable face mask + SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face + SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face + SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node /* presets */ Default = SDF_RS, END @@ -226,7 +226,7 @@ public: const btCollisionObject* m_colObj; /* Rigid body */ btVector3 m_normal; /* Outward normal */ btScalar m_offset; /* Offset from origin */ - btVector3 m_bary; /* Barycentric weights for faces */ + btVector3 m_bary; /* Barycentric weights for faces */ }; /* sMedium */ @@ -258,20 +258,29 @@ public: Material* m_material; // Material }; /* Node */ + struct RenderNode + { + btVector3 m_x; + btVector3 m_uv1; + btVector3 m_normal; + }; struct Node : Feature { btVector3 m_x; // Position btVector3 m_q; // Previous step position/Test position btVector3 m_v; // Velocity - btVector3 m_vn; // Previous step velocity + btVector3 m_vn; // Previous step velocity btVector3 m_f; // Force accumulator btVector3 m_n; // Normal btScalar m_im; // 1/mass btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data - btScalar m_penetration; // depth of penetration + int m_constrained; // depth of penetration int m_battach : 1; // Attached - int index; + int index; + btVector3 m_splitv; // velocity associated with split impulse + btMatrix3x3 m_effectiveMass; // effective mass in contact + btMatrix3x3 m_effectiveMass_inv; // inverse of effective mass }; /* Link */ ATTRIBUTE_ALIGNED16(struct) @@ -287,40 +296,47 @@ public: BT_DECLARE_ALIGNED_ALLOCATOR(); }; + struct RenderFace + { + RenderNode* m_n[3]; // Node pointers + }; + /* Face */ struct Face : Feature { - Node* m_n[3]; // Node pointers - btVector3 m_normal; // Normal - btScalar m_ra; // Rest area - btDbvtNode* m_leaf; // Leaf data - btVector4 m_pcontact; // barycentric weights of the persistent contact - btVector3 m_n0, m_n1, m_vn; - int m_index; + Node* m_n[3]; // Node pointers + btVector3 m_normal; // Normal + btScalar m_ra; // Rest area + btDbvtNode* m_leaf; // Leaf data + btVector4 m_pcontact; // barycentric weights of the persistent contact + btVector3 m_n0, m_n1, m_vn; + int m_index; }; /* Tetra */ struct Tetra : Feature { - Node* m_n[4]; // Node pointers - btScalar m_rv; // Rest volume - btDbvtNode* m_leaf; // Leaf data - btVector3 m_c0[4]; // gradients - btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) - btScalar m_c2; // m_c1/sum(|g0..3|^2) - btMatrix3x3 m_Dm_inverse; // rest Dm^-1 - btMatrix3x3 m_F; - btScalar m_element_measure; + Node* m_n[4]; // Node pointers + btScalar m_rv; // Rest volume + btDbvtNode* m_leaf; // Leaf data + btVector3 m_c0[4]; // gradients + btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) + btScalar m_c2; // m_c1/sum(|g0..3|^2) + btMatrix3x3 m_Dm_inverse; // rest Dm^-1 + btMatrix3x3 m_F; + btScalar m_element_measure; + btVector4 m_P_inv[3]; // first three columns of P_inv matrix + }; + + /* TetraScratch */ + struct TetraScratch + { + btMatrix3x3 m_F; // deformation gradient F + btScalar m_trace; // trace of F^T * F + btScalar m_J; // det(F) + btMatrix3x3 m_cofF; // cofactor of F + btMatrix3x3 m_corotation; // corotatio of the tetra }; - - /* TetraScratch */ - struct TetraScratch - { - btMatrix3x3 m_F; // deformation gradient F - btScalar m_trace; // trace of F^T * F - btScalar m_J; // det(F) - btMatrix3x3 m_cofF; // cofactor of F - }; - + /* RContact */ struct RContact { @@ -331,67 +347,68 @@ public: btScalar m_c2; // ima*dt btScalar m_c3; // Friction btScalar m_c4; // Hardness - - // jacobians and unit impulse responses for multibody - btMultiBodyJacobianData jacobianData_normal; - btMultiBodyJacobianData jacobianData_t1; - btMultiBodyJacobianData jacobianData_t2; - btVector3 t1; - btVector3 t2; + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; }; - - class DeformableRigidContact - { - public: - sCti m_cti; // Contact infos - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // inverse mass of node/face - btScalar m_c3; // Friction - btScalar m_c4; // Hardness - - // jacobians and unit impulse responses for multibody - btMultiBodyJacobianData jacobianData_normal; - btMultiBodyJacobianData jacobianData_t1; - btMultiBodyJacobianData jacobianData_t2; - btVector3 t1; - btVector3 t2; - }; - - class DeformableNodeRigidContact : public DeformableRigidContact - { - public: - Node* m_node; // Owner node - }; - - class DeformableNodeRigidAnchor : public DeformableNodeRigidContact - { - public: - btVector3 m_local; // Anchor position in body space - }; - - class DeformableFaceRigidContact : public DeformableRigidContact - { - public: - Face* m_face; // Owner face - btVector3 m_contactPoint; // Contact point - btVector3 m_bary; // Barycentric weights - btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; - }; - - struct DeformableFaceNodeContact - { - Node* m_node; // Node - Face* m_face; // Face - btVector3 m_bary; // Barycentric weights - btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; - btVector3 m_normal; // Normal - btScalar m_margin; // Margin - btScalar m_friction; // Friction - btScalar m_imf; // inverse mass of the face at contact point - btScalar m_c0; // scale of the impulse matrix; - }; - + + class DeformableRigidContact + { + public: + sCti m_cti; // Contact infos + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // inverse mass of node/face + btScalar m_c3; // Friction + btScalar m_c4; // Hardness + btMatrix3x3 m_c5; // inverse effective mass + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; + }; + + class DeformableNodeRigidContact : public DeformableRigidContact + { + public: + Node* m_node; // Owner node + }; + + class DeformableNodeRigidAnchor : public DeformableNodeRigidContact + { + public: + btVector3 m_local; // Anchor position in body space + }; + + class DeformableFaceRigidContact : public DeformableRigidContact + { + public: + Face* m_face; // Owner face + btVector3 m_contactPoint; // Contact point + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + }; + + struct DeformableFaceNodeContact + { + Node* m_node; // Node + Face* m_face; // Face + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + btVector3 m_normal; // Normal + btScalar m_margin; // Margin + btScalar m_friction; // Friction + btScalar m_imf; // inverse mass of the face at contact point + btScalar m_c0; // scale of the impulse matrix; + }; + /* SContact */ struct SContact { @@ -718,19 +735,19 @@ public: tVSolverArray m_vsequence; // Velocity solvers sequence tPSolverArray m_psequence; // Position solvers sequence tPSolverArray m_dsequence; // Drift solvers sequence - btScalar drag; // deformable air drag - btScalar m_maxStress; // Maximum principle first Piola stress + btScalar drag; // deformable air drag + btScalar m_maxStress; // Maximum principle first Piola stress }; /* SolverState */ struct SolverState { //if you add new variables, always initialize them! SolverState() - :sdt(0), - isdt(0), - velmrg(0), - radmrg(0), - updmrg(0) + : sdt(0), + isdt(0), + velmrg(0), + radmrg(0), + updmrg(0) { } btScalar sdt; // dt*timescale @@ -769,9 +786,11 @@ public: typedef btAlignedObjectArray<Cluster*> tClusterArray; typedef btAlignedObjectArray<Note> tNoteArray; typedef btAlignedObjectArray<Node> tNodeArray; + typedef btAlignedObjectArray< RenderNode> tRenderNodeArray; typedef btAlignedObjectArray<btDbvtNode*> tLeafArray; typedef btAlignedObjectArray<Link> tLinkArray; typedef btAlignedObjectArray<Face> tFaceArray; + typedef btAlignedObjectArray<RenderFace> tRenderFaceArray; typedef btAlignedObjectArray<Tetra> tTetraArray; typedef btAlignedObjectArray<Anchor> tAnchorArray; typedef btAlignedObjectArray<RContact> tRContactArray; @@ -791,40 +810,42 @@ public: btSoftBodyWorldInfo* m_worldInfo; // World info tNoteArray m_notes; // Notes tNodeArray m_nodes; // Nodes - tNodeArray m_renderNodes; // Nodes + tRenderNodeArray m_renderNodes; // Render Nodes tLinkArray m_links; // Links tFaceArray m_faces; // Faces - tFaceArray m_renderFaces; // Faces + tRenderFaceArray m_renderFaces; // Faces tTetraArray m_tetras; // Tetras - btAlignedObjectArray<TetraScratch> m_tetraScratches; - btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; - tAnchorArray m_anchors; // Anchors - btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; - tRContactArray m_rcontacts; // Rigid contacts - btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; - btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; - btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; - tSContactArray m_scontacts; // Soft contacts - tJointArray m_joints; // Joints - tMaterialArray m_materials; // Materials - btScalar m_timeacc; // Time accumulator - btVector3 m_bounds[2]; // Spatial bounds - bool m_bUpdateRtCst; // Update runtime constants - btDbvt m_ndbvt; // Nodes tree - btDbvt m_fdbvt; // Faces tree - btDbvntNode* m_fdbvnt; // Faces tree with normals - btDbvt m_cdbvt; // Clusters tree - tClusterArray m_clusters; // Clusters - btScalar m_dampingCoefficient; // Damping Coefficient + btAlignedObjectArray<TetraScratch> m_tetraScratches; + btAlignedObjectArray<TetraScratch> m_tetraScratchesTn; + tAnchorArray m_anchors; // Anchors + btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors; + tRContactArray m_rcontacts; // Rigid contacts + btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts; + btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts; + btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts; + tSContactArray m_scontacts; // Soft contacts + tJointArray m_joints; // Joints + tMaterialArray m_materials; // Materials + btScalar m_timeacc; // Time accumulator + btVector3 m_bounds[2]; // Spatial bounds + bool m_bUpdateRtCst; // Update runtime constants + btDbvt m_ndbvt; // Nodes tree + btDbvt m_fdbvt; // Faces tree + btDbvntNode* m_fdbvnt; // Faces tree with normals + btDbvt m_cdbvt; // Clusters tree + tClusterArray m_clusters; // Clusters + btScalar m_dampingCoefficient; // Damping Coefficient btScalar m_sleepingThreshold; btScalar m_maxSpeedSquared; - btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection + btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection btScalar m_repulsionStiffness; - btAlignedObjectArray<btVector3> m_X; // initial positions + btScalar m_gravityFactor; + bool m_cacheBarycenter; + btAlignedObjectArray<btVector3> m_X; // initial positions btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights; btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents; - btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation + btAlignedObjectArray<btScalar> m_z; // vertical distance used in extrapolation bool m_useSelfCollision; bool m_softSoftCollision; @@ -856,11 +877,11 @@ public: { return m_worldInfo; } - - void setDampingCoefficient(btScalar damping_coeff) - { - m_dampingCoefficient = damping_coeff; - } + + void setDampingCoefficient(btScalar damping_coeff) + { + m_dampingCoefficient = damping_coeff; + } ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) @@ -921,11 +942,12 @@ public: Material* mat = 0); /* Append anchor */ - void appendDeformableAnchor(int node, btRigidBody* body); - void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); - void appendAnchor(int node, + void appendDeformableAnchor(int node, btRigidBody* body); + void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); + void appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); + void removeAnchor(int node); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1); void appendLinearJoint(const LJoint::Specs& specs, Body body = Body()); @@ -976,10 +998,10 @@ public: void setLinearVelocity(const btVector3& linVel); /* Set the angular velocity of the center of mass */ void setAngularVelocity(const btVector3& angVel); - /* Get best fit rigid transform */ - btTransform getRigidTransform(); - /* Transform to given pose */ - void transformTo(const btTransform& trs); + /* Get best fit rigid transform */ + btTransform getRigidTransform(); + /* Transform to given pose */ + void transformTo(const btTransform& trs); /* Transform */ void transform(const btTransform& trs); /* Translate */ @@ -1068,11 +1090,11 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); void defaultCollisionHandler(btSoftBody* psb); - void setSelfCollision(bool useSelfCollision); - bool useSelfCollision(); - void updateDeactivation(btScalar timeStep); - void setZeroVelocity(); - bool wantsSleeping(); + void setSelfCollision(bool useSelfCollision); + bool useSelfCollision(); + void updateDeactivation(btScalar timeStep); + void setZeroVelocity(); + bool wantsSleeping(); // // Functionality to deal with new accelerated solvers. @@ -1151,8 +1173,8 @@ public: void rebuildNodeTree(); btVector3 evaluateCom() const; bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; - bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; + bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& f, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); void updatePose(); @@ -1166,14 +1188,16 @@ public: void solveClusters(btScalar sor); void applyClusters(bool drift); void dampClusters(); - void setSpringStiffness(btScalar k); - void initializeDmInverse(); - void updateDeformation(); - void advanceDeformation(); + void setSpringStiffness(btScalar k); + void setGravityFactor(btScalar gravFactor); + void setCacheBarycenter(bool cacheBarycenter); + void initializeDmInverse(); + void updateDeformation(); + void advanceDeformation(); void applyForces(); - void setMaxStress(btScalar maxStress); - void interpolateRenderMesh(); - void setCollisionQuadrature(int N); + void setMaxStress(btScalar maxStress); + void interpolateRenderMesh(); + void setCollisionQuadrature(int N); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti); @@ -1182,14 +1206,15 @@ public: static psolver_t getSolver(ePSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver); void geometricCollisionHandler(btSoftBody* psb); -#define SAFE_EPSILON SIMD_EPSILON*100.0 +#define SAFE_EPSILON SIMD_EPSILON * 100.0 void updateNode(btDbvtNode* node, bool use_velocity, bool margin) { if (node->isleaf()) { btSoftBody::Node* n = (btSoftBody::Node*)(node->data); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision if (use_velocity) { btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v}; @@ -1207,38 +1232,40 @@ public: { updateNode(node->childs[0], use_velocity, margin); updateNode(node->childs[1], use_velocity, margin); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; Merge(node->childs[0]->volume, node->childs[1]->volume, vol); node->volume = vol; } } - - void updateNodeTree(bool use_velocity, bool margin) + + void updateNodeTree(bool use_velocity, bool margin) { if (m_ndbvt.m_root) updateNode(m_ndbvt.m_root, use_velocity, margin); } - template <class DBVTNODE> // btDbvtNode or btDbvntNode + template <class DBVTNODE> // btDbvtNode or btDbvntNode void updateFace(DBVTNODE* node, bool use_velocity, bool margin) { if (node->isleaf()) { btSoftBody::Face* f = (btSoftBody::Face*)(node->data); - btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + btScalar pad = margin ? m_sst.radmrg : SAFE_EPSILON; // use user defined margin or margin for floating point precision + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; if (use_velocity) { btVector3 points[6] = {f->m_n[0]->m_x, f->m_n[0]->m_x + m_sst.sdt * f->m_n[0]->m_v, - f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, - f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; + f->m_n[1]->m_x, f->m_n[1]->m_x + m_sst.sdt * f->m_n[1]->m_v, + f->m_n[2]->m_x, f->m_n[2]->m_x + m_sst.sdt * f->m_n[2]->m_v}; vol = btDbvtVolume::FromPoints(points, 6); } else { btVector3 points[3] = {f->m_n[0]->m_x, - f->m_n[1]->m_x, - f->m_n[2]->m_x}; + f->m_n[1]->m_x, + f->m_n[2]->m_x}; vol = btDbvtVolume::FromPoints(points, 3); } vol.Expand(btVector3(pad, pad, pad)); @@ -1249,7 +1276,8 @@ public: { updateFace(node->childs[0], use_velocity, margin); updateFace(node->childs[1], use_velocity, margin); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; Merge(node->childs[0]->volume, node->childs[1]->volume, vol); node->volume = vol; } @@ -1271,7 +1299,7 @@ public: return (a * coord.x() + b * coord.y() + c * coord.z()); } - void applyRepulsionForce(btScalar timeStep, bool applySpringForce) + void applyRepulsionForce(btScalar timeStep, bool applySpringForce) { btAlignedObjectArray<int> indices; { @@ -1297,58 +1325,60 @@ public: const btVector3& n = c.m_normal; btVector3 l = node->m_x - BaryEval(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, w); btScalar d = c.m_margin - n.dot(l); - d = btMax(btScalar(0),d); - + d = btMax(btScalar(0), d); + const btVector3& va = node->m_v; btVector3 vb = BaryEval(face->m_n[0]->m_v, face->m_n[1]->m_v, face->m_n[2]->m_v, w); btVector3 vr = va - vb; - const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing + const btScalar vn = btDot(vr, n); // dn < 0 <==> opposing if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep) continue; - btVector3 vt = vr - vn*n; + btVector3 vt = vr - vn * n; btScalar I = 0; - btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im; + btScalar mass = node->m_im == 0 ? 0 : btScalar(1) / node->m_im; if (applySpringForce) I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); if (vn < 0) I += 0.5 * mass * vn; - btScalar face_penetration = 0, node_penetration = node->m_penetration; + int face_penetration = 0, node_penetration = node->m_constrained; for (int i = 0; i < 3; ++i) - face_penetration = btMax(face_penetration, face->m_n[i]->m_penetration); - btScalar I_tilde = .5 *I /(1.0+w.length2()); - -// double the impulse if node or face is constrained. - if (face_penetration > 0 || node_penetration > 0) - I_tilde *= 2.0; - if (face_penetration <= node_penetration) + face_penetration |= face->m_n[i]->m_constrained; + btScalar I_tilde = 2.0 * I / (1.0 + w.length2()); + + // double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) + { + I_tilde *= 2.0; + } + if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) - face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im; + face->m_n[j]->m_v += w[j] * n * I_tilde * node->m_im; } - if (face_penetration >= node_penetration) + if (node_penetration <= 0) { - node->m_v -= I_tilde*node->m_im*n; + node->m_v -= I_tilde * node->m_im * n; } - + // apply frictional impulse btScalar vt_norm = vt.safeNorm(); if (vt_norm > SIMD_EPSILON) { btScalar delta_vn = -2 * I * node->m_im; btScalar mu = c.m_friction; - btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0))*vt_norm; - I = 0.5 * mass * (vt_norm-vt_new); + btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm; + I = 0.5 * mass * (vt_norm - vt_new); vt.safeNormalize(); - I_tilde = .5 *I /(1.0+w.length2()); -// double the impulse if node or face is constrained. -// if (face_penetration > 0 || node_penetration > 0) -// I_tilde *= 2.0; - if (face_penetration <= node_penetration) + I_tilde = 2.0 * I / (1.0 + w.length2()); + // double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) + I_tilde *= 2.0; + if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im; } - if (face_penetration >= node_penetration) + if (node_penetration <= 0) { node->m_v -= I_tilde * node->m_im * vt; } @@ -1356,7 +1386,7 @@ public: } } virtual int calculateSerializeBufferSize() const; - + ///fills the dataBuffer and returns the struct name (and 0 on failure) virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; }; diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp index c1a87c7d57..f63e48f9a5 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp @@ -727,7 +727,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const int resy, int fixeds, bool gendiags, - btScalar perturbation) + btScalar perturbation) { #define IDX(_x_, _y_) ((_y_)*rx + (_x_)) /* Create nodes */ @@ -747,12 +747,12 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const for (int ix = 0; ix < rx; ++ix) { const btScalar tx = ix / (btScalar)(rx - 1); - btScalar pert = perturbation * btScalar(rand())/RAND_MAX; - btVector3 temp1 = py1; - temp1.setY(py1.getY() + pert); - btVector3 temp = py0; - pert = perturbation * btScalar(rand())/RAND_MAX; - temp.setY(py0.getY() + pert); + btScalar pert = perturbation * btScalar(rand()) / RAND_MAX; + btVector3 temp1 = py1; + temp1.setY(py1.getY() + pert); + btVector3 temp = py0; + pert = perturbation * btScalar(rand()) / RAND_MAX; + temp.setY(py0.getY() + pert); x[IDX(ix, iy)] = lerp(temp, temp1, tx); m[IDX(ix, iy)] = 1; } @@ -1233,9 +1233,9 @@ if(face&&face[0]) } } } - psb->initializeDmInverse(); - psb->m_tetraScratches.resize(psb->m_tetras.size()); - psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Links: %u\r\n", psb->m_links.size()); printf("Faces: %u\r\n", psb->m_faces.size()); @@ -1245,61 +1245,62 @@ if(face&&face[0]) btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) { - std::ifstream fs; - fs.open(vtk_file); - btAssert(fs); - - typedef btAlignedObjectArray<int> Index; - std::string line; - btAlignedObjectArray<btVector3> X; - btVector3 position; - btAlignedObjectArray<Index> indices; - bool reading_points = false; - bool reading_tets = false; - size_t n_points = 0; - size_t n_tets = 0; - size_t x_count = 0; - size_t indices_count = 0; - while (std::getline(fs, line)) - { - std::stringstream ss(line); - if (line.size() == (size_t)(0)) - { - } - else if (line.substr(0, 6) == "POINTS") - { - reading_points = true; - reading_tets = false; - ss.ignore(128, ' '); // ignore "POINTS" - ss >> n_points; - X.resize(n_points); - } - else if (line.substr(0, 5) == "CELLS") - { - reading_points = false; - reading_tets = true; - ss.ignore(128, ' '); // ignore "CELLS" - ss >> n_tets; - indices.resize(n_tets); - } - else if (line.substr(0, 10) == "CELL_TYPES") - { - reading_points = false; - reading_tets = false; - } - else if (reading_points) - { - btScalar p; - ss >> p; - position.setX(p); - ss >> p; - position.setY(p); - ss >> p; - position.setZ(p); - X[x_count++] = position; - } - else if (reading_tets) - { + std::ifstream fs; + fs.open(vtk_file); + btAssert(fs); + + typedef btAlignedObjectArray<int> Index; + std::string line; + btAlignedObjectArray<btVector3> X; + btVector3 position; + btAlignedObjectArray<Index> indices; + bool reading_points = false; + bool reading_tets = false; + size_t n_points = 0; + size_t n_tets = 0; + size_t x_count = 0; + size_t indices_count = 0; + while (std::getline(fs, line)) + { + std::stringstream ss(line); + if (line.size() == (size_t)(0)) + { + } + else if (line.substr(0, 6) == "POINTS") + { + reading_points = true; + reading_tets = false; + ss.ignore(128, ' '); // ignore "POINTS" + ss >> n_points; + X.resize(n_points); + } + else if (line.substr(0, 5) == "CELLS") + { + reading_points = false; + reading_tets = true; + ss.ignore(128, ' '); // ignore "CELLS" + ss >> n_tets; + indices.resize(n_tets); + } + else if (line.substr(0, 10) == "CELL_TYPES") + { + reading_points = false; + reading_tets = false; + } + else if (reading_points) + { + btScalar p; + ss >> p; + position.setX(p); + ss >> p; + position.setY(p); + ss >> p; + position.setZ(p); + //printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ()); + X[x_count++] = position; + } + else if (reading_tets) + { int d; ss >> d; if (d != 4) @@ -1308,317 +1309,355 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, fs.close(); return 0; } - ss.ignore(128, ' '); // ignore "4" - Index tet; - tet.resize(4); - for (size_t i = 0; i < 4; i++) - { - ss >> tet[i]; - printf("%d ", tet[i]); - } - printf("\n"); - indices[indices_count++] = tet; - } - } - btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); - - for (int i = 0; i < n_tets; ++i) - { - const Index& ni = indices[i]; - psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); - { - psb->appendLink(ni[0], ni[1], 0, true); - psb->appendLink(ni[1], ni[2], 0, true); - psb->appendLink(ni[2], ni[0], 0, true); - psb->appendLink(ni[0], ni[3], 0, true); - psb->appendLink(ni[1], ni[3], 0, true); - psb->appendLink(ni[2], ni[3], 0, true); - } - } - - - generateBoundaryFaces(psb); - psb->initializeDmInverse(); - psb->m_tetraScratches.resize(psb->m_tetras.size()); - psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); - printf("Nodes: %u\r\n", psb->m_nodes.size()); - printf("Links: %u\r\n", psb->m_links.size()); - printf("Faces: %u\r\n", psb->m_faces.size()); - printf("Tetras: %u\r\n", psb->m_tetras.size()); - - fs.close(); - return psb; + ss.ignore(128, ' '); // ignore "4" + Index tet; + tet.resize(4); + for (size_t i = 0; i < 4; i++) + { + ss >> tet[i]; + //printf("%d ", tet[i]); + } + //printf("\n"); + indices[indices_count++] = tet; + } + } + btSoftBody* psb = new btSoftBody(&worldInfo, n_points, &X[0], 0); + + for (int i = 0; i < n_tets; ++i) + { + const Index& ni = indices[i]; + psb->appendTetra(ni[0], ni[1], ni[2], ni[3]); + { + psb->appendLink(ni[0], ni[1], 0, true); + psb->appendLink(ni[1], ni[2], 0, true); + psb->appendLink(ni[2], ni[0], 0, true); + psb->appendLink(ni[0], ni[3], 0, true); + psb->appendLink(ni[1], ni[3], 0, true); + psb->appendLink(ni[2], ni[3], 0, true); + } + } + + generateBoundaryFaces(psb); + psb->initializeDmInverse(); + psb->m_tetraScratches.resize(psb->m_tetras.size()); + psb->m_tetraScratchesTn.resize(psb->m_tetras.size()); + printf("Nodes: %u\r\n", psb->m_nodes.size()); + printf("Links: %u\r\n", psb->m_links.size()); + printf("Faces: %u\r\n", psb->m_faces.size()); + printf("Tetras: %u\r\n", psb->m_tetras.size()); + + fs.close(); + return psb; } void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb) { - int counter = 0; - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - psb->m_nodes[i].index = counter++; - } - typedef btAlignedObjectArray<int> Index; - btAlignedObjectArray<Index> indices; - indices.resize(psb->m_tetras.size()); - for (int i = 0; i < indices.size(); ++i) - { - Index index; - index.push_back(psb->m_tetras[i].m_n[0]->index); - index.push_back(psb->m_tetras[i].m_n[1]->index); - index.push_back(psb->m_tetras[i].m_n[2]->index); - index.push_back(psb->m_tetras[i].m_n[3]->index); - indices[i] = index; - } - - std::map<std::vector<int>, std::vector<int> > dict; - for (int i = 0; i < indices.size(); ++i) - { - for (int j = 0; j < 4; ++j) - { - std::vector<int> f; - if (j == 0) - { - f.push_back(indices[i][1]); - f.push_back(indices[i][0]); - f.push_back(indices[i][2]); - } - if (j == 1) - { - f.push_back(indices[i][3]); - f.push_back(indices[i][0]); - f.push_back(indices[i][1]); - } - if (j == 2) - { - f.push_back(indices[i][3]); - f.push_back(indices[i][1]); - f.push_back(indices[i][2]); - } - if (j == 3) - { - f.push_back(indices[i][2]); - f.push_back(indices[i][0]); - f.push_back(indices[i][3]); - } - std::vector<int> f_sorted = f; - std::sort(f_sorted.begin(), f_sorted.end()); - if (dict.find(f_sorted) != dict.end()) - { - dict.erase(f_sorted); - } - else - { - dict.insert(std::make_pair(f_sorted, f)); - } - } - } - - for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) - { - std::vector<int> f = it->second; - psb->appendFace(f[0], f[1], f[2]); - } + int counter = 0; + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + psb->m_nodes[i].index = counter++; + } + typedef btAlignedObjectArray<int> Index; + btAlignedObjectArray<Index> indices; + indices.resize(psb->m_tetras.size()); + for (int i = 0; i < indices.size(); ++i) + { + Index index; + index.push_back(psb->m_tetras[i].m_n[0]->index); + index.push_back(psb->m_tetras[i].m_n[1]->index); + index.push_back(psb->m_tetras[i].m_n[2]->index); + index.push_back(psb->m_tetras[i].m_n[3]->index); + indices[i] = index; + } + + std::map<std::vector<int>, std::vector<int> > dict; + for (int i = 0; i < indices.size(); ++i) + { + for (int j = 0; j < 4; ++j) + { + std::vector<int> f; + if (j == 0) + { + f.push_back(indices[i][1]); + f.push_back(indices[i][0]); + f.push_back(indices[i][2]); + } + if (j == 1) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][0]); + f.push_back(indices[i][1]); + } + if (j == 2) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][1]); + f.push_back(indices[i][2]); + } + if (j == 3) + { + f.push_back(indices[i][2]); + f.push_back(indices[i][0]); + f.push_back(indices[i][3]); + } + std::vector<int> f_sorted = f; + std::sort(f_sorted.begin(), f_sorted.end()); + if (dict.find(f_sorted) != dict.end()) + { + dict.erase(f_sorted); + } + else + { + dict.insert(std::make_pair(f_sorted, f)); + } + } + } + + for (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it) + { + std::vector<int> f = it->second; + psb->appendFace(f[0], f[1], f[2]); + //printf("f %d %d %d\n", f[0] + 1, f[1] + 1, f[2] + 1); + } } +//Write the surface mesh to an obj file. void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) { - std::ofstream fs; - fs.open(filename); - btAssert(fs); - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - fs << "v"; - for (int d = 0; d < 3; d++) - { - fs << " " << psb->m_nodes[i].m_x[d]; - } - fs << "\n"; - } - - for (int i = 0; i < psb->m_faces.size(); ++i) - { - fs << "f"; - for (int n = 0; n < 3; n++) - { - fs << " " << psb->m_faces[i].m_n[n]->index + 1; - } - fs << "\n"; - } - fs.close(); + std::ofstream fs; + fs.open(filename); + btAssert(fs); + + if (psb->m_tetras.size() > 0) + { + // For tetrahedron mesh, we need to re-index the surface mesh for it to be in obj file/ + std::map<int, int> dict; + for (int i = 0; i < psb->m_faces.size(); i++) + { + for (int d = 0; d < 3; d++) + { + int index = psb->m_faces[i].m_n[d]->index; + if (dict.find(index) == dict.end()) + { + int dict_size = dict.size(); + dict[index] = dict_size; + fs << "v"; + for (int k = 0; k < 3; k++) + { + fs << " " << psb->m_nodes[index].m_x[k]; + } + fs << "\n"; + } + } + } + // Write surface mesh. + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << dict[psb->m_faces[i].m_n[n]->index] + 1; + } + fs << "\n"; + } + } + else + { + // For trimesh, directly write out all the nodes and faces.xs + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_x[d]; + } + fs << "\n"; + } + + for (int i = 0; i < psb->m_faces.size(); ++i) + { + fs << "f"; + for (int n = 0; n < 3; n++) + { + fs << " " << psb->m_faces[i].m_n[n]->index + 1; + } + fs << "\n"; + } + } + fs.close(); } void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) { - std::ifstream fs_read; - fs_read.open(filename); - std::string line; - btVector3 pos; - btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; - while (std::getline(fs_read, line)) - { - std::stringstream ss(line); - if (line[0] == 'v') - { - } - else if (line[0] == 'f') - { - ss.ignore(); - int id0, id1, id2; - ss >> id0; - ss >> id1; - ss >> id2; - btAlignedObjectArray<int> new_face; - new_face.push_back(id1); - new_face.push_back(id0); - new_face.push_back(id2); - additional_faces.push_back(new_face); - } - } - fs_read.close(); - - std::ofstream fs_write; - fs_write.open(filename, std::ios_base::app); - for (int i = 0; i < additional_faces.size(); ++i) - { - fs_write << "f"; - for (int n = 0; n < 3; n++) - { - fs_write << " " << additional_faces[i][n]; - } - fs_write << "\n"; - } - fs_write.close(); + std::ifstream fs_read; + fs_read.open(filename); + std::string line; + btVector3 pos; + btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces; + while (std::getline(fs_read, line)) + { + std::stringstream ss(line); + if (line[0] == 'v') + { + } + else if (line[0] == 'f') + { + ss.ignore(); + int id0, id1, id2; + ss >> id0; + ss >> id1; + ss >> id2; + btAlignedObjectArray<int> new_face; + new_face.push_back(id1); + new_face.push_back(id0); + new_face.push_back(id2); + additional_faces.push_back(new_face); + } + } + fs_read.close(); + + std::ofstream fs_write; + fs_write.open(filename, std::ios_base::app); + for (int i = 0; i < additional_faces.size(); ++i) + { + fs_write << "f"; + for (int n = 0; n < 3; n++) + { + fs_write << " " << additional_faces[i][n]; + } + fs_write << "\n"; + } + fs_write.close(); } // Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary) { - btVector3 vap = p - a; - btVector3 vbp = p - b; - - btVector3 vab = b - a; - btVector3 vac = c - a; - btVector3 vad = d - a; - - btVector3 vbc = c - b; - btVector3 vbd = d - b; - btScalar va6 = (vbp.cross(vbd)).dot(vbc); - btScalar vb6 = (vap.cross(vac)).dot(vad); - btScalar vc6 = (vap.cross(vad)).dot(vab); - btScalar vd6 = (vap.cross(vab)).dot(vac); - btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); - bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6); + btVector3 vap = p - a; + btVector3 vbp = p - b; + + btVector3 vab = b - a; + btVector3 vac = c - a; + btVector3 vad = d - a; + + btVector3 vbc = c - b; + btVector3 vbd = d - b; + btScalar va6 = (vbp.cross(vbd)).dot(vbc); + btScalar vb6 = (vap.cross(vac)).dot(vad); + btScalar vc6 = (vap.cross(vad)).dot(vab); + btScalar vd6 = (vap.cross(vab)).dot(vac); + btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad)); + bary = btVector4(va6 * v6, vb6 * v6, vc6 * v6, vd6 * v6); } // Given a simplex with vertices a,b,c, find the barycentric weights of p in this simplex. bary[3] = 0. void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary) { - btVector3 v0 = b - a, v1 = c - a, v2 = p - a; - btScalar d00 = btDot(v0, v0); - btScalar d01 = btDot(v0, v1); - btScalar d11 = btDot(v1, v1); - btScalar d20 = btDot(v2, v0); - btScalar d21 = btDot(v2, v1); - btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); - bary[1] = (d11 * d20 - d01 * d21) * invDenom; - bary[2] = (d00 * d21 - d01 * d20) * invDenom; - bary[0] = 1.0 - bary[1] - bary[2]; - bary[3] = 0; + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = btDot(v0, v0); + btScalar d01 = btDot(v0, v1); + btScalar d11 = btDot(v1, v1); + btScalar d20 = btDot(v2, v0); + btScalar d21 = btDot(v2, v1); + btScalar invDenom = 1.0 / (d00 * d11 - d01 * d01); + bary[1] = (d11 * d20 - d01 * d21) * invDenom; + bary[2] = (d00 * d21 - d01 * d20) * invDenom; + bary[0] = 1.0 - bary[1] - bary[2]; + bary[3] = 0; } // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) { - psb->m_z.resize(0); - psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); - psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); - for (int i = 0; i < psb->m_renderNodes.size(); ++i) - { - const btVector3& p = psb->m_renderNodes[i].m_x; - btVector4 bary; - btVector4 optimal_bary; - btScalar min_bary_weight = -1e3; - btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; - for (int j = 0; j < psb->m_tetras.size(); ++j) - { - const btSoftBody::Tetra& t = psb->m_tetras[j]; - getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); - btScalar new_min_bary_weight = bary[0]; - for (int k = 1; k < 4; ++k) - { - new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); - } - if (new_min_bary_weight > min_bary_weight) - { - btAlignedObjectArray<const btSoftBody::Node*> parents; - parents.push_back(t.m_n[0]); - parents.push_back(t.m_n[1]); - parents.push_back(t.m_n[2]); - parents.push_back(t.m_n[3]); - optimal_parents = parents; - optimal_bary = bary; - min_bary_weight = new_min_bary_weight; - // stop searching if p is inside the tetrahedron at hand - if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.) - { - break; - } - } - } - psb->m_renderNodesInterpolationWeights[i] = optimal_bary; - psb->m_renderNodesParents[i] = optimal_parents; - } + psb->m_z.resize(0); + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -1e3; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + const btSoftBody::Tetra& t = psb->m_tetras[j]; + getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 4; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + if (new_min_bary_weight > min_bary_weight) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(t.m_n[0]); + parents.push_back(t.m_n[1]); + parents.push_back(t.m_n[2]); + parents.push_back(t.m_n[3]); + optimal_parents = parents; + optimal_bary = bary; + min_bary_weight = new_min_bary_weight; + // stop searching if p is inside the tetrahedron at hand + if (bary[0] >= 0. && bary[1] >= 0. && bary[2] >= 0. && bary[3] >= 0.) + { + break; + } + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + } } - // Iterate through all render nodes to find the simulation triangle that's closest to the node in the barycentric sense. void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) { - psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); - psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); - psb->m_z.resize(psb->m_renderNodes.size()); - for (int i = 0; i < psb->m_renderNodes.size(); ++i) - { - const btVector3& p = psb->m_renderNodes[i].m_x; - btVector4 bary; - btVector4 optimal_bary; - btScalar min_bary_weight = -SIMD_INFINITY; - btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; - btScalar dist = 0, optimal_dist = 0; - for (int j = 0; j < psb->m_faces.size(); ++j) - { - const btSoftBody::Face& f = psb->m_faces[j]; - btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); - btVector3 unit_n = n.normalized(); - dist = (p-f.m_n[0]->m_x).dot(unit_n); - btVector3 proj_p = p - dist*unit_n; - getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); - btScalar new_min_bary_weight = bary[0]; - for (int k = 1; k < 3; ++k) - { - new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); - } - - // p is out of the current best triangle, we found a traingle that's better - bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.); - // p is inside of the current best triangle, we found a triangle that's better - bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist)); - - if (better_than_closest_outisde || better_than_best_inside) - { - btAlignedObjectArray<const btSoftBody::Node*> parents; - parents.push_back(f.m_n[0]); - parents.push_back(f.m_n[1]); - parents.push_back(f.m_n[2]); - optimal_parents = parents; - optimal_bary = bary; - optimal_dist = dist; - min_bary_weight = new_min_bary_weight; - } - } - psb->m_renderNodesInterpolationWeights[i] = optimal_bary; - psb->m_renderNodesParents[i] = optimal_parents; - psb->m_z[i] = optimal_dist; - } + psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); + psb->m_renderNodesParents.resize(psb->m_renderNodes.size()); + psb->m_z.resize(psb->m_renderNodes.size()); + for (int i = 0; i < psb->m_renderNodes.size(); ++i) + { + const btVector3& p = psb->m_renderNodes[i].m_x; + btVector4 bary; + btVector4 optimal_bary; + btScalar min_bary_weight = -SIMD_INFINITY; + btAlignedObjectArray<const btSoftBody::Node*> optimal_parents; + btScalar dist = 0, optimal_dist = 0; + for (int j = 0; j < psb->m_faces.size(); ++j) + { + const btSoftBody::Face& f = psb->m_faces[j]; + btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + btVector3 unit_n = n.normalized(); + dist = (p - f.m_n[0]->m_x).dot(unit_n); + btVector3 proj_p = p - dist * unit_n; + getBarycentricWeights(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, proj_p, bary); + btScalar new_min_bary_weight = bary[0]; + for (int k = 1; k < 3; ++k) + { + new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); + } + + // p is out of the current best triangle, we found a traingle that's better + bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight < 0.); + // p is inside of the current best triangle, we found a triangle that's better + bool better_than_best_inside = (new_min_bary_weight >= 0 && min_bary_weight >= 0 && btFabs(dist) < btFabs(optimal_dist)); + + if (better_than_closest_outisde || better_than_best_inside) + { + btAlignedObjectArray<const btSoftBody::Node*> parents; + parents.push_back(f.m_n[0]); + parents.push_back(f.m_n[1]); + parents.push_back(f.m_n[2]); + optimal_parents = parents; + optimal_bary = bary; + optimal_dist = dist; + min_bary_weight = new_min_bary_weight; + } + } + psb->m_renderNodesInterpolationWeights[i] = optimal_bary; + psb->m_renderNodesParents[i] = optimal_parents; + psb->m_z[i] = optimal_dist; + } } diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h index abe1870890..237d29761d 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h @@ -93,7 +93,7 @@ struct btSoftBodyHelpers int resy, int fixeds, bool gendiags, - btScalar perturbation = 0.); + btScalar perturbation = 0.); /* Create a patch with UV Texture Coordinates */ static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, const btVector3& corner00, @@ -142,21 +142,21 @@ struct btSoftBodyHelpers bool bfacelinks, bool btetralinks, bool bfacesfromtetras); - static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); + static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); - static void writeObj(const char* file, const btSoftBody* psb); - - static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); - - static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); - - static void interpolateBarycentricWeights(btSoftBody* psb); - - static void extrapolateBarycentricWeights(btSoftBody* psb); - - static void generateBoundaryFaces(btSoftBody* psb); - - static void duplicateFaces(const char* filename, const btSoftBody* psb); + static void writeObj(const char* file, const btSoftBody* psb); + + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); + + static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary); + + static void interpolateBarycentricWeights(btSoftBody* psb); + + static void extrapolateBarycentricWeights(btSoftBody* psb); + + static void generateBoundaryFaces(btSoftBody* psb); + + static void duplicateFaces(const char* filename, const btSoftBody* psb); /// Sort the list of links to move link calculations that are dependent upon earlier /// ones as far as possible away from the calculation of those values /// This tends to make adjacent loop iterations not dependent upon one another, diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h index b9ebc95b6b..c17bbb5cd4 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h @@ -32,86 +32,85 @@ subject to the following restrictions: // Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, - btMultiBodyJacobianData& jacobianData, - const btVector3& contact_point, - const btVector3& dir) -{ - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - jacobianData.m_jacobians.resize(ndof); - jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); - btScalar* jac = &jacobianData.m_jacobians[0]; - - multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); - multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac = &jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); } static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u) { - btScalar ux = u.getX(); - btScalar uy = u.getY(); - btScalar uz = u.getZ(); - btScalar ax = std::abs(ux); - btScalar ay = std::abs(uy); - btScalar az = std::abs(uz); - btVector3 v; - if (ax <= ay && ax <= az) - v = btVector3(0, -uz, uy); - else if (ay <= ax && ay <= az) - v = btVector3(-uz, 0, ux); - else - v = btVector3(-uy, ux, 0); - v.normalize(); - return v; + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; } static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary) { - btVector3 x43 = x4-x3; - if (std::abs(x43.dot(normal)) > mrg) - return false; - btVector3 x13 = x1-x3; - btVector3 x23 = x2-x3; - btScalar a11 = x13.length2(); - btScalar a22 = x23.length2(); - btScalar a12 = x13.dot(x23); - btScalar b1 = x13.dot(x43); - btScalar b2 = x23.dot(x43); - btScalar det = a11*a22 - a12*a12; - if (det < SIMD_EPSILON) - return false; - btScalar w1 = (b1*a22-b2*a12)/det; - btScalar w2 = (b2*a11-b1*a12)/det; - btScalar w3 = 1-w1-w2; - btScalar delta = mrg / std::sqrt(0.5*std::abs(x13.cross(x23).safeNorm())); - bary = btVector3(w1,w2,w3); - for (int i = 0; i < 3; ++i) - { - if (bary[i] < -delta || bary[i] > 1+delta) - return false; - } - return true; + btVector3 x43 = x4 - x3; + if (std::abs(x43.dot(normal)) > mrg) + return false; + btVector3 x13 = x1 - x3; + btVector3 x23 = x2 - x3; + btScalar a11 = x13.length2(); + btScalar a22 = x23.length2(); + btScalar a12 = x13.dot(x23); + btScalar b1 = x13.dot(x43); + btScalar b2 = x23.dot(x43); + btScalar det = a11 * a22 - a12 * a12; + if (det < SIMD_EPSILON) + return false; + btScalar w1 = (b1 * a22 - b2 * a12) / det; + btScalar w2 = (b2 * a11 - b1 * a12) / det; + btScalar w3 = 1 - w1 - w2; + btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm())); + bary = btVector3(w1, w2, w3); + for (int i = 0; i < 3; ++i) + { + if (bary[i] < -delta || bary[i] > 1 + delta) + return false; + } + return true; } static const int KDOP_COUNT = 13; -static btVector3 dop[KDOP_COUNT]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1), - btVector3(1,1,0), - btVector3(1,0,1), - btVector3(0,1,1), - btVector3(1,-1,0), - btVector3(1,0,-1), - btVector3(0,1,-1), - btVector3(1,1,1), - btVector3(1,-1,1), - btVector3(1,1,-1), - btVector3(1,-1,-1) -}; +static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0), + btVector3(0, 1, 0), + btVector3(0, 0, 1), + btVector3(1, 1, 0), + btVector3(1, 0, 1), + btVector3(0, 1, 1), + btVector3(1, -1, 0), + btVector3(1, 0, -1), + btVector3(0, 1, -1), + btVector3(1, 1, 1), + btVector3(1, -1, 1), + btVector3(1, 1, -1), + btVector3(1, -1, -1)}; static inline int getSign(const btVector3& n, const btVector3& x) { btScalar d = n.dot(x); - if (d>SIMD_EPSILON) + if (d > SIMD_EPSILON) return 1; - if (d<-SIMD_EPSILON) + if (d < -SIMD_EPSILON) return -1; return 0; } @@ -119,13 +118,12 @@ static inline int getSign(const btVector3& n, const btVector3& x) static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x, - face->m_n[1]->m_x - node->m_x, - face->m_n[2]->m_x - node->m_x, - face->m_n[0]->m_x + dt*face->m_n[0]->m_v - node->m_x, - face->m_n[1]->m_x + dt*face->m_n[1]->m_v - node->m_x, - face->m_n[2]->m_x + dt*face->m_n[2]->m_v - node->m_x - }; - btVector3 segment = dt*node->m_v; + face->m_n[1]->m_x - node->m_x, + face->m_n[2]->m_x - node->m_x, + face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x, + face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x, + face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x}; + btVector3 segment = dt * node->m_v; for (int i = 0; i < KDOP_COUNT; ++i) { int s = getSign(dop[i], segment); @@ -143,488 +141,494 @@ static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, c static SIMD_FORCE_INLINE bool nearZero(const btScalar& a) { - return (a>-SAFE_EPSILON && a<SAFE_EPSILON); + return (a > -SAFE_EPSILON && a < SAFE_EPSILON); } static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b) { - return (nearZero(a) || nearZero(b) || (a>SAFE_EPSILON && b>SAFE_EPSILON) || (a<-SAFE_EPSILON && b<-SAFE_EPSILON)); + return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON)); } static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b) { - return !sameSign(a, b); -} -inline btScalar evaluateBezier2(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &t, const btScalar &s) -{ - btScalar s2 = s*s; - btScalar t2 = t*t; - - return p0*s2+p1*btScalar(2.0)*s*t+p2*t2; -} -inline btScalar evaluateBezier(const btScalar &p0, const btScalar &p1, const btScalar &p2, const btScalar &p3, const btScalar &t, const btScalar &s) -{ - btScalar s2 = s*s; - btScalar s3 = s2*s; - btScalar t2 = t*t; - btScalar t3 = t2*t; - - return p0*s3+p1*btScalar(3.0)*s2*t+p2*btScalar(3.0)*s*t2+p3*t3; -} -static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar <0, btScalar <1) -{ - if (sameSign(t0, t1)) { - lt0 = t0; - lt1 = t0; - return true; - } - - if (type_c || diffSign(k0, k3)) { - btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); - if (t0<-0) - ft = -ft; - - if (sameSign(ft, k0)) { - lt0 = t1; - lt1 = t1; - } - else { - lt0 = t0; - lt1 = t0; - } - return true; - } - - if (!type_c) { - btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); - if (t0<-0) - ft = -ft; - - if (diffSign(ft, k0)) { - lt0 = t0; - lt1 = t1; - return true; - } - - btScalar fk = evaluateBezier2(k1-k0, k2-k1, k3-k2, t0, -t1); - - if (sameSign(fk, k1-k0)) - lt0 = lt1 = t1; - else - lt0 = lt1 = t0; - - return true; - } - return false; + return !sameSign(a, b); +} +inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s) +{ + btScalar s2 = s * s; + btScalar t2 = t * t; + + return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2; +} +inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s) +{ + btScalar s2 = s * s; + btScalar s3 = s2 * s; + btScalar t2 = t * t; + btScalar t3 = t2 * t; + + return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3; +} +static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1) +{ + if (sameSign(t0, t1)) + { + lt0 = t0; + lt1 = t0; + return true; + } + + if (type_c || diffSign(k0, k3)) + { + btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); + if (t0 < -0) + ft = -ft; + + if (sameSign(ft, k0)) + { + lt0 = t1; + lt1 = t1; + } + else + { + lt0 = t0; + lt1 = t0; + } + return true; + } + + if (!type_c) + { + btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); + if (t0 < -0) + ft = -ft; + + if (diffSign(ft, k0)) + { + lt0 = t0; + lt1 = t1; + return true; + } + + btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1); + + if (sameSign(fk, k1 - k0)) + lt0 = lt1 = t1; + else + lt0 = lt1 = t0; + + return true; + } + return false; } static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3) { - const btVector3& n0 = face->m_n0; - const btVector3& n1 = face->m_n1; - btVector3 n_hat = n0 + n1 - face->m_vn; - btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x; - btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q; - k0 = (p0ma0).dot(n0) * 3.0; - k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0); - k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1); - k3 = (p1ma1).dot(n1) * 3.0; + const btVector3& n0 = face->m_n0; + const btVector3& n1 = face->m_n1; + btVector3 n_hat = n0 + n1 - face->m_vn; + btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x; + btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q; + k0 = (p0ma0).dot(n0) * 3.0; + k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0); + k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1); + k3 = (p1ma1).dot(n1) * 3.0; } static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1) { - btScalar denom = 4.0 * (j1-j2) * (j1-j0) + (j2-j0) * (j2-j0); - u0 = (2.0*(j1-j2)*(3.0*k1-2.0*k0-k3) - (j0-j2)*(3.0*k2-2.0*k3-k0)) / denom; - u1 = (2.0*(j1-j0)*(3.0*k2-2.0*k3-k0) - (j2-j0)*(3.0*k1-2.0*k0-k3)) / denom; - v0 = k0-u0*j0; - v1 = k3-u1*j2; + btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0); + u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom; + u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom; + v0 = k0 - u0 * j0; + v1 = k3 - u1 * j2; } static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3) { - btScalar u0, u1, v0, v1; - btScalar j0 = 3.0*(k1-k0); - btScalar j1 = 3.0*(k2-k1); - btScalar j2 = 3.0*(k3-k2); - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (sameSign(v0, v1)) - { - btScalar Ypa = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*v0*(1.0-v0) + j2*v0*v0; // Y'(v0) - if (sameSign(Ypa, j0)) - { - return (diffSign(k0,v1)); - } - } - return diffSign(k0,v0); -} - -static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2) -{ - const btVector3& a0 = a->m_x; - const btVector3& b0 = b->m_x; - const btVector3& c0 = c->m_x; - const btVector3& va = a->m_v; - const btVector3& vb = b->m_v; - const btVector3& vc = c->m_v; - const btVector3 a1 = a0 + dt*va; - const btVector3 b1 = b0 + dt*vb; - const btVector3 c1 = c0 + dt*vc; - btVector3 n0 = (b0-a0).cross(c0-a0); - btVector3 n1 = (b1-a1).cross(c1-a1); - btVector3 n_hat = n0+n1 - dt*dt*(vb-va).cross(vc-va); - const btVector3& p0 = p->m_x; - const btVector3& vp = p->m_v; - btVector3 p1 = p0 + dt*vp; - btVector3 m0 = (b0-p0).cross(c0-p0); - btVector3 m1 = (b1-p1).cross(c1-p1); - btVector3 m_hat = m0+m1 - dt*dt*(vb-vp).cross(vc-vp); - btScalar l0 = m0.dot(n0); - btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0)); - btScalar l2 = btScalar(1)/btScalar(6)*(m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0)); - btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat)); - btScalar l4 = m1.dot(n1); - - btScalar k1p = 0.25 * k0 + 0.75 * k1; - btScalar k2p = 0.5 * k1 + 0.5 * k2; - btScalar k3p = 0.75 * k2 + 0.25 * k3; - - btScalar s0 = (l1 * k0 - l0 * k1p)*4.0; - btScalar s1 = (l2 * k0 - l0 * k2p)*2.0; - btScalar s2 = (l3 * k0 - l0 * k3p)*btScalar(4)/btScalar(3); - btScalar s3 = l4 * k0 - l0 * k3; - - j0 = (s1*k0 - s0*k1) * 3.0; - j1 = (s2*k0 - s0*k2) * 1.5; - j2 = (s3*k0 - s0*k3); + btScalar u0, u1, v0, v1; + btScalar j0 = 3.0 * (k1 - k0); + btScalar j1 = 3.0 * (k2 - k1); + btScalar j2 = 3.0 * (k3 - k2); + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (sameSign(v0, v1)) + { + btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0; // Y'(v0) + if (sameSign(Ypa, j0)) + { + return (diffSign(k0, v1)); + } + } + return diffSign(k0, v0); +} + +static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2) +{ + const btVector3& a0 = a->m_x; + const btVector3& b0 = b->m_x; + const btVector3& c0 = c->m_x; + const btVector3& va = a->m_v; + const btVector3& vb = b->m_v; + const btVector3& vc = c->m_v; + const btVector3 a1 = a0 + dt * va; + const btVector3 b1 = b0 + dt * vb; + const btVector3 c1 = c0 + dt * vc; + btVector3 n0 = (b0 - a0).cross(c0 - a0); + btVector3 n1 = (b1 - a1).cross(c1 - a1); + btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va); + const btVector3& p0 = p->m_x; + const btVector3& vp = p->m_v; + btVector3 p1 = p0 + dt * vp; + btVector3 m0 = (b0 - p0).cross(c0 - p0); + btVector3 m1 = (b1 - p1).cross(c1 - p1); + btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp); + btScalar l0 = m0.dot(n0); + btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0)); + btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0)); + btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat)); + btScalar l4 = m1.dot(n1); + + btScalar k1p = 0.25 * k0 + 0.75 * k1; + btScalar k2p = 0.5 * k1 + 0.5 * k2; + btScalar k3p = 0.75 * k2 + 0.25 * k3; + + btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0; + btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0; + btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3); + btScalar s3 = l4 * k0 - l0 * k3; + + j0 = (s1 * k0 - s0 * k1) * 3.0; + j1 = (s2 * k0 - s0 * k2) * 1.5; + j2 = (s3 * k0 - s0 * k3); } static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) { - btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0) - btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(v0) + btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) + btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(v0) - btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0; - btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0; - sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; - btScalar K = (sameSign(sign_Ytp,k0)) ? v1 : v0; - return diffSign(L,K); + btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0; + btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0; + sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; + btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0; + return diffSign(L, K); } static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) { - btScalar Yu0 = k0*(1.0-u0)*(1.0-u0)*(1.0-u0) + 3.0*k1*u0*(1.0-u0)*(1.0-u0) + 3.0*k2*u0*u0*(1.0-u0) + k3*u0*u0*u0; // Y(u0) - btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2; - if (diffSign(sign_Ytp,k0)) - { - L1 = u0; - L2 = u1; - } - else - { - btScalar Yp_u0 = j0*(1.0-u0)*(1.0-u0) + 2.0*j1*(1.0-u0)*u0 + j2*u0*u0; - if (sameSign(Yp_u0,j0)) - { - L1 = u1; - L2 = u1; - } - else - { - L1 = u0; - L2 = u0; - } - } - btScalar Yv0 = k0*(1.0-v0)*(1.0-v0)*(1.0-v0) + 3.0*k1*v0*(1.0-v0)*(1.0-v0) + 3.0*k2*v0*v0*(1.0-v0) + k3*v0*v0*v0; // Y(uv0) - sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; - btScalar K1, K2; - if (diffSign(sign_Ytp,k0)) - { - K1 = v0; - K2 = v1; - } - else - { - btScalar Yp_v0 = j0*(1.0-v0)*(1.0-v0) + 2.0*j1*(1.0-v0)*v0 + j2*v0*v0; - if (sameSign(Yp_v0,j0)) - { - K1 = v1; - K2 = v1; - } - else - { - K1 = v0; - K2 = v0; - } - } - return (diffSign(K1, L1) || diffSign(L2, K2)); + btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) + btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2; + if (diffSign(sign_Ytp, k0)) + { + L1 = u0; + L2 = u1; + } + else + { + btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0; + if (sameSign(Yp_u0, j0)) + { + L1 = u1; + L2 = u1; + } + else + { + L1 = u0; + L2 = u0; + } + } + btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(uv0) + sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; + btScalar K1, K2; + if (diffSign(sign_Ytp, k0)) + { + K1 = v0; + K2 = v1; + } + else + { + btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0; + if (sameSign(Yp_v0, j0)) + { + K1 = v1; + K2 = v1; + } + else + { + K1 = v0; + K2 = v0; + } + } + return (diffSign(K1, L1) || diffSign(L2, K2)); } static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - btScalar j0, j1, j2, u0, u1, v0, v1; - // p1 - getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - // p2 - getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - // p3 - getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination1Internal(k0,k1,k2,k3,u0,u1,v0,v1)) - return false; - } - return true; + btScalar j0, j1, j2, u0, u1, v0, v1; + // p1 + getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + // p2 + getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + // p3 + getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) + return false; + } + return true; } static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - btScalar j0, j1, j2, u0, u1, v0, v1; - // p1 - getJs(k0,k1,k2,k3,face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0 = true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - // p2 - getJs(k0,k1,k2,k3,face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0=true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - // p3 - getJs(k0,k1,k2,k3,face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); - if (nearZero(j0+j2-j1*2.0)) - { - btScalar lt0, lt1; - bool bt0=true, bt1=true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1); - if (!signDetermination2Internal(k0,k1,k2,k3,j0,j1,j2,u0,u1,v0,v1)) - return false; - } - return true; + btScalar j0, j1, j2, u0, u1, v0, v1; + // p1 + getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + // p2 + getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + // p3 + getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); + if (nearZero(j0 + j2 - j1 * 2.0)) + { + btScalar lt0, lt1; + bool bt0 = true, bt1 = true; + getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); + if (lt0 < -SAFE_EPSILON) + bt0 = false; + if (lt1 < -SAFE_EPSILON) + bt1 = false; + if (!bt0 && !bt1) + return false; + } + else + { + polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); + if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) + return false; + } + return true; } static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - // Coplanar test - if (diffSign(k1-k0, k3-k2)) - { - // Case b: - if (sameSign(k0, k3) && !rootFindingLemma(k0,k1,k2,k3)) - return false; - // inside test - return signDetermination2(k0, k1, k2, k3, face, node, dt); - } - else - { - // Case c: - if (sameSign(k0, k3)) - return false; - // inside test - return signDetermination1(k0, k1, k2, k3, face, node, dt); - } - return false; + // Coplanar test + if (diffSign(k1 - k0, k3 - k2)) + { + // Case b: + if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3)) + return false; + // inside test + return signDetermination2(k0, k1, k2, k3, face, node, dt); + } + else + { + // Case c: + if (sameSign(k0, k3)) + return false; + // inside test + return signDetermination1(k0, k1, k2, k3, face, node, dt); + } + return false; } static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg) { - if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg) - return true; - if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg) - return true; - return false; + if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg) + return true; + if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg) + return true; + return false; } static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) { - if (conservativeCulling(k0, k1, k2, k3, mrg)) - return false; - return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); + if (conservativeCulling(k0, k1, k2, k3, mrg)) + return false; + return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); } static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12) { - k10 = k0*(1.0-t0) + k1*t0; - btScalar k11 = k1*(1.0-t0) + k2*t0; - k12 = k2*(1.0-t0) + k3*t0; - k20 = k10*(1.0-t0) + k11*t0; - k21 = k11*(1.0-t0) + k12*t0; - k30 = k20*(1.0-t0) + k21*t0; + k10 = k0 * (1.0 - t0) + k1 * t0; + btScalar k11 = k1 * (1.0 - t0) + k2 * t0; + k12 = k2 * (1.0 - t0) + k3 * t0; + k20 = k10 * (1.0 - t0) + k11 * t0; + k21 = k11 * (1.0 - t0) + k12 * t0; + k30 = k20 * (1.0 - t0) + k21 * t0; } static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg) { - btScalar k0, k1, k2, k3; - getBernsteinCoeff(face, node, dt, k0, k1, k2, k3); - if (conservativeCulling(k0, k1, k2, k3, mrg)) - return false; - return true; - if (diffSign(k2-2.0*k1+k0, k3-2.0*k2+k1)) - { - btScalar k10, k20, k30, k21, k12; - btScalar t0 = (k2-2.0*k1+k0)/(k0-3.0*k1+3.0*k2-k3); - deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12); - return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt); - } - return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); + btScalar k0, k1, k2, k3; + getBernsteinCoeff(face, node, dt, k0, k1, k2, k3); + if (conservativeCulling(k0, k1, k2, k3, mrg)) + return false; + return true; + if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1)) + { + btScalar k10, k20, k30, k21, k12; + btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3); + deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12); + return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt); + } + return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); } static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) { - if (hasSeparatingPlane(face, node, dt)) - return false; - btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x; - btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x; - btVector3 x41 = node->m_x - face->m_n[0]->m_x; - btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v; - btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v; - btVector3 v41 = node->m_v - face->m_n[0]->m_v; - btVector3 a = x21.cross(x31); - btVector3 b = x21.cross(v31) + v21.cross(x31); - btVector3 c = v21.cross(v31); - btVector3 d = x41; - btVector3 e = v41; - btScalar a0 = a.dot(d); - btScalar a1 = a.dot(e) + b.dot(d); - btScalar a2 = c.dot(d) + b.dot(e); - btScalar a3 = c.dot(e); - btScalar eps = SAFE_EPSILON; - int num_roots = 0; - btScalar roots[3]; - if (std::abs(a3) < eps) - { - // cubic term is zero - if (std::abs(a2) < eps) - { - if (std::abs(a1) < eps) - { - if (std::abs(a0) < eps) - { - num_roots = 2; - roots[0] = 0; - roots[1] = dt; - } - } - else - { - num_roots = 1; - roots[0] = -a0/a1; - } - } - else - { - num_roots = SolveP2(roots, a1/a2, a0/a2); - } - } - else - { - num_roots = SolveP3(roots, a2/a3, a1/a3, a0/a3); - } -// std::sort(roots, roots+num_roots); - if (num_roots > 1) - { - if (roots[0] > roots[1]) - btSwap(roots[0], roots[1]); - } - if (num_roots > 2) - { - if (roots[0] > roots[2]) - btSwap(roots[0], roots[2]); - if (roots[1] > roots[2]) - btSwap(roots[1], roots[2]); - } - for (int r = 0; r < num_roots; ++r) - { - double root = roots[r]; - if (root <= 0) - continue; - if (root > dt + SIMD_EPSILON) - return false; - btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v; - btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v; - btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v; - btVector3 x4 = node->m_x + root * node->m_v; - btVector3 normal = (x2-x1).cross(x3-x1); - normal.safeNormalize(); - if (proximityTest(x1, x2, x3, x4, normal, mrg, bary)) - return true; - } - return false; + if (hasSeparatingPlane(face, node, dt)) + return false; + btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x; + btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x; + btVector3 x41 = node->m_x - face->m_n[0]->m_x; + btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v; + btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v; + btVector3 v41 = node->m_v - face->m_n[0]->m_v; + btVector3 a = x21.cross(x31); + btVector3 b = x21.cross(v31) + v21.cross(x31); + btVector3 c = v21.cross(v31); + btVector3 d = x41; + btVector3 e = v41; + btScalar a0 = a.dot(d); + btScalar a1 = a.dot(e) + b.dot(d); + btScalar a2 = c.dot(d) + b.dot(e); + btScalar a3 = c.dot(e); + btScalar eps = SAFE_EPSILON; + int num_roots = 0; + btScalar roots[3]; + if (std::abs(a3) < eps) + { + // cubic term is zero + if (std::abs(a2) < eps) + { + if (std::abs(a1) < eps) + { + if (std::abs(a0) < eps) + { + num_roots = 2; + roots[0] = 0; + roots[1] = dt; + } + } + else + { + num_roots = 1; + roots[0] = -a0 / a1; + } + } + else + { + num_roots = SolveP2(roots, a1 / a2, a0 / a2); + } + } + else + { + num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3); + } + // std::sort(roots, roots+num_roots); + if (num_roots > 1) + { + if (roots[0] > roots[1]) + btSwap(roots[0], roots[1]); + } + if (num_roots > 2) + { + if (roots[0] > roots[2]) + btSwap(roots[0], roots[2]); + if (roots[1] > roots[2]) + btSwap(roots[1], roots[2]); + } + for (int r = 0; r < num_roots; ++r) + { + double root = roots[r]; + if (root <= 0) + continue; + if (root > dt + SIMD_EPSILON) + return false; + btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v; + btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v; + btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v; + btVector3 x4 = node->m_x + root * node->m_v; + btVector3 normal = (x2 - x1).cross(x3 - x1); + normal.safeNormalize(); + if (proximityTest(x1, x2, x3, x4, normal, mrg, bary)) + return true; + } + return false; } static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) { - if (!bernsteinVFTest(face, node, dt, mrg)) - return false; - if (!continuousCollisionDetection(face, node, dt, 1e-6, bary)) - return false; - return true; + if (!bernsteinVFTest(face, node, dt, mrg)) + return false; + if (!continuousCollisionDetection(face, node, dt, 1e-6, bary)) + return false; + return true; } // @@ -902,62 +906,61 @@ static inline btMatrix3x3 Diagonal(btScalar x) static inline btMatrix3x3 Diagonal(const btVector3& v) { - btMatrix3x3 m; - m[0] = btVector3(v.getX(), 0, 0); - m[1] = btVector3(0, v.getY(), 0); - m[2] = btVector3(0, 0, v.getZ()); - return (m); -} - -static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof) -{ - btScalar result = 0; - for (int i = 0; i < ndof; ++i) - result += a[i] * b[i]; - return result; -} - -static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3, - const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) -{ - btMatrix3x3 m; - btScalar a11 = Dot(v1,u1,ndof); - btScalar a12 = Dot(v1,u2,ndof); - btScalar a13 = Dot(v1,u3,ndof); - - btScalar a21 = Dot(v2,u1,ndof); - btScalar a22 = Dot(v2,u2,ndof); - btScalar a23 = Dot(v2,u3,ndof); - - btScalar a31 = Dot(v3,u1,ndof); - btScalar a32 = Dot(v3,u2,ndof); - btScalar a33 = Dot(v3,u3,ndof); - m[0] = btVector3(a11, a12, a13); - m[1] = btVector3(a21, a22, a23); - m[2] = btVector3(a31, a32, a33); - return (m); -} - -static inline btMatrix3x3 OuterProduct(const btVector3& v1,const btVector3& v2) -{ - btMatrix3x3 m; - btScalar a11 = v1[0] * v2[0]; - btScalar a12 = v1[0] * v2[1]; - btScalar a13 = v1[0] * v2[2]; - - btScalar a21 = v1[1] * v2[0]; - btScalar a22 = v1[1] * v2[1]; - btScalar a23 = v1[1] * v2[2]; - - btScalar a31 = v1[2] * v2[0]; - btScalar a32 = v1[2] * v2[1]; - btScalar a33 = v1[2] * v2[2]; - m[0] = btVector3(a11, a12, a13); - m[1] = btVector3(a21, a22, a23); - m[2] = btVector3(a31, a32, a33); - return (m); + btMatrix3x3 m; + m[0] = btVector3(v.getX(), 0, 0); + m[1] = btVector3(0, v.getY(), 0); + m[2] = btVector3(0, 0, v.getZ()); + return (m); +} + +static inline btScalar Dot(const btScalar* a, const btScalar* b, int ndof) +{ + btScalar result = 0; + for (int i = 0; i < ndof; ++i) + result += a[i] * b[i]; + return result; } +static inline btMatrix3x3 OuterProduct(const btScalar* v1, const btScalar* v2, const btScalar* v3, + const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) +{ + btMatrix3x3 m; + btScalar a11 = Dot(v1, u1, ndof); + btScalar a12 = Dot(v1, u2, ndof); + btScalar a13 = Dot(v1, u3, ndof); + + btScalar a21 = Dot(v2, u1, ndof); + btScalar a22 = Dot(v2, u2, ndof); + btScalar a23 = Dot(v2, u3, ndof); + + btScalar a31 = Dot(v3, u1, ndof); + btScalar a32 = Dot(v3, u2, ndof); + btScalar a33 = Dot(v3, u3, ndof); + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} + +static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2) +{ + btMatrix3x3 m; + btScalar a11 = v1[0] * v2[0]; + btScalar a12 = v1[0] * v2[1]; + btScalar a13 = v1[0] * v2[2]; + + btScalar a21 = v1[1] * v2[0]; + btScalar a22 = v1[1] * v2[1]; + btScalar a23 = v1[1] * v2[2]; + + btScalar a31 = v1[2] * v2[0]; + btScalar a32 = v1[2] * v2[1]; + btScalar a33 = v1[2] * v2[2]; + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} // static inline btMatrix3x3 Add(const btMatrix3x3& a, @@ -1008,6 +1011,20 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt, } // +static inline btMatrix3x3 ImpulseMatrix(btScalar dt, + const btMatrix3x3& effective_mass_inv, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) +{ + return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse()); + // btMatrix3x3 iimb = MassMatrix(imb, iwi, r); + // if (iimb.determinant() == 0) + // return effective_mass_inv.inverse(); + // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse(); +} + +// static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra, btScalar imb, const btMatrix3x3& iib, const btVector3& rb) { @@ -1091,73 +1108,70 @@ static inline void ProjectOrigin(const btVector3& a, // static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t) { - btScalar a, f, u, v; - - btVector3 e1 = v1 - v0; - btVector3 e2 = v2 - v0; - btVector3 h = dir.cross(e2); - a = e1.dot(h); - - if (a > -0.00001 && a < 0.00001) - return (false); - - f = btScalar(1) / a; - btVector3 s = origin - v0; - u = f * s.dot(h); - - if (u < 0.0 || u > 1.0) - return (false); - - btVector3 q = s.cross(e1); - v = f * dir.dot(q); - if (v < 0.0 || u + v > 1.0) - return (false); - // at this stage we can compute t to find out where - // the intersection point is on the line - t = f * e2.dot(q); - if (t > 0) // ray intersection - return (true); - else // this means that there is a line intersection - // but not a ray intersection - return (false); + btScalar a, f, u, v; + + btVector3 e1 = v1 - v0; + btVector3 e2 = v2 - v0; + btVector3 h = dir.cross(e2); + a = e1.dot(h); + + if (a > -0.00001 && a < 0.00001) + return (false); + + f = btScalar(1) / a; + btVector3 s = origin - v0; + u = f * s.dot(h); + + if (u < 0.0 || u > 1.0) + return (false); + + btVector3 q = s.cross(e1); + v = f * dir.dot(q); + if (v < 0.0 || u + v > 1.0) + return (false); + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * e2.dot(q); + if (t > 0) // ray intersection + return (true); + else // this means that there is a line intersection + // but not a ray intersection + return (false); } static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal) { - btVector3 dir = rayEnd - rayStart; - btScalar dir_norm = dir.norm(); - if (dir_norm < SIMD_EPSILON) - return false; - dir.normalize(); - - btScalar t; - - bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); - - if (ret) - { - if (t <= dir_norm) - { - sect = rayStart + dir * t; - } - else - { - ret = false; - } - } - - if (ret) - { - btVector3 n = (p3-p1).cross(p2-p1); - n.safeNormalize(); - if (n.dot(dir) < 0) - normal = n; - else - normal = -n; - } - return ret; -} + btVector3 dir = rayEnd - rayStart; + btScalar dir_norm = dir.norm(); + if (dir_norm < SIMD_EPSILON) + return false; + dir.normalize(); + btScalar t; + bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); + + if (ret) + { + if (t <= dir_norm) + { + sect = rayStart + dir * t; + } + else + { + ret = false; + } + } + if (ret) + { + btVector3 n = (p3 - p1).cross(p2 - p1); + n.safeNormalize(); + if (n.dot(dir) < 0) + normal = n; + else + normal = -n; + } + return ret; +} // template <typename T> @@ -1586,57 +1600,57 @@ struct btSoftColliders psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); } }; - // - // CollideSDF_RS - // - struct CollideSDF_RS : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; - DoNode(*node); - } - void DoNode(btSoftBody::Node& n) const - { - const btScalar m = n.m_im > 0 ? dynmargin : stamargin; - btSoftBody::RContact c; - - if ((!n.m_battach) && - psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) - { - const btScalar ima = n.m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); - const btVector3 vb = n.m_x - n.m_q; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, c.m_cti.m_normal); - const btVector3 fv = vr - c.m_cti.m_normal * dn; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_c2 = ima * psb->m_sst.sdt; - c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } - } - } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; - }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; + + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + { + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; // // CollideSDF_RD @@ -1654,72 +1668,74 @@ struct btSoftColliders btSoftBody::DeformableNodeRigidContact c; if (!n.m_battach) - { + { // check for collision at x_{n+1}^* if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) - { - const btScalar ima = n.m_im; - // todo: collision between multibody and fixed deformable node will be missed. - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - // resolve contact at x_n - psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); - btSoftBody::sCti& cti = c.m_cti; - c.m_node = &n; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 normal = cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - } - } - psb->m_nodeRigidContacts.push_back(c); - } - } + { + const btScalar ima = n.m_im; + // todo: collision between multibody and fixed deformable node will be missed. + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_node = &n; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = n.m_effectiveMass_inv; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); + // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_nodeRigidContacts.push_back(c); + } + } } } btSoftBody* psb; @@ -1728,112 +1744,111 @@ struct btSoftColliders btScalar dynmargin; btScalar stamargin; }; - - // - // CollideSDF_RDF - // - struct CollideSDF_RDF : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; - DoNode(*face); - } - void DoNode(btSoftBody::Face& f) const - { - btSoftBody::Node* n0 = f.m_n[0]; - btSoftBody::Node* n1 = f.m_n[1]; - btSoftBody::Node* n2 = f.m_n[2]; - const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; - btSoftBody::DeformableFaceRigidContact c; - btVector3 contact_point; - btVector3 bary; - if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) - { - f.m_pcontact[3] = 1; - btScalar ima = n0->m_im + n1->m_im + n2->m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - // todo: collision between multibody and fixed deformable face will be missed. - const btScalar ms = ima + imb; - if (ms > 0) - { - // resolve contact at x_n -// psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); - btSoftBody::sCti& cti = c.m_cti; - c.m_contactPoint = contact_point; - c.m_bary = bary; - // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices - c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary; - c.m_face = &f; + + // + // CollideSDF_RDF + // + struct CollideSDF_RDF : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; + DoNode(*face); + } + void DoNode(btSoftBody::Face& f) const + { + btSoftBody::Node* n0 = f.m_n[0]; + btSoftBody::Node* n1 = f.m_n[1]; + btSoftBody::Node* n2 = f.m_n[2]; + const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin; + btSoftBody::DeformableFaceRigidContact c; + btVector3 contact_point; + btVector3 bary; + if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) + { + btScalar ima = n0->m_im + n1->m_im + n2->m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + // todo: collision between multibody and fixed deformable face will be missed. + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + // psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_contactPoint = contact_point; + c.m_bary = bary; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices + c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary; + c.m_face = &f; // friction is handled by the nodes to prevent sticking -// const btScalar fc = 0; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - - // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf - ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im; - c.m_c2 = ima; - c.m_c3 = fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = contact_point - wtr.getOrigin(); - - // we do not scale the impulse matrix by dt - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - c.m_c1 = ra; - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 normal = cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - c.m_c0 = rot.transpose() * local_impulse_matrix * rot; - c.jacobianData_normal = jacobianData_normal; - c.jacobianData_t1 = jacobianData_t1; - c.jacobianData_t2 = jacobianData_t2; - c.t1 = t1; - c.t2 = t2; - } - } - psb->m_faceRigidContacts.push_back(c); - } - } - else - { - f.m_pcontact[3] = 0; - } - } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; - }; - + // const btScalar fc = 0; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf + ima = bary.getX() * c.m_weights.getX() * n0->m_im + bary.getY() * c.m_weights.getY() * n1->m_im + bary.getZ() * c.m_weights.getZ() * n2->m_im; + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = Diagonal(ima); + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = contact_point - wtr.getOrigin(); + + // we do not scale the impulse matrix by dt + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_faceRigidContacts.push_back(c); + } + } + // Set caching barycenters to be false after collision detection. + // Only turn on when contact is static. + f.m_pcontact[3] = 0; + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + // // CollideVF_SS // @@ -1844,12 +1859,12 @@ struct btSoftColliders { btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - for (int i = 0; i < 3; ++i) - { - if (face->m_n[i] == node) - continue; - } - + for (int i = 0; i < 3; ++i) + { + if (face->m_n[i] == node) + continue; + } + btVector3 o = node->m_x; btVector3 p; btScalar d = SIMD_INFINITY; @@ -1879,7 +1894,7 @@ struct btSoftColliders c.m_node = node; c.m_face = face; c.m_weights = w; - c.m_friction = btMax (psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR; c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR; psb[0]->m_scontacts.push_back(c); @@ -1889,206 +1904,205 @@ struct btSoftColliders btSoftBody* psb[2]; btScalar mrg; }; - - - // - // CollideVF_DD - // - struct CollideVF_DD : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; - btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - btVector3 bary; - if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) - { - const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; - const btVector3 w = bary; - const btScalar ma = node->m_im; - btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); - if ((n[0]->m_im <= 0) || - (n[1]->m_im <= 0) || - (n[2]->m_im <= 0)) - { - mb = 0; - } - const btScalar ms = ma + mb; - if (ms > 0) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = w; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar mrg; - bool useFaceNormal; - }; - - // - // CollideFF_DD - // - struct CollideFF_DD : btDbvt::ICollide - { - void Process(const btDbvntNode* lface1, - const btDbvntNode* lface2) - { - btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; - if (f1 != f2) - { - Repel(f1, f2); - Repel(f2, f1); - } - } - void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) - { - //#define REPEL_NEIGHBOR 1 + + // + // CollideVF_DD + // + struct CollideVF_DD : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 bary; + if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) + { + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + const btVector3 w = bary; + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + if ((n[0]->m_im <= 0) || + (n[1]->m_im <= 0) || + (n[2]->m_im <= 0)) + { + mb = 0; + } + const btScalar ms = ma + mb; + if (ms > 0) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = w; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + bool useFaceNormal; + }; + + // + // CollideFF_DD + // + struct CollideFF_DD : btDbvt::ICollide + { + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; + if (f1 != f2) + { + Repel(f1, f2); + Repel(f2, f1); + } + } + void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) + { + //#define REPEL_NEIGHBOR 1 #ifndef REPEL_NEIGHBOR - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - return; - } - } + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + return; + } + } #endif - bool skip = false; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; + bool skip = false; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; #ifdef REPEL_NEIGHBOR - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - { - skip = true; - break; - } - } - if (skip) - { - skip = false; - continue; - } + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + { + skip = false; + continue; + } #endif - btSoftBody::Face* face = f2; - btVector3 bary; - if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) - continue; - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - btSoftBody* psb[2]; - btScalar mrg; - bool useFaceNormal; - }; - - struct CollideCCD : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; - btSoftBody::Face* face = (btSoftBody::Face*)lface->data; - btVector3 bary; - if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - void Process(const btDbvntNode* lface1, - const btDbvntNode* lface2) - { - btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; - if (f1 != f2) - { - Repel(f1, f2); - Repel(f2, f1); - } - } - void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) - { - //#define REPEL_NEIGHBOR 1 + btSoftBody::Face* face = f2; + btVector3 bary; + if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) + continue; + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + btSoftBody* psb[2]; + btScalar mrg; + bool useFaceNormal; + }; + + struct CollideCCD : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 bary; + if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + void Process(const btDbvntNode* lface1, + const btDbvntNode* lface2) + { + btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; + btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; + if (f1 != f2) + { + Repel(f1, f2); + Repel(f2, f1); + } + } + void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) + { + //#define REPEL_NEIGHBOR 1 #ifndef REPEL_NEIGHBOR - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - return; - } - } + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + return; + } + } #endif - bool skip = false; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; + bool skip = false; + for (int node_id = 0; node_id < 3; ++node_id) + { + btSoftBody::Node* node = f1->m_n[node_id]; #ifdef REPEL_NEIGHBOR - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - { - skip = true; - break; - } - } - if (skip) - { - skip = false; - continue; - } + for (int i = 0; i < 3; ++i) + { + if (f2->m_n[i] == node) + { + skip = true; + break; + } + } + if (skip) + { + skip = false; + continue; + } #endif - btSoftBody::Face* face = f2; - btVector3 bary; + btSoftBody::Face* face = f2; + btVector3 bary; if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar dt, mrg; - bool useFaceNormal; - }; + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = face->m_normal; + if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) + c.m_normal = -face->m_normal; + c.m_node = node; + c.m_face = face; + c.m_bary = bary; + c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; + psb[0]->m_faceNodeContacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar dt, mrg; + bool useFaceNormal; + }; }; #endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h b/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h index c4ac4141aa..dbb2624eee 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h @@ -36,7 +36,7 @@ public: CL_SIMD_SOLVER, DX_SOLVER, DX_SIMD_SOLVER, - DEFORMABLE_SOLVER + DEFORMABLE_SOLVER }; protected: diff --git a/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp index 282dbf75f0..329bd19d71 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp @@ -100,6 +100,11 @@ void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeSte ///update soft bodies m_softBodySolver->updateSoftBodies(); + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; + psb->interpolateRenderMesh(); + } // End solver-wise simulation step // /////////////////////////////// } diff --git a/thirdparty/bullet/BulletSoftBody/btSparseSDF.h b/thirdparty/bullet/BulletSoftBody/btSparseSDF.h index eb290a1dbd..d611726bcd 100644 --- a/thirdparty/bullet/BulletSoftBody/btSparseSDF.h +++ b/thirdparty/bullet/BulletSoftBody/btSparseSDF.h @@ -22,36 +22,36 @@ subject to the following restrictions: // Fast Hash -#if !defined (get16bits) -#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\ -+(unsigned int)(((const unsigned char *)(d))[0]) ) +#if !defined(get16bits) +#define get16bits(d) ((((unsigned int)(((const unsigned char*)(d))[1])) << 8) + (unsigned int)(((const unsigned char*)(d))[0])) #endif // // super hash function by Paul Hsieh // -inline unsigned int HsiehHash (const char * data, int len) { - unsigned int hash = len, tmp; - len>>=2; - - /* Main loop */ - for (;len > 0; len--) { - hash += get16bits (data); - tmp = (get16bits (data+2) << 11) ^ hash; - hash = (hash << 16) ^ tmp; - data += 2*sizeof (unsigned short); - hash += hash >> 11; - } +inline unsigned int HsiehHash(const char* data, int len) +{ + unsigned int hash = len, tmp; + len >>= 2; + /* Main loop */ + for (; len > 0; len--) + { + hash += get16bits(data); + tmp = (get16bits(data + 2) << 11) ^ hash; + hash = (hash << 16) ^ tmp; + data += 2 * sizeof(unsigned short); + hash += hash >> 11; + } - /* Force "avalanching" of final 127 bits */ - hash ^= hash << 3; - hash += hash >> 5; - hash ^= hash << 4; - hash += hash >> 17; - hash ^= hash << 25; - hash += hash >> 6; + /* Force "avalanching" of final 127 bits */ + hash ^= hash << 3; + hash += hash >> 5; + hash ^= hash << 4; + hash += hash >> 17; + hash ^= hash << 25; + hash += hash >> 6; - return hash; + return hash; } template <const int CELLSIZE> @@ -81,7 +81,7 @@ struct btSparseSdf btAlignedObjectArray<Cell*> cells; btScalar voxelsz; - btScalar m_defaultVoxelsz; + btScalar m_defaultVoxelsz; int puid; int ncells; int m_clampCells; @@ -103,16 +103,16 @@ struct btSparseSdf //if this limit is reached, the SDF is reset (at the cost of some performance during the reset) m_clampCells = clampCells; cells.resize(hashsize, 0); - m_defaultVoxelsz = 0.25; + m_defaultVoxelsz = 0.25; Reset(); } // - - void setDefaultVoxelsz(btScalar sz) - { - m_defaultVoxelsz = sz; - } - + + void setDefaultVoxelsz(btScalar sz) + { + m_defaultVoxelsz = sz; + } + void Reset() { for (int i = 0, ni = cells.size(); i < ni; ++i) @@ -162,7 +162,7 @@ struct btSparseSdf nqueries = 1; nprobes = 1; ++puid; ///@todo: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ + /* else setup a priority list... */ } // int RemoveReferences(btCollisionShape* pcs) @@ -221,7 +221,7 @@ struct btSparseSdf else { // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]); - //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); + //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); c = c->next; } @@ -363,7 +363,7 @@ struct btSparseSdf myset.p = (void*)shape; const char* ptr = (const char*)&myset; - unsigned int result = HsiehHash(ptr, sizeof(btS) ); + unsigned int result = HsiehHash(ptr, sizeof(btS)); return result; } diff --git a/thirdparty/bullet/BulletSoftBody/poly34.cpp b/thirdparty/bullet/BulletSoftBody/poly34.cpp index 819d0c79f7..ec7549c8e8 100644 --- a/thirdparty/bullet/BulletSoftBody/poly34.cpp +++ b/thirdparty/bullet/BulletSoftBody/poly34.cpp @@ -6,7 +6,7 @@ // #include <math.h> -#include "poly34.h" // solution of cubic and quartic equation +#include "poly34.h" // solution of cubic and quartic equation #define TwoPi 6.28318530717958648 const btScalar eps = SIMD_EPSILON; @@ -15,50 +15,53 @@ const btScalar eps = SIMD_EPSILON; //============================================================================= static SIMD_FORCE_INLINE btScalar _root3(btScalar x) { - btScalar s = 1.; - while (x < 1.) { - x *= 8.; - s *= 0.5; - } - while (x > 8.) { - x *= 0.125; - s *= 2.; - } - btScalar r = 1.5; - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - r -= 1. / 3. * (r - x / (r * r)); - return r * s; + btScalar s = 1.; + while (x < 1.) + { + x *= 8.; + s *= 0.5; + } + while (x > 8.) + { + x *= 0.125; + s *= 2.; + } + btScalar r = 1.5; + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + r -= 1. / 3. * (r - x / (r * r)); + return r * s; } btScalar SIMD_FORCE_INLINE root3(btScalar x) { - if (x > 0) - return _root3(x); - else if (x < 0) - return -_root3(-x); - else - return 0.; + if (x > 0) + return _root3(x); + else if (x < 0) + return -_root3(-x); + else + return 0.; } // x - array of size 2 // return 2: 2 real roots x[0], x[1] // return 0: pair of complex roots: x[0]i*x[1] int SolveP2(btScalar* x, btScalar a, btScalar b) -{ // solve equation x^2 + a*x + b = 0 - btScalar D = 0.25 * a * a - b; - if (D >= 0) { - D = sqrt(D); - x[0] = -0.5 * a + D; - x[1] = -0.5 * a - D; - return 2; - } - x[0] = -0.5 * a; - x[1] = sqrt(-D); - return 0; +{ // solve equation x^2 + a*x + b = 0 + btScalar D = 0.25 * a * a - b; + if (D >= 0) + { + D = sqrt(D); + x[0] = -0.5 * a + D; + x[1] = -0.5 * a - D; + return 2; + } + x[0] = -0.5 * a; + x[1] = sqrt(-D); + return 0; } //--------------------------------------------------------------------------- // x - array of size 3 @@ -66,217 +69,228 @@ int SolveP2(btScalar* x, btScalar a, btScalar b) // 2 real roots: x[0], x[1], return 2 // 1 real root : x[0], x[1] i*x[2], return 1 int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c) -{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 - btScalar a2 = a * a; - btScalar q = (a2 - 3 * b) / 9; - if (q < 0) - q = eps; - btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; - // equation x^3 + q*x + r = 0 - btScalar r2 = r * r; - btScalar q3 = q * q * q; - btScalar A, B; - if (r2 <= (q3 + eps)) { //<<-- FIXED! - btScalar t = r / sqrt(q3); - if (t < -1) - t = -1; - if (t > 1) - t = 1; - t = acos(t); - a /= 3; - q = -2 * sqrt(q); - x[0] = q * cos(t / 3) - a; - x[1] = q * cos((t + TwoPi) / 3) - a; - x[2] = q * cos((t - TwoPi) / 3) - a; - return (3); - } - else { - //A =-pow(fabs(r)+sqrt(r2-q3),1./3); - A = -root3(fabs(r) + sqrt(r2 - q3)); - if (r < 0) - A = -A; - B = (A == 0 ? 0 : q / A); - - a /= 3; - x[0] = (A + B) - a; - x[1] = -0.5 * (A + B) - a; - x[2] = 0.5 * sqrt(3.) * (A - B); - if (fabs(x[2]) < eps) { - x[2] = x[1]; - return (2); - } - return (1); - } -} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { +{ // solve cubic equation x^3 + a*x^2 + b*x + c = 0 + btScalar a2 = a * a; + btScalar q = (a2 - 3 * b) / 9; + if (q < 0) + q = eps; + btScalar r = (a * (2 * a2 - 9 * b) + 27 * c) / 54; + // equation x^3 + q*x + r = 0 + btScalar r2 = r * r; + btScalar q3 = q * q * q; + btScalar A, B; + if (r2 <= (q3 + eps)) + { //<<-- FIXED! + btScalar t = r / sqrt(q3); + if (t < -1) + t = -1; + if (t > 1) + t = 1; + t = acos(t); + a /= 3; + q = -2 * sqrt(q); + x[0] = q * cos(t / 3) - a; + x[1] = q * cos((t + TwoPi) / 3) - a; + x[2] = q * cos((t - TwoPi) / 3) - a; + return (3); + } + else + { + //A =-pow(fabs(r)+sqrt(r2-q3),1./3); + A = -root3(fabs(r) + sqrt(r2 - q3)); + if (r < 0) + A = -A; + B = (A == 0 ? 0 : q / A); + + a /= 3; + x[0] = (A + B) - a; + x[1] = -0.5 * (A + B) - a; + x[2] = 0.5 * sqrt(3.) * (A - B); + if (fabs(x[2]) < eps) + { + x[2] = x[1]; + return (2); + } + return (1); + } +} // SolveP3(btScalar *x,btScalar a,btScalar b,btScalar c) { //--------------------------------------------------------------------------- // a>=0! -void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b) // returns: a+i*s = sqrt(x+i*y) { - btScalar r = sqrt(x * x + y * y); - if (y == 0) { - r = sqrt(r); - if (x >= 0) { - a = r; - b = 0; - } - else { - a = 0; - b = r; - } - } - else { // y != 0 - a = sqrt(0.5 * (x + r)); - b = 0.5 * y / a; - } + btScalar r = sqrt(x * x + y * y); + if (y == 0) + { + r = sqrt(r); + if (x >= 0) + { + a = r; + b = 0; + } + else + { + a = 0; + b = r; + } + } + else + { // y != 0 + a = sqrt(0.5 * (x + r)); + b = 0.5 * y / a; + } } //--------------------------------------------------------------------------- -int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 +int SolveP4Bi(btScalar* x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 + d = 0 { - btScalar D = b * b - 4 * d; - if (D >= 0) { - btScalar sD = sqrt(D); - btScalar x1 = (-b + sD) / 2; - btScalar x2 = (-b - sD) / 2; // x2 <= x1 - if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots - { - btScalar sx1 = sqrt(x1); - btScalar sx2 = sqrt(x2); - x[0] = -sx1; - x[1] = sx1; - x[2] = -sx2; - x[3] = sx2; - return 4; - } - if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots - { - btScalar sx1 = sqrt(-x1); - btScalar sx2 = sqrt(-x2); - x[0] = 0; - x[1] = sx1; - x[2] = 0; - x[3] = sx2; - return 0; - } - // now x2 < 0 <= x1 , two real roots and one pair of imginary root - btScalar sx1 = sqrt(x1); - btScalar sx2 = sqrt(-x2); - x[0] = -sx1; - x[1] = sx1; - x[2] = 0; - x[3] = sx2; - return 2; - } - else { // if( D < 0 ), two pair of compex roots - btScalar sD2 = 0.5 * sqrt(-D); - CSqrt(-0.5 * b, sD2, x[0], x[1]); - CSqrt(-0.5 * b, -sD2, x[2], x[3]); - return 0; - } // if( D>=0 ) -} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d + btScalar D = b * b - 4 * d; + if (D >= 0) + { + btScalar sD = sqrt(D); + btScalar x1 = (-b + sD) / 2; + btScalar x2 = (-b - sD) / 2; // x2 <= x1 + if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots + { + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(x2); + x[0] = -sx1; + x[1] = sx1; + x[2] = -sx2; + x[3] = sx2; + return 4; + } + if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots + { + btScalar sx1 = sqrt(-x1); + btScalar sx2 = sqrt(-x2); + x[0] = 0; + x[1] = sx1; + x[2] = 0; + x[3] = sx2; + return 0; + } + // now x2 < 0 <= x1 , two real roots and one pair of imginary root + btScalar sx1 = sqrt(x1); + btScalar sx2 = sqrt(-x2); + x[0] = -sx1; + x[1] = sx1; + x[2] = 0; + x[3] = sx2; + return 2; + } + else + { // if( D < 0 ), two pair of compex roots + btScalar sD2 = 0.5 * sqrt(-D); + CSqrt(-0.5 * b, sD2, x[0], x[1]); + CSqrt(-0.5 * b, -sD2, x[2], x[3]); + return 0; + } // if( D>=0 ) +} // SolveP4Bi(btScalar *x, btScalar b, btScalar d) // solve equation x^4 + b*x^2 d //--------------------------------------------------------------------------- #define SWAP(a, b) \ -{ \ -t = b; \ -b = a; \ -a = t; \ -} -static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c + { \ + t = b; \ + b = a; \ + a = t; \ + } +static void dblSort3(btScalar& a, btScalar& b, btScalar& c) // make: a <= b <= c { - btScalar t; - if (a > b) - SWAP(a, b); // now a<=b - if (c < b) { - SWAP(b, c); // now a<=b, b<=c - if (a > b) - SWAP(a, b); // now a<=b - } + btScalar t; + if (a > b) + SWAP(a, b); // now a<=b + if (c < b) + { + SWAP(b, c); // now a<=b, b<=c + if (a > b) + SWAP(a, b); // now a<=b + } } //--------------------------------------------------------------------------- -int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d +int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d { - //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0 - if (fabs(c) < 1e-14 * (fabs(b) + fabs(d))) - return SolveP4Bi(x, b, d); // After that, c!=0 - - int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent - // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0 - if (res3 > 1) // 3 real roots, - { - dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2] - // Note: x[0]*x[1]*x[2]= c*c > 0 - if (x[0] > 0) // all roots are positive - { - btScalar sz1 = sqrt(x[0]); - btScalar sz2 = sqrt(x[1]); - btScalar sz3 = sqrt(x[2]); - // Note: sz1*sz2*sz3= -c (and not equal to 0) - if (c > 0) { - x[0] = (-sz1 - sz2 - sz3) / 2; - x[1] = (-sz1 + sz2 + sz3) / 2; - x[2] = (+sz1 - sz2 + sz3) / 2; - x[3] = (+sz1 + sz2 - sz3) / 2; - return 4; - } - // now: c<0 - x[0] = (-sz1 - sz2 + sz3) / 2; - x[1] = (-sz1 + sz2 - sz3) / 2; - x[2] = (+sz1 - sz2 - sz3) / 2; - x[3] = (+sz1 + sz2 + sz3) / 2; - return 4; - } // if( x[0] > 0) // all roots are positive - // now x[0] <= x[1] < 0, x[2] > 0 - // two pair of comlex roots - btScalar sz1 = sqrt(-x[0]); - btScalar sz2 = sqrt(-x[1]); - btScalar sz3 = sqrt(x[2]); - - if (c > 0) // sign = -1 - { - x[0] = -sz3 / 2; - x[1] = (sz1 - sz2) / 2; // x[0]i*x[1] - x[2] = sz3 / 2; - x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3] - return 0; - } - // now: c<0 , sign = +1 - x[0] = sz3 / 2; - x[1] = (-sz1 + sz2) / 2; - x[2] = -sz3 / 2; - x[3] = (sz1 + sz2) / 2; - return 0; - } // if( res3>1 ) // 3 real roots, - // now resoventa have 1 real and pair of compex roots - // x[0] - real root, and x[0]>0, - // x[1]i*x[2] - complex roots, - // x[0] must be >=0. But one times x[0]=~ 1e-17, so: - if (x[0] < 0) - x[0] = 0; - btScalar sz1 = sqrt(x[0]); - btScalar szr, szi; - CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2] - if (c > 0) // sign = -1 - { - x[0] = -sz1 / 2 - szr; // 1st real root - x[1] = -sz1 / 2 + szr; // 2nd real root - x[2] = sz1 / 2; - x[3] = szi; - return 2; - } - // now: c<0 , sign = +1 - x[0] = sz1 / 2 - szr; // 1st real root - x[1] = sz1 / 2 + szr; // 2nd real root - x[2] = -sz1 / 2; - x[3] = szi; - return 2; -} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d + //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0 + if (fabs(c) < 1e-14 * (fabs(b) + fabs(d))) + return SolveP4Bi(x, b, d); // After that, c!=0 + + int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent + // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0 + if (res3 > 1) // 3 real roots, + { + dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2] + // Note: x[0]*x[1]*x[2]= c*c > 0 + if (x[0] > 0) // all roots are positive + { + btScalar sz1 = sqrt(x[0]); + btScalar sz2 = sqrt(x[1]); + btScalar sz3 = sqrt(x[2]); + // Note: sz1*sz2*sz3= -c (and not equal to 0) + if (c > 0) + { + x[0] = (-sz1 - sz2 - sz3) / 2; + x[1] = (-sz1 + sz2 + sz3) / 2; + x[2] = (+sz1 - sz2 + sz3) / 2; + x[3] = (+sz1 + sz2 - sz3) / 2; + return 4; + } + // now: c<0 + x[0] = (-sz1 - sz2 + sz3) / 2; + x[1] = (-sz1 + sz2 - sz3) / 2; + x[2] = (+sz1 - sz2 - sz3) / 2; + x[3] = (+sz1 + sz2 + sz3) / 2; + return 4; + } // if( x[0] > 0) // all roots are positive + // now x[0] <= x[1] < 0, x[2] > 0 + // two pair of comlex roots + btScalar sz1 = sqrt(-x[0]); + btScalar sz2 = sqrt(-x[1]); + btScalar sz3 = sqrt(x[2]); + + if (c > 0) // sign = -1 + { + x[0] = -sz3 / 2; + x[1] = (sz1 - sz2) / 2; // x[0]i*x[1] + x[2] = sz3 / 2; + x[3] = (-sz1 - sz2) / 2; // x[2]i*x[3] + return 0; + } + // now: c<0 , sign = +1 + x[0] = sz3 / 2; + x[1] = (-sz1 + sz2) / 2; + x[2] = -sz3 / 2; + x[3] = (sz1 + sz2) / 2; + return 0; + } // if( res3>1 ) // 3 real roots, + // now resoventa have 1 real and pair of compex roots + // x[0] - real root, and x[0]>0, + // x[1]i*x[2] - complex roots, + // x[0] must be >=0. But one times x[0]=~ 1e-17, so: + if (x[0] < 0) + x[0] = 0; + btScalar sz1 = sqrt(x[0]); + btScalar szr, szi; + CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2] + if (c > 0) // sign = -1 + { + x[0] = -sz1 / 2 - szr; // 1st real root + x[1] = -sz1 / 2 + szr; // 2nd real root + x[2] = sz1 / 2; + x[3] = szi; + return 2; + } + // now: c<0 , sign = +1 + x[0] = sz1 / 2 - szr; // 1st real root + x[1] = sz1 / 2 + szr; // 2nd real root + x[2] = -sz1 / 2; + x[3] = szi; + return 2; +} // SolveP4De(btScalar *x, btScalar b, btScalar c, btScalar d) // solve equation x^4 + b*x^2 + c*x + d //----------------------------------------------------------------------------- -btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d { - btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) - if (fxs == 0) - return x; //return 1e99; <<-- FIXED! - btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) - return x - fx / fxs; + btScalar fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x) + if (fxs == 0) + return x; //return 1e99; <<-- FIXED! + btScalar fx = (((x + a) * x + b) * x + c) * x + d; // f(x) + return x - fx / fxs; } //----------------------------------------------------------------------------- // x - array of size 4 @@ -284,136 +298,150 @@ btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d) // o // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], // return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d) -{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method - // move to a=0: - btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); - btScalar c1 = c + 0.5 * a * (0.25 * a * a - b); - btScalar b1 = b - 0.375 * a * a; - int res = SolveP4De(x, b1, c1, d1); - if (res == 4) { - x[0] -= a / 4; - x[1] -= a / 4; - x[2] -= a / 4; - x[3] -= a / 4; - } - else if (res == 2) { - x[0] -= a / 4; - x[1] -= a / 4; - x[2] -= a / 4; - } - else { - x[0] -= a / 4; - x[2] -= a / 4; - } - // one Newton step for each real root: - if (res > 0) { - x[0] = N4Step(x[0], a, b, c, d); - x[1] = N4Step(x[1], a, b, c, d); - } - if (res > 2) { - x[2] = N4Step(x[2], a, b, c, d); - x[3] = N4Step(x[3], a, b, c, d); - } - return res; +{ // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method + // move to a=0: + btScalar d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c); + btScalar c1 = c + 0.5 * a * (0.25 * a * a - b); + btScalar b1 = b - 0.375 * a * a; + int res = SolveP4De(x, b1, c1, d1); + if (res == 4) + { + x[0] -= a / 4; + x[1] -= a / 4; + x[2] -= a / 4; + x[3] -= a / 4; + } + else if (res == 2) + { + x[0] -= a / 4; + x[1] -= a / 4; + x[2] -= a / 4; + } + else + { + x[0] -= a / 4; + x[2] -= a / 4; + } + // one Newton step for each real root: + if (res > 0) + { + x[0] = N4Step(x[0], a, b, c, d); + x[1] = N4Step(x[1], a, b, c, d); + } + if (res > 2) + { + x[2] = N4Step(x[2], a, b, c, d); + x[3] = N4Step(x[3], a, b, c, d); + } + return res; } //----------------------------------------------------------------------------- #define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e) //----------------------------------------------------------------------------- -btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 { - int cnt; - if (fabs(e) < eps) - return 0; - - btScalar brd = fabs(a); // brd - border of real roots - if (fabs(b) > brd) - brd = fabs(b); - if (fabs(c) > brd) - brd = fabs(c); - if (fabs(d) > brd) - brd = fabs(d); - if (fabs(e) > brd) - brd = fabs(e); - brd++; // brd - border of real roots - - btScalar x0, f0; // less than root - btScalar x1, f1; // greater than root - btScalar x2, f2, f2s; // next values, f(x2), f'(x2) - btScalar dx = 0; - - if (e < 0) { - x0 = 0; - x1 = brd; - f0 = e; - f1 = F5(x1); - x2 = 0.01 * brd; - } // positive root - else { - x0 = -brd; - x1 = 0; - f0 = F5(x0); - f1 = e; - x2 = -0.01 * brd; - } // negative root - - if (fabs(f0) < eps) - return x0; - if (fabs(f1) < eps) - return x1; - - // now x0<x1, f(x0)<0, f(x1)>0 - // Firstly 10 bisections - for (cnt = 0; cnt < 10; cnt++) { - x2 = (x0 + x1) / 2; // next point - //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point - f2 = F5(x2); // f(x2) - if (fabs(f2) < eps) - return x2; - if (f2 > 0) { - x1 = x2; - f1 = f2; - } - else { - x0 = x2; - f0 = f2; - } - } - - // At each step: - // x0<x1, f(x0)<0, f(x1)>0. - // x2 - next value - // we hope that x0 < x2 < x1, but not necessarily - do { - if (cnt++ > 50) - break; - if (x2 <= x0 || x2 >= x1) - x2 = (x0 + x1) / 2; // now x0 < x2 < x1 - f2 = F5(x2); // f(x2) - if (fabs(f2) < eps) - return x2; - if (f2 > 0) { - x1 = x2; - f1 = f2; - } - else { - x0 = x2; - f0 = f2; - } - f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2) - if (fabs(f2s) < eps) { - x2 = 1e99; - continue; - } - dx = f2 / f2s; - x2 -= dx; - } while (fabs(dx) > eps); - return x2; -} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 + int cnt; + if (fabs(e) < eps) + return 0; + + btScalar brd = fabs(a); // brd - border of real roots + if (fabs(b) > brd) + brd = fabs(b); + if (fabs(c) > brd) + brd = fabs(c); + if (fabs(d) > brd) + brd = fabs(d); + if (fabs(e) > brd) + brd = fabs(e); + brd++; // brd - border of real roots + + btScalar x0, f0; // less than root + btScalar x1, f1; // greater than root + btScalar x2, f2, f2s; // next values, f(x2), f'(x2) + btScalar dx = 0; + + if (e < 0) + { + x0 = 0; + x1 = brd; + f0 = e; + f1 = F5(x1); + x2 = 0.01 * brd; + } // positive root + else + { + x0 = -brd; + x1 = 0; + f0 = F5(x0); + f1 = e; + x2 = -0.01 * brd; + } // negative root + + if (fabs(f0) < eps) + return x0; + if (fabs(f1) < eps) + return x1; + + // now x0<x1, f(x0)<0, f(x1)>0 + // Firstly 10 bisections + for (cnt = 0; cnt < 10; cnt++) + { + x2 = (x0 + x1) / 2; // next point + //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point + f2 = F5(x2); // f(x2) + if (fabs(f2) < eps) + return x2; + if (f2 > 0) + { + x1 = x2; + f1 = f2; + } + else + { + x0 = x2; + f0 = f2; + } + } + + // At each step: + // x0<x1, f(x0)<0, f(x1)>0. + // x2 - next value + // we hope that x0 < x2 < x1, but not necessarily + do + { + if (cnt++ > 50) + break; + if (x2 <= x0 || x2 >= x1) + x2 = (x0 + x1) / 2; // now x0 < x2 < x1 + f2 = F5(x2); // f(x2) + if (fabs(f2) < eps) + return x2; + if (f2 > 0) + { + x1 = x2; + f1 = f2; + } + else + { + x0 = x2; + f0 = f2; + } + f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2) + if (fabs(f2s) < eps) + { + x2 = 1e99; + continue; + } + dx = f2 / f2s; + x2 -= dx; + } while (fabs(dx) > eps); + return x2; +} // SolveP5_1(btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- -int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 { - btScalar r = x[0] = SolveP5_1(a, b, c, d, e); - btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; - return 1 + SolveP4(x + 1, a1, b1, c1, d1); -} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 + btScalar r = x[0] = SolveP5_1(a, b, c, d, e); + btScalar a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1; + return 1 + SolveP4(x + 1, a1, b1, c1, d1); +} // SolveP5(btScalar *x,btScalar a,btScalar b,btScalar c,btScalar d,btScalar e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- diff --git a/thirdparty/bullet/BulletSoftBody/poly34.h b/thirdparty/bullet/BulletSoftBody/poly34.h index 32ad5d7da5..35a52c5fec 100644 --- a/thirdparty/bullet/BulletSoftBody/poly34.h +++ b/thirdparty/bullet/BulletSoftBody/poly34.h @@ -8,31 +8,31 @@ // x - array of size 2 // return 2: 2 real roots x[0], x[1] // return 0: pair of complex roots: x[0]i*x[1] -int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0 +int SolveP2(btScalar* x, btScalar a, btScalar b); // solve equation x^2 + a*x + b = 0 // x - array of size 3 // return 3: 3 real roots x[0], x[1], x[2] // return 1: 1 real root x[0] and pair of complex roots: x[1]i*x[2] -int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 +int SolveP3(btScalar* x, btScalar a, btScalar b, btScalar c); // solve cubic equation x^3 + a*x^2 + b*x + c = 0 // x - array of size 4 // return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots // return 2: 2 real roots x[0], x[1] and complex x[2]i*x[3], // return 0: two pair of complex roots: x[0]i*x[1], x[2]i*x[3], -int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method +int SolveP4(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d); // solve equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method // x - array of size 5 // return 5: 5 real roots x[0], x[1], x[2], x[3], x[4], possible multiple roots // return 3: 3 real roots x[0], x[1], x[2] and complex x[3]i*x[4], // return 1: 1 real root x[0] and two pair of complex roots: x[1]i*x[2], x[3]i*x[4], -int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP5(btScalar* x, btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 //----------------------------------------------------------------------------- // And some additional functions for internal use. // Your may remove this definitions from here -int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0 -int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0 -void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y) -btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d -btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 +int SolveP4Bi(btScalar* x, btScalar b, btScalar d); // solve equation x^4 + b*x^2 + d = 0 +int SolveP4De(btScalar* x, btScalar b, btScalar c, btScalar d); // solve equation x^4 + b*x^2 + c*x + d = 0 +void CSqrt(btScalar x, btScalar y, btScalar& a, btScalar& b); // returns as a+i*s, sqrt(x+i*y) +btScalar N4Step(btScalar x, btScalar a, btScalar b, btScalar c, btScalar d); // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d +btScalar SolveP5_1(btScalar a, btScalar b, btScalar c, btScalar d, btScalar e); // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0 #endif diff --git a/thirdparty/bullet/LinearMath/btAlignedAllocator.cpp b/thirdparty/bullet/LinearMath/btAlignedAllocator.cpp index 39b302b600..be8f8aa6d0 100644 --- a/thirdparty/bullet/LinearMath/btAlignedAllocator.cpp +++ b/thirdparty/bullet/LinearMath/btAlignedAllocator.cpp @@ -138,7 +138,7 @@ struct btDebugPtrMagic }; }; -void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filename) +void *btAlignedAllocInternal(size_t size, int alignment, int line, const char *filename) { if (size == 0) { @@ -195,7 +195,7 @@ void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filenam return (ret); } -void btAlignedFreeInternal(void *ptr, int line, char *filename) +void btAlignedFreeInternal(void *ptr, int line, const char *filename) { void *real; diff --git a/thirdparty/bullet/LinearMath/btAlignedAllocator.h b/thirdparty/bullet/LinearMath/btAlignedAllocator.h index ce4d3585f1..971f62bfb0 100644 --- a/thirdparty/bullet/LinearMath/btAlignedAllocator.h +++ b/thirdparty/bullet/LinearMath/btAlignedAllocator.h @@ -35,9 +35,9 @@ int btDumpMemoryLeaks(); #define btAlignedFree(ptr) \ btAlignedFreeInternal(ptr, __LINE__, __FILE__) -void* btAlignedAllocInternal(size_t size, int alignment, int line, char* filename); +void* btAlignedAllocInternal(size_t size, int alignment, int line, const char* filename); -void btAlignedFreeInternal(void* ptr, int line, char* filename); +void btAlignedFreeInternal(void* ptr, int line, const char* filename); #else void* btAlignedAllocInternal(size_t size, int alignment); diff --git a/thirdparty/bullet/LinearMath/btConvexHullComputer.cpp b/thirdparty/bullet/LinearMath/btConvexHullComputer.cpp index 8bbfdc5f25..12125fd2de 100644 --- a/thirdparty/bullet/LinearMath/btConvexHullComputer.cpp +++ b/thirdparty/bullet/LinearMath/btConvexHullComputer.cpp @@ -105,7 +105,7 @@ public: Point64 cross(const Point32& b) const { - return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + return Point64(((int64_t)y) * b.z - ((int64_t)z) * b.y, ((int64_t)z) * b.x - ((int64_t)x) * b.z, ((int64_t)x) * b.y - ((int64_t)y) * b.x); } Point64 cross(const Point64& b) const @@ -115,7 +115,7 @@ public: int64_t dot(const Point32& b) const { - return x * b.x + y * b.y + z * b.z; + return ((int64_t)x) * b.x + ((int64_t)y) * b.y + ((int64_t)z) * b.z; } int64_t dot(const Point64& b) const @@ -2673,6 +2673,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in } vertices.resize(0); + original_vertex_index.resize(0); edges.resize(0); faces.resize(0); @@ -2683,6 +2684,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in { btConvexHullInternal::Vertex* v = oldVertices[copied]; vertices.push_back(hull.getCoordinates(v)); + original_vertex_index.push_back(v->point.index); btConvexHullInternal::Edge* firstEdge = v->edges; if (firstEdge) { diff --git a/thirdparty/bullet/LinearMath/btConvexHullComputer.h b/thirdparty/bullet/LinearMath/btConvexHullComputer.h index cba684f2dc..18b26eea9a 100644 --- a/thirdparty/bullet/LinearMath/btConvexHullComputer.h +++ b/thirdparty/bullet/LinearMath/btConvexHullComputer.h @@ -66,6 +66,9 @@ public: // Vertices of the output hull btAlignedObjectArray<btVector3> vertices; + // The original vertex index in the input coords array + btAlignedObjectArray<int> original_vertex_index; + // Edges of the output hull btAlignedObjectArray<Edge> edges; diff --git a/thirdparty/bullet/LinearMath/btReducedVector.h b/thirdparty/bullet/LinearMath/btReducedVector.h index 83b5e581e5..313a4271f0 100644 --- a/thirdparty/bullet/LinearMath/btReducedVector.h +++ b/thirdparty/bullet/LinearMath/btReducedVector.h @@ -267,7 +267,7 @@ public: std::sort(tuples.begin(), tuples.end()); btAlignedObjectArray<int> new_indices; btAlignedObjectArray<btVector3> new_vecs; - for (int i = 0; i < tuples.size(); ++i) + for (size_t i = 0; i < tuples.size(); ++i) { new_indices.push_back(tuples[i].b); new_vecs.push_back(m_vecs[tuples[i].a]); diff --git a/thirdparty/bullet/LinearMath/btScalar.h b/thirdparty/bullet/LinearMath/btScalar.h index 86d94e8974..36b90cc944 100644 --- a/thirdparty/bullet/LinearMath/btScalar.h +++ b/thirdparty/bullet/LinearMath/btScalar.h @@ -25,7 +25,7 @@ subject to the following restrictions: #include <float.h> /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 289 +#define BT_BULLET_VERSION 307 inline int btGetVersion() { diff --git a/thirdparty/bullet/LinearMath/btSerializer.h b/thirdparty/bullet/LinearMath/btSerializer.h index 2ee712047f..9abcf031d0 100644 --- a/thirdparty/bullet/LinearMath/btSerializer.h +++ b/thirdparty/bullet/LinearMath/btSerializer.h @@ -479,9 +479,9 @@ public: buffer[8] = 'V'; } - buffer[9] = '2'; - buffer[10] = '8'; - buffer[11] = '9'; + buffer[9] = '3'; + buffer[10] = '0'; + buffer[11] = '7'; } virtual void startSerialization() diff --git a/thirdparty/mbedtls/include/mbedtls/bignum.h b/thirdparty/mbedtls/include/mbedtls/bignum.h index 4bb9fa3d43..4d04b336e7 100644 --- a/thirdparty/mbedtls/include/mbedtls/bignum.h +++ b/thirdparty/mbedtls/include/mbedtls/bignum.h @@ -88,12 +88,12 @@ * Maximum window size used for modular exponentiation. Default: 6 * Minimum value: 1. Maximum value: 6. * - * Result is an array of ( 2 << MBEDTLS_MPI_WINDOW_SIZE ) MPIs used + * Result is an array of ( 2 ** MBEDTLS_MPI_WINDOW_SIZE ) MPIs used * for the sliding window calculation. (So 64 by default) * * Reduction in size, reduces speed. */ -#define MBEDTLS_MPI_WINDOW_SIZE 6 /**< Maximum windows size used. */ +#define MBEDTLS_MPI_WINDOW_SIZE 6 /**< Maximum window size used. */ #endif /* !MBEDTLS_MPI_WINDOW_SIZE */ #if !defined(MBEDTLS_MPI_MAX_SIZE) diff --git a/thirdparty/mbedtls/include/mbedtls/ccm.h b/thirdparty/mbedtls/include/mbedtls/ccm.h index 3dcdc91894..d50c6ec993 100644 --- a/thirdparty/mbedtls/include/mbedtls/ccm.h +++ b/thirdparty/mbedtls/include/mbedtls/ccm.h @@ -175,7 +175,7 @@ void mbedtls_ccm_free( mbedtls_ccm_context *ctx ); * than zero, \p output must be a writable buffer of at least * that length. * \param tag The buffer holding the authentication field. This must be a - * readable buffer of at least \p tag_len Bytes. + * writable buffer of at least \p tag_len Bytes. * \param tag_len The length of the authentication field to generate in Bytes: * 4, 6, 8, 10, 12, 14 or 16. * @@ -220,7 +220,7 @@ int mbedtls_ccm_encrypt_and_tag( mbedtls_ccm_context *ctx, size_t length, * than zero, \p output must be a writable buffer of at least * that length. * \param tag The buffer holding the authentication field. This must be a - * readable buffer of at least \p tag_len Bytes. + * writable buffer of at least \p tag_len Bytes. * \param tag_len The length of the authentication field to generate in Bytes: * 0, 4, 6, 8, 10, 12, 14 or 16. * diff --git a/thirdparty/mbedtls/include/mbedtls/config.h b/thirdparty/mbedtls/include/mbedtls/config.h index 217998a5eb..e17bc7e306 100644 --- a/thirdparty/mbedtls/include/mbedtls/config.h +++ b/thirdparty/mbedtls/include/mbedtls/config.h @@ -3128,7 +3128,7 @@ */ /* MPI / BIGNUM options */ -//#define MBEDTLS_MPI_WINDOW_SIZE 6 /**< Maximum windows size used. */ +//#define MBEDTLS_MPI_WINDOW_SIZE 6 /**< Maximum window size used. */ //#define MBEDTLS_MPI_MAX_SIZE 1024 /**< Maximum number of bytes for usable MPIs. */ /* CTR_DRBG options */ diff --git a/thirdparty/mbedtls/include/mbedtls/ctr_drbg.h b/thirdparty/mbedtls/include/mbedtls/ctr_drbg.h index 7e5f2e5769..278fbbbb7a 100644 --- a/thirdparty/mbedtls/include/mbedtls/ctr_drbg.h +++ b/thirdparty/mbedtls/include/mbedtls/ctr_drbg.h @@ -224,6 +224,11 @@ mbedtls_ctr_drbg_context; * and prepares it for mbedtls_ctr_drbg_seed() * or mbedtls_ctr_drbg_free(). * + * \note The reseed interval is + * #MBEDTLS_CTR_DRBG_RESEED_INTERVAL by default. + * You can override it by calling + * mbedtls_ctr_drbg_set_reseed_interval(). + * * \param ctx The CTR_DRBG context to initialize. */ void mbedtls_ctr_drbg_init( mbedtls_ctr_drbg_context *ctx ); @@ -305,7 +310,8 @@ int mbedtls_ctr_drbg_seed( mbedtls_ctr_drbg_context *ctx, size_t len ); /** - * \brief This function clears CTR_CRBG context data. + * \brief This function resets CTR_DRBG context to the state immediately + * after initial call of mbedtls_ctr_drbg_init(). * * \param ctx The CTR_DRBG context to clear. */ diff --git a/thirdparty/mbedtls/include/mbedtls/gcm.h b/thirdparty/mbedtls/include/mbedtls/gcm.h index 4e4434ed4d..1201fbd4f1 100644 --- a/thirdparty/mbedtls/include/mbedtls/gcm.h +++ b/thirdparty/mbedtls/include/mbedtls/gcm.h @@ -182,7 +182,7 @@ int mbedtls_gcm_setkey( mbedtls_gcm_context *ctx, * than zero, this must be a writable buffer of at least that * size in Bytes. * \param tag_len The length of the tag to generate. - * \param tag The buffer for holding the tag. This must be a readable + * \param tag The buffer for holding the tag. This must be a writable * buffer of at least \p tag_len Bytes. * * \return \c 0 if the encryption or decryption was performed @@ -310,7 +310,7 @@ int mbedtls_gcm_update( mbedtls_gcm_context *ctx, * tag. The tag can have a maximum length of 16 Bytes. * * \param ctx The GCM context. This must be initialized. - * \param tag The buffer for holding the tag. This must be a readable + * \param tag The buffer for holding the tag. This must be a writable * buffer of at least \p tag_len Bytes. * \param tag_len The length of the tag to generate. This must be at least * four. diff --git a/thirdparty/mbedtls/include/mbedtls/hmac_drbg.h b/thirdparty/mbedtls/include/mbedtls/hmac_drbg.h index 6883678204..970c033c15 100644 --- a/thirdparty/mbedtls/include/mbedtls/hmac_drbg.h +++ b/thirdparty/mbedtls/include/mbedtls/hmac_drbg.h @@ -138,6 +138,10 @@ typedef struct mbedtls_hmac_drbg_context * This function makes the context ready for mbedtls_hmac_drbg_seed(), * mbedtls_hmac_drbg_seed_buf() or mbedtls_hmac_drbg_free(). * + * \note The reseed interval is #MBEDTLS_HMAC_DRBG_RESEED_INTERVAL + * by default. Override this value by calling + * mbedtls_hmac_drbg_set_reseed_interval(). + * * \param ctx HMAC_DRBG context to be initialized. */ void mbedtls_hmac_drbg_init( mbedtls_hmac_drbg_context *ctx ); @@ -361,7 +365,8 @@ int mbedtls_hmac_drbg_random_with_add( void *p_rng, int mbedtls_hmac_drbg_random( void *p_rng, unsigned char *output, size_t out_len ); /** - * \brief Free an HMAC_DRBG context + * \brief This function resets HMAC_DRBG context to the state immediately + * after initial call of mbedtls_hmac_drbg_init(). * * \param ctx The HMAC_DRBG context to free. */ diff --git a/thirdparty/mbedtls/include/mbedtls/sha512.h b/thirdparty/mbedtls/include/mbedtls/sha512.h index 9ff78ecf41..5e5a15e000 100644 --- a/thirdparty/mbedtls/include/mbedtls/sha512.h +++ b/thirdparty/mbedtls/include/mbedtls/sha512.h @@ -152,8 +152,7 @@ int mbedtls_sha512_update_ret( mbedtls_sha512_context *ctx, /** * \brief This function finishes the SHA-512 operation, and writes - * the result to the output buffer. This function is for - * internal use only. + * the result to the output buffer. * * \param ctx The SHA-512 context. This must be initialized * and have a hash operation started. @@ -169,6 +168,7 @@ int mbedtls_sha512_finish_ret( mbedtls_sha512_context *ctx, /** * \brief This function processes a single data block within * the ongoing SHA-512 computation. + * This function is for internal use only. * * \param ctx The SHA-512 context. This must be initialized. * \param data The buffer holding one block of data. This diff --git a/thirdparty/mbedtls/include/mbedtls/ssl.h b/thirdparty/mbedtls/include/mbedtls/ssl.h index d3ee3c4e6f..fe33ac8d57 100644 --- a/thirdparty/mbedtls/include/mbedtls/ssl.h +++ b/thirdparty/mbedtls/include/mbedtls/ssl.h @@ -1409,7 +1409,7 @@ void mbedtls_ssl_conf_dbg( mbedtls_ssl_config *conf, * \note For DTLS, you need to provide either a non-NULL * f_recv_timeout callback, or a f_recv that doesn't block. * - * \note See the documentations of \c mbedtls_ssl_sent_t, + * \note See the documentations of \c mbedtls_ssl_send_t, * \c mbedtls_ssl_recv_t and \c mbedtls_ssl_recv_timeout_t for * the conventions those callbacks must follow. * diff --git a/thirdparty/mbedtls/include/mbedtls/version.h b/thirdparty/mbedtls/include/mbedtls/version.h index d09b45002d..5f0a8f114c 100644 --- a/thirdparty/mbedtls/include/mbedtls/version.h +++ b/thirdparty/mbedtls/include/mbedtls/version.h @@ -65,16 +65,16 @@ */ #define MBEDTLS_VERSION_MAJOR 2 #define MBEDTLS_VERSION_MINOR 16 -#define MBEDTLS_VERSION_PATCH 8 +#define MBEDTLS_VERSION_PATCH 9 /** * The single version number has the following structure: * MMNNPP00 * Major version | Minor version | Patch version */ -#define MBEDTLS_VERSION_NUMBER 0x02100800 -#define MBEDTLS_VERSION_STRING "2.16.8" -#define MBEDTLS_VERSION_STRING_FULL "mbed TLS 2.16.8" +#define MBEDTLS_VERSION_NUMBER 0x02100900 +#define MBEDTLS_VERSION_STRING "2.16.9" +#define MBEDTLS_VERSION_STRING_FULL "mbed TLS 2.16.9" #if defined(MBEDTLS_VERSION_C) diff --git a/thirdparty/mbedtls/library/aes.c b/thirdparty/mbedtls/library/aes.c index 9b337505fd..da0e5b6bdc 100644 --- a/thirdparty/mbedtls/library/aes.c +++ b/thirdparty/mbedtls/library/aes.c @@ -760,6 +760,7 @@ exit: return( ret ); } +#endif /* !MBEDTLS_AES_SETKEY_DEC_ALT */ #if defined(MBEDTLS_CIPHER_MODE_XTS) static int mbedtls_aes_xts_decode_keys( const unsigned char *key, @@ -838,8 +839,6 @@ int mbedtls_aes_xts_setkey_dec( mbedtls_aes_xts_context *ctx, } #endif /* MBEDTLS_CIPHER_MODE_XTS */ -#endif /* !MBEDTLS_AES_SETKEY_DEC_ALT */ - #define AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3) \ do \ { \ @@ -897,63 +896,56 @@ int mbedtls_internal_aes_encrypt( mbedtls_aes_context *ctx, unsigned char output[16] ) { int i; - uint32_t *RK, X0, X1, X2, X3, Y0, Y1, Y2, Y3; - - RK = ctx->rk; + uint32_t *RK = ctx->rk; + struct + { + uint32_t X[4]; + uint32_t Y[4]; + } t; - GET_UINT32_LE( X0, input, 0 ); X0 ^= *RK++; - GET_UINT32_LE( X1, input, 4 ); X1 ^= *RK++; - GET_UINT32_LE( X2, input, 8 ); X2 ^= *RK++; - GET_UINT32_LE( X3, input, 12 ); X3 ^= *RK++; + GET_UINT32_LE( t.X[0], input, 0 ); t.X[0] ^= *RK++; + GET_UINT32_LE( t.X[1], input, 4 ); t.X[1] ^= *RK++; + GET_UINT32_LE( t.X[2], input, 8 ); t.X[2] ^= *RK++; + GET_UINT32_LE( t.X[3], input, 12 ); t.X[3] ^= *RK++; for( i = ( ctx->nr >> 1 ) - 1; i > 0; i-- ) { - AES_FROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 ); - AES_FROUND( X0, X1, X2, X3, Y0, Y1, Y2, Y3 ); + AES_FROUND( t.Y[0], t.Y[1], t.Y[2], t.Y[3], t.X[0], t.X[1], t.X[2], t.X[3] ); + AES_FROUND( t.X[0], t.X[1], t.X[2], t.X[3], t.Y[0], t.Y[1], t.Y[2], t.Y[3] ); } - AES_FROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 ); - - X0 = *RK++ ^ \ - ( (uint32_t) FSb[ ( Y0 ) & 0xFF ] ) ^ - ( (uint32_t) FSb[ ( Y1 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) FSb[ ( Y2 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) FSb[ ( Y3 >> 24 ) & 0xFF ] << 24 ); - - X1 = *RK++ ^ \ - ( (uint32_t) FSb[ ( Y1 ) & 0xFF ] ) ^ - ( (uint32_t) FSb[ ( Y2 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) FSb[ ( Y3 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) FSb[ ( Y0 >> 24 ) & 0xFF ] << 24 ); - - X2 = *RK++ ^ \ - ( (uint32_t) FSb[ ( Y2 ) & 0xFF ] ) ^ - ( (uint32_t) FSb[ ( Y3 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) FSb[ ( Y0 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) FSb[ ( Y1 >> 24 ) & 0xFF ] << 24 ); - - X3 = *RK++ ^ \ - ( (uint32_t) FSb[ ( Y3 ) & 0xFF ] ) ^ - ( (uint32_t) FSb[ ( Y0 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) FSb[ ( Y1 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) FSb[ ( Y2 >> 24 ) & 0xFF ] << 24 ); - - PUT_UINT32_LE( X0, output, 0 ); - PUT_UINT32_LE( X1, output, 4 ); - PUT_UINT32_LE( X2, output, 8 ); - PUT_UINT32_LE( X3, output, 12 ); - - mbedtls_platform_zeroize( &X0, sizeof( X0 ) ); - mbedtls_platform_zeroize( &X1, sizeof( X1 ) ); - mbedtls_platform_zeroize( &X2, sizeof( X2 ) ); - mbedtls_platform_zeroize( &X3, sizeof( X3 ) ); - - mbedtls_platform_zeroize( &Y0, sizeof( Y0 ) ); - mbedtls_platform_zeroize( &Y1, sizeof( Y1 ) ); - mbedtls_platform_zeroize( &Y2, sizeof( Y2 ) ); - mbedtls_platform_zeroize( &Y3, sizeof( Y3 ) ); - - mbedtls_platform_zeroize( &RK, sizeof( RK ) ); + AES_FROUND( t.Y[0], t.Y[1], t.Y[2], t.Y[3], t.X[0], t.X[1], t.X[2], t.X[3] ); + + t.X[0] = *RK++ ^ \ + ( (uint32_t) FSb[ ( t.Y[0] ) & 0xFF ] ) ^ + ( (uint32_t) FSb[ ( t.Y[1] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) FSb[ ( t.Y[2] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) FSb[ ( t.Y[3] >> 24 ) & 0xFF ] << 24 ); + + t.X[1] = *RK++ ^ \ + ( (uint32_t) FSb[ ( t.Y[1] ) & 0xFF ] ) ^ + ( (uint32_t) FSb[ ( t.Y[2] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) FSb[ ( t.Y[3] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) FSb[ ( t.Y[0] >> 24 ) & 0xFF ] << 24 ); + + t.X[2] = *RK++ ^ \ + ( (uint32_t) FSb[ ( t.Y[2] ) & 0xFF ] ) ^ + ( (uint32_t) FSb[ ( t.Y[3] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) FSb[ ( t.Y[0] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) FSb[ ( t.Y[1] >> 24 ) & 0xFF ] << 24 ); + + t.X[3] = *RK++ ^ \ + ( (uint32_t) FSb[ ( t.Y[3] ) & 0xFF ] ) ^ + ( (uint32_t) FSb[ ( t.Y[0] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) FSb[ ( t.Y[1] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) FSb[ ( t.Y[2] >> 24 ) & 0xFF ] << 24 ); + + PUT_UINT32_LE( t.X[0], output, 0 ); + PUT_UINT32_LE( t.X[1], output, 4 ); + PUT_UINT32_LE( t.X[2], output, 8 ); + PUT_UINT32_LE( t.X[3], output, 12 ); + + mbedtls_platform_zeroize( &t, sizeof( t ) ); return( 0 ); } @@ -977,63 +969,56 @@ int mbedtls_internal_aes_decrypt( mbedtls_aes_context *ctx, unsigned char output[16] ) { int i; - uint32_t *RK, X0, X1, X2, X3, Y0, Y1, Y2, Y3; - - RK = ctx->rk; + uint32_t *RK = ctx->rk; + struct + { + uint32_t X[4]; + uint32_t Y[4]; + } t; - GET_UINT32_LE( X0, input, 0 ); X0 ^= *RK++; - GET_UINT32_LE( X1, input, 4 ); X1 ^= *RK++; - GET_UINT32_LE( X2, input, 8 ); X2 ^= *RK++; - GET_UINT32_LE( X3, input, 12 ); X3 ^= *RK++; + GET_UINT32_LE( t.X[0], input, 0 ); t.X[0] ^= *RK++; + GET_UINT32_LE( t.X[1], input, 4 ); t.X[1] ^= *RK++; + GET_UINT32_LE( t.X[2], input, 8 ); t.X[2] ^= *RK++; + GET_UINT32_LE( t.X[3], input, 12 ); t.X[3] ^= *RK++; for( i = ( ctx->nr >> 1 ) - 1; i > 0; i-- ) { - AES_RROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 ); - AES_RROUND( X0, X1, X2, X3, Y0, Y1, Y2, Y3 ); + AES_RROUND( t.Y[0], t.Y[1], t.Y[2], t.Y[3], t.X[0], t.X[1], t.X[2], t.X[3] ); + AES_RROUND( t.X[0], t.X[1], t.X[2], t.X[3], t.Y[0], t.Y[1], t.Y[2], t.Y[3] ); } - AES_RROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 ); - - X0 = *RK++ ^ \ - ( (uint32_t) RSb[ ( Y0 ) & 0xFF ] ) ^ - ( (uint32_t) RSb[ ( Y3 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) RSb[ ( Y2 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) RSb[ ( Y1 >> 24 ) & 0xFF ] << 24 ); - - X1 = *RK++ ^ \ - ( (uint32_t) RSb[ ( Y1 ) & 0xFF ] ) ^ - ( (uint32_t) RSb[ ( Y0 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) RSb[ ( Y3 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) RSb[ ( Y2 >> 24 ) & 0xFF ] << 24 ); - - X2 = *RK++ ^ \ - ( (uint32_t) RSb[ ( Y2 ) & 0xFF ] ) ^ - ( (uint32_t) RSb[ ( Y1 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) RSb[ ( Y0 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) RSb[ ( Y3 >> 24 ) & 0xFF ] << 24 ); - - X3 = *RK++ ^ \ - ( (uint32_t) RSb[ ( Y3 ) & 0xFF ] ) ^ - ( (uint32_t) RSb[ ( Y2 >> 8 ) & 0xFF ] << 8 ) ^ - ( (uint32_t) RSb[ ( Y1 >> 16 ) & 0xFF ] << 16 ) ^ - ( (uint32_t) RSb[ ( Y0 >> 24 ) & 0xFF ] << 24 ); - - PUT_UINT32_LE( X0, output, 0 ); - PUT_UINT32_LE( X1, output, 4 ); - PUT_UINT32_LE( X2, output, 8 ); - PUT_UINT32_LE( X3, output, 12 ); - - mbedtls_platform_zeroize( &X0, sizeof( X0 ) ); - mbedtls_platform_zeroize( &X1, sizeof( X1 ) ); - mbedtls_platform_zeroize( &X2, sizeof( X2 ) ); - mbedtls_platform_zeroize( &X3, sizeof( X3 ) ); - - mbedtls_platform_zeroize( &Y0, sizeof( Y0 ) ); - mbedtls_platform_zeroize( &Y1, sizeof( Y1 ) ); - mbedtls_platform_zeroize( &Y2, sizeof( Y2 ) ); - mbedtls_platform_zeroize( &Y3, sizeof( Y3 ) ); - - mbedtls_platform_zeroize( &RK, sizeof( RK ) ); + AES_RROUND( t.Y[0], t.Y[1], t.Y[2], t.Y[3], t.X[0], t.X[1], t.X[2], t.X[3] ); + + t.X[0] = *RK++ ^ \ + ( (uint32_t) RSb[ ( t.Y[0] ) & 0xFF ] ) ^ + ( (uint32_t) RSb[ ( t.Y[3] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) RSb[ ( t.Y[2] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) RSb[ ( t.Y[1] >> 24 ) & 0xFF ] << 24 ); + + t.X[1] = *RK++ ^ \ + ( (uint32_t) RSb[ ( t.Y[1] ) & 0xFF ] ) ^ + ( (uint32_t) RSb[ ( t.Y[0] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) RSb[ ( t.Y[3] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) RSb[ ( t.Y[2] >> 24 ) & 0xFF ] << 24 ); + + t.X[2] = *RK++ ^ \ + ( (uint32_t) RSb[ ( t.Y[2] ) & 0xFF ] ) ^ + ( (uint32_t) RSb[ ( t.Y[1] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) RSb[ ( t.Y[0] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) RSb[ ( t.Y[3] >> 24 ) & 0xFF ] << 24 ); + + t.X[3] = *RK++ ^ \ + ( (uint32_t) RSb[ ( t.Y[3] ) & 0xFF ] ) ^ + ( (uint32_t) RSb[ ( t.Y[2] >> 8 ) & 0xFF ] << 8 ) ^ + ( (uint32_t) RSb[ ( t.Y[1] >> 16 ) & 0xFF ] << 16 ) ^ + ( (uint32_t) RSb[ ( t.Y[0] >> 24 ) & 0xFF ] << 24 ); + + PUT_UINT32_LE( t.X[0], output, 0 ); + PUT_UINT32_LE( t.X[1], output, 4 ); + PUT_UINT32_LE( t.X[2], output, 8 ); + PUT_UINT32_LE( t.X[3], output, 12 ); + + mbedtls_platform_zeroize( &t, sizeof( t ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/bignum.c b/thirdparty/mbedtls/library/bignum.c index dfe976d648..2feb727d89 100644 --- a/thirdparty/mbedtls/library/bignum.c +++ b/thirdparty/mbedtls/library/bignum.c @@ -1364,7 +1364,10 @@ int mbedtls_mpi_sub_abs( mbedtls_mpi *X, const mbedtls_mpi *A, const mbedtls_mpi /* If we ran out of space for the carry, it means that the result * is negative. */ if( n == X->n ) - return( MBEDTLS_ERR_MPI_NEGATIVE_VALUE ); + { + ret = MBEDTLS_ERR_MPI_NEGATIVE_VALUE; + goto cleanup; + } --X->p[n]; } @@ -2044,7 +2047,7 @@ int mbedtls_mpi_exp_mod( mbedtls_mpi *X, const mbedtls_mpi *A, size_t i, j, nblimbs; size_t bufsize, nbits; mbedtls_mpi_uint ei, mm, state; - mbedtls_mpi RR, T, W[ 2 << MBEDTLS_MPI_WINDOW_SIZE ], Apos; + mbedtls_mpi RR, T, W[ 1 << MBEDTLS_MPI_WINDOW_SIZE ], Apos; int neg; MPI_VALIDATE_RET( X != NULL ); @@ -2058,6 +2061,10 @@ int mbedtls_mpi_exp_mod( mbedtls_mpi *X, const mbedtls_mpi *A, if( mbedtls_mpi_cmp_int( E, 0 ) < 0 ) return( MBEDTLS_ERR_MPI_BAD_INPUT_DATA ); + if( mbedtls_mpi_bitlen( E ) > MBEDTLS_MPI_MAX_BITS || + mbedtls_mpi_bitlen( N ) > MBEDTLS_MPI_MAX_BITS ) + return ( MBEDTLS_ERR_MPI_BAD_INPUT_DATA ); + /* * Init temps and window size */ @@ -2334,7 +2341,7 @@ int mbedtls_mpi_fill_random( mbedtls_mpi *X, size_t size, MBEDTLS_MPI_CHK( mbedtls_mpi_lset( X, 0 ) ); Xp = (unsigned char*) X->p; - f_rng( p_rng, Xp + overhead, size ); + MBEDTLS_MPI_CHK( f_rng( p_rng, Xp + overhead, size ) ); mpi_bigendian_to_host( X->p, limbs ); diff --git a/thirdparty/mbedtls/library/cipher_wrap.c b/thirdparty/mbedtls/library/cipher_wrap.c index 1dcac21be1..5973ca6ba2 100644 --- a/thirdparty/mbedtls/library/cipher_wrap.c +++ b/thirdparty/mbedtls/library/cipher_wrap.c @@ -779,7 +779,7 @@ static const mbedtls_cipher_info_t camellia_128_ecb_info = { MBEDTLS_MODE_ECB, 128, "CAMELLIA-128-ECB", - 16, + 0, 0, 16, &camellia_info @@ -790,7 +790,7 @@ static const mbedtls_cipher_info_t camellia_192_ecb_info = { MBEDTLS_MODE_ECB, 192, "CAMELLIA-192-ECB", - 16, + 0, 0, 16, &camellia_info @@ -801,7 +801,7 @@ static const mbedtls_cipher_info_t camellia_256_ecb_info = { MBEDTLS_MODE_ECB, 256, "CAMELLIA-256-ECB", - 16, + 0, 0, 16, &camellia_info @@ -1155,7 +1155,7 @@ static const mbedtls_cipher_info_t aria_128_ecb_info = { MBEDTLS_MODE_ECB, 128, "ARIA-128-ECB", - 16, + 0, 0, 16, &aria_info @@ -1166,7 +1166,7 @@ static const mbedtls_cipher_info_t aria_192_ecb_info = { MBEDTLS_MODE_ECB, 192, "ARIA-192-ECB", - 16, + 0, 0, 16, &aria_info @@ -1177,7 +1177,7 @@ static const mbedtls_cipher_info_t aria_256_ecb_info = { MBEDTLS_MODE_ECB, 256, "ARIA-256-ECB", - 16, + 0, 0, 16, &aria_info @@ -1579,7 +1579,7 @@ static const mbedtls_cipher_info_t des_ecb_info = { MBEDTLS_MODE_ECB, MBEDTLS_KEY_LENGTH_DES, "DES-ECB", - 8, + 0, 0, 8, &des_info @@ -1630,7 +1630,7 @@ static const mbedtls_cipher_info_t des_ede_ecb_info = { MBEDTLS_MODE_ECB, MBEDTLS_KEY_LENGTH_DES_EDE, "DES-EDE-ECB", - 8, + 0, 0, 8, &des_ede_info @@ -1681,7 +1681,7 @@ static const mbedtls_cipher_info_t des_ede3_ecb_info = { MBEDTLS_MODE_ECB, MBEDTLS_KEY_LENGTH_DES_EDE3, "DES-EDE3-ECB", - 8, + 0, 0, 8, &des_ede3_info @@ -1796,7 +1796,7 @@ static const mbedtls_cipher_info_t blowfish_ecb_info = { MBEDTLS_MODE_ECB, 128, "BLOWFISH-ECB", - 8, + 0, MBEDTLS_CIPHER_VARIABLE_KEY_LEN, 8, &blowfish_info diff --git a/thirdparty/mbedtls/library/cmac.c b/thirdparty/mbedtls/library/cmac.c index 1a1200b52b..409f67958e 100644 --- a/thirdparty/mbedtls/library/cmac.c +++ b/thirdparty/mbedtls/library/cmac.c @@ -450,7 +450,7 @@ exit: */ int mbedtls_aes_cmac_prf_128( const unsigned char *key, size_t key_length, const unsigned char *input, size_t in_len, - unsigned char *output ) + unsigned char output[16] ) { int ret; const mbedtls_cipher_info_t *cipher_info; diff --git a/thirdparty/mbedtls/library/ctr_drbg.c b/thirdparty/mbedtls/library/ctr_drbg.c index b98df29a9b..e92008bbe8 100644 --- a/thirdparty/mbedtls/library/ctr_drbg.c +++ b/thirdparty/mbedtls/library/ctr_drbg.c @@ -82,11 +82,17 @@ void mbedtls_ctr_drbg_init( mbedtls_ctr_drbg_context *ctx ) { memset( ctx, 0, sizeof( mbedtls_ctr_drbg_context ) ); + ctx->reseed_interval = MBEDTLS_CTR_DRBG_RESEED_INTERVAL; + #if defined(MBEDTLS_THREADING_C) mbedtls_mutex_init( &ctx->mutex ); #endif } +/* + * This function resets CTR_DRBG context to the state immediately + * after initial call of mbedtls_ctr_drbg_init(). + */ void mbedtls_ctr_drbg_free( mbedtls_ctr_drbg_context *ctx ) { if( ctx == NULL ) @@ -97,6 +103,10 @@ void mbedtls_ctr_drbg_free( mbedtls_ctr_drbg_context *ctx ) #endif mbedtls_aes_free( &ctx->aes_ctx ); mbedtls_platform_zeroize( ctx, sizeof( mbedtls_ctr_drbg_context ) ); + ctx->reseed_interval = MBEDTLS_CTR_DRBG_RESEED_INTERVAL; +#if defined(MBEDTLS_THREADING_C) + mbedtls_mutex_init( &ctx->mutex ); +#endif } void mbedtls_ctr_drbg_set_prediction_resistance( mbedtls_ctr_drbg_context *ctx, int resistance ) @@ -419,7 +429,6 @@ int mbedtls_ctr_drbg_seed( mbedtls_ctr_drbg_context *ctx, if( ctx->entropy_len == 0 ) ctx->entropy_len = MBEDTLS_CTR_DRBG_ENTROPY_LEN; - ctx->reseed_interval = MBEDTLS_CTR_DRBG_RESEED_INTERVAL; /* * Initialize with an empty key diff --git a/thirdparty/mbedtls/library/ecp_curves.c b/thirdparty/mbedtls/library/ecp_curves.c index cc4c5b71c0..b04596b561 100644 --- a/thirdparty/mbedtls/library/ecp_curves.c +++ b/thirdparty/mbedtls/library/ecp_curves.c @@ -1044,7 +1044,7 @@ static inline void sub32( uint32_t *dst, uint32_t src, signed char *carry ) STORE32; i++; \ cur = c > 0 ? c : 0; STORE32; \ cur = 0; while( ++i < MAX32 ) { STORE32; } \ - if( c < 0 ) fix_negative( N, c, &C, bits ); + if( c < 0 ) MBEDTLS_MPI_CHK( fix_negative( N, c, &C, bits ) ); /* * If the result is negative, we get it in the form diff --git a/thirdparty/mbedtls/library/entropy_poll.c b/thirdparty/mbedtls/library/entropy_poll.c index 26b7e4e2b9..2095a7dd34 100644 --- a/thirdparty/mbedtls/library/entropy_poll.c +++ b/thirdparty/mbedtls/library/entropy_poll.c @@ -44,7 +44,7 @@ * ********** */ -#if defined(__linux__) +#if defined(__linux__) && !defined(_GNU_SOURCE) /* Ensure that syscall() is available even when compiling with -std=c99 */ #define _GNU_SOURCE #endif diff --git a/thirdparty/mbedtls/library/error.c b/thirdparty/mbedtls/library/error.c index eb52052b51..b83b8d1f1b 100644 --- a/thirdparty/mbedtls/library/error.c +++ b/thirdparty/mbedtls/library/error.c @@ -51,20 +51,19 @@ #endif #if defined(MBEDTLS_ERROR_C) || defined(MBEDTLS_ERROR_STRERROR_DUMMY) + #include "mbedtls/error.h" -#include <string.h> -#endif + +#if defined(MBEDTLS_ERROR_C) #if defined(MBEDTLS_PLATFORM_C) #include "mbedtls/platform.h" #else #define mbedtls_snprintf snprintf -#define mbedtls_time_t time_t #endif -#if defined(MBEDTLS_ERROR_C) - #include <stdio.h> +#include <string.h> #if defined(MBEDTLS_AES_C) #include "mbedtls/aes.h" @@ -929,8 +928,6 @@ void mbedtls_strerror( int ret, char *buf, size_t buflen ) #else /* MBEDTLS_ERROR_C */ -#if defined(MBEDTLS_ERROR_STRERROR_DUMMY) - /* * Provide an non-function in case MBEDTLS_ERROR_C is not defined */ @@ -942,6 +939,6 @@ void mbedtls_strerror( int ret, char *buf, size_t buflen ) buf[0] = '\0'; } -#endif /* MBEDTLS_ERROR_STRERROR_DUMMY */ - #endif /* MBEDTLS_ERROR_C */ + +#endif /* MBEDTLS_ERROR_C || MBEDTLS_ERROR_STRERROR_DUMMY */ diff --git a/thirdparty/mbedtls/library/hmac_drbg.c b/thirdparty/mbedtls/library/hmac_drbg.c index 9fbfc30660..10cbd462ba 100644 --- a/thirdparty/mbedtls/library/hmac_drbg.c +++ b/thirdparty/mbedtls/library/hmac_drbg.c @@ -83,6 +83,8 @@ void mbedtls_hmac_drbg_init( mbedtls_hmac_drbg_context *ctx ) { memset( ctx, 0, sizeof( mbedtls_hmac_drbg_context ) ); + ctx->reseed_interval = MBEDTLS_HMAC_DRBG_RESEED_INTERVAL; + #if defined(MBEDTLS_THREADING_C) mbedtls_mutex_init( &ctx->mutex ); #endif @@ -296,8 +298,6 @@ int mbedtls_hmac_drbg_seed( mbedtls_hmac_drbg_context *ctx, ctx->f_entropy = f_entropy; ctx->p_entropy = p_entropy; - ctx->reseed_interval = MBEDTLS_HMAC_DRBG_RESEED_INTERVAL; - if( ctx->entropy_len == 0 ) { /* @@ -442,7 +442,8 @@ int mbedtls_hmac_drbg_random( void *p_rng, unsigned char *output, size_t out_len } /* - * Free an HMAC_DRBG context + * This function resets HMAC_DRBG context to the state immediately + * after initial call of mbedtls_hmac_drbg_init(). */ void mbedtls_hmac_drbg_free( mbedtls_hmac_drbg_context *ctx ) { @@ -454,6 +455,10 @@ void mbedtls_hmac_drbg_free( mbedtls_hmac_drbg_context *ctx ) #endif mbedtls_md_free( &ctx->md_ctx ); mbedtls_platform_zeroize( ctx, sizeof( mbedtls_hmac_drbg_context ) ); + ctx->reseed_interval = MBEDTLS_HMAC_DRBG_RESEED_INTERVAL; +#if defined(MBEDTLS_THREADING_C) + mbedtls_mutex_init( &ctx->mutex ); +#endif } #if defined(MBEDTLS_FS_IO) diff --git a/thirdparty/mbedtls/library/md2.c b/thirdparty/mbedtls/library/md2.c index cbdaaabdc7..fdcb630a1f 100644 --- a/thirdparty/mbedtls/library/md2.c +++ b/thirdparty/mbedtls/library/md2.c @@ -177,6 +177,9 @@ int mbedtls_internal_md2_process( mbedtls_md2_context *ctx ) t = ctx->cksum[i]; } + /* Zeroise variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &t, sizeof( t ) ); + return( 0 ); } diff --git a/thirdparty/mbedtls/library/md4.c b/thirdparty/mbedtls/library/md4.c index cb16dce54a..95e893e654 100644 --- a/thirdparty/mbedtls/library/md4.c +++ b/thirdparty/mbedtls/library/md4.c @@ -143,31 +143,34 @@ void mbedtls_md4_starts( mbedtls_md4_context *ctx ) int mbedtls_internal_md4_process( mbedtls_md4_context *ctx, const unsigned char data[64] ) { - uint32_t X[16], A, B, C, D; - - GET_UINT32_LE( X[ 0], data, 0 ); - GET_UINT32_LE( X[ 1], data, 4 ); - GET_UINT32_LE( X[ 2], data, 8 ); - GET_UINT32_LE( X[ 3], data, 12 ); - GET_UINT32_LE( X[ 4], data, 16 ); - GET_UINT32_LE( X[ 5], data, 20 ); - GET_UINT32_LE( X[ 6], data, 24 ); - GET_UINT32_LE( X[ 7], data, 28 ); - GET_UINT32_LE( X[ 8], data, 32 ); - GET_UINT32_LE( X[ 9], data, 36 ); - GET_UINT32_LE( X[10], data, 40 ); - GET_UINT32_LE( X[11], data, 44 ); - GET_UINT32_LE( X[12], data, 48 ); - GET_UINT32_LE( X[13], data, 52 ); - GET_UINT32_LE( X[14], data, 56 ); - GET_UINT32_LE( X[15], data, 60 ); + struct + { + uint32_t X[16], A, B, C, D; + } local; + + GET_UINT32_LE( local.X[ 0], data, 0 ); + GET_UINT32_LE( local.X[ 1], data, 4 ); + GET_UINT32_LE( local.X[ 2], data, 8 ); + GET_UINT32_LE( local.X[ 3], data, 12 ); + GET_UINT32_LE( local.X[ 4], data, 16 ); + GET_UINT32_LE( local.X[ 5], data, 20 ); + GET_UINT32_LE( local.X[ 6], data, 24 ); + GET_UINT32_LE( local.X[ 7], data, 28 ); + GET_UINT32_LE( local.X[ 8], data, 32 ); + GET_UINT32_LE( local.X[ 9], data, 36 ); + GET_UINT32_LE( local.X[10], data, 40 ); + GET_UINT32_LE( local.X[11], data, 44 ); + GET_UINT32_LE( local.X[12], data, 48 ); + GET_UINT32_LE( local.X[13], data, 52 ); + GET_UINT32_LE( local.X[14], data, 56 ); + GET_UINT32_LE( local.X[15], data, 60 ); #define S(x,n) (((x) << (n)) | (((x) & 0xFFFFFFFF) >> (32 - (n)))) - A = ctx->state[0]; - B = ctx->state[1]; - C = ctx->state[2]; - D = ctx->state[3]; + local.A = ctx->state[0]; + local.B = ctx->state[1]; + local.C = ctx->state[2]; + local.D = ctx->state[3]; #define F(x, y, z) (((x) & (y)) | ((~(x)) & (z))) #define P(a,b,c,d,x,s) \ @@ -178,22 +181,22 @@ int mbedtls_internal_md4_process( mbedtls_md4_context *ctx, } while( 0 ) - P( A, B, C, D, X[ 0], 3 ); - P( D, A, B, C, X[ 1], 7 ); - P( C, D, A, B, X[ 2], 11 ); - P( B, C, D, A, X[ 3], 19 ); - P( A, B, C, D, X[ 4], 3 ); - P( D, A, B, C, X[ 5], 7 ); - P( C, D, A, B, X[ 6], 11 ); - P( B, C, D, A, X[ 7], 19 ); - P( A, B, C, D, X[ 8], 3 ); - P( D, A, B, C, X[ 9], 7 ); - P( C, D, A, B, X[10], 11 ); - P( B, C, D, A, X[11], 19 ); - P( A, B, C, D, X[12], 3 ); - P( D, A, B, C, X[13], 7 ); - P( C, D, A, B, X[14], 11 ); - P( B, C, D, A, X[15], 19 ); + P( local.A, local.B, local.C, local.D, local.X[ 0], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 1], 7 ); + P( local.C, local.D, local.A, local.B, local.X[ 2], 11 ); + P( local.B, local.C, local.D, local.A, local.X[ 3], 19 ); + P( local.A, local.B, local.C, local.D, local.X[ 4], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 5], 7 ); + P( local.C, local.D, local.A, local.B, local.X[ 6], 11 ); + P( local.B, local.C, local.D, local.A, local.X[ 7], 19 ); + P( local.A, local.B, local.C, local.D, local.X[ 8], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 9], 7 ); + P( local.C, local.D, local.A, local.B, local.X[10], 11 ); + P( local.B, local.C, local.D, local.A, local.X[11], 19 ); + P( local.A, local.B, local.C, local.D, local.X[12], 3 ); + P( local.D, local.A, local.B, local.C, local.X[13], 7 ); + P( local.C, local.D, local.A, local.B, local.X[14], 11 ); + P( local.B, local.C, local.D, local.A, local.X[15], 19 ); #undef P #undef F @@ -206,22 +209,22 @@ int mbedtls_internal_md4_process( mbedtls_md4_context *ctx, (a) = S((a),(s)); \ } while( 0 ) - P( A, B, C, D, X[ 0], 3 ); - P( D, A, B, C, X[ 4], 5 ); - P( C, D, A, B, X[ 8], 9 ); - P( B, C, D, A, X[12], 13 ); - P( A, B, C, D, X[ 1], 3 ); - P( D, A, B, C, X[ 5], 5 ); - P( C, D, A, B, X[ 9], 9 ); - P( B, C, D, A, X[13], 13 ); - P( A, B, C, D, X[ 2], 3 ); - P( D, A, B, C, X[ 6], 5 ); - P( C, D, A, B, X[10], 9 ); - P( B, C, D, A, X[14], 13 ); - P( A, B, C, D, X[ 3], 3 ); - P( D, A, B, C, X[ 7], 5 ); - P( C, D, A, B, X[11], 9 ); - P( B, C, D, A, X[15], 13 ); + P( local.A, local.B, local.C, local.D, local.X[ 0], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 4], 5 ); + P( local.C, local.D, local.A, local.B, local.X[ 8], 9 ); + P( local.B, local.C, local.D, local.A, local.X[12], 13 ); + P( local.A, local.B, local.C, local.D, local.X[ 1], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 5], 5 ); + P( local.C, local.D, local.A, local.B, local.X[ 9], 9 ); + P( local.B, local.C, local.D, local.A, local.X[13], 13 ); + P( local.A, local.B, local.C, local.D, local.X[ 2], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 6], 5 ); + P( local.C, local.D, local.A, local.B, local.X[10], 9 ); + P( local.B, local.C, local.D, local.A, local.X[14], 13 ); + P( local.A, local.B, local.C, local.D, local.X[ 3], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 7], 5 ); + P( local.C, local.D, local.A, local.B, local.X[11], 9 ); + P( local.B, local.C, local.D, local.A, local.X[15], 13 ); #undef P #undef F @@ -234,30 +237,33 @@ int mbedtls_internal_md4_process( mbedtls_md4_context *ctx, (a) = S((a),(s)); \ } while( 0 ) - P( A, B, C, D, X[ 0], 3 ); - P( D, A, B, C, X[ 8], 9 ); - P( C, D, A, B, X[ 4], 11 ); - P( B, C, D, A, X[12], 15 ); - P( A, B, C, D, X[ 2], 3 ); - P( D, A, B, C, X[10], 9 ); - P( C, D, A, B, X[ 6], 11 ); - P( B, C, D, A, X[14], 15 ); - P( A, B, C, D, X[ 1], 3 ); - P( D, A, B, C, X[ 9], 9 ); - P( C, D, A, B, X[ 5], 11 ); - P( B, C, D, A, X[13], 15 ); - P( A, B, C, D, X[ 3], 3 ); - P( D, A, B, C, X[11], 9 ); - P( C, D, A, B, X[ 7], 11 ); - P( B, C, D, A, X[15], 15 ); + P( local.A, local.B, local.C, local.D, local.X[ 0], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 8], 9 ); + P( local.C, local.D, local.A, local.B, local.X[ 4], 11 ); + P( local.B, local.C, local.D, local.A, local.X[12], 15 ); + P( local.A, local.B, local.C, local.D, local.X[ 2], 3 ); + P( local.D, local.A, local.B, local.C, local.X[10], 9 ); + P( local.C, local.D, local.A, local.B, local.X[ 6], 11 ); + P( local.B, local.C, local.D, local.A, local.X[14], 15 ); + P( local.A, local.B, local.C, local.D, local.X[ 1], 3 ); + P( local.D, local.A, local.B, local.C, local.X[ 9], 9 ); + P( local.C, local.D, local.A, local.B, local.X[ 5], 11 ); + P( local.B, local.C, local.D, local.A, local.X[13], 15 ); + P( local.A, local.B, local.C, local.D, local.X[ 3], 3 ); + P( local.D, local.A, local.B, local.C, local.X[11], 9 ); + P( local.C, local.D, local.A, local.B, local.X[ 7], 11 ); + P( local.B, local.C, local.D, local.A, local.X[15], 15 ); #undef F #undef P - ctx->state[0] += A; - ctx->state[1] += B; - ctx->state[2] += C; - ctx->state[3] += D; + ctx->state[0] += local.A; + ctx->state[1] += local.B; + ctx->state[2] += local.C; + ctx->state[3] += local.D; + + /* Zeroise variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/md5.c b/thirdparty/mbedtls/library/md5.c index fe25925214..d2b634fbb1 100644 --- a/thirdparty/mbedtls/library/md5.c +++ b/thirdparty/mbedtls/library/md5.c @@ -142,128 +142,134 @@ void mbedtls_md5_starts( mbedtls_md5_context *ctx ) int mbedtls_internal_md5_process( mbedtls_md5_context *ctx, const unsigned char data[64] ) { - uint32_t X[16], A, B, C, D; - - GET_UINT32_LE( X[ 0], data, 0 ); - GET_UINT32_LE( X[ 1], data, 4 ); - GET_UINT32_LE( X[ 2], data, 8 ); - GET_UINT32_LE( X[ 3], data, 12 ); - GET_UINT32_LE( X[ 4], data, 16 ); - GET_UINT32_LE( X[ 5], data, 20 ); - GET_UINT32_LE( X[ 6], data, 24 ); - GET_UINT32_LE( X[ 7], data, 28 ); - GET_UINT32_LE( X[ 8], data, 32 ); - GET_UINT32_LE( X[ 9], data, 36 ); - GET_UINT32_LE( X[10], data, 40 ); - GET_UINT32_LE( X[11], data, 44 ); - GET_UINT32_LE( X[12], data, 48 ); - GET_UINT32_LE( X[13], data, 52 ); - GET_UINT32_LE( X[14], data, 56 ); - GET_UINT32_LE( X[15], data, 60 ); + struct + { + uint32_t X[16], A, B, C, D; + } local; + + GET_UINT32_LE( local.X[ 0], data, 0 ); + GET_UINT32_LE( local.X[ 1], data, 4 ); + GET_UINT32_LE( local.X[ 2], data, 8 ); + GET_UINT32_LE( local.X[ 3], data, 12 ); + GET_UINT32_LE( local.X[ 4], data, 16 ); + GET_UINT32_LE( local.X[ 5], data, 20 ); + GET_UINT32_LE( local.X[ 6], data, 24 ); + GET_UINT32_LE( local.X[ 7], data, 28 ); + GET_UINT32_LE( local.X[ 8], data, 32 ); + GET_UINT32_LE( local.X[ 9], data, 36 ); + GET_UINT32_LE( local.X[10], data, 40 ); + GET_UINT32_LE( local.X[11], data, 44 ); + GET_UINT32_LE( local.X[12], data, 48 ); + GET_UINT32_LE( local.X[13], data, 52 ); + GET_UINT32_LE( local.X[14], data, 56 ); + GET_UINT32_LE( local.X[15], data, 60 ); #define S(x,n) \ ( ( (x) << (n) ) | ( ( (x) & 0xFFFFFFFF) >> ( 32 - (n) ) ) ) -#define P(a,b,c,d,k,s,t) \ - do \ - { \ - (a) += F((b),(c),(d)) + X[(k)] + (t); \ - (a) = S((a),(s)) + (b); \ +#define P(a,b,c,d,k,s,t) \ + do \ + { \ + (a) += F((b),(c),(d)) + local.X[(k)] + (t); \ + (a) = S((a),(s)) + (b); \ } while( 0 ) - A = ctx->state[0]; - B = ctx->state[1]; - C = ctx->state[2]; - D = ctx->state[3]; + local.A = ctx->state[0]; + local.B = ctx->state[1]; + local.C = ctx->state[2]; + local.D = ctx->state[3]; #define F(x,y,z) ((z) ^ ((x) & ((y) ^ (z)))) - P( A, B, C, D, 0, 7, 0xD76AA478 ); - P( D, A, B, C, 1, 12, 0xE8C7B756 ); - P( C, D, A, B, 2, 17, 0x242070DB ); - P( B, C, D, A, 3, 22, 0xC1BDCEEE ); - P( A, B, C, D, 4, 7, 0xF57C0FAF ); - P( D, A, B, C, 5, 12, 0x4787C62A ); - P( C, D, A, B, 6, 17, 0xA8304613 ); - P( B, C, D, A, 7, 22, 0xFD469501 ); - P( A, B, C, D, 8, 7, 0x698098D8 ); - P( D, A, B, C, 9, 12, 0x8B44F7AF ); - P( C, D, A, B, 10, 17, 0xFFFF5BB1 ); - P( B, C, D, A, 11, 22, 0x895CD7BE ); - P( A, B, C, D, 12, 7, 0x6B901122 ); - P( D, A, B, C, 13, 12, 0xFD987193 ); - P( C, D, A, B, 14, 17, 0xA679438E ); - P( B, C, D, A, 15, 22, 0x49B40821 ); + P( local.A, local.B, local.C, local.D, 0, 7, 0xD76AA478 ); + P( local.D, local.A, local.B, local.C, 1, 12, 0xE8C7B756 ); + P( local.C, local.D, local.A, local.B, 2, 17, 0x242070DB ); + P( local.B, local.C, local.D, local.A, 3, 22, 0xC1BDCEEE ); + P( local.A, local.B, local.C, local.D, 4, 7, 0xF57C0FAF ); + P( local.D, local.A, local.B, local.C, 5, 12, 0x4787C62A ); + P( local.C, local.D, local.A, local.B, 6, 17, 0xA8304613 ); + P( local.B, local.C, local.D, local.A, 7, 22, 0xFD469501 ); + P( local.A, local.B, local.C, local.D, 8, 7, 0x698098D8 ); + P( local.D, local.A, local.B, local.C, 9, 12, 0x8B44F7AF ); + P( local.C, local.D, local.A, local.B, 10, 17, 0xFFFF5BB1 ); + P( local.B, local.C, local.D, local.A, 11, 22, 0x895CD7BE ); + P( local.A, local.B, local.C, local.D, 12, 7, 0x6B901122 ); + P( local.D, local.A, local.B, local.C, 13, 12, 0xFD987193 ); + P( local.C, local.D, local.A, local.B, 14, 17, 0xA679438E ); + P( local.B, local.C, local.D, local.A, 15, 22, 0x49B40821 ); #undef F #define F(x,y,z) ((y) ^ ((z) & ((x) ^ (y)))) - P( A, B, C, D, 1, 5, 0xF61E2562 ); - P( D, A, B, C, 6, 9, 0xC040B340 ); - P( C, D, A, B, 11, 14, 0x265E5A51 ); - P( B, C, D, A, 0, 20, 0xE9B6C7AA ); - P( A, B, C, D, 5, 5, 0xD62F105D ); - P( D, A, B, C, 10, 9, 0x02441453 ); - P( C, D, A, B, 15, 14, 0xD8A1E681 ); - P( B, C, D, A, 4, 20, 0xE7D3FBC8 ); - P( A, B, C, D, 9, 5, 0x21E1CDE6 ); - P( D, A, B, C, 14, 9, 0xC33707D6 ); - P( C, D, A, B, 3, 14, 0xF4D50D87 ); - P( B, C, D, A, 8, 20, 0x455A14ED ); - P( A, B, C, D, 13, 5, 0xA9E3E905 ); - P( D, A, B, C, 2, 9, 0xFCEFA3F8 ); - P( C, D, A, B, 7, 14, 0x676F02D9 ); - P( B, C, D, A, 12, 20, 0x8D2A4C8A ); + P( local.A, local.B, local.C, local.D, 1, 5, 0xF61E2562 ); + P( local.D, local.A, local.B, local.C, 6, 9, 0xC040B340 ); + P( local.C, local.D, local.A, local.B, 11, 14, 0x265E5A51 ); + P( local.B, local.C, local.D, local.A, 0, 20, 0xE9B6C7AA ); + P( local.A, local.B, local.C, local.D, 5, 5, 0xD62F105D ); + P( local.D, local.A, local.B, local.C, 10, 9, 0x02441453 ); + P( local.C, local.D, local.A, local.B, 15, 14, 0xD8A1E681 ); + P( local.B, local.C, local.D, local.A, 4, 20, 0xE7D3FBC8 ); + P( local.A, local.B, local.C, local.D, 9, 5, 0x21E1CDE6 ); + P( local.D, local.A, local.B, local.C, 14, 9, 0xC33707D6 ); + P( local.C, local.D, local.A, local.B, 3, 14, 0xF4D50D87 ); + P( local.B, local.C, local.D, local.A, 8, 20, 0x455A14ED ); + P( local.A, local.B, local.C, local.D, 13, 5, 0xA9E3E905 ); + P( local.D, local.A, local.B, local.C, 2, 9, 0xFCEFA3F8 ); + P( local.C, local.D, local.A, local.B, 7, 14, 0x676F02D9 ); + P( local.B, local.C, local.D, local.A, 12, 20, 0x8D2A4C8A ); #undef F #define F(x,y,z) ((x) ^ (y) ^ (z)) - P( A, B, C, D, 5, 4, 0xFFFA3942 ); - P( D, A, B, C, 8, 11, 0x8771F681 ); - P( C, D, A, B, 11, 16, 0x6D9D6122 ); - P( B, C, D, A, 14, 23, 0xFDE5380C ); - P( A, B, C, D, 1, 4, 0xA4BEEA44 ); - P( D, A, B, C, 4, 11, 0x4BDECFA9 ); - P( C, D, A, B, 7, 16, 0xF6BB4B60 ); - P( B, C, D, A, 10, 23, 0xBEBFBC70 ); - P( A, B, C, D, 13, 4, 0x289B7EC6 ); - P( D, A, B, C, 0, 11, 0xEAA127FA ); - P( C, D, A, B, 3, 16, 0xD4EF3085 ); - P( B, C, D, A, 6, 23, 0x04881D05 ); - P( A, B, C, D, 9, 4, 0xD9D4D039 ); - P( D, A, B, C, 12, 11, 0xE6DB99E5 ); - P( C, D, A, B, 15, 16, 0x1FA27CF8 ); - P( B, C, D, A, 2, 23, 0xC4AC5665 ); + P( local.A, local.B, local.C, local.D, 5, 4, 0xFFFA3942 ); + P( local.D, local.A, local.B, local.C, 8, 11, 0x8771F681 ); + P( local.C, local.D, local.A, local.B, 11, 16, 0x6D9D6122 ); + P( local.B, local.C, local.D, local.A, 14, 23, 0xFDE5380C ); + P( local.A, local.B, local.C, local.D, 1, 4, 0xA4BEEA44 ); + P( local.D, local.A, local.B, local.C, 4, 11, 0x4BDECFA9 ); + P( local.C, local.D, local.A, local.B, 7, 16, 0xF6BB4B60 ); + P( local.B, local.C, local.D, local.A, 10, 23, 0xBEBFBC70 ); + P( local.A, local.B, local.C, local.D, 13, 4, 0x289B7EC6 ); + P( local.D, local.A, local.B, local.C, 0, 11, 0xEAA127FA ); + P( local.C, local.D, local.A, local.B, 3, 16, 0xD4EF3085 ); + P( local.B, local.C, local.D, local.A, 6, 23, 0x04881D05 ); + P( local.A, local.B, local.C, local.D, 9, 4, 0xD9D4D039 ); + P( local.D, local.A, local.B, local.C, 12, 11, 0xE6DB99E5 ); + P( local.C, local.D, local.A, local.B, 15, 16, 0x1FA27CF8 ); + P( local.B, local.C, local.D, local.A, 2, 23, 0xC4AC5665 ); #undef F #define F(x,y,z) ((y) ^ ((x) | ~(z))) - P( A, B, C, D, 0, 6, 0xF4292244 ); - P( D, A, B, C, 7, 10, 0x432AFF97 ); - P( C, D, A, B, 14, 15, 0xAB9423A7 ); - P( B, C, D, A, 5, 21, 0xFC93A039 ); - P( A, B, C, D, 12, 6, 0x655B59C3 ); - P( D, A, B, C, 3, 10, 0x8F0CCC92 ); - P( C, D, A, B, 10, 15, 0xFFEFF47D ); - P( B, C, D, A, 1, 21, 0x85845DD1 ); - P( A, B, C, D, 8, 6, 0x6FA87E4F ); - P( D, A, B, C, 15, 10, 0xFE2CE6E0 ); - P( C, D, A, B, 6, 15, 0xA3014314 ); - P( B, C, D, A, 13, 21, 0x4E0811A1 ); - P( A, B, C, D, 4, 6, 0xF7537E82 ); - P( D, A, B, C, 11, 10, 0xBD3AF235 ); - P( C, D, A, B, 2, 15, 0x2AD7D2BB ); - P( B, C, D, A, 9, 21, 0xEB86D391 ); + P( local.A, local.B, local.C, local.D, 0, 6, 0xF4292244 ); + P( local.D, local.A, local.B, local.C, 7, 10, 0x432AFF97 ); + P( local.C, local.D, local.A, local.B, 14, 15, 0xAB9423A7 ); + P( local.B, local.C, local.D, local.A, 5, 21, 0xFC93A039 ); + P( local.A, local.B, local.C, local.D, 12, 6, 0x655B59C3 ); + P( local.D, local.A, local.B, local.C, 3, 10, 0x8F0CCC92 ); + P( local.C, local.D, local.A, local.B, 10, 15, 0xFFEFF47D ); + P( local.B, local.C, local.D, local.A, 1, 21, 0x85845DD1 ); + P( local.A, local.B, local.C, local.D, 8, 6, 0x6FA87E4F ); + P( local.D, local.A, local.B, local.C, 15, 10, 0xFE2CE6E0 ); + P( local.C, local.D, local.A, local.B, 6, 15, 0xA3014314 ); + P( local.B, local.C, local.D, local.A, 13, 21, 0x4E0811A1 ); + P( local.A, local.B, local.C, local.D, 4, 6, 0xF7537E82 ); + P( local.D, local.A, local.B, local.C, 11, 10, 0xBD3AF235 ); + P( local.C, local.D, local.A, local.B, 2, 15, 0x2AD7D2BB ); + P( local.B, local.C, local.D, local.A, 9, 21, 0xEB86D391 ); #undef F - ctx->state[0] += A; - ctx->state[1] += B; - ctx->state[2] += C; - ctx->state[3] += D; + ctx->state[0] += local.A; + ctx->state[1] += local.B; + ctx->state[2] += local.C; + ctx->state[3] += local.D; + + /* Zeroise variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/pem.c b/thirdparty/mbedtls/library/pem.c index a7a2f7f5cf..50e663ccdb 100644 --- a/thirdparty/mbedtls/library/pem.c +++ b/thirdparty/mbedtls/library/pem.c @@ -508,8 +508,12 @@ int mbedtls_pem_write_buffer( const char *header, const char *footer, *p++ = '\0'; *olen = p - buf; + /* Clean any remaining data previously written to the buffer */ + memset( buf + *olen, 0, buf_len - *olen ); + mbedtls_free( encode_buf ); return( 0 ); } #endif /* MBEDTLS_PEM_WRITE_C */ #endif /* MBEDTLS_PEM_PARSE_C || MBEDTLS_PEM_WRITE_C */ + diff --git a/thirdparty/mbedtls/library/pkcs5.c b/thirdparty/mbedtls/library/pkcs5.c index 8a80aa5d05..c4447f1546 100644 --- a/thirdparty/mbedtls/library/pkcs5.c +++ b/thirdparty/mbedtls/library/pkcs5.c @@ -247,7 +247,7 @@ int mbedtls_pkcs5_pbkdf2_hmac( mbedtls_md_context_t *ctx, const unsigned char *p unsigned int iteration_count, uint32_t key_length, unsigned char *output ) { - int ret, j; + int ret = 0, j; unsigned int i; unsigned char md1[MBEDTLS_MD_MAX_SIZE]; unsigned char work[MBEDTLS_MD_MAX_SIZE]; @@ -269,16 +269,16 @@ int mbedtls_pkcs5_pbkdf2_hmac( mbedtls_md_context_t *ctx, const unsigned char *p // U1 ends up in work // if( ( ret = mbedtls_md_hmac_starts( ctx, password, plen ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_update( ctx, salt, slen ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_update( ctx, counter, 4 ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_finish( ctx, work ) ) != 0 ) - return( ret ); + goto cleanup; memcpy( md1, work, md_size ); @@ -287,13 +287,13 @@ int mbedtls_pkcs5_pbkdf2_hmac( mbedtls_md_context_t *ctx, const unsigned char *p // U2 ends up in md1 // if( ( ret = mbedtls_md_hmac_starts( ctx, password, plen ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_update( ctx, md1, md_size ) ) != 0 ) - return( ret ); + goto cleanup; if( ( ret = mbedtls_md_hmac_finish( ctx, md1 ) ) != 0 ) - return( ret ); + goto cleanup; // U1 xor U2 // @@ -312,7 +312,12 @@ int mbedtls_pkcs5_pbkdf2_hmac( mbedtls_md_context_t *ctx, const unsigned char *p break; } - return( 0 ); +cleanup: + /* Zeroise buffers to clear sensitive data from memory. */ + mbedtls_platform_zeroize( work, MBEDTLS_MD_MAX_SIZE ); + mbedtls_platform_zeroize( md1, MBEDTLS_MD_MAX_SIZE ); + + return( ret ); } #if defined(MBEDTLS_SELF_TEST) diff --git a/thirdparty/mbedtls/library/pkparse.c b/thirdparty/mbedtls/library/pkparse.c index 086807d836..e410f3aae1 100644 --- a/thirdparty/mbedtls/library/pkparse.c +++ b/thirdparty/mbedtls/library/pkparse.c @@ -692,7 +692,7 @@ int mbedtls_pk_parse_subpubkey( unsigned char **p, const unsigned char *end, ret = MBEDTLS_ERR_PK_UNKNOWN_PK_ALG; if( ret == 0 && *p != end ) - ret = MBEDTLS_ERR_PK_INVALID_PUBKEY + ret = MBEDTLS_ERR_PK_INVALID_PUBKEY + MBEDTLS_ERR_ASN1_LENGTH_MISMATCH; if( ret != 0 ) diff --git a/thirdparty/mbedtls/library/platform_util.c b/thirdparty/mbedtls/library/platform_util.c index 3ba2aead12..c8cd52d52a 100644 --- a/thirdparty/mbedtls/library/platform_util.c +++ b/thirdparty/mbedtls/library/platform_util.c @@ -115,7 +115,7 @@ void mbedtls_platform_zeroize( void *buf, size_t len ) #if !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) || \ ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) && \ - _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) + _POSIX_THREAD_SAFE_FUNCTIONS >= 200112L ) ) /* * This is a convenience shorthand macro to avoid checking the long * preprocessor conditions above. Ideally, we could expose this macro in @@ -129,7 +129,7 @@ void mbedtls_platform_zeroize( void *buf, size_t len ) #endif /* !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) || \ ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) && \ - _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) */ + _POSIX_THREAD_SAFE_FUNCTIONS >= 200112L ) ) */ struct tm *mbedtls_platform_gmtime_r( const mbedtls_time_t *tt, struct tm *tm_buf ) diff --git a/thirdparty/mbedtls/library/ripemd160.c b/thirdparty/mbedtls/library/ripemd160.c index 0b6efcb574..d6ee933b2e 100644 --- a/thirdparty/mbedtls/library/ripemd160.c +++ b/thirdparty/mbedtls/library/ripemd160.c @@ -147,30 +147,33 @@ void mbedtls_ripemd160_starts( mbedtls_ripemd160_context *ctx ) int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, const unsigned char data[64] ) { - uint32_t A, B, C, D, E, Ap, Bp, Cp, Dp, Ep, X[16]; - - GET_UINT32_LE( X[ 0], data, 0 ); - GET_UINT32_LE( X[ 1], data, 4 ); - GET_UINT32_LE( X[ 2], data, 8 ); - GET_UINT32_LE( X[ 3], data, 12 ); - GET_UINT32_LE( X[ 4], data, 16 ); - GET_UINT32_LE( X[ 5], data, 20 ); - GET_UINT32_LE( X[ 6], data, 24 ); - GET_UINT32_LE( X[ 7], data, 28 ); - GET_UINT32_LE( X[ 8], data, 32 ); - GET_UINT32_LE( X[ 9], data, 36 ); - GET_UINT32_LE( X[10], data, 40 ); - GET_UINT32_LE( X[11], data, 44 ); - GET_UINT32_LE( X[12], data, 48 ); - GET_UINT32_LE( X[13], data, 52 ); - GET_UINT32_LE( X[14], data, 56 ); - GET_UINT32_LE( X[15], data, 60 ); - - A = Ap = ctx->state[0]; - B = Bp = ctx->state[1]; - C = Cp = ctx->state[2]; - D = Dp = ctx->state[3]; - E = Ep = ctx->state[4]; + struct + { + uint32_t A, B, C, D, E, Ap, Bp, Cp, Dp, Ep, X[16]; + } local; + + GET_UINT32_LE( local.X[ 0], data, 0 ); + GET_UINT32_LE( local.X[ 1], data, 4 ); + GET_UINT32_LE( local.X[ 2], data, 8 ); + GET_UINT32_LE( local.X[ 3], data, 12 ); + GET_UINT32_LE( local.X[ 4], data, 16 ); + GET_UINT32_LE( local.X[ 5], data, 20 ); + GET_UINT32_LE( local.X[ 6], data, 24 ); + GET_UINT32_LE( local.X[ 7], data, 28 ); + GET_UINT32_LE( local.X[ 8], data, 32 ); + GET_UINT32_LE( local.X[ 9], data, 36 ); + GET_UINT32_LE( local.X[10], data, 40 ); + GET_UINT32_LE( local.X[11], data, 44 ); + GET_UINT32_LE( local.X[12], data, 48 ); + GET_UINT32_LE( local.X[13], data, 52 ); + GET_UINT32_LE( local.X[14], data, 56 ); + GET_UINT32_LE( local.X[15], data, 60 ); + + local.A = local.Ap = ctx->state[0]; + local.B = local.Bp = ctx->state[1]; + local.C = local.Cp = ctx->state[2]; + local.D = local.Dp = ctx->state[3]; + local.E = local.Ep = ctx->state[4]; #define F1( x, y, z ) ( (x) ^ (y) ^ (z) ) #define F2( x, y, z ) ( ( (x) & (y) ) | ( ~(x) & (z) ) ) @@ -180,12 +183,12 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define S( x, n ) ( ( (x) << (n) ) | ( (x) >> (32 - (n)) ) ) -#define P( a, b, c, d, e, r, s, f, k ) \ - do \ - { \ - (a) += f( (b), (c), (d) ) + X[r] + (k); \ - (a) = S( (a), (s) ) + (e); \ - (c) = S( (c), 10 ); \ +#define P( a, b, c, d, e, r, s, f, k ) \ + do \ + { \ + (a) += f( (b), (c), (d) ) + local.X[r] + (k); \ + (a) = S( (a), (s) ) + (e); \ + (c) = S( (c), 10 ); \ } while( 0 ) #define P2( a, b, c, d, e, r, s, rp, sp ) \ @@ -200,22 +203,22 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0x00000000 #define Fp F5 #define Kp 0x50A28BE6 - P2( A, B, C, D, E, 0, 11, 5, 8 ); - P2( E, A, B, C, D, 1, 14, 14, 9 ); - P2( D, E, A, B, C, 2, 15, 7, 9 ); - P2( C, D, E, A, B, 3, 12, 0, 11 ); - P2( B, C, D, E, A, 4, 5, 9, 13 ); - P2( A, B, C, D, E, 5, 8, 2, 15 ); - P2( E, A, B, C, D, 6, 7, 11, 15 ); - P2( D, E, A, B, C, 7, 9, 4, 5 ); - P2( C, D, E, A, B, 8, 11, 13, 7 ); - P2( B, C, D, E, A, 9, 13, 6, 7 ); - P2( A, B, C, D, E, 10, 14, 15, 8 ); - P2( E, A, B, C, D, 11, 15, 8, 11 ); - P2( D, E, A, B, C, 12, 6, 1, 14 ); - P2( C, D, E, A, B, 13, 7, 10, 14 ); - P2( B, C, D, E, A, 14, 9, 3, 12 ); - P2( A, B, C, D, E, 15, 8, 12, 6 ); + P2( local.A, local.B, local.C, local.D, local.E, 0, 11, 5, 8 ); + P2( local.E, local.A, local.B, local.C, local.D, 1, 14, 14, 9 ); + P2( local.D, local.E, local.A, local.B, local.C, 2, 15, 7, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 3, 12, 0, 11 ); + P2( local.B, local.C, local.D, local.E, local.A, 4, 5, 9, 13 ); + P2( local.A, local.B, local.C, local.D, local.E, 5, 8, 2, 15 ); + P2( local.E, local.A, local.B, local.C, local.D, 6, 7, 11, 15 ); + P2( local.D, local.E, local.A, local.B, local.C, 7, 9, 4, 5 ); + P2( local.C, local.D, local.E, local.A, local.B, 8, 11, 13, 7 ); + P2( local.B, local.C, local.D, local.E, local.A, 9, 13, 6, 7 ); + P2( local.A, local.B, local.C, local.D, local.E, 10, 14, 15, 8 ); + P2( local.E, local.A, local.B, local.C, local.D, 11, 15, 8, 11 ); + P2( local.D, local.E, local.A, local.B, local.C, 12, 6, 1, 14 ); + P2( local.C, local.D, local.E, local.A, local.B, 13, 7, 10, 14 ); + P2( local.B, local.C, local.D, local.E, local.A, 14, 9, 3, 12 ); + P2( local.A, local.B, local.C, local.D, local.E, 15, 8, 12, 6 ); #undef F #undef K #undef Fp @@ -225,22 +228,22 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0x5A827999 #define Fp F4 #define Kp 0x5C4DD124 - P2( E, A, B, C, D, 7, 7, 6, 9 ); - P2( D, E, A, B, C, 4, 6, 11, 13 ); - P2( C, D, E, A, B, 13, 8, 3, 15 ); - P2( B, C, D, E, A, 1, 13, 7, 7 ); - P2( A, B, C, D, E, 10, 11, 0, 12 ); - P2( E, A, B, C, D, 6, 9, 13, 8 ); - P2( D, E, A, B, C, 15, 7, 5, 9 ); - P2( C, D, E, A, B, 3, 15, 10, 11 ); - P2( B, C, D, E, A, 12, 7, 14, 7 ); - P2( A, B, C, D, E, 0, 12, 15, 7 ); - P2( E, A, B, C, D, 9, 15, 8, 12 ); - P2( D, E, A, B, C, 5, 9, 12, 7 ); - P2( C, D, E, A, B, 2, 11, 4, 6 ); - P2( B, C, D, E, A, 14, 7, 9, 15 ); - P2( A, B, C, D, E, 11, 13, 1, 13 ); - P2( E, A, B, C, D, 8, 12, 2, 11 ); + P2( local.E, local.A, local.B, local.C, local.D, 7, 7, 6, 9 ); + P2( local.D, local.E, local.A, local.B, local.C, 4, 6, 11, 13 ); + P2( local.C, local.D, local.E, local.A, local.B, 13, 8, 3, 15 ); + P2( local.B, local.C, local.D, local.E, local.A, 1, 13, 7, 7 ); + P2( local.A, local.B, local.C, local.D, local.E, 10, 11, 0, 12 ); + P2( local.E, local.A, local.B, local.C, local.D, 6, 9, 13, 8 ); + P2( local.D, local.E, local.A, local.B, local.C, 15, 7, 5, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 3, 15, 10, 11 ); + P2( local.B, local.C, local.D, local.E, local.A, 12, 7, 14, 7 ); + P2( local.A, local.B, local.C, local.D, local.E, 0, 12, 15, 7 ); + P2( local.E, local.A, local.B, local.C, local.D, 9, 15, 8, 12 ); + P2( local.D, local.E, local.A, local.B, local.C, 5, 9, 12, 7 ); + P2( local.C, local.D, local.E, local.A, local.B, 2, 11, 4, 6 ); + P2( local.B, local.C, local.D, local.E, local.A, 14, 7, 9, 15 ); + P2( local.A, local.B, local.C, local.D, local.E, 11, 13, 1, 13 ); + P2( local.E, local.A, local.B, local.C, local.D, 8, 12, 2, 11 ); #undef F #undef K #undef Fp @@ -250,22 +253,22 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0x6ED9EBA1 #define Fp F3 #define Kp 0x6D703EF3 - P2( D, E, A, B, C, 3, 11, 15, 9 ); - P2( C, D, E, A, B, 10, 13, 5, 7 ); - P2( B, C, D, E, A, 14, 6, 1, 15 ); - P2( A, B, C, D, E, 4, 7, 3, 11 ); - P2( E, A, B, C, D, 9, 14, 7, 8 ); - P2( D, E, A, B, C, 15, 9, 14, 6 ); - P2( C, D, E, A, B, 8, 13, 6, 6 ); - P2( B, C, D, E, A, 1, 15, 9, 14 ); - P2( A, B, C, D, E, 2, 14, 11, 12 ); - P2( E, A, B, C, D, 7, 8, 8, 13 ); - P2( D, E, A, B, C, 0, 13, 12, 5 ); - P2( C, D, E, A, B, 6, 6, 2, 14 ); - P2( B, C, D, E, A, 13, 5, 10, 13 ); - P2( A, B, C, D, E, 11, 12, 0, 13 ); - P2( E, A, B, C, D, 5, 7, 4, 7 ); - P2( D, E, A, B, C, 12, 5, 13, 5 ); + P2( local.D, local.E, local.A, local.B, local.C, 3, 11, 15, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 10, 13, 5, 7 ); + P2( local.B, local.C, local.D, local.E, local.A, 14, 6, 1, 15 ); + P2( local.A, local.B, local.C, local.D, local.E, 4, 7, 3, 11 ); + P2( local.E, local.A, local.B, local.C, local.D, 9, 14, 7, 8 ); + P2( local.D, local.E, local.A, local.B, local.C, 15, 9, 14, 6 ); + P2( local.C, local.D, local.E, local.A, local.B, 8, 13, 6, 6 ); + P2( local.B, local.C, local.D, local.E, local.A, 1, 15, 9, 14 ); + P2( local.A, local.B, local.C, local.D, local.E, 2, 14, 11, 12 ); + P2( local.E, local.A, local.B, local.C, local.D, 7, 8, 8, 13 ); + P2( local.D, local.E, local.A, local.B, local.C, 0, 13, 12, 5 ); + P2( local.C, local.D, local.E, local.A, local.B, 6, 6, 2, 14 ); + P2( local.B, local.C, local.D, local.E, local.A, 13, 5, 10, 13 ); + P2( local.A, local.B, local.C, local.D, local.E, 11, 12, 0, 13 ); + P2( local.E, local.A, local.B, local.C, local.D, 5, 7, 4, 7 ); + P2( local.D, local.E, local.A, local.B, local.C, 12, 5, 13, 5 ); #undef F #undef K #undef Fp @@ -275,22 +278,22 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0x8F1BBCDC #define Fp F2 #define Kp 0x7A6D76E9 - P2( C, D, E, A, B, 1, 11, 8, 15 ); - P2( B, C, D, E, A, 9, 12, 6, 5 ); - P2( A, B, C, D, E, 11, 14, 4, 8 ); - P2( E, A, B, C, D, 10, 15, 1, 11 ); - P2( D, E, A, B, C, 0, 14, 3, 14 ); - P2( C, D, E, A, B, 8, 15, 11, 14 ); - P2( B, C, D, E, A, 12, 9, 15, 6 ); - P2( A, B, C, D, E, 4, 8, 0, 14 ); - P2( E, A, B, C, D, 13, 9, 5, 6 ); - P2( D, E, A, B, C, 3, 14, 12, 9 ); - P2( C, D, E, A, B, 7, 5, 2, 12 ); - P2( B, C, D, E, A, 15, 6, 13, 9 ); - P2( A, B, C, D, E, 14, 8, 9, 12 ); - P2( E, A, B, C, D, 5, 6, 7, 5 ); - P2( D, E, A, B, C, 6, 5, 10, 15 ); - P2( C, D, E, A, B, 2, 12, 14, 8 ); + P2( local.C, local.D, local.E, local.A, local.B, 1, 11, 8, 15 ); + P2( local.B, local.C, local.D, local.E, local.A, 9, 12, 6, 5 ); + P2( local.A, local.B, local.C, local.D, local.E, 11, 14, 4, 8 ); + P2( local.E, local.A, local.B, local.C, local.D, 10, 15, 1, 11 ); + P2( local.D, local.E, local.A, local.B, local.C, 0, 14, 3, 14 ); + P2( local.C, local.D, local.E, local.A, local.B, 8, 15, 11, 14 ); + P2( local.B, local.C, local.D, local.E, local.A, 12, 9, 15, 6 ); + P2( local.A, local.B, local.C, local.D, local.E, 4, 8, 0, 14 ); + P2( local.E, local.A, local.B, local.C, local.D, 13, 9, 5, 6 ); + P2( local.D, local.E, local.A, local.B, local.C, 3, 14, 12, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 7, 5, 2, 12 ); + P2( local.B, local.C, local.D, local.E, local.A, 15, 6, 13, 9 ); + P2( local.A, local.B, local.C, local.D, local.E, 14, 8, 9, 12 ); + P2( local.E, local.A, local.B, local.C, local.D, 5, 6, 7, 5 ); + P2( local.D, local.E, local.A, local.B, local.C, 6, 5, 10, 15 ); + P2( local.C, local.D, local.E, local.A, local.B, 2, 12, 14, 8 ); #undef F #undef K #undef Fp @@ -300,33 +303,36 @@ int mbedtls_internal_ripemd160_process( mbedtls_ripemd160_context *ctx, #define K 0xA953FD4E #define Fp F1 #define Kp 0x00000000 - P2( B, C, D, E, A, 4, 9, 12, 8 ); - P2( A, B, C, D, E, 0, 15, 15, 5 ); - P2( E, A, B, C, D, 5, 5, 10, 12 ); - P2( D, E, A, B, C, 9, 11, 4, 9 ); - P2( C, D, E, A, B, 7, 6, 1, 12 ); - P2( B, C, D, E, A, 12, 8, 5, 5 ); - P2( A, B, C, D, E, 2, 13, 8, 14 ); - P2( E, A, B, C, D, 10, 12, 7, 6 ); - P2( D, E, A, B, C, 14, 5, 6, 8 ); - P2( C, D, E, A, B, 1, 12, 2, 13 ); - P2( B, C, D, E, A, 3, 13, 13, 6 ); - P2( A, B, C, D, E, 8, 14, 14, 5 ); - P2( E, A, B, C, D, 11, 11, 0, 15 ); - P2( D, E, A, B, C, 6, 8, 3, 13 ); - P2( C, D, E, A, B, 15, 5, 9, 11 ); - P2( B, C, D, E, A, 13, 6, 11, 11 ); + P2( local.B, local.C, local.D, local.E, local.A, 4, 9, 12, 8 ); + P2( local.A, local.B, local.C, local.D, local.E, 0, 15, 15, 5 ); + P2( local.E, local.A, local.B, local.C, local.D, 5, 5, 10, 12 ); + P2( local.D, local.E, local.A, local.B, local.C, 9, 11, 4, 9 ); + P2( local.C, local.D, local.E, local.A, local.B, 7, 6, 1, 12 ); + P2( local.B, local.C, local.D, local.E, local.A, 12, 8, 5, 5 ); + P2( local.A, local.B, local.C, local.D, local.E, 2, 13, 8, 14 ); + P2( local.E, local.A, local.B, local.C, local.D, 10, 12, 7, 6 ); + P2( local.D, local.E, local.A, local.B, local.C, 14, 5, 6, 8 ); + P2( local.C, local.D, local.E, local.A, local.B, 1, 12, 2, 13 ); + P2( local.B, local.C, local.D, local.E, local.A, 3, 13, 13, 6 ); + P2( local.A, local.B, local.C, local.D, local.E, 8, 14, 14, 5 ); + P2( local.E, local.A, local.B, local.C, local.D, 11, 11, 0, 15 ); + P2( local.D, local.E, local.A, local.B, local.C, 6, 8, 3, 13 ); + P2( local.C, local.D, local.E, local.A, local.B, 15, 5, 9, 11 ); + P2( local.B, local.C, local.D, local.E, local.A, 13, 6, 11, 11 ); #undef F #undef K #undef Fp #undef Kp - C = ctx->state[1] + C + Dp; - ctx->state[1] = ctx->state[2] + D + Ep; - ctx->state[2] = ctx->state[3] + E + Ap; - ctx->state[3] = ctx->state[4] + A + Bp; - ctx->state[4] = ctx->state[0] + B + Cp; - ctx->state[0] = C; + local.C = ctx->state[1] + local.C + local.Dp; + ctx->state[1] = ctx->state[2] + local.D + local.Ep; + ctx->state[2] = ctx->state[3] + local.E + local.Ap; + ctx->state[3] = ctx->state[4] + local.A + local.Bp; + ctx->state[4] = ctx->state[0] + local.B + local.Cp; + ctx->state[0] = local.C; + + /* Zeroise variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/rsa.c b/thirdparty/mbedtls/library/rsa.c index 42becbf17b..a25c633bc6 100644 --- a/thirdparty/mbedtls/library/rsa.c +++ b/thirdparty/mbedtls/library/rsa.c @@ -841,15 +841,14 @@ static int rsa_prepare_blinding( mbedtls_rsa_context *ctx, * which one, we just loop and choose new values for both of them. * (Each iteration succeeds with overwhelming probability.) */ ret = mbedtls_mpi_inv_mod( &ctx->Vi, &ctx->Vi, &ctx->N ); - if( ret == MBEDTLS_ERR_MPI_NOT_ACCEPTABLE ) - continue; - if( ret != 0 ) + if( ret != 0 && ret != MBEDTLS_ERR_MPI_NOT_ACCEPTABLE ) goto cleanup; - /* Finish the computation of Vf^-1 = R * (R Vf)^-1 */ - MBEDTLS_MPI_CHK( mbedtls_mpi_mul_mpi( &ctx->Vi, &ctx->Vi, &R ) ); - MBEDTLS_MPI_CHK( mbedtls_mpi_mod_mpi( &ctx->Vi, &ctx->Vi, &ctx->N ) ); - } while( 0 ); + } while( ret == MBEDTLS_ERR_MPI_NOT_ACCEPTABLE ); + + /* Finish the computation of Vf^-1 = R * (R Vf)^-1 */ + MBEDTLS_MPI_CHK( mbedtls_mpi_mul_mpi( &ctx->Vi, &ctx->Vi, &R ) ); + MBEDTLS_MPI_CHK( mbedtls_mpi_mod_mpi( &ctx->Vi, &ctx->Vi, &ctx->N ) ); /* Blinding value: Vi = Vf^(-e) mod N * (Vi already contains Vf^-1 at this point) */ diff --git a/thirdparty/mbedtls/library/sha1.c b/thirdparty/mbedtls/library/sha1.c index 8682abd740..e99a5e8635 100644 --- a/thirdparty/mbedtls/library/sha1.c +++ b/thirdparty/mbedtls/library/sha1.c @@ -155,35 +155,40 @@ void mbedtls_sha1_starts( mbedtls_sha1_context *ctx ) int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, const unsigned char data[64] ) { - uint32_t temp, W[16], A, B, C, D, E; + struct + { + uint32_t temp, W[16], A, B, C, D, E; + } local; SHA1_VALIDATE_RET( ctx != NULL ); SHA1_VALIDATE_RET( (const unsigned char *)data != NULL ); - GET_UINT32_BE( W[ 0], data, 0 ); - GET_UINT32_BE( W[ 1], data, 4 ); - GET_UINT32_BE( W[ 2], data, 8 ); - GET_UINT32_BE( W[ 3], data, 12 ); - GET_UINT32_BE( W[ 4], data, 16 ); - GET_UINT32_BE( W[ 5], data, 20 ); - GET_UINT32_BE( W[ 6], data, 24 ); - GET_UINT32_BE( W[ 7], data, 28 ); - GET_UINT32_BE( W[ 8], data, 32 ); - GET_UINT32_BE( W[ 9], data, 36 ); - GET_UINT32_BE( W[10], data, 40 ); - GET_UINT32_BE( W[11], data, 44 ); - GET_UINT32_BE( W[12], data, 48 ); - GET_UINT32_BE( W[13], data, 52 ); - GET_UINT32_BE( W[14], data, 56 ); - GET_UINT32_BE( W[15], data, 60 ); + GET_UINT32_BE( local.W[ 0], data, 0 ); + GET_UINT32_BE( local.W[ 1], data, 4 ); + GET_UINT32_BE( local.W[ 2], data, 8 ); + GET_UINT32_BE( local.W[ 3], data, 12 ); + GET_UINT32_BE( local.W[ 4], data, 16 ); + GET_UINT32_BE( local.W[ 5], data, 20 ); + GET_UINT32_BE( local.W[ 6], data, 24 ); + GET_UINT32_BE( local.W[ 7], data, 28 ); + GET_UINT32_BE( local.W[ 8], data, 32 ); + GET_UINT32_BE( local.W[ 9], data, 36 ); + GET_UINT32_BE( local.W[10], data, 40 ); + GET_UINT32_BE( local.W[11], data, 44 ); + GET_UINT32_BE( local.W[12], data, 48 ); + GET_UINT32_BE( local.W[13], data, 52 ); + GET_UINT32_BE( local.W[14], data, 56 ); + GET_UINT32_BE( local.W[15], data, 60 ); #define S(x,n) (((x) << (n)) | (((x) & 0xFFFFFFFF) >> (32 - (n)))) #define R(t) \ ( \ - temp = W[( (t) - 3 ) & 0x0F] ^ W[( (t) - 8 ) & 0x0F] ^ \ - W[( (t) - 14 ) & 0x0F] ^ W[ (t) & 0x0F], \ - ( W[(t) & 0x0F] = S(temp,1) ) \ + local.temp = local.W[( (t) - 3 ) & 0x0F] ^ \ + local.W[( (t) - 8 ) & 0x0F] ^ \ + local.W[( (t) - 14 ) & 0x0F] ^ \ + local.W[ (t) & 0x0F], \ + ( local.W[(t) & 0x0F] = S(local.temp,1) ) \ ) #define P(a,b,c,d,e,x) \ @@ -193,35 +198,35 @@ int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, (b) = S((b),30); \ } while( 0 ) - A = ctx->state[0]; - B = ctx->state[1]; - C = ctx->state[2]; - D = ctx->state[3]; - E = ctx->state[4]; + local.A = ctx->state[0]; + local.B = ctx->state[1]; + local.C = ctx->state[2]; + local.D = ctx->state[3]; + local.E = ctx->state[4]; #define F(x,y,z) ((z) ^ ((x) & ((y) ^ (z)))) #define K 0x5A827999 - P( A, B, C, D, E, W[0] ); - P( E, A, B, C, D, W[1] ); - P( D, E, A, B, C, W[2] ); - P( C, D, E, A, B, W[3] ); - P( B, C, D, E, A, W[4] ); - P( A, B, C, D, E, W[5] ); - P( E, A, B, C, D, W[6] ); - P( D, E, A, B, C, W[7] ); - P( C, D, E, A, B, W[8] ); - P( B, C, D, E, A, W[9] ); - P( A, B, C, D, E, W[10] ); - P( E, A, B, C, D, W[11] ); - P( D, E, A, B, C, W[12] ); - P( C, D, E, A, B, W[13] ); - P( B, C, D, E, A, W[14] ); - P( A, B, C, D, E, W[15] ); - P( E, A, B, C, D, R(16) ); - P( D, E, A, B, C, R(17) ); - P( C, D, E, A, B, R(18) ); - P( B, C, D, E, A, R(19) ); + P( local.A, local.B, local.C, local.D, local.E, local.W[0] ); + P( local.E, local.A, local.B, local.C, local.D, local.W[1] ); + P( local.D, local.E, local.A, local.B, local.C, local.W[2] ); + P( local.C, local.D, local.E, local.A, local.B, local.W[3] ); + P( local.B, local.C, local.D, local.E, local.A, local.W[4] ); + P( local.A, local.B, local.C, local.D, local.E, local.W[5] ); + P( local.E, local.A, local.B, local.C, local.D, local.W[6] ); + P( local.D, local.E, local.A, local.B, local.C, local.W[7] ); + P( local.C, local.D, local.E, local.A, local.B, local.W[8] ); + P( local.B, local.C, local.D, local.E, local.A, local.W[9] ); + P( local.A, local.B, local.C, local.D, local.E, local.W[10] ); + P( local.E, local.A, local.B, local.C, local.D, local.W[11] ); + P( local.D, local.E, local.A, local.B, local.C, local.W[12] ); + P( local.C, local.D, local.E, local.A, local.B, local.W[13] ); + P( local.B, local.C, local.D, local.E, local.A, local.W[14] ); + P( local.A, local.B, local.C, local.D, local.E, local.W[15] ); + P( local.E, local.A, local.B, local.C, local.D, R(16) ); + P( local.D, local.E, local.A, local.B, local.C, R(17) ); + P( local.C, local.D, local.E, local.A, local.B, R(18) ); + P( local.B, local.C, local.D, local.E, local.A, R(19) ); #undef K #undef F @@ -229,26 +234,26 @@ int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, #define F(x,y,z) ((x) ^ (y) ^ (z)) #define K 0x6ED9EBA1 - P( A, B, C, D, E, R(20) ); - P( E, A, B, C, D, R(21) ); - P( D, E, A, B, C, R(22) ); - P( C, D, E, A, B, R(23) ); - P( B, C, D, E, A, R(24) ); - P( A, B, C, D, E, R(25) ); - P( E, A, B, C, D, R(26) ); - P( D, E, A, B, C, R(27) ); - P( C, D, E, A, B, R(28) ); - P( B, C, D, E, A, R(29) ); - P( A, B, C, D, E, R(30) ); - P( E, A, B, C, D, R(31) ); - P( D, E, A, B, C, R(32) ); - P( C, D, E, A, B, R(33) ); - P( B, C, D, E, A, R(34) ); - P( A, B, C, D, E, R(35) ); - P( E, A, B, C, D, R(36) ); - P( D, E, A, B, C, R(37) ); - P( C, D, E, A, B, R(38) ); - P( B, C, D, E, A, R(39) ); + P( local.A, local.B, local.C, local.D, local.E, R(20) ); + P( local.E, local.A, local.B, local.C, local.D, R(21) ); + P( local.D, local.E, local.A, local.B, local.C, R(22) ); + P( local.C, local.D, local.E, local.A, local.B, R(23) ); + P( local.B, local.C, local.D, local.E, local.A, R(24) ); + P( local.A, local.B, local.C, local.D, local.E, R(25) ); + P( local.E, local.A, local.B, local.C, local.D, R(26) ); + P( local.D, local.E, local.A, local.B, local.C, R(27) ); + P( local.C, local.D, local.E, local.A, local.B, R(28) ); + P( local.B, local.C, local.D, local.E, local.A, R(29) ); + P( local.A, local.B, local.C, local.D, local.E, R(30) ); + P( local.E, local.A, local.B, local.C, local.D, R(31) ); + P( local.D, local.E, local.A, local.B, local.C, R(32) ); + P( local.C, local.D, local.E, local.A, local.B, R(33) ); + P( local.B, local.C, local.D, local.E, local.A, R(34) ); + P( local.A, local.B, local.C, local.D, local.E, R(35) ); + P( local.E, local.A, local.B, local.C, local.D, R(36) ); + P( local.D, local.E, local.A, local.B, local.C, R(37) ); + P( local.C, local.D, local.E, local.A, local.B, R(38) ); + P( local.B, local.C, local.D, local.E, local.A, R(39) ); #undef K #undef F @@ -256,26 +261,26 @@ int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, #define F(x,y,z) (((x) & (y)) | ((z) & ((x) | (y)))) #define K 0x8F1BBCDC - P( A, B, C, D, E, R(40) ); - P( E, A, B, C, D, R(41) ); - P( D, E, A, B, C, R(42) ); - P( C, D, E, A, B, R(43) ); - P( B, C, D, E, A, R(44) ); - P( A, B, C, D, E, R(45) ); - P( E, A, B, C, D, R(46) ); - P( D, E, A, B, C, R(47) ); - P( C, D, E, A, B, R(48) ); - P( B, C, D, E, A, R(49) ); - P( A, B, C, D, E, R(50) ); - P( E, A, B, C, D, R(51) ); - P( D, E, A, B, C, R(52) ); - P( C, D, E, A, B, R(53) ); - P( B, C, D, E, A, R(54) ); - P( A, B, C, D, E, R(55) ); - P( E, A, B, C, D, R(56) ); - P( D, E, A, B, C, R(57) ); - P( C, D, E, A, B, R(58) ); - P( B, C, D, E, A, R(59) ); + P( local.A, local.B, local.C, local.D, local.E, R(40) ); + P( local.E, local.A, local.B, local.C, local.D, R(41) ); + P( local.D, local.E, local.A, local.B, local.C, R(42) ); + P( local.C, local.D, local.E, local.A, local.B, R(43) ); + P( local.B, local.C, local.D, local.E, local.A, R(44) ); + P( local.A, local.B, local.C, local.D, local.E, R(45) ); + P( local.E, local.A, local.B, local.C, local.D, R(46) ); + P( local.D, local.E, local.A, local.B, local.C, R(47) ); + P( local.C, local.D, local.E, local.A, local.B, R(48) ); + P( local.B, local.C, local.D, local.E, local.A, R(49) ); + P( local.A, local.B, local.C, local.D, local.E, R(50) ); + P( local.E, local.A, local.B, local.C, local.D, R(51) ); + P( local.D, local.E, local.A, local.B, local.C, R(52) ); + P( local.C, local.D, local.E, local.A, local.B, R(53) ); + P( local.B, local.C, local.D, local.E, local.A, R(54) ); + P( local.A, local.B, local.C, local.D, local.E, R(55) ); + P( local.E, local.A, local.B, local.C, local.D, R(56) ); + P( local.D, local.E, local.A, local.B, local.C, R(57) ); + P( local.C, local.D, local.E, local.A, local.B, R(58) ); + P( local.B, local.C, local.D, local.E, local.A, R(59) ); #undef K #undef F @@ -283,35 +288,38 @@ int mbedtls_internal_sha1_process( mbedtls_sha1_context *ctx, #define F(x,y,z) ((x) ^ (y) ^ (z)) #define K 0xCA62C1D6 - P( A, B, C, D, E, R(60) ); - P( E, A, B, C, D, R(61) ); - P( D, E, A, B, C, R(62) ); - P( C, D, E, A, B, R(63) ); - P( B, C, D, E, A, R(64) ); - P( A, B, C, D, E, R(65) ); - P( E, A, B, C, D, R(66) ); - P( D, E, A, B, C, R(67) ); - P( C, D, E, A, B, R(68) ); - P( B, C, D, E, A, R(69) ); - P( A, B, C, D, E, R(70) ); - P( E, A, B, C, D, R(71) ); - P( D, E, A, B, C, R(72) ); - P( C, D, E, A, B, R(73) ); - P( B, C, D, E, A, R(74) ); - P( A, B, C, D, E, R(75) ); - P( E, A, B, C, D, R(76) ); - P( D, E, A, B, C, R(77) ); - P( C, D, E, A, B, R(78) ); - P( B, C, D, E, A, R(79) ); + P( local.A, local.B, local.C, local.D, local.E, R(60) ); + P( local.E, local.A, local.B, local.C, local.D, R(61) ); + P( local.D, local.E, local.A, local.B, local.C, R(62) ); + P( local.C, local.D, local.E, local.A, local.B, R(63) ); + P( local.B, local.C, local.D, local.E, local.A, R(64) ); + P( local.A, local.B, local.C, local.D, local.E, R(65) ); + P( local.E, local.A, local.B, local.C, local.D, R(66) ); + P( local.D, local.E, local.A, local.B, local.C, R(67) ); + P( local.C, local.D, local.E, local.A, local.B, R(68) ); + P( local.B, local.C, local.D, local.E, local.A, R(69) ); + P( local.A, local.B, local.C, local.D, local.E, R(70) ); + P( local.E, local.A, local.B, local.C, local.D, R(71) ); + P( local.D, local.E, local.A, local.B, local.C, R(72) ); + P( local.C, local.D, local.E, local.A, local.B, R(73) ); + P( local.B, local.C, local.D, local.E, local.A, R(74) ); + P( local.A, local.B, local.C, local.D, local.E, R(75) ); + P( local.E, local.A, local.B, local.C, local.D, R(76) ); + P( local.D, local.E, local.A, local.B, local.C, R(77) ); + P( local.C, local.D, local.E, local.A, local.B, R(78) ); + P( local.B, local.C, local.D, local.E, local.A, R(79) ); #undef K #undef F - ctx->state[0] += A; - ctx->state[1] += B; - ctx->state[2] += C; - ctx->state[3] += D; - ctx->state[4] += E; + ctx->state[0] += local.A; + ctx->state[1] += local.B; + ctx->state[2] += local.C; + ctx->state[3] += local.D; + ctx->state[4] += local.E; + + /* Zeroise buffers and variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/sha256.c b/thirdparty/mbedtls/library/sha256.c index 5169584b68..75a8f8a2b2 100644 --- a/thirdparty/mbedtls/library/sha256.c +++ b/thirdparty/mbedtls/library/sha256.c @@ -209,77 +209,104 @@ static const uint32_t K[] = #define F0(x,y,z) (((x) & (y)) | ((z) & ((x) | (y)))) #define F1(x,y,z) ((z) ^ ((x) & ((y) ^ (z)))) -#define R(t) \ - ( \ - W[t] = S1(W[(t) - 2]) + W[(t) - 7] + \ - S0(W[(t) - 15]) + W[(t) - 16] \ +#define R(t) \ + ( \ + local.W[t] = S1(local.W[(t) - 2]) + local.W[(t) - 7] + \ + S0(local.W[(t) - 15]) + local.W[(t) - 16] \ ) -#define P(a,b,c,d,e,f,g,h,x,K) \ - do \ - { \ - temp1 = (h) + S3(e) + F1((e),(f),(g)) + (K) + (x); \ - temp2 = S2(a) + F0((a),(b),(c)); \ - (d) += temp1; (h) = temp1 + temp2; \ +#define P(a,b,c,d,e,f,g,h,x,K) \ + do \ + { \ + local.temp1 = (h) + S3(e) + F1((e),(f),(g)) + (K) + (x); \ + local.temp2 = S2(a) + F0((a),(b),(c)); \ + (d) += local.temp1; (h) = local.temp1 + local.temp2; \ } while( 0 ) int mbedtls_internal_sha256_process( mbedtls_sha256_context *ctx, const unsigned char data[64] ) { - uint32_t temp1, temp2, W[64]; - uint32_t A[8]; + struct + { + uint32_t temp1, temp2, W[64]; + uint32_t A[8]; + } local; + unsigned int i; SHA256_VALIDATE_RET( ctx != NULL ); SHA256_VALIDATE_RET( (const unsigned char *)data != NULL ); for( i = 0; i < 8; i++ ) - A[i] = ctx->state[i]; + local.A[i] = ctx->state[i]; #if defined(MBEDTLS_SHA256_SMALLER) for( i = 0; i < 64; i++ ) { if( i < 16 ) - GET_UINT32_BE( W[i], data, 4 * i ); + GET_UINT32_BE( local.W[i], data, 4 * i ); else R( i ); - P( A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7], W[i], K[i] ); + P( local.A[0], local.A[1], local.A[2], local.A[3], local.A[4], + local.A[5], local.A[6], local.A[7], local.W[i], K[i] ); - temp1 = A[7]; A[7] = A[6]; A[6] = A[5]; A[5] = A[4]; A[4] = A[3]; - A[3] = A[2]; A[2] = A[1]; A[1] = A[0]; A[0] = temp1; + local.temp1 = local.A[7]; local.A[7] = local.A[6]; + local.A[6] = local.A[5]; local.A[5] = local.A[4]; + local.A[4] = local.A[3]; local.A[3] = local.A[2]; + local.A[2] = local.A[1]; local.A[1] = local.A[0]; + local.A[0] = local.temp1; } #else /* MBEDTLS_SHA256_SMALLER */ for( i = 0; i < 16; i++ ) - GET_UINT32_BE( W[i], data, 4 * i ); + GET_UINT32_BE( local.W[i], data, 4 * i ); for( i = 0; i < 16; i += 8 ) { - P( A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7], W[i+0], K[i+0] ); - P( A[7], A[0], A[1], A[2], A[3], A[4], A[5], A[6], W[i+1], K[i+1] ); - P( A[6], A[7], A[0], A[1], A[2], A[3], A[4], A[5], W[i+2], K[i+2] ); - P( A[5], A[6], A[7], A[0], A[1], A[2], A[3], A[4], W[i+3], K[i+3] ); - P( A[4], A[5], A[6], A[7], A[0], A[1], A[2], A[3], W[i+4], K[i+4] ); - P( A[3], A[4], A[5], A[6], A[7], A[0], A[1], A[2], W[i+5], K[i+5] ); - P( A[2], A[3], A[4], A[5], A[6], A[7], A[0], A[1], W[i+6], K[i+6] ); - P( A[1], A[2], A[3], A[4], A[5], A[6], A[7], A[0], W[i+7], K[i+7] ); + P( local.A[0], local.A[1], local.A[2], local.A[3], local.A[4], + local.A[5], local.A[6], local.A[7], local.W[i+0], K[i+0] ); + P( local.A[7], local.A[0], local.A[1], local.A[2], local.A[3], + local.A[4], local.A[5], local.A[6], local.W[i+1], K[i+1] ); + P( local.A[6], local.A[7], local.A[0], local.A[1], local.A[2], + local.A[3], local.A[4], local.A[5], local.W[i+2], K[i+2] ); + P( local.A[5], local.A[6], local.A[7], local.A[0], local.A[1], + local.A[2], local.A[3], local.A[4], local.W[i+3], K[i+3] ); + P( local.A[4], local.A[5], local.A[6], local.A[7], local.A[0], + local.A[1], local.A[2], local.A[3], local.W[i+4], K[i+4] ); + P( local.A[3], local.A[4], local.A[5], local.A[6], local.A[7], + local.A[0], local.A[1], local.A[2], local.W[i+5], K[i+5] ); + P( local.A[2], local.A[3], local.A[4], local.A[5], local.A[6], + local.A[7], local.A[0], local.A[1], local.W[i+6], K[i+6] ); + P( local.A[1], local.A[2], local.A[3], local.A[4], local.A[5], + local.A[6], local.A[7], local.A[0], local.W[i+7], K[i+7] ); } for( i = 16; i < 64; i += 8 ) { - P( A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7], R(i+0), K[i+0] ); - P( A[7], A[0], A[1], A[2], A[3], A[4], A[5], A[6], R(i+1), K[i+1] ); - P( A[6], A[7], A[0], A[1], A[2], A[3], A[4], A[5], R(i+2), K[i+2] ); - P( A[5], A[6], A[7], A[0], A[1], A[2], A[3], A[4], R(i+3), K[i+3] ); - P( A[4], A[5], A[6], A[7], A[0], A[1], A[2], A[3], R(i+4), K[i+4] ); - P( A[3], A[4], A[5], A[6], A[7], A[0], A[1], A[2], R(i+5), K[i+5] ); - P( A[2], A[3], A[4], A[5], A[6], A[7], A[0], A[1], R(i+6), K[i+6] ); - P( A[1], A[2], A[3], A[4], A[5], A[6], A[7], A[0], R(i+7), K[i+7] ); + P( local.A[0], local.A[1], local.A[2], local.A[3], local.A[4], + local.A[5], local.A[6], local.A[7], R(i+0), K[i+0] ); + P( local.A[7], local.A[0], local.A[1], local.A[2], local.A[3], + local.A[4], local.A[5], local.A[6], R(i+1), K[i+1] ); + P( local.A[6], local.A[7], local.A[0], local.A[1], local.A[2], + local.A[3], local.A[4], local.A[5], R(i+2), K[i+2] ); + P( local.A[5], local.A[6], local.A[7], local.A[0], local.A[1], + local.A[2], local.A[3], local.A[4], R(i+3), K[i+3] ); + P( local.A[4], local.A[5], local.A[6], local.A[7], local.A[0], + local.A[1], local.A[2], local.A[3], R(i+4), K[i+4] ); + P( local.A[3], local.A[4], local.A[5], local.A[6], local.A[7], + local.A[0], local.A[1], local.A[2], R(i+5), K[i+5] ); + P( local.A[2], local.A[3], local.A[4], local.A[5], local.A[6], + local.A[7], local.A[0], local.A[1], R(i+6), K[i+6] ); + P( local.A[1], local.A[2], local.A[3], local.A[4], local.A[5], + local.A[6], local.A[7], local.A[0], R(i+7), K[i+7] ); } #endif /* MBEDTLS_SHA256_SMALLER */ for( i = 0; i < 8; i++ ) - ctx->state[i] += A[i]; + ctx->state[i] += local.A[i]; + + /* Zeroise buffers and variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/sha512.c b/thirdparty/mbedtls/library/sha512.c index 36d5d96146..986037ab7c 100644 --- a/thirdparty/mbedtls/library/sha512.c +++ b/thirdparty/mbedtls/library/sha512.c @@ -243,8 +243,11 @@ int mbedtls_internal_sha512_process( mbedtls_sha512_context *ctx, const unsigned char data[128] ) { int i; - uint64_t temp1, temp2, W[80]; - uint64_t A, B, C, D, E, F, G, H; + struct + { + uint64_t temp1, temp2, W[80]; + uint64_t A, B, C, D, E, F, G, H; + } local; SHA512_VALIDATE_RET( ctx != NULL ); SHA512_VALIDATE_RET( (const unsigned char *)data != NULL ); @@ -261,56 +264,67 @@ int mbedtls_internal_sha512_process( mbedtls_sha512_context *ctx, #define F0(x,y,z) (((x) & (y)) | ((z) & ((x) | (y)))) #define F1(x,y,z) ((z) ^ ((x) & ((y) ^ (z)))) -#define P(a,b,c,d,e,f,g,h,x,K) \ - do \ - { \ - temp1 = (h) + S3(e) + F1((e),(f),(g)) + (K) + (x); \ - temp2 = S2(a) + F0((a),(b),(c)); \ - (d) += temp1; (h) = temp1 + temp2; \ +#define P(a,b,c,d,e,f,g,h,x,K) \ + do \ + { \ + local.temp1 = (h) + S3(e) + F1((e),(f),(g)) + (K) + (x); \ + local.temp2 = S2(a) + F0((a),(b),(c)); \ + (d) += local.temp1; (h) = local.temp1 + local.temp2; \ } while( 0 ) for( i = 0; i < 16; i++ ) { - GET_UINT64_BE( W[i], data, i << 3 ); + GET_UINT64_BE( local.W[i], data, i << 3 ); } for( ; i < 80; i++ ) { - W[i] = S1(W[i - 2]) + W[i - 7] + - S0(W[i - 15]) + W[i - 16]; + local.W[i] = S1(local.W[i - 2]) + local.W[i - 7] + + S0(local.W[i - 15]) + local.W[i - 16]; } - A = ctx->state[0]; - B = ctx->state[1]; - C = ctx->state[2]; - D = ctx->state[3]; - E = ctx->state[4]; - F = ctx->state[5]; - G = ctx->state[6]; - H = ctx->state[7]; + local.A = ctx->state[0]; + local.B = ctx->state[1]; + local.C = ctx->state[2]; + local.D = ctx->state[3]; + local.E = ctx->state[4]; + local.F = ctx->state[5]; + local.G = ctx->state[6]; + local.H = ctx->state[7]; i = 0; do { - P( A, B, C, D, E, F, G, H, W[i], K[i] ); i++; - P( H, A, B, C, D, E, F, G, W[i], K[i] ); i++; - P( G, H, A, B, C, D, E, F, W[i], K[i] ); i++; - P( F, G, H, A, B, C, D, E, W[i], K[i] ); i++; - P( E, F, G, H, A, B, C, D, W[i], K[i] ); i++; - P( D, E, F, G, H, A, B, C, W[i], K[i] ); i++; - P( C, D, E, F, G, H, A, B, W[i], K[i] ); i++; - P( B, C, D, E, F, G, H, A, W[i], K[i] ); i++; + P( local.A, local.B, local.C, local.D, local.E, + local.F, local.G, local.H, local.W[i], K[i] ); i++; + P( local.H, local.A, local.B, local.C, local.D, + local.E, local.F, local.G, local.W[i], K[i] ); i++; + P( local.G, local.H, local.A, local.B, local.C, + local.D, local.E, local.F, local.W[i], K[i] ); i++; + P( local.F, local.G, local.H, local.A, local.B, + local.C, local.D, local.E, local.W[i], K[i] ); i++; + P( local.E, local.F, local.G, local.H, local.A, + local.B, local.C, local.D, local.W[i], K[i] ); i++; + P( local.D, local.E, local.F, local.G, local.H, + local.A, local.B, local.C, local.W[i], K[i] ); i++; + P( local.C, local.D, local.E, local.F, local.G, + local.H, local.A, local.B, local.W[i], K[i] ); i++; + P( local.B, local.C, local.D, local.E, local.F, + local.G, local.H, local.A, local.W[i], K[i] ); i++; } while( i < 80 ); - ctx->state[0] += A; - ctx->state[1] += B; - ctx->state[2] += C; - ctx->state[3] += D; - ctx->state[4] += E; - ctx->state[5] += F; - ctx->state[6] += G; - ctx->state[7] += H; + ctx->state[0] += local.A; + ctx->state[1] += local.B; + ctx->state[2] += local.C; + ctx->state[3] += local.D; + ctx->state[4] += local.E; + ctx->state[5] += local.F; + ctx->state[6] += local.G; + ctx->state[7] += local.H; + + /* Zeroise buffers and variables to clear sensitive data from memory. */ + mbedtls_platform_zeroize( &local, sizeof( local ) ); return( 0 ); } diff --git a/thirdparty/mbedtls/library/ssl_srv.c b/thirdparty/mbedtls/library/ssl_srv.c index 97b778452c..cbf6142ac2 100644 --- a/thirdparty/mbedtls/library/ssl_srv.c +++ b/thirdparty/mbedtls/library/ssl_srv.c @@ -3587,11 +3587,12 @@ static int ssl_parse_encrypted_pms( mbedtls_ssl_context *ssl, /* In case of a failure in decryption, the decryption may write less than * 2 bytes of output, but we always read the first two bytes. It doesn't * matter in the end because diff will be nonzero in that case due to - * peer_pmslen being less than 48, and we only care whether diff is 0. - * But do initialize peer_pms for robustness anyway. This also makes - * memory analyzers happy (don't access uninitialized memory, even - * if it's an unsigned char). */ + * ret being nonzero, and we only care whether diff is 0. + * But do initialize peer_pms and peer_pmslen for robustness anyway. This + * also makes memory analyzers happy (don't access uninitialized memory, + * even if it's an unsigned char). */ peer_pms[0] = peer_pms[1] = ~0; + peer_pmslen = 0; ret = ssl_decrypt_encrypted_pms( ssl, p, end, peer_pms, diff --git a/thirdparty/mbedtls/library/ssl_tls.c b/thirdparty/mbedtls/library/ssl_tls.c index 2471600c9a..c749a8611c 100644 --- a/thirdparty/mbedtls/library/ssl_tls.c +++ b/thirdparty/mbedtls/library/ssl_tls.c @@ -621,7 +621,7 @@ static void ssl_calc_finished_tls( mbedtls_ssl_context *, unsigned char *, int ) #if defined(MBEDTLS_SSL_PROTO_TLS1_2) #if defined(MBEDTLS_SHA256_C) static void ssl_update_checksum_sha256( mbedtls_ssl_context *, const unsigned char *, size_t ); -static void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *,unsigned char * ); +static void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *, unsigned char * ); static void ssl_calc_finished_tls_sha256( mbedtls_ssl_context *,unsigned char *, int ); #endif @@ -1142,7 +1142,7 @@ int mbedtls_ssl_derive_keys( mbedtls_ssl_context *ssl ) } #if defined(MBEDTLS_SSL_PROTO_SSL3) -void ssl_calc_verify_ssl( mbedtls_ssl_context *ssl, unsigned char hash[36] ) +void ssl_calc_verify_ssl( mbedtls_ssl_context *ssl, unsigned char *hash ) { mbedtls_md5_context md5; mbedtls_sha1_context sha1; @@ -1191,7 +1191,7 @@ void ssl_calc_verify_ssl( mbedtls_ssl_context *ssl, unsigned char hash[36] ) #endif /* MBEDTLS_SSL_PROTO_SSL3 */ #if defined(MBEDTLS_SSL_PROTO_TLS1) || defined(MBEDTLS_SSL_PROTO_TLS1_1) -void ssl_calc_verify_tls( mbedtls_ssl_context *ssl, unsigned char hash[36] ) +void ssl_calc_verify_tls( mbedtls_ssl_context *ssl, unsigned char *hash ) { mbedtls_md5_context md5; mbedtls_sha1_context sha1; @@ -1219,7 +1219,7 @@ void ssl_calc_verify_tls( mbedtls_ssl_context *ssl, unsigned char hash[36] ) #if defined(MBEDTLS_SSL_PROTO_TLS1_2) #if defined(MBEDTLS_SHA256_C) -void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *ssl, unsigned char hash[32] ) +void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *ssl, unsigned char *hash ) { mbedtls_sha256_context sha256; @@ -1240,7 +1240,7 @@ void ssl_calc_verify_tls_sha256( mbedtls_ssl_context *ssl, unsigned char hash[32 #endif /* MBEDTLS_SHA256_C */ #if defined(MBEDTLS_SHA512_C) -void ssl_calc_verify_tls_sha384( mbedtls_ssl_context *ssl, unsigned char hash[48] ) +void ssl_calc_verify_tls_sha384( mbedtls_ssl_context *ssl, unsigned char *hash ) { mbedtls_sha512_context sha512; @@ -6363,6 +6363,9 @@ static void ssl_calc_finished_tls_sha256( #endif /* MBEDTLS_SHA256_C */ #if defined(MBEDTLS_SHA512_C) + +typedef int (*finish_sha384_t)(mbedtls_sha512_context*, unsigned char*); + static void ssl_calc_finished_tls_sha384( mbedtls_ssl_context *ssl, unsigned char *buf, int from ) { @@ -6370,6 +6373,12 @@ static void ssl_calc_finished_tls_sha384( const char *sender; mbedtls_sha512_context sha512; unsigned char padbuf[48]; + /* + * For SHA-384, we can save 16 bytes by keeping padbuf 48 bytes long. + * However, to avoid stringop-overflow warning in gcc, we have to cast + * mbedtls_sha512_finish_ret(). + */ + finish_sha384_t finish_sha384 = (finish_sha384_t)mbedtls_sha512_finish_ret; mbedtls_ssl_session *session = ssl->session_negotiate; if( !session ) @@ -6396,7 +6405,7 @@ static void ssl_calc_finished_tls_sha384( ? "client finished" : "server finished"; - mbedtls_sha512_finish_ret( &sha512, padbuf ); + finish_sha384( &sha512, padbuf ); ssl->handshake->tls_prf( session->master, 48, sender, padbuf, 48, buf, len ); diff --git a/thirdparty/mbedtls/library/threading.c b/thirdparty/mbedtls/library/threading.c index 61c4b94041..f4f29cff5e 100644 --- a/thirdparty/mbedtls/library/threading.c +++ b/thirdparty/mbedtls/library/threading.c @@ -73,7 +73,7 @@ #if !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) || \ ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) && \ - _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) + _POSIX_THREAD_SAFE_FUNCTIONS >= 200112L ) ) /* * This is a convenience shorthand macro to avoid checking the long * preprocessor conditions above. Ideally, we could expose this macro in @@ -88,7 +88,7 @@ #endif /* !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) || \ ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) && \ - _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) */ + _POSIX_THREAD_SAFE_FUNCTIONS >= 200112L ) ) */ #endif /* MBEDTLS_HAVE_TIME_DATE && !MBEDTLS_PLATFORM_GMTIME_R_ALT */ diff --git a/thirdparty/mbedtls/library/x509_crt.c b/thirdparty/mbedtls/library/x509_crt.c index de40eaaf58..c458c25ff4 100644 --- a/thirdparty/mbedtls/library/x509_crt.c +++ b/thirdparty/mbedtls/library/x509_crt.c @@ -1101,6 +1101,7 @@ static int x509_crt_parse_der_core( mbedtls_x509_crt *crt, const unsigned char * if( crt->sig_oid.len != sig_oid2.len || memcmp( crt->sig_oid.p, sig_oid2.p, crt->sig_oid.len ) != 0 || + sig_params1.tag != sig_params2.tag || sig_params1.len != sig_params2.len || ( sig_params1.len != 0 && memcmp( sig_params1.p, sig_params2.p, sig_params1.len ) != 0 ) ) diff --git a/thirdparty/meshoptimizer/LICENSE.md b/thirdparty/meshoptimizer/LICENSE.md new file mode 100644 index 0000000000..4fcd766d22 --- /dev/null +++ b/thirdparty/meshoptimizer/LICENSE.md @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2016-2020 Arseny Kapoulkine + +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 +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/thirdparty/meshoptimizer/allocator.cpp b/thirdparty/meshoptimizer/allocator.cpp new file mode 100644 index 0000000000..da7cc540b2 --- /dev/null +++ b/thirdparty/meshoptimizer/allocator.cpp @@ -0,0 +1,8 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +void meshopt_setAllocator(void* (*allocate)(size_t), void (*deallocate)(void*)) +{ + meshopt_Allocator::Storage::allocate = allocate; + meshopt_Allocator::Storage::deallocate = deallocate; +} diff --git a/thirdparty/meshoptimizer/clusterizer.cpp b/thirdparty/meshoptimizer/clusterizer.cpp new file mode 100644 index 0000000000..f7d88c5136 --- /dev/null +++ b/thirdparty/meshoptimizer/clusterizer.cpp @@ -0,0 +1,351 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <math.h> +#include <string.h> + +// This work is based on: +// Graham Wihlidal. Optimizing the Graphics Pipeline with Compute. 2016 +// Matthaeus Chajdas. GeometryFX 1.2 - Cluster Culling. 2016 +// Jack Ritter. An Efficient Bounding Sphere. 1990 +namespace meshopt +{ + +static void computeBoundingSphere(float result[4], const float points[][3], size_t count) +{ + assert(count > 0); + + // find extremum points along all 3 axes; for each axis we get a pair of points with min/max coordinates + size_t pmin[3] = {0, 0, 0}; + size_t pmax[3] = {0, 0, 0}; + + for (size_t i = 0; i < count; ++i) + { + const float* p = points[i]; + + for (int axis = 0; axis < 3; ++axis) + { + pmin[axis] = (p[axis] < points[pmin[axis]][axis]) ? i : pmin[axis]; + pmax[axis] = (p[axis] > points[pmax[axis]][axis]) ? i : pmax[axis]; + } + } + + // find the pair of points with largest distance + float paxisd2 = 0; + int paxis = 0; + + for (int axis = 0; axis < 3; ++axis) + { + const float* p1 = points[pmin[axis]]; + const float* p2 = points[pmax[axis]]; + + float d2 = (p2[0] - p1[0]) * (p2[0] - p1[0]) + (p2[1] - p1[1]) * (p2[1] - p1[1]) + (p2[2] - p1[2]) * (p2[2] - p1[2]); + + if (d2 > paxisd2) + { + paxisd2 = d2; + paxis = axis; + } + } + + // use the longest segment as the initial sphere diameter + const float* p1 = points[pmin[paxis]]; + const float* p2 = points[pmax[paxis]]; + + float center[3] = {(p1[0] + p2[0]) / 2, (p1[1] + p2[1]) / 2, (p1[2] + p2[2]) / 2}; + float radius = sqrtf(paxisd2) / 2; + + // iteratively adjust the sphere up until all points fit + for (size_t i = 0; i < count; ++i) + { + const float* p = points[i]; + float d2 = (p[0] - center[0]) * (p[0] - center[0]) + (p[1] - center[1]) * (p[1] - center[1]) + (p[2] - center[2]) * (p[2] - center[2]); + + if (d2 > radius * radius) + { + float d = sqrtf(d2); + assert(d > 0); + + float k = 0.5f + (radius / d) / 2; + + center[0] = center[0] * k + p[0] * (1 - k); + center[1] = center[1] * k + p[1] * (1 - k); + center[2] = center[2] * k + p[2] * (1 - k); + radius = (radius + d) / 2; + } + } + + result[0] = center[0]; + result[1] = center[1]; + result[2] = center[2]; + result[3] = radius; +} + +} // namespace meshopt + +size_t meshopt_buildMeshletsBound(size_t index_count, size_t max_vertices, size_t max_triangles) +{ + assert(index_count % 3 == 0); + assert(max_vertices >= 3); + assert(max_triangles >= 1); + + // meshlet construction is limited by max vertices and max triangles per meshlet + // the worst case is that the input is an unindexed stream since this equally stresses both limits + // note that we assume that in the worst case, we leave 2 vertices unpacked in each meshlet - if we have space for 3 we can pack any triangle + size_t max_vertices_conservative = max_vertices - 2; + size_t meshlet_limit_vertices = (index_count + max_vertices_conservative - 1) / max_vertices_conservative; + size_t meshlet_limit_triangles = (index_count / 3 + max_triangles - 1) / max_triangles; + + return meshlet_limit_vertices > meshlet_limit_triangles ? meshlet_limit_vertices : meshlet_limit_triangles; +} + +size_t meshopt_buildMeshlets(meshopt_Meshlet* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, size_t max_vertices, size_t max_triangles) +{ + assert(index_count % 3 == 0); + assert(max_vertices >= 3); + assert(max_triangles >= 1); + + meshopt_Allocator allocator; + + meshopt_Meshlet meshlet; + memset(&meshlet, 0, sizeof(meshlet)); + + assert(max_vertices <= sizeof(meshlet.vertices) / sizeof(meshlet.vertices[0])); + assert(max_triangles <= sizeof(meshlet.indices) / 3); + + // index of the vertex in the meshlet, 0xff if the vertex isn't used + unsigned char* used = allocator.allocate<unsigned char>(vertex_count); + memset(used, -1, vertex_count); + + size_t offset = 0; + + for (size_t i = 0; i < index_count; i += 3) + { + unsigned int a = indices[i + 0], b = indices[i + 1], c = indices[i + 2]; + assert(a < vertex_count && b < vertex_count && c < vertex_count); + + unsigned char& av = used[a]; + unsigned char& bv = used[b]; + unsigned char& cv = used[c]; + + unsigned int used_extra = (av == 0xff) + (bv == 0xff) + (cv == 0xff); + + if (meshlet.vertex_count + used_extra > max_vertices || meshlet.triangle_count >= max_triangles) + { + destination[offset++] = meshlet; + + for (size_t j = 0; j < meshlet.vertex_count; ++j) + used[meshlet.vertices[j]] = 0xff; + + memset(&meshlet, 0, sizeof(meshlet)); + } + + if (av == 0xff) + { + av = meshlet.vertex_count; + meshlet.vertices[meshlet.vertex_count++] = a; + } + + if (bv == 0xff) + { + bv = meshlet.vertex_count; + meshlet.vertices[meshlet.vertex_count++] = b; + } + + if (cv == 0xff) + { + cv = meshlet.vertex_count; + meshlet.vertices[meshlet.vertex_count++] = c; + } + + meshlet.indices[meshlet.triangle_count][0] = av; + meshlet.indices[meshlet.triangle_count][1] = bv; + meshlet.indices[meshlet.triangle_count][2] = cv; + meshlet.triangle_count++; + } + + if (meshlet.triangle_count) + destination[offset++] = meshlet; + + assert(offset <= meshopt_buildMeshletsBound(index_count, max_vertices, max_triangles)); + + return offset; +} + +meshopt_Bounds meshopt_computeClusterBounds(const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + + assert(index_count / 3 <= 256); + + (void)vertex_count; + + size_t vertex_stride_float = vertex_positions_stride / sizeof(float); + + // compute triangle normals and gather triangle corners + float normals[256][3]; + float corners[256][3][3]; + size_t triangles = 0; + + for (size_t i = 0; i < index_count; i += 3) + { + unsigned int a = indices[i + 0], b = indices[i + 1], c = indices[i + 2]; + assert(a < vertex_count && b < vertex_count && c < vertex_count); + + const float* p0 = vertex_positions + vertex_stride_float * a; + const float* p1 = vertex_positions + vertex_stride_float * b; + const float* p2 = vertex_positions + vertex_stride_float * c; + + float p10[3] = {p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]}; + float p20[3] = {p2[0] - p0[0], p2[1] - p0[1], p2[2] - p0[2]}; + + float normalx = p10[1] * p20[2] - p10[2] * p20[1]; + float normaly = p10[2] * p20[0] - p10[0] * p20[2]; + float normalz = p10[0] * p20[1] - p10[1] * p20[0]; + + float area = sqrtf(normalx * normalx + normaly * normaly + normalz * normalz); + + // no need to include degenerate triangles - they will be invisible anyway + if (area == 0.f) + continue; + + // record triangle normals & corners for future use; normal and corner 0 define a plane equation + normals[triangles][0] = normalx / area; + normals[triangles][1] = normaly / area; + normals[triangles][2] = normalz / area; + memcpy(corners[triangles][0], p0, 3 * sizeof(float)); + memcpy(corners[triangles][1], p1, 3 * sizeof(float)); + memcpy(corners[triangles][2], p2, 3 * sizeof(float)); + triangles++; + } + + meshopt_Bounds bounds = {}; + + // degenerate cluster, no valid triangles => trivial reject (cone data is 0) + if (triangles == 0) + return bounds; + + // compute cluster bounding sphere; we'll use the center to determine normal cone apex as well + float psphere[4] = {}; + computeBoundingSphere(psphere, corners[0], triangles * 3); + + float center[3] = {psphere[0], psphere[1], psphere[2]}; + + // treating triangle normals as points, find the bounding sphere - the sphere center determines the optimal cone axis + float nsphere[4] = {}; + computeBoundingSphere(nsphere, normals, triangles); + + float axis[3] = {nsphere[0], nsphere[1], nsphere[2]}; + float axislength = sqrtf(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]); + float invaxislength = axislength == 0.f ? 0.f : 1.f / axislength; + + axis[0] *= invaxislength; + axis[1] *= invaxislength; + axis[2] *= invaxislength; + + // compute a tight cone around all normals, mindp = cos(angle/2) + float mindp = 1.f; + + for (size_t i = 0; i < triangles; ++i) + { + float dp = normals[i][0] * axis[0] + normals[i][1] * axis[1] + normals[i][2] * axis[2]; + + mindp = (dp < mindp) ? dp : mindp; + } + + // fill bounding sphere info; note that below we can return bounds without cone information for degenerate cones + bounds.center[0] = center[0]; + bounds.center[1] = center[1]; + bounds.center[2] = center[2]; + bounds.radius = psphere[3]; + + // degenerate cluster, normal cone is larger than a hemisphere => trivial accept + // note that if mindp is positive but close to 0, the triangle intersection code below gets less stable + // we arbitrarily decide that if a normal cone is ~168 degrees wide or more, the cone isn't useful + if (mindp <= 0.1f) + { + bounds.cone_cutoff = 1; + bounds.cone_cutoff_s8 = 127; + return bounds; + } + + float maxt = 0; + + // we need to find the point on center-t*axis ray that lies in negative half-space of all triangles + for (size_t i = 0; i < triangles; ++i) + { + // dot(center-t*axis-corner, trinormal) = 0 + // dot(center-corner, trinormal) - t * dot(axis, trinormal) = 0 + float cx = center[0] - corners[i][0][0]; + float cy = center[1] - corners[i][0][1]; + float cz = center[2] - corners[i][0][2]; + + float dc = cx * normals[i][0] + cy * normals[i][1] + cz * normals[i][2]; + float dn = axis[0] * normals[i][0] + axis[1] * normals[i][1] + axis[2] * normals[i][2]; + + // dn should be larger than mindp cutoff above + assert(dn > 0.f); + float t = dc / dn; + + maxt = (t > maxt) ? t : maxt; + } + + // cone apex should be in the negative half-space of all cluster triangles by construction + bounds.cone_apex[0] = center[0] - axis[0] * maxt; + bounds.cone_apex[1] = center[1] - axis[1] * maxt; + bounds.cone_apex[2] = center[2] - axis[2] * maxt; + + // note: this axis is the axis of the normal cone, but our test for perspective camera effectively negates the axis + bounds.cone_axis[0] = axis[0]; + bounds.cone_axis[1] = axis[1]; + bounds.cone_axis[2] = axis[2]; + + // cos(a) for normal cone is mindp; we need to add 90 degrees on both sides and invert the cone + // which gives us -cos(a+90) = -(-sin(a)) = sin(a) = sqrt(1 - cos^2(a)) + bounds.cone_cutoff = sqrtf(1 - mindp * mindp); + + // quantize axis & cutoff to 8-bit SNORM format + bounds.cone_axis_s8[0] = (signed char)(meshopt_quantizeSnorm(bounds.cone_axis[0], 8)); + bounds.cone_axis_s8[1] = (signed char)(meshopt_quantizeSnorm(bounds.cone_axis[1], 8)); + bounds.cone_axis_s8[2] = (signed char)(meshopt_quantizeSnorm(bounds.cone_axis[2], 8)); + + // for the 8-bit test to be conservative, we need to adjust the cutoff by measuring the max. error + float cone_axis_s8_e0 = fabsf(bounds.cone_axis_s8[0] / 127.f - bounds.cone_axis[0]); + float cone_axis_s8_e1 = fabsf(bounds.cone_axis_s8[1] / 127.f - bounds.cone_axis[1]); + float cone_axis_s8_e2 = fabsf(bounds.cone_axis_s8[2] / 127.f - bounds.cone_axis[2]); + + // note that we need to round this up instead of rounding to nearest, hence +1 + int cone_cutoff_s8 = int(127 * (bounds.cone_cutoff + cone_axis_s8_e0 + cone_axis_s8_e1 + cone_axis_s8_e2) + 1); + + bounds.cone_cutoff_s8 = (cone_cutoff_s8 > 127) ? 127 : (signed char)(cone_cutoff_s8); + + return bounds; +} + +meshopt_Bounds meshopt_computeMeshletBounds(const meshopt_Meshlet* meshlet, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride) +{ + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + + unsigned int indices[sizeof(meshlet->indices) / sizeof(meshlet->indices[0][0])]; + + for (size_t i = 0; i < meshlet->triangle_count; ++i) + { + unsigned int a = meshlet->vertices[meshlet->indices[i][0]]; + unsigned int b = meshlet->vertices[meshlet->indices[i][1]]; + unsigned int c = meshlet->vertices[meshlet->indices[i][2]]; + + assert(a < vertex_count && b < vertex_count && c < vertex_count); + + indices[i * 3 + 0] = a; + indices[i * 3 + 1] = b; + indices[i * 3 + 2] = c; + } + + return meshopt_computeClusterBounds(indices, meshlet->triangle_count * 3, vertex_positions, vertex_count, vertex_positions_stride); +} diff --git a/thirdparty/meshoptimizer/indexcodec.cpp b/thirdparty/meshoptimizer/indexcodec.cpp new file mode 100644 index 0000000000..eeb541e5be --- /dev/null +++ b/thirdparty/meshoptimizer/indexcodec.cpp @@ -0,0 +1,752 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <string.h> + +#ifndef TRACE +#define TRACE 0 +#endif + +#if TRACE +#include <stdio.h> +#endif + +// This work is based on: +// Fabian Giesen. Simple lossless index buffer compression & follow-up. 2013 +// Conor Stokes. Vertex Cache Optimised Index Buffer Compression. 2014 +namespace meshopt +{ + +const unsigned char kIndexHeader = 0xe0; +const unsigned char kSequenceHeader = 0xd0; + +static int gEncodeIndexVersion = 0; + +typedef unsigned int VertexFifo[16]; +typedef unsigned int EdgeFifo[16][2]; + +static const unsigned int kTriangleIndexOrder[3][3] = { + {0, 1, 2}, + {1, 2, 0}, + {2, 0, 1}, +}; + +static const unsigned char kCodeAuxEncodingTable[16] = { + 0x00, 0x76, 0x87, 0x56, 0x67, 0x78, 0xa9, 0x86, 0x65, 0x89, 0x68, 0x98, 0x01, 0x69, + 0, 0, // last two entries aren't used for encoding +}; + +static int rotateTriangle(unsigned int a, unsigned int b, unsigned int c, unsigned int next) +{ + (void)a; + + return (b == next) ? 1 : (c == next) ? 2 : 0; +} + +static int getEdgeFifo(EdgeFifo fifo, unsigned int a, unsigned int b, unsigned int c, size_t offset) +{ + for (int i = 0; i < 16; ++i) + { + size_t index = (offset - 1 - i) & 15; + + unsigned int e0 = fifo[index][0]; + unsigned int e1 = fifo[index][1]; + + if (e0 == a && e1 == b) + return (i << 2) | 0; + if (e0 == b && e1 == c) + return (i << 2) | 1; + if (e0 == c && e1 == a) + return (i << 2) | 2; + } + + return -1; +} + +static void pushEdgeFifo(EdgeFifo fifo, unsigned int a, unsigned int b, size_t& offset) +{ + fifo[offset][0] = a; + fifo[offset][1] = b; + offset = (offset + 1) & 15; +} + +static int getVertexFifo(VertexFifo fifo, unsigned int v, size_t offset) +{ + for (int i = 0; i < 16; ++i) + { + size_t index = (offset - 1 - i) & 15; + + if (fifo[index] == v) + return i; + } + + return -1; +} + +static void pushVertexFifo(VertexFifo fifo, unsigned int v, size_t& offset, int cond = 1) +{ + fifo[offset] = v; + offset = (offset + cond) & 15; +} + +static void encodeVByte(unsigned char*& data, unsigned int v) +{ + // encode 32-bit value in up to 5 7-bit groups + do + { + *data++ = (v & 127) | (v > 127 ? 128 : 0); + v >>= 7; + } while (v); +} + +static unsigned int decodeVByte(const unsigned char*& data) +{ + unsigned char lead = *data++; + + // fast path: single byte + if (lead < 128) + return lead; + + // slow path: up to 4 extra bytes + // note that this loop always terminates, which is important for malformed data + unsigned int result = lead & 127; + unsigned int shift = 7; + + for (int i = 0; i < 4; ++i) + { + unsigned char group = *data++; + result |= (group & 127) << shift; + shift += 7; + + if (group < 128) + break; + } + + return result; +} + +static void encodeIndex(unsigned char*& data, unsigned int index, unsigned int last) +{ + unsigned int d = index - last; + unsigned int v = (d << 1) ^ (int(d) >> 31); + + encodeVByte(data, v); +} + +static unsigned int decodeIndex(const unsigned char*& data, unsigned int last) +{ + unsigned int v = decodeVByte(data); + unsigned int d = (v >> 1) ^ -int(v & 1); + + return last + d; +} + +static int getCodeAuxIndex(unsigned char v, const unsigned char* table) +{ + for (int i = 0; i < 16; ++i) + if (table[i] == v) + return i; + + return -1; +} + +static void writeTriangle(void* destination, size_t offset, size_t index_size, unsigned int a, unsigned int b, unsigned int c) +{ + if (index_size == 2) + { + static_cast<unsigned short*>(destination)[offset + 0] = (unsigned short)(a); + static_cast<unsigned short*>(destination)[offset + 1] = (unsigned short)(b); + static_cast<unsigned short*>(destination)[offset + 2] = (unsigned short)(c); + } + else + { + static_cast<unsigned int*>(destination)[offset + 0] = a; + static_cast<unsigned int*>(destination)[offset + 1] = b; + static_cast<unsigned int*>(destination)[offset + 2] = c; + } +} + +#if TRACE +static size_t sortTop16(unsigned char dest[16], size_t stats[256]) +{ + size_t destsize = 0; + + for (size_t i = 0; i < 256; ++i) + { + size_t j = 0; + for (; j < destsize; ++j) + { + if (stats[i] >= stats[dest[j]]) + { + if (destsize < 16) + destsize++; + + memmove(&dest[j + 1], &dest[j], destsize - 1 - j); + dest[j] = (unsigned char)i; + break; + } + } + + if (j == destsize && destsize < 16) + { + dest[destsize] = (unsigned char)i; + destsize++; + } + } + + return destsize; +} +#endif + +} // namespace meshopt + +size_t meshopt_encodeIndexBuffer(unsigned char* buffer, size_t buffer_size, const unsigned int* indices, size_t index_count) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + +#if TRACE + size_t codestats[256] = {}; + size_t codeauxstats[256] = {}; +#endif + + // the minimum valid encoding is header, 1 byte per triangle and a 16-byte codeaux table + if (buffer_size < 1 + index_count / 3 + 16) + return 0; + + int version = gEncodeIndexVersion; + + buffer[0] = (unsigned char)(kIndexHeader | version); + + EdgeFifo edgefifo; + memset(edgefifo, -1, sizeof(edgefifo)); + + VertexFifo vertexfifo; + memset(vertexfifo, -1, sizeof(vertexfifo)); + + size_t edgefifooffset = 0; + size_t vertexfifooffset = 0; + + unsigned int next = 0; + unsigned int last = 0; + + unsigned char* code = buffer + 1; + unsigned char* data = code + index_count / 3; + unsigned char* data_safe_end = buffer + buffer_size - 16; + + int fecmax = version >= 1 ? 13 : 15; + + // use static encoding table; it's possible to pack the result and then build an optimal table and repack + // for now we keep it simple and use the table that has been generated based on symbol frequency on a training mesh set + const unsigned char* codeaux_table = kCodeAuxEncodingTable; + + for (size_t i = 0; i < index_count; i += 3) + { + // make sure we have enough space to write a triangle + // each triangle writes at most 16 bytes: 1b for codeaux and 5b for each free index + // after this we can be sure we can write without extra bounds checks + if (data > data_safe_end) + return 0; + + int fer = getEdgeFifo(edgefifo, indices[i + 0], indices[i + 1], indices[i + 2], edgefifooffset); + + if (fer >= 0 && (fer >> 2) < 15) + { + const unsigned int* order = kTriangleIndexOrder[fer & 3]; + + unsigned int a = indices[i + order[0]], b = indices[i + order[1]], c = indices[i + order[2]]; + + // encode edge index and vertex fifo index, next or free index + int fe = fer >> 2; + int fc = getVertexFifo(vertexfifo, c, vertexfifooffset); + + int fec = (fc >= 1 && fc < fecmax) ? fc : (c == next) ? (next++, 0) : 15; + + if (fec == 15 && version >= 1) + { + // encode last-1 and last+1 to optimize strip-like sequences + if (c + 1 == last) + fec = 13, last = c; + if (c == last + 1) + fec = 14, last = c; + } + + *code++ = (unsigned char)((fe << 4) | fec); + +#if TRACE + codestats[code[-1]]++; +#endif + + // note that we need to update the last index since free indices are delta-encoded + if (fec == 15) + encodeIndex(data, c, last), last = c; + + // we only need to push third vertex since first two are likely already in the vertex fifo + if (fec == 0 || fec >= fecmax) + pushVertexFifo(vertexfifo, c, vertexfifooffset); + + // we only need to push two new edges to edge fifo since the third one is already there + pushEdgeFifo(edgefifo, c, b, edgefifooffset); + pushEdgeFifo(edgefifo, a, c, edgefifooffset); + } + else + { + int rotation = rotateTriangle(indices[i + 0], indices[i + 1], indices[i + 2], next); + const unsigned int* order = kTriangleIndexOrder[rotation]; + + unsigned int a = indices[i + order[0]], b = indices[i + order[1]], c = indices[i + order[2]]; + + // if a/b/c are 0/1/2, we emit a reset code + bool reset = false; + + if (a == 0 && b == 1 && c == 2 && next > 0 && version >= 1) + { + reset = true; + next = 0; + + // reset vertex fifo to make sure we don't accidentally reference vertices from that in the future + // this makes sure next continues to get incremented instead of being stuck + memset(vertexfifo, -1, sizeof(vertexfifo)); + } + + int fb = getVertexFifo(vertexfifo, b, vertexfifooffset); + int fc = getVertexFifo(vertexfifo, c, vertexfifooffset); + + // after rotation, a is almost always equal to next, so we don't waste bits on FIFO encoding for a + int fea = (a == next) ? (next++, 0) : 15; + int feb = (fb >= 0 && fb < 14) ? (fb + 1) : (b == next) ? (next++, 0) : 15; + int fec = (fc >= 0 && fc < 14) ? (fc + 1) : (c == next) ? (next++, 0) : 15; + + // we encode feb & fec in 4 bits using a table if possible, and as a full byte otherwise + unsigned char codeaux = (unsigned char)((feb << 4) | fec); + int codeauxindex = getCodeAuxIndex(codeaux, codeaux_table); + + // <14 encodes an index into codeaux table, 14 encodes fea=0, 15 encodes fea=15 + if (fea == 0 && codeauxindex >= 0 && codeauxindex < 14 && !reset) + { + *code++ = (unsigned char)((15 << 4) | codeauxindex); + } + else + { + *code++ = (unsigned char)((15 << 4) | 14 | fea); + *data++ = codeaux; + } + +#if TRACE + codestats[code[-1]]++; + codeauxstats[codeaux]++; +#endif + + // note that we need to update the last index since free indices are delta-encoded + if (fea == 15) + encodeIndex(data, a, last), last = a; + + if (feb == 15) + encodeIndex(data, b, last), last = b; + + if (fec == 15) + encodeIndex(data, c, last), last = c; + + // only push vertices that weren't already in fifo + if (fea == 0 || fea == 15) + pushVertexFifo(vertexfifo, a, vertexfifooffset); + + if (feb == 0 || feb == 15) + pushVertexFifo(vertexfifo, b, vertexfifooffset); + + if (fec == 0 || fec == 15) + pushVertexFifo(vertexfifo, c, vertexfifooffset); + + // all three edges aren't in the fifo; pushing all of them is important so that we can match them for later triangles + pushEdgeFifo(edgefifo, b, a, edgefifooffset); + pushEdgeFifo(edgefifo, c, b, edgefifooffset); + pushEdgeFifo(edgefifo, a, c, edgefifooffset); + } + } + + // make sure we have enough space to write codeaux table + if (data > data_safe_end) + return 0; + + // add codeaux encoding table to the end of the stream; this is used for decoding codeaux *and* as padding + // we need padding for decoding to be able to assume that each triangle is encoded as <= 16 bytes of extra data + // this is enough space for aux byte + 5 bytes per varint index which is the absolute worst case for any input + for (size_t i = 0; i < 16; ++i) + { + // decoder assumes that table entries never refer to separately encoded indices + assert((codeaux_table[i] & 0xf) != 0xf && (codeaux_table[i] >> 4) != 0xf); + + *data++ = codeaux_table[i]; + } + + // since we encode restarts as codeaux without a table reference, we need to make sure 00 is encoded as a table reference + assert(codeaux_table[0] == 0); + + assert(data >= buffer + index_count / 3 + 16); + assert(data <= buffer + buffer_size); + +#if TRACE + unsigned char codetop[16], codeauxtop[16]; + size_t codetopsize = sortTop16(codetop, codestats); + size_t codeauxtopsize = sortTop16(codeauxtop, codeauxstats); + + size_t sumcode = 0, sumcodeaux = 0; + for (size_t i = 0; i < 256; ++i) + sumcode += codestats[i], sumcodeaux += codeauxstats[i]; + + size_t acccode = 0, acccodeaux = 0; + + printf("code\t\t\t\t\tcodeaux\n"); + + for (size_t i = 0; i < codetopsize && i < codeauxtopsize; ++i) + { + acccode += codestats[codetop[i]]; + acccodeaux += codeauxstats[codeauxtop[i]]; + + printf("%2d: %02x = %d (%.1f%% ..%.1f%%)\t\t%2d: %02x = %d (%.1f%% ..%.1f%%)\n", + int(i), codetop[i], int(codestats[codetop[i]]), double(codestats[codetop[i]]) / double(sumcode) * 100, double(acccode) / double(sumcode) * 100, + int(i), codeauxtop[i], int(codeauxstats[codeauxtop[i]]), double(codeauxstats[codeauxtop[i]]) / double(sumcodeaux) * 100, double(acccodeaux) / double(sumcodeaux) * 100); + } +#endif + + return data - buffer; +} + +size_t meshopt_encodeIndexBufferBound(size_t index_count, size_t vertex_count) +{ + assert(index_count % 3 == 0); + + // compute number of bits required for each index + unsigned int vertex_bits = 1; + + while (vertex_bits < 32 && vertex_count > size_t(1) << vertex_bits) + vertex_bits++; + + // worst-case encoding is 2 header bytes + 3 varint-7 encoded index deltas + unsigned int vertex_groups = (vertex_bits + 1 + 6) / 7; + + return 1 + (index_count / 3) * (2 + 3 * vertex_groups) + 16; +} + +void meshopt_encodeIndexVersion(int version) +{ + assert(unsigned(version) <= 1); + + meshopt::gEncodeIndexVersion = version; +} + +int meshopt_decodeIndexBuffer(void* destination, size_t index_count, size_t index_size, const unsigned char* buffer, size_t buffer_size) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + assert(index_size == 2 || index_size == 4); + + // the minimum valid encoding is header, 1 byte per triangle and a 16-byte codeaux table + if (buffer_size < 1 + index_count / 3 + 16) + return -2; + + if ((buffer[0] & 0xf0) != kIndexHeader) + return -1; + + int version = buffer[0] & 0x0f; + if (version > 1) + return -1; + + EdgeFifo edgefifo; + memset(edgefifo, -1, sizeof(edgefifo)); + + VertexFifo vertexfifo; + memset(vertexfifo, -1, sizeof(vertexfifo)); + + size_t edgefifooffset = 0; + size_t vertexfifooffset = 0; + + unsigned int next = 0; + unsigned int last = 0; + + int fecmax = version >= 1 ? 13 : 15; + + // since we store 16-byte codeaux table at the end, triangle data has to begin before data_safe_end + const unsigned char* code = buffer + 1; + const unsigned char* data = code + index_count / 3; + const unsigned char* data_safe_end = buffer + buffer_size - 16; + + const unsigned char* codeaux_table = data_safe_end; + + for (size_t i = 0; i < index_count; i += 3) + { + // make sure we have enough data to read for a triangle + // each triangle reads at most 16 bytes of data: 1b for codeaux and 5b for each free index + // after this we can be sure we can read without extra bounds checks + if (data > data_safe_end) + return -2; + + unsigned char codetri = *code++; + + if (codetri < 0xf0) + { + int fe = codetri >> 4; + + // fifo reads are wrapped around 16 entry buffer + unsigned int a = edgefifo[(edgefifooffset - 1 - fe) & 15][0]; + unsigned int b = edgefifo[(edgefifooffset - 1 - fe) & 15][1]; + + int fec = codetri & 15; + + // note: this is the most common path in the entire decoder + // inside this if we try to stay branchless (by using cmov/etc.) since these aren't predictable + if (fec < fecmax) + { + // fifo reads are wrapped around 16 entry buffer + unsigned int cf = vertexfifo[(vertexfifooffset - 1 - fec) & 15]; + unsigned int c = (fec == 0) ? next : cf; + + int fec0 = fec == 0; + next += fec0; + + // output triangle + writeTriangle(destination, i, index_size, a, b, c); + + // push vertex/edge fifo must match the encoding step *exactly* otherwise the data will not be decoded correctly + pushVertexFifo(vertexfifo, c, vertexfifooffset, fec0); + + pushEdgeFifo(edgefifo, c, b, edgefifooffset); + pushEdgeFifo(edgefifo, a, c, edgefifooffset); + } + else + { + unsigned int c = 0; + + // fec - (fec ^ 3) decodes 13, 14 into -1, 1 + // note that we need to update the last index since free indices are delta-encoded + last = c = (fec != 15) ? last + (fec - (fec ^ 3)) : decodeIndex(data, last); + + // output triangle + writeTriangle(destination, i, index_size, a, b, c); + + // push vertex/edge fifo must match the encoding step *exactly* otherwise the data will not be decoded correctly + pushVertexFifo(vertexfifo, c, vertexfifooffset); + + pushEdgeFifo(edgefifo, c, b, edgefifooffset); + pushEdgeFifo(edgefifo, a, c, edgefifooffset); + } + } + else + { + // fast path: read codeaux from the table + if (codetri < 0xfe) + { + unsigned char codeaux = codeaux_table[codetri & 15]; + + // note: table can't contain feb/fec=15 + int feb = codeaux >> 4; + int fec = codeaux & 15; + + // fifo reads are wrapped around 16 entry buffer + // also note that we increment next for all three vertices before decoding indices - this matches encoder behavior + unsigned int a = next++; + + unsigned int bf = vertexfifo[(vertexfifooffset - feb) & 15]; + unsigned int b = (feb == 0) ? next : bf; + + int feb0 = feb == 0; + next += feb0; + + unsigned int cf = vertexfifo[(vertexfifooffset - fec) & 15]; + unsigned int c = (fec == 0) ? next : cf; + + int fec0 = fec == 0; + next += fec0; + + // output triangle + writeTriangle(destination, i, index_size, a, b, c); + + // push vertex/edge fifo must match the encoding step *exactly* otherwise the data will not be decoded correctly + pushVertexFifo(vertexfifo, a, vertexfifooffset); + pushVertexFifo(vertexfifo, b, vertexfifooffset, feb0); + pushVertexFifo(vertexfifo, c, vertexfifooffset, fec0); + + pushEdgeFifo(edgefifo, b, a, edgefifooffset); + pushEdgeFifo(edgefifo, c, b, edgefifooffset); + pushEdgeFifo(edgefifo, a, c, edgefifooffset); + } + else + { + // slow path: read a full byte for codeaux instead of using a table lookup + unsigned char codeaux = *data++; + + int fea = codetri == 0xfe ? 0 : 15; + int feb = codeaux >> 4; + int fec = codeaux & 15; + + // reset: codeaux is 0 but encoded as not-a-table + if (codeaux == 0) + next = 0; + + // fifo reads are wrapped around 16 entry buffer + // also note that we increment next for all three vertices before decoding indices - this matches encoder behavior + unsigned int a = (fea == 0) ? next++ : 0; + unsigned int b = (feb == 0) ? next++ : vertexfifo[(vertexfifooffset - feb) & 15]; + unsigned int c = (fec == 0) ? next++ : vertexfifo[(vertexfifooffset - fec) & 15]; + + // note that we need to update the last index since free indices are delta-encoded + if (fea == 15) + last = a = decodeIndex(data, last); + + if (feb == 15) + last = b = decodeIndex(data, last); + + if (fec == 15) + last = c = decodeIndex(data, last); + + // output triangle + writeTriangle(destination, i, index_size, a, b, c); + + // push vertex/edge fifo must match the encoding step *exactly* otherwise the data will not be decoded correctly + pushVertexFifo(vertexfifo, a, vertexfifooffset); + pushVertexFifo(vertexfifo, b, vertexfifooffset, (feb == 0) | (feb == 15)); + pushVertexFifo(vertexfifo, c, vertexfifooffset, (fec == 0) | (fec == 15)); + + pushEdgeFifo(edgefifo, b, a, edgefifooffset); + pushEdgeFifo(edgefifo, c, b, edgefifooffset); + pushEdgeFifo(edgefifo, a, c, edgefifooffset); + } + } + } + + // we should've read all data bytes and stopped at the boundary between data and codeaux table + if (data != data_safe_end) + return -3; + + return 0; +} + +size_t meshopt_encodeIndexSequence(unsigned char* buffer, size_t buffer_size, const unsigned int* indices, size_t index_count) +{ + using namespace meshopt; + + // the minimum valid encoding is header, 1 byte per index and a 4-byte tail + if (buffer_size < 1 + index_count + 4) + return 0; + + int version = gEncodeIndexVersion; + + buffer[0] = (unsigned char)(kSequenceHeader | version); + + unsigned int last[2] = {}; + unsigned int current = 0; + + unsigned char* data = buffer + 1; + unsigned char* data_safe_end = buffer + buffer_size - 4; + + for (size_t i = 0; i < index_count; ++i) + { + // make sure we have enough data to write + // each index writes at most 5 bytes of data; there's a 4 byte tail after data_safe_end + // after this we can be sure we can write without extra bounds checks + if (data >= data_safe_end) + return 0; + + unsigned int index = indices[i]; + + // this is a heuristic that switches between baselines when the delta grows too large + // we want the encoded delta to fit into one byte (7 bits), but 2 bits are used for sign and baseline index + // for now we immediately switch the baseline when delta grows too large - this can be adjusted arbitrarily + int cd = int(index - last[current]); + current ^= ((cd < 0 ? -cd : cd) >= 30); + + // encode delta from the last index + unsigned int d = index - last[current]; + unsigned int v = (d << 1) ^ (int(d) >> 31); + + // note: low bit encodes the index of the last baseline which will be used for reconstruction + encodeVByte(data, (v << 1) | current); + + // update last for the next iteration that uses it + last[current] = index; + } + + // make sure we have enough space to write tail + if (data > data_safe_end) + return 0; + + for (int k = 0; k < 4; ++k) + *data++ = 0; + + return data - buffer; +} + +size_t meshopt_encodeIndexSequenceBound(size_t index_count, size_t vertex_count) +{ + // compute number of bits required for each index + unsigned int vertex_bits = 1; + + while (vertex_bits < 32 && vertex_count > size_t(1) << vertex_bits) + vertex_bits++; + + // worst-case encoding is 1 varint-7 encoded index delta for a K bit value and an extra bit + unsigned int vertex_groups = (vertex_bits + 1 + 1 + 6) / 7; + + return 1 + index_count * vertex_groups + 4; +} + +int meshopt_decodeIndexSequence(void* destination, size_t index_count, size_t index_size, const unsigned char* buffer, size_t buffer_size) +{ + using namespace meshopt; + + // the minimum valid encoding is header, 1 byte per index and a 4-byte tail + if (buffer_size < 1 + index_count + 4) + return -2; + + if ((buffer[0] & 0xf0) != kSequenceHeader) + return -1; + + int version = buffer[0] & 0x0f; + if (version > 1) + return -1; + + const unsigned char* data = buffer + 1; + const unsigned char* data_safe_end = buffer + buffer_size - 4; + + unsigned int last[2] = {}; + + for (size_t i = 0; i < index_count; ++i) + { + // make sure we have enough data to read + // each index reads at most 5 bytes of data; there's a 4 byte tail after data_safe_end + // after this we can be sure we can read without extra bounds checks + if (data >= data_safe_end) + return -2; + + unsigned int v = decodeVByte(data); + + // decode the index of the last baseline + unsigned int current = v & 1; + v >>= 1; + + // reconstruct index as a delta + unsigned int d = (v >> 1) ^ -int(v & 1); + unsigned int index = last[current] + d; + + // update last for the next iteration that uses it + last[current] = index; + + if (index_size == 2) + { + static_cast<unsigned short*>(destination)[i] = (unsigned short)(index); + } + else + { + static_cast<unsigned int*>(destination)[i] = index; + } + } + + // we should've read all data bytes and stopped at the boundary between data and tail + if (data != data_safe_end) + return -3; + + return 0; +} diff --git a/thirdparty/meshoptimizer/indexgenerator.cpp b/thirdparty/meshoptimizer/indexgenerator.cpp new file mode 100644 index 0000000000..aa4a30efa4 --- /dev/null +++ b/thirdparty/meshoptimizer/indexgenerator.cpp @@ -0,0 +1,347 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <string.h> + +namespace meshopt +{ + +static unsigned int hashUpdate4(unsigned int h, const unsigned char* key, size_t len) +{ + // MurmurHash2 + const unsigned int m = 0x5bd1e995; + const int r = 24; + + while (len >= 4) + { + unsigned int k = *reinterpret_cast<const unsigned int*>(key); + + k *= m; + k ^= k >> r; + k *= m; + + h *= m; + h ^= k; + + key += 4; + len -= 4; + } + + return h; +} + +struct VertexHasher +{ + const unsigned char* vertices; + size_t vertex_size; + size_t vertex_stride; + + size_t hash(unsigned int index) const + { + return hashUpdate4(0, vertices + index * vertex_stride, vertex_size); + } + + bool equal(unsigned int lhs, unsigned int rhs) const + { + return memcmp(vertices + lhs * vertex_stride, vertices + rhs * vertex_stride, vertex_size) == 0; + } +}; + +struct VertexStreamHasher +{ + const meshopt_Stream* streams; + size_t stream_count; + + size_t hash(unsigned int index) const + { + unsigned int h = 0; + + for (size_t i = 0; i < stream_count; ++i) + { + const meshopt_Stream& s = streams[i]; + const unsigned char* data = static_cast<const unsigned char*>(s.data); + + h = hashUpdate4(h, data + index * s.stride, s.size); + } + + return h; + } + + bool equal(unsigned int lhs, unsigned int rhs) const + { + for (size_t i = 0; i < stream_count; ++i) + { + const meshopt_Stream& s = streams[i]; + const unsigned char* data = static_cast<const unsigned char*>(s.data); + + if (memcmp(data + lhs * s.stride, data + rhs * s.stride, s.size) != 0) + return false; + } + + return true; + } +}; + +static size_t hashBuckets(size_t count) +{ + size_t buckets = 1; + while (buckets < count) + buckets *= 2; + + return buckets; +} + +template <typename T, typename Hash> +static T* hashLookup(T* table, size_t buckets, const Hash& hash, const T& key, const T& empty) +{ + assert(buckets > 0); + assert((buckets & (buckets - 1)) == 0); + + size_t hashmod = buckets - 1; + size_t bucket = hash.hash(key) & hashmod; + + for (size_t probe = 0; probe <= hashmod; ++probe) + { + T& item = table[bucket]; + + if (item == empty) + return &item; + + if (hash.equal(item, key)) + return &item; + + // hash collision, quadratic probing + bucket = (bucket + probe + 1) & hashmod; + } + + assert(false && "Hash table is full"); // unreachable + return 0; +} + +} // namespace meshopt + +size_t meshopt_generateVertexRemap(unsigned int* destination, const unsigned int* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size) +{ + using namespace meshopt; + + assert(indices || index_count == vertex_count); + assert(index_count % 3 == 0); + assert(vertex_size > 0 && vertex_size <= 256); + + meshopt_Allocator allocator; + + memset(destination, -1, vertex_count * sizeof(unsigned int)); + + VertexHasher hasher = {static_cast<const unsigned char*>(vertices), vertex_size, vertex_size}; + + size_t table_size = hashBuckets(vertex_count); + unsigned int* table = allocator.allocate<unsigned int>(table_size); + memset(table, -1, table_size * sizeof(unsigned int)); + + unsigned int next_vertex = 0; + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices ? indices[i] : unsigned(i); + assert(index < vertex_count); + + if (destination[index] == ~0u) + { + unsigned int* entry = hashLookup(table, table_size, hasher, index, ~0u); + + if (*entry == ~0u) + { + *entry = index; + + destination[index] = next_vertex++; + } + else + { + assert(destination[*entry] != ~0u); + + destination[index] = destination[*entry]; + } + } + } + + assert(next_vertex <= vertex_count); + + return next_vertex; +} + +size_t meshopt_generateVertexRemapMulti(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, const struct meshopt_Stream* streams, size_t stream_count) +{ + using namespace meshopt; + + assert(indices || index_count == vertex_count); + assert(index_count % 3 == 0); + assert(stream_count > 0 && stream_count <= 16); + + for (size_t i = 0; i < stream_count; ++i) + { + assert(streams[i].size > 0 && streams[i].size <= 256); + assert(streams[i].size <= streams[i].stride); + } + + meshopt_Allocator allocator; + + memset(destination, -1, vertex_count * sizeof(unsigned int)); + + VertexStreamHasher hasher = {streams, stream_count}; + + size_t table_size = hashBuckets(vertex_count); + unsigned int* table = allocator.allocate<unsigned int>(table_size); + memset(table, -1, table_size * sizeof(unsigned int)); + + unsigned int next_vertex = 0; + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices ? indices[i] : unsigned(i); + assert(index < vertex_count); + + if (destination[index] == ~0u) + { + unsigned int* entry = hashLookup(table, table_size, hasher, index, ~0u); + + if (*entry == ~0u) + { + *entry = index; + + destination[index] = next_vertex++; + } + else + { + assert(destination[*entry] != ~0u); + + destination[index] = destination[*entry]; + } + } + } + + assert(next_vertex <= vertex_count); + + return next_vertex; +} + +void meshopt_remapVertexBuffer(void* destination, const void* vertices, size_t vertex_count, size_t vertex_size, const unsigned int* remap) +{ + assert(vertex_size > 0 && vertex_size <= 256); + + meshopt_Allocator allocator; + + // support in-place remap + if (destination == vertices) + { + unsigned char* vertices_copy = allocator.allocate<unsigned char>(vertex_count * vertex_size); + memcpy(vertices_copy, vertices, vertex_count * vertex_size); + vertices = vertices_copy; + } + + for (size_t i = 0; i < vertex_count; ++i) + { + if (remap[i] != ~0u) + { + assert(remap[i] < vertex_count); + + memcpy(static_cast<unsigned char*>(destination) + remap[i] * vertex_size, static_cast<const unsigned char*>(vertices) + i * vertex_size, vertex_size); + } + } +} + +void meshopt_remapIndexBuffer(unsigned int* destination, const unsigned int* indices, size_t index_count, const unsigned int* remap) +{ + assert(index_count % 3 == 0); + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices ? indices[i] : unsigned(i); + assert(remap[index] != ~0u); + + destination[i] = remap[index]; + } +} + +void meshopt_generateShadowIndexBuffer(unsigned int* destination, const unsigned int* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size, size_t vertex_stride) +{ + using namespace meshopt; + + assert(indices); + assert(index_count % 3 == 0); + assert(vertex_size > 0 && vertex_size <= 256); + assert(vertex_size <= vertex_stride); + + meshopt_Allocator allocator; + + unsigned int* remap = allocator.allocate<unsigned int>(vertex_count); + memset(remap, -1, vertex_count * sizeof(unsigned int)); + + VertexHasher hasher = {static_cast<const unsigned char*>(vertices), vertex_size, vertex_stride}; + + size_t table_size = hashBuckets(vertex_count); + unsigned int* table = allocator.allocate<unsigned int>(table_size); + memset(table, -1, table_size * sizeof(unsigned int)); + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices[i]; + assert(index < vertex_count); + + if (remap[index] == ~0u) + { + unsigned int* entry = hashLookup(table, table_size, hasher, index, ~0u); + + if (*entry == ~0u) + *entry = index; + + remap[index] = *entry; + } + + destination[i] = remap[index]; + } +} + +void meshopt_generateShadowIndexBufferMulti(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, const struct meshopt_Stream* streams, size_t stream_count) +{ + using namespace meshopt; + + assert(indices); + assert(index_count % 3 == 0); + assert(stream_count > 0 && stream_count <= 16); + + for (size_t i = 0; i < stream_count; ++i) + { + assert(streams[i].size > 0 && streams[i].size <= 256); + assert(streams[i].size <= streams[i].stride); + } + + meshopt_Allocator allocator; + + unsigned int* remap = allocator.allocate<unsigned int>(vertex_count); + memset(remap, -1, vertex_count * sizeof(unsigned int)); + + VertexStreamHasher hasher = {streams, stream_count}; + + size_t table_size = hashBuckets(vertex_count); + unsigned int* table = allocator.allocate<unsigned int>(table_size); + memset(table, -1, table_size * sizeof(unsigned int)); + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices[i]; + assert(index < vertex_count); + + if (remap[index] == ~0u) + { + unsigned int* entry = hashLookup(table, table_size, hasher, index, ~0u); + + if (*entry == ~0u) + *entry = index; + + remap[index] = *entry; + } + + destination[i] = remap[index]; + } +} diff --git a/thirdparty/meshoptimizer/meshoptimizer.h b/thirdparty/meshoptimizer/meshoptimizer.h new file mode 100644 index 0000000000..fde00f9c82 --- /dev/null +++ b/thirdparty/meshoptimizer/meshoptimizer.h @@ -0,0 +1,951 @@ +/** + * meshoptimizer - version 0.15 + * + * Copyright (C) 2016-2020, by Arseny Kapoulkine (arseny.kapoulkine@gmail.com) + * Report bugs and download new versions at https://github.com/zeux/meshoptimizer + * + * This library is distributed under the MIT License. See notice at the end of this file. + */ +#pragma once + +#include <assert.h> +#include <stddef.h> + +/* Version macro; major * 1000 + minor * 10 + patch */ +#define MESHOPTIMIZER_VERSION 150 /* 0.15 */ + +/* If no API is defined, assume default */ +#ifndef MESHOPTIMIZER_API +#define MESHOPTIMIZER_API +#endif + +/* Experimental APIs have unstable interface and might have implementation that's not fully tested or optimized */ +#define MESHOPTIMIZER_EXPERIMENTAL MESHOPTIMIZER_API + +/* C interface */ +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Vertex attribute stream, similar to glVertexPointer + * Each element takes size bytes, with stride controlling the spacing between successive elements. + */ +struct meshopt_Stream +{ + const void* data; + size_t size; + size_t stride; +}; + +/** + * Generates a vertex remap table from the vertex buffer and an optional index buffer and returns number of unique vertices + * As a result, all vertices that are binary equivalent map to the same (new) location, with no gaps in the resulting sequence. + * Resulting remap table maps old vertices to new vertices and can be used in meshopt_remapVertexBuffer/meshopt_remapIndexBuffer. + * Note that binary equivalence considers all vertex_size bytes, including padding which should be zero-initialized. + * + * destination must contain enough space for the resulting remap table (vertex_count elements) + * indices can be NULL if the input is unindexed + */ +MESHOPTIMIZER_API size_t meshopt_generateVertexRemap(unsigned int* destination, const unsigned int* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size); + +/** + * Generates a vertex remap table from multiple vertex streams and an optional index buffer and returns number of unique vertices + * As a result, all vertices that are binary equivalent map to the same (new) location, with no gaps in the resulting sequence. + * Resulting remap table maps old vertices to new vertices and can be used in meshopt_remapVertexBuffer/meshopt_remapIndexBuffer. + * To remap vertex buffers, you will need to call meshopt_remapVertexBuffer for each vertex stream. + * Note that binary equivalence considers all size bytes in each stream, including padding which should be zero-initialized. + * + * destination must contain enough space for the resulting remap table (vertex_count elements) + * indices can be NULL if the input is unindexed + */ +MESHOPTIMIZER_API size_t meshopt_generateVertexRemapMulti(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, const struct meshopt_Stream* streams, size_t stream_count); + +/** + * Generates vertex buffer from the source vertex buffer and remap table generated by meshopt_generateVertexRemap + * + * destination must contain enough space for the resulting vertex buffer (unique_vertex_count elements, returned by meshopt_generateVertexRemap) + * vertex_count should be the initial vertex count and not the value returned by meshopt_generateVertexRemap + */ +MESHOPTIMIZER_API void meshopt_remapVertexBuffer(void* destination, const void* vertices, size_t vertex_count, size_t vertex_size, const unsigned int* remap); + +/** + * Generate index buffer from the source index buffer and remap table generated by meshopt_generateVertexRemap + * + * destination must contain enough space for the resulting index buffer (index_count elements) + * indices can be NULL if the input is unindexed + */ +MESHOPTIMIZER_API void meshopt_remapIndexBuffer(unsigned int* destination, const unsigned int* indices, size_t index_count, const unsigned int* remap); + +/** + * Generate index buffer that can be used for more efficient rendering when only a subset of the vertex attributes is necessary + * All vertices that are binary equivalent (wrt first vertex_size bytes) map to the first vertex in the original vertex buffer. + * This makes it possible to use the index buffer for Z pre-pass or shadowmap rendering, while using the original index buffer for regular rendering. + * Note that binary equivalence considers all vertex_size bytes, including padding which should be zero-initialized. + * + * destination must contain enough space for the resulting index buffer (index_count elements) + */ +MESHOPTIMIZER_API void meshopt_generateShadowIndexBuffer(unsigned int* destination, const unsigned int* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size, size_t vertex_stride); + +/** + * Generate index buffer that can be used for more efficient rendering when only a subset of the vertex attributes is necessary + * All vertices that are binary equivalent (wrt specified streams) map to the first vertex in the original vertex buffer. + * This makes it possible to use the index buffer for Z pre-pass or shadowmap rendering, while using the original index buffer for regular rendering. + * Note that binary equivalence considers all size bytes in each stream, including padding which should be zero-initialized. + * + * destination must contain enough space for the resulting index buffer (index_count elements) + */ +MESHOPTIMIZER_API void meshopt_generateShadowIndexBufferMulti(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, const struct meshopt_Stream* streams, size_t stream_count); + +/** + * Vertex transform cache optimizer + * Reorders indices to reduce the number of GPU vertex shader invocations + * If index buffer contains multiple ranges for multiple draw calls, this functions needs to be called on each range individually. + * + * destination must contain enough space for the resulting index buffer (index_count elements) + */ +MESHOPTIMIZER_API void meshopt_optimizeVertexCache(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count); + +/** + * Vertex transform cache optimizer for strip-like caches + * Produces inferior results to meshopt_optimizeVertexCache from the GPU vertex cache perspective + * However, the resulting index order is more optimal if the goal is to reduce the triangle strip length or improve compression efficiency + * + * destination must contain enough space for the resulting index buffer (index_count elements) + */ +MESHOPTIMIZER_API void meshopt_optimizeVertexCacheStrip(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count); + +/** + * Vertex transform cache optimizer for FIFO caches + * Reorders indices to reduce the number of GPU vertex shader invocations + * Generally takes ~3x less time to optimize meshes but produces inferior results compared to meshopt_optimizeVertexCache + * If index buffer contains multiple ranges for multiple draw calls, this functions needs to be called on each range individually. + * + * destination must contain enough space for the resulting index buffer (index_count elements) + * cache_size should be less than the actual GPU cache size to avoid cache thrashing + */ +MESHOPTIMIZER_API void meshopt_optimizeVertexCacheFifo(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, unsigned int cache_size); + +/** + * Overdraw optimizer + * Reorders indices to reduce the number of GPU vertex shader invocations and the pixel overdraw + * If index buffer contains multiple ranges for multiple draw calls, this functions needs to be called on each range individually. + * + * destination must contain enough space for the resulting index buffer (index_count elements) + * indices must contain index data that is the result of meshopt_optimizeVertexCache (*not* the original mesh indices!) + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + * threshold indicates how much the overdraw optimizer can degrade vertex cache efficiency (1.05 = up to 5%) to reduce overdraw more efficiently + */ +MESHOPTIMIZER_API void meshopt_optimizeOverdraw(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, float threshold); + +/** + * Vertex fetch cache optimizer + * Reorders vertices and changes indices to reduce the amount of GPU memory fetches during vertex processing + * Returns the number of unique vertices, which is the same as input vertex count unless some vertices are unused + * This functions works for a single vertex stream; for multiple vertex streams, use meshopt_optimizeVertexFetchRemap + meshopt_remapVertexBuffer for each stream. + * + * destination must contain enough space for the resulting vertex buffer (vertex_count elements) + * indices is used both as an input and as an output index buffer + */ +MESHOPTIMIZER_API size_t meshopt_optimizeVertexFetch(void* destination, unsigned int* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size); + +/** + * Vertex fetch cache optimizer + * Generates vertex remap to reduce the amount of GPU memory fetches during vertex processing + * Returns the number of unique vertices, which is the same as input vertex count unless some vertices are unused + * The resulting remap table should be used to reorder vertex/index buffers using meshopt_remapVertexBuffer/meshopt_remapIndexBuffer + * + * destination must contain enough space for the resulting remap table (vertex_count elements) + */ +MESHOPTIMIZER_API size_t meshopt_optimizeVertexFetchRemap(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count); + +/** + * Index buffer encoder + * Encodes index data into an array of bytes that is generally much smaller (<1.5 bytes/triangle) and compresses better (<1 bytes/triangle) compared to original. + * Input index buffer must represent a triangle list. + * Returns encoded data size on success, 0 on error; the only error condition is if buffer doesn't have enough space + * For maximum efficiency the index buffer being encoded has to be optimized for vertex cache and vertex fetch first. + * + * buffer must contain enough space for the encoded index buffer (use meshopt_encodeIndexBufferBound to compute worst case size) + */ +MESHOPTIMIZER_API size_t meshopt_encodeIndexBuffer(unsigned char* buffer, size_t buffer_size, const unsigned int* indices, size_t index_count); +MESHOPTIMIZER_API size_t meshopt_encodeIndexBufferBound(size_t index_count, size_t vertex_count); + +/** + * Experimental: Set index encoder format version + * version must specify the data format version to encode; valid values are 0 (decodable by all library versions) and 1 (decodable by 0.14+) + */ +MESHOPTIMIZER_EXPERIMENTAL void meshopt_encodeIndexVersion(int version); + +/** + * Index buffer decoder + * Decodes index data from an array of bytes generated by meshopt_encodeIndexBuffer + * Returns 0 if decoding was successful, and an error code otherwise + * The decoder is safe to use for untrusted input, but it may produce garbage data (e.g. out of range indices). + * + * destination must contain enough space for the resulting index buffer (index_count elements) + */ +MESHOPTIMIZER_API int meshopt_decodeIndexBuffer(void* destination, size_t index_count, size_t index_size, const unsigned char* buffer, size_t buffer_size); + +/** + * Experimental: Index sequence encoder + * Encodes index sequence into an array of bytes that is generally smaller and compresses better compared to original. + * Input index sequence can represent arbitrary topology; for triangle lists meshopt_encodeIndexBuffer is likely to be better. + * Returns encoded data size on success, 0 on error; the only error condition is if buffer doesn't have enough space + * + * buffer must contain enough space for the encoded index sequence (use meshopt_encodeIndexSequenceBound to compute worst case size) + */ +MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_encodeIndexSequence(unsigned char* buffer, size_t buffer_size, const unsigned int* indices, size_t index_count); +MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_encodeIndexSequenceBound(size_t index_count, size_t vertex_count); + +/** + * Index sequence decoder + * Decodes index data from an array of bytes generated by meshopt_encodeIndexSequence + * Returns 0 if decoding was successful, and an error code otherwise + * The decoder is safe to use for untrusted input, but it may produce garbage data (e.g. out of range indices). + * + * destination must contain enough space for the resulting index sequence (index_count elements) + */ +MESHOPTIMIZER_EXPERIMENTAL int meshopt_decodeIndexSequence(void* destination, size_t index_count, size_t index_size, const unsigned char* buffer, size_t buffer_size); + +/** + * Vertex buffer encoder + * Encodes vertex data into an array of bytes that is generally smaller and compresses better compared to original. + * Returns encoded data size on success, 0 on error; the only error condition is if buffer doesn't have enough space + * This function works for a single vertex stream; for multiple vertex streams, call meshopt_encodeVertexBuffer for each stream. + * Note that all vertex_size bytes of each vertex are encoded verbatim, including padding which should be zero-initialized. + * + * buffer must contain enough space for the encoded vertex buffer (use meshopt_encodeVertexBufferBound to compute worst case size) + */ +MESHOPTIMIZER_API size_t meshopt_encodeVertexBuffer(unsigned char* buffer, size_t buffer_size, const void* vertices, size_t vertex_count, size_t vertex_size); +MESHOPTIMIZER_API size_t meshopt_encodeVertexBufferBound(size_t vertex_count, size_t vertex_size); + +/** + * Experimental: Set vertex encoder format version + * version must specify the data format version to encode; valid values are 0 (decodable by all library versions) + */ +MESHOPTIMIZER_EXPERIMENTAL void meshopt_encodeVertexVersion(int version); + +/** + * Vertex buffer decoder + * Decodes vertex data from an array of bytes generated by meshopt_encodeVertexBuffer + * Returns 0 if decoding was successful, and an error code otherwise + * The decoder is safe to use for untrusted input, but it may produce garbage data. + * + * destination must contain enough space for the resulting vertex buffer (vertex_count * vertex_size bytes) + */ +MESHOPTIMIZER_API int meshopt_decodeVertexBuffer(void* destination, size_t vertex_count, size_t vertex_size, const unsigned char* buffer, size_t buffer_size); + +/** + * Vertex buffer filters + * These functions can be used to filter output of meshopt_decodeVertexBuffer in-place. + * count must be aligned by 4 and stride is fixed for each function to facilitate SIMD implementation. + * + * meshopt_decodeFilterOct decodes octahedral encoding of a unit vector with K-bit (K <= 16) signed X/Y as an input; Z must store 1.0f. + * Each component is stored as an 8-bit or 16-bit normalized integer; stride must be equal to 4 or 8. W is preserved as is. + * + * meshopt_decodeFilterQuat decodes 3-component quaternion encoding with K-bit (4 <= K <= 16) component encoding and a 2-bit component index indicating which component to reconstruct. + * Each component is stored as an 16-bit integer; stride must be equal to 8. + * + * meshopt_decodeFilterExp decodes exponential encoding of floating-point data with 8-bit exponent and 24-bit integer mantissa as 2^E*M. + * Each 32-bit component is decoded in isolation; stride must be divisible by 4. + */ +MESHOPTIMIZER_EXPERIMENTAL void meshopt_decodeFilterOct(void* buffer, size_t vertex_count, size_t vertex_size); +MESHOPTIMIZER_EXPERIMENTAL void meshopt_decodeFilterQuat(void* buffer, size_t vertex_count, size_t vertex_size); +MESHOPTIMIZER_EXPERIMENTAL void meshopt_decodeFilterExp(void* buffer, size_t vertex_count, size_t vertex_size); + +/** + * Experimental: Mesh simplifier + * Reduces the number of triangles in the mesh, attempting to preserve mesh appearance as much as possible + * The algorithm tries to preserve mesh topology and can stop short of the target goal based on topology constraints or target error. + * If not all attributes from the input mesh are required, it's recommended to reindex the mesh using meshopt_generateShadowIndexBuffer prior to simplification. + * Returns the number of indices after simplification, with destination containing new index data + * The resulting index buffer references vertices from the original vertex buffer. + * If the original vertex data isn't required, creating a compact vertex buffer using meshopt_optimizeVertexFetch is recommended. + * + * destination must contain enough space for the *source* index buffer (since optimization is iterative, this means index_count elements - *not* target_index_count!) + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + */ +// -- GODOT start -- +//MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); +MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_resulting_error); +// -- GODOT end -- + +/** + * Experimental: Mesh simplifier (sloppy) + * Reduces the number of triangles in the mesh, sacrificing mesh apperance for simplification performance + * The algorithm doesn't preserve mesh topology but is always able to reach target triangle count. + * Returns the number of indices after simplification, with destination containing new index data + * The resulting index buffer references vertices from the original vertex buffer. + * If the original vertex data isn't required, creating a compact vertex buffer using meshopt_optimizeVertexFetch is recommended. + * + * destination must contain enough space for the target index buffer + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + */ +MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplifySloppy(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count); + +/** + * Experimental: Point cloud simplifier + * Reduces the number of points in the cloud to reach the given target + * Returns the number of points after simplification, with destination containing new index data + * The resulting index buffer references vertices from the original vertex buffer. + * If the original vertex data isn't required, creating a compact vertex buffer using meshopt_optimizeVertexFetch is recommended. + * + * destination must contain enough space for the target index buffer + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + */ +MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplifyPoints(unsigned int* destination, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_vertex_count); + +/** + * Mesh stripifier + * Converts a previously vertex cache optimized triangle list to triangle strip, stitching strips using restart index or degenerate triangles + * Returns the number of indices in the resulting strip, with destination containing new index data + * For maximum efficiency the index buffer being converted has to be optimized for vertex cache first. + * Using restart indices can result in ~10% smaller index buffers, but on some GPUs restart indices may result in decreased performance. + * + * destination must contain enough space for the target index buffer, worst case can be computed with meshopt_stripifyBound + * restart_index should be 0xffff or 0xffffffff depending on index size, or 0 to use degenerate triangles + */ +MESHOPTIMIZER_API size_t meshopt_stripify(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, unsigned int restart_index); +MESHOPTIMIZER_API size_t meshopt_stripifyBound(size_t index_count); + +/** + * Mesh unstripifier + * Converts a triangle strip to a triangle list + * Returns the number of indices in the resulting list, with destination containing new index data + * + * destination must contain enough space for the target index buffer, worst case can be computed with meshopt_unstripifyBound + */ +MESHOPTIMIZER_API size_t meshopt_unstripify(unsigned int* destination, const unsigned int* indices, size_t index_count, unsigned int restart_index); +MESHOPTIMIZER_API size_t meshopt_unstripifyBound(size_t index_count); + +struct meshopt_VertexCacheStatistics +{ + unsigned int vertices_transformed; + unsigned int warps_executed; + float acmr; /* transformed vertices / triangle count; best case 0.5, worst case 3.0, optimum depends on topology */ + float atvr; /* transformed vertices / vertex count; best case 1.0, worst case 6.0, optimum is 1.0 (each vertex is transformed once) */ +}; + +/** + * Vertex transform cache analyzer + * Returns cache hit statistics using a simplified FIFO model + * Results may not match actual GPU performance + */ +MESHOPTIMIZER_API struct meshopt_VertexCacheStatistics meshopt_analyzeVertexCache(const unsigned int* indices, size_t index_count, size_t vertex_count, unsigned int cache_size, unsigned int warp_size, unsigned int primgroup_size); + +struct meshopt_OverdrawStatistics +{ + unsigned int pixels_covered; + unsigned int pixels_shaded; + float overdraw; /* shaded pixels / covered pixels; best case 1.0 */ +}; + +/** + * Overdraw analyzer + * Returns overdraw statistics using a software rasterizer + * Results may not match actual GPU performance + * + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + */ +MESHOPTIMIZER_API struct meshopt_OverdrawStatistics meshopt_analyzeOverdraw(const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride); + +struct meshopt_VertexFetchStatistics +{ + unsigned int bytes_fetched; + float overfetch; /* fetched bytes / vertex buffer size; best case 1.0 (each byte is fetched once) */ +}; + +/** + * Vertex fetch cache analyzer + * Returns cache hit statistics using a simplified direct mapped model + * Results may not match actual GPU performance + */ +MESHOPTIMIZER_API struct meshopt_VertexFetchStatistics meshopt_analyzeVertexFetch(const unsigned int* indices, size_t index_count, size_t vertex_count, size_t vertex_size); + +struct meshopt_Meshlet +{ + unsigned int vertices[64]; + unsigned char indices[126][3]; + unsigned char triangle_count; + unsigned char vertex_count; +}; + +/** + * Experimental: Meshlet builder + * Splits the mesh into a set of meshlets where each meshlet has a micro index buffer indexing into meshlet vertices that refer to the original vertex buffer + * The resulting data can be used to render meshes using NVidia programmable mesh shading pipeline, or in other cluster-based renderers. + * For maximum efficiency the index buffer being converted has to be optimized for vertex cache first. + * + * destination must contain enough space for all meshlets, worst case size can be computed with meshopt_buildMeshletsBound + * max_vertices and max_triangles can't exceed limits statically declared in meshopt_Meshlet (max_vertices <= 64, max_triangles <= 126) + */ +MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_buildMeshlets(struct meshopt_Meshlet* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, size_t max_vertices, size_t max_triangles); +MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_buildMeshletsBound(size_t index_count, size_t max_vertices, size_t max_triangles); + +struct meshopt_Bounds +{ + /* bounding sphere, useful for frustum and occlusion culling */ + float center[3]; + float radius; + + /* normal cone, useful for backface culling */ + float cone_apex[3]; + float cone_axis[3]; + float cone_cutoff; /* = cos(angle/2) */ + + /* normal cone axis and cutoff, stored in 8-bit SNORM format; decode using x/127.0 */ + signed char cone_axis_s8[3]; + signed char cone_cutoff_s8; +}; + +/** + * Experimental: Cluster bounds generator + * Creates bounding volumes that can be used for frustum, backface and occlusion culling. + * + * For backface culling with orthographic projection, use the following formula to reject backfacing clusters: + * dot(view, cone_axis) >= cone_cutoff + * + * For perspective projection, you can the formula that needs cone apex in addition to axis & cutoff: + * dot(normalize(cone_apex - camera_position), cone_axis) >= cone_cutoff + * + * Alternatively, you can use the formula that doesn't need cone apex and uses bounding sphere instead: + * dot(normalize(center - camera_position), cone_axis) >= cone_cutoff + radius / length(center - camera_position) + * or an equivalent formula that doesn't have a singularity at center = camera_position: + * dot(center - camera_position, cone_axis) >= cone_cutoff * length(center - camera_position) + radius + * + * The formula that uses the apex is slightly more accurate but needs the apex; if you are already using bounding sphere + * to do frustum/occlusion culling, the formula that doesn't use the apex may be preferable. + * + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + * index_count should be less than or equal to 256*3 (the function assumes clusters of limited size) + */ +MESHOPTIMIZER_EXPERIMENTAL struct meshopt_Bounds meshopt_computeClusterBounds(const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride); +MESHOPTIMIZER_EXPERIMENTAL struct meshopt_Bounds meshopt_computeMeshletBounds(const struct meshopt_Meshlet* meshlet, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride); + +/** + * Experimental: Spatial sorter + * Generates a remap table that can be used to reorder points for spatial locality. + * Resulting remap table maps old vertices to new vertices and can be used in meshopt_remapVertexBuffer. + * + * destination must contain enough space for the resulting remap table (vertex_count elements) + */ +MESHOPTIMIZER_EXPERIMENTAL void meshopt_spatialSortRemap(unsigned int* destination, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride); + +/** + * Experimental: Spatial sorter + * Reorders triangles for spatial locality, and generates a new index buffer. The resulting index buffer can be used with other functions like optimizeVertexCache. + * + * destination must contain enough space for the resulting index buffer (index_count elements) + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + */ +MESHOPTIMIZER_EXPERIMENTAL void meshopt_spatialSortTriangles(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride); + +/** + * Set allocation callbacks + * These callbacks will be used instead of the default operator new/operator delete for all temporary allocations in the library. + * Note that all algorithms only allocate memory for temporary use. + * allocate/deallocate are always called in a stack-like order - last pointer to be allocated is deallocated first. + */ +MESHOPTIMIZER_API void meshopt_setAllocator(void* (*allocate)(size_t), void (*deallocate)(void*)); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +/* Quantization into commonly supported data formats */ +#ifdef __cplusplus +/** + * Quantize a float in [0..1] range into an N-bit fixed point unorm value + * Assumes reconstruction function (q / (2^N-1)), which is the case for fixed-function normalized fixed point conversion + * Maximum reconstruction error: 1/2^(N+1) + */ +inline int meshopt_quantizeUnorm(float v, int N); + +/** + * Quantize a float in [-1..1] range into an N-bit fixed point snorm value + * Assumes reconstruction function (q / (2^(N-1)-1)), which is the case for fixed-function normalized fixed point conversion (except early OpenGL versions) + * Maximum reconstruction error: 1/2^N + */ +inline int meshopt_quantizeSnorm(float v, int N); + +/** + * Quantize a float into half-precision floating point value + * Generates +-inf for overflow, preserves NaN, flushes denormals to zero, rounds to nearest + * Representable magnitude range: [6e-5; 65504] + * Maximum relative reconstruction error: 5e-4 + */ +inline unsigned short meshopt_quantizeHalf(float v); + +/** + * Quantize a float into a floating point value with a limited number of significant mantissa bits + * Generates +-inf for overflow, preserves NaN, flushes denormals to zero, rounds to nearest + * Assumes N is in a valid mantissa precision range, which is 1..23 + */ +inline float meshopt_quantizeFloat(float v, int N); +#endif + +/** + * C++ template interface + * + * These functions mirror the C interface the library provides, providing template-based overloads so that + * the caller can use an arbitrary type for the index data, both for input and output. + * When the supplied type is the same size as that of unsigned int, the wrappers are zero-cost; when it's not, + * the wrappers end up allocating memory and copying index data to convert from one type to another. + */ +#if defined(__cplusplus) && !defined(MESHOPTIMIZER_NO_WRAPPERS) +template <typename T> +inline size_t meshopt_generateVertexRemap(unsigned int* destination, const T* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size); +template <typename T> +inline size_t meshopt_generateVertexRemapMulti(unsigned int* destination, const T* indices, size_t index_count, size_t vertex_count, const meshopt_Stream* streams, size_t stream_count); +template <typename T> +inline void meshopt_remapIndexBuffer(T* destination, const T* indices, size_t index_count, const unsigned int* remap); +template <typename T> +inline void meshopt_generateShadowIndexBuffer(T* destination, const T* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size, size_t vertex_stride); +template <typename T> +inline void meshopt_generateShadowIndexBufferMulti(T* destination, const T* indices, size_t index_count, size_t vertex_count, const meshopt_Stream* streams, size_t stream_count); +template <typename T> +inline void meshopt_optimizeVertexCache(T* destination, const T* indices, size_t index_count, size_t vertex_count); +template <typename T> +inline void meshopt_optimizeVertexCacheStrip(T* destination, const T* indices, size_t index_count, size_t vertex_count); +template <typename T> +inline void meshopt_optimizeVertexCacheFifo(T* destination, const T* indices, size_t index_count, size_t vertex_count, unsigned int cache_size); +template <typename T> +inline void meshopt_optimizeOverdraw(T* destination, const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, float threshold); +template <typename T> +inline size_t meshopt_optimizeVertexFetchRemap(unsigned int* destination, const T* indices, size_t index_count, size_t vertex_count); +template <typename T> +inline size_t meshopt_optimizeVertexFetch(void* destination, T* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size); +template <typename T> +inline size_t meshopt_encodeIndexBuffer(unsigned char* buffer, size_t buffer_size, const T* indices, size_t index_count); +template <typename T> +inline int meshopt_decodeIndexBuffer(T* destination, size_t index_count, const unsigned char* buffer, size_t buffer_size); +template <typename T> +inline size_t meshopt_encodeIndexSequence(unsigned char* buffer, size_t buffer_size, const T* indices, size_t index_count); +template <typename T> +inline int meshopt_decodeIndexSequence(T* destination, size_t index_count, const unsigned char* buffer, size_t buffer_size); +template <typename T> +inline size_t meshopt_simplify(T* destination, const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); +template <typename T> +inline size_t meshopt_simplifySloppy(T* destination, const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count); +template <typename T> +inline size_t meshopt_stripify(T* destination, const T* indices, size_t index_count, size_t vertex_count, T restart_index); +template <typename T> +inline size_t meshopt_unstripify(T* destination, const T* indices, size_t index_count, T restart_index); +template <typename T> +inline meshopt_VertexCacheStatistics meshopt_analyzeVertexCache(const T* indices, size_t index_count, size_t vertex_count, unsigned int cache_size, unsigned int warp_size, unsigned int buffer_size); +template <typename T> +inline meshopt_OverdrawStatistics meshopt_analyzeOverdraw(const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride); +template <typename T> +inline meshopt_VertexFetchStatistics meshopt_analyzeVertexFetch(const T* indices, size_t index_count, size_t vertex_count, size_t vertex_size); +template <typename T> +inline size_t meshopt_buildMeshlets(meshopt_Meshlet* destination, const T* indices, size_t index_count, size_t vertex_count, size_t max_vertices, size_t max_triangles); +template <typename T> +inline meshopt_Bounds meshopt_computeClusterBounds(const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride); +template <typename T> +inline void meshopt_spatialSortTriangles(T* destination, const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride); +#endif + +/* Inline implementation */ +#ifdef __cplusplus +inline int meshopt_quantizeUnorm(float v, int N) +{ + const float scale = float((1 << N) - 1); + + v = (v >= 0) ? v : 0; + v = (v <= 1) ? v : 1; + + return int(v * scale + 0.5f); +} + +inline int meshopt_quantizeSnorm(float v, int N) +{ + const float scale = float((1 << (N - 1)) - 1); + + float round = (v >= 0 ? 0.5f : -0.5f); + + v = (v >= -1) ? v : -1; + v = (v <= +1) ? v : +1; + + return int(v * scale + round); +} + +inline unsigned short meshopt_quantizeHalf(float v) +{ + union { float f; unsigned int ui; } u = {v}; + unsigned int ui = u.ui; + + int s = (ui >> 16) & 0x8000; + int em = ui & 0x7fffffff; + + /* bias exponent and round to nearest; 112 is relative exponent bias (127-15) */ + int h = (em - (112 << 23) + (1 << 12)) >> 13; + + /* underflow: flush to zero; 113 encodes exponent -14 */ + h = (em < (113 << 23)) ? 0 : h; + + /* overflow: infinity; 143 encodes exponent 16 */ + h = (em >= (143 << 23)) ? 0x7c00 : h; + + /* NaN; note that we convert all types of NaN to qNaN */ + h = (em > (255 << 23)) ? 0x7e00 : h; + + return (unsigned short)(s | h); +} + +inline float meshopt_quantizeFloat(float v, int N) +{ + union { float f; unsigned int ui; } u = {v}; + unsigned int ui = u.ui; + + const int mask = (1 << (23 - N)) - 1; + const int round = (1 << (23 - N)) >> 1; + + int e = ui & 0x7f800000; + unsigned int rui = (ui + round) & ~mask; + + /* round all numbers except inf/nan; this is important to make sure nan doesn't overflow into -0 */ + ui = e == 0x7f800000 ? ui : rui; + + /* flush denormals to zero */ + ui = e == 0 ? 0 : ui; + + u.ui = ui; + return u.f; +} +#endif + +/* Internal implementation helpers */ +#ifdef __cplusplus +class meshopt_Allocator +{ +public: + template <typename T> + struct StorageT + { + static void* (*allocate)(size_t); + static void (*deallocate)(void*); + }; + + typedef StorageT<void> Storage; + + meshopt_Allocator() + : blocks() + , count(0) + { + } + + ~meshopt_Allocator() + { + for (size_t i = count; i > 0; --i) + Storage::deallocate(blocks[i - 1]); + } + + template <typename T> T* allocate(size_t size) + { + assert(count < sizeof(blocks) / sizeof(blocks[0])); + T* result = static_cast<T*>(Storage::allocate(size > size_t(-1) / sizeof(T) ? size_t(-1) : size * sizeof(T))); + blocks[count++] = result; + return result; + } + +private: + void* blocks[24]; + size_t count; +}; + +// This makes sure that allocate/deallocate are lazily generated in translation units that need them and are deduplicated by the linker +template <typename T> void* (*meshopt_Allocator::StorageT<T>::allocate)(size_t) = operator new; +template <typename T> void (*meshopt_Allocator::StorageT<T>::deallocate)(void*) = operator delete; +#endif + +/* Inline implementation for C++ templated wrappers */ +#if defined(__cplusplus) && !defined(MESHOPTIMIZER_NO_WRAPPERS) +template <typename T, bool ZeroCopy = sizeof(T) == sizeof(unsigned int)> +struct meshopt_IndexAdapter; + +template <typename T> +struct meshopt_IndexAdapter<T, false> +{ + T* result; + unsigned int* data; + size_t count; + + meshopt_IndexAdapter(T* result_, const T* input, size_t count_) + : result(result_) + , data(0) + , count(count_) + { + size_t size = count > size_t(-1) / sizeof(unsigned int) ? size_t(-1) : count * sizeof(unsigned int); + + data = static_cast<unsigned int*>(meshopt_Allocator::Storage::allocate(size)); + + if (input) + { + for (size_t i = 0; i < count; ++i) + data[i] = input[i]; + } + } + + ~meshopt_IndexAdapter() + { + if (result) + { + for (size_t i = 0; i < count; ++i) + result[i] = T(data[i]); + } + + meshopt_Allocator::Storage::deallocate(data); + } +}; + +template <typename T> +struct meshopt_IndexAdapter<T, true> +{ + unsigned int* data; + + meshopt_IndexAdapter(T* result, const T* input, size_t) + : data(reinterpret_cast<unsigned int*>(result ? result : const_cast<T*>(input))) + { + } +}; + +template <typename T> +inline size_t meshopt_generateVertexRemap(unsigned int* destination, const T* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size) +{ + meshopt_IndexAdapter<T> in(0, indices, indices ? index_count : 0); + + return meshopt_generateVertexRemap(destination, indices ? in.data : 0, index_count, vertices, vertex_count, vertex_size); +} + +template <typename T> +inline size_t meshopt_generateVertexRemapMulti(unsigned int* destination, const T* indices, size_t index_count, size_t vertex_count, const meshopt_Stream* streams, size_t stream_count) +{ + meshopt_IndexAdapter<T> in(0, indices, indices ? index_count : 0); + + return meshopt_generateVertexRemapMulti(destination, indices ? in.data : 0, index_count, vertex_count, streams, stream_count); +} + +template <typename T> +inline void meshopt_remapIndexBuffer(T* destination, const T* indices, size_t index_count, const unsigned int* remap) +{ + meshopt_IndexAdapter<T> in(0, indices, indices ? index_count : 0); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + meshopt_remapIndexBuffer(out.data, indices ? in.data : 0, index_count, remap); +} + +template <typename T> +inline void meshopt_generateShadowIndexBuffer(T* destination, const T* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size, size_t vertex_stride) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + meshopt_generateShadowIndexBuffer(out.data, in.data, index_count, vertices, vertex_count, vertex_size, vertex_stride); +} + +template <typename T> +inline void meshopt_generateShadowIndexBufferMulti(T* destination, const T* indices, size_t index_count, size_t vertex_count, const meshopt_Stream* streams, size_t stream_count) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + meshopt_generateShadowIndexBufferMulti(out.data, in.data, index_count, vertex_count, streams, stream_count); +} + +template <typename T> +inline void meshopt_optimizeVertexCache(T* destination, const T* indices, size_t index_count, size_t vertex_count) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + meshopt_optimizeVertexCache(out.data, in.data, index_count, vertex_count); +} + +template <typename T> +inline void meshopt_optimizeVertexCacheStrip(T* destination, const T* indices, size_t index_count, size_t vertex_count) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + meshopt_optimizeVertexCacheStrip(out.data, in.data, index_count, vertex_count); +} + +template <typename T> +inline void meshopt_optimizeVertexCacheFifo(T* destination, const T* indices, size_t index_count, size_t vertex_count, unsigned int cache_size) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + meshopt_optimizeVertexCacheFifo(out.data, in.data, index_count, vertex_count, cache_size); +} + +template <typename T> +inline void meshopt_optimizeOverdraw(T* destination, const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, float threshold) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + meshopt_optimizeOverdraw(out.data, in.data, index_count, vertex_positions, vertex_count, vertex_positions_stride, threshold); +} + +template <typename T> +inline size_t meshopt_optimizeVertexFetchRemap(unsigned int* destination, const T* indices, size_t index_count, size_t vertex_count) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + + return meshopt_optimizeVertexFetchRemap(destination, in.data, index_count, vertex_count); +} + +template <typename T> +inline size_t meshopt_optimizeVertexFetch(void* destination, T* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size) +{ + meshopt_IndexAdapter<T> inout(indices, indices, index_count); + + return meshopt_optimizeVertexFetch(destination, inout.data, index_count, vertices, vertex_count, vertex_size); +} + +template <typename T> +inline size_t meshopt_encodeIndexBuffer(unsigned char* buffer, size_t buffer_size, const T* indices, size_t index_count) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + + return meshopt_encodeIndexBuffer(buffer, buffer_size, in.data, index_count); +} + +template <typename T> +inline int meshopt_decodeIndexBuffer(T* destination, size_t index_count, const unsigned char* buffer, size_t buffer_size) +{ + char index_size_valid[sizeof(T) == 2 || sizeof(T) == 4 ? 1 : -1]; + (void)index_size_valid; + + return meshopt_decodeIndexBuffer(destination, index_count, sizeof(T), buffer, buffer_size); +} + +template <typename T> +inline size_t meshopt_encodeIndexSequence(unsigned char* buffer, size_t buffer_size, const T* indices, size_t index_count) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + + return meshopt_encodeIndexSequence(buffer, buffer_size, in.data, index_count); +} + +template <typename T> +inline int meshopt_decodeIndexSequence(T* destination, size_t index_count, const unsigned char* buffer, size_t buffer_size) +{ + char index_size_valid[sizeof(T) == 2 || sizeof(T) == 4 ? 1 : -1]; + (void)index_size_valid; + + return meshopt_decodeIndexSequence(destination, index_count, sizeof(T), buffer, buffer_size); +} + +template <typename T> +inline size_t meshopt_simplify(T* destination, const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + return meshopt_simplify(out.data, in.data, index_count, vertex_positions, vertex_count, vertex_positions_stride, target_index_count, target_error); +} + +template <typename T> +inline size_t meshopt_simplifySloppy(T* destination, const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, target_index_count); + + return meshopt_simplifySloppy(out.data, in.data, index_count, vertex_positions, vertex_count, vertex_positions_stride, target_index_count); +} + +template <typename T> +inline size_t meshopt_stripify(T* destination, const T* indices, size_t index_count, size_t vertex_count, T restart_index) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, (index_count / 3) * 5); + + return meshopt_stripify(out.data, in.data, index_count, vertex_count, unsigned(restart_index)); +} + +template <typename T> +inline size_t meshopt_unstripify(T* destination, const T* indices, size_t index_count, T restart_index) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, (index_count - 2) * 3); + + return meshopt_unstripify(out.data, in.data, index_count, unsigned(restart_index)); +} + +template <typename T> +inline meshopt_VertexCacheStatistics meshopt_analyzeVertexCache(const T* indices, size_t index_count, size_t vertex_count, unsigned int cache_size, unsigned int warp_size, unsigned int buffer_size) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + + return meshopt_analyzeVertexCache(in.data, index_count, vertex_count, cache_size, warp_size, buffer_size); +} + +template <typename T> +inline meshopt_OverdrawStatistics meshopt_analyzeOverdraw(const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + + return meshopt_analyzeOverdraw(in.data, index_count, vertex_positions, vertex_count, vertex_positions_stride); +} + +template <typename T> +inline meshopt_VertexFetchStatistics meshopt_analyzeVertexFetch(const T* indices, size_t index_count, size_t vertex_count, size_t vertex_size) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + + return meshopt_analyzeVertexFetch(in.data, index_count, vertex_count, vertex_size); +} + +template <typename T> +inline size_t meshopt_buildMeshlets(meshopt_Meshlet* destination, const T* indices, size_t index_count, size_t vertex_count, size_t max_vertices, size_t max_triangles) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + + return meshopt_buildMeshlets(destination, in.data, index_count, vertex_count, max_vertices, max_triangles); +} + +template <typename T> +inline meshopt_Bounds meshopt_computeClusterBounds(const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + + return meshopt_computeClusterBounds(in.data, index_count, vertex_positions, vertex_count, vertex_positions_stride); +} + +template <typename T> +inline void meshopt_spatialSortTriangles(T* destination, const T* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride) +{ + meshopt_IndexAdapter<T> in(0, indices, index_count); + meshopt_IndexAdapter<T> out(destination, 0, index_count); + + meshopt_spatialSortTriangles(out.data, in.data, index_count, vertex_positions, vertex_count, vertex_positions_stride); +} +#endif + +/** + * Copyright (c) 2016-2020 Arseny Kapoulkine + * + * 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 AUTHORS OR COPYRIGHT + * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, + * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR + * OTHER DEALINGS IN THE SOFTWARE. + */ diff --git a/thirdparty/meshoptimizer/overdrawanalyzer.cpp b/thirdparty/meshoptimizer/overdrawanalyzer.cpp new file mode 100644 index 0000000000..8d5859ba39 --- /dev/null +++ b/thirdparty/meshoptimizer/overdrawanalyzer.cpp @@ -0,0 +1,230 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <float.h> +#include <string.h> + +// This work is based on: +// Nicolas Capens. Advanced Rasterization. 2004 +namespace meshopt +{ + +const int kViewport = 256; + +struct OverdrawBuffer +{ + float z[kViewport][kViewport][2]; + unsigned int overdraw[kViewport][kViewport][2]; +}; + +#ifndef min +#define min(a, b) ((a) < (b) ? (a) : (b)) +#endif + +#ifndef max +#define max(a, b) ((a) > (b) ? (a) : (b)) +#endif + +static float computeDepthGradients(float& dzdx, float& dzdy, float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3) +{ + // z2 = z1 + dzdx * (x2 - x1) + dzdy * (y2 - y1) + // z3 = z1 + dzdx * (x3 - x1) + dzdy * (y3 - y1) + // (x2-x1 y2-y1)(dzdx) = (z2-z1) + // (x3-x1 y3-y1)(dzdy) (z3-z1) + // we'll solve it with Cramer's rule + float det = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1); + float invdet = (det == 0) ? 0 : 1 / det; + + dzdx = (z2 - z1) * (y3 - y1) - (y2 - y1) * (z3 - z1) * invdet; + dzdy = (x2 - x1) * (z3 - z1) - (z2 - z1) * (x3 - x1) * invdet; + + return det; +} + +// half-space fixed point triangle rasterizer +static void rasterize(OverdrawBuffer* buffer, float v1x, float v1y, float v1z, float v2x, float v2y, float v2z, float v3x, float v3y, float v3z) +{ + // compute depth gradients + float DZx, DZy; + float det = computeDepthGradients(DZx, DZy, v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z); + int sign = det > 0; + + // flip backfacing triangles to simplify rasterization logic + if (sign) + { + // flipping v2 & v3 preserves depth gradients since they're based on v1 + float t; + t = v2x, v2x = v3x, v3x = t; + t = v2y, v2y = v3y, v3y = t; + t = v2z, v2z = v3z, v3z = t; + + // flip depth since we rasterize backfacing triangles to second buffer with reverse Z; only v1z is used below + v1z = kViewport - v1z; + DZx = -DZx; + DZy = -DZy; + } + + // coordinates, 28.4 fixed point + int X1 = int(16.0f * v1x + 0.5f); + int X2 = int(16.0f * v2x + 0.5f); + int X3 = int(16.0f * v3x + 0.5f); + + int Y1 = int(16.0f * v1y + 0.5f); + int Y2 = int(16.0f * v2y + 0.5f); + int Y3 = int(16.0f * v3y + 0.5f); + + // bounding rectangle, clipped against viewport + // since we rasterize pixels with covered centers, min >0.5 should round up + // as for max, due to top-left filling convention we will never rasterize right/bottom edges + // so max >= 0.5 should round down + int minx = max((min(X1, min(X2, X3)) + 7) >> 4, 0); + int maxx = min((max(X1, max(X2, X3)) + 7) >> 4, kViewport); + int miny = max((min(Y1, min(Y2, Y3)) + 7) >> 4, 0); + int maxy = min((max(Y1, max(Y2, Y3)) + 7) >> 4, kViewport); + + // deltas, 28.4 fixed point + int DX12 = X1 - X2; + int DX23 = X2 - X3; + int DX31 = X3 - X1; + + int DY12 = Y1 - Y2; + int DY23 = Y2 - Y3; + int DY31 = Y3 - Y1; + + // fill convention correction + int TL1 = DY12 < 0 || (DY12 == 0 && DX12 > 0); + int TL2 = DY23 < 0 || (DY23 == 0 && DX23 > 0); + int TL3 = DY31 < 0 || (DY31 == 0 && DX31 > 0); + + // half edge equations, 24.8 fixed point + // note that we offset minx/miny by half pixel since we want to rasterize pixels with covered centers + int FX = (minx << 4) + 8; + int FY = (miny << 4) + 8; + int CY1 = DX12 * (FY - Y1) - DY12 * (FX - X1) + TL1 - 1; + int CY2 = DX23 * (FY - Y2) - DY23 * (FX - X2) + TL2 - 1; + int CY3 = DX31 * (FY - Y3) - DY31 * (FX - X3) + TL3 - 1; + float ZY = v1z + (DZx * float(FX - X1) + DZy * float(FY - Y1)) * (1 / 16.f); + + for (int y = miny; y < maxy; y++) + { + int CX1 = CY1; + int CX2 = CY2; + int CX3 = CY3; + float ZX = ZY; + + for (int x = minx; x < maxx; x++) + { + // check if all CXn are non-negative + if ((CX1 | CX2 | CX3) >= 0) + { + if (ZX >= buffer->z[y][x][sign]) + { + buffer->z[y][x][sign] = ZX; + buffer->overdraw[y][x][sign]++; + } + } + + // signed left shift is UB for negative numbers so use unsigned-signed casts + CX1 -= int(unsigned(DY12) << 4); + CX2 -= int(unsigned(DY23) << 4); + CX3 -= int(unsigned(DY31) << 4); + ZX += DZx; + } + + // signed left shift is UB for negative numbers so use unsigned-signed casts + CY1 += int(unsigned(DX12) << 4); + CY2 += int(unsigned(DX23) << 4); + CY3 += int(unsigned(DX31) << 4); + ZY += DZy; + } +} + +} // namespace meshopt + +meshopt_OverdrawStatistics meshopt_analyzeOverdraw(const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + + meshopt_Allocator allocator; + + size_t vertex_stride_float = vertex_positions_stride / sizeof(float); + + meshopt_OverdrawStatistics result = {}; + + float minv[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + float maxv[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + + for (size_t i = 0; i < vertex_count; ++i) + { + const float* v = vertex_positions + i * vertex_stride_float; + + for (int j = 0; j < 3; ++j) + { + minv[j] = min(minv[j], v[j]); + maxv[j] = max(maxv[j], v[j]); + } + } + + float extent = max(maxv[0] - minv[0], max(maxv[1] - minv[1], maxv[2] - minv[2])); + float scale = kViewport / extent; + + float* triangles = allocator.allocate<float>(index_count * 3); + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices[i]; + assert(index < vertex_count); + + const float* v = vertex_positions + index * vertex_stride_float; + + triangles[i * 3 + 0] = (v[0] - minv[0]) * scale; + triangles[i * 3 + 1] = (v[1] - minv[1]) * scale; + triangles[i * 3 + 2] = (v[2] - minv[2]) * scale; + } + + OverdrawBuffer* buffer = allocator.allocate<OverdrawBuffer>(1); + + for (int axis = 0; axis < 3; ++axis) + { + memset(buffer, 0, sizeof(OverdrawBuffer)); + + for (size_t i = 0; i < index_count; i += 3) + { + const float* vn0 = &triangles[3 * (i + 0)]; + const float* vn1 = &triangles[3 * (i + 1)]; + const float* vn2 = &triangles[3 * (i + 2)]; + + switch (axis) + { + case 0: + rasterize(buffer, vn0[2], vn0[1], vn0[0], vn1[2], vn1[1], vn1[0], vn2[2], vn2[1], vn2[0]); + break; + case 1: + rasterize(buffer, vn0[0], vn0[2], vn0[1], vn1[0], vn1[2], vn1[1], vn2[0], vn2[2], vn2[1]); + break; + case 2: + rasterize(buffer, vn0[1], vn0[0], vn0[2], vn1[1], vn1[0], vn1[2], vn2[1], vn2[0], vn2[2]); + break; + } + } + + for (int y = 0; y < kViewport; ++y) + for (int x = 0; x < kViewport; ++x) + for (int s = 0; s < 2; ++s) + { + unsigned int overdraw = buffer->overdraw[y][x][s]; + + result.pixels_covered += overdraw > 0; + result.pixels_shaded += overdraw; + } + } + + result.overdraw = result.pixels_covered ? float(result.pixels_shaded) / float(result.pixels_covered) : 0.f; + + return result; +} diff --git a/thirdparty/meshoptimizer/overdrawoptimizer.cpp b/thirdparty/meshoptimizer/overdrawoptimizer.cpp new file mode 100644 index 0000000000..143656ed76 --- /dev/null +++ b/thirdparty/meshoptimizer/overdrawoptimizer.cpp @@ -0,0 +1,333 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <math.h> +#include <string.h> + +// This work is based on: +// Pedro Sander, Diego Nehab and Joshua Barczak. Fast Triangle Reordering for Vertex Locality and Reduced Overdraw. 2007 +namespace meshopt +{ + +static void calculateSortData(float* sort_data, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_positions_stride, const unsigned int* clusters, size_t cluster_count) +{ + size_t vertex_stride_float = vertex_positions_stride / sizeof(float); + + float mesh_centroid[3] = {}; + + for (size_t i = 0; i < index_count; ++i) + { + const float* p = vertex_positions + vertex_stride_float * indices[i]; + + mesh_centroid[0] += p[0]; + mesh_centroid[1] += p[1]; + mesh_centroid[2] += p[2]; + } + + mesh_centroid[0] /= index_count; + mesh_centroid[1] /= index_count; + mesh_centroid[2] /= index_count; + + for (size_t cluster = 0; cluster < cluster_count; ++cluster) + { + size_t cluster_begin = clusters[cluster] * 3; + size_t cluster_end = (cluster + 1 < cluster_count) ? clusters[cluster + 1] * 3 : index_count; + assert(cluster_begin < cluster_end); + + float cluster_area = 0; + float cluster_centroid[3] = {}; + float cluster_normal[3] = {}; + + for (size_t i = cluster_begin; i < cluster_end; i += 3) + { + const float* p0 = vertex_positions + vertex_stride_float * indices[i + 0]; + const float* p1 = vertex_positions + vertex_stride_float * indices[i + 1]; + const float* p2 = vertex_positions + vertex_stride_float * indices[i + 2]; + + float p10[3] = {p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]}; + float p20[3] = {p2[0] - p0[0], p2[1] - p0[1], p2[2] - p0[2]}; + + float normalx = p10[1] * p20[2] - p10[2] * p20[1]; + float normaly = p10[2] * p20[0] - p10[0] * p20[2]; + float normalz = p10[0] * p20[1] - p10[1] * p20[0]; + + float area = sqrtf(normalx * normalx + normaly * normaly + normalz * normalz); + + cluster_centroid[0] += (p0[0] + p1[0] + p2[0]) * (area / 3); + cluster_centroid[1] += (p0[1] + p1[1] + p2[1]) * (area / 3); + cluster_centroid[2] += (p0[2] + p1[2] + p2[2]) * (area / 3); + cluster_normal[0] += normalx; + cluster_normal[1] += normaly; + cluster_normal[2] += normalz; + cluster_area += area; + } + + float inv_cluster_area = cluster_area == 0 ? 0 : 1 / cluster_area; + + cluster_centroid[0] *= inv_cluster_area; + cluster_centroid[1] *= inv_cluster_area; + cluster_centroid[2] *= inv_cluster_area; + + float cluster_normal_length = sqrtf(cluster_normal[0] * cluster_normal[0] + cluster_normal[1] * cluster_normal[1] + cluster_normal[2] * cluster_normal[2]); + float inv_cluster_normal_length = cluster_normal_length == 0 ? 0 : 1 / cluster_normal_length; + + cluster_normal[0] *= inv_cluster_normal_length; + cluster_normal[1] *= inv_cluster_normal_length; + cluster_normal[2] *= inv_cluster_normal_length; + + float centroid_vector[3] = {cluster_centroid[0] - mesh_centroid[0], cluster_centroid[1] - mesh_centroid[1], cluster_centroid[2] - mesh_centroid[2]}; + + sort_data[cluster] = centroid_vector[0] * cluster_normal[0] + centroid_vector[1] * cluster_normal[1] + centroid_vector[2] * cluster_normal[2]; + } +} + +static void calculateSortOrderRadix(unsigned int* sort_order, const float* sort_data, unsigned short* sort_keys, size_t cluster_count) +{ + // compute sort data bounds and renormalize, using fixed point snorm + float sort_data_max = 1e-3f; + + for (size_t i = 0; i < cluster_count; ++i) + { + float dpa = fabsf(sort_data[i]); + + sort_data_max = (sort_data_max < dpa) ? dpa : sort_data_max; + } + + const int sort_bits = 11; + + for (size_t i = 0; i < cluster_count; ++i) + { + // note that we flip distribution since high dot product should come first + float sort_key = 0.5f - 0.5f * (sort_data[i] / sort_data_max); + + sort_keys[i] = meshopt_quantizeUnorm(sort_key, sort_bits) & ((1 << sort_bits) - 1); + } + + // fill histogram for counting sort + unsigned int histogram[1 << sort_bits]; + memset(histogram, 0, sizeof(histogram)); + + for (size_t i = 0; i < cluster_count; ++i) + { + histogram[sort_keys[i]]++; + } + + // compute offsets based on histogram data + size_t histogram_sum = 0; + + for (size_t i = 0; i < 1 << sort_bits; ++i) + { + size_t count = histogram[i]; + histogram[i] = unsigned(histogram_sum); + histogram_sum += count; + } + + assert(histogram_sum == cluster_count); + + // compute sort order based on offsets + for (size_t i = 0; i < cluster_count; ++i) + { + sort_order[histogram[sort_keys[i]]++] = unsigned(i); + } +} + +static unsigned int updateCache(unsigned int a, unsigned int b, unsigned int c, unsigned int cache_size, unsigned int* cache_timestamps, unsigned int& timestamp) +{ + unsigned int cache_misses = 0; + + // if vertex is not in cache, put it in cache + if (timestamp - cache_timestamps[a] > cache_size) + { + cache_timestamps[a] = timestamp++; + cache_misses++; + } + + if (timestamp - cache_timestamps[b] > cache_size) + { + cache_timestamps[b] = timestamp++; + cache_misses++; + } + + if (timestamp - cache_timestamps[c] > cache_size) + { + cache_timestamps[c] = timestamp++; + cache_misses++; + } + + return cache_misses; +} + +static size_t generateHardBoundaries(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, unsigned int cache_size, unsigned int* cache_timestamps) +{ + memset(cache_timestamps, 0, vertex_count * sizeof(unsigned int)); + + unsigned int timestamp = cache_size + 1; + + size_t face_count = index_count / 3; + + size_t result = 0; + + for (size_t i = 0; i < face_count; ++i) + { + unsigned int m = updateCache(indices[i * 3 + 0], indices[i * 3 + 1], indices[i * 3 + 2], cache_size, &cache_timestamps[0], timestamp); + + // when all three vertices are not in the cache it's usually relatively safe to assume that this is a new patch in the mesh + // that is disjoint from previous vertices; sometimes it might come back to reference existing vertices but that frequently + // suggests an inefficiency in the vertex cache optimization algorithm + // usually the first triangle has 3 misses unless it's degenerate - thus we make sure the first cluster always starts with 0 + if (i == 0 || m == 3) + { + destination[result++] = unsigned(i); + } + } + + assert(result <= index_count / 3); + + return result; +} + +static size_t generateSoftBoundaries(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, const unsigned int* clusters, size_t cluster_count, unsigned int cache_size, float threshold, unsigned int* cache_timestamps) +{ + memset(cache_timestamps, 0, vertex_count * sizeof(unsigned int)); + + unsigned int timestamp = 0; + + size_t result = 0; + + for (size_t it = 0; it < cluster_count; ++it) + { + size_t start = clusters[it]; + size_t end = (it + 1 < cluster_count) ? clusters[it + 1] : index_count / 3; + assert(start < end); + + // reset cache + timestamp += cache_size + 1; + + // measure cluster ACMR + unsigned int cluster_misses = 0; + + for (size_t i = start; i < end; ++i) + { + unsigned int m = updateCache(indices[i * 3 + 0], indices[i * 3 + 1], indices[i * 3 + 2], cache_size, &cache_timestamps[0], timestamp); + + cluster_misses += m; + } + + float cluster_threshold = threshold * (float(cluster_misses) / float(end - start)); + + // first cluster always starts from the hard cluster boundary + destination[result++] = unsigned(start); + + // reset cache + timestamp += cache_size + 1; + + unsigned int running_misses = 0; + unsigned int running_faces = 0; + + for (size_t i = start; i < end; ++i) + { + unsigned int m = updateCache(indices[i * 3 + 0], indices[i * 3 + 1], indices[i * 3 + 2], cache_size, &cache_timestamps[0], timestamp); + + running_misses += m; + running_faces += 1; + + if (float(running_misses) / float(running_faces) <= cluster_threshold) + { + // we have reached the target ACMR with the current triangle so we need to start a new cluster on the next one + // note that this may mean that we add 'end` to destination for the last triangle, which will imply that the last + // cluster is empty; however, the 'pop_back' after the loop will clean it up + destination[result++] = unsigned(i + 1); + + // reset cache + timestamp += cache_size + 1; + + running_misses = 0; + running_faces = 0; + } + } + + // each time we reach the target ACMR we flush the cluster + // this means that the last cluster is by definition not very good - there are frequent cases where we are left with a few triangles + // in the last cluster, producing a very bad ACMR and significantly penalizing the overall results + // thus we remove the last cluster boundary, merging the last complete cluster with the last incomplete one + // there are sometimes cases when the last cluster is actually good enough - in which case the code above would have added 'end' + // to the cluster boundary array which we need to remove anyway - this code will do that automatically + if (destination[result - 1] != start) + { + result--; + } + } + + assert(result >= cluster_count); + assert(result <= index_count / 3); + + return result; +} + +} // namespace meshopt + +void meshopt_optimizeOverdraw(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, float threshold) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + + meshopt_Allocator allocator; + + // guard for empty meshes + if (index_count == 0 || vertex_count == 0) + return; + + // support in-place optimization + if (destination == indices) + { + unsigned int* indices_copy = allocator.allocate<unsigned int>(index_count); + memcpy(indices_copy, indices, index_count * sizeof(unsigned int)); + indices = indices_copy; + } + + unsigned int cache_size = 16; + + unsigned int* cache_timestamps = allocator.allocate<unsigned int>(vertex_count); + + // generate hard boundaries from full-triangle cache misses + unsigned int* hard_clusters = allocator.allocate<unsigned int>(index_count / 3); + size_t hard_cluster_count = generateHardBoundaries(hard_clusters, indices, index_count, vertex_count, cache_size, cache_timestamps); + + // generate soft boundaries + unsigned int* soft_clusters = allocator.allocate<unsigned int>(index_count / 3 + 1); + size_t soft_cluster_count = generateSoftBoundaries(soft_clusters, indices, index_count, vertex_count, hard_clusters, hard_cluster_count, cache_size, threshold, cache_timestamps); + + const unsigned int* clusters = soft_clusters; + size_t cluster_count = soft_cluster_count; + + // fill sort data + float* sort_data = allocator.allocate<float>(cluster_count); + calculateSortData(sort_data, indices, index_count, vertex_positions, vertex_positions_stride, clusters, cluster_count); + + // sort clusters using sort data + unsigned short* sort_keys = allocator.allocate<unsigned short>(cluster_count); + unsigned int* sort_order = allocator.allocate<unsigned int>(cluster_count); + calculateSortOrderRadix(sort_order, sort_data, sort_keys, cluster_count); + + // fill output buffer + size_t offset = 0; + + for (size_t it = 0; it < cluster_count; ++it) + { + unsigned int cluster = sort_order[it]; + assert(cluster < cluster_count); + + size_t cluster_begin = clusters[cluster] * 3; + size_t cluster_end = (cluster + 1 < cluster_count) ? clusters[cluster + 1] * 3 : index_count; + assert(cluster_begin < cluster_end); + + memcpy(destination + offset, indices + cluster_begin, (cluster_end - cluster_begin) * sizeof(unsigned int)); + offset += cluster_end - cluster_begin; + } + + assert(offset == index_count); +} diff --git a/thirdparty/meshoptimizer/patches/simplifier_get_resulting_error.patch b/thirdparty/meshoptimizer/patches/simplifier_get_resulting_error.patch new file mode 100644 index 0000000000..1be38e45d2 --- /dev/null +++ b/thirdparty/meshoptimizer/patches/simplifier_get_resulting_error.patch @@ -0,0 +1,96 @@ +diff --git a/thirdparty/meshoptimizer/meshoptimizer.h b/thirdparty/meshoptimizer/meshoptimizer.h +index a442d103c8..fde00f9c82 100644 +--- a/thirdparty/meshoptimizer/meshoptimizer.h ++++ b/thirdparty/meshoptimizer/meshoptimizer.h +@@ -266,7 +266,10 @@ MESHOPTIMIZER_EXPERIMENTAL void meshopt_decodeFilterExp(void* buffer, size_t ver + * destination must contain enough space for the *source* index buffer (since optimization is iterative, this means index_count elements - *not* target_index_count!) + * vertex_positions should have float3 position in the first 12 bytes of each vertex - similar to glVertexPointer + */ +-MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); ++// -- GODOT start -- ++//MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error); ++MESHOPTIMIZER_EXPERIMENTAL size_t meshopt_simplify(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_resulting_error); ++// -- GODOT end -- + + /** + * Experimental: Mesh simplifier (sloppy) +diff --git a/thirdparty/meshoptimizer/simplifier.cpp b/thirdparty/meshoptimizer/simplifier.cpp +index bd523275ce..51cf634186 100644 +--- a/thirdparty/meshoptimizer/simplifier.cpp ++++ b/thirdparty/meshoptimizer/simplifier.cpp +@@ -1143,7 +1143,10 @@ unsigned int* meshopt_simplifyDebugLoop = 0; + unsigned int* meshopt_simplifyDebugLoopBack = 0; + #endif + +-size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error) ++// -- GODOT start -- ++//size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error) ++size_t meshopt_simplify(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_resulting_error) ++// -- GODOT end -- + { + using namespace meshopt; + +@@ -1198,10 +1201,13 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, + if (result != indices) + memcpy(result, indices, index_count * sizeof(unsigned int)); + ++// -- GODOT start -- + #if TRACE + size_t pass_count = 0; +- float worst_error = 0; ++ //float worst_error = 0; + #endif ++ float worst_error = 0; ++// -- GODOT end -- + + Collapse* edge_collapses = allocator.allocate<Collapse>(index_count); + unsigned int* collapse_order = allocator.allocate<unsigned int>(index_count); +@@ -1213,6 +1219,12 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, + // target_error input is linear; we need to adjust it to match quadricError units + float error_limit = target_error * target_error; + ++// -- GODOT start -- ++ if (r_resulting_error) { ++ *r_resulting_error = 1.0; ++ } ++// -- GODOT end -- ++ + while (result_count > target_index_count) + { + size_t edge_collapse_count = pickEdgeCollapses(edge_collapses, result, result_count, remap, vertex_kind, loop); +@@ -1257,7 +1269,8 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, + size_t new_count = remapIndexBuffer(result, result_count, collapse_remap); + assert(new_count < result_count); + +-#if TRACE ++// -- GODOT start -- ++//#if TRACE + float pass_error = 0.f; + for (size_t i = 0; i < edge_collapse_count; ++i) + { +@@ -1267,15 +1280,24 @@ size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, + pass_error = c.error; + } + +- pass_count++; ++ //pass_count++; + worst_error = (worst_error < pass_error) ? pass_error : worst_error; + ++#if TRACE ++ pass_count++; + printf("pass %d: triangles: %d -> %d, collapses: %d/%d (goal: %d), error: %e (limit %e goal %e)\n", int(pass_count), int(result_count / 3), int(new_count / 3), int(collapses), int(edge_collapse_count), int(edge_collapse_goal), pass_error, error_limit, error_goal); + #endif ++// -- GODOT end -- + + result_count = new_count; + } + ++// -- GODOT start -- ++ if (r_resulting_error) { ++ *r_resulting_error = sqrt(worst_error); ++ } ++// -- GODOT end -- ++ + #if TRACE + printf("passes: %d, worst error: %e\n", int(pass_count), worst_error); + #endif diff --git a/thirdparty/meshoptimizer/simplifier.cpp b/thirdparty/meshoptimizer/simplifier.cpp new file mode 100644 index 0000000000..b195a8cb5d --- /dev/null +++ b/thirdparty/meshoptimizer/simplifier.cpp @@ -0,0 +1,1562 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <float.h> +#include <math.h> +#include <string.h> + + +#ifndef TRACE +#define TRACE 0 +#endif + +#if TRACE +#include <stdio.h> +#endif + +// This work is based on: +// Michael Garland and Paul S. Heckbert. Surface simplification using quadric error metrics. 1997 +// Michael Garland. Quadric-based polygonal surface simplification. 1999 +// Peter Lindstrom. Out-of-Core Simplification of Large Polygonal Models. 2000 +// Matthias Teschner, Bruno Heidelberger, Matthias Mueller, Danat Pomeranets, Markus Gross. Optimized Spatial Hashing for Collision Detection of Deformable Objects. 2003 +// Peter Van Sandt, Yannis Chronis, Jignesh M. Patel. Efficiently Searching In-Memory Sorted Arrays: Revenge of the Interpolation Search? 2019 +namespace meshopt +{ + +struct EdgeAdjacency +{ + unsigned int* counts; + unsigned int* offsets; + unsigned int* data; +}; + +static void buildEdgeAdjacency(EdgeAdjacency& adjacency, const unsigned int* indices, size_t index_count, size_t vertex_count, meshopt_Allocator& allocator) +{ + size_t face_count = index_count / 3; + + // allocate arrays + adjacency.counts = allocator.allocate<unsigned int>(vertex_count); + adjacency.offsets = allocator.allocate<unsigned int>(vertex_count); + adjacency.data = allocator.allocate<unsigned int>(index_count); + + // fill edge counts + memset(adjacency.counts, 0, vertex_count * sizeof(unsigned int)); + + for (size_t i = 0; i < index_count; ++i) + { + assert(indices[i] < vertex_count); + + adjacency.counts[indices[i]]++; + } + + // fill offset table + unsigned int offset = 0; + + for (size_t i = 0; i < vertex_count; ++i) + { + adjacency.offsets[i] = offset; + offset += adjacency.counts[i]; + } + + assert(offset == index_count); + + // fill edge data + for (size_t i = 0; i < face_count; ++i) + { + unsigned int a = indices[i * 3 + 0], b = indices[i * 3 + 1], c = indices[i * 3 + 2]; + + adjacency.data[adjacency.offsets[a]++] = b; + adjacency.data[adjacency.offsets[b]++] = c; + adjacency.data[adjacency.offsets[c]++] = a; + } + + // fix offsets that have been disturbed by the previous pass + for (size_t i = 0; i < vertex_count; ++i) + { + assert(adjacency.offsets[i] >= adjacency.counts[i]); + + adjacency.offsets[i] -= adjacency.counts[i]; + } +} + +struct PositionHasher +{ + const float* vertex_positions; + size_t vertex_stride_float; + + size_t hash(unsigned int index) const + { + const unsigned int* key = reinterpret_cast<const unsigned int*>(vertex_positions + index * vertex_stride_float); + + // Optimized Spatial Hashing for Collision Detection of Deformable Objects + return (key[0] * 73856093) ^ (key[1] * 19349663) ^ (key[2] * 83492791); + } + + bool equal(unsigned int lhs, unsigned int rhs) const + { + return memcmp(vertex_positions + lhs * vertex_stride_float, vertex_positions + rhs * vertex_stride_float, sizeof(float) * 3) == 0; + } +}; + +static size_t hashBuckets2(size_t count) +{ + size_t buckets = 1; + while (buckets < count) + buckets *= 2; + + return buckets; +} + +template <typename T, typename Hash> +static T* hashLookup2(T* table, size_t buckets, const Hash& hash, const T& key, const T& empty) +{ + assert(buckets > 0); + assert((buckets & (buckets - 1)) == 0); + + size_t hashmod = buckets - 1; + size_t bucket = hash.hash(key) & hashmod; + + for (size_t probe = 0; probe <= hashmod; ++probe) + { + T& item = table[bucket]; + + if (item == empty) + return &item; + + if (hash.equal(item, key)) + return &item; + + // hash collision, quadratic probing + bucket = (bucket + probe + 1) & hashmod; + } + + assert(false && "Hash table is full"); // unreachable + return 0; +} + +static void buildPositionRemap(unsigned int* remap, unsigned int* wedge, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, meshopt_Allocator& allocator) +{ + PositionHasher hasher = {vertex_positions_data, vertex_positions_stride / sizeof(float)}; + + size_t table_size = hashBuckets2(vertex_count); + unsigned int* table = allocator.allocate<unsigned int>(table_size); + memset(table, -1, table_size * sizeof(unsigned int)); + + // build forward remap: for each vertex, which other (canonical) vertex does it map to? + // we use position equivalence for this, and remap vertices to other existing vertices + for (size_t i = 0; i < vertex_count; ++i) + { + unsigned int index = unsigned(i); + unsigned int* entry = hashLookup2(table, table_size, hasher, index, ~0u); + + if (*entry == ~0u) + *entry = index; + + remap[index] = *entry; + } + + // build wedge table: for each vertex, which other vertex is the next wedge that also maps to the same vertex? + // entries in table form a (cyclic) wedge loop per vertex; for manifold vertices, wedge[i] == remap[i] == i + for (size_t i = 0; i < vertex_count; ++i) + wedge[i] = unsigned(i); + + for (size_t i = 0; i < vertex_count; ++i) + if (remap[i] != i) + { + unsigned int r = remap[i]; + + wedge[i] = wedge[r]; + wedge[r] = unsigned(i); + } +} + +enum VertexKind +{ + Kind_Manifold, // not on an attribute seam, not on any boundary + Kind_Border, // not on an attribute seam, has exactly two open edges + Kind_Seam, // on an attribute seam with exactly two attribute seam edges + Kind_Complex, // none of the above; these vertices can move as long as all wedges move to the target vertex + Kind_Locked, // none of the above; these vertices can't move + + Kind_Count +}; + +// manifold vertices can collapse onto anything +// border/seam vertices can only be collapsed onto border/seam respectively +// complex vertices can collapse onto complex/locked +// a rule of thumb is that collapsing kind A into kind B preserves the kind B in the target vertex +// for example, while we could collapse Complex into Manifold, this would mean the target vertex isn't Manifold anymore +const unsigned char kCanCollapse[Kind_Count][Kind_Count] = { + {1, 1, 1, 1, 1}, + {0, 1, 0, 0, 0}, + {0, 0, 1, 0, 0}, + {0, 0, 0, 1, 1}, + {0, 0, 0, 0, 0}, +}; + +// if a vertex is manifold or seam, adjoining edges are guaranteed to have an opposite edge +// note that for seam edges, the opposite edge isn't present in the attribute-based topology +// but is present if you consider a position-only mesh variant +const unsigned char kHasOpposite[Kind_Count][Kind_Count] = { + {1, 1, 1, 0, 1}, + {1, 0, 1, 0, 0}, + {1, 1, 1, 0, 1}, + {0, 0, 0, 0, 0}, + {1, 0, 1, 0, 0}, +}; + +static bool hasEdge(const EdgeAdjacency& adjacency, unsigned int a, unsigned int b) +{ + unsigned int count = adjacency.counts[a]; + const unsigned int* data = adjacency.data + adjacency.offsets[a]; + + for (size_t i = 0; i < count; ++i) + if (data[i] == b) + return true; + + return false; +} + +static void classifyVertices(unsigned char* result, unsigned int* loop, unsigned int* loopback, size_t vertex_count, const EdgeAdjacency& adjacency, const unsigned int* remap, const unsigned int* wedge) +{ + memset(loop, -1, vertex_count * sizeof(unsigned int)); + memset(loopback, -1, vertex_count * sizeof(unsigned int)); + + // incoming & outgoing open edges: ~0u if no open edges, i if there are more than 1 + // note that this is the same data as required in loop[] arrays; loop[] data is only valid for border/seam + // but here it's okay to fill the data out for other types of vertices as well + unsigned int* openinc = loopback; + unsigned int* openout = loop; + + for (size_t i = 0; i < vertex_count; ++i) + { + unsigned int vertex = unsigned(i); + + unsigned int count = adjacency.counts[vertex]; + const unsigned int* data = adjacency.data + adjacency.offsets[vertex]; + + for (size_t j = 0; j < count; ++j) + { + unsigned int target = data[j]; + + if (!hasEdge(adjacency, target, vertex)) + { + openinc[target] = (openinc[target] == ~0u) ? vertex : target; + openout[vertex] = (openout[vertex] == ~0u) ? target : vertex; + } + } + } + +#if TRACE + size_t lockedstats[4] = {}; +#define TRACELOCKED(i) lockedstats[i]++; +#else +#define TRACELOCKED(i) (void)0 +#endif + + for (size_t i = 0; i < vertex_count; ++i) + { + if (remap[i] == i) + { + if (wedge[i] == i) + { + // no attribute seam, need to check if it's manifold + unsigned int openi = openinc[i], openo = openout[i]; + + // note: we classify any vertices with no open edges as manifold + // this is technically incorrect - if 4 triangles share an edge, we'll classify vertices as manifold + // it's unclear if this is a problem in practice + if (openi == ~0u && openo == ~0u) + { + result[i] = Kind_Manifold; + } + else if (openi != i && openo != i) + { + result[i] = Kind_Border; + } + else + { + result[i] = Kind_Locked; + TRACELOCKED(0); + } + } + else if (wedge[wedge[i]] == i) + { + // attribute seam; need to distinguish between Seam and Locked + unsigned int w = wedge[i]; + unsigned int openiv = openinc[i], openov = openout[i]; + unsigned int openiw = openinc[w], openow = openout[w]; + + // seam should have one open half-edge for each vertex, and the edges need to "connect" - point to the same vertex post-remap + if (openiv != ~0u && openiv != i && openov != ~0u && openov != i && + openiw != ~0u && openiw != w && openow != ~0u && openow != w) + { + if (remap[openiv] == remap[openow] && remap[openov] == remap[openiw]) + { + result[i] = Kind_Seam; + } + else + { + result[i] = Kind_Locked; + TRACELOCKED(1); + } + } + else + { + result[i] = Kind_Locked; + TRACELOCKED(2); + } + } + else + { + // more than one vertex maps to this one; we don't have classification available + result[i] = Kind_Locked; + TRACELOCKED(3); + } + } + else + { + assert(remap[i] < i); + + result[i] = result[remap[i]]; + } + } + +#if TRACE + printf("locked: many open edges %d, disconnected seam %d, many seam edges %d, many wedges %d\n", + int(lockedstats[0]), int(lockedstats[1]), int(lockedstats[2]), int(lockedstats[3])); +#endif +} + +struct Vector3 +{ + float x, y, z; +}; +// -- GODOT start -- +//static void rescalePositions(Vector3* result, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride) +static float rescalePositions(Vector3* result, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride) +// -- GODOT end -- + +{ + size_t vertex_stride_float = vertex_positions_stride / sizeof(float); + + float minv[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + float maxv[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + + for (size_t i = 0; i < vertex_count; ++i) + { + const float* v = vertex_positions_data + i * vertex_stride_float; + + result[i].x = v[0]; + result[i].y = v[1]; + result[i].z = v[2]; + + for (int j = 0; j < 3; ++j) + { + float vj = v[j]; + + minv[j] = minv[j] > vj ? vj : minv[j]; + maxv[j] = maxv[j] < vj ? vj : maxv[j]; + } + } + + float extent = 0.f; + + extent = (maxv[0] - minv[0]) < extent ? extent : (maxv[0] - minv[0]); + extent = (maxv[1] - minv[1]) < extent ? extent : (maxv[1] - minv[1]); + extent = (maxv[2] - minv[2]) < extent ? extent : (maxv[2] - minv[2]); + + float scale = extent == 0 ? 0.f : 1.f / extent; + + for (size_t i = 0; i < vertex_count; ++i) + { + result[i].x = (result[i].x - minv[0]) * scale; + result[i].y = (result[i].y - minv[1]) * scale; + result[i].z = (result[i].z - minv[2]) * scale; + } +// -- GODOT start -- + return extent; +// -- GODOT end -- + +} + +struct Quadric +{ + float a00, a11, a22; + float a10, a20, a21; + float b0, b1, b2, c; + float w; +}; + +struct Collapse +{ + unsigned int v0; + unsigned int v1; + + union + { + unsigned int bidi; + float error; + unsigned int errorui; + }; +}; + +static float normalize(Vector3& v) +{ + float length = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z); + + if (length > 0) + { + v.x /= length; + v.y /= length; + v.z /= length; + } + + return length; +} + +static void quadricAdd(Quadric& Q, const Quadric& R) +{ + Q.a00 += R.a00; + Q.a11 += R.a11; + Q.a22 += R.a22; + Q.a10 += R.a10; + Q.a20 += R.a20; + Q.a21 += R.a21; + Q.b0 += R.b0; + Q.b1 += R.b1; + Q.b2 += R.b2; + Q.c += R.c; + Q.w += R.w; +} + +static float quadricError(const Quadric& Q, const Vector3& v) +{ + float rx = Q.b0; + float ry = Q.b1; + float rz = Q.b2; + + rx += Q.a10 * v.y; + ry += Q.a21 * v.z; + rz += Q.a20 * v.x; + + rx *= 2; + ry *= 2; + rz *= 2; + + rx += Q.a00 * v.x; + ry += Q.a11 * v.y; + rz += Q.a22 * v.z; + + float r = Q.c; + r += rx * v.x; + r += ry * v.y; + r += rz * v.z; + + float s = Q.w == 0.f ? 0.f : 1.f / Q.w; + + return fabsf(r) * s; +} + +static void quadricFromPlane(Quadric& Q, float a, float b, float c, float d, float w) +{ + float aw = a * w; + float bw = b * w; + float cw = c * w; + float dw = d * w; + + Q.a00 = a * aw; + Q.a11 = b * bw; + Q.a22 = c * cw; + Q.a10 = a * bw; + Q.a20 = a * cw; + Q.a21 = b * cw; + Q.b0 = a * dw; + Q.b1 = b * dw; + Q.b2 = c * dw; + Q.c = d * dw; + Q.w = w; +} + +static void quadricFromPoint(Quadric& Q, float x, float y, float z, float w) +{ + // we need to encode (x - X) ^ 2 + (y - Y)^2 + (z - Z)^2 into the quadric + Q.a00 = w; + Q.a11 = w; + Q.a22 = w; + Q.a10 = 0.f; + Q.a20 = 0.f; + Q.a21 = 0.f; + Q.b0 = -2.f * x * w; + Q.b1 = -2.f * y * w; + Q.b2 = -2.f * z * w; + Q.c = (x * x + y * y + z * z) * w; + Q.w = w; +} + +static void quadricFromTriangle(Quadric& Q, const Vector3& p0, const Vector3& p1, const Vector3& p2, float weight) +{ + Vector3 p10 = {p1.x - p0.x, p1.y - p0.y, p1.z - p0.z}; + Vector3 p20 = {p2.x - p0.x, p2.y - p0.y, p2.z - p0.z}; + + // normal = cross(p1 - p0, p2 - p0) + Vector3 normal = {p10.y * p20.z - p10.z * p20.y, p10.z * p20.x - p10.x * p20.z, p10.x * p20.y - p10.y * p20.x}; + float area = normalize(normal); + + float distance = normal.x * p0.x + normal.y * p0.y + normal.z * p0.z; + + // we use sqrtf(area) so that the error is scaled linearly; this tends to improve silhouettes + quadricFromPlane(Q, normal.x, normal.y, normal.z, -distance, sqrtf(area) * weight); +} + +static void quadricFromTriangleEdge(Quadric& Q, const Vector3& p0, const Vector3& p1, const Vector3& p2, float weight) +{ + Vector3 p10 = {p1.x - p0.x, p1.y - p0.y, p1.z - p0.z}; + float length = normalize(p10); + + // p20p = length of projection of p2-p0 onto normalize(p1 - p0) + Vector3 p20 = {p2.x - p0.x, p2.y - p0.y, p2.z - p0.z}; + float p20p = p20.x * p10.x + p20.y * p10.y + p20.z * p10.z; + + // normal = altitude of triangle from point p2 onto edge p1-p0 + Vector3 normal = {p20.x - p10.x * p20p, p20.y - p10.y * p20p, p20.z - p10.z * p20p}; + normalize(normal); + + float distance = normal.x * p0.x + normal.y * p0.y + normal.z * p0.z; + + // note: the weight is scaled linearly with edge length; this has to match the triangle weight + quadricFromPlane(Q, normal.x, normal.y, normal.z, -distance, length * weight); +} + +static void fillFaceQuadrics(Quadric* vertex_quadrics, const unsigned int* indices, size_t index_count, const Vector3* vertex_positions, const unsigned int* remap) +{ + for (size_t i = 0; i < index_count; i += 3) + { + unsigned int i0 = indices[i + 0]; + unsigned int i1 = indices[i + 1]; + unsigned int i2 = indices[i + 2]; + + Quadric Q; + quadricFromTriangle(Q, vertex_positions[i0], vertex_positions[i1], vertex_positions[i2], 1.f); + + quadricAdd(vertex_quadrics[remap[i0]], Q); + quadricAdd(vertex_quadrics[remap[i1]], Q); + quadricAdd(vertex_quadrics[remap[i2]], Q); + } +} + +static void fillEdgeQuadrics(Quadric* vertex_quadrics, const unsigned int* indices, size_t index_count, const Vector3* vertex_positions, const unsigned int* remap, const unsigned char* vertex_kind, const unsigned int* loop, const unsigned int* loopback) +{ + for (size_t i = 0; i < index_count; i += 3) + { + static const int next[3] = {1, 2, 0}; + + for (int e = 0; e < 3; ++e) + { + unsigned int i0 = indices[i + e]; + unsigned int i1 = indices[i + next[e]]; + + unsigned char k0 = vertex_kind[i0]; + unsigned char k1 = vertex_kind[i1]; + + // check that either i0 or i1 are border/seam and are on the same edge loop + // note that we need to add the error even for edged that connect e.g. border & locked + // if we don't do that, the adjacent border->border edge won't have correct errors for corners + if (k0 != Kind_Border && k0 != Kind_Seam && k1 != Kind_Border && k1 != Kind_Seam) + continue; + + if ((k0 == Kind_Border || k0 == Kind_Seam) && loop[i0] != i1) + continue; + + if ((k1 == Kind_Border || k1 == Kind_Seam) && loopback[i1] != i0) + continue; + + // seam edges should occur twice (i0->i1 and i1->i0) - skip redundant edges + if (kHasOpposite[k0][k1] && remap[i1] > remap[i0]) + continue; + + unsigned int i2 = indices[i + next[next[e]]]; + + // we try hard to maintain border edge geometry; seam edges can move more freely + // due to topological restrictions on collapses, seam quadrics slightly improves collapse structure but aren't critical + const float kEdgeWeightSeam = 1.f; + const float kEdgeWeightBorder = 10.f; + + float edgeWeight = (k0 == Kind_Border || k1 == Kind_Border) ? kEdgeWeightBorder : kEdgeWeightSeam; + + Quadric Q; + quadricFromTriangleEdge(Q, vertex_positions[i0], vertex_positions[i1], vertex_positions[i2], edgeWeight); + + quadricAdd(vertex_quadrics[remap[i0]], Q); + quadricAdd(vertex_quadrics[remap[i1]], Q); + } + } +} + +static size_t pickEdgeCollapses(Collapse* collapses, const unsigned int* indices, size_t index_count, const unsigned int* remap, const unsigned char* vertex_kind, const unsigned int* loop) +{ + size_t collapse_count = 0; + + for (size_t i = 0; i < index_count; i += 3) + { + static const int next[3] = {1, 2, 0}; + + for (int e = 0; e < 3; ++e) + { + unsigned int i0 = indices[i + e]; + unsigned int i1 = indices[i + next[e]]; + + // this can happen either when input has a zero-length edge, or when we perform collapses for complex + // topology w/seams and collapse a manifold vertex that connects to both wedges onto one of them + // we leave edges like this alone since they may be important for preserving mesh integrity + if (remap[i0] == remap[i1]) + continue; + + unsigned char k0 = vertex_kind[i0]; + unsigned char k1 = vertex_kind[i1]; + + // the edge has to be collapsible in at least one direction + if (!(kCanCollapse[k0][k1] | kCanCollapse[k1][k0])) + continue; + + // manifold and seam edges should occur twice (i0->i1 and i1->i0) - skip redundant edges + if (kHasOpposite[k0][k1] && remap[i1] > remap[i0]) + continue; + + // two vertices are on a border or a seam, but there's no direct edge between them + // this indicates that they belong to two different edge loops and we should not collapse this edge + // loop[] tracks half edges so we only need to check i0->i1 + if (k0 == k1 && (k0 == Kind_Border || k0 == Kind_Seam) && loop[i0] != i1) + continue; + + // edge can be collapsed in either direction - we will pick the one with minimum error + // note: we evaluate error later during collapse ranking, here we just tag the edge as bidirectional + if (kCanCollapse[k0][k1] & kCanCollapse[k1][k0]) + { + Collapse c = {i0, i1, {/* bidi= */ 1}}; + collapses[collapse_count++] = c; + } + else + { + // edge can only be collapsed in one direction + unsigned int e0 = kCanCollapse[k0][k1] ? i0 : i1; + unsigned int e1 = kCanCollapse[k0][k1] ? i1 : i0; + + Collapse c = {e0, e1, {/* bidi= */ 0}}; + collapses[collapse_count++] = c; + } + } + } + + return collapse_count; +} + +static void rankEdgeCollapses(Collapse* collapses, size_t collapse_count, const Vector3* vertex_positions, const Quadric* vertex_quadrics, const unsigned int* remap) +{ + for (size_t i = 0; i < collapse_count; ++i) + { + Collapse& c = collapses[i]; + + unsigned int i0 = c.v0; + unsigned int i1 = c.v1; + + // most edges are bidirectional which means we need to evaluate errors for two collapses + // to keep this code branchless we just use the same edge for unidirectional edges + unsigned int j0 = c.bidi ? i1 : i0; + unsigned int j1 = c.bidi ? i0 : i1; + + const Quadric& qi = vertex_quadrics[remap[i0]]; + const Quadric& qj = vertex_quadrics[remap[j0]]; + + float ei = quadricError(qi, vertex_positions[i1]); + float ej = quadricError(qj, vertex_positions[j1]); + + // pick edge direction with minimal error + c.v0 = ei <= ej ? i0 : j0; + c.v1 = ei <= ej ? i1 : j1; + c.error = ei <= ej ? ei : ej; + } +} + +#if TRACE > 1 +static void dumpEdgeCollapses(const Collapse* collapses, size_t collapse_count, const unsigned char* vertex_kind) +{ + size_t ckinds[Kind_Count][Kind_Count] = {}; + float cerrors[Kind_Count][Kind_Count] = {}; + + for (int k0 = 0; k0 < Kind_Count; ++k0) + for (int k1 = 0; k1 < Kind_Count; ++k1) + cerrors[k0][k1] = FLT_MAX; + + for (size_t i = 0; i < collapse_count; ++i) + { + unsigned int i0 = collapses[i].v0; + unsigned int i1 = collapses[i].v1; + + unsigned char k0 = vertex_kind[i0]; + unsigned char k1 = vertex_kind[i1]; + + ckinds[k0][k1]++; + cerrors[k0][k1] = (collapses[i].error < cerrors[k0][k1]) ? collapses[i].error : cerrors[k0][k1]; + } + + for (int k0 = 0; k0 < Kind_Count; ++k0) + for (int k1 = 0; k1 < Kind_Count; ++k1) + if (ckinds[k0][k1]) + printf("collapses %d -> %d: %d, min error %e\n", k0, k1, int(ckinds[k0][k1]), cerrors[k0][k1]); +} + +static void dumpLockedCollapses(const unsigned int* indices, size_t index_count, const unsigned char* vertex_kind) +{ + size_t locked_collapses[Kind_Count][Kind_Count] = {}; + + for (size_t i = 0; i < index_count; i += 3) + { + static const int next[3] = {1, 2, 0}; + + for (int e = 0; e < 3; ++e) + { + unsigned int i0 = indices[i + e]; + unsigned int i1 = indices[i + next[e]]; + + unsigned char k0 = vertex_kind[i0]; + unsigned char k1 = vertex_kind[i1]; + + locked_collapses[k0][k1] += !kCanCollapse[k0][k1] && !kCanCollapse[k1][k0]; + } + } + + for (int k0 = 0; k0 < Kind_Count; ++k0) + for (int k1 = 0; k1 < Kind_Count; ++k1) + if (locked_collapses[k0][k1]) + printf("locked collapses %d -> %d: %d\n", k0, k1, int(locked_collapses[k0][k1])); +} +#endif + +static void sortEdgeCollapses(unsigned int* sort_order, const Collapse* collapses, size_t collapse_count) +{ + const int sort_bits = 11; + + // fill histogram for counting sort + unsigned int histogram[1 << sort_bits]; + memset(histogram, 0, sizeof(histogram)); + + for (size_t i = 0; i < collapse_count; ++i) + { + // skip sign bit since error is non-negative + unsigned int key = (collapses[i].errorui << 1) >> (32 - sort_bits); + + histogram[key]++; + } + + // compute offsets based on histogram data + size_t histogram_sum = 0; + + for (size_t i = 0; i < 1 << sort_bits; ++i) + { + size_t count = histogram[i]; + histogram[i] = unsigned(histogram_sum); + histogram_sum += count; + } + + assert(histogram_sum == collapse_count); + + // compute sort order based on offsets + for (size_t i = 0; i < collapse_count; ++i) + { + // skip sign bit since error is non-negative + unsigned int key = (collapses[i].errorui << 1) >> (32 - sort_bits); + + sort_order[histogram[key]++] = unsigned(i); + } +} + +static size_t performEdgeCollapses(unsigned int* collapse_remap, unsigned char* collapse_locked, Quadric* vertex_quadrics, const Collapse* collapses, size_t collapse_count, const unsigned int* collapse_order, const unsigned int* remap, const unsigned int* wedge, const unsigned char* vertex_kind, size_t triangle_collapse_goal, float error_goal, float error_limit) +{ + size_t edge_collapses = 0; + size_t triangle_collapses = 0; + + for (size_t i = 0; i < collapse_count; ++i) + { + const Collapse& c = collapses[collapse_order[i]]; + + if (c.error > error_limit) + break; + + if (c.error > error_goal && triangle_collapses > triangle_collapse_goal / 10) + break; + + if (triangle_collapses >= triangle_collapse_goal) + break; + + unsigned int i0 = c.v0; + unsigned int i1 = c.v1; + + unsigned int r0 = remap[i0]; + unsigned int r1 = remap[i1]; + + // we don't collapse vertices that had source or target vertex involved in a collapse + // it's important to not move the vertices twice since it complicates the tracking/remapping logic + // it's important to not move other vertices towards a moved vertex to preserve error since we don't re-rank collapses mid-pass + if (collapse_locked[r0] | collapse_locked[r1]) + continue; + + assert(collapse_remap[r0] == r0); + assert(collapse_remap[r1] == r1); + + quadricAdd(vertex_quadrics[r1], vertex_quadrics[r0]); + + if (vertex_kind[i0] == Kind_Complex) + { + unsigned int v = i0; + + do + { + collapse_remap[v] = r1; + v = wedge[v]; + } while (v != i0); + } + else if (vertex_kind[i0] == Kind_Seam) + { + // remap v0 to v1 and seam pair of v0 to seam pair of v1 + unsigned int s0 = wedge[i0]; + unsigned int s1 = wedge[i1]; + + assert(s0 != i0 && s1 != i1); + assert(wedge[s0] == i0 && wedge[s1] == i1); + + collapse_remap[i0] = i1; + collapse_remap[s0] = s1; + } + else + { + assert(wedge[i0] == i0); + + collapse_remap[i0] = i1; + } + + collapse_locked[r0] = 1; + collapse_locked[r1] = 1; + + // border edges collapse 1 triangle, other edges collapse 2 or more + triangle_collapses += (vertex_kind[i0] == Kind_Border) ? 1 : 2; + edge_collapses++; + } + + return edge_collapses; +} + +static size_t remapIndexBuffer(unsigned int* indices, size_t index_count, const unsigned int* collapse_remap) +{ + size_t write = 0; + + for (size_t i = 0; i < index_count; i += 3) + { + unsigned int v0 = collapse_remap[indices[i + 0]]; + unsigned int v1 = collapse_remap[indices[i + 1]]; + unsigned int v2 = collapse_remap[indices[i + 2]]; + + // we never move the vertex twice during a single pass + assert(collapse_remap[v0] == v0); + assert(collapse_remap[v1] == v1); + assert(collapse_remap[v2] == v2); + + if (v0 != v1 && v0 != v2 && v1 != v2) + { + indices[write + 0] = v0; + indices[write + 1] = v1; + indices[write + 2] = v2; + write += 3; + } + } + + return write; +} + +static void remapEdgeLoops(unsigned int* loop, size_t vertex_count, const unsigned int* collapse_remap) +{ + for (size_t i = 0; i < vertex_count; ++i) + { + if (loop[i] != ~0u) + { + unsigned int l = loop[i]; + unsigned int r = collapse_remap[l]; + + // i == r is a special case when the seam edge is collapsed in a direction opposite to where loop goes + loop[i] = (i == r) ? loop[l] : r; + } + } +} + +struct CellHasher +{ + const unsigned int* vertex_ids; + + size_t hash(unsigned int i) const + { + unsigned int h = vertex_ids[i]; + + // MurmurHash2 finalizer + h ^= h >> 13; + h *= 0x5bd1e995; + h ^= h >> 15; + return h; + } + + bool equal(unsigned int lhs, unsigned int rhs) const + { + return vertex_ids[lhs] == vertex_ids[rhs]; + } +}; + +struct IdHasher +{ + size_t hash(unsigned int id) const + { + unsigned int h = id; + + // MurmurHash2 finalizer + h ^= h >> 13; + h *= 0x5bd1e995; + h ^= h >> 15; + return h; + } + + bool equal(unsigned int lhs, unsigned int rhs) const + { + return lhs == rhs; + } +}; + +struct TriangleHasher +{ + unsigned int* indices; + + size_t hash(unsigned int i) const + { + const unsigned int* tri = indices + i * 3; + + // Optimized Spatial Hashing for Collision Detection of Deformable Objects + return (tri[0] * 73856093) ^ (tri[1] * 19349663) ^ (tri[2] * 83492791); + } + + bool equal(unsigned int lhs, unsigned int rhs) const + { + const unsigned int* lt = indices + lhs * 3; + const unsigned int* rt = indices + rhs * 3; + + return lt[0] == rt[0] && lt[1] == rt[1] && lt[2] == rt[2]; + } +}; + +static void computeVertexIds(unsigned int* vertex_ids, const Vector3* vertex_positions, size_t vertex_count, int grid_size) +{ + assert(grid_size >= 1 && grid_size <= 1024); + float cell_scale = float(grid_size - 1); + + for (size_t i = 0; i < vertex_count; ++i) + { + const Vector3& v = vertex_positions[i]; + + int xi = int(v.x * cell_scale + 0.5f); + int yi = int(v.y * cell_scale + 0.5f); + int zi = int(v.z * cell_scale + 0.5f); + + vertex_ids[i] = (xi << 20) | (yi << 10) | zi; + } +} + +static size_t countTriangles(const unsigned int* vertex_ids, const unsigned int* indices, size_t index_count) +{ + size_t result = 0; + + for (size_t i = 0; i < index_count; i += 3) + { + unsigned int id0 = vertex_ids[indices[i + 0]]; + unsigned int id1 = vertex_ids[indices[i + 1]]; + unsigned int id2 = vertex_ids[indices[i + 2]]; + + result += (id0 != id1) & (id0 != id2) & (id1 != id2); + } + + return result; +} + +static size_t fillVertexCells(unsigned int* table, size_t table_size, unsigned int* vertex_cells, const unsigned int* vertex_ids, size_t vertex_count) +{ + CellHasher hasher = {vertex_ids}; + + memset(table, -1, table_size * sizeof(unsigned int)); + + size_t result = 0; + + for (size_t i = 0; i < vertex_count; ++i) + { + unsigned int* entry = hashLookup2(table, table_size, hasher, unsigned(i), ~0u); + + if (*entry == ~0u) + { + *entry = unsigned(i); + vertex_cells[i] = unsigned(result++); + } + else + { + vertex_cells[i] = vertex_cells[*entry]; + } + } + + return result; +} + +static size_t countVertexCells(unsigned int* table, size_t table_size, const unsigned int* vertex_ids, size_t vertex_count) +{ + IdHasher hasher; + + memset(table, -1, table_size * sizeof(unsigned int)); + + size_t result = 0; + + for (size_t i = 0; i < vertex_count; ++i) + { + unsigned int id = vertex_ids[i]; + unsigned int* entry = hashLookup2(table, table_size, hasher, id, ~0u); + + result += (*entry == ~0u); + *entry = id; + } + + return result; +} + +static void fillCellQuadrics(Quadric* cell_quadrics, const unsigned int* indices, size_t index_count, const Vector3* vertex_positions, const unsigned int* vertex_cells) +{ + for (size_t i = 0; i < index_count; i += 3) + { + unsigned int i0 = indices[i + 0]; + unsigned int i1 = indices[i + 1]; + unsigned int i2 = indices[i + 2]; + + unsigned int c0 = vertex_cells[i0]; + unsigned int c1 = vertex_cells[i1]; + unsigned int c2 = vertex_cells[i2]; + + bool single_cell = (c0 == c1) & (c0 == c2); + + Quadric Q; + quadricFromTriangle(Q, vertex_positions[i0], vertex_positions[i1], vertex_positions[i2], single_cell ? 3.f : 1.f); + + if (single_cell) + { + quadricAdd(cell_quadrics[c0], Q); + } + else + { + quadricAdd(cell_quadrics[c0], Q); + quadricAdd(cell_quadrics[c1], Q); + quadricAdd(cell_quadrics[c2], Q); + } + } +} + +static void fillCellQuadrics(Quadric* cell_quadrics, const Vector3* vertex_positions, size_t vertex_count, const unsigned int* vertex_cells) +{ + for (size_t i = 0; i < vertex_count; ++i) + { + unsigned int c = vertex_cells[i]; + const Vector3& v = vertex_positions[i]; + + Quadric Q; + quadricFromPoint(Q, v.x, v.y, v.z, 1.f); + + quadricAdd(cell_quadrics[c], Q); + } +} + +static void fillCellRemap(unsigned int* cell_remap, float* cell_errors, size_t cell_count, const unsigned int* vertex_cells, const Quadric* cell_quadrics, const Vector3* vertex_positions, size_t vertex_count) +{ + memset(cell_remap, -1, cell_count * sizeof(unsigned int)); + + for (size_t i = 0; i < vertex_count; ++i) + { + unsigned int cell = vertex_cells[i]; + float error = quadricError(cell_quadrics[cell], vertex_positions[i]); + + if (cell_remap[cell] == ~0u || cell_errors[cell] > error) + { + cell_remap[cell] = unsigned(i); + cell_errors[cell] = error; + } + } +} + +static size_t filterTriangles(unsigned int* destination, unsigned int* tritable, size_t tritable_size, const unsigned int* indices, size_t index_count, const unsigned int* vertex_cells, const unsigned int* cell_remap) +{ + TriangleHasher hasher = {destination}; + + memset(tritable, -1, tritable_size * sizeof(unsigned int)); + + size_t result = 0; + + for (size_t i = 0; i < index_count; i += 3) + { + unsigned int c0 = vertex_cells[indices[i + 0]]; + unsigned int c1 = vertex_cells[indices[i + 1]]; + unsigned int c2 = vertex_cells[indices[i + 2]]; + + if (c0 != c1 && c0 != c2 && c1 != c2) + { + unsigned int a = cell_remap[c0]; + unsigned int b = cell_remap[c1]; + unsigned int c = cell_remap[c2]; + + if (b < a && b < c) + { + unsigned int t = a; + a = b, b = c, c = t; + } + else if (c < a && c < b) + { + unsigned int t = c; + c = b, b = a, a = t; + } + + destination[result * 3 + 0] = a; + destination[result * 3 + 1] = b; + destination[result * 3 + 2] = c; + + unsigned int* entry = hashLookup2(tritable, tritable_size, hasher, unsigned(result), ~0u); + + if (*entry == ~0u) + *entry = unsigned(result++); + } + } + + return result * 3; +} + +static float interpolate(float y, float x0, float y0, float x1, float y1, float x2, float y2) +{ + // three point interpolation from "revenge of interpolation search" paper + float num = (y1 - y) * (x1 - x2) * (x1 - x0) * (y2 - y0); + float den = (y2 - y) * (x1 - x2) * (y0 - y1) + (y0 - y) * (x1 - x0) * (y1 - y2); + return x1 + num / den; +} + +} // namespace meshopt + +#ifndef NDEBUG +unsigned char* meshopt_simplifyDebugKind = 0; +unsigned int* meshopt_simplifyDebugLoop = 0; +unsigned int* meshopt_simplifyDebugLoopBack = 0; +#endif + +// -- GODOT start -- +//size_t meshopt_simplify(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error) +size_t meshopt_simplify(unsigned int *destination, const unsigned int *indices, size_t index_count, const float *vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count, float target_error, float *r_resulting_error) +// -- GODOT end -- +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + assert(target_index_count <= index_count); + + meshopt_Allocator allocator; + + unsigned int* result = destination; + + // build adjacency information + EdgeAdjacency adjacency = {}; + buildEdgeAdjacency(adjacency, indices, index_count, vertex_count, allocator); + + // build position remap that maps each vertex to the one with identical position + unsigned int* remap = allocator.allocate<unsigned int>(vertex_count); + unsigned int* wedge = allocator.allocate<unsigned int>(vertex_count); + buildPositionRemap(remap, wedge, vertex_positions_data, vertex_count, vertex_positions_stride, allocator); + + // classify vertices; vertex kind determines collapse rules, see kCanCollapse + unsigned char* vertex_kind = allocator.allocate<unsigned char>(vertex_count); + unsigned int* loop = allocator.allocate<unsigned int>(vertex_count); + unsigned int* loopback = allocator.allocate<unsigned int>(vertex_count); + classifyVertices(vertex_kind, loop, loopback, vertex_count, adjacency, remap, wedge); + +#if TRACE + size_t unique_positions = 0; + for (size_t i = 0; i < vertex_count; ++i) + unique_positions += remap[i] == i; + + printf("position remap: %d vertices => %d positions\n", int(vertex_count), int(unique_positions)); + + size_t kinds[Kind_Count] = {}; + for (size_t i = 0; i < vertex_count; ++i) + kinds[vertex_kind[i]] += remap[i] == i; + + printf("kinds: manifold %d, border %d, seam %d, complex %d, locked %d\n", + int(kinds[Kind_Manifold]), int(kinds[Kind_Border]), int(kinds[Kind_Seam]), int(kinds[Kind_Complex]), int(kinds[Kind_Locked])); +#endif + + Vector3* vertex_positions = allocator.allocate<Vector3>(vertex_count); +// -- GODOT start -- + //rescalePositions(vertex_positions, vertex_positions_data, vertex_count, vertex_positions_stride); + float extent = rescalePositions(vertex_positions, vertex_positions_data, vertex_count, vertex_positions_stride); +// -- GODOT end -- + + Quadric* vertex_quadrics = allocator.allocate<Quadric>(vertex_count); + memset(vertex_quadrics, 0, vertex_count * sizeof(Quadric)); + + fillFaceQuadrics(vertex_quadrics, indices, index_count, vertex_positions, remap); + fillEdgeQuadrics(vertex_quadrics, indices, index_count, vertex_positions, remap, vertex_kind, loop, loopback); + + if (result != indices) + memcpy(result, indices, index_count * sizeof(unsigned int)); + +// -- GODOT start -- +#if TRACE + size_t pass_count = 0; + //float worst_error = 0; +#endif + float worst_error = 0; +// -- GODOT end -- + + Collapse* edge_collapses = allocator.allocate<Collapse>(index_count); + unsigned int* collapse_order = allocator.allocate<unsigned int>(index_count); + unsigned int* collapse_remap = allocator.allocate<unsigned int>(vertex_count); + unsigned char* collapse_locked = allocator.allocate<unsigned char>(vertex_count); + + size_t result_count = index_count; + + // target_error input is linear; we need to adjust it to match quadricError units + float error_limit = target_error * target_error; + +// -- GODOT start -- + if (r_resulting_error) { + *r_resulting_error = 1.0; + } +// -- GODOT end -- + + while (result_count > target_index_count) + { + size_t edge_collapse_count = pickEdgeCollapses(edge_collapses, result, result_count, remap, vertex_kind, loop); + + // no edges can be collapsed any more due to topology restrictions + if (edge_collapse_count == 0) + break; + + rankEdgeCollapses(edge_collapses, edge_collapse_count, vertex_positions, vertex_quadrics, remap); + +#if TRACE > 1 + dumpEdgeCollapses(edge_collapses, edge_collapse_count, vertex_kind); +#endif + + sortEdgeCollapses(collapse_order, edge_collapses, edge_collapse_count); + + // most collapses remove 2 triangles; use this to establish a bound on the pass in terms of error limit + // note that edge_collapse_goal is an estimate; triangle_collapse_goal will be used to actually limit collapses + size_t triangle_collapse_goal = (result_count - target_index_count) / 3; + size_t edge_collapse_goal = triangle_collapse_goal / 2; + + // we limit the error in each pass based on the error of optimal last collapse; since many collapses will be locked + // as they will share vertices with other successfull collapses, we need to increase the acceptable error by this factor + const float kPassErrorBound = 1.5f; + + float error_goal = edge_collapse_goal < edge_collapse_count ? edge_collapses[collapse_order[edge_collapse_goal]].error * kPassErrorBound : FLT_MAX; + + for (size_t i = 0; i < vertex_count; ++i) + collapse_remap[i] = unsigned(i); + + memset(collapse_locked, 0, vertex_count); + + size_t collapses = performEdgeCollapses(collapse_remap, collapse_locked, vertex_quadrics, edge_collapses, edge_collapse_count, collapse_order, remap, wedge, vertex_kind, triangle_collapse_goal, error_goal, error_limit); + + // no edges can be collapsed any more due to hitting the error limit or triangle collapse limit + if (collapses == 0) + break; + + remapEdgeLoops(loop, vertex_count, collapse_remap); + remapEdgeLoops(loopback, vertex_count, collapse_remap); + + size_t new_count = remapIndexBuffer(result, result_count, collapse_remap); + assert(new_count < result_count); + +// -- GODOT start -- +//#if TRACE + float pass_error = 0.f; + for (size_t i = 0; i < edge_collapse_count; ++i) + { + Collapse& c = edge_collapses[collapse_order[i]]; + + if (collapse_remap[c.v0] == c.v1) + pass_error = c.error; + } + + //pass_count++; + worst_error = (worst_error < pass_error) ? pass_error : worst_error; + +#if TRACE + pass_count++; + printf("pass %d: triangles: %d -> %d, collapses: %d/%d (goal: %d), error: %e (limit %e goal %e)\n", int(pass_count), int(result_count / 3), int(new_count / 3), int(collapses), int(edge_collapse_count), int(edge_collapse_goal), pass_error, error_limit, error_goal); +#endif +// -- GODOT end -- + + result_count = new_count; + } + +// -- GODOT start -- + if (r_resulting_error) { + *r_resulting_error = sqrt(worst_error) * extent; + } +// -- GODOT end -- + +#if TRACE + printf("passes: %d, worst error: %e\n", int(pass_count), worst_error); +#endif + +#if TRACE > 1 + dumpLockedCollapses(result, result_count, vertex_kind); +#endif + +#ifndef NDEBUG + if (meshopt_simplifyDebugKind) + memcpy(meshopt_simplifyDebugKind, vertex_kind, vertex_count); + + if (meshopt_simplifyDebugLoop) + memcpy(meshopt_simplifyDebugLoop, loop, vertex_count * sizeof(unsigned int)); + + if (meshopt_simplifyDebugLoopBack) + memcpy(meshopt_simplifyDebugLoopBack, loopback, vertex_count * sizeof(unsigned int)); +#endif + + return result_count; +} + +size_t meshopt_simplifySloppy(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_index_count) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + assert(target_index_count <= index_count); + + // we expect to get ~2 triangles/vertex in the output + size_t target_cell_count = target_index_count / 6; + + if (target_cell_count == 0) + return 0; + + meshopt_Allocator allocator; + + Vector3* vertex_positions = allocator.allocate<Vector3>(vertex_count); + rescalePositions(vertex_positions, vertex_positions_data, vertex_count, vertex_positions_stride); + + // find the optimal grid size using guided binary search +#if TRACE + printf("source: %d vertices, %d triangles\n", int(vertex_count), int(index_count / 3)); + printf("target: %d cells, %d triangles\n", int(target_cell_count), int(target_index_count / 3)); +#endif + + unsigned int* vertex_ids = allocator.allocate<unsigned int>(vertex_count); + + const int kInterpolationPasses = 5; + + // invariant: # of triangles in min_grid <= target_count + int min_grid = 0; + int max_grid = 1025; + size_t min_triangles = 0; + size_t max_triangles = index_count / 3; + + // instead of starting in the middle, let's guess as to what the answer might be! triangle count usually grows as a square of grid size... + int next_grid_size = int(sqrtf(float(target_cell_count)) + 0.5f); + + for (int pass = 0; pass < 10 + kInterpolationPasses; ++pass) + { + assert(min_triangles < target_index_count / 3); + assert(max_grid - min_grid > 1); + + // we clamp the prediction of the grid size to make sure that the search converges + int grid_size = next_grid_size; + grid_size = (grid_size <= min_grid) ? min_grid + 1 : (grid_size >= max_grid) ? max_grid - 1 : grid_size; + + computeVertexIds(vertex_ids, vertex_positions, vertex_count, grid_size); + size_t triangles = countTriangles(vertex_ids, indices, index_count); + +#if TRACE + printf("pass %d (%s): grid size %d, triangles %d, %s\n", + pass, (pass == 0) ? "guess" : (pass <= kInterpolationPasses) ? "lerp" : "binary", + grid_size, int(triangles), + (triangles <= target_index_count / 3) ? "under" : "over"); +#endif + + float tip = interpolate(float(target_index_count / 3), float(min_grid), float(min_triangles), float(grid_size), float(triangles), float(max_grid), float(max_triangles)); + + if (triangles <= target_index_count / 3) + { + min_grid = grid_size; + min_triangles = triangles; + } + else + { + max_grid = grid_size; + max_triangles = triangles; + } + + if (triangles == target_index_count / 3 || max_grid - min_grid <= 1) + break; + + // we start by using interpolation search - it usually converges faster + // however, interpolation search has a worst case of O(N) so we switch to binary search after a few iterations which converges in O(logN) + next_grid_size = (pass < kInterpolationPasses) ? int(tip + 0.5f) : (min_grid + max_grid) / 2; + } + + if (min_triangles == 0) + return 0; + + // build vertex->cell association by mapping all vertices with the same quantized position to the same cell + size_t table_size = hashBuckets2(vertex_count); + unsigned int* table = allocator.allocate<unsigned int>(table_size); + + unsigned int* vertex_cells = allocator.allocate<unsigned int>(vertex_count); + + computeVertexIds(vertex_ids, vertex_positions, vertex_count, min_grid); + size_t cell_count = fillVertexCells(table, table_size, vertex_cells, vertex_ids, vertex_count); + + // build a quadric for each target cell + Quadric* cell_quadrics = allocator.allocate<Quadric>(cell_count); + memset(cell_quadrics, 0, cell_count * sizeof(Quadric)); + + fillCellQuadrics(cell_quadrics, indices, index_count, vertex_positions, vertex_cells); + + // for each target cell, find the vertex with the minimal error + unsigned int* cell_remap = allocator.allocate<unsigned int>(cell_count); + float* cell_errors = allocator.allocate<float>(cell_count); + + fillCellRemap(cell_remap, cell_errors, cell_count, vertex_cells, cell_quadrics, vertex_positions, vertex_count); + + // collapse triangles! + // note that we need to filter out triangles that we've already output because we very frequently generate redundant triangles between cells :( + size_t tritable_size = hashBuckets2(min_triangles); + unsigned int* tritable = allocator.allocate<unsigned int>(tritable_size); + + size_t write = filterTriangles(destination, tritable, tritable_size, indices, index_count, vertex_cells, cell_remap); + assert(write <= target_index_count); + +#if TRACE + printf("result: %d cells, %d triangles (%d unfiltered)\n", int(cell_count), int(write / 3), int(min_triangles)); +#endif + + return write; +} + +size_t meshopt_simplifyPoints(unsigned int* destination, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride, size_t target_vertex_count) +{ + using namespace meshopt; + + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + assert(target_vertex_count <= vertex_count); + + size_t target_cell_count = target_vertex_count; + + if (target_cell_count == 0) + return 0; + + meshopt_Allocator allocator; + + Vector3* vertex_positions = allocator.allocate<Vector3>(vertex_count); + rescalePositions(vertex_positions, vertex_positions_data, vertex_count, vertex_positions_stride); + + // find the optimal grid size using guided binary search +#if TRACE + printf("source: %d vertices\n", int(vertex_count)); + printf("target: %d cells\n", int(target_cell_count)); +#endif + + unsigned int* vertex_ids = allocator.allocate<unsigned int>(vertex_count); + + size_t table_size = hashBuckets2(vertex_count); + unsigned int* table = allocator.allocate<unsigned int>(table_size); + + const int kInterpolationPasses = 5; + + // invariant: # of vertices in min_grid <= target_count + int min_grid = 0; + int max_grid = 1025; + size_t min_vertices = 0; + size_t max_vertices = vertex_count; + + // instead of starting in the middle, let's guess as to what the answer might be! triangle count usually grows as a square of grid size... + int next_grid_size = int(sqrtf(float(target_cell_count)) + 0.5f); + + for (int pass = 0; pass < 10 + kInterpolationPasses; ++pass) + { + assert(min_vertices < target_vertex_count); + assert(max_grid - min_grid > 1); + + // we clamp the prediction of the grid size to make sure that the search converges + int grid_size = next_grid_size; + grid_size = (grid_size <= min_grid) ? min_grid + 1 : (grid_size >= max_grid) ? max_grid - 1 : grid_size; + + computeVertexIds(vertex_ids, vertex_positions, vertex_count, grid_size); + size_t vertices = countVertexCells(table, table_size, vertex_ids, vertex_count); + +#if TRACE + printf("pass %d (%s): grid size %d, vertices %d, %s\n", + pass, (pass == 0) ? "guess" : (pass <= kInterpolationPasses) ? "lerp" : "binary", + grid_size, int(vertices), + (vertices <= target_vertex_count) ? "under" : "over"); +#endif + + float tip = interpolate(float(target_vertex_count), float(min_grid), float(min_vertices), float(grid_size), float(vertices), float(max_grid), float(max_vertices)); + + if (vertices <= target_vertex_count) + { + min_grid = grid_size; + min_vertices = vertices; + } + else + { + max_grid = grid_size; + max_vertices = vertices; + } + + if (vertices == target_vertex_count || max_grid - min_grid <= 1) + break; + + // we start by using interpolation search - it usually converges faster + // however, interpolation search has a worst case of O(N) so we switch to binary search after a few iterations which converges in O(logN) + next_grid_size = (pass < kInterpolationPasses) ? int(tip + 0.5f) : (min_grid + max_grid) / 2; + } + + if (min_vertices == 0) + return 0; + + // build vertex->cell association by mapping all vertices with the same quantized position to the same cell + unsigned int* vertex_cells = allocator.allocate<unsigned int>(vertex_count); + + computeVertexIds(vertex_ids, vertex_positions, vertex_count, min_grid); + size_t cell_count = fillVertexCells(table, table_size, vertex_cells, vertex_ids, vertex_count); + + // build a quadric for each target cell + Quadric* cell_quadrics = allocator.allocate<Quadric>(cell_count); + memset(cell_quadrics, 0, cell_count * sizeof(Quadric)); + + fillCellQuadrics(cell_quadrics, vertex_positions, vertex_count, vertex_cells); + + // for each target cell, find the vertex with the minimal error + unsigned int* cell_remap = allocator.allocate<unsigned int>(cell_count); + float* cell_errors = allocator.allocate<float>(cell_count); + + fillCellRemap(cell_remap, cell_errors, cell_count, vertex_cells, cell_quadrics, vertex_positions, vertex_count); + + // copy results to the output + assert(cell_count <= target_vertex_count); + memcpy(destination, cell_remap, sizeof(unsigned int) * cell_count); + +#if TRACE + printf("result: %d cells\n", int(cell_count)); +#endif + + return cell_count; +} diff --git a/thirdparty/meshoptimizer/spatialorder.cpp b/thirdparty/meshoptimizer/spatialorder.cpp new file mode 100644 index 0000000000..b09f80ac6f --- /dev/null +++ b/thirdparty/meshoptimizer/spatialorder.cpp @@ -0,0 +1,194 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <float.h> +#include <string.h> + +// This work is based on: +// Fabian Giesen. Decoding Morton codes. 2009 +namespace meshopt +{ + +// "Insert" two 0 bits after each of the 10 low bits of x +inline unsigned int part1By2(unsigned int x) +{ + x &= 0x000003ff; // x = ---- ---- ---- ---- ---- --98 7654 3210 + x = (x ^ (x << 16)) & 0xff0000ff; // x = ---- --98 ---- ---- ---- ---- 7654 3210 + x = (x ^ (x << 8)) & 0x0300f00f; // x = ---- --98 ---- ---- 7654 ---- ---- 3210 + x = (x ^ (x << 4)) & 0x030c30c3; // x = ---- --98 ---- 76-- --54 ---- 32-- --10 + x = (x ^ (x << 2)) & 0x09249249; // x = ---- 9--8 --7- -6-- 5--4 --3- -2-- 1--0 + return x; +} + +static void computeOrder(unsigned int* result, const float* vertex_positions_data, size_t vertex_count, size_t vertex_positions_stride) +{ + size_t vertex_stride_float = vertex_positions_stride / sizeof(float); + + float minv[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + float maxv[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + + for (size_t i = 0; i < vertex_count; ++i) + { + const float* v = vertex_positions_data + i * vertex_stride_float; + + for (int j = 0; j < 3; ++j) + { + float vj = v[j]; + + minv[j] = minv[j] > vj ? vj : minv[j]; + maxv[j] = maxv[j] < vj ? vj : maxv[j]; + } + } + + float extent = 0.f; + + extent = (maxv[0] - minv[0]) < extent ? extent : (maxv[0] - minv[0]); + extent = (maxv[1] - minv[1]) < extent ? extent : (maxv[1] - minv[1]); + extent = (maxv[2] - minv[2]) < extent ? extent : (maxv[2] - minv[2]); + + float scale = extent == 0 ? 0.f : 1.f / extent; + + // generate Morton order based on the position inside a unit cube + for (size_t i = 0; i < vertex_count; ++i) + { + const float* v = vertex_positions_data + i * vertex_stride_float; + + int x = int((v[0] - minv[0]) * scale * 1023.f + 0.5f); + int y = int((v[1] - minv[1]) * scale * 1023.f + 0.5f); + int z = int((v[2] - minv[2]) * scale * 1023.f + 0.5f); + + result[i] = part1By2(x) | (part1By2(y) << 1) | (part1By2(z) << 2); + } +} + +static void computeHistogram(unsigned int (&hist)[1024][3], const unsigned int* data, size_t count) +{ + memset(hist, 0, sizeof(hist)); + + // compute 3 10-bit histograms in parallel + for (size_t i = 0; i < count; ++i) + { + unsigned int id = data[i]; + + hist[(id >> 0) & 1023][0]++; + hist[(id >> 10) & 1023][1]++; + hist[(id >> 20) & 1023][2]++; + } + + unsigned int sumx = 0, sumy = 0, sumz = 0; + + // replace histogram data with prefix histogram sums in-place + for (int i = 0; i < 1024; ++i) + { + unsigned int hx = hist[i][0], hy = hist[i][1], hz = hist[i][2]; + + hist[i][0] = sumx; + hist[i][1] = sumy; + hist[i][2] = sumz; + + sumx += hx; + sumy += hy; + sumz += hz; + } + + assert(sumx == count && sumy == count && sumz == count); +} + +static void radixPass(unsigned int* destination, const unsigned int* source, const unsigned int* keys, size_t count, unsigned int (&hist)[1024][3], int pass) +{ + int bitoff = pass * 10; + + for (size_t i = 0; i < count; ++i) + { + unsigned int id = (keys[source[i]] >> bitoff) & 1023; + + destination[hist[id][pass]++] = source[i]; + } +} + +} // namespace meshopt + +void meshopt_spatialSortRemap(unsigned int* destination, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride) +{ + using namespace meshopt; + + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + + meshopt_Allocator allocator; + + unsigned int* keys = allocator.allocate<unsigned int>(vertex_count); + computeOrder(keys, vertex_positions, vertex_count, vertex_positions_stride); + + unsigned int hist[1024][3]; + computeHistogram(hist, keys, vertex_count); + + unsigned int* scratch = allocator.allocate<unsigned int>(vertex_count); + + for (size_t i = 0; i < vertex_count; ++i) + destination[i] = unsigned(i); + + // 3-pass radix sort computes the resulting order into scratch + radixPass(scratch, destination, keys, vertex_count, hist, 0); + radixPass(destination, scratch, keys, vertex_count, hist, 1); + radixPass(scratch, destination, keys, vertex_count, hist, 2); + + // since our remap table is mapping old=>new, we need to reverse it + for (size_t i = 0; i < vertex_count; ++i) + destination[scratch[i]] = unsigned(i); +} + +void meshopt_spatialSortTriangles(unsigned int* destination, const unsigned int* indices, size_t index_count, const float* vertex_positions, size_t vertex_count, size_t vertex_positions_stride) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + assert(vertex_positions_stride > 0 && vertex_positions_stride <= 256); + assert(vertex_positions_stride % sizeof(float) == 0); + + (void)vertex_count; + + size_t face_count = index_count / 3; + size_t vertex_stride_float = vertex_positions_stride / sizeof(float); + + meshopt_Allocator allocator; + + float* centroids = allocator.allocate<float>(face_count * 3); + + for (size_t i = 0; i < face_count; ++i) + { + unsigned int a = indices[i * 3 + 0], b = indices[i * 3 + 1], c = indices[i * 3 + 2]; + assert(a < vertex_count && b < vertex_count && c < vertex_count); + + const float* va = vertex_positions + a * vertex_stride_float; + const float* vb = vertex_positions + b * vertex_stride_float; + const float* vc = vertex_positions + c * vertex_stride_float; + + centroids[i * 3 + 0] = (va[0] + vb[0] + vc[0]) / 3.f; + centroids[i * 3 + 1] = (va[1] + vb[1] + vc[1]) / 3.f; + centroids[i * 3 + 2] = (va[2] + vb[2] + vc[2]) / 3.f; + } + + unsigned int* remap = allocator.allocate<unsigned int>(face_count); + + meshopt_spatialSortRemap(remap, centroids, face_count, sizeof(float) * 3); + + // support in-order remap + if (destination == indices) + { + unsigned int* indices_copy = allocator.allocate<unsigned int>(index_count); + memcpy(indices_copy, indices, index_count * sizeof(unsigned int)); + indices = indices_copy; + } + + for (size_t i = 0; i < face_count; ++i) + { + unsigned int a = indices[i * 3 + 0], b = indices[i * 3 + 1], c = indices[i * 3 + 2]; + unsigned int r = remap[i]; + + destination[r * 3 + 0] = a; + destination[r * 3 + 1] = b; + destination[r * 3 + 2] = c; + } +} diff --git a/thirdparty/meshoptimizer/stripifier.cpp b/thirdparty/meshoptimizer/stripifier.cpp new file mode 100644 index 0000000000..8ce17ef3dc --- /dev/null +++ b/thirdparty/meshoptimizer/stripifier.cpp @@ -0,0 +1,295 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <limits.h> +#include <string.h> + +// This work is based on: +// Francine Evans, Steven Skiena and Amitabh Varshney. Optimizing Triangle Strips for Fast Rendering. 1996 +namespace meshopt +{ + +static unsigned int findStripFirst(const unsigned int buffer[][3], unsigned int buffer_size, const unsigned int* valence) +{ + unsigned int index = 0; + unsigned int iv = ~0u; + + for (size_t i = 0; i < buffer_size; ++i) + { + unsigned int va = valence[buffer[i][0]], vb = valence[buffer[i][1]], vc = valence[buffer[i][2]]; + unsigned int v = (va < vb && va < vc) ? va : (vb < vc) ? vb : vc; + + if (v < iv) + { + index = unsigned(i); + iv = v; + } + } + + return index; +} + +static int findStripNext(const unsigned int buffer[][3], unsigned int buffer_size, unsigned int e0, unsigned int e1) +{ + for (size_t i = 0; i < buffer_size; ++i) + { + unsigned int a = buffer[i][0], b = buffer[i][1], c = buffer[i][2]; + + if (e0 == a && e1 == b) + return (int(i) << 2) | 2; + else if (e0 == b && e1 == c) + return (int(i) << 2) | 0; + else if (e0 == c && e1 == a) + return (int(i) << 2) | 1; + } + + return -1; +} + +} // namespace meshopt + +size_t meshopt_stripify(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, unsigned int restart_index) +{ + assert(destination != indices); + assert(index_count % 3 == 0); + + using namespace meshopt; + + meshopt_Allocator allocator; + + const size_t buffer_capacity = 8; + + unsigned int buffer[buffer_capacity][3] = {}; + unsigned int buffer_size = 0; + + size_t index_offset = 0; + + unsigned int strip[2] = {}; + unsigned int parity = 0; + + size_t strip_size = 0; + + // compute vertex valence; this is used to prioritize starting triangle for strips + unsigned int* valence = allocator.allocate<unsigned int>(vertex_count); + memset(valence, 0, vertex_count * sizeof(unsigned int)); + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices[i]; + assert(index < vertex_count); + + valence[index]++; + } + + int next = -1; + + while (buffer_size > 0 || index_offset < index_count) + { + assert(next < 0 || (size_t(next >> 2) < buffer_size && (next & 3) < 3)); + + // fill triangle buffer + while (buffer_size < buffer_capacity && index_offset < index_count) + { + buffer[buffer_size][0] = indices[index_offset + 0]; + buffer[buffer_size][1] = indices[index_offset + 1]; + buffer[buffer_size][2] = indices[index_offset + 2]; + + buffer_size++; + index_offset += 3; + } + + assert(buffer_size > 0); + + if (next >= 0) + { + unsigned int i = next >> 2; + unsigned int a = buffer[i][0], b = buffer[i][1], c = buffer[i][2]; + unsigned int v = buffer[i][next & 3]; + + // ordered removal from the buffer + memmove(buffer[i], buffer[i + 1], (buffer_size - i - 1) * sizeof(buffer[0])); + buffer_size--; + + // update vertex valences for strip start heuristic + valence[a]--; + valence[b]--; + valence[c]--; + + // find next triangle (note that edge order flips on every iteration) + // in some cases we need to perform a swap to pick a different outgoing triangle edge + // for [a b c], the default strip edge is [b c], but we might want to use [a c] + int cont = findStripNext(buffer, buffer_size, parity ? strip[1] : v, parity ? v : strip[1]); + int swap = cont < 0 ? findStripNext(buffer, buffer_size, parity ? v : strip[0], parity ? strip[0] : v) : -1; + + if (cont < 0 && swap >= 0) + { + // [a b c] => [a b a c] + destination[strip_size++] = strip[0]; + destination[strip_size++] = v; + + // next strip has same winding + // ? a b => b a v + strip[1] = v; + + next = swap; + } + else + { + // emit the next vertex in the strip + destination[strip_size++] = v; + + // next strip has flipped winding + strip[0] = strip[1]; + strip[1] = v; + parity ^= 1; + + next = cont; + } + } + else + { + // if we didn't find anything, we need to find the next new triangle + // we use a heuristic to maximize the strip length + unsigned int i = findStripFirst(buffer, buffer_size, &valence[0]); + unsigned int a = buffer[i][0], b = buffer[i][1], c = buffer[i][2]; + + // ordered removal from the buffer + memmove(buffer[i], buffer[i + 1], (buffer_size - i - 1) * sizeof(buffer[0])); + buffer_size--; + + // update vertex valences for strip start heuristic + valence[a]--; + valence[b]--; + valence[c]--; + + // we need to pre-rotate the triangle so that we will find a match in the existing buffer on the next iteration + int ea = findStripNext(buffer, buffer_size, c, b); + int eb = findStripNext(buffer, buffer_size, a, c); + int ec = findStripNext(buffer, buffer_size, b, a); + + // in some cases we can have several matching edges; since we can pick any edge, we pick the one with the smallest + // triangle index in the buffer. this reduces the effect of stripification on ACMR and additionally - for unclear + // reasons - slightly improves the stripification efficiency + int mine = INT_MAX; + mine = (ea >= 0 && mine > ea) ? ea : mine; + mine = (eb >= 0 && mine > eb) ? eb : mine; + mine = (ec >= 0 && mine > ec) ? ec : mine; + + if (ea == mine) + { + // keep abc + next = ea; + } + else if (eb == mine) + { + // abc -> bca + unsigned int t = a; + a = b, b = c, c = t; + + next = eb; + } + else if (ec == mine) + { + // abc -> cab + unsigned int t = c; + c = b, b = a, a = t; + + next = ec; + } + + if (restart_index) + { + if (strip_size) + destination[strip_size++] = restart_index; + + destination[strip_size++] = a; + destination[strip_size++] = b; + destination[strip_size++] = c; + + // new strip always starts with the same edge winding + strip[0] = b; + strip[1] = c; + parity = 1; + } + else + { + if (strip_size) + { + // connect last strip using degenerate triangles + destination[strip_size++] = strip[1]; + destination[strip_size++] = a; + } + + // note that we may need to flip the emitted triangle based on parity + // we always end up with outgoing edge "cb" in the end + unsigned int e0 = parity ? c : b; + unsigned int e1 = parity ? b : c; + + destination[strip_size++] = a; + destination[strip_size++] = e0; + destination[strip_size++] = e1; + + strip[0] = e0; + strip[1] = e1; + parity ^= 1; + } + } + } + + return strip_size; +} + +size_t meshopt_stripifyBound(size_t index_count) +{ + assert(index_count % 3 == 0); + + // worst case without restarts is 2 degenerate indices and 3 indices per triangle + // worst case with restarts is 1 restart index and 3 indices per triangle + return (index_count / 3) * 5; +} + +size_t meshopt_unstripify(unsigned int* destination, const unsigned int* indices, size_t index_count, unsigned int restart_index) +{ + assert(destination != indices); + + size_t offset = 0; + size_t start = 0; + + for (size_t i = 0; i < index_count; ++i) + { + if (restart_index && indices[i] == restart_index) + { + start = i + 1; + } + else if (i - start >= 2) + { + unsigned int a = indices[i - 2], b = indices[i - 1], c = indices[i]; + + // flip winding for odd triangles + if ((i - start) & 1) + { + unsigned int t = a; + a = b, b = t; + } + + // although we use restart indices, strip swaps still produce degenerate triangles, so skip them + if (a != b && a != c && b != c) + { + destination[offset + 0] = a; + destination[offset + 1] = b; + destination[offset + 2] = c; + offset += 3; + } + } + } + + return offset; +} + +size_t meshopt_unstripifyBound(size_t index_count) +{ + assert(index_count == 0 || index_count >= 3); + + return (index_count == 0) ? 0 : (index_count - 2) * 3; +} diff --git a/thirdparty/meshoptimizer/vcacheanalyzer.cpp b/thirdparty/meshoptimizer/vcacheanalyzer.cpp new file mode 100644 index 0000000000..3682743820 --- /dev/null +++ b/thirdparty/meshoptimizer/vcacheanalyzer.cpp @@ -0,0 +1,73 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <string.h> + +meshopt_VertexCacheStatistics meshopt_analyzeVertexCache(const unsigned int* indices, size_t index_count, size_t vertex_count, unsigned int cache_size, unsigned int warp_size, unsigned int primgroup_size) +{ + assert(index_count % 3 == 0); + assert(cache_size >= 3); + assert(warp_size == 0 || warp_size >= 3); + + meshopt_Allocator allocator; + + meshopt_VertexCacheStatistics result = {}; + + unsigned int warp_offset = 0; + unsigned int primgroup_offset = 0; + + unsigned int* cache_timestamps = allocator.allocate<unsigned int>(vertex_count); + memset(cache_timestamps, 0, vertex_count * sizeof(unsigned int)); + + unsigned int timestamp = cache_size + 1; + + for (size_t i = 0; i < index_count; i += 3) + { + unsigned int a = indices[i + 0], b = indices[i + 1], c = indices[i + 2]; + assert(a < vertex_count && b < vertex_count && c < vertex_count); + + bool ac = (timestamp - cache_timestamps[a]) > cache_size; + bool bc = (timestamp - cache_timestamps[b]) > cache_size; + bool cc = (timestamp - cache_timestamps[c]) > cache_size; + + // flush cache if triangle doesn't fit into warp or into the primitive buffer + if ((primgroup_size && primgroup_offset == primgroup_size) || (warp_size && warp_offset + ac + bc + cc > warp_size)) + { + result.warps_executed += warp_offset > 0; + + warp_offset = 0; + primgroup_offset = 0; + + // reset cache + timestamp += cache_size + 1; + } + + // update cache and add vertices to warp + for (int j = 0; j < 3; ++j) + { + unsigned int index = indices[i + j]; + + if (timestamp - cache_timestamps[index] > cache_size) + { + cache_timestamps[index] = timestamp++; + result.vertices_transformed++; + warp_offset++; + } + } + + primgroup_offset++; + } + + size_t unique_vertex_count = 0; + + for (size_t i = 0; i < vertex_count; ++i) + unique_vertex_count += cache_timestamps[i] > 0; + + result.warps_executed += warp_offset > 0; + + result.acmr = index_count == 0 ? 0 : float(result.vertices_transformed) / float(index_count / 3); + result.atvr = unique_vertex_count == 0 ? 0 : float(result.vertices_transformed) / float(unique_vertex_count); + + return result; +} diff --git a/thirdparty/meshoptimizer/vcacheoptimizer.cpp b/thirdparty/meshoptimizer/vcacheoptimizer.cpp new file mode 100644 index 0000000000..fb8ade4b77 --- /dev/null +++ b/thirdparty/meshoptimizer/vcacheoptimizer.cpp @@ -0,0 +1,473 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <string.h> + +// This work is based on: +// Tom Forsyth. Linear-Speed Vertex Cache Optimisation. 2006 +// Pedro Sander, Diego Nehab and Joshua Barczak. Fast Triangle Reordering for Vertex Locality and Reduced Overdraw. 2007 +namespace meshopt +{ + +const size_t kCacheSizeMax = 16; +const size_t kValenceMax = 8; + +struct VertexScoreTable +{ + float cache[1 + kCacheSizeMax]; + float live[1 + kValenceMax]; +}; + +// Tuned to minimize the ACMR of a GPU that has a cache profile similar to NVidia and AMD +static const VertexScoreTable kVertexScoreTable = { + {0.f, 0.779f, 0.791f, 0.789f, 0.981f, 0.843f, 0.726f, 0.847f, 0.882f, 0.867f, 0.799f, 0.642f, 0.613f, 0.600f, 0.568f, 0.372f, 0.234f}, + {0.f, 0.995f, 0.713f, 0.450f, 0.404f, 0.059f, 0.005f, 0.147f, 0.006f}, +}; + +// Tuned to minimize the encoded index buffer size +static const VertexScoreTable kVertexScoreTableStrip = { + {0.f, 1.000f, 1.000f, 1.000f, 0.453f, 0.561f, 0.490f, 0.459f, 0.179f, 0.526f, 0.000f, 0.227f, 0.184f, 0.490f, 0.112f, 0.050f, 0.131f}, + {0.f, 0.956f, 0.786f, 0.577f, 0.558f, 0.618f, 0.549f, 0.499f, 0.489f}, +}; + +struct TriangleAdjacency +{ + unsigned int* counts; + unsigned int* offsets; + unsigned int* data; +}; + +static void buildTriangleAdjacency(TriangleAdjacency& adjacency, const unsigned int* indices, size_t index_count, size_t vertex_count, meshopt_Allocator& allocator) +{ + size_t face_count = index_count / 3; + + // allocate arrays + adjacency.counts = allocator.allocate<unsigned int>(vertex_count); + adjacency.offsets = allocator.allocate<unsigned int>(vertex_count); + adjacency.data = allocator.allocate<unsigned int>(index_count); + + // fill triangle counts + memset(adjacency.counts, 0, vertex_count * sizeof(unsigned int)); + + for (size_t i = 0; i < index_count; ++i) + { + assert(indices[i] < vertex_count); + + adjacency.counts[indices[i]]++; + } + + // fill offset table + unsigned int offset = 0; + + for (size_t i = 0; i < vertex_count; ++i) + { + adjacency.offsets[i] = offset; + offset += adjacency.counts[i]; + } + + assert(offset == index_count); + + // fill triangle data + for (size_t i = 0; i < face_count; ++i) + { + unsigned int a = indices[i * 3 + 0], b = indices[i * 3 + 1], c = indices[i * 3 + 2]; + + adjacency.data[adjacency.offsets[a]++] = unsigned(i); + adjacency.data[adjacency.offsets[b]++] = unsigned(i); + adjacency.data[adjacency.offsets[c]++] = unsigned(i); + } + + // fix offsets that have been disturbed by the previous pass + for (size_t i = 0; i < vertex_count; ++i) + { + assert(adjacency.offsets[i] >= adjacency.counts[i]); + + adjacency.offsets[i] -= adjacency.counts[i]; + } +} + +static unsigned int getNextVertexDeadEnd(const unsigned int* dead_end, unsigned int& dead_end_top, unsigned int& input_cursor, const unsigned int* live_triangles, size_t vertex_count) +{ + // check dead-end stack + while (dead_end_top) + { + unsigned int vertex = dead_end[--dead_end_top]; + + if (live_triangles[vertex] > 0) + return vertex; + } + + // input order + while (input_cursor < vertex_count) + { + if (live_triangles[input_cursor] > 0) + return input_cursor; + + ++input_cursor; + } + + return ~0u; +} + +static unsigned int getNextVertexNeighbour(const unsigned int* next_candidates_begin, const unsigned int* next_candidates_end, const unsigned int* live_triangles, const unsigned int* cache_timestamps, unsigned int timestamp, unsigned int cache_size) +{ + unsigned int best_candidate = ~0u; + int best_priority = -1; + + for (const unsigned int* next_candidate = next_candidates_begin; next_candidate != next_candidates_end; ++next_candidate) + { + unsigned int vertex = *next_candidate; + + // otherwise we don't need to process it + if (live_triangles[vertex] > 0) + { + int priority = 0; + + // will it be in cache after fanning? + if (2 * live_triangles[vertex] + timestamp - cache_timestamps[vertex] <= cache_size) + { + priority = timestamp - cache_timestamps[vertex]; // position in cache + } + + if (priority > best_priority) + { + best_candidate = vertex; + best_priority = priority; + } + } + } + + return best_candidate; +} + +static float vertexScore(const VertexScoreTable* table, int cache_position, unsigned int live_triangles) +{ + assert(cache_position >= -1 && cache_position < int(kCacheSizeMax)); + + unsigned int live_triangles_clamped = live_triangles < kValenceMax ? live_triangles : kValenceMax; + + return table->cache[1 + cache_position] + table->live[live_triangles_clamped]; +} + +static unsigned int getNextTriangleDeadEnd(unsigned int& input_cursor, const unsigned char* emitted_flags, size_t face_count) +{ + // input order + while (input_cursor < face_count) + { + if (!emitted_flags[input_cursor]) + return input_cursor; + + ++input_cursor; + } + + return ~0u; +} + +} // namespace meshopt + +void meshopt_optimizeVertexCacheTable(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, const meshopt::VertexScoreTable* table) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + + meshopt_Allocator allocator; + + // guard for empty meshes + if (index_count == 0 || vertex_count == 0) + return; + + // support in-place optimization + if (destination == indices) + { + unsigned int* indices_copy = allocator.allocate<unsigned int>(index_count); + memcpy(indices_copy, indices, index_count * sizeof(unsigned int)); + indices = indices_copy; + } + + unsigned int cache_size = 16; + assert(cache_size <= kCacheSizeMax); + + size_t face_count = index_count / 3; + + // build adjacency information + TriangleAdjacency adjacency = {}; + buildTriangleAdjacency(adjacency, indices, index_count, vertex_count, allocator); + + // live triangle counts + unsigned int* live_triangles = allocator.allocate<unsigned int>(vertex_count); + memcpy(live_triangles, adjacency.counts, vertex_count * sizeof(unsigned int)); + + // emitted flags + unsigned char* emitted_flags = allocator.allocate<unsigned char>(face_count); + memset(emitted_flags, 0, face_count); + + // compute initial vertex scores + float* vertex_scores = allocator.allocate<float>(vertex_count); + + for (size_t i = 0; i < vertex_count; ++i) + vertex_scores[i] = vertexScore(table, -1, live_triangles[i]); + + // compute triangle scores + float* triangle_scores = allocator.allocate<float>(face_count); + + for (size_t i = 0; i < face_count; ++i) + { + unsigned int a = indices[i * 3 + 0]; + unsigned int b = indices[i * 3 + 1]; + unsigned int c = indices[i * 3 + 2]; + + triangle_scores[i] = vertex_scores[a] + vertex_scores[b] + vertex_scores[c]; + } + + unsigned int cache_holder[2 * (kCacheSizeMax + 3)]; + unsigned int* cache = cache_holder; + unsigned int* cache_new = cache_holder + kCacheSizeMax + 3; + size_t cache_count = 0; + + unsigned int current_triangle = 0; + unsigned int input_cursor = 1; + + unsigned int output_triangle = 0; + + while (current_triangle != ~0u) + { + assert(output_triangle < face_count); + + unsigned int a = indices[current_triangle * 3 + 0]; + unsigned int b = indices[current_triangle * 3 + 1]; + unsigned int c = indices[current_triangle * 3 + 2]; + + // output indices + destination[output_triangle * 3 + 0] = a; + destination[output_triangle * 3 + 1] = b; + destination[output_triangle * 3 + 2] = c; + output_triangle++; + + // update emitted flags + emitted_flags[current_triangle] = true; + triangle_scores[current_triangle] = 0; + + // new triangle + size_t cache_write = 0; + cache_new[cache_write++] = a; + cache_new[cache_write++] = b; + cache_new[cache_write++] = c; + + // old triangles + for (size_t i = 0; i < cache_count; ++i) + { + unsigned int index = cache[i]; + + if (index != a && index != b && index != c) + { + cache_new[cache_write++] = index; + } + } + + unsigned int* cache_temp = cache; + cache = cache_new, cache_new = cache_temp; + cache_count = cache_write > cache_size ? cache_size : cache_write; + + // update live triangle counts + live_triangles[a]--; + live_triangles[b]--; + live_triangles[c]--; + + // remove emitted triangle from adjacency data + // this makes sure that we spend less time traversing these lists on subsequent iterations + for (size_t k = 0; k < 3; ++k) + { + unsigned int index = indices[current_triangle * 3 + k]; + + unsigned int* neighbours = &adjacency.data[0] + adjacency.offsets[index]; + size_t neighbours_size = adjacency.counts[index]; + + for (size_t i = 0; i < neighbours_size; ++i) + { + unsigned int tri = neighbours[i]; + + if (tri == current_triangle) + { + neighbours[i] = neighbours[neighbours_size - 1]; + adjacency.counts[index]--; + break; + } + } + } + + unsigned int best_triangle = ~0u; + float best_score = 0; + + // update cache positions, vertex scores and triangle scores, and find next best triangle + for (size_t i = 0; i < cache_write; ++i) + { + unsigned int index = cache[i]; + + int cache_position = i >= cache_size ? -1 : int(i); + + // update vertex score + float score = vertexScore(table, cache_position, live_triangles[index]); + float score_diff = score - vertex_scores[index]; + + vertex_scores[index] = score; + + // update scores of vertex triangles + const unsigned int* neighbours_begin = &adjacency.data[0] + adjacency.offsets[index]; + const unsigned int* neighbours_end = neighbours_begin + adjacency.counts[index]; + + for (const unsigned int* it = neighbours_begin; it != neighbours_end; ++it) + { + unsigned int tri = *it; + assert(!emitted_flags[tri]); + + float tri_score = triangle_scores[tri] + score_diff; + assert(tri_score > 0); + + if (best_score < tri_score) + { + best_triangle = tri; + best_score = tri_score; + } + + triangle_scores[tri] = tri_score; + } + } + + // step through input triangles in order if we hit a dead-end + current_triangle = best_triangle; + + if (current_triangle == ~0u) + { + current_triangle = getNextTriangleDeadEnd(input_cursor, &emitted_flags[0], face_count); + } + } + + assert(input_cursor == face_count); + assert(output_triangle == face_count); +} + +void meshopt_optimizeVertexCache(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count) +{ + meshopt_optimizeVertexCacheTable(destination, indices, index_count, vertex_count, &meshopt::kVertexScoreTable); +} + +void meshopt_optimizeVertexCacheStrip(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count) +{ + meshopt_optimizeVertexCacheTable(destination, indices, index_count, vertex_count, &meshopt::kVertexScoreTableStrip); +} + +void meshopt_optimizeVertexCacheFifo(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count, unsigned int cache_size) +{ + using namespace meshopt; + + assert(index_count % 3 == 0); + assert(cache_size >= 3); + + meshopt_Allocator allocator; + + // guard for empty meshes + if (index_count == 0 || vertex_count == 0) + return; + + // support in-place optimization + if (destination == indices) + { + unsigned int* indices_copy = allocator.allocate<unsigned int>(index_count); + memcpy(indices_copy, indices, index_count * sizeof(unsigned int)); + indices = indices_copy; + } + + size_t face_count = index_count / 3; + + // build adjacency information + TriangleAdjacency adjacency = {}; + buildTriangleAdjacency(adjacency, indices, index_count, vertex_count, allocator); + + // live triangle counts + unsigned int* live_triangles = allocator.allocate<unsigned int>(vertex_count); + memcpy(live_triangles, adjacency.counts, vertex_count * sizeof(unsigned int)); + + // cache time stamps + unsigned int* cache_timestamps = allocator.allocate<unsigned int>(vertex_count); + memset(cache_timestamps, 0, vertex_count * sizeof(unsigned int)); + + // dead-end stack + unsigned int* dead_end = allocator.allocate<unsigned int>(index_count); + unsigned int dead_end_top = 0; + + // emitted flags + unsigned char* emitted_flags = allocator.allocate<unsigned char>(face_count); + memset(emitted_flags, 0, face_count); + + unsigned int current_vertex = 0; + + unsigned int timestamp = cache_size + 1; + unsigned int input_cursor = 1; // vertex to restart from in case of dead-end + + unsigned int output_triangle = 0; + + while (current_vertex != ~0u) + { + const unsigned int* next_candidates_begin = &dead_end[0] + dead_end_top; + + // emit all vertex neighbours + const unsigned int* neighbours_begin = &adjacency.data[0] + adjacency.offsets[current_vertex]; + const unsigned int* neighbours_end = neighbours_begin + adjacency.counts[current_vertex]; + + for (const unsigned int* it = neighbours_begin; it != neighbours_end; ++it) + { + unsigned int triangle = *it; + + if (!emitted_flags[triangle]) + { + unsigned int a = indices[triangle * 3 + 0], b = indices[triangle * 3 + 1], c = indices[triangle * 3 + 2]; + + // output indices + destination[output_triangle * 3 + 0] = a; + destination[output_triangle * 3 + 1] = b; + destination[output_triangle * 3 + 2] = c; + output_triangle++; + + // update dead-end stack + dead_end[dead_end_top + 0] = a; + dead_end[dead_end_top + 1] = b; + dead_end[dead_end_top + 2] = c; + dead_end_top += 3; + + // update live triangle counts + live_triangles[a]--; + live_triangles[b]--; + live_triangles[c]--; + + // update cache info + // if vertex is not in cache, put it in cache + if (timestamp - cache_timestamps[a] > cache_size) + cache_timestamps[a] = timestamp++; + + if (timestamp - cache_timestamps[b] > cache_size) + cache_timestamps[b] = timestamp++; + + if (timestamp - cache_timestamps[c] > cache_size) + cache_timestamps[c] = timestamp++; + + // update emitted flags + emitted_flags[triangle] = true; + } + } + + // next candidates are the ones we pushed to dead-end stack just now + const unsigned int* next_candidates_end = &dead_end[0] + dead_end_top; + + // get next vertex + current_vertex = getNextVertexNeighbour(next_candidates_begin, next_candidates_end, &live_triangles[0], &cache_timestamps[0], timestamp, cache_size); + + if (current_vertex == ~0u) + { + current_vertex = getNextVertexDeadEnd(&dead_end[0], dead_end_top, input_cursor, &live_triangles[0], vertex_count); + } + } + + assert(output_triangle == face_count); +} diff --git a/thirdparty/meshoptimizer/vertexcodec.cpp b/thirdparty/meshoptimizer/vertexcodec.cpp new file mode 100644 index 0000000000..784c9a13db --- /dev/null +++ b/thirdparty/meshoptimizer/vertexcodec.cpp @@ -0,0 +1,1265 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <string.h> + +// The block below auto-detects SIMD ISA that can be used on the target platform +#ifndef MESHOPTIMIZER_NO_SIMD + +// The SIMD implementation requires SSSE3, which can be enabled unconditionally through compiler settings +#if defined(__AVX__) || defined(__SSSE3__) +#define SIMD_SSE +#endif + +// An experimental implementation using AVX512 instructions; it's only enabled when AVX512 is enabled through compiler settings +#if defined(__AVX512VBMI2__) && defined(__AVX512VBMI__) && defined(__AVX512VL__) && defined(__POPCNT__) +#undef SIMD_SSE +#define SIMD_AVX +#endif + +// MSVC supports compiling SSSE3 code regardless of compile options; we use a cpuid-based scalar fallback +#if !defined(SIMD_SSE) && !defined(SIMD_AVX) && defined(_MSC_VER) && !defined(__clang__) && (defined(_M_IX86) || defined(_M_X64)) +#define SIMD_SSE +#define SIMD_FALLBACK +#endif + +// GCC 4.9+ and clang 3.8+ support targeting SIMD ISA from individual functions; we use a cpuid-based scalar fallback +#if !defined(SIMD_SSE) && !defined(SIMD_AVX) && ((defined(__clang__) && __clang_major__ * 100 + __clang_minor__ >= 308) || (defined(__GNUC__) && __GNUC__ * 100 + __GNUC_MINOR__ >= 409)) && (defined(__i386__) || defined(__x86_64__)) +#define SIMD_SSE +#define SIMD_FALLBACK +#define SIMD_TARGET __attribute__((target("ssse3"))) +#endif + +// GCC/clang define these when NEON support is available +#if defined(__ARM_NEON__) || defined(__ARM_NEON) +#define SIMD_NEON +#endif + +// On MSVC, we assume that ARM builds always target NEON-capable devices +#if !defined(SIMD_NEON) && defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64)) +#define SIMD_NEON +#endif + +// When targeting Wasm SIMD we can't use runtime cpuid checks so we unconditionally enable SIMD +#if defined(__wasm_simd128__) +#define SIMD_WASM +#endif + +#ifndef SIMD_TARGET +#define SIMD_TARGET +#endif + +#endif // !MESHOPTIMIZER_NO_SIMD + +#ifdef SIMD_SSE +#include <tmmintrin.h> +#endif + +#if defined(SIMD_SSE) && defined(SIMD_FALLBACK) +#ifdef _MSC_VER +#include <intrin.h> // __cpuid +#else +#include <cpuid.h> // __cpuid +#endif +#endif + +#ifdef SIMD_AVX +#include <immintrin.h> +#endif + +#ifdef SIMD_NEON +#if defined(_MSC_VER) && defined(_M_ARM64) +#include <arm64_neon.h> +#else +#include <arm_neon.h> +#endif +#endif + +#ifdef SIMD_WASM +#include <wasm_simd128.h> +#endif + +#ifndef TRACE +#define TRACE 0 +#endif + +#if TRACE +#include <stdio.h> +#endif + +#ifdef SIMD_WASM +#define wasmx_splat_v32x4(v, i) wasm_v32x4_shuffle(v, v, i, i, i, i) +#define wasmx_unpacklo_v8x16(a, b) wasm_v8x16_shuffle(a, b, 0, 16, 1, 17, 2, 18, 3, 19, 4, 20, 5, 21, 6, 22, 7, 23) +#define wasmx_unpackhi_v8x16(a, b) wasm_v8x16_shuffle(a, b, 8, 24, 9, 25, 10, 26, 11, 27, 12, 28, 13, 29, 14, 30, 15, 31) +#define wasmx_unpacklo_v16x8(a, b) wasm_v16x8_shuffle(a, b, 0, 8, 1, 9, 2, 10, 3, 11) +#define wasmx_unpackhi_v16x8(a, b) wasm_v16x8_shuffle(a, b, 4, 12, 5, 13, 6, 14, 7, 15) +#define wasmx_unpacklo_v64x2(a, b) wasm_v64x2_shuffle(a, b, 0, 2) +#define wasmx_unpackhi_v64x2(a, b) wasm_v64x2_shuffle(a, b, 1, 3) +#endif + +namespace meshopt +{ + +const unsigned char kVertexHeader = 0xa0; + +static int gEncodeVertexVersion = 0; + +const size_t kVertexBlockSizeBytes = 8192; +const size_t kVertexBlockMaxSize = 256; +const size_t kByteGroupSize = 16; +const size_t kByteGroupDecodeLimit = 24; +const size_t kTailMaxSize = 32; + +static size_t getVertexBlockSize(size_t vertex_size) +{ + // make sure the entire block fits into the scratch buffer + size_t result = kVertexBlockSizeBytes / vertex_size; + + // align to byte group size; we encode each byte as a byte group + // if vertex block is misaligned, it results in wasted bytes, so just truncate the block size + result &= ~(kByteGroupSize - 1); + + return (result < kVertexBlockMaxSize) ? result : kVertexBlockMaxSize; +} + +inline unsigned char zigzag8(unsigned char v) +{ + return ((signed char)(v) >> 7) ^ (v << 1); +} + +inline unsigned char unzigzag8(unsigned char v) +{ + return -(v & 1) ^ (v >> 1); +} + +#if TRACE +struct Stats +{ + size_t size; + size_t header; + size_t bitg[4]; + size_t bitb[4]; +}; + +Stats* bytestats; +Stats vertexstats[256]; +#endif + +static bool encodeBytesGroupZero(const unsigned char* buffer) +{ + for (size_t i = 0; i < kByteGroupSize; ++i) + if (buffer[i]) + return false; + + return true; +} + +static size_t encodeBytesGroupMeasure(const unsigned char* buffer, int bits) +{ + assert(bits >= 1 && bits <= 8); + + if (bits == 1) + return encodeBytesGroupZero(buffer) ? 0 : size_t(-1); + + if (bits == 8) + return kByteGroupSize; + + size_t result = kByteGroupSize * bits / 8; + + unsigned char sentinel = (1 << bits) - 1; + + for (size_t i = 0; i < kByteGroupSize; ++i) + result += buffer[i] >= sentinel; + + return result; +} + +static unsigned char* encodeBytesGroup(unsigned char* data, const unsigned char* buffer, int bits) +{ + assert(bits >= 1 && bits <= 8); + + if (bits == 1) + return data; + + if (bits == 8) + { + memcpy(data, buffer, kByteGroupSize); + return data + kByteGroupSize; + } + + size_t byte_size = 8 / bits; + assert(kByteGroupSize % byte_size == 0); + + // fixed portion: bits bits for each value + // variable portion: full byte for each out-of-range value (using 1...1 as sentinel) + unsigned char sentinel = (1 << bits) - 1; + + for (size_t i = 0; i < kByteGroupSize; i += byte_size) + { + unsigned char byte = 0; + + for (size_t k = 0; k < byte_size; ++k) + { + unsigned char enc = (buffer[i + k] >= sentinel) ? sentinel : buffer[i + k]; + + byte <<= bits; + byte |= enc; + } + + *data++ = byte; + } + + for (size_t i = 0; i < kByteGroupSize; ++i) + { + if (buffer[i] >= sentinel) + { + *data++ = buffer[i]; + } + } + + return data; +} + +static unsigned char* encodeBytes(unsigned char* data, unsigned char* data_end, const unsigned char* buffer, size_t buffer_size) +{ + assert(buffer_size % kByteGroupSize == 0); + + unsigned char* header = data; + + // round number of groups to 4 to get number of header bytes + size_t header_size = (buffer_size / kByteGroupSize + 3) / 4; + + if (size_t(data_end - data) < header_size) + return 0; + + data += header_size; + + memset(header, 0, header_size); + + for (size_t i = 0; i < buffer_size; i += kByteGroupSize) + { + if (size_t(data_end - data) < kByteGroupDecodeLimit) + return 0; + + int best_bits = 8; + size_t best_size = encodeBytesGroupMeasure(buffer + i, 8); + + for (int bits = 1; bits < 8; bits *= 2) + { + size_t size = encodeBytesGroupMeasure(buffer + i, bits); + + if (size < best_size) + { + best_bits = bits; + best_size = size; + } + } + + int bitslog2 = (best_bits == 1) ? 0 : (best_bits == 2) ? 1 : (best_bits == 4) ? 2 : 3; + assert((1 << bitslog2) == best_bits); + + size_t header_offset = i / kByteGroupSize; + + header[header_offset / 4] |= bitslog2 << ((header_offset % 4) * 2); + + unsigned char* next = encodeBytesGroup(data, buffer + i, best_bits); + + assert(data + best_size == next); + data = next; + +#if TRACE > 1 + bytestats->bitg[bitslog2]++; + bytestats->bitb[bitslog2] += best_size; +#endif + } + +#if TRACE > 1 + bytestats->header += header_size; +#endif + + return data; +} + +static unsigned char* encodeVertexBlock(unsigned char* data, unsigned char* data_end, const unsigned char* vertex_data, size_t vertex_count, size_t vertex_size, unsigned char last_vertex[256]) +{ + assert(vertex_count > 0 && vertex_count <= kVertexBlockMaxSize); + + unsigned char buffer[kVertexBlockMaxSize]; + assert(sizeof(buffer) % kByteGroupSize == 0); + + // we sometimes encode elements we didn't fill when rounding to kByteGroupSize + memset(buffer, 0, sizeof(buffer)); + + for (size_t k = 0; k < vertex_size; ++k) + { + size_t vertex_offset = k; + + unsigned char p = last_vertex[k]; + + for (size_t i = 0; i < vertex_count; ++i) + { + buffer[i] = zigzag8(vertex_data[vertex_offset] - p); + + p = vertex_data[vertex_offset]; + + vertex_offset += vertex_size; + } + +#if TRACE + const unsigned char* olddata = data; + bytestats = &vertexstats[k]; +#endif + + data = encodeBytes(data, data_end, buffer, (vertex_count + kByteGroupSize - 1) & ~(kByteGroupSize - 1)); + if (!data) + return 0; + +#if TRACE + bytestats = 0; + vertexstats[k].size += data - olddata; +#endif + } + + memcpy(last_vertex, &vertex_data[vertex_size * (vertex_count - 1)], vertex_size); + + return data; +} + +#if defined(SIMD_FALLBACK) || (!defined(SIMD_SSE) && !defined(SIMD_NEON) && !defined(SIMD_AVX)) +static const unsigned char* decodeBytesGroup(const unsigned char* data, unsigned char* buffer, int bitslog2) +{ +#define READ() byte = *data++ +#define NEXT(bits) enc = byte >> (8 - bits), byte <<= bits, encv = *data_var, *buffer++ = (enc == (1 << bits) - 1) ? encv : enc, data_var += (enc == (1 << bits) - 1) + + unsigned char byte, enc, encv; + const unsigned char* data_var; + + switch (bitslog2) + { + case 0: + memset(buffer, 0, kByteGroupSize); + return data; + case 1: + data_var = data + 4; + + // 4 groups with 4 2-bit values in each byte + READ(), NEXT(2), NEXT(2), NEXT(2), NEXT(2); + READ(), NEXT(2), NEXT(2), NEXT(2), NEXT(2); + READ(), NEXT(2), NEXT(2), NEXT(2), NEXT(2); + READ(), NEXT(2), NEXT(2), NEXT(2), NEXT(2); + + return data_var; + case 2: + data_var = data + 8; + + // 8 groups with 2 4-bit values in each byte + READ(), NEXT(4), NEXT(4); + READ(), NEXT(4), NEXT(4); + READ(), NEXT(4), NEXT(4); + READ(), NEXT(4), NEXT(4); + READ(), NEXT(4), NEXT(4); + READ(), NEXT(4), NEXT(4); + READ(), NEXT(4), NEXT(4); + READ(), NEXT(4), NEXT(4); + + return data_var; + case 3: + memcpy(buffer, data, kByteGroupSize); + return data + kByteGroupSize; + default: + assert(!"Unexpected bit length"); // unreachable since bitslog2 is a 2-bit value + return data; + } + +#undef READ +#undef NEXT +} + +static const unsigned char* decodeBytes(const unsigned char* data, const unsigned char* data_end, unsigned char* buffer, size_t buffer_size) +{ + assert(buffer_size % kByteGroupSize == 0); + + const unsigned char* header = data; + + // round number of groups to 4 to get number of header bytes + size_t header_size = (buffer_size / kByteGroupSize + 3) / 4; + + if (size_t(data_end - data) < header_size) + return 0; + + data += header_size; + + for (size_t i = 0; i < buffer_size; i += kByteGroupSize) + { + if (size_t(data_end - data) < kByteGroupDecodeLimit) + return 0; + + size_t header_offset = i / kByteGroupSize; + + int bitslog2 = (header[header_offset / 4] >> ((header_offset % 4) * 2)) & 3; + + data = decodeBytesGroup(data, buffer + i, bitslog2); + } + + return data; +} + +static const unsigned char* decodeVertexBlock(const unsigned char* data, const unsigned char* data_end, unsigned char* vertex_data, size_t vertex_count, size_t vertex_size, unsigned char last_vertex[256]) +{ + assert(vertex_count > 0 && vertex_count <= kVertexBlockMaxSize); + + unsigned char buffer[kVertexBlockMaxSize]; + unsigned char transposed[kVertexBlockSizeBytes]; + + size_t vertex_count_aligned = (vertex_count + kByteGroupSize - 1) & ~(kByteGroupSize - 1); + + for (size_t k = 0; k < vertex_size; ++k) + { + data = decodeBytes(data, data_end, buffer, vertex_count_aligned); + if (!data) + return 0; + + size_t vertex_offset = k; + + unsigned char p = last_vertex[k]; + + for (size_t i = 0; i < vertex_count; ++i) + { + unsigned char v = unzigzag8(buffer[i]) + p; + + transposed[vertex_offset] = v; + p = v; + + vertex_offset += vertex_size; + } + } + + memcpy(vertex_data, transposed, vertex_count * vertex_size); + + memcpy(last_vertex, &transposed[vertex_size * (vertex_count - 1)], vertex_size); + + return data; +} +#endif + +#if defined(SIMD_SSE) || defined(SIMD_NEON) || defined(SIMD_WASM) +static unsigned char kDecodeBytesGroupShuffle[256][8]; +static unsigned char kDecodeBytesGroupCount[256]; + +#ifdef __wasm__ +__attribute__((cold)) // this saves 500 bytes in the output binary - we don't need to vectorize this loop! +#endif +static bool +decodeBytesGroupBuildTables() +{ + for (int mask = 0; mask < 256; ++mask) + { + unsigned char shuffle[8]; + unsigned char count = 0; + + for (int i = 0; i < 8; ++i) + { + int maski = (mask >> i) & 1; + shuffle[i] = maski ? count : 0x80; + count += (unsigned char)(maski); + } + + memcpy(kDecodeBytesGroupShuffle[mask], shuffle, 8); + kDecodeBytesGroupCount[mask] = count; + } + + return true; +} + +static bool gDecodeBytesGroupInitialized = decodeBytesGroupBuildTables(); +#endif + +#ifdef SIMD_SSE +SIMD_TARGET +static __m128i decodeShuffleMask(unsigned char mask0, unsigned char mask1) +{ + __m128i sm0 = _mm_loadl_epi64(reinterpret_cast<const __m128i*>(&kDecodeBytesGroupShuffle[mask0])); + __m128i sm1 = _mm_loadl_epi64(reinterpret_cast<const __m128i*>(&kDecodeBytesGroupShuffle[mask1])); + __m128i sm1off = _mm_set1_epi8(kDecodeBytesGroupCount[mask0]); + + __m128i sm1r = _mm_add_epi8(sm1, sm1off); + + return _mm_unpacklo_epi64(sm0, sm1r); +} + +SIMD_TARGET +static const unsigned char* decodeBytesGroupSimd(const unsigned char* data, unsigned char* buffer, int bitslog2) +{ + switch (bitslog2) + { + case 0: + { + __m128i result = _mm_setzero_si128(); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(buffer), result); + + return data; + } + + case 1: + { +#ifdef __GNUC__ + typedef int __attribute__((aligned(1))) unaligned_int; +#else + typedef int unaligned_int; +#endif + + __m128i sel2 = _mm_cvtsi32_si128(*reinterpret_cast<const unaligned_int*>(data)); + __m128i rest = _mm_loadu_si128(reinterpret_cast<const __m128i*>(data + 4)); + + __m128i sel22 = _mm_unpacklo_epi8(_mm_srli_epi16(sel2, 4), sel2); + __m128i sel2222 = _mm_unpacklo_epi8(_mm_srli_epi16(sel22, 2), sel22); + __m128i sel = _mm_and_si128(sel2222, _mm_set1_epi8(3)); + + __m128i mask = _mm_cmpeq_epi8(sel, _mm_set1_epi8(3)); + int mask16 = _mm_movemask_epi8(mask); + unsigned char mask0 = (unsigned char)(mask16 & 255); + unsigned char mask1 = (unsigned char)(mask16 >> 8); + + __m128i shuf = decodeShuffleMask(mask0, mask1); + + __m128i result = _mm_or_si128(_mm_shuffle_epi8(rest, shuf), _mm_andnot_si128(mask, sel)); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(buffer), result); + + return data + 4 + kDecodeBytesGroupCount[mask0] + kDecodeBytesGroupCount[mask1]; + } + + case 2: + { + __m128i sel4 = _mm_loadl_epi64(reinterpret_cast<const __m128i*>(data)); + __m128i rest = _mm_loadu_si128(reinterpret_cast<const __m128i*>(data + 8)); + + __m128i sel44 = _mm_unpacklo_epi8(_mm_srli_epi16(sel4, 4), sel4); + __m128i sel = _mm_and_si128(sel44, _mm_set1_epi8(15)); + + __m128i mask = _mm_cmpeq_epi8(sel, _mm_set1_epi8(15)); + int mask16 = _mm_movemask_epi8(mask); + unsigned char mask0 = (unsigned char)(mask16 & 255); + unsigned char mask1 = (unsigned char)(mask16 >> 8); + + __m128i shuf = decodeShuffleMask(mask0, mask1); + + __m128i result = _mm_or_si128(_mm_shuffle_epi8(rest, shuf), _mm_andnot_si128(mask, sel)); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(buffer), result); + + return data + 8 + kDecodeBytesGroupCount[mask0] + kDecodeBytesGroupCount[mask1]; + } + + case 3: + { + __m128i result = _mm_loadu_si128(reinterpret_cast<const __m128i*>(data)); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(buffer), result); + + return data + 16; + } + + default: + assert(!"Unexpected bit length"); // unreachable since bitslog2 is a 2-bit value + return data; + } +} +#endif + +#ifdef SIMD_AVX +static const __m128i decodeBytesGroupConfig[] = { + _mm_set1_epi8(3), + _mm_set1_epi8(15), + _mm_setr_epi8(6, 4, 2, 0, 14, 12, 10, 8, 22, 20, 18, 16, 30, 28, 26, 24), + _mm_setr_epi8(4, 0, 12, 8, 20, 16, 28, 24, 36, 32, 44, 40, 52, 48, 60, 56), +}; + +static const unsigned char* decodeBytesGroupSimd(const unsigned char* data, unsigned char* buffer, int bitslog2) +{ + switch (bitslog2) + { + case 0: + { + __m128i result = _mm_setzero_si128(); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(buffer), result); + + return data; + } + + case 1: + case 2: + { + const unsigned char* skip = data + (bitslog2 << 2); + + __m128i selb = _mm_loadl_epi64(reinterpret_cast<const __m128i*>(data)); + __m128i rest = _mm_loadu_si128(reinterpret_cast<const __m128i*>(skip)); + + __m128i sent = decodeBytesGroupConfig[bitslog2 - 1]; + __m128i ctrl = decodeBytesGroupConfig[bitslog2 + 1]; + + __m128i selw = _mm_shuffle_epi32(selb, 0x44); + __m128i sel = _mm_and_si128(sent, _mm_multishift_epi64_epi8(ctrl, selw)); + __mmask16 mask16 = _mm_cmp_epi8_mask(sel, sent, _MM_CMPINT_EQ); + + __m128i result = _mm_mask_expand_epi8(sel, mask16, rest); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(buffer), result); + + return skip + _mm_popcnt_u32(mask16); + } + + case 3: + { + __m128i result = _mm_loadu_si128(reinterpret_cast<const __m128i*>(data)); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(buffer), result); + + return data + 16; + } + + default: + assert(!"Unexpected bit length"); // unreachable since bitslog2 is a 2-bit value + return data; + } +} +#endif + +#ifdef SIMD_NEON +static uint8x16_t shuffleBytes(unsigned char mask0, unsigned char mask1, uint8x8_t rest0, uint8x8_t rest1) +{ + uint8x8_t sm0 = vld1_u8(kDecodeBytesGroupShuffle[mask0]); + uint8x8_t sm1 = vld1_u8(kDecodeBytesGroupShuffle[mask1]); + + uint8x8_t r0 = vtbl1_u8(rest0, sm0); + uint8x8_t r1 = vtbl1_u8(rest1, sm1); + + return vcombine_u8(r0, r1); +} + +static void neonMoveMask(uint8x16_t mask, unsigned char& mask0, unsigned char& mask1) +{ + static const unsigned char byte_mask_data[16] = {1, 2, 4, 8, 16, 32, 64, 128, 1, 2, 4, 8, 16, 32, 64, 128}; + + uint8x16_t byte_mask = vld1q_u8(byte_mask_data); + uint8x16_t masked = vandq_u8(mask, byte_mask); + +#ifdef __aarch64__ + // aarch64 has horizontal sums; MSVC doesn't expose this via arm64_neon.h so this path is exclusive to clang/gcc + mask0 = vaddv_u8(vget_low_u8(masked)); + mask1 = vaddv_u8(vget_high_u8(masked)); +#else + // we need horizontal sums of each half of masked, which can be done in 3 steps (yielding sums of sizes 2, 4, 8) + uint8x8_t sum1 = vpadd_u8(vget_low_u8(masked), vget_high_u8(masked)); + uint8x8_t sum2 = vpadd_u8(sum1, sum1); + uint8x8_t sum3 = vpadd_u8(sum2, sum2); + + mask0 = vget_lane_u8(sum3, 0); + mask1 = vget_lane_u8(sum3, 1); +#endif +} + +static const unsigned char* decodeBytesGroupSimd(const unsigned char* data, unsigned char* buffer, int bitslog2) +{ + switch (bitslog2) + { + case 0: + { + uint8x16_t result = vdupq_n_u8(0); + + vst1q_u8(buffer, result); + + return data; + } + + case 1: + { + uint8x8_t sel2 = vld1_u8(data); + uint8x8_t sel22 = vzip_u8(vshr_n_u8(sel2, 4), sel2).val[0]; + uint8x8x2_t sel2222 = vzip_u8(vshr_n_u8(sel22, 2), sel22); + uint8x16_t sel = vandq_u8(vcombine_u8(sel2222.val[0], sel2222.val[1]), vdupq_n_u8(3)); + + uint8x16_t mask = vceqq_u8(sel, vdupq_n_u8(3)); + unsigned char mask0, mask1; + neonMoveMask(mask, mask0, mask1); + + uint8x8_t rest0 = vld1_u8(data + 4); + uint8x8_t rest1 = vld1_u8(data + 4 + kDecodeBytesGroupCount[mask0]); + + uint8x16_t result = vbslq_u8(mask, shuffleBytes(mask0, mask1, rest0, rest1), sel); + + vst1q_u8(buffer, result); + + return data + 4 + kDecodeBytesGroupCount[mask0] + kDecodeBytesGroupCount[mask1]; + } + + case 2: + { + uint8x8_t sel4 = vld1_u8(data); + uint8x8x2_t sel44 = vzip_u8(vshr_n_u8(sel4, 4), vand_u8(sel4, vdup_n_u8(15))); + uint8x16_t sel = vcombine_u8(sel44.val[0], sel44.val[1]); + + uint8x16_t mask = vceqq_u8(sel, vdupq_n_u8(15)); + unsigned char mask0, mask1; + neonMoveMask(mask, mask0, mask1); + + uint8x8_t rest0 = vld1_u8(data + 8); + uint8x8_t rest1 = vld1_u8(data + 8 + kDecodeBytesGroupCount[mask0]); + + uint8x16_t result = vbslq_u8(mask, shuffleBytes(mask0, mask1, rest0, rest1), sel); + + vst1q_u8(buffer, result); + + return data + 8 + kDecodeBytesGroupCount[mask0] + kDecodeBytesGroupCount[mask1]; + } + + case 3: + { + uint8x16_t result = vld1q_u8(data); + + vst1q_u8(buffer, result); + + return data + 16; + } + + default: + assert(!"Unexpected bit length"); // unreachable since bitslog2 is a 2-bit value + return data; + } +} +#endif + +#ifdef SIMD_WASM +SIMD_TARGET +static v128_t decodeShuffleMask(unsigned char mask0, unsigned char mask1) +{ + v128_t sm0 = wasm_v128_load(&kDecodeBytesGroupShuffle[mask0]); + v128_t sm1 = wasm_v128_load(&kDecodeBytesGroupShuffle[mask1]); + + v128_t sm1off = wasm_v128_load(&kDecodeBytesGroupCount[mask0]); + sm1off = wasm_v8x16_shuffle(sm1off, sm1off, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + v128_t sm1r = wasm_i8x16_add(sm1, sm1off); + + return wasmx_unpacklo_v64x2(sm0, sm1r); +} + +SIMD_TARGET +static void wasmMoveMask(v128_t mask, unsigned char& mask0, unsigned char& mask1) +{ + v128_t mask_0 = wasm_v32x4_shuffle(mask, mask, 0, 2, 1, 3); + + uint64_t mask_1a = wasm_i64x2_extract_lane(mask_0, 0) & 0x0804020108040201ull; + uint64_t mask_1b = wasm_i64x2_extract_lane(mask_0, 1) & 0x8040201080402010ull; + + // TODO: This can use v8x16_bitmask in the future + uint64_t mask_2 = mask_1a | mask_1b; + uint64_t mask_4 = mask_2 | (mask_2 >> 16); + uint64_t mask_8 = mask_4 | (mask_4 >> 8); + + mask0 = uint8_t(mask_8); + mask1 = uint8_t(mask_8 >> 32); +} + +SIMD_TARGET +static const unsigned char* decodeBytesGroupSimd(const unsigned char* data, unsigned char* buffer, int bitslog2) +{ + unsigned char byte, enc, encv; + const unsigned char* data_var; + + switch (bitslog2) + { + case 0: + { + v128_t result = wasm_i8x16_splat(0); + + wasm_v128_store(buffer, result); + + return data; + } + + case 1: + { + v128_t sel2 = wasm_v128_load(data); + v128_t rest = wasm_v128_load(data + 4); + + v128_t sel22 = wasmx_unpacklo_v8x16(wasm_i16x8_shr(sel2, 4), sel2); + v128_t sel2222 = wasmx_unpacklo_v8x16(wasm_i16x8_shr(sel22, 2), sel22); + v128_t sel = wasm_v128_and(sel2222, wasm_i8x16_splat(3)); + + v128_t mask = wasm_i8x16_eq(sel, wasm_i8x16_splat(3)); + + unsigned char mask0, mask1; + wasmMoveMask(mask, mask0, mask1); + + v128_t shuf = decodeShuffleMask(mask0, mask1); + + v128_t result = wasm_v128_bitselect(wasm_v8x16_swizzle(rest, shuf), sel, mask); + + wasm_v128_store(buffer, result); + + return data + 4 + kDecodeBytesGroupCount[mask0] + kDecodeBytesGroupCount[mask1]; + } + + case 2: + { + v128_t sel4 = wasm_v128_load(data); + v128_t rest = wasm_v128_load(data + 8); + + v128_t sel44 = wasmx_unpacklo_v8x16(wasm_i16x8_shr(sel4, 4), sel4); + v128_t sel = wasm_v128_and(sel44, wasm_i8x16_splat(15)); + + v128_t mask = wasm_i8x16_eq(sel, wasm_i8x16_splat(15)); + + unsigned char mask0, mask1; + wasmMoveMask(mask, mask0, mask1); + + v128_t shuf = decodeShuffleMask(mask0, mask1); + + v128_t result = wasm_v128_bitselect(wasm_v8x16_swizzle(rest, shuf), sel, mask); + + wasm_v128_store(buffer, result); + + return data + 8 + kDecodeBytesGroupCount[mask0] + kDecodeBytesGroupCount[mask1]; + } + + case 3: + { + v128_t result = wasm_v128_load(data); + + wasm_v128_store(buffer, result); + + return data + 16; + } + + default: + assert(!"Unexpected bit length"); // unreachable since bitslog2 is a 2-bit value + return data; + } +} +#endif + +#if defined(SIMD_SSE) || defined(SIMD_AVX) +SIMD_TARGET +static void transpose8(__m128i& x0, __m128i& x1, __m128i& x2, __m128i& x3) +{ + __m128i t0 = _mm_unpacklo_epi8(x0, x1); + __m128i t1 = _mm_unpackhi_epi8(x0, x1); + __m128i t2 = _mm_unpacklo_epi8(x2, x3); + __m128i t3 = _mm_unpackhi_epi8(x2, x3); + + x0 = _mm_unpacklo_epi16(t0, t2); + x1 = _mm_unpackhi_epi16(t0, t2); + x2 = _mm_unpacklo_epi16(t1, t3); + x3 = _mm_unpackhi_epi16(t1, t3); +} + +SIMD_TARGET +static __m128i unzigzag8(__m128i v) +{ + __m128i xl = _mm_sub_epi8(_mm_setzero_si128(), _mm_and_si128(v, _mm_set1_epi8(1))); + __m128i xr = _mm_and_si128(_mm_srli_epi16(v, 1), _mm_set1_epi8(127)); + + return _mm_xor_si128(xl, xr); +} +#endif + +#ifdef SIMD_NEON +static void transpose8(uint8x16_t& x0, uint8x16_t& x1, uint8x16_t& x2, uint8x16_t& x3) +{ + uint8x16x2_t t01 = vzipq_u8(x0, x1); + uint8x16x2_t t23 = vzipq_u8(x2, x3); + + uint16x8x2_t x01 = vzipq_u16(vreinterpretq_u16_u8(t01.val[0]), vreinterpretq_u16_u8(t23.val[0])); + uint16x8x2_t x23 = vzipq_u16(vreinterpretq_u16_u8(t01.val[1]), vreinterpretq_u16_u8(t23.val[1])); + + x0 = vreinterpretq_u8_u16(x01.val[0]); + x1 = vreinterpretq_u8_u16(x01.val[1]); + x2 = vreinterpretq_u8_u16(x23.val[0]); + x3 = vreinterpretq_u8_u16(x23.val[1]); +} + +static uint8x16_t unzigzag8(uint8x16_t v) +{ + uint8x16_t xl = vreinterpretq_u8_s8(vnegq_s8(vreinterpretq_s8_u8(vandq_u8(v, vdupq_n_u8(1))))); + uint8x16_t xr = vshrq_n_u8(v, 1); + + return veorq_u8(xl, xr); +} +#endif + +#ifdef SIMD_WASM +SIMD_TARGET +static void transpose8(v128_t& x0, v128_t& x1, v128_t& x2, v128_t& x3) +{ + v128_t t0 = wasmx_unpacklo_v8x16(x0, x1); + v128_t t1 = wasmx_unpackhi_v8x16(x0, x1); + v128_t t2 = wasmx_unpacklo_v8x16(x2, x3); + v128_t t3 = wasmx_unpackhi_v8x16(x2, x3); + + x0 = wasmx_unpacklo_v16x8(t0, t2); + x1 = wasmx_unpackhi_v16x8(t0, t2); + x2 = wasmx_unpacklo_v16x8(t1, t3); + x3 = wasmx_unpackhi_v16x8(t1, t3); +} + +SIMD_TARGET +static v128_t unzigzag8(v128_t v) +{ + v128_t xl = wasm_i8x16_neg(wasm_v128_and(v, wasm_i8x16_splat(1))); + v128_t xr = wasm_u8x16_shr(v, 1); + + return wasm_v128_xor(xl, xr); +} +#endif + +#if defined(SIMD_SSE) || defined(SIMD_AVX) || defined(SIMD_NEON) || defined(SIMD_WASM) +SIMD_TARGET +static const unsigned char* decodeBytesSimd(const unsigned char* data, const unsigned char* data_end, unsigned char* buffer, size_t buffer_size) +{ + assert(buffer_size % kByteGroupSize == 0); + assert(kByteGroupSize == 16); + + const unsigned char* header = data; + + // round number of groups to 4 to get number of header bytes + size_t header_size = (buffer_size / kByteGroupSize + 3) / 4; + + if (size_t(data_end - data) < header_size) + return 0; + + data += header_size; + + size_t i = 0; + + // fast-path: process 4 groups at a time, do a shared bounds check - each group reads <=24b + for (; i + kByteGroupSize * 4 <= buffer_size && size_t(data_end - data) >= kByteGroupDecodeLimit * 4; i += kByteGroupSize * 4) + { + size_t header_offset = i / kByteGroupSize; + unsigned char header_byte = header[header_offset / 4]; + + data = decodeBytesGroupSimd(data, buffer + i + kByteGroupSize * 0, (header_byte >> 0) & 3); + data = decodeBytesGroupSimd(data, buffer + i + kByteGroupSize * 1, (header_byte >> 2) & 3); + data = decodeBytesGroupSimd(data, buffer + i + kByteGroupSize * 2, (header_byte >> 4) & 3); + data = decodeBytesGroupSimd(data, buffer + i + kByteGroupSize * 3, (header_byte >> 6) & 3); + } + + // slow-path: process remaining groups + for (; i < buffer_size; i += kByteGroupSize) + { + if (size_t(data_end - data) < kByteGroupDecodeLimit) + return 0; + + size_t header_offset = i / kByteGroupSize; + + int bitslog2 = (header[header_offset / 4] >> ((header_offset % 4) * 2)) & 3; + + data = decodeBytesGroupSimd(data, buffer + i, bitslog2); + } + + return data; +} + +SIMD_TARGET +static const unsigned char* decodeVertexBlockSimd(const unsigned char* data, const unsigned char* data_end, unsigned char* vertex_data, size_t vertex_count, size_t vertex_size, unsigned char last_vertex[256]) +{ + assert(vertex_count > 0 && vertex_count <= kVertexBlockMaxSize); + + unsigned char buffer[kVertexBlockMaxSize * 4]; + unsigned char transposed[kVertexBlockSizeBytes]; + + size_t vertex_count_aligned = (vertex_count + kByteGroupSize - 1) & ~(kByteGroupSize - 1); + + for (size_t k = 0; k < vertex_size; k += 4) + { + for (size_t j = 0; j < 4; ++j) + { + data = decodeBytesSimd(data, data_end, buffer + j * vertex_count_aligned, vertex_count_aligned); + if (!data) + return 0; + } + +#if defined(SIMD_SSE) || defined(SIMD_AVX) +#define TEMP __m128i +#define PREP() __m128i pi = _mm_cvtsi32_si128(*reinterpret_cast<const int*>(last_vertex + k)) +#define LOAD(i) __m128i r##i = _mm_loadu_si128(reinterpret_cast<const __m128i*>(buffer + j + i * vertex_count_aligned)) +#define GRP4(i) t0 = _mm_shuffle_epi32(r##i, 0), t1 = _mm_shuffle_epi32(r##i, 1), t2 = _mm_shuffle_epi32(r##i, 2), t3 = _mm_shuffle_epi32(r##i, 3) +#define FIXD(i) t##i = pi = _mm_add_epi8(pi, t##i) +#define SAVE(i) *reinterpret_cast<int*>(savep) = _mm_cvtsi128_si32(t##i), savep += vertex_size +#endif + +#ifdef SIMD_NEON +#define TEMP uint8x8_t +#define PREP() uint8x8_t pi = vreinterpret_u8_u32(vld1_lane_u32(reinterpret_cast<uint32_t*>(last_vertex + k), vdup_n_u32(0), 0)) +#define LOAD(i) uint8x16_t r##i = vld1q_u8(buffer + j + i * vertex_count_aligned) +#define GRP4(i) t0 = vget_low_u8(r##i), t1 = vreinterpret_u8_u32(vdup_lane_u32(vreinterpret_u32_u8(t0), 1)), t2 = vget_high_u8(r##i), t3 = vreinterpret_u8_u32(vdup_lane_u32(vreinterpret_u32_u8(t2), 1)) +#define FIXD(i) t##i = pi = vadd_u8(pi, t##i) +#define SAVE(i) vst1_lane_u32(reinterpret_cast<uint32_t*>(savep), vreinterpret_u32_u8(t##i), 0), savep += vertex_size +#endif + +#ifdef SIMD_WASM +#define TEMP v128_t +#define PREP() v128_t pi = wasm_v128_load(last_vertex + k) +#define LOAD(i) v128_t r##i = wasm_v128_load(buffer + j + i * vertex_count_aligned) +#define GRP4(i) t0 = wasmx_splat_v32x4(r##i, 0), t1 = wasmx_splat_v32x4(r##i, 1), t2 = wasmx_splat_v32x4(r##i, 2), t3 = wasmx_splat_v32x4(r##i, 3) +#define FIXD(i) t##i = pi = wasm_i8x16_add(pi, t##i) +#define SAVE(i) *reinterpret_cast<int*>(savep) = wasm_i32x4_extract_lane(t##i, 0), savep += vertex_size +#endif + + PREP(); + + unsigned char* savep = transposed + k; + + for (size_t j = 0; j < vertex_count_aligned; j += 16) + { + LOAD(0); + LOAD(1); + LOAD(2); + LOAD(3); + + r0 = unzigzag8(r0); + r1 = unzigzag8(r1); + r2 = unzigzag8(r2); + r3 = unzigzag8(r3); + + transpose8(r0, r1, r2, r3); + + TEMP t0, t1, t2, t3; + + GRP4(0); + FIXD(0), FIXD(1), FIXD(2), FIXD(3); + SAVE(0), SAVE(1), SAVE(2), SAVE(3); + + GRP4(1); + FIXD(0), FIXD(1), FIXD(2), FIXD(3); + SAVE(0), SAVE(1), SAVE(2), SAVE(3); + + GRP4(2); + FIXD(0), FIXD(1), FIXD(2), FIXD(3); + SAVE(0), SAVE(1), SAVE(2), SAVE(3); + + GRP4(3); + FIXD(0), FIXD(1), FIXD(2), FIXD(3); + SAVE(0), SAVE(1), SAVE(2), SAVE(3); + +#undef TEMP +#undef PREP +#undef LOAD +#undef GRP4 +#undef FIXD +#undef SAVE + } + } + + memcpy(vertex_data, transposed, vertex_count * vertex_size); + + memcpy(last_vertex, &transposed[vertex_size * (vertex_count - 1)], vertex_size); + + return data; +} +#endif + +#if defined(SIMD_SSE) && defined(SIMD_FALLBACK) +static unsigned int getCpuFeatures() +{ + int cpuinfo[4] = {}; +#ifdef _MSC_VER + __cpuid(cpuinfo, 1); +#else + __cpuid(1, cpuinfo[0], cpuinfo[1], cpuinfo[2], cpuinfo[3]); +#endif + return cpuinfo[2]; +} + +unsigned int cpuid = getCpuFeatures(); +#endif + +} // namespace meshopt + +size_t meshopt_encodeVertexBuffer(unsigned char* buffer, size_t buffer_size, const void* vertices, size_t vertex_count, size_t vertex_size) +{ + using namespace meshopt; + + assert(vertex_size > 0 && vertex_size <= 256); + assert(vertex_size % 4 == 0); + +#if TRACE + memset(vertexstats, 0, sizeof(vertexstats)); +#endif + + const unsigned char* vertex_data = static_cast<const unsigned char*>(vertices); + + unsigned char* data = buffer; + unsigned char* data_end = buffer + buffer_size; + + if (size_t(data_end - data) < 1 + vertex_size) + return 0; + + int version = gEncodeVertexVersion; + + *data++ = (unsigned char)(kVertexHeader | version); + + unsigned char first_vertex[256] = {}; + if (vertex_count > 0) + memcpy(first_vertex, vertex_data, vertex_size); + + unsigned char last_vertex[256] = {}; + memcpy(last_vertex, first_vertex, vertex_size); + + size_t vertex_block_size = getVertexBlockSize(vertex_size); + + size_t vertex_offset = 0; + + while (vertex_offset < vertex_count) + { + size_t block_size = (vertex_offset + vertex_block_size < vertex_count) ? vertex_block_size : vertex_count - vertex_offset; + + data = encodeVertexBlock(data, data_end, vertex_data + vertex_offset * vertex_size, block_size, vertex_size, last_vertex); + if (!data) + return 0; + + vertex_offset += block_size; + } + + size_t tail_size = vertex_size < kTailMaxSize ? kTailMaxSize : vertex_size; + + if (size_t(data_end - data) < tail_size) + return 0; + + // write first vertex to the end of the stream and pad it to 32 bytes; this is important to simplify bounds checks in decoder + if (vertex_size < kTailMaxSize) + { + memset(data, 0, kTailMaxSize - vertex_size); + data += kTailMaxSize - vertex_size; + } + + memcpy(data, first_vertex, vertex_size); + data += vertex_size; + + assert(data >= buffer + tail_size); + assert(data <= buffer + buffer_size); + +#if TRACE + size_t total_size = data - buffer; + + for (size_t k = 0; k < vertex_size; ++k) + { + const Stats& vsk = vertexstats[k]; + + printf("%2d: %d bytes\t%.1f%%\t%.1f bpv", int(k), int(vsk.size), double(vsk.size) / double(total_size) * 100, double(vsk.size) / double(vertex_count) * 8); + +#if TRACE > 1 + printf("\t\thdr %d bytes\tbit0 %d (%d bytes)\tbit1 %d (%d bytes)\tbit2 %d (%d bytes)\tbit3 %d (%d bytes)", + int(vsk.header), + int(vsk.bitg[0]), int(vsk.bitb[0]), + int(vsk.bitg[1]), int(vsk.bitb[1]), + int(vsk.bitg[2]), int(vsk.bitb[2]), + int(vsk.bitg[3]), int(vsk.bitb[3])); +#endif + + printf("\n"); + } +#endif + + return data - buffer; +} + +size_t meshopt_encodeVertexBufferBound(size_t vertex_count, size_t vertex_size) +{ + using namespace meshopt; + + assert(vertex_size > 0 && vertex_size <= 256); + assert(vertex_size % 4 == 0); + + size_t vertex_block_size = getVertexBlockSize(vertex_size); + size_t vertex_block_count = (vertex_count + vertex_block_size - 1) / vertex_block_size; + + size_t vertex_block_header_size = (vertex_block_size / kByteGroupSize + 3) / 4; + size_t vertex_block_data_size = vertex_block_size; + + size_t tail_size = vertex_size < kTailMaxSize ? kTailMaxSize : vertex_size; + + return 1 + vertex_block_count * vertex_size * (vertex_block_header_size + vertex_block_data_size) + tail_size; +} + +void meshopt_encodeVertexVersion(int version) +{ + assert(unsigned(version) <= 0); + + meshopt::gEncodeVertexVersion = version; +} + +int meshopt_decodeVertexBuffer(void* destination, size_t vertex_count, size_t vertex_size, const unsigned char* buffer, size_t buffer_size) +{ + using namespace meshopt; + + assert(vertex_size > 0 && vertex_size <= 256); + assert(vertex_size % 4 == 0); + + const unsigned char* (*decode)(const unsigned char*, const unsigned char*, unsigned char*, size_t, size_t, unsigned char[256]) = 0; + +#if defined(SIMD_SSE) && defined(SIMD_FALLBACK) + decode = (cpuid & (1 << 9)) ? decodeVertexBlockSimd : decodeVertexBlock; +#elif defined(SIMD_SSE) || defined(SIMD_AVX) || defined(SIMD_NEON) || defined(SIMD_WASM) + decode = decodeVertexBlockSimd; +#else + decode = decodeVertexBlock; +#endif + +#if defined(SIMD_SSE) || defined(SIMD_NEON) || defined(SIMD_WASM) + assert(gDecodeBytesGroupInitialized); + (void)gDecodeBytesGroupInitialized; +#endif + + unsigned char* vertex_data = static_cast<unsigned char*>(destination); + + const unsigned char* data = buffer; + const unsigned char* data_end = buffer + buffer_size; + + if (size_t(data_end - data) < 1 + vertex_size) + return -2; + + unsigned char data_header = *data++; + + if ((data_header & 0xf0) != kVertexHeader) + return -1; + + int version = data_header & 0x0f; + if (version > 0) + return -1; + + unsigned char last_vertex[256]; + memcpy(last_vertex, data_end - vertex_size, vertex_size); + + size_t vertex_block_size = getVertexBlockSize(vertex_size); + + size_t vertex_offset = 0; + + while (vertex_offset < vertex_count) + { + size_t block_size = (vertex_offset + vertex_block_size < vertex_count) ? vertex_block_size : vertex_count - vertex_offset; + + data = decode(data, data_end, vertex_data + vertex_offset * vertex_size, block_size, vertex_size, last_vertex); + if (!data) + return -2; + + vertex_offset += block_size; + } + + size_t tail_size = vertex_size < kTailMaxSize ? kTailMaxSize : vertex_size; + + if (size_t(data_end - data) != tail_size) + return -3; + + return 0; +} + +#undef SIMD_NEON +#undef SIMD_SSE +#undef SIMD_AVX +#undef SIMD_WASM +#undef SIMD_FALLBACK +#undef SIMD_TARGET diff --git a/thirdparty/meshoptimizer/vertexfilter.cpp b/thirdparty/meshoptimizer/vertexfilter.cpp new file mode 100644 index 0000000000..e7ad2c9d39 --- /dev/null +++ b/thirdparty/meshoptimizer/vertexfilter.cpp @@ -0,0 +1,825 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <math.h> + +// The block below auto-detects SIMD ISA that can be used on the target platform +#ifndef MESHOPTIMIZER_NO_SIMD + +// The SIMD implementation requires SSE2, which can be enabled unconditionally through compiler settings +#if defined(__SSE2__) +#define SIMD_SSE +#endif + +// MSVC supports compiling SSE2 code regardless of compile options; we assume all 32-bit CPUs support SSE2 +#if !defined(SIMD_SSE) && defined(_MSC_VER) && !defined(__clang__) && (defined(_M_IX86) || defined(_M_X64)) +#define SIMD_SSE +#endif + +// GCC/clang define these when NEON support is available +#if defined(__ARM_NEON__) || defined(__ARM_NEON) +#define SIMD_NEON +#endif + +// On MSVC, we assume that ARM builds always target NEON-capable devices +#if !defined(SIMD_NEON) && defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64)) +#define SIMD_NEON +#endif + +// When targeting Wasm SIMD we can't use runtime cpuid checks so we unconditionally enable SIMD +#if defined(__wasm_simd128__) +#define SIMD_WASM +#endif + +#endif // !MESHOPTIMIZER_NO_SIMD + +#ifdef SIMD_SSE +#include <emmintrin.h> +#include <stdint.h> +#endif + +#ifdef _MSC_VER +#include <intrin.h> +#endif + +#ifdef SIMD_NEON +#if defined(_MSC_VER) && defined(_M_ARM64) +#include <arm64_neon.h> +#else +#include <arm_neon.h> +#endif +#endif + +#ifdef SIMD_WASM +#include <wasm_simd128.h> +#endif + +#ifdef SIMD_WASM +#define wasmx_unpacklo_v16x8(a, b) wasm_v16x8_shuffle(a, b, 0, 8, 1, 9, 2, 10, 3, 11) +#define wasmx_unpackhi_v16x8(a, b) wasm_v16x8_shuffle(a, b, 4, 12, 5, 13, 6, 14, 7, 15) +#define wasmx_unziplo_v32x4(a, b) wasm_v32x4_shuffle(a, b, 0, 2, 4, 6) +#define wasmx_unziphi_v32x4(a, b) wasm_v32x4_shuffle(a, b, 1, 3, 5, 7) +#endif + +namespace meshopt +{ + +#if !defined(SIMD_SSE) && !defined(SIMD_NEON) && !defined(SIMD_WASM) +template <typename T> +static void decodeFilterOct(T* data, size_t count) +{ + const float max = float((1 << (sizeof(T) * 8 - 1)) - 1); + + for (size_t i = 0; i < count; ++i) + { + // convert x and y to floats and reconstruct z; this assumes zf encodes 1.f at the same bit count + float x = float(data[i * 4 + 0]); + float y = float(data[i * 4 + 1]); + float z = float(data[i * 4 + 2]) - fabsf(x) - fabsf(y); + + // fixup octahedral coordinates for z<0 + float t = (z >= 0.f) ? 0.f : z; + + x += (x >= 0.f) ? t : -t; + y += (y >= 0.f) ? t : -t; + + // compute normal length & scale + float l = sqrtf(x * x + y * y + z * z); + float s = max / l; + + // rounded signed float->int + int xf = int(x * s + (x >= 0.f ? 0.5f : -0.5f)); + int yf = int(y * s + (y >= 0.f ? 0.5f : -0.5f)); + int zf = int(z * s + (z >= 0.f ? 0.5f : -0.5f)); + + data[i * 4 + 0] = T(xf); + data[i * 4 + 1] = T(yf); + data[i * 4 + 2] = T(zf); + } +} + +static void decodeFilterQuat(short* data, size_t count) +{ + const float scale = 1.f / sqrtf(2.f); + + for (size_t i = 0; i < count; ++i) + { + // recover scale from the high byte of the component + int sf = data[i * 4 + 3] | 3; + float ss = scale / float(sf); + + // convert x/y/z to [-1..1] (scaled...) + float x = float(data[i * 4 + 0]) * ss; + float y = float(data[i * 4 + 1]) * ss; + float z = float(data[i * 4 + 2]) * ss; + + // reconstruct w as a square root; we clamp to 0.f to avoid NaN due to precision errors + float ww = 1.f - x * x - y * y - z * z; + float w = sqrtf(ww >= 0.f ? ww : 0.f); + + // rounded signed float->int + int xf = int(x * 32767.f + (x >= 0.f ? 0.5f : -0.5f)); + int yf = int(y * 32767.f + (y >= 0.f ? 0.5f : -0.5f)); + int zf = int(z * 32767.f + (z >= 0.f ? 0.5f : -0.5f)); + int wf = int(w * 32767.f + 0.5f); + + int qc = data[i * 4 + 3] & 3; + + // output order is dictated by input index + data[i * 4 + ((qc + 1) & 3)] = short(xf); + data[i * 4 + ((qc + 2) & 3)] = short(yf); + data[i * 4 + ((qc + 3) & 3)] = short(zf); + data[i * 4 + ((qc + 0) & 3)] = short(wf); + } +} + +static void decodeFilterExp(unsigned int* data, size_t count) +{ + for (size_t i = 0; i < count; ++i) + { + unsigned int v = data[i]; + + // decode mantissa and exponent + int m = int(v << 8) >> 8; + int e = int(v) >> 24; + + union + { + float f; + unsigned int ui; + } u; + + // optimized version of ldexp(float(m), e) + u.ui = unsigned(e + 127) << 23; + u.f = u.f * float(m); + + data[i] = u.ui; + } +} +#endif + +#if defined(SIMD_SSE) || defined(SIMD_NEON) || defined(SIMD_WASM) +inline uint64_t rotateleft64(uint64_t v, int x) +{ +#if defined(_MSC_VER) && !defined(__clang__) + return _rotl64(v, x); +// Apple's Clang 8 is actually vanilla Clang 3.9, there we need to look for +// version 11 instead: https://en.wikipedia.org/wiki/Xcode#Toolchain_versions +#elif defined(__clang__) && ((!defined(__apple_build_version__) && __clang_major__ >= 8) || __clang_major__ >= 11) + return __builtin_rotateleft64(v, x); +#else + return (v << (x & 63)) | (v >> ((64 - x) & 63)); +#endif +} +#endif + +#ifdef SIMD_SSE +static void decodeFilterOctSimd(signed char* data, size_t count) +{ + const __m128 sign = _mm_set1_ps(-0.f); + + for (size_t i = 0; i < count; i += 4) + { + __m128i n4 = _mm_loadu_si128(reinterpret_cast<__m128i*>(&data[i * 4])); + + // sign-extends each of x,y in [x y ? ?] with arithmetic shifts + __m128i xf = _mm_srai_epi32(_mm_slli_epi32(n4, 24), 24); + __m128i yf = _mm_srai_epi32(_mm_slli_epi32(n4, 16), 24); + + // unpack z; note that z is unsigned so we technically don't need to sign extend it + __m128i zf = _mm_srai_epi32(_mm_slli_epi32(n4, 8), 24); + + // convert x and y to floats and reconstruct z; this assumes zf encodes 1.f at the same bit count + __m128 x = _mm_cvtepi32_ps(xf); + __m128 y = _mm_cvtepi32_ps(yf); + __m128 z = _mm_sub_ps(_mm_cvtepi32_ps(zf), _mm_add_ps(_mm_andnot_ps(sign, x), _mm_andnot_ps(sign, y))); + + // fixup octahedral coordinates for z<0 + __m128 t = _mm_min_ps(z, _mm_setzero_ps()); + + x = _mm_add_ps(x, _mm_xor_ps(t, _mm_and_ps(x, sign))); + y = _mm_add_ps(y, _mm_xor_ps(t, _mm_and_ps(y, sign))); + + // compute normal length & scale + __m128 ll = _mm_add_ps(_mm_mul_ps(x, x), _mm_add_ps(_mm_mul_ps(y, y), _mm_mul_ps(z, z))); + __m128 s = _mm_mul_ps(_mm_set1_ps(127.f), _mm_rsqrt_ps(ll)); + + // rounded signed float->int + __m128i xr = _mm_cvtps_epi32(_mm_mul_ps(x, s)); + __m128i yr = _mm_cvtps_epi32(_mm_mul_ps(y, s)); + __m128i zr = _mm_cvtps_epi32(_mm_mul_ps(z, s)); + + // combine xr/yr/zr into final value + __m128i res = _mm_and_si128(n4, _mm_set1_epi32(0xff000000)); + res = _mm_or_si128(res, _mm_and_si128(xr, _mm_set1_epi32(0xff))); + res = _mm_or_si128(res, _mm_slli_epi32(_mm_and_si128(yr, _mm_set1_epi32(0xff)), 8)); + res = _mm_or_si128(res, _mm_slli_epi32(_mm_and_si128(zr, _mm_set1_epi32(0xff)), 16)); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(&data[i * 4]), res); + } +} + +static void decodeFilterOctSimd(short* data, size_t count) +{ + const __m128 sign = _mm_set1_ps(-0.f); + + for (size_t i = 0; i < count; i += 4) + { + __m128 n4_0 = _mm_loadu_ps(reinterpret_cast<float*>(&data[(i + 0) * 4])); + __m128 n4_1 = _mm_loadu_ps(reinterpret_cast<float*>(&data[(i + 2) * 4])); + + // gather both x/y 16-bit pairs in each 32-bit lane + __m128i n4 = _mm_castps_si128(_mm_shuffle_ps(n4_0, n4_1, _MM_SHUFFLE(2, 0, 2, 0))); + + // sign-extends each of x,y in [x y] with arithmetic shifts + __m128i xf = _mm_srai_epi32(_mm_slli_epi32(n4, 16), 16); + __m128i yf = _mm_srai_epi32(n4, 16); + + // unpack z; note that z is unsigned so we don't need to sign extend it + __m128i z4 = _mm_castps_si128(_mm_shuffle_ps(n4_0, n4_1, _MM_SHUFFLE(3, 1, 3, 1))); + __m128i zf = _mm_and_si128(z4, _mm_set1_epi32(0x7fff)); + + // convert x and y to floats and reconstruct z; this assumes zf encodes 1.f at the same bit count + __m128 x = _mm_cvtepi32_ps(xf); + __m128 y = _mm_cvtepi32_ps(yf); + __m128 z = _mm_sub_ps(_mm_cvtepi32_ps(zf), _mm_add_ps(_mm_andnot_ps(sign, x), _mm_andnot_ps(sign, y))); + + // fixup octahedral coordinates for z<0 + __m128 t = _mm_min_ps(z, _mm_setzero_ps()); + + x = _mm_add_ps(x, _mm_xor_ps(t, _mm_and_ps(x, sign))); + y = _mm_add_ps(y, _mm_xor_ps(t, _mm_and_ps(y, sign))); + + // compute normal length & scale + __m128 ll = _mm_add_ps(_mm_mul_ps(x, x), _mm_add_ps(_mm_mul_ps(y, y), _mm_mul_ps(z, z))); + __m128 s = _mm_div_ps(_mm_set1_ps(32767.f), _mm_sqrt_ps(ll)); + + // rounded signed float->int + __m128i xr = _mm_cvtps_epi32(_mm_mul_ps(x, s)); + __m128i yr = _mm_cvtps_epi32(_mm_mul_ps(y, s)); + __m128i zr = _mm_cvtps_epi32(_mm_mul_ps(z, s)); + + // mix x/z and y/0 to make 16-bit unpack easier + __m128i xzr = _mm_or_si128(_mm_and_si128(xr, _mm_set1_epi32(0xffff)), _mm_slli_epi32(zr, 16)); + __m128i y0r = _mm_and_si128(yr, _mm_set1_epi32(0xffff)); + + // pack x/y/z using 16-bit unpacks; note that this has 0 where we should have .w + __m128i res_0 = _mm_unpacklo_epi16(xzr, y0r); + __m128i res_1 = _mm_unpackhi_epi16(xzr, y0r); + + // patch in .w + res_0 = _mm_or_si128(res_0, _mm_and_si128(_mm_castps_si128(n4_0), _mm_set1_epi64x(0xffff000000000000))); + res_1 = _mm_or_si128(res_1, _mm_and_si128(_mm_castps_si128(n4_1), _mm_set1_epi64x(0xffff000000000000))); + + _mm_storeu_si128(reinterpret_cast<__m128i*>(&data[(i + 0) * 4]), res_0); + _mm_storeu_si128(reinterpret_cast<__m128i*>(&data[(i + 2) * 4]), res_1); + } +} + +static void decodeFilterQuatSimd(short* data, size_t count) +{ + const float scale = 1.f / sqrtf(2.f); + + for (size_t i = 0; i < count; i += 4) + { + __m128 q4_0 = _mm_loadu_ps(reinterpret_cast<float*>(&data[(i + 0) * 4])); + __m128 q4_1 = _mm_loadu_ps(reinterpret_cast<float*>(&data[(i + 2) * 4])); + + // gather both x/y 16-bit pairs in each 32-bit lane + __m128i q4_xy = _mm_castps_si128(_mm_shuffle_ps(q4_0, q4_1, _MM_SHUFFLE(2, 0, 2, 0))); + __m128i q4_zc = _mm_castps_si128(_mm_shuffle_ps(q4_0, q4_1, _MM_SHUFFLE(3, 1, 3, 1))); + + // sign-extends each of x,y in [x y] with arithmetic shifts + __m128i xf = _mm_srai_epi32(_mm_slli_epi32(q4_xy, 16), 16); + __m128i yf = _mm_srai_epi32(q4_xy, 16); + __m128i zf = _mm_srai_epi32(_mm_slli_epi32(q4_zc, 16), 16); + __m128i cf = _mm_srai_epi32(q4_zc, 16); + + // get a floating-point scaler using zc with bottom 2 bits set to 1 (which represents 1.f) + __m128i sf = _mm_or_si128(cf, _mm_set1_epi32(3)); + __m128 ss = _mm_div_ps(_mm_set1_ps(scale), _mm_cvtepi32_ps(sf)); + + // convert x/y/z to [-1..1] (scaled...) + __m128 x = _mm_mul_ps(_mm_cvtepi32_ps(xf), ss); + __m128 y = _mm_mul_ps(_mm_cvtepi32_ps(yf), ss); + __m128 z = _mm_mul_ps(_mm_cvtepi32_ps(zf), ss); + + // reconstruct w as a square root; we clamp to 0.f to avoid NaN due to precision errors + __m128 ww = _mm_sub_ps(_mm_set1_ps(1.f), _mm_add_ps(_mm_mul_ps(x, x), _mm_add_ps(_mm_mul_ps(y, y), _mm_mul_ps(z, z)))); + __m128 w = _mm_sqrt_ps(_mm_max_ps(ww, _mm_setzero_ps())); + + __m128 s = _mm_set1_ps(32767.f); + + // rounded signed float->int + __m128i xr = _mm_cvtps_epi32(_mm_mul_ps(x, s)); + __m128i yr = _mm_cvtps_epi32(_mm_mul_ps(y, s)); + __m128i zr = _mm_cvtps_epi32(_mm_mul_ps(z, s)); + __m128i wr = _mm_cvtps_epi32(_mm_mul_ps(w, s)); + + // mix x/z and w/y to make 16-bit unpack easier + __m128i xzr = _mm_or_si128(_mm_and_si128(xr, _mm_set1_epi32(0xffff)), _mm_slli_epi32(zr, 16)); + __m128i wyr = _mm_or_si128(_mm_and_si128(wr, _mm_set1_epi32(0xffff)), _mm_slli_epi32(yr, 16)); + + // pack x/y/z/w using 16-bit unpacks; we pack wxyz by default (for qc=0) + __m128i res_0 = _mm_unpacklo_epi16(wyr, xzr); + __m128i res_1 = _mm_unpackhi_epi16(wyr, xzr); + + // store results to stack so that we can rotate using scalar instructions + uint64_t res[4]; + _mm_storeu_si128(reinterpret_cast<__m128i*>(&res[0]), res_0); + _mm_storeu_si128(reinterpret_cast<__m128i*>(&res[2]), res_1); + + // rotate and store + uint64_t* out = reinterpret_cast<uint64_t*>(&data[i * 4]); + + out[0] = rotateleft64(res[0], data[(i + 0) * 4 + 3] << 4); + out[1] = rotateleft64(res[1], data[(i + 1) * 4 + 3] << 4); + out[2] = rotateleft64(res[2], data[(i + 2) * 4 + 3] << 4); + out[3] = rotateleft64(res[3], data[(i + 3) * 4 + 3] << 4); + } +} + +static void decodeFilterExpSimd(unsigned int* data, size_t count) +{ + for (size_t i = 0; i < count; i += 4) + { + __m128i v = _mm_loadu_si128(reinterpret_cast<__m128i*>(&data[i])); + + // decode exponent into 2^x directly + __m128i ef = _mm_srai_epi32(v, 24); + __m128i es = _mm_slli_epi32(_mm_add_epi32(ef, _mm_set1_epi32(127)), 23); + + // decode 24-bit mantissa into floating-point value + __m128i mf = _mm_srai_epi32(_mm_slli_epi32(v, 8), 8); + __m128 m = _mm_cvtepi32_ps(mf); + + __m128 r = _mm_mul_ps(_mm_castsi128_ps(es), m); + + _mm_storeu_ps(reinterpret_cast<float*>(&data[i]), r); + } +} +#endif + +#if defined(SIMD_NEON) && !defined(__aarch64__) && !defined(_M_ARM64) +inline float32x4_t vsqrtq_f32(float32x4_t x) +{ + float32x4_t r = vrsqrteq_f32(x); + r = vmulq_f32(r, vrsqrtsq_f32(vmulq_f32(r, x), r)); // refine rsqrt estimate + return vmulq_f32(r, x); +} + +inline float32x4_t vdivq_f32(float32x4_t x, float32x4_t y) +{ + float32x4_t r = vrecpeq_f32(y); + r = vmulq_f32(r, vrecpsq_f32(y, r)); // refine rcp estimate + return vmulq_f32(x, r); +} +#endif + +#ifdef SIMD_NEON +static void decodeFilterOctSimd(signed char* data, size_t count) +{ + const int32x4_t sign = vdupq_n_s32(0x80000000); + + for (size_t i = 0; i < count; i += 4) + { + int32x4_t n4 = vld1q_s32(reinterpret_cast<int32_t*>(&data[i * 4])); + + // sign-extends each of x,y in [x y ? ?] with arithmetic shifts + int32x4_t xf = vshrq_n_s32(vshlq_n_s32(n4, 24), 24); + int32x4_t yf = vshrq_n_s32(vshlq_n_s32(n4, 16), 24); + + // unpack z; note that z is unsigned so we technically don't need to sign extend it + int32x4_t zf = vshrq_n_s32(vshlq_n_s32(n4, 8), 24); + + // convert x and y to floats and reconstruct z; this assumes zf encodes 1.f at the same bit count + float32x4_t x = vcvtq_f32_s32(xf); + float32x4_t y = vcvtq_f32_s32(yf); + float32x4_t z = vsubq_f32(vcvtq_f32_s32(zf), vaddq_f32(vabsq_f32(x), vabsq_f32(y))); + + // fixup octahedral coordinates for z<0 + float32x4_t t = vminq_f32(z, vdupq_n_f32(0.f)); + + x = vaddq_f32(x, vreinterpretq_f32_s32(veorq_s32(vreinterpretq_s32_f32(t), vandq_s32(vreinterpretq_s32_f32(x), sign)))); + y = vaddq_f32(y, vreinterpretq_f32_s32(veorq_s32(vreinterpretq_s32_f32(t), vandq_s32(vreinterpretq_s32_f32(y), sign)))); + + // compute normal length & scale + float32x4_t ll = vaddq_f32(vmulq_f32(x, x), vaddq_f32(vmulq_f32(y, y), vmulq_f32(z, z))); + float32x4_t rl = vrsqrteq_f32(ll); + float32x4_t s = vmulq_f32(vdupq_n_f32(127.f), rl); + + // fast rounded signed float->int: addition triggers renormalization after which mantissa stores the integer value + // note: the result is offset by 0x4B40_0000, but we only need the low 16 bits so we can omit the subtraction + const float32x4_t fsnap = vdupq_n_f32(3 << 22); + + int32x4_t xr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(x, s), fsnap)); + int32x4_t yr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(y, s), fsnap)); + int32x4_t zr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(z, s), fsnap)); + + // combine xr/yr/zr into final value + int32x4_t res = vandq_s32(n4, vdupq_n_s32(0xff000000)); + res = vorrq_s32(res, vandq_s32(xr, vdupq_n_s32(0xff))); + res = vorrq_s32(res, vshlq_n_s32(vandq_s32(yr, vdupq_n_s32(0xff)), 8)); + res = vorrq_s32(res, vshlq_n_s32(vandq_s32(zr, vdupq_n_s32(0xff)), 16)); + + vst1q_s32(reinterpret_cast<int32_t*>(&data[i * 4]), res); + } +} + +static void decodeFilterOctSimd(short* data, size_t count) +{ + const int32x4_t sign = vdupq_n_s32(0x80000000); + + for (size_t i = 0; i < count; i += 4) + { + int32x4_t n4_0 = vld1q_s32(reinterpret_cast<int32_t*>(&data[(i + 0) * 4])); + int32x4_t n4_1 = vld1q_s32(reinterpret_cast<int32_t*>(&data[(i + 2) * 4])); + + // gather both x/y 16-bit pairs in each 32-bit lane + int32x4_t n4 = vuzpq_s32(n4_0, n4_1).val[0]; + + // sign-extends each of x,y in [x y] with arithmetic shifts + int32x4_t xf = vshrq_n_s32(vshlq_n_s32(n4, 16), 16); + int32x4_t yf = vshrq_n_s32(n4, 16); + + // unpack z; note that z is unsigned so we don't need to sign extend it + int32x4_t z4 = vuzpq_s32(n4_0, n4_1).val[1]; + int32x4_t zf = vandq_s32(z4, vdupq_n_s32(0x7fff)); + + // convert x and y to floats and reconstruct z; this assumes zf encodes 1.f at the same bit count + float32x4_t x = vcvtq_f32_s32(xf); + float32x4_t y = vcvtq_f32_s32(yf); + float32x4_t z = vsubq_f32(vcvtq_f32_s32(zf), vaddq_f32(vabsq_f32(x), vabsq_f32(y))); + + // fixup octahedral coordinates for z<0 + float32x4_t t = vminq_f32(z, vdupq_n_f32(0.f)); + + x = vaddq_f32(x, vreinterpretq_f32_s32(veorq_s32(vreinterpretq_s32_f32(t), vandq_s32(vreinterpretq_s32_f32(x), sign)))); + y = vaddq_f32(y, vreinterpretq_f32_s32(veorq_s32(vreinterpretq_s32_f32(t), vandq_s32(vreinterpretq_s32_f32(y), sign)))); + + // compute normal length & scale + float32x4_t ll = vaddq_f32(vmulq_f32(x, x), vaddq_f32(vmulq_f32(y, y), vmulq_f32(z, z))); + float32x4_t rl = vrsqrteq_f32(ll); + rl = vmulq_f32(rl, vrsqrtsq_f32(vmulq_f32(rl, ll), rl)); // refine rsqrt estimate + float32x4_t s = vmulq_f32(vdupq_n_f32(32767.f), rl); + + // fast rounded signed float->int: addition triggers renormalization after which mantissa stores the integer value + // note: the result is offset by 0x4B40_0000, but we only need the low 16 bits so we can omit the subtraction + const float32x4_t fsnap = vdupq_n_f32(3 << 22); + + int32x4_t xr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(x, s), fsnap)); + int32x4_t yr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(y, s), fsnap)); + int32x4_t zr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(z, s), fsnap)); + + // mix x/z and y/0 to make 16-bit unpack easier + int32x4_t xzr = vorrq_s32(vandq_s32(xr, vdupq_n_s32(0xffff)), vshlq_n_s32(zr, 16)); + int32x4_t y0r = vandq_s32(yr, vdupq_n_s32(0xffff)); + + // pack x/y/z using 16-bit unpacks; note that this has 0 where we should have .w + int32x4_t res_0 = vreinterpretq_s32_s16(vzipq_s16(vreinterpretq_s16_s32(xzr), vreinterpretq_s16_s32(y0r)).val[0]); + int32x4_t res_1 = vreinterpretq_s32_s16(vzipq_s16(vreinterpretq_s16_s32(xzr), vreinterpretq_s16_s32(y0r)).val[1]); + + // patch in .w + res_0 = vbslq_s32(vreinterpretq_u32_u64(vdupq_n_u64(0xffff000000000000)), n4_0, res_0); + res_1 = vbslq_s32(vreinterpretq_u32_u64(vdupq_n_u64(0xffff000000000000)), n4_1, res_1); + + vst1q_s32(reinterpret_cast<int32_t*>(&data[(i + 0) * 4]), res_0); + vst1q_s32(reinterpret_cast<int32_t*>(&data[(i + 2) * 4]), res_1); + } +} + +static void decodeFilterQuatSimd(short* data, size_t count) +{ + const float scale = 1.f / sqrtf(2.f); + + for (size_t i = 0; i < count; i += 4) + { + int32x4_t q4_0 = vld1q_s32(reinterpret_cast<int32_t*>(&data[(i + 0) * 4])); + int32x4_t q4_1 = vld1q_s32(reinterpret_cast<int32_t*>(&data[(i + 2) * 4])); + + // gather both x/y 16-bit pairs in each 32-bit lane + int32x4_t q4_xy = vuzpq_s32(q4_0, q4_1).val[0]; + int32x4_t q4_zc = vuzpq_s32(q4_0, q4_1).val[1]; + + // sign-extends each of x,y in [x y] with arithmetic shifts + int32x4_t xf = vshrq_n_s32(vshlq_n_s32(q4_xy, 16), 16); + int32x4_t yf = vshrq_n_s32(q4_xy, 16); + int32x4_t zf = vshrq_n_s32(vshlq_n_s32(q4_zc, 16), 16); + int32x4_t cf = vshrq_n_s32(q4_zc, 16); + + // get a floating-point scaler using zc with bottom 2 bits set to 1 (which represents 1.f) + int32x4_t sf = vorrq_s32(cf, vdupq_n_s32(3)); + float32x4_t ss = vdivq_f32(vdupq_n_f32(scale), vcvtq_f32_s32(sf)); + + // convert x/y/z to [-1..1] (scaled...) + float32x4_t x = vmulq_f32(vcvtq_f32_s32(xf), ss); + float32x4_t y = vmulq_f32(vcvtq_f32_s32(yf), ss); + float32x4_t z = vmulq_f32(vcvtq_f32_s32(zf), ss); + + // reconstruct w as a square root; we clamp to 0.f to avoid NaN due to precision errors + float32x4_t ww = vsubq_f32(vdupq_n_f32(1.f), vaddq_f32(vmulq_f32(x, x), vaddq_f32(vmulq_f32(y, y), vmulq_f32(z, z)))); + float32x4_t w = vsqrtq_f32(vmaxq_f32(ww, vdupq_n_f32(0.f))); + + float32x4_t s = vdupq_n_f32(32767.f); + + // fast rounded signed float->int: addition triggers renormalization after which mantissa stores the integer value + // note: the result is offset by 0x4B40_0000, but we only need the low 16 bits so we can omit the subtraction + const float32x4_t fsnap = vdupq_n_f32(3 << 22); + + int32x4_t xr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(x, s), fsnap)); + int32x4_t yr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(y, s), fsnap)); + int32x4_t zr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(z, s), fsnap)); + int32x4_t wr = vreinterpretq_s32_f32(vaddq_f32(vmulq_f32(w, s), fsnap)); + + // mix x/z and w/y to make 16-bit unpack easier + int32x4_t xzr = vorrq_s32(vandq_s32(xr, vdupq_n_s32(0xffff)), vshlq_n_s32(zr, 16)); + int32x4_t wyr = vorrq_s32(vandq_s32(wr, vdupq_n_s32(0xffff)), vshlq_n_s32(yr, 16)); + + // pack x/y/z/w using 16-bit unpacks; we pack wxyz by default (for qc=0) + int32x4_t res_0 = vreinterpretq_s32_s16(vzipq_s16(vreinterpretq_s16_s32(wyr), vreinterpretq_s16_s32(xzr)).val[0]); + int32x4_t res_1 = vreinterpretq_s32_s16(vzipq_s16(vreinterpretq_s16_s32(wyr), vreinterpretq_s16_s32(xzr)).val[1]); + + // rotate and store + uint64_t* out = (uint64_t*)&data[i * 4]; + + out[0] = rotateleft64(vgetq_lane_u64(vreinterpretq_u64_s32(res_0), 0), vgetq_lane_s32(cf, 0) << 4); + out[1] = rotateleft64(vgetq_lane_u64(vreinterpretq_u64_s32(res_0), 1), vgetq_lane_s32(cf, 1) << 4); + out[2] = rotateleft64(vgetq_lane_u64(vreinterpretq_u64_s32(res_1), 0), vgetq_lane_s32(cf, 2) << 4); + out[3] = rotateleft64(vgetq_lane_u64(vreinterpretq_u64_s32(res_1), 1), vgetq_lane_s32(cf, 3) << 4); + } +} + +static void decodeFilterExpSimd(unsigned int* data, size_t count) +{ + for (size_t i = 0; i < count; i += 4) + { + int32x4_t v = vld1q_s32(reinterpret_cast<int32_t*>(&data[i])); + + // decode exponent into 2^x directly + int32x4_t ef = vshrq_n_s32(v, 24); + int32x4_t es = vshlq_n_s32(vaddq_s32(ef, vdupq_n_s32(127)), 23); + + // decode 24-bit mantissa into floating-point value + int32x4_t mf = vshrq_n_s32(vshlq_n_s32(v, 8), 8); + float32x4_t m = vcvtq_f32_s32(mf); + + float32x4_t r = vmulq_f32(vreinterpretq_f32_s32(es), m); + + vst1q_f32(reinterpret_cast<float*>(&data[i]), r); + } +} +#endif + +#ifdef SIMD_WASM +static void decodeFilterOctSimd(signed char* data, size_t count) +{ + const v128_t sign = wasm_f32x4_splat(-0.f); + + for (size_t i = 0; i < count; i += 4) + { + v128_t n4 = wasm_v128_load(&data[i * 4]); + + // sign-extends each of x,y in [x y ? ?] with arithmetic shifts + v128_t xf = wasm_i32x4_shr(wasm_i32x4_shl(n4, 24), 24); + v128_t yf = wasm_i32x4_shr(wasm_i32x4_shl(n4, 16), 24); + + // unpack z; note that z is unsigned so we technically don't need to sign extend it + v128_t zf = wasm_i32x4_shr(wasm_i32x4_shl(n4, 8), 24); + + // convert x and y to floats and reconstruct z; this assumes zf encodes 1.f at the same bit count + v128_t x = wasm_f32x4_convert_i32x4(xf); + v128_t y = wasm_f32x4_convert_i32x4(yf); + v128_t z = wasm_f32x4_sub(wasm_f32x4_convert_i32x4(zf), wasm_f32x4_add(wasm_f32x4_abs(x), wasm_f32x4_abs(y))); + + // fixup octahedral coordinates for z<0 + // note: i32x4_min with 0 is equvalent to f32x4_min + v128_t t = wasm_i32x4_min(z, wasm_i32x4_splat(0)); + + x = wasm_f32x4_add(x, wasm_v128_xor(t, wasm_v128_and(x, sign))); + y = wasm_f32x4_add(y, wasm_v128_xor(t, wasm_v128_and(y, sign))); + + // compute normal length & scale + v128_t ll = wasm_f32x4_add(wasm_f32x4_mul(x, x), wasm_f32x4_add(wasm_f32x4_mul(y, y), wasm_f32x4_mul(z, z))); + v128_t s = wasm_f32x4_div(wasm_f32x4_splat(127.f), wasm_f32x4_sqrt(ll)); + + // fast rounded signed float->int: addition triggers renormalization after which mantissa stores the integer value + // note: the result is offset by 0x4B40_0000, but we only need the low 8 bits so we can omit the subtraction + const v128_t fsnap = wasm_f32x4_splat(3 << 22); + + v128_t xr = wasm_f32x4_add(wasm_f32x4_mul(x, s), fsnap); + v128_t yr = wasm_f32x4_add(wasm_f32x4_mul(y, s), fsnap); + v128_t zr = wasm_f32x4_add(wasm_f32x4_mul(z, s), fsnap); + + // combine xr/yr/zr into final value + v128_t res = wasm_v128_and(n4, wasm_i32x4_splat(0xff000000)); + res = wasm_v128_or(res, wasm_v128_and(xr, wasm_i32x4_splat(0xff))); + res = wasm_v128_or(res, wasm_i32x4_shl(wasm_v128_and(yr, wasm_i32x4_splat(0xff)), 8)); + res = wasm_v128_or(res, wasm_i32x4_shl(wasm_v128_and(zr, wasm_i32x4_splat(0xff)), 16)); + + wasm_v128_store(&data[i * 4], res); + } +} + +static void decodeFilterOctSimd(short* data, size_t count) +{ + const v128_t sign = wasm_f32x4_splat(-0.f); + const v128_t zmask = wasm_i32x4_splat(0x7fff); + + for (size_t i = 0; i < count; i += 4) + { + v128_t n4_0 = wasm_v128_load(&data[(i + 0) * 4]); + v128_t n4_1 = wasm_v128_load(&data[(i + 2) * 4]); + + // gather both x/y 16-bit pairs in each 32-bit lane + v128_t n4 = wasmx_unziplo_v32x4(n4_0, n4_1); + + // sign-extends each of x,y in [x y] with arithmetic shifts + v128_t xf = wasm_i32x4_shr(wasm_i32x4_shl(n4, 16), 16); + v128_t yf = wasm_i32x4_shr(n4, 16); + + // unpack z; note that z is unsigned so we don't need to sign extend it + v128_t z4 = wasmx_unziphi_v32x4(n4_0, n4_1); + v128_t zf = wasm_v128_and(z4, zmask); + + // convert x and y to floats and reconstruct z; this assumes zf encodes 1.f at the same bit count + v128_t x = wasm_f32x4_convert_i32x4(xf); + v128_t y = wasm_f32x4_convert_i32x4(yf); + v128_t z = wasm_f32x4_sub(wasm_f32x4_convert_i32x4(zf), wasm_f32x4_add(wasm_f32x4_abs(x), wasm_f32x4_abs(y))); + + // fixup octahedral coordinates for z<0 + // note: i32x4_min with 0 is equvalent to f32x4_min + v128_t t = wasm_i32x4_min(z, wasm_i32x4_splat(0)); + + x = wasm_f32x4_add(x, wasm_v128_xor(t, wasm_v128_and(x, sign))); + y = wasm_f32x4_add(y, wasm_v128_xor(t, wasm_v128_and(y, sign))); + + // compute normal length & scale + v128_t ll = wasm_f32x4_add(wasm_f32x4_mul(x, x), wasm_f32x4_add(wasm_f32x4_mul(y, y), wasm_f32x4_mul(z, z))); + v128_t s = wasm_f32x4_div(wasm_f32x4_splat(32767.f), wasm_f32x4_sqrt(ll)); + + // fast rounded signed float->int: addition triggers renormalization after which mantissa stores the integer value + // note: the result is offset by 0x4B40_0000, but we only need the low 16 bits so we can omit the subtraction + const v128_t fsnap = wasm_f32x4_splat(3 << 22); + + v128_t xr = wasm_f32x4_add(wasm_f32x4_mul(x, s), fsnap); + v128_t yr = wasm_f32x4_add(wasm_f32x4_mul(y, s), fsnap); + v128_t zr = wasm_f32x4_add(wasm_f32x4_mul(z, s), fsnap); + + // mix x/z and y/0 to make 16-bit unpack easier + v128_t xzr = wasm_v128_or(wasm_v128_and(xr, wasm_i32x4_splat(0xffff)), wasm_i32x4_shl(zr, 16)); + v128_t y0r = wasm_v128_and(yr, wasm_i32x4_splat(0xffff)); + + // pack x/y/z using 16-bit unpacks; note that this has 0 where we should have .w + v128_t res_0 = wasmx_unpacklo_v16x8(xzr, y0r); + v128_t res_1 = wasmx_unpackhi_v16x8(xzr, y0r); + + // patch in .w + res_0 = wasm_v128_or(res_0, wasm_v128_and(n4_0, wasm_i64x2_splat(0xffff000000000000))); + res_1 = wasm_v128_or(res_1, wasm_v128_and(n4_1, wasm_i64x2_splat(0xffff000000000000))); + + wasm_v128_store(&data[(i + 0) * 4], res_0); + wasm_v128_store(&data[(i + 2) * 4], res_1); + } +} + +static void decodeFilterQuatSimd(short* data, size_t count) +{ + const float scale = 1.f / sqrtf(2.f); + + for (size_t i = 0; i < count; i += 4) + { + v128_t q4_0 = wasm_v128_load(&data[(i + 0) * 4]); + v128_t q4_1 = wasm_v128_load(&data[(i + 2) * 4]); + + // gather both x/y 16-bit pairs in each 32-bit lane + v128_t q4_xy = wasmx_unziplo_v32x4(q4_0, q4_1); + v128_t q4_zc = wasmx_unziphi_v32x4(q4_0, q4_1); + + // sign-extends each of x,y in [x y] with arithmetic shifts + v128_t xf = wasm_i32x4_shr(wasm_i32x4_shl(q4_xy, 16), 16); + v128_t yf = wasm_i32x4_shr(q4_xy, 16); + v128_t zf = wasm_i32x4_shr(wasm_i32x4_shl(q4_zc, 16), 16); + v128_t cf = wasm_i32x4_shr(q4_zc, 16); + + // get a floating-point scaler using zc with bottom 2 bits set to 1 (which represents 1.f) + v128_t sf = wasm_v128_or(cf, wasm_i32x4_splat(3)); + v128_t ss = wasm_f32x4_div(wasm_f32x4_splat(scale), wasm_f32x4_convert_i32x4(sf)); + + // convert x/y/z to [-1..1] (scaled...) + v128_t x = wasm_f32x4_mul(wasm_f32x4_convert_i32x4(xf), ss); + v128_t y = wasm_f32x4_mul(wasm_f32x4_convert_i32x4(yf), ss); + v128_t z = wasm_f32x4_mul(wasm_f32x4_convert_i32x4(zf), ss); + + // reconstruct w as a square root; we clamp to 0.f to avoid NaN due to precision errors + // note: i32x4_max with 0 is equivalent to f32x4_max + v128_t ww = wasm_f32x4_sub(wasm_f32x4_splat(1.f), wasm_f32x4_add(wasm_f32x4_mul(x, x), wasm_f32x4_add(wasm_f32x4_mul(y, y), wasm_f32x4_mul(z, z)))); + v128_t w = wasm_f32x4_sqrt(wasm_i32x4_max(ww, wasm_i32x4_splat(0))); + + v128_t s = wasm_f32x4_splat(32767.f); + + // fast rounded signed float->int: addition triggers renormalization after which mantissa stores the integer value + // note: the result is offset by 0x4B40_0000, but we only need the low 16 bits so we can omit the subtraction + const v128_t fsnap = wasm_f32x4_splat(3 << 22); + + v128_t xr = wasm_f32x4_add(wasm_f32x4_mul(x, s), fsnap); + v128_t yr = wasm_f32x4_add(wasm_f32x4_mul(y, s), fsnap); + v128_t zr = wasm_f32x4_add(wasm_f32x4_mul(z, s), fsnap); + v128_t wr = wasm_f32x4_add(wasm_f32x4_mul(w, s), fsnap); + + // mix x/z and w/y to make 16-bit unpack easier + v128_t xzr = wasm_v128_or(wasm_v128_and(xr, wasm_i32x4_splat(0xffff)), wasm_i32x4_shl(zr, 16)); + v128_t wyr = wasm_v128_or(wasm_v128_and(wr, wasm_i32x4_splat(0xffff)), wasm_i32x4_shl(yr, 16)); + + // pack x/y/z/w using 16-bit unpacks; we pack wxyz by default (for qc=0) + v128_t res_0 = wasmx_unpacklo_v16x8(wyr, xzr); + v128_t res_1 = wasmx_unpackhi_v16x8(wyr, xzr); + + // compute component index shifted left by 4 (and moved into i32x4 slot) + // TODO: volatile here works around LLVM mis-optimizing code; https://github.com/emscripten-core/emscripten/issues/11449 + volatile v128_t cm = wasm_i32x4_shl(cf, 4); + + // rotate and store + uint64_t* out = reinterpret_cast<uint64_t*>(&data[i * 4]); + + out[0] = rotateleft64(wasm_i64x2_extract_lane(res_0, 0), wasm_i32x4_extract_lane(cm, 0)); + out[1] = rotateleft64(wasm_i64x2_extract_lane(res_0, 1), wasm_i32x4_extract_lane(cm, 1)); + out[2] = rotateleft64(wasm_i64x2_extract_lane(res_1, 0), wasm_i32x4_extract_lane(cm, 2)); + out[3] = rotateleft64(wasm_i64x2_extract_lane(res_1, 1), wasm_i32x4_extract_lane(cm, 3)); + } +} + +static void decodeFilterExpSimd(unsigned int* data, size_t count) +{ + for (size_t i = 0; i < count; i += 4) + { + v128_t v = wasm_v128_load(&data[i]); + + // decode exponent into 2^x directly + v128_t ef = wasm_i32x4_shr(v, 24); + v128_t es = wasm_i32x4_shl(wasm_i32x4_add(ef, wasm_i32x4_splat(127)), 23); + + // decode 24-bit mantissa into floating-point value + v128_t mf = wasm_i32x4_shr(wasm_i32x4_shl(v, 8), 8); + v128_t m = wasm_f32x4_convert_i32x4(mf); + + v128_t r = wasm_f32x4_mul(es, m); + + wasm_v128_store(&data[i], r); + } +} +#endif + +} // namespace meshopt + +void meshopt_decodeFilterOct(void* buffer, size_t vertex_count, size_t vertex_size) +{ + using namespace meshopt; + + assert(vertex_count % 4 == 0); + assert(vertex_size == 4 || vertex_size == 8); + +#if defined(SIMD_SSE) || defined(SIMD_NEON) || defined(SIMD_WASM) + if (vertex_size == 4) + decodeFilterOctSimd(static_cast<signed char*>(buffer), vertex_count); + else + decodeFilterOctSimd(static_cast<short*>(buffer), vertex_count); +#else + if (vertex_size == 4) + decodeFilterOct(static_cast<signed char*>(buffer), vertex_count); + else + decodeFilterOct(static_cast<short*>(buffer), vertex_count); +#endif +} + +void meshopt_decodeFilterQuat(void* buffer, size_t vertex_count, size_t vertex_size) +{ + using namespace meshopt; + + assert(vertex_count % 4 == 0); + assert(vertex_size == 8); + (void)vertex_size; + +#if defined(SIMD_SSE) || defined(SIMD_NEON) || defined(SIMD_WASM) + decodeFilterQuatSimd(static_cast<short*>(buffer), vertex_count); +#else + decodeFilterQuat(static_cast<short*>(buffer), vertex_count); +#endif +} + +void meshopt_decodeFilterExp(void* buffer, size_t vertex_count, size_t vertex_size) +{ + using namespace meshopt; + + assert(vertex_count % 4 == 0); + assert(vertex_size % 4 == 0); + +#if defined(SIMD_SSE) || defined(SIMD_NEON) || defined(SIMD_WASM) + decodeFilterExpSimd(static_cast<unsigned int*>(buffer), vertex_count * (vertex_size / 4)); +#else + decodeFilterExp(static_cast<unsigned int*>(buffer), vertex_count * (vertex_size / 4)); +#endif +} + +#undef SIMD_SSE +#undef SIMD_NEON +#undef SIMD_WASM diff --git a/thirdparty/meshoptimizer/vfetchanalyzer.cpp b/thirdparty/meshoptimizer/vfetchanalyzer.cpp new file mode 100644 index 0000000000..51dca873f8 --- /dev/null +++ b/thirdparty/meshoptimizer/vfetchanalyzer.cpp @@ -0,0 +1,58 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <string.h> + +meshopt_VertexFetchStatistics meshopt_analyzeVertexFetch(const unsigned int* indices, size_t index_count, size_t vertex_count, size_t vertex_size) +{ + assert(index_count % 3 == 0); + assert(vertex_size > 0 && vertex_size <= 256); + + meshopt_Allocator allocator; + + meshopt_VertexFetchStatistics result = {}; + + unsigned char* vertex_visited = allocator.allocate<unsigned char>(vertex_count); + memset(vertex_visited, 0, vertex_count); + + const size_t kCacheLine = 64; + const size_t kCacheSize = 128 * 1024; + + // simple direct mapped cache; on typical mesh data this is close to 4-way cache, and this model is a gross approximation anyway + size_t cache[kCacheSize / kCacheLine] = {}; + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices[i]; + assert(index < vertex_count); + + vertex_visited[index] = 1; + + size_t start_address = index * vertex_size; + size_t end_address = start_address + vertex_size; + + size_t start_tag = start_address / kCacheLine; + size_t end_tag = (end_address + kCacheLine - 1) / kCacheLine; + + assert(start_tag < end_tag); + + for (size_t tag = start_tag; tag < end_tag; ++tag) + { + size_t line = tag % (sizeof(cache) / sizeof(cache[0])); + + // we store +1 since cache is filled with 0 by default + result.bytes_fetched += (cache[line] != tag + 1) * kCacheLine; + cache[line] = tag + 1; + } + } + + size_t unique_vertex_count = 0; + + for (size_t i = 0; i < vertex_count; ++i) + unique_vertex_count += vertex_visited[i]; + + result.overfetch = unique_vertex_count == 0 ? 0 : float(result.bytes_fetched) / float(unique_vertex_count * vertex_size); + + return result; +} diff --git a/thirdparty/meshoptimizer/vfetchoptimizer.cpp b/thirdparty/meshoptimizer/vfetchoptimizer.cpp new file mode 100644 index 0000000000..465d6df5ca --- /dev/null +++ b/thirdparty/meshoptimizer/vfetchoptimizer.cpp @@ -0,0 +1,74 @@ +// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details +#include "meshoptimizer.h" + +#include <assert.h> +#include <string.h> + +size_t meshopt_optimizeVertexFetchRemap(unsigned int* destination, const unsigned int* indices, size_t index_count, size_t vertex_count) +{ + assert(index_count % 3 == 0); + + memset(destination, -1, vertex_count * sizeof(unsigned int)); + + unsigned int next_vertex = 0; + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices[i]; + assert(index < vertex_count); + + if (destination[index] == ~0u) + { + destination[index] = next_vertex++; + } + } + + assert(next_vertex <= vertex_count); + + return next_vertex; +} + +size_t meshopt_optimizeVertexFetch(void* destination, unsigned int* indices, size_t index_count, const void* vertices, size_t vertex_count, size_t vertex_size) +{ + assert(index_count % 3 == 0); + assert(vertex_size > 0 && vertex_size <= 256); + + meshopt_Allocator allocator; + + // support in-place optimization + if (destination == vertices) + { + unsigned char* vertices_copy = allocator.allocate<unsigned char>(vertex_count * vertex_size); + memcpy(vertices_copy, vertices, vertex_count * vertex_size); + vertices = vertices_copy; + } + + // build vertex remap table + unsigned int* vertex_remap = allocator.allocate<unsigned int>(vertex_count); + memset(vertex_remap, -1, vertex_count * sizeof(unsigned int)); + + unsigned int next_vertex = 0; + + for (size_t i = 0; i < index_count; ++i) + { + unsigned int index = indices[i]; + assert(index < vertex_count); + + unsigned int& remap = vertex_remap[index]; + + if (remap == ~0u) // vertex was not added to destination VB + { + // add vertex + memcpy(static_cast<unsigned char*>(destination) + next_vertex * vertex_size, static_cast<const unsigned char*>(vertices) + index * vertex_size, vertex_size); + + remap = next_vertex++; + } + + // modify indices in place + indices[i] = remap; + } + + assert(next_vertex <= vertex_count); + + return next_vertex; +} diff --git a/thirdparty/minimp3/LICENSE b/thirdparty/minimp3/LICENSE new file mode 100644 index 0000000000..2c4afabdb6 --- /dev/null +++ b/thirdparty/minimp3/LICENSE @@ -0,0 +1,117 @@ +CC0 1.0 Universal + +Statement of Purpose + +The laws of most jurisdictions throughout the world automatically confer +exclusive Copyright and Related Rights (defined below) upon the creator and +subsequent owner(s) (each and all, an "owner") of an original work of +authorship and/or a database (each, a "Work"). + +Certain owners wish to permanently relinquish those rights to a Work for the +purpose of contributing to a commons of creative, cultural and scientific +works ("Commons") that the public can reliably and without fear of later +claims of infringement build upon, modify, incorporate in other works, reuse +and redistribute as freely as possible in any form whatsoever and for any +purposes, including without limitation commercial purposes. These owners may +contribute to the Commons to promote the ideal of a free culture and the +further production of creative, cultural and scientific works, or to gain +reputation or greater distribution for their Work in part through the use and +efforts of others. + +For these and/or other purposes and motivations, and without any expectation +of additional consideration or compensation, the person associating CC0 with a +Work (the "Affirmer"), to the extent that he or she is an owner of Copyright +and Related Rights in the Work, voluntarily elects to apply CC0 to the Work +and publicly distribute the Work under its terms, with knowledge of his or her +Copyright and Related Rights in the Work and the meaning and intended legal +effect of CC0 on those rights. + +1. Copyright and Related Rights. A Work made available under CC0 may be +protected by copyright and related or neighboring rights ("Copyright and +Related Rights"). Copyright and Related Rights include, but are not limited +to, the following: + + i. the right to reproduce, adapt, distribute, perform, display, communicate, + and translate a Work; + + ii. moral rights retained by the original author(s) and/or performer(s); + + iii. publicity and privacy rights pertaining to a person's image or likeness + depicted in a Work; + + iv. rights protecting against unfair competition in regards to a Work, + subject to the limitations in paragraph 4(a), below; + + v. rights protecting the extraction, dissemination, use and reuse of data in + a Work; + + vi. database rights (such as those arising under Directive 96/9/EC of the + European Parliament and of the Council of 11 March 1996 on the legal + protection of databases, and under any national implementation thereof, + including any amended or successor version of such directive); and + + vii. other similar, equivalent or corresponding rights throughout the world + based on applicable law or treaty, and any national implementations thereof. + +2. Waiver. To the greatest extent permitted by, but not in contravention of, +applicable law, Affirmer hereby overtly, fully, permanently, irrevocably and +unconditionally waives, abandons, and surrenders all of Affirmer's Copyright +and Related Rights and associated claims and causes of action, whether now +known or unknown (including existing as well as future claims and causes of +action), in the Work (i) in all territories worldwide, (ii) for the maximum +duration provided by applicable law or treaty (including future time +extensions), (iii) in any current or future medium and for any number of +copies, and (iv) for any purpose whatsoever, including without limitation +commercial, advertising or promotional purposes (the "Waiver"). Affirmer makes +the Waiver for the benefit of each member of the public at large and to the +detriment of Affirmer's heirs and successors, fully intending that such Waiver +shall not be subject to revocation, rescission, cancellation, termination, or +any other legal or equitable action to disrupt the quiet enjoyment of the Work +by the public as contemplated by Affirmer's express Statement of Purpose. + +3. Public License Fallback. Should any part of the Waiver for any reason be +judged legally invalid or ineffective under applicable law, then the Waiver +shall be preserved to the maximum extent permitted taking into account +Affirmer's express Statement of Purpose. In addition, to the extent the Waiver +is so judged Affirmer hereby grants to each affected person a royalty-free, +non transferable, non sublicensable, non exclusive, irrevocable and +unconditional license to exercise Affirmer's Copyright and Related Rights in +the Work (i) in all territories worldwide, (ii) for the maximum duration +provided by applicable law or treaty (including future time extensions), (iii) +in any current or future medium and for any number of copies, and (iv) for any +purpose whatsoever, including without limitation commercial, advertising or +promotional purposes (the "License"). The License shall be deemed effective as +of the date CC0 was applied by Affirmer to the Work. Should any part of the +License for any reason be judged legally invalid or ineffective under +applicable law, such partial invalidity or ineffectiveness shall not +invalidate the remainder of the License, and in such case Affirmer hereby +affirms that he or she will not (i) exercise any of his or her remaining +Copyright and Related Rights in the Work or (ii) assert any associated claims +and causes of action with respect to the Work, in either case contrary to +Affirmer's express Statement of Purpose. + +4. Limitations and Disclaimers. + + a. No trademark or patent rights held by Affirmer are waived, abandoned, + surrendered, licensed or otherwise affected by this document. + + b. Affirmer offers the Work as-is and makes no representations or warranties + of any kind concerning the Work, express, implied, statutory or otherwise, + including without limitation warranties of title, merchantability, fitness + for a particular purpose, non infringement, or the absence of latent or + other defects, accuracy, or the present or absence of errors, whether or not + discoverable, all to the greatest extent permissible under applicable law. + + c. Affirmer disclaims responsibility for clearing rights of other persons + that may apply to the Work or any use thereof, including without limitation + any person's Copyright and Related Rights in the Work. Further, Affirmer + disclaims responsibility for obtaining any necessary consents, permissions + or other rights required for any use of the Work. + + d. Affirmer understands and acknowledges that Creative Commons is not a + party to this document and has no duty or obligation with respect to this + CC0 or use of the Work. + +For more information, please see +<http://creativecommons.org/publicdomain/zero/1.0/> + diff --git a/thirdparty/minimp3/minimp3.h b/thirdparty/minimp3/minimp3.h new file mode 100644 index 0000000000..796cbc1f8e --- /dev/null +++ b/thirdparty/minimp3/minimp3.h @@ -0,0 +1,1855 @@ +#ifndef MINIMP3_H +#define MINIMP3_H +/* + https://github.com/lieff/minimp3 + To the extent possible under law, the author(s) have dedicated all copyright and related and neighboring rights to this software to the public domain worldwide. + This software is distributed without any warranty. + See <http://creativecommons.org/publicdomain/zero/1.0/>. +*/ +#include <stdint.h> + +#define MINIMP3_MAX_SAMPLES_PER_FRAME (1152*2) + +typedef struct +{ + int frame_bytes, frame_offset, channels, hz, layer, bitrate_kbps; +} mp3dec_frame_info_t; + +typedef struct +{ + float mdct_overlap[2][9*32], qmf_state[15*2*32]; + int reserv, free_format_bytes; + unsigned char header[4], reserv_buf[511]; +} mp3dec_t; + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +void mp3dec_init(mp3dec_t *dec); +#ifndef MINIMP3_FLOAT_OUTPUT +typedef int16_t mp3d_sample_t; +#else /* MINIMP3_FLOAT_OUTPUT */ +typedef float mp3d_sample_t; +void mp3dec_f32_to_s16(const float *in, int16_t *out, int num_samples); +#endif /* MINIMP3_FLOAT_OUTPUT */ +int mp3dec_decode_frame(mp3dec_t *dec, const uint8_t *mp3, int mp3_bytes, mp3d_sample_t *pcm, mp3dec_frame_info_t *info); + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* MINIMP3_H */ +#if defined(MINIMP3_IMPLEMENTATION) && !defined(_MINIMP3_IMPLEMENTATION_GUARD) +#define _MINIMP3_IMPLEMENTATION_GUARD + +#include <stdlib.h> +#include <string.h> + +#define MAX_FREE_FORMAT_FRAME_SIZE 2304 /* more than ISO spec's */ +#ifndef MAX_FRAME_SYNC_MATCHES +#define MAX_FRAME_SYNC_MATCHES 10 +#endif /* MAX_FRAME_SYNC_MATCHES */ + +#define MAX_L3_FRAME_PAYLOAD_BYTES MAX_FREE_FORMAT_FRAME_SIZE /* MUST be >= 320000/8/32000*1152 = 1440 */ + +#define MAX_BITRESERVOIR_BYTES 511 +#define SHORT_BLOCK_TYPE 2 +#define STOP_BLOCK_TYPE 3 +#define MODE_MONO 3 +#define MODE_JOINT_STEREO 1 +#define HDR_SIZE 4 +#define HDR_IS_MONO(h) (((h[3]) & 0xC0) == 0xC0) +#define HDR_IS_MS_STEREO(h) (((h[3]) & 0xE0) == 0x60) +#define HDR_IS_FREE_FORMAT(h) (((h[2]) & 0xF0) == 0) +#define HDR_IS_CRC(h) (!((h[1]) & 1)) +#define HDR_TEST_PADDING(h) ((h[2]) & 0x2) +#define HDR_TEST_MPEG1(h) ((h[1]) & 0x8) +#define HDR_TEST_NOT_MPEG25(h) ((h[1]) & 0x10) +#define HDR_TEST_I_STEREO(h) ((h[3]) & 0x10) +#define HDR_TEST_MS_STEREO(h) ((h[3]) & 0x20) +#define HDR_GET_STEREO_MODE(h) (((h[3]) >> 6) & 3) +#define HDR_GET_STEREO_MODE_EXT(h) (((h[3]) >> 4) & 3) +#define HDR_GET_LAYER(h) (((h[1]) >> 1) & 3) +#define HDR_GET_BITRATE(h) ((h[2]) >> 4) +#define HDR_GET_SAMPLE_RATE(h) (((h[2]) >> 2) & 3) +#define HDR_GET_MY_SAMPLE_RATE(h) (HDR_GET_SAMPLE_RATE(h) + (((h[1] >> 3) & 1) + ((h[1] >> 4) & 1))*3) +#define HDR_IS_FRAME_576(h) ((h[1] & 14) == 2) +#define HDR_IS_LAYER_1(h) ((h[1] & 6) == 6) + +#define BITS_DEQUANTIZER_OUT -1 +#define MAX_SCF (255 + BITS_DEQUANTIZER_OUT*4 - 210) +#define MAX_SCFI ((MAX_SCF + 3) & ~3) + +#define MINIMP3_MIN(a, b) ((a) > (b) ? (b) : (a)) +#define MINIMP3_MAX(a, b) ((a) < (b) ? (b) : (a)) + +#if !defined(MINIMP3_NO_SIMD) + +#if !defined(MINIMP3_ONLY_SIMD) && (defined(_M_X64) || defined(__x86_64__) || defined(__aarch64__) || defined(_M_ARM64)) +/* x64 always have SSE2, arm64 always have neon, no need for generic code */ +#define MINIMP3_ONLY_SIMD +#endif /* SIMD checks... */ + +#if (defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64))) || ((defined(__i386__) || defined(__x86_64__)) && defined(__SSE2__)) +#if defined(_MSC_VER) +#include <intrin.h> +#endif /* defined(_MSC_VER) */ +#include <immintrin.h> +#define HAVE_SSE 1 +#define HAVE_SIMD 1 +#define VSTORE _mm_storeu_ps +#define VLD _mm_loadu_ps +#define VSET _mm_set1_ps +#define VADD _mm_add_ps +#define VSUB _mm_sub_ps +#define VMUL _mm_mul_ps +#define VMAC(a, x, y) _mm_add_ps(a, _mm_mul_ps(x, y)) +#define VMSB(a, x, y) _mm_sub_ps(a, _mm_mul_ps(x, y)) +#define VMUL_S(x, s) _mm_mul_ps(x, _mm_set1_ps(s)) +#define VREV(x) _mm_shuffle_ps(x, x, _MM_SHUFFLE(0, 1, 2, 3)) +typedef __m128 f4; +#if defined(_MSC_VER) || defined(MINIMP3_ONLY_SIMD) +#define minimp3_cpuid __cpuid +#else /* defined(_MSC_VER) || defined(MINIMP3_ONLY_SIMD) */ +static __inline__ __attribute__((always_inline)) void minimp3_cpuid(int CPUInfo[], const int InfoType) +{ +#if defined(__PIC__) + __asm__ __volatile__( +#if defined(__x86_64__) + "push %%rbx\n" + "cpuid\n" + "xchgl %%ebx, %1\n" + "pop %%rbx\n" +#else /* defined(__x86_64__) */ + "xchgl %%ebx, %1\n" + "cpuid\n" + "xchgl %%ebx, %1\n" +#endif /* defined(__x86_64__) */ + : "=a" (CPUInfo[0]), "=r" (CPUInfo[1]), "=c" (CPUInfo[2]), "=d" (CPUInfo[3]) + : "a" (InfoType)); +#else /* defined(__PIC__) */ + __asm__ __volatile__( + "cpuid" + : "=a" (CPUInfo[0]), "=b" (CPUInfo[1]), "=c" (CPUInfo[2]), "=d" (CPUInfo[3]) + : "a" (InfoType)); +#endif /* defined(__PIC__)*/ +} +#endif /* defined(_MSC_VER) || defined(MINIMP3_ONLY_SIMD) */ +static int have_simd(void) +{ +#ifdef MINIMP3_ONLY_SIMD + return 1; +#else /* MINIMP3_ONLY_SIMD */ + static int g_have_simd; + int CPUInfo[4]; +#ifdef MINIMP3_TEST + static int g_counter; + if (g_counter++ > 100) + return 0; +#endif /* MINIMP3_TEST */ + if (g_have_simd) + goto end; + minimp3_cpuid(CPUInfo, 0); + g_have_simd = 1; + if (CPUInfo[0] > 0) + { + minimp3_cpuid(CPUInfo, 1); + g_have_simd = (CPUInfo[3] & (1 << 26)) + 1; /* SSE2 */ + } +end: + return g_have_simd - 1; +#endif /* MINIMP3_ONLY_SIMD */ +} +#elif defined(__ARM_NEON) || defined(__aarch64__) || defined(_M_ARM64) +#include <arm_neon.h> +#define HAVE_SSE 0 +#define HAVE_SIMD 1 +#define VSTORE vst1q_f32 +#define VLD vld1q_f32 +#define VSET vmovq_n_f32 +#define VADD vaddq_f32 +#define VSUB vsubq_f32 +#define VMUL vmulq_f32 +#define VMAC(a, x, y) vmlaq_f32(a, x, y) +#define VMSB(a, x, y) vmlsq_f32(a, x, y) +#define VMUL_S(x, s) vmulq_f32(x, vmovq_n_f32(s)) +#define VREV(x) vcombine_f32(vget_high_f32(vrev64q_f32(x)), vget_low_f32(vrev64q_f32(x))) +typedef float32x4_t f4; +static int have_simd() +{ /* TODO: detect neon for !MINIMP3_ONLY_SIMD */ + return 1; +} +#else /* SIMD checks... */ +#define HAVE_SSE 0 +#define HAVE_SIMD 0 +#ifdef MINIMP3_ONLY_SIMD +#error MINIMP3_ONLY_SIMD used, but SSE/NEON not enabled +#endif /* MINIMP3_ONLY_SIMD */ +#endif /* SIMD checks... */ +#else /* !defined(MINIMP3_NO_SIMD) */ +#define HAVE_SIMD 0 +#endif /* !defined(MINIMP3_NO_SIMD) */ + +#if defined(__ARM_ARCH) && (__ARM_ARCH >= 6) && !defined(__aarch64__) && !defined(_M_ARM64) +#define HAVE_ARMV6 1 +static __inline__ __attribute__((always_inline)) int32_t minimp3_clip_int16_arm(int32_t a) +{ + int32_t x = 0; + __asm__ ("ssat %0, #16, %1" : "=r"(x) : "r"(a)); + return x; +} +#else +#define HAVE_ARMV6 0 +#endif + +typedef struct +{ + const uint8_t *buf; + int pos, limit; +} bs_t; + +typedef struct +{ + float scf[3*64]; + uint8_t total_bands, stereo_bands, bitalloc[64], scfcod[64]; +} L12_scale_info; + +typedef struct +{ + uint8_t tab_offset, code_tab_width, band_count; +} L12_subband_alloc_t; + +typedef struct +{ + const uint8_t *sfbtab; + uint16_t part_23_length, big_values, scalefac_compress; + uint8_t global_gain, block_type, mixed_block_flag, n_long_sfb, n_short_sfb; + uint8_t table_select[3], region_count[3], subblock_gain[3]; + uint8_t preflag, scalefac_scale, count1_table, scfsi; +} L3_gr_info_t; + +typedef struct +{ + bs_t bs; + uint8_t maindata[MAX_BITRESERVOIR_BYTES + MAX_L3_FRAME_PAYLOAD_BYTES]; + L3_gr_info_t gr_info[4]; + float grbuf[2][576], scf[40], syn[18 + 15][2*32]; + uint8_t ist_pos[2][39]; +} mp3dec_scratch_t; + +static void bs_init(bs_t *bs, const uint8_t *data, int bytes) +{ + bs->buf = data; + bs->pos = 0; + bs->limit = bytes*8; +} + +static uint32_t get_bits(bs_t *bs, int n) +{ + uint32_t next, cache = 0, s = bs->pos & 7; + int shl = n + s; + const uint8_t *p = bs->buf + (bs->pos >> 3); + if ((bs->pos += n) > bs->limit) + return 0; + next = *p++ & (255 >> s); + while ((shl -= 8) > 0) + { + cache |= next << shl; + next = *p++; + } + return cache | (next >> -shl); +} + +static int hdr_valid(const uint8_t *h) +{ + return h[0] == 0xff && + ((h[1] & 0xF0) == 0xf0 || (h[1] & 0xFE) == 0xe2) && + (HDR_GET_LAYER(h) != 0) && + (HDR_GET_BITRATE(h) != 15) && + (HDR_GET_SAMPLE_RATE(h) != 3); +} + +static int hdr_compare(const uint8_t *h1, const uint8_t *h2) +{ + return hdr_valid(h2) && + ((h1[1] ^ h2[1]) & 0xFE) == 0 && + ((h1[2] ^ h2[2]) & 0x0C) == 0 && + !(HDR_IS_FREE_FORMAT(h1) ^ HDR_IS_FREE_FORMAT(h2)); +} + +static unsigned hdr_bitrate_kbps(const uint8_t *h) +{ + static const uint8_t halfrate[2][3][15] = { + { { 0,4,8,12,16,20,24,28,32,40,48,56,64,72,80 }, { 0,4,8,12,16,20,24,28,32,40,48,56,64,72,80 }, { 0,16,24,28,32,40,48,56,64,72,80,88,96,112,128 } }, + { { 0,16,20,24,28,32,40,48,56,64,80,96,112,128,160 }, { 0,16,24,28,32,40,48,56,64,80,96,112,128,160,192 }, { 0,16,32,48,64,80,96,112,128,144,160,176,192,208,224 } }, + }; + return 2*halfrate[!!HDR_TEST_MPEG1(h)][HDR_GET_LAYER(h) - 1][HDR_GET_BITRATE(h)]; +} + +static unsigned hdr_sample_rate_hz(const uint8_t *h) +{ + static const unsigned g_hz[3] = { 44100, 48000, 32000 }; + return g_hz[HDR_GET_SAMPLE_RATE(h)] >> (int)!HDR_TEST_MPEG1(h) >> (int)!HDR_TEST_NOT_MPEG25(h); +} + +static unsigned hdr_frame_samples(const uint8_t *h) +{ + return HDR_IS_LAYER_1(h) ? 384 : (1152 >> (int)HDR_IS_FRAME_576(h)); +} + +static int hdr_frame_bytes(const uint8_t *h, int free_format_size) +{ + int frame_bytes = hdr_frame_samples(h)*hdr_bitrate_kbps(h)*125/hdr_sample_rate_hz(h); + if (HDR_IS_LAYER_1(h)) + { + frame_bytes &= ~3; /* slot align */ + } + return frame_bytes ? frame_bytes : free_format_size; +} + +static int hdr_padding(const uint8_t *h) +{ + return HDR_TEST_PADDING(h) ? (HDR_IS_LAYER_1(h) ? 4 : 1) : 0; +} + +#ifndef MINIMP3_ONLY_MP3 +static const L12_subband_alloc_t *L12_subband_alloc_table(const uint8_t *hdr, L12_scale_info *sci) +{ + const L12_subband_alloc_t *alloc; + int mode = HDR_GET_STEREO_MODE(hdr); + int nbands, stereo_bands = (mode == MODE_MONO) ? 0 : (mode == MODE_JOINT_STEREO) ? (HDR_GET_STEREO_MODE_EXT(hdr) << 2) + 4 : 32; + + if (HDR_IS_LAYER_1(hdr)) + { + static const L12_subband_alloc_t g_alloc_L1[] = { { 76, 4, 32 } }; + alloc = g_alloc_L1; + nbands = 32; + } else if (!HDR_TEST_MPEG1(hdr)) + { + static const L12_subband_alloc_t g_alloc_L2M2[] = { { 60, 4, 4 }, { 44, 3, 7 }, { 44, 2, 19 } }; + alloc = g_alloc_L2M2; + nbands = 30; + } else + { + static const L12_subband_alloc_t g_alloc_L2M1[] = { { 0, 4, 3 }, { 16, 4, 8 }, { 32, 3, 12 }, { 40, 2, 7 } }; + int sample_rate_idx = HDR_GET_SAMPLE_RATE(hdr); + unsigned kbps = hdr_bitrate_kbps(hdr) >> (int)(mode != MODE_MONO); + if (!kbps) /* free-format */ + { + kbps = 192; + } + + alloc = g_alloc_L2M1; + nbands = 27; + if (kbps < 56) + { + static const L12_subband_alloc_t g_alloc_L2M1_lowrate[] = { { 44, 4, 2 }, { 44, 3, 10 } }; + alloc = g_alloc_L2M1_lowrate; + nbands = sample_rate_idx == 2 ? 12 : 8; + } else if (kbps >= 96 && sample_rate_idx != 1) + { + nbands = 30; + } + } + + sci->total_bands = (uint8_t)nbands; + sci->stereo_bands = (uint8_t)MINIMP3_MIN(stereo_bands, nbands); + + return alloc; +} + +static void L12_read_scalefactors(bs_t *bs, uint8_t *pba, uint8_t *scfcod, int bands, float *scf) +{ + static const float g_deq_L12[18*3] = { +#define DQ(x) 9.53674316e-07f/x, 7.56931807e-07f/x, 6.00777173e-07f/x + DQ(3),DQ(7),DQ(15),DQ(31),DQ(63),DQ(127),DQ(255),DQ(511),DQ(1023),DQ(2047),DQ(4095),DQ(8191),DQ(16383),DQ(32767),DQ(65535),DQ(3),DQ(5),DQ(9) + }; + int i, m; + for (i = 0; i < bands; i++) + { + float s = 0; + int ba = *pba++; + int mask = ba ? 4 + ((19 >> scfcod[i]) & 3) : 0; + for (m = 4; m; m >>= 1) + { + if (mask & m) + { + int b = get_bits(bs, 6); + s = g_deq_L12[ba*3 - 6 + b % 3]*(1 << 21 >> b/3); + } + *scf++ = s; + } + } +} + +static void L12_read_scale_info(const uint8_t *hdr, bs_t *bs, L12_scale_info *sci) +{ + static const uint8_t g_bitalloc_code_tab[] = { + 0,17, 3, 4, 5,6,7, 8,9,10,11,12,13,14,15,16, + 0,17,18, 3,19,4,5, 6,7, 8, 9,10,11,12,13,16, + 0,17,18, 3,19,4,5,16, + 0,17,18,16, + 0,17,18,19, 4,5,6, 7,8, 9,10,11,12,13,14,15, + 0,17,18, 3,19,4,5, 6,7, 8, 9,10,11,12,13,14, + 0, 2, 3, 4, 5,6,7, 8,9,10,11,12,13,14,15,16 + }; + const L12_subband_alloc_t *subband_alloc = L12_subband_alloc_table(hdr, sci); + + int i, k = 0, ba_bits = 0; + const uint8_t *ba_code_tab = g_bitalloc_code_tab; + + for (i = 0; i < sci->total_bands; i++) + { + uint8_t ba; + if (i == k) + { + k += subband_alloc->band_count; + ba_bits = subband_alloc->code_tab_width; + ba_code_tab = g_bitalloc_code_tab + subband_alloc->tab_offset; + subband_alloc++; + } + ba = ba_code_tab[get_bits(bs, ba_bits)]; + sci->bitalloc[2*i] = ba; + if (i < sci->stereo_bands) + { + ba = ba_code_tab[get_bits(bs, ba_bits)]; + } + sci->bitalloc[2*i + 1] = sci->stereo_bands ? ba : 0; + } + + for (i = 0; i < 2*sci->total_bands; i++) + { + sci->scfcod[i] = sci->bitalloc[i] ? HDR_IS_LAYER_1(hdr) ? 2 : get_bits(bs, 2) : 6; + } + + L12_read_scalefactors(bs, sci->bitalloc, sci->scfcod, sci->total_bands*2, sci->scf); + + for (i = sci->stereo_bands; i < sci->total_bands; i++) + { + sci->bitalloc[2*i + 1] = 0; + } +} + +static int L12_dequantize_granule(float *grbuf, bs_t *bs, L12_scale_info *sci, int group_size) +{ + int i, j, k, choff = 576; + for (j = 0; j < 4; j++) + { + float *dst = grbuf + group_size*j; + for (i = 0; i < 2*sci->total_bands; i++) + { + int ba = sci->bitalloc[i]; + if (ba != 0) + { + if (ba < 17) + { + int half = (1 << (ba - 1)) - 1; + for (k = 0; k < group_size; k++) + { + dst[k] = (float)((int)get_bits(bs, ba) - half); + } + } else + { + unsigned mod = (2 << (ba - 17)) + 1; /* 3, 5, 9 */ + unsigned code = get_bits(bs, mod + 2 - (mod >> 3)); /* 5, 7, 10 */ + for (k = 0; k < group_size; k++, code /= mod) + { + dst[k] = (float)((int)(code % mod - mod/2)); + } + } + } + dst += choff; + choff = 18 - choff; + } + } + return group_size*4; +} + +static void L12_apply_scf_384(L12_scale_info *sci, const float *scf, float *dst) +{ + int i, k; + memcpy(dst + 576 + sci->stereo_bands*18, dst + sci->stereo_bands*18, (sci->total_bands - sci->stereo_bands)*18*sizeof(float)); + for (i = 0; i < sci->total_bands; i++, dst += 18, scf += 6) + { + for (k = 0; k < 12; k++) + { + dst[k + 0] *= scf[0]; + dst[k + 576] *= scf[3]; + } + } +} +#endif /* MINIMP3_ONLY_MP3 */ + +static int L3_read_side_info(bs_t *bs, L3_gr_info_t *gr, const uint8_t *hdr) +{ + static const uint8_t g_scf_long[8][23] = { + { 6,6,6,6,6,6,8,10,12,14,16,20,24,28,32,38,46,52,60,68,58,54,0 }, + { 12,12,12,12,12,12,16,20,24,28,32,40,48,56,64,76,90,2,2,2,2,2,0 }, + { 6,6,6,6,6,6,8,10,12,14,16,20,24,28,32,38,46,52,60,68,58,54,0 }, + { 6,6,6,6,6,6,8,10,12,14,16,18,22,26,32,38,46,54,62,70,76,36,0 }, + { 6,6,6,6,6,6,8,10,12,14,16,20,24,28,32,38,46,52,60,68,58,54,0 }, + { 4,4,4,4,4,4,6,6,8,8,10,12,16,20,24,28,34,42,50,54,76,158,0 }, + { 4,4,4,4,4,4,6,6,6,8,10,12,16,18,22,28,34,40,46,54,54,192,0 }, + { 4,4,4,4,4,4,6,6,8,10,12,16,20,24,30,38,46,56,68,84,102,26,0 } + }; + static const uint8_t g_scf_short[8][40] = { + { 4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,30,30,30,40,40,40,18,18,18,0 }, + { 8,8,8,8,8,8,8,8,8,12,12,12,16,16,16,20,20,20,24,24,24,28,28,28,36,36,36,2,2,2,2,2,2,2,2,2,26,26,26,0 }, + { 4,4,4,4,4,4,4,4,4,6,6,6,6,6,6,8,8,8,10,10,10,14,14,14,18,18,18,26,26,26,32,32,32,42,42,42,18,18,18,0 }, + { 4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,32,32,32,44,44,44,12,12,12,0 }, + { 4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,30,30,30,40,40,40,18,18,18,0 }, + { 4,4,4,4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,22,22,22,30,30,30,56,56,56,0 }, + { 4,4,4,4,4,4,4,4,4,4,4,4,6,6,6,6,6,6,10,10,10,12,12,12,14,14,14,16,16,16,20,20,20,26,26,26,66,66,66,0 }, + { 4,4,4,4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,12,12,12,16,16,16,20,20,20,26,26,26,34,34,34,42,42,42,12,12,12,0 } + }; + static const uint8_t g_scf_mixed[8][40] = { + { 6,6,6,6,6,6,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,30,30,30,40,40,40,18,18,18,0 }, + { 12,12,12,4,4,4,8,8,8,12,12,12,16,16,16,20,20,20,24,24,24,28,28,28,36,36,36,2,2,2,2,2,2,2,2,2,26,26,26,0 }, + { 6,6,6,6,6,6,6,6,6,6,6,6,8,8,8,10,10,10,14,14,14,18,18,18,26,26,26,32,32,32,42,42,42,18,18,18,0 }, + { 6,6,6,6,6,6,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,32,32,32,44,44,44,12,12,12,0 }, + { 6,6,6,6,6,6,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,30,30,30,40,40,40,18,18,18,0 }, + { 4,4,4,4,4,4,6,6,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,22,22,22,30,30,30,56,56,56,0 }, + { 4,4,4,4,4,4,6,6,4,4,4,6,6,6,6,6,6,10,10,10,12,12,12,14,14,14,16,16,16,20,20,20,26,26,26,66,66,66,0 }, + { 4,4,4,4,4,4,6,6,4,4,4,6,6,6,8,8,8,12,12,12,16,16,16,20,20,20,26,26,26,34,34,34,42,42,42,12,12,12,0 } + }; + + unsigned tables, scfsi = 0; + int main_data_begin, part_23_sum = 0; + int sr_idx = HDR_GET_MY_SAMPLE_RATE(hdr); sr_idx -= (sr_idx != 0); + int gr_count = HDR_IS_MONO(hdr) ? 1 : 2; + + if (HDR_TEST_MPEG1(hdr)) + { + gr_count *= 2; + main_data_begin = get_bits(bs, 9); + scfsi = get_bits(bs, 7 + gr_count); + } else + { + main_data_begin = get_bits(bs, 8 + gr_count) >> gr_count; + } + + do + { + if (HDR_IS_MONO(hdr)) + { + scfsi <<= 4; + } + gr->part_23_length = (uint16_t)get_bits(bs, 12); + part_23_sum += gr->part_23_length; + gr->big_values = (uint16_t)get_bits(bs, 9); + if (gr->big_values > 288) + { + return -1; + } + gr->global_gain = (uint8_t)get_bits(bs, 8); + gr->scalefac_compress = (uint16_t)get_bits(bs, HDR_TEST_MPEG1(hdr) ? 4 : 9); + gr->sfbtab = g_scf_long[sr_idx]; + gr->n_long_sfb = 22; + gr->n_short_sfb = 0; + if (get_bits(bs, 1)) + { + gr->block_type = (uint8_t)get_bits(bs, 2); + if (!gr->block_type) + { + return -1; + } + gr->mixed_block_flag = (uint8_t)get_bits(bs, 1); + gr->region_count[0] = 7; + gr->region_count[1] = 255; + if (gr->block_type == SHORT_BLOCK_TYPE) + { + scfsi &= 0x0F0F; + if (!gr->mixed_block_flag) + { + gr->region_count[0] = 8; + gr->sfbtab = g_scf_short[sr_idx]; + gr->n_long_sfb = 0; + gr->n_short_sfb = 39; + } else + { + gr->sfbtab = g_scf_mixed[sr_idx]; + gr->n_long_sfb = HDR_TEST_MPEG1(hdr) ? 8 : 6; + gr->n_short_sfb = 30; + } + } + tables = get_bits(bs, 10); + tables <<= 5; + gr->subblock_gain[0] = (uint8_t)get_bits(bs, 3); + gr->subblock_gain[1] = (uint8_t)get_bits(bs, 3); + gr->subblock_gain[2] = (uint8_t)get_bits(bs, 3); + } else + { + gr->block_type = 0; + gr->mixed_block_flag = 0; + tables = get_bits(bs, 15); + gr->region_count[0] = (uint8_t)get_bits(bs, 4); + gr->region_count[1] = (uint8_t)get_bits(bs, 3); + gr->region_count[2] = 255; + } + gr->table_select[0] = (uint8_t)(tables >> 10); + gr->table_select[1] = (uint8_t)((tables >> 5) & 31); + gr->table_select[2] = (uint8_t)((tables) & 31); + gr->preflag = HDR_TEST_MPEG1(hdr) ? get_bits(bs, 1) : (gr->scalefac_compress >= 500); + gr->scalefac_scale = (uint8_t)get_bits(bs, 1); + gr->count1_table = (uint8_t)get_bits(bs, 1); + gr->scfsi = (uint8_t)((scfsi >> 12) & 15); + scfsi <<= 4; + gr++; + } while(--gr_count); + + if (part_23_sum + bs->pos > bs->limit + main_data_begin*8) + { + return -1; + } + + return main_data_begin; +} + +static void L3_read_scalefactors(uint8_t *scf, uint8_t *ist_pos, const uint8_t *scf_size, const uint8_t *scf_count, bs_t *bitbuf, int scfsi) +{ + int i, k; + for (i = 0; i < 4 && scf_count[i]; i++, scfsi *= 2) + { + int cnt = scf_count[i]; + if (scfsi & 8) + { + memcpy(scf, ist_pos, cnt); + } else + { + int bits = scf_size[i]; + if (!bits) + { + memset(scf, 0, cnt); + memset(ist_pos, 0, cnt); + } else + { + int max_scf = (scfsi < 0) ? (1 << bits) - 1 : -1; + for (k = 0; k < cnt; k++) + { + int s = get_bits(bitbuf, bits); + ist_pos[k] = (s == max_scf ? -1 : s); + scf[k] = s; + } + } + } + ist_pos += cnt; + scf += cnt; + } + scf[0] = scf[1] = scf[2] = 0; +} + +static float L3_ldexp_q2(float y, int exp_q2) +{ + static const float g_expfrac[4] = { 9.31322575e-10f,7.83145814e-10f,6.58544508e-10f,5.53767716e-10f }; + int e; + do + { + e = MINIMP3_MIN(30*4, exp_q2); + y *= g_expfrac[e & 3]*(1 << 30 >> (e >> 2)); + } while ((exp_q2 -= e) > 0); + return y; +} + +static void L3_decode_scalefactors(const uint8_t *hdr, uint8_t *ist_pos, bs_t *bs, const L3_gr_info_t *gr, float *scf, int ch) +{ + static const uint8_t g_scf_partitions[3][28] = { + { 6,5,5, 5,6,5,5,5,6,5, 7,3,11,10,0,0, 7, 7, 7,0, 6, 6,6,3, 8, 8,5,0 }, + { 8,9,6,12,6,9,9,9,6,9,12,6,15,18,0,0, 6,15,12,0, 6,12,9,6, 6,18,9,0 }, + { 9,9,6,12,9,9,9,9,9,9,12,6,18,18,0,0,12,12,12,0,12, 9,9,6,15,12,9,0 } + }; + const uint8_t *scf_partition = g_scf_partitions[!!gr->n_short_sfb + !gr->n_long_sfb]; + uint8_t scf_size[4], iscf[40]; + int i, scf_shift = gr->scalefac_scale + 1, gain_exp, scfsi = gr->scfsi; + float gain; + + if (HDR_TEST_MPEG1(hdr)) + { + static const uint8_t g_scfc_decode[16] = { 0,1,2,3, 12,5,6,7, 9,10,11,13, 14,15,18,19 }; + int part = g_scfc_decode[gr->scalefac_compress]; + scf_size[1] = scf_size[0] = (uint8_t)(part >> 2); + scf_size[3] = scf_size[2] = (uint8_t)(part & 3); + } else + { + static const uint8_t g_mod[6*4] = { 5,5,4,4,5,5,4,1,4,3,1,1,5,6,6,1,4,4,4,1,4,3,1,1 }; + int k, modprod, sfc, ist = HDR_TEST_I_STEREO(hdr) && ch; + sfc = gr->scalefac_compress >> ist; + for (k = ist*3*4; sfc >= 0; sfc -= modprod, k += 4) + { + for (modprod = 1, i = 3; i >= 0; i--) + { + scf_size[i] = (uint8_t)(sfc / modprod % g_mod[k + i]); + modprod *= g_mod[k + i]; + } + } + scf_partition += k; + scfsi = -16; + } + L3_read_scalefactors(iscf, ist_pos, scf_size, scf_partition, bs, scfsi); + + if (gr->n_short_sfb) + { + int sh = 3 - scf_shift; + for (i = 0; i < gr->n_short_sfb; i += 3) + { + iscf[gr->n_long_sfb + i + 0] += gr->subblock_gain[0] << sh; + iscf[gr->n_long_sfb + i + 1] += gr->subblock_gain[1] << sh; + iscf[gr->n_long_sfb + i + 2] += gr->subblock_gain[2] << sh; + } + } else if (gr->preflag) + { + static const uint8_t g_preamp[10] = { 1,1,1,1,2,2,3,3,3,2 }; + for (i = 0; i < 10; i++) + { + iscf[11 + i] += g_preamp[i]; + } + } + + gain_exp = gr->global_gain + BITS_DEQUANTIZER_OUT*4 - 210 - (HDR_IS_MS_STEREO(hdr) ? 2 : 0); + gain = L3_ldexp_q2(1 << (MAX_SCFI/4), MAX_SCFI - gain_exp); + for (i = 0; i < (int)(gr->n_long_sfb + gr->n_short_sfb); i++) + { + scf[i] = L3_ldexp_q2(gain, iscf[i] << scf_shift); + } +} + +static const float g_pow43[129 + 16] = { + 0,-1,-2.519842f,-4.326749f,-6.349604f,-8.549880f,-10.902724f,-13.390518f,-16.000000f,-18.720754f,-21.544347f,-24.463781f,-27.473142f,-30.567351f,-33.741992f,-36.993181f, + 0,1,2.519842f,4.326749f,6.349604f,8.549880f,10.902724f,13.390518f,16.000000f,18.720754f,21.544347f,24.463781f,27.473142f,30.567351f,33.741992f,36.993181f,40.317474f,43.711787f,47.173345f,50.699631f,54.288352f,57.937408f,61.644865f,65.408941f,69.227979f,73.100443f,77.024898f,81.000000f,85.024491f,89.097188f,93.216975f,97.382800f,101.593667f,105.848633f,110.146801f,114.487321f,118.869381f,123.292209f,127.755065f,132.257246f,136.798076f,141.376907f,145.993119f,150.646117f,155.335327f,160.060199f,164.820202f,169.614826f,174.443577f,179.305980f,184.201575f,189.129918f,194.090580f,199.083145f,204.107210f,209.162385f,214.248292f,219.364564f,224.510845f,229.686789f,234.892058f,240.126328f,245.389280f,250.680604f,256.000000f,261.347174f,266.721841f,272.123723f,277.552547f,283.008049f,288.489971f,293.998060f,299.532071f,305.091761f,310.676898f,316.287249f,321.922592f,327.582707f,333.267377f,338.976394f,344.709550f,350.466646f,356.247482f,362.051866f,367.879608f,373.730522f,379.604427f,385.501143f,391.420496f,397.362314f,403.326427f,409.312672f,415.320884f,421.350905f,427.402579f,433.475750f,439.570269f,445.685987f,451.822757f,457.980436f,464.158883f,470.357960f,476.577530f,482.817459f,489.077615f,495.357868f,501.658090f,507.978156f,514.317941f,520.677324f,527.056184f,533.454404f,539.871867f,546.308458f,552.764065f,559.238575f,565.731879f,572.243870f,578.774440f,585.323483f,591.890898f,598.476581f,605.080431f,611.702349f,618.342238f,625.000000f,631.675540f,638.368763f,645.079578f +}; + +static float L3_pow_43(int x) +{ + float frac; + int sign, mult = 256; + + if (x < 129) + { + return g_pow43[16 + x]; + } + + if (x < 1024) + { + mult = 16; + x <<= 3; + } + + sign = 2*x & 64; + frac = (float)((x & 63) - sign) / ((x & ~63) + sign); + return g_pow43[16 + ((x + sign) >> 6)]*(1.f + frac*((4.f/3) + frac*(2.f/9)))*mult; +} + +static void L3_huffman(float *dst, bs_t *bs, const L3_gr_info_t *gr_info, const float *scf, int layer3gr_limit) +{ + static const int16_t tabs[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 785,785,785,785,784,784,784,784,513,513,513,513,513,513,513,513,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256, + -255,1313,1298,1282,785,785,785,785,784,784,784,784,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,290,288, + -255,1313,1298,1282,769,769,769,769,529,529,529,529,529,529,529,529,528,528,528,528,528,528,528,528,512,512,512,512,512,512,512,512,290,288, + -253,-318,-351,-367,785,785,785,785,784,784,784,784,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,819,818,547,547,275,275,275,275,561,560,515,546,289,274,288,258, + -254,-287,1329,1299,1314,1312,1057,1057,1042,1042,1026,1026,784,784,784,784,529,529,529,529,529,529,529,529,769,769,769,769,768,768,768,768,563,560,306,306,291,259, + -252,-413,-477,-542,1298,-575,1041,1041,784,784,784,784,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,-383,-399,1107,1092,1106,1061,849,849,789,789,1104,1091,773,773,1076,1075,341,340,325,309,834,804,577,577,532,532,516,516,832,818,803,816,561,561,531,531,515,546,289,289,288,258, + -252,-429,-493,-559,1057,1057,1042,1042,529,529,529,529,529,529,529,529,784,784,784,784,769,769,769,769,512,512,512,512,512,512,512,512,-382,1077,-415,1106,1061,1104,849,849,789,789,1091,1076,1029,1075,834,834,597,581,340,340,339,324,804,833,532,532,832,772,818,803,817,787,816,771,290,290,290,290,288,258, + -253,-349,-414,-447,-463,1329,1299,-479,1314,1312,1057,1057,1042,1042,1026,1026,785,785,785,785,784,784,784,784,769,769,769,769,768,768,768,768,-319,851,821,-335,836,850,805,849,341,340,325,336,533,533,579,579,564,564,773,832,578,548,563,516,321,276,306,291,304,259, + -251,-572,-733,-830,-863,-879,1041,1041,784,784,784,784,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,-511,-527,-543,1396,1351,1381,1366,1395,1335,1380,-559,1334,1138,1138,1063,1063,1350,1392,1031,1031,1062,1062,1364,1363,1120,1120,1333,1348,881,881,881,881,375,374,359,373,343,358,341,325,791,791,1123,1122,-703,1105,1045,-719,865,865,790,790,774,774,1104,1029,338,293,323,308,-799,-815,833,788,772,818,803,816,322,292,307,320,561,531,515,546,289,274,288,258, + -251,-525,-605,-685,-765,-831,-846,1298,1057,1057,1312,1282,785,785,785,785,784,784,784,784,769,769,769,769,512,512,512,512,512,512,512,512,1399,1398,1383,1367,1382,1396,1351,-511,1381,1366,1139,1139,1079,1079,1124,1124,1364,1349,1363,1333,882,882,882,882,807,807,807,807,1094,1094,1136,1136,373,341,535,535,881,775,867,822,774,-591,324,338,-671,849,550,550,866,864,609,609,293,336,534,534,789,835,773,-751,834,804,308,307,833,788,832,772,562,562,547,547,305,275,560,515,290,290, + -252,-397,-477,-557,-622,-653,-719,-735,-750,1329,1299,1314,1057,1057,1042,1042,1312,1282,1024,1024,785,785,785,785,784,784,784,784,769,769,769,769,-383,1127,1141,1111,1126,1140,1095,1110,869,869,883,883,1079,1109,882,882,375,374,807,868,838,881,791,-463,867,822,368,263,852,837,836,-543,610,610,550,550,352,336,534,534,865,774,851,821,850,805,593,533,579,564,773,832,578,578,548,548,577,577,307,276,306,291,516,560,259,259, + -250,-2107,-2507,-2764,-2909,-2974,-3007,-3023,1041,1041,1040,1040,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,-767,-1052,-1213,-1277,-1358,-1405,-1469,-1535,-1550,-1582,-1614,-1647,-1662,-1694,-1726,-1759,-1774,-1807,-1822,-1854,-1886,1565,-1919,-1935,-1951,-1967,1731,1730,1580,1717,-1983,1729,1564,-1999,1548,-2015,-2031,1715,1595,-2047,1714,-2063,1610,-2079,1609,-2095,1323,1323,1457,1457,1307,1307,1712,1547,1641,1700,1699,1594,1685,1625,1442,1442,1322,1322,-780,-973,-910,1279,1278,1277,1262,1276,1261,1275,1215,1260,1229,-959,974,974,989,989,-943,735,478,478,495,463,506,414,-1039,1003,958,1017,927,942,987,957,431,476,1272,1167,1228,-1183,1256,-1199,895,895,941,941,1242,1227,1212,1135,1014,1014,490,489,503,487,910,1013,985,925,863,894,970,955,1012,847,-1343,831,755,755,984,909,428,366,754,559,-1391,752,486,457,924,997,698,698,983,893,740,740,908,877,739,739,667,667,953,938,497,287,271,271,683,606,590,712,726,574,302,302,738,736,481,286,526,725,605,711,636,724,696,651,589,681,666,710,364,467,573,695,466,466,301,465,379,379,709,604,665,679,316,316,634,633,436,436,464,269,424,394,452,332,438,363,347,408,393,448,331,422,362,407,392,421,346,406,391,376,375,359,1441,1306,-2367,1290,-2383,1337,-2399,-2415,1426,1321,-2431,1411,1336,-2447,-2463,-2479,1169,1169,1049,1049,1424,1289,1412,1352,1319,-2495,1154,1154,1064,1064,1153,1153,416,390,360,404,403,389,344,374,373,343,358,372,327,357,342,311,356,326,1395,1394,1137,1137,1047,1047,1365,1392,1287,1379,1334,1364,1349,1378,1318,1363,792,792,792,792,1152,1152,1032,1032,1121,1121,1046,1046,1120,1120,1030,1030,-2895,1106,1061,1104,849,849,789,789,1091,1076,1029,1090,1060,1075,833,833,309,324,532,532,832,772,818,803,561,561,531,560,515,546,289,274,288,258, + -250,-1179,-1579,-1836,-1996,-2124,-2253,-2333,-2413,-2477,-2542,-2574,-2607,-2622,-2655,1314,1313,1298,1312,1282,785,785,785,785,1040,1040,1025,1025,768,768,768,768,-766,-798,-830,-862,-895,-911,-927,-943,-959,-975,-991,-1007,-1023,-1039,-1055,-1070,1724,1647,-1103,-1119,1631,1767,1662,1738,1708,1723,-1135,1780,1615,1779,1599,1677,1646,1778,1583,-1151,1777,1567,1737,1692,1765,1722,1707,1630,1751,1661,1764,1614,1736,1676,1763,1750,1645,1598,1721,1691,1762,1706,1582,1761,1566,-1167,1749,1629,767,766,751,765,494,494,735,764,719,749,734,763,447,447,748,718,477,506,431,491,446,476,461,505,415,430,475,445,504,399,460,489,414,503,383,474,429,459,502,502,746,752,488,398,501,473,413,472,486,271,480,270,-1439,-1455,1357,-1471,-1487,-1503,1341,1325,-1519,1489,1463,1403,1309,-1535,1372,1448,1418,1476,1356,1462,1387,-1551,1475,1340,1447,1402,1386,-1567,1068,1068,1474,1461,455,380,468,440,395,425,410,454,364,467,466,464,453,269,409,448,268,432,1371,1473,1432,1417,1308,1460,1355,1446,1459,1431,1083,1083,1401,1416,1458,1445,1067,1067,1370,1457,1051,1051,1291,1430,1385,1444,1354,1415,1400,1443,1082,1082,1173,1113,1186,1066,1185,1050,-1967,1158,1128,1172,1097,1171,1081,-1983,1157,1112,416,266,375,400,1170,1142,1127,1065,793,793,1169,1033,1156,1096,1141,1111,1155,1080,1126,1140,898,898,808,808,897,897,792,792,1095,1152,1032,1125,1110,1139,1079,1124,882,807,838,881,853,791,-2319,867,368,263,822,852,837,866,806,865,-2399,851,352,262,534,534,821,836,594,594,549,549,593,593,533,533,848,773,579,579,564,578,548,563,276,276,577,576,306,291,516,560,305,305,275,259, + -251,-892,-2058,-2620,-2828,-2957,-3023,-3039,1041,1041,1040,1040,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,-511,-527,-543,-559,1530,-575,-591,1528,1527,1407,1526,1391,1023,1023,1023,1023,1525,1375,1268,1268,1103,1103,1087,1087,1039,1039,1523,-604,815,815,815,815,510,495,509,479,508,463,507,447,431,505,415,399,-734,-782,1262,-815,1259,1244,-831,1258,1228,-847,-863,1196,-879,1253,987,987,748,-767,493,493,462,477,414,414,686,669,478,446,461,445,474,429,487,458,412,471,1266,1264,1009,1009,799,799,-1019,-1276,-1452,-1581,-1677,-1757,-1821,-1886,-1933,-1997,1257,1257,1483,1468,1512,1422,1497,1406,1467,1496,1421,1510,1134,1134,1225,1225,1466,1451,1374,1405,1252,1252,1358,1480,1164,1164,1251,1251,1238,1238,1389,1465,-1407,1054,1101,-1423,1207,-1439,830,830,1248,1038,1237,1117,1223,1148,1236,1208,411,426,395,410,379,269,1193,1222,1132,1235,1221,1116,976,976,1192,1162,1177,1220,1131,1191,963,963,-1647,961,780,-1663,558,558,994,993,437,408,393,407,829,978,813,797,947,-1743,721,721,377,392,844,950,828,890,706,706,812,859,796,960,948,843,934,874,571,571,-1919,690,555,689,421,346,539,539,944,779,918,873,932,842,903,888,570,570,931,917,674,674,-2575,1562,-2591,1609,-2607,1654,1322,1322,1441,1441,1696,1546,1683,1593,1669,1624,1426,1426,1321,1321,1639,1680,1425,1425,1305,1305,1545,1668,1608,1623,1667,1592,1638,1666,1320,1320,1652,1607,1409,1409,1304,1304,1288,1288,1664,1637,1395,1395,1335,1335,1622,1636,1394,1394,1319,1319,1606,1621,1392,1392,1137,1137,1137,1137,345,390,360,375,404,373,1047,-2751,-2767,-2783,1062,1121,1046,-2799,1077,-2815,1106,1061,789,789,1105,1104,263,355,310,340,325,354,352,262,339,324,1091,1076,1029,1090,1060,1075,833,833,788,788,1088,1028,818,818,803,803,561,561,531,531,816,771,546,546,289,274,288,258, + -253,-317,-381,-446,-478,-509,1279,1279,-811,-1179,-1451,-1756,-1900,-2028,-2189,-2253,-2333,-2414,-2445,-2511,-2526,1313,1298,-2559,1041,1041,1040,1040,1025,1025,1024,1024,1022,1007,1021,991,1020,975,1019,959,687,687,1018,1017,671,671,655,655,1016,1015,639,639,758,758,623,623,757,607,756,591,755,575,754,559,543,543,1009,783,-575,-621,-685,-749,496,-590,750,749,734,748,974,989,1003,958,988,973,1002,942,987,957,972,1001,926,986,941,971,956,1000,910,985,925,999,894,970,-1071,-1087,-1102,1390,-1135,1436,1509,1451,1374,-1151,1405,1358,1480,1420,-1167,1507,1494,1389,1342,1465,1435,1450,1326,1505,1310,1493,1373,1479,1404,1492,1464,1419,428,443,472,397,736,526,464,464,486,457,442,471,484,482,1357,1449,1434,1478,1388,1491,1341,1490,1325,1489,1463,1403,1309,1477,1372,1448,1418,1433,1476,1356,1462,1387,-1439,1475,1340,1447,1402,1474,1324,1461,1371,1473,269,448,1432,1417,1308,1460,-1711,1459,-1727,1441,1099,1099,1446,1386,1431,1401,-1743,1289,1083,1083,1160,1160,1458,1445,1067,1067,1370,1457,1307,1430,1129,1129,1098,1098,268,432,267,416,266,400,-1887,1144,1187,1082,1173,1113,1186,1066,1050,1158,1128,1143,1172,1097,1171,1081,420,391,1157,1112,1170,1142,1127,1065,1169,1049,1156,1096,1141,1111,1155,1080,1126,1154,1064,1153,1140,1095,1048,-2159,1125,1110,1137,-2175,823,823,1139,1138,807,807,384,264,368,263,868,838,853,791,867,822,852,837,866,806,865,790,-2319,851,821,836,352,262,850,805,849,-2399,533,533,835,820,336,261,578,548,563,577,532,532,832,772,562,562,547,547,305,275,560,515,290,290,288,258 }; + static const uint8_t tab32[] = { 130,162,193,209,44,28,76,140,9,9,9,9,9,9,9,9,190,254,222,238,126,94,157,157,109,61,173,205 }; + static const uint8_t tab33[] = { 252,236,220,204,188,172,156,140,124,108,92,76,60,44,28,12 }; + static const int16_t tabindex[2*16] = { 0,32,64,98,0,132,180,218,292,364,426,538,648,746,0,1126,1460,1460,1460,1460,1460,1460,1460,1460,1842,1842,1842,1842,1842,1842,1842,1842 }; + static const uint8_t g_linbits[] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,2,3,4,6,8,10,13,4,5,6,7,8,9,11,13 }; + +#define PEEK_BITS(n) (bs_cache >> (32 - n)) +#define FLUSH_BITS(n) { bs_cache <<= (n); bs_sh += (n); } +#define CHECK_BITS while (bs_sh >= 0) { bs_cache |= (uint32_t)*bs_next_ptr++ << bs_sh; bs_sh -= 8; } +#define BSPOS ((bs_next_ptr - bs->buf)*8 - 24 + bs_sh) + + float one = 0.0f; + int ireg = 0, big_val_cnt = gr_info->big_values; + const uint8_t *sfb = gr_info->sfbtab; + const uint8_t *bs_next_ptr = bs->buf + bs->pos/8; + uint32_t bs_cache = (((bs_next_ptr[0]*256u + bs_next_ptr[1])*256u + bs_next_ptr[2])*256u + bs_next_ptr[3]) << (bs->pos & 7); + int pairs_to_decode, np, bs_sh = (bs->pos & 7) - 8; + bs_next_ptr += 4; + + while (big_val_cnt > 0) + { + int tab_num = gr_info->table_select[ireg]; + int sfb_cnt = gr_info->region_count[ireg++]; + const int16_t *codebook = tabs + tabindex[tab_num]; + int linbits = g_linbits[tab_num]; + if (linbits) + { + do + { + np = *sfb++ / 2; + pairs_to_decode = MINIMP3_MIN(big_val_cnt, np); + one = *scf++; + do + { + int j, w = 5; + int leaf = codebook[PEEK_BITS(w)]; + while (leaf < 0) + { + FLUSH_BITS(w); + w = leaf & 7; + leaf = codebook[PEEK_BITS(w) - (leaf >> 3)]; + } + FLUSH_BITS(leaf >> 8); + + for (j = 0; j < 2; j++, dst++, leaf >>= 4) + { + int lsb = leaf & 0x0F; + if (lsb == 15) + { + lsb += PEEK_BITS(linbits); + FLUSH_BITS(linbits); + CHECK_BITS; + *dst = one*L3_pow_43(lsb)*((int32_t)bs_cache < 0 ? -1: 1); + } else + { + *dst = g_pow43[16 + lsb - 16*(bs_cache >> 31)]*one; + } + FLUSH_BITS(lsb ? 1 : 0); + } + CHECK_BITS; + } while (--pairs_to_decode); + } while ((big_val_cnt -= np) > 0 && --sfb_cnt >= 0); + } else + { + do + { + np = *sfb++ / 2; + pairs_to_decode = MINIMP3_MIN(big_val_cnt, np); + one = *scf++; + do + { + int j, w = 5; + int leaf = codebook[PEEK_BITS(w)]; + while (leaf < 0) + { + FLUSH_BITS(w); + w = leaf & 7; + leaf = codebook[PEEK_BITS(w) - (leaf >> 3)]; + } + FLUSH_BITS(leaf >> 8); + + for (j = 0; j < 2; j++, dst++, leaf >>= 4) + { + int lsb = leaf & 0x0F; + *dst = g_pow43[16 + lsb - 16*(bs_cache >> 31)]*one; + FLUSH_BITS(lsb ? 1 : 0); + } + CHECK_BITS; + } while (--pairs_to_decode); + } while ((big_val_cnt -= np) > 0 && --sfb_cnt >= 0); + } + } + + for (np = 1 - big_val_cnt;; dst += 4) + { + const uint8_t *codebook_count1 = (gr_info->count1_table) ? tab33 : tab32; + int leaf = codebook_count1[PEEK_BITS(4)]; + if (!(leaf & 8)) + { + leaf = codebook_count1[(leaf >> 3) + (bs_cache << 4 >> (32 - (leaf & 3)))]; + } + FLUSH_BITS(leaf & 7); + if (BSPOS > layer3gr_limit) + { + break; + } +#define RELOAD_SCALEFACTOR if (!--np) { np = *sfb++/2; if (!np) break; one = *scf++; } +#define DEQ_COUNT1(s) if (leaf & (128 >> s)) { dst[s] = ((int32_t)bs_cache < 0) ? -one : one; FLUSH_BITS(1) } + RELOAD_SCALEFACTOR; + DEQ_COUNT1(0); + DEQ_COUNT1(1); + RELOAD_SCALEFACTOR; + DEQ_COUNT1(2); + DEQ_COUNT1(3); + CHECK_BITS; + } + + bs->pos = layer3gr_limit; +} + +static void L3_midside_stereo(float *left, int n) +{ + int i = 0; + float *right = left + 576; +#if HAVE_SIMD + if (have_simd()) for (; i < n - 3; i += 4) + { + f4 vl = VLD(left + i); + f4 vr = VLD(right + i); + VSTORE(left + i, VADD(vl, vr)); + VSTORE(right + i, VSUB(vl, vr)); + } +#endif /* HAVE_SIMD */ + for (; i < n; i++) + { + float a = left[i]; + float b = right[i]; + left[i] = a + b; + right[i] = a - b; + } +} + +static void L3_intensity_stereo_band(float *left, int n, float kl, float kr) +{ + int i; + for (i = 0; i < n; i++) + { + left[i + 576] = left[i]*kr; + left[i] = left[i]*kl; + } +} + +static void L3_stereo_top_band(const float *right, const uint8_t *sfb, int nbands, int max_band[3]) +{ + int i, k; + + max_band[0] = max_band[1] = max_band[2] = -1; + + for (i = 0; i < nbands; i++) + { + for (k = 0; k < sfb[i]; k += 2) + { + if (right[k] != 0 || right[k + 1] != 0) + { + max_band[i % 3] = i; + break; + } + } + right += sfb[i]; + } +} + +static void L3_stereo_process(float *left, const uint8_t *ist_pos, const uint8_t *sfb, const uint8_t *hdr, int max_band[3], int mpeg2_sh) +{ + static const float g_pan[7*2] = { 0,1,0.21132487f,0.78867513f,0.36602540f,0.63397460f,0.5f,0.5f,0.63397460f,0.36602540f,0.78867513f,0.21132487f,1,0 }; + unsigned i, max_pos = HDR_TEST_MPEG1(hdr) ? 7 : 64; + + for (i = 0; sfb[i]; i++) + { + unsigned ipos = ist_pos[i]; + if ((int)i > max_band[i % 3] && ipos < max_pos) + { + float kl, kr, s = HDR_TEST_MS_STEREO(hdr) ? 1.41421356f : 1; + if (HDR_TEST_MPEG1(hdr)) + { + kl = g_pan[2*ipos]; + kr = g_pan[2*ipos + 1]; + } else + { + kl = 1; + kr = L3_ldexp_q2(1, (ipos + 1) >> 1 << mpeg2_sh); + if (ipos & 1) + { + kl = kr; + kr = 1; + } + } + L3_intensity_stereo_band(left, sfb[i], kl*s, kr*s); + } else if (HDR_TEST_MS_STEREO(hdr)) + { + L3_midside_stereo(left, sfb[i]); + } + left += sfb[i]; + } +} + +static void L3_intensity_stereo(float *left, uint8_t *ist_pos, const L3_gr_info_t *gr, const uint8_t *hdr) +{ + int max_band[3], n_sfb = gr->n_long_sfb + gr->n_short_sfb; + int i, max_blocks = gr->n_short_sfb ? 3 : 1; + + L3_stereo_top_band(left + 576, gr->sfbtab, n_sfb, max_band); + if (gr->n_long_sfb) + { + max_band[0] = max_band[1] = max_band[2] = MINIMP3_MAX(MINIMP3_MAX(max_band[0], max_band[1]), max_band[2]); + } + for (i = 0; i < max_blocks; i++) + { + int default_pos = HDR_TEST_MPEG1(hdr) ? 3 : 0; + int itop = n_sfb - max_blocks + i; + int prev = itop - max_blocks; + ist_pos[itop] = max_band[i] >= prev ? default_pos : ist_pos[prev]; + } + L3_stereo_process(left, ist_pos, gr->sfbtab, hdr, max_band, gr[1].scalefac_compress & 1); +} + +static void L3_reorder(float *grbuf, float *scratch, const uint8_t *sfb) +{ + int i, len; + float *src = grbuf, *dst = scratch; + + for (;0 != (len = *sfb); sfb += 3, src += 2*len) + { + for (i = 0; i < len; i++, src++) + { + *dst++ = src[0*len]; + *dst++ = src[1*len]; + *dst++ = src[2*len]; + } + } + memcpy(grbuf, scratch, (dst - scratch)*sizeof(float)); +} + +static void L3_antialias(float *grbuf, int nbands) +{ + static const float g_aa[2][8] = { + {0.85749293f,0.88174200f,0.94962865f,0.98331459f,0.99551782f,0.99916056f,0.99989920f,0.99999316f}, + {0.51449576f,0.47173197f,0.31337745f,0.18191320f,0.09457419f,0.04096558f,0.01419856f,0.00369997f} + }; + + for (; nbands > 0; nbands--, grbuf += 18) + { + int i = 0; +#if HAVE_SIMD + if (have_simd()) for (; i < 8; i += 4) + { + f4 vu = VLD(grbuf + 18 + i); + f4 vd = VLD(grbuf + 14 - i); + f4 vc0 = VLD(g_aa[0] + i); + f4 vc1 = VLD(g_aa[1] + i); + vd = VREV(vd); + VSTORE(grbuf + 18 + i, VSUB(VMUL(vu, vc0), VMUL(vd, vc1))); + vd = VADD(VMUL(vu, vc1), VMUL(vd, vc0)); + VSTORE(grbuf + 14 - i, VREV(vd)); + } +#endif /* HAVE_SIMD */ +#ifndef MINIMP3_ONLY_SIMD + for(; i < 8; i++) + { + float u = grbuf[18 + i]; + float d = grbuf[17 - i]; + grbuf[18 + i] = u*g_aa[0][i] - d*g_aa[1][i]; + grbuf[17 - i] = u*g_aa[1][i] + d*g_aa[0][i]; + } +#endif /* MINIMP3_ONLY_SIMD */ + } +} + +static void L3_dct3_9(float *y) +{ + float s0, s1, s2, s3, s4, s5, s6, s7, s8, t0, t2, t4; + + s0 = y[0]; s2 = y[2]; s4 = y[4]; s6 = y[6]; s8 = y[8]; + t0 = s0 + s6*0.5f; + s0 -= s6; + t4 = (s4 + s2)*0.93969262f; + t2 = (s8 + s2)*0.76604444f; + s6 = (s4 - s8)*0.17364818f; + s4 += s8 - s2; + + s2 = s0 - s4*0.5f; + y[4] = s4 + s0; + s8 = t0 - t2 + s6; + s0 = t0 - t4 + t2; + s4 = t0 + t4 - s6; + + s1 = y[1]; s3 = y[3]; s5 = y[5]; s7 = y[7]; + + s3 *= 0.86602540f; + t0 = (s5 + s1)*0.98480775f; + t4 = (s5 - s7)*0.34202014f; + t2 = (s1 + s7)*0.64278761f; + s1 = (s1 - s5 - s7)*0.86602540f; + + s5 = t0 - s3 - t2; + s7 = t4 - s3 - t0; + s3 = t4 + s3 - t2; + + y[0] = s4 - s7; + y[1] = s2 + s1; + y[2] = s0 - s3; + y[3] = s8 + s5; + y[5] = s8 - s5; + y[6] = s0 + s3; + y[7] = s2 - s1; + y[8] = s4 + s7; +} + +static void L3_imdct36(float *grbuf, float *overlap, const float *window, int nbands) +{ + int i, j; + static const float g_twid9[18] = { + 0.73727734f,0.79335334f,0.84339145f,0.88701083f,0.92387953f,0.95371695f,0.97629601f,0.99144486f,0.99904822f,0.67559021f,0.60876143f,0.53729961f,0.46174861f,0.38268343f,0.30070580f,0.21643961f,0.13052619f,0.04361938f + }; + + for (j = 0; j < nbands; j++, grbuf += 18, overlap += 9) + { + float co[9], si[9]; + co[0] = -grbuf[0]; + si[0] = grbuf[17]; + for (i = 0; i < 4; i++) + { + si[8 - 2*i] = grbuf[4*i + 1] - grbuf[4*i + 2]; + co[1 + 2*i] = grbuf[4*i + 1] + grbuf[4*i + 2]; + si[7 - 2*i] = grbuf[4*i + 4] - grbuf[4*i + 3]; + co[2 + 2*i] = -(grbuf[4*i + 3] + grbuf[4*i + 4]); + } + L3_dct3_9(co); + L3_dct3_9(si); + + si[1] = -si[1]; + si[3] = -si[3]; + si[5] = -si[5]; + si[7] = -si[7]; + + i = 0; + +#if HAVE_SIMD + if (have_simd()) for (; i < 8; i += 4) + { + f4 vovl = VLD(overlap + i); + f4 vc = VLD(co + i); + f4 vs = VLD(si + i); + f4 vr0 = VLD(g_twid9 + i); + f4 vr1 = VLD(g_twid9 + 9 + i); + f4 vw0 = VLD(window + i); + f4 vw1 = VLD(window + 9 + i); + f4 vsum = VADD(VMUL(vc, vr1), VMUL(vs, vr0)); + VSTORE(overlap + i, VSUB(VMUL(vc, vr0), VMUL(vs, vr1))); + VSTORE(grbuf + i, VSUB(VMUL(vovl, vw0), VMUL(vsum, vw1))); + vsum = VADD(VMUL(vovl, vw1), VMUL(vsum, vw0)); + VSTORE(grbuf + 14 - i, VREV(vsum)); + } +#endif /* HAVE_SIMD */ + for (; i < 9; i++) + { + float ovl = overlap[i]; + float sum = co[i]*g_twid9[9 + i] + si[i]*g_twid9[0 + i]; + overlap[i] = co[i]*g_twid9[0 + i] - si[i]*g_twid9[9 + i]; + grbuf[i] = ovl*window[0 + i] - sum*window[9 + i]; + grbuf[17 - i] = ovl*window[9 + i] + sum*window[0 + i]; + } + } +} + +static void L3_idct3(float x0, float x1, float x2, float *dst) +{ + float m1 = x1*0.86602540f; + float a1 = x0 - x2*0.5f; + dst[1] = x0 + x2; + dst[0] = a1 + m1; + dst[2] = a1 - m1; +} + +static void L3_imdct12(float *x, float *dst, float *overlap) +{ + static const float g_twid3[6] = { 0.79335334f,0.92387953f,0.99144486f, 0.60876143f,0.38268343f,0.13052619f }; + float co[3], si[3]; + int i; + + L3_idct3(-x[0], x[6] + x[3], x[12] + x[9], co); + L3_idct3(x[15], x[12] - x[9], x[6] - x[3], si); + si[1] = -si[1]; + + for (i = 0; i < 3; i++) + { + float ovl = overlap[i]; + float sum = co[i]*g_twid3[3 + i] + si[i]*g_twid3[0 + i]; + overlap[i] = co[i]*g_twid3[0 + i] - si[i]*g_twid3[3 + i]; + dst[i] = ovl*g_twid3[2 - i] - sum*g_twid3[5 - i]; + dst[5 - i] = ovl*g_twid3[5 - i] + sum*g_twid3[2 - i]; + } +} + +static void L3_imdct_short(float *grbuf, float *overlap, int nbands) +{ + for (;nbands > 0; nbands--, overlap += 9, grbuf += 18) + { + float tmp[18]; + memcpy(tmp, grbuf, sizeof(tmp)); + memcpy(grbuf, overlap, 6*sizeof(float)); + L3_imdct12(tmp, grbuf + 6, overlap + 6); + L3_imdct12(tmp + 1, grbuf + 12, overlap + 6); + L3_imdct12(tmp + 2, overlap, overlap + 6); + } +} + +static void L3_change_sign(float *grbuf) +{ + int b, i; + for (b = 0, grbuf += 18; b < 32; b += 2, grbuf += 36) + for (i = 1; i < 18; i += 2) + grbuf[i] = -grbuf[i]; +} + +static void L3_imdct_gr(float *grbuf, float *overlap, unsigned block_type, unsigned n_long_bands) +{ + static const float g_mdct_window[2][18] = { + { 0.99904822f,0.99144486f,0.97629601f,0.95371695f,0.92387953f,0.88701083f,0.84339145f,0.79335334f,0.73727734f,0.04361938f,0.13052619f,0.21643961f,0.30070580f,0.38268343f,0.46174861f,0.53729961f,0.60876143f,0.67559021f }, + { 1,1,1,1,1,1,0.99144486f,0.92387953f,0.79335334f,0,0,0,0,0,0,0.13052619f,0.38268343f,0.60876143f } + }; + if (n_long_bands) + { + L3_imdct36(grbuf, overlap, g_mdct_window[0], n_long_bands); + grbuf += 18*n_long_bands; + overlap += 9*n_long_bands; + } + if (block_type == SHORT_BLOCK_TYPE) + L3_imdct_short(grbuf, overlap, 32 - n_long_bands); + else + L3_imdct36(grbuf, overlap, g_mdct_window[block_type == STOP_BLOCK_TYPE], 32 - n_long_bands); +} + +static void L3_save_reservoir(mp3dec_t *h, mp3dec_scratch_t *s) +{ + int pos = (s->bs.pos + 7)/8u; + int remains = s->bs.limit/8u - pos; + if (remains > MAX_BITRESERVOIR_BYTES) + { + pos += remains - MAX_BITRESERVOIR_BYTES; + remains = MAX_BITRESERVOIR_BYTES; + } + if (remains > 0) + { + memmove(h->reserv_buf, s->maindata + pos, remains); + } + h->reserv = remains; +} + +static int L3_restore_reservoir(mp3dec_t *h, bs_t *bs, mp3dec_scratch_t *s, int main_data_begin) +{ + int frame_bytes = (bs->limit - bs->pos)/8; + int bytes_have = MINIMP3_MIN(h->reserv, main_data_begin); + memcpy(s->maindata, h->reserv_buf + MINIMP3_MAX(0, h->reserv - main_data_begin), MINIMP3_MIN(h->reserv, main_data_begin)); + memcpy(s->maindata + bytes_have, bs->buf + bs->pos/8, frame_bytes); + bs_init(&s->bs, s->maindata, bytes_have + frame_bytes); + return h->reserv >= main_data_begin; +} + +static void L3_decode(mp3dec_t *h, mp3dec_scratch_t *s, L3_gr_info_t *gr_info, int nch) +{ + int ch; + + for (ch = 0; ch < nch; ch++) + { + int layer3gr_limit = s->bs.pos + gr_info[ch].part_23_length; + L3_decode_scalefactors(h->header, s->ist_pos[ch], &s->bs, gr_info + ch, s->scf, ch); + L3_huffman(s->grbuf[ch], &s->bs, gr_info + ch, s->scf, layer3gr_limit); + } + + if (HDR_TEST_I_STEREO(h->header)) + { + L3_intensity_stereo(s->grbuf[0], s->ist_pos[1], gr_info, h->header); + } else if (HDR_IS_MS_STEREO(h->header)) + { + L3_midside_stereo(s->grbuf[0], 576); + } + + for (ch = 0; ch < nch; ch++, gr_info++) + { + int aa_bands = 31; + int n_long_bands = (gr_info->mixed_block_flag ? 2 : 0) << (int)(HDR_GET_MY_SAMPLE_RATE(h->header) == 2); + + if (gr_info->n_short_sfb) + { + aa_bands = n_long_bands - 1; + L3_reorder(s->grbuf[ch] + n_long_bands*18, s->syn[0], gr_info->sfbtab + gr_info->n_long_sfb); + } + + L3_antialias(s->grbuf[ch], aa_bands); + L3_imdct_gr(s->grbuf[ch], h->mdct_overlap[ch], gr_info->block_type, n_long_bands); + L3_change_sign(s->grbuf[ch]); + } +} + +static void mp3d_DCT_II(float *grbuf, int n) +{ + static const float g_sec[24] = { + 10.19000816f,0.50060302f,0.50241929f,3.40760851f,0.50547093f,0.52249861f,2.05778098f,0.51544732f,0.56694406f,1.48416460f,0.53104258f,0.64682180f,1.16943991f,0.55310392f,0.78815460f,0.97256821f,0.58293498f,1.06067765f,0.83934963f,0.62250412f,1.72244716f,0.74453628f,0.67480832f,5.10114861f + }; + int i, k = 0; +#if HAVE_SIMD + if (have_simd()) for (; k < n; k += 4) + { + f4 t[4][8], *x; + float *y = grbuf + k; + + for (x = t[0], i = 0; i < 8; i++, x++) + { + f4 x0 = VLD(&y[i*18]); + f4 x1 = VLD(&y[(15 - i)*18]); + f4 x2 = VLD(&y[(16 + i)*18]); + f4 x3 = VLD(&y[(31 - i)*18]); + f4 t0 = VADD(x0, x3); + f4 t1 = VADD(x1, x2); + f4 t2 = VMUL_S(VSUB(x1, x2), g_sec[3*i + 0]); + f4 t3 = VMUL_S(VSUB(x0, x3), g_sec[3*i + 1]); + x[0] = VADD(t0, t1); + x[8] = VMUL_S(VSUB(t0, t1), g_sec[3*i + 2]); + x[16] = VADD(t3, t2); + x[24] = VMUL_S(VSUB(t3, t2), g_sec[3*i + 2]); + } + for (x = t[0], i = 0; i < 4; i++, x += 8) + { + f4 x0 = x[0], x1 = x[1], x2 = x[2], x3 = x[3], x4 = x[4], x5 = x[5], x6 = x[6], x7 = x[7], xt; + xt = VSUB(x0, x7); x0 = VADD(x0, x7); + x7 = VSUB(x1, x6); x1 = VADD(x1, x6); + x6 = VSUB(x2, x5); x2 = VADD(x2, x5); + x5 = VSUB(x3, x4); x3 = VADD(x3, x4); + x4 = VSUB(x0, x3); x0 = VADD(x0, x3); + x3 = VSUB(x1, x2); x1 = VADD(x1, x2); + x[0] = VADD(x0, x1); + x[4] = VMUL_S(VSUB(x0, x1), 0.70710677f); + x5 = VADD(x5, x6); + x6 = VMUL_S(VADD(x6, x7), 0.70710677f); + x7 = VADD(x7, xt); + x3 = VMUL_S(VADD(x3, x4), 0.70710677f); + x5 = VSUB(x5, VMUL_S(x7, 0.198912367f)); /* rotate by PI/8 */ + x7 = VADD(x7, VMUL_S(x5, 0.382683432f)); + x5 = VSUB(x5, VMUL_S(x7, 0.198912367f)); + x0 = VSUB(xt, x6); xt = VADD(xt, x6); + x[1] = VMUL_S(VADD(xt, x7), 0.50979561f); + x[2] = VMUL_S(VADD(x4, x3), 0.54119611f); + x[3] = VMUL_S(VSUB(x0, x5), 0.60134488f); + x[5] = VMUL_S(VADD(x0, x5), 0.89997619f); + x[6] = VMUL_S(VSUB(x4, x3), 1.30656302f); + x[7] = VMUL_S(VSUB(xt, x7), 2.56291556f); + } + + if (k > n - 3) + { +#if HAVE_SSE +#define VSAVE2(i, v) _mm_storel_pi((__m64 *)(void*)&y[i*18], v) +#else /* HAVE_SSE */ +#define VSAVE2(i, v) vst1_f32((float32_t *)&y[i*18], vget_low_f32(v)) +#endif /* HAVE_SSE */ + for (i = 0; i < 7; i++, y += 4*18) + { + f4 s = VADD(t[3][i], t[3][i + 1]); + VSAVE2(0, t[0][i]); + VSAVE2(1, VADD(t[2][i], s)); + VSAVE2(2, VADD(t[1][i], t[1][i + 1])); + VSAVE2(3, VADD(t[2][1 + i], s)); + } + VSAVE2(0, t[0][7]); + VSAVE2(1, VADD(t[2][7], t[3][7])); + VSAVE2(2, t[1][7]); + VSAVE2(3, t[3][7]); + } else + { +#define VSAVE4(i, v) VSTORE(&y[i*18], v) + for (i = 0; i < 7; i++, y += 4*18) + { + f4 s = VADD(t[3][i], t[3][i + 1]); + VSAVE4(0, t[0][i]); + VSAVE4(1, VADD(t[2][i], s)); + VSAVE4(2, VADD(t[1][i], t[1][i + 1])); + VSAVE4(3, VADD(t[2][1 + i], s)); + } + VSAVE4(0, t[0][7]); + VSAVE4(1, VADD(t[2][7], t[3][7])); + VSAVE4(2, t[1][7]); + VSAVE4(3, t[3][7]); + } + } else +#endif /* HAVE_SIMD */ +#ifdef MINIMP3_ONLY_SIMD + {} +#else /* MINIMP3_ONLY_SIMD */ + for (; k < n; k++) + { + float t[4][8], *x, *y = grbuf + k; + + for (x = t[0], i = 0; i < 8; i++, x++) + { + float x0 = y[i*18]; + float x1 = y[(15 - i)*18]; + float x2 = y[(16 + i)*18]; + float x3 = y[(31 - i)*18]; + float t0 = x0 + x3; + float t1 = x1 + x2; + float t2 = (x1 - x2)*g_sec[3*i + 0]; + float t3 = (x0 - x3)*g_sec[3*i + 1]; + x[0] = t0 + t1; + x[8] = (t0 - t1)*g_sec[3*i + 2]; + x[16] = t3 + t2; + x[24] = (t3 - t2)*g_sec[3*i + 2]; + } + for (x = t[0], i = 0; i < 4; i++, x += 8) + { + float x0 = x[0], x1 = x[1], x2 = x[2], x3 = x[3], x4 = x[4], x5 = x[5], x6 = x[6], x7 = x[7], xt; + xt = x0 - x7; x0 += x7; + x7 = x1 - x6; x1 += x6; + x6 = x2 - x5; x2 += x5; + x5 = x3 - x4; x3 += x4; + x4 = x0 - x3; x0 += x3; + x3 = x1 - x2; x1 += x2; + x[0] = x0 + x1; + x[4] = (x0 - x1)*0.70710677f; + x5 = x5 + x6; + x6 = (x6 + x7)*0.70710677f; + x7 = x7 + xt; + x3 = (x3 + x4)*0.70710677f; + x5 -= x7*0.198912367f; /* rotate by PI/8 */ + x7 += x5*0.382683432f; + x5 -= x7*0.198912367f; + x0 = xt - x6; xt += x6; + x[1] = (xt + x7)*0.50979561f; + x[2] = (x4 + x3)*0.54119611f; + x[3] = (x0 - x5)*0.60134488f; + x[5] = (x0 + x5)*0.89997619f; + x[6] = (x4 - x3)*1.30656302f; + x[7] = (xt - x7)*2.56291556f; + + } + for (i = 0; i < 7; i++, y += 4*18) + { + y[0*18] = t[0][i]; + y[1*18] = t[2][i] + t[3][i] + t[3][i + 1]; + y[2*18] = t[1][i] + t[1][i + 1]; + y[3*18] = t[2][i + 1] + t[3][i] + t[3][i + 1]; + } + y[0*18] = t[0][7]; + y[1*18] = t[2][7] + t[3][7]; + y[2*18] = t[1][7]; + y[3*18] = t[3][7]; + } +#endif /* MINIMP3_ONLY_SIMD */ +} + +#ifndef MINIMP3_FLOAT_OUTPUT +static int16_t mp3d_scale_pcm(float sample) +{ +#if HAVE_ARMV6 + int32_t s32 = (int32_t)(sample + .5f); + s32 -= (s32 < 0); + int16_t s = (int16_t)minimp3_clip_int16_arm(s32); +#else + if (sample >= 32766.5) return (int16_t) 32767; + if (sample <= -32767.5) return (int16_t)-32768; + int16_t s = (int16_t)(sample + .5f); + s -= (s < 0); /* away from zero, to be compliant */ +#endif + return s; +} +#else /* MINIMP3_FLOAT_OUTPUT */ +static float mp3d_scale_pcm(float sample) +{ + return sample*(1.f/32768.f); +} +#endif /* MINIMP3_FLOAT_OUTPUT */ + +static void mp3d_synth_pair(mp3d_sample_t *pcm, int nch, const float *z) +{ + float a; + a = (z[14*64] - z[ 0]) * 29; + a += (z[ 1*64] + z[13*64]) * 213; + a += (z[12*64] - z[ 2*64]) * 459; + a += (z[ 3*64] + z[11*64]) * 2037; + a += (z[10*64] - z[ 4*64]) * 5153; + a += (z[ 5*64] + z[ 9*64]) * 6574; + a += (z[ 8*64] - z[ 6*64]) * 37489; + a += z[ 7*64] * 75038; + pcm[0] = mp3d_scale_pcm(a); + + z += 2; + a = z[14*64] * 104; + a += z[12*64] * 1567; + a += z[10*64] * 9727; + a += z[ 8*64] * 64019; + a += z[ 6*64] * -9975; + a += z[ 4*64] * -45; + a += z[ 2*64] * 146; + a += z[ 0*64] * -5; + pcm[16*nch] = mp3d_scale_pcm(a); +} + +static void mp3d_synth(float *xl, mp3d_sample_t *dstl, int nch, float *lins) +{ + int i; + float *xr = xl + 576*(nch - 1); + mp3d_sample_t *dstr = dstl + (nch - 1); + + static const float g_win[] = { + -1,26,-31,208,218,401,-519,2063,2000,4788,-5517,7134,5959,35640,-39336,74992, + -1,24,-35,202,222,347,-581,2080,1952,4425,-5879,7640,5288,33791,-41176,74856, + -1,21,-38,196,225,294,-645,2087,1893,4063,-6237,8092,4561,31947,-43006,74630, + -1,19,-41,190,227,244,-711,2085,1822,3705,-6589,8492,3776,30112,-44821,74313, + -1,17,-45,183,228,197,-779,2075,1739,3351,-6935,8840,2935,28289,-46617,73908, + -1,16,-49,176,228,153,-848,2057,1644,3004,-7271,9139,2037,26482,-48390,73415, + -2,14,-53,169,227,111,-919,2032,1535,2663,-7597,9389,1082,24694,-50137,72835, + -2,13,-58,161,224,72,-991,2001,1414,2330,-7910,9592,70,22929,-51853,72169, + -2,11,-63,154,221,36,-1064,1962,1280,2006,-8209,9750,-998,21189,-53534,71420, + -2,10,-68,147,215,2,-1137,1919,1131,1692,-8491,9863,-2122,19478,-55178,70590, + -3,9,-73,139,208,-29,-1210,1870,970,1388,-8755,9935,-3300,17799,-56778,69679, + -3,8,-79,132,200,-57,-1283,1817,794,1095,-8998,9966,-4533,16155,-58333,68692, + -4,7,-85,125,189,-83,-1356,1759,605,814,-9219,9959,-5818,14548,-59838,67629, + -4,7,-91,117,177,-106,-1428,1698,402,545,-9416,9916,-7154,12980,-61289,66494, + -5,6,-97,111,163,-127,-1498,1634,185,288,-9585,9838,-8540,11455,-62684,65290 + }; + float *zlin = lins + 15*64; + const float *w = g_win; + + zlin[4*15] = xl[18*16]; + zlin[4*15 + 1] = xr[18*16]; + zlin[4*15 + 2] = xl[0]; + zlin[4*15 + 3] = xr[0]; + + zlin[4*31] = xl[1 + 18*16]; + zlin[4*31 + 1] = xr[1 + 18*16]; + zlin[4*31 + 2] = xl[1]; + zlin[4*31 + 3] = xr[1]; + + mp3d_synth_pair(dstr, nch, lins + 4*15 + 1); + mp3d_synth_pair(dstr + 32*nch, nch, lins + 4*15 + 64 + 1); + mp3d_synth_pair(dstl, nch, lins + 4*15); + mp3d_synth_pair(dstl + 32*nch, nch, lins + 4*15 + 64); + +#if HAVE_SIMD + if (have_simd()) for (i = 14; i >= 0; i--) + { +#define VLOAD(k) f4 w0 = VSET(*w++); f4 w1 = VSET(*w++); f4 vz = VLD(&zlin[4*i - 64*k]); f4 vy = VLD(&zlin[4*i - 64*(15 - k)]); +#define V0(k) { VLOAD(k) b = VADD(VMUL(vz, w1), VMUL(vy, w0)) ; a = VSUB(VMUL(vz, w0), VMUL(vy, w1)); } +#define V1(k) { VLOAD(k) b = VADD(b, VADD(VMUL(vz, w1), VMUL(vy, w0))); a = VADD(a, VSUB(VMUL(vz, w0), VMUL(vy, w1))); } +#define V2(k) { VLOAD(k) b = VADD(b, VADD(VMUL(vz, w1), VMUL(vy, w0))); a = VADD(a, VSUB(VMUL(vy, w1), VMUL(vz, w0))); } + f4 a, b; + zlin[4*i] = xl[18*(31 - i)]; + zlin[4*i + 1] = xr[18*(31 - i)]; + zlin[4*i + 2] = xl[1 + 18*(31 - i)]; + zlin[4*i + 3] = xr[1 + 18*(31 - i)]; + zlin[4*i + 64] = xl[1 + 18*(1 + i)]; + zlin[4*i + 64 + 1] = xr[1 + 18*(1 + i)]; + zlin[4*i - 64 + 2] = xl[18*(1 + i)]; + zlin[4*i - 64 + 3] = xr[18*(1 + i)]; + + V0(0) V2(1) V1(2) V2(3) V1(4) V2(5) V1(6) V2(7) + + { +#ifndef MINIMP3_FLOAT_OUTPUT +#if HAVE_SSE + static const f4 g_max = { 32767.0f, 32767.0f, 32767.0f, 32767.0f }; + static const f4 g_min = { -32768.0f, -32768.0f, -32768.0f, -32768.0f }; + __m128i pcm8 = _mm_packs_epi32(_mm_cvtps_epi32(_mm_max_ps(_mm_min_ps(a, g_max), g_min)), + _mm_cvtps_epi32(_mm_max_ps(_mm_min_ps(b, g_max), g_min))); + dstr[(15 - i)*nch] = _mm_extract_epi16(pcm8, 1); + dstr[(17 + i)*nch] = _mm_extract_epi16(pcm8, 5); + dstl[(15 - i)*nch] = _mm_extract_epi16(pcm8, 0); + dstl[(17 + i)*nch] = _mm_extract_epi16(pcm8, 4); + dstr[(47 - i)*nch] = _mm_extract_epi16(pcm8, 3); + dstr[(49 + i)*nch] = _mm_extract_epi16(pcm8, 7); + dstl[(47 - i)*nch] = _mm_extract_epi16(pcm8, 2); + dstl[(49 + i)*nch] = _mm_extract_epi16(pcm8, 6); +#else /* HAVE_SSE */ + int16x4_t pcma, pcmb; + a = VADD(a, VSET(0.5f)); + b = VADD(b, VSET(0.5f)); + pcma = vqmovn_s32(vqaddq_s32(vcvtq_s32_f32(a), vreinterpretq_s32_u32(vcltq_f32(a, VSET(0))))); + pcmb = vqmovn_s32(vqaddq_s32(vcvtq_s32_f32(b), vreinterpretq_s32_u32(vcltq_f32(b, VSET(0))))); + vst1_lane_s16(dstr + (15 - i)*nch, pcma, 1); + vst1_lane_s16(dstr + (17 + i)*nch, pcmb, 1); + vst1_lane_s16(dstl + (15 - i)*nch, pcma, 0); + vst1_lane_s16(dstl + (17 + i)*nch, pcmb, 0); + vst1_lane_s16(dstr + (47 - i)*nch, pcma, 3); + vst1_lane_s16(dstr + (49 + i)*nch, pcmb, 3); + vst1_lane_s16(dstl + (47 - i)*nch, pcma, 2); + vst1_lane_s16(dstl + (49 + i)*nch, pcmb, 2); +#endif /* HAVE_SSE */ + +#else /* MINIMP3_FLOAT_OUTPUT */ + + static const f4 g_scale = { 1.0f/32768.0f, 1.0f/32768.0f, 1.0f/32768.0f, 1.0f/32768.0f }; + a = VMUL(a, g_scale); + b = VMUL(b, g_scale); +#if HAVE_SSE + _mm_store_ss(dstr + (15 - i)*nch, _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1))); + _mm_store_ss(dstr + (17 + i)*nch, _mm_shuffle_ps(b, b, _MM_SHUFFLE(1, 1, 1, 1))); + _mm_store_ss(dstl + (15 - i)*nch, _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0))); + _mm_store_ss(dstl + (17 + i)*nch, _mm_shuffle_ps(b, b, _MM_SHUFFLE(0, 0, 0, 0))); + _mm_store_ss(dstr + (47 - i)*nch, _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 3, 3, 3))); + _mm_store_ss(dstr + (49 + i)*nch, _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 3, 3, 3))); + _mm_store_ss(dstl + (47 - i)*nch, _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2))); + _mm_store_ss(dstl + (49 + i)*nch, _mm_shuffle_ps(b, b, _MM_SHUFFLE(2, 2, 2, 2))); +#else /* HAVE_SSE */ + vst1q_lane_f32(dstr + (15 - i)*nch, a, 1); + vst1q_lane_f32(dstr + (17 + i)*nch, b, 1); + vst1q_lane_f32(dstl + (15 - i)*nch, a, 0); + vst1q_lane_f32(dstl + (17 + i)*nch, b, 0); + vst1q_lane_f32(dstr + (47 - i)*nch, a, 3); + vst1q_lane_f32(dstr + (49 + i)*nch, b, 3); + vst1q_lane_f32(dstl + (47 - i)*nch, a, 2); + vst1q_lane_f32(dstl + (49 + i)*nch, b, 2); +#endif /* HAVE_SSE */ +#endif /* MINIMP3_FLOAT_OUTPUT */ + } + } else +#endif /* HAVE_SIMD */ +#ifdef MINIMP3_ONLY_SIMD + {} +#else /* MINIMP3_ONLY_SIMD */ + for (i = 14; i >= 0; i--) + { +#define LOAD(k) float w0 = *w++; float w1 = *w++; float *vz = &zlin[4*i - k*64]; float *vy = &zlin[4*i - (15 - k)*64]; +#define S0(k) { int j; LOAD(k); for (j = 0; j < 4; j++) b[j] = vz[j]*w1 + vy[j]*w0, a[j] = vz[j]*w0 - vy[j]*w1; } +#define S1(k) { int j; LOAD(k); for (j = 0; j < 4; j++) b[j] += vz[j]*w1 + vy[j]*w0, a[j] += vz[j]*w0 - vy[j]*w1; } +#define S2(k) { int j; LOAD(k); for (j = 0; j < 4; j++) b[j] += vz[j]*w1 + vy[j]*w0, a[j] += vy[j]*w1 - vz[j]*w0; } + float a[4], b[4]; + + zlin[4*i] = xl[18*(31 - i)]; + zlin[4*i + 1] = xr[18*(31 - i)]; + zlin[4*i + 2] = xl[1 + 18*(31 - i)]; + zlin[4*i + 3] = xr[1 + 18*(31 - i)]; + zlin[4*(i + 16)] = xl[1 + 18*(1 + i)]; + zlin[4*(i + 16) + 1] = xr[1 + 18*(1 + i)]; + zlin[4*(i - 16) + 2] = xl[18*(1 + i)]; + zlin[4*(i - 16) + 3] = xr[18*(1 + i)]; + + S0(0) S2(1) S1(2) S2(3) S1(4) S2(5) S1(6) S2(7) + + dstr[(15 - i)*nch] = mp3d_scale_pcm(a[1]); + dstr[(17 + i)*nch] = mp3d_scale_pcm(b[1]); + dstl[(15 - i)*nch] = mp3d_scale_pcm(a[0]); + dstl[(17 + i)*nch] = mp3d_scale_pcm(b[0]); + dstr[(47 - i)*nch] = mp3d_scale_pcm(a[3]); + dstr[(49 + i)*nch] = mp3d_scale_pcm(b[3]); + dstl[(47 - i)*nch] = mp3d_scale_pcm(a[2]); + dstl[(49 + i)*nch] = mp3d_scale_pcm(b[2]); + } +#endif /* MINIMP3_ONLY_SIMD */ +} + +static void mp3d_synth_granule(float *qmf_state, float *grbuf, int nbands, int nch, mp3d_sample_t *pcm, float *lins) +{ + int i; + for (i = 0; i < nch; i++) + { + mp3d_DCT_II(grbuf + 576*i, nbands); + } + + memcpy(lins, qmf_state, sizeof(float)*15*64); + + for (i = 0; i < nbands; i += 2) + { + mp3d_synth(grbuf + i, pcm + 32*nch*i, nch, lins + i*64); + } +#ifndef MINIMP3_NONSTANDARD_BUT_LOGICAL + if (nch == 1) + { + for (i = 0; i < 15*64; i += 2) + { + qmf_state[i] = lins[nbands*64 + i]; + } + } else +#endif /* MINIMP3_NONSTANDARD_BUT_LOGICAL */ + { + memcpy(qmf_state, lins + nbands*64, sizeof(float)*15*64); + } +} + +static int mp3d_match_frame(const uint8_t *hdr, int mp3_bytes, int frame_bytes) +{ + int i, nmatch; + for (i = 0, nmatch = 0; nmatch < MAX_FRAME_SYNC_MATCHES; nmatch++) + { + i += hdr_frame_bytes(hdr + i, frame_bytes) + hdr_padding(hdr + i); + if (i + HDR_SIZE > mp3_bytes) + return nmatch > 0; + if (!hdr_compare(hdr, hdr + i)) + return 0; + } + return 1; +} + +static int mp3d_find_frame(const uint8_t *mp3, int mp3_bytes, int *free_format_bytes, int *ptr_frame_bytes) +{ + int i, k; + for (i = 0; i < mp3_bytes - HDR_SIZE; i++, mp3++) + { + if (hdr_valid(mp3)) + { + int frame_bytes = hdr_frame_bytes(mp3, *free_format_bytes); + int frame_and_padding = frame_bytes + hdr_padding(mp3); + + for (k = HDR_SIZE; !frame_bytes && k < MAX_FREE_FORMAT_FRAME_SIZE && i + 2*k < mp3_bytes - HDR_SIZE; k++) + { + if (hdr_compare(mp3, mp3 + k)) + { + int fb = k - hdr_padding(mp3); + int nextfb = fb + hdr_padding(mp3 + k); + if (i + k + nextfb + HDR_SIZE > mp3_bytes || !hdr_compare(mp3, mp3 + k + nextfb)) + continue; + frame_and_padding = k; + frame_bytes = fb; + *free_format_bytes = fb; + } + } + if ((frame_bytes && i + frame_and_padding <= mp3_bytes && + mp3d_match_frame(mp3, mp3_bytes - i, frame_bytes)) || + (!i && frame_and_padding == mp3_bytes)) + { + *ptr_frame_bytes = frame_and_padding; + return i; + } + *free_format_bytes = 0; + } + } + *ptr_frame_bytes = 0; + return mp3_bytes; +} + +void mp3dec_init(mp3dec_t *dec) +{ + dec->header[0] = 0; +} + +int mp3dec_decode_frame(mp3dec_t *dec, const uint8_t *mp3, int mp3_bytes, mp3d_sample_t *pcm, mp3dec_frame_info_t *info) +{ + int i = 0, igr, frame_size = 0, success = 1; + const uint8_t *hdr; + bs_t bs_frame[1]; + mp3dec_scratch_t scratch; + + if (mp3_bytes > 4 && dec->header[0] == 0xff && hdr_compare(dec->header, mp3)) + { + frame_size = hdr_frame_bytes(mp3, dec->free_format_bytes) + hdr_padding(mp3); + if (frame_size != mp3_bytes && (frame_size + HDR_SIZE > mp3_bytes || !hdr_compare(mp3, mp3 + frame_size))) + { + frame_size = 0; + } + } + if (!frame_size) + { + memset(dec, 0, sizeof(mp3dec_t)); + i = mp3d_find_frame(mp3, mp3_bytes, &dec->free_format_bytes, &frame_size); + if (!frame_size || i + frame_size > mp3_bytes) + { + info->frame_bytes = i; + return 0; + } + } + + hdr = mp3 + i; + memcpy(dec->header, hdr, HDR_SIZE); + info->frame_bytes = i + frame_size; + info->frame_offset = i; + info->channels = HDR_IS_MONO(hdr) ? 1 : 2; + info->hz = hdr_sample_rate_hz(hdr); + info->layer = 4 - HDR_GET_LAYER(hdr); + info->bitrate_kbps = hdr_bitrate_kbps(hdr); + + if (!pcm) + { + return hdr_frame_samples(hdr); + } + + bs_init(bs_frame, hdr + HDR_SIZE, frame_size - HDR_SIZE); + if (HDR_IS_CRC(hdr)) + { + get_bits(bs_frame, 16); + } + + if (info->layer == 3) + { + int main_data_begin = L3_read_side_info(bs_frame, scratch.gr_info, hdr); + if (main_data_begin < 0 || bs_frame->pos > bs_frame->limit) + { + mp3dec_init(dec); + return 0; + } + success = L3_restore_reservoir(dec, bs_frame, &scratch, main_data_begin); + if (success) + { + for (igr = 0; igr < (HDR_TEST_MPEG1(hdr) ? 2 : 1); igr++, pcm += 576*info->channels) + { + memset(scratch.grbuf[0], 0, 576*2*sizeof(float)); + L3_decode(dec, &scratch, scratch.gr_info + igr*info->channels, info->channels); + mp3d_synth_granule(dec->qmf_state, scratch.grbuf[0], 18, info->channels, pcm, scratch.syn[0]); + } + } + L3_save_reservoir(dec, &scratch); + } else + { +#ifdef MINIMP3_ONLY_MP3 + return 0; +#else /* MINIMP3_ONLY_MP3 */ + L12_scale_info sci[1]; + L12_read_scale_info(hdr, bs_frame, sci); + + memset(scratch.grbuf[0], 0, 576*2*sizeof(float)); + for (i = 0, igr = 0; igr < 3; igr++) + { + if (12 == (i += L12_dequantize_granule(scratch.grbuf[0] + i, bs_frame, sci, info->layer | 1))) + { + i = 0; + L12_apply_scf_384(sci, sci->scf + igr, scratch.grbuf[0]); + mp3d_synth_granule(dec->qmf_state, scratch.grbuf[0], 12, info->channels, pcm, scratch.syn[0]); + memset(scratch.grbuf[0], 0, 576*2*sizeof(float)); + pcm += 384*info->channels; + } + if (bs_frame->pos > bs_frame->limit) + { + mp3dec_init(dec); + return 0; + } + } +#endif /* MINIMP3_ONLY_MP3 */ + } + return success*hdr_frame_samples(dec->header); +} + +#ifdef MINIMP3_FLOAT_OUTPUT +void mp3dec_f32_to_s16(const float *in, int16_t *out, int num_samples) +{ + int i = 0; +#if HAVE_SIMD + int aligned_count = num_samples & ~7; + for(; i < aligned_count; i += 8) + { + static const f4 g_scale = { 32768.0f, 32768.0f, 32768.0f, 32768.0f }; + f4 a = VMUL(VLD(&in[i ]), g_scale); + f4 b = VMUL(VLD(&in[i+4]), g_scale); +#if HAVE_SSE + static const f4 g_max = { 32767.0f, 32767.0f, 32767.0f, 32767.0f }; + static const f4 g_min = { -32768.0f, -32768.0f, -32768.0f, -32768.0f }; + __m128i pcm8 = _mm_packs_epi32(_mm_cvtps_epi32(_mm_max_ps(_mm_min_ps(a, g_max), g_min)), + _mm_cvtps_epi32(_mm_max_ps(_mm_min_ps(b, g_max), g_min))); + out[i ] = _mm_extract_epi16(pcm8, 0); + out[i+1] = _mm_extract_epi16(pcm8, 1); + out[i+2] = _mm_extract_epi16(pcm8, 2); + out[i+3] = _mm_extract_epi16(pcm8, 3); + out[i+4] = _mm_extract_epi16(pcm8, 4); + out[i+5] = _mm_extract_epi16(pcm8, 5); + out[i+6] = _mm_extract_epi16(pcm8, 6); + out[i+7] = _mm_extract_epi16(pcm8, 7); +#else /* HAVE_SSE */ + int16x4_t pcma, pcmb; + a = VADD(a, VSET(0.5f)); + b = VADD(b, VSET(0.5f)); + pcma = vqmovn_s32(vqaddq_s32(vcvtq_s32_f32(a), vreinterpretq_s32_u32(vcltq_f32(a, VSET(0))))); + pcmb = vqmovn_s32(vqaddq_s32(vcvtq_s32_f32(b), vreinterpretq_s32_u32(vcltq_f32(b, VSET(0))))); + vst1_lane_s16(out+i , pcma, 0); + vst1_lane_s16(out+i+1, pcma, 1); + vst1_lane_s16(out+i+2, pcma, 2); + vst1_lane_s16(out+i+3, pcma, 3); + vst1_lane_s16(out+i+4, pcmb, 0); + vst1_lane_s16(out+i+5, pcmb, 1); + vst1_lane_s16(out+i+6, pcmb, 2); + vst1_lane_s16(out+i+7, pcmb, 3); +#endif /* HAVE_SSE */ + } +#endif /* HAVE_SIMD */ + for(; i < num_samples; i++) + { + float sample = in[i] * 32768.0f; + if (sample >= 32766.5) + out[i] = (int16_t) 32767; + else if (sample <= -32767.5) + out[i] = (int16_t)-32768; + else + { + int16_t s = (int16_t)(sample + .5f); + s -= (s < 0); /* away from zero, to be compliant */ + out[i] = s; + } + } +} +#endif /* MINIMP3_FLOAT_OUTPUT */ +#endif /* MINIMP3_IMPLEMENTATION && !_MINIMP3_IMPLEMENTATION_GUARD */ diff --git a/thirdparty/minimp3/minimp3_ex.h b/thirdparty/minimp3/minimp3_ex.h new file mode 100644 index 0000000000..e29dd15b2e --- /dev/null +++ b/thirdparty/minimp3/minimp3_ex.h @@ -0,0 +1,1394 @@ +#ifndef MINIMP3_EXT_H +#define MINIMP3_EXT_H +/* + https://github.com/lieff/minimp3 + To the extent possible under law, the author(s) have dedicated all copyright and related and neighboring rights to this software to the public domain worldwide. + This software is distributed without any warranty. + See <http://creativecommons.org/publicdomain/zero/1.0/>. +*/ +#include "minimp3.h" + +/* flags for mp3dec_ex_open_* functions */ +#define MP3D_SEEK_TO_BYTE 0 /* mp3dec_ex_seek seeks to byte in stream */ +#define MP3D_SEEK_TO_SAMPLE 1 /* mp3dec_ex_seek precisely seeks to sample using index (created during duration calculation scan or when mp3dec_ex_seek called) */ +#define MP3D_DO_NOT_SCAN 2 /* do not scan whole stream for duration if vbrtag not found, mp3dec_ex_t::samples will be filled only if mp3dec_ex_t::vbr_tag_found == 1 */ +#ifdef MINIMP3_ALLOW_MONO_STEREO_TRANSITION +#define MP3D_ALLOW_MONO_STEREO_TRANSITION 4 +#define MP3D_FLAGS_MASK 7 +#else +#define MP3D_FLAGS_MASK 3 +#endif + +/* compile-time config */ +#define MINIMP3_PREDECODE_FRAMES 2 /* frames to pre-decode and skip after seek (to fill internal structures) */ +/*#define MINIMP3_SEEK_IDX_LINEAR_SEARCH*/ /* define to use linear index search instead of binary search on seek */ +#define MINIMP3_IO_SIZE (128*1024) /* io buffer size for streaming functions, must be greater than MINIMP3_BUF_SIZE */ +#define MINIMP3_BUF_SIZE (16*1024) /* buffer which can hold minimum 10 consecutive mp3 frames (~16KB) worst case */ +/*#define MINIMP3_SCAN_LIMIT (256*1024)*/ /* how many bytes will be scanned to search first valid mp3 frame, to prevent stall on large non-mp3 files */ +#define MINIMP3_ENABLE_RING 0 /* WIP enable hardware magic ring buffer if available, to make less input buffer memmove(s) in callback IO mode */ + +/* return error codes */ +#define MP3D_E_PARAM -1 +#define MP3D_E_MEMORY -2 +#define MP3D_E_IOERROR -3 +#define MP3D_E_USER -4 /* can be used to stop processing from callbacks without indicating specific error */ +#define MP3D_E_DECODE -5 /* decode error which can't be safely skipped, such as sample rate, layer and channels change */ + +typedef struct +{ + mp3d_sample_t *buffer; + size_t samples; /* channels included, byte size = samples*sizeof(mp3d_sample_t) */ + int channels, hz, layer, avg_bitrate_kbps; +} mp3dec_file_info_t; + +typedef struct +{ + const uint8_t *buffer; + size_t size; +} mp3dec_map_info_t; + +typedef struct +{ + uint64_t sample; + uint64_t offset; +} mp3dec_frame_t; + +typedef struct +{ + mp3dec_frame_t *frames; + size_t num_frames, capacity; +} mp3dec_index_t; + +typedef size_t (*MP3D_READ_CB)(void *buf, size_t size, void *user_data); +typedef int (*MP3D_SEEK_CB)(uint64_t position, void *user_data); + +typedef struct +{ + MP3D_READ_CB read; + void *read_data; + MP3D_SEEK_CB seek; + void *seek_data; +} mp3dec_io_t; + +typedef struct +{ + mp3dec_t mp3d; + mp3dec_map_info_t file; + mp3dec_io_t *io; + mp3dec_index_t index; + uint64_t offset, samples, detected_samples, cur_sample, start_offset, end_offset; + mp3dec_frame_info_t info; + mp3d_sample_t buffer[MINIMP3_MAX_SAMPLES_PER_FRAME]; + size_t input_consumed, input_filled; + int is_file, flags, vbr_tag_found, indexes_built; + int free_format_bytes; + int buffer_samples, buffer_consumed, to_skip, start_delay; + int last_error; +} mp3dec_ex_t; + +typedef int (*MP3D_ITERATE_CB)(void *user_data, const uint8_t *frame, int frame_size, int free_format_bytes, size_t buf_size, uint64_t offset, mp3dec_frame_info_t *info); +typedef int (*MP3D_PROGRESS_CB)(void *user_data, size_t file_size, uint64_t offset, mp3dec_frame_info_t *info); + +#ifdef __cplusplus +extern "C" { +#endif + +/* detect mp3/mpa format */ +int mp3dec_detect_buf(const uint8_t *buf, size_t buf_size); +int mp3dec_detect_cb(mp3dec_io_t *io, uint8_t *buf, size_t buf_size); +/* decode whole buffer block */ +int mp3dec_load_buf(mp3dec_t *dec, const uint8_t *buf, size_t buf_size, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data); +int mp3dec_load_cb(mp3dec_t *dec, mp3dec_io_t *io, uint8_t *buf, size_t buf_size, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data); +/* iterate through frames */ +int mp3dec_iterate_buf(const uint8_t *buf, size_t buf_size, MP3D_ITERATE_CB callback, void *user_data); +int mp3dec_iterate_cb(mp3dec_io_t *io, uint8_t *buf, size_t buf_size, MP3D_ITERATE_CB callback, void *user_data); +/* streaming decoder with seeking capability */ +int mp3dec_ex_open_buf(mp3dec_ex_t *dec, const uint8_t *buf, size_t buf_size, int flags); +int mp3dec_ex_open_cb(mp3dec_ex_t *dec, mp3dec_io_t *io, int flags); +void mp3dec_ex_close(mp3dec_ex_t *dec); +int mp3dec_ex_seek(mp3dec_ex_t *dec, uint64_t position); +size_t mp3dec_ex_read_frame(mp3dec_ex_t *dec, mp3d_sample_t **buf, mp3dec_frame_info_t *frame_info, size_t max_samples); +size_t mp3dec_ex_read(mp3dec_ex_t *dec, mp3d_sample_t *buf, size_t samples); +#ifndef MINIMP3_NO_STDIO +/* stdio versions of file detect, load, iterate and stream */ +int mp3dec_detect(const char *file_name); +int mp3dec_load(mp3dec_t *dec, const char *file_name, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data); +int mp3dec_iterate(const char *file_name, MP3D_ITERATE_CB callback, void *user_data); +int mp3dec_ex_open(mp3dec_ex_t *dec, const char *file_name, int flags); +#ifdef _WIN32 +int mp3dec_detect_w(const wchar_t *file_name); +int mp3dec_load_w(mp3dec_t *dec, const wchar_t *file_name, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data); +int mp3dec_iterate_w(const wchar_t *file_name, MP3D_ITERATE_CB callback, void *user_data); +int mp3dec_ex_open_w(mp3dec_ex_t *dec, const wchar_t *file_name, int flags); +#endif +#endif + +#ifdef __cplusplus +} +#endif +#endif /*MINIMP3_EXT_H*/ + +#ifdef MINIMP3_IMPLEMENTATION +#include <limits.h> + +static void mp3dec_skip_id3v1(const uint8_t *buf, size_t *pbuf_size) +{ + size_t buf_size = *pbuf_size; +#ifndef MINIMP3_NOSKIP_ID3V1 + if (buf_size >= 128 && !memcmp(buf + buf_size - 128, "TAG", 3)) + { + buf_size -= 128; + if (buf_size >= 227 && !memcmp(buf + buf_size - 227, "TAG+", 4)) + buf_size -= 227; + } +#endif +#ifndef MINIMP3_NOSKIP_APEV2 + if (buf_size > 32 && !memcmp(buf + buf_size - 32, "APETAGEX", 8)) + { + buf_size -= 32; + const uint8_t *tag = buf + buf_size + 8 + 4; + uint32_t tag_size = (uint32_t)(tag[3] << 24) | (tag[2] << 16) | (tag[1] << 8) | tag[0]; + if (buf_size >= tag_size) + buf_size -= tag_size; + } +#endif + *pbuf_size = buf_size; +} + +static size_t mp3dec_skip_id3v2(const uint8_t *buf, size_t buf_size) +{ +#define MINIMP3_ID3_DETECT_SIZE 10 +#ifndef MINIMP3_NOSKIP_ID3V2 + if (buf_size >= MINIMP3_ID3_DETECT_SIZE && !memcmp(buf, "ID3", 3) && !((buf[5] & 15) || (buf[6] & 0x80) || (buf[7] & 0x80) || (buf[8] & 0x80) || (buf[9] & 0x80))) + { + size_t id3v2size = (((buf[6] & 0x7f) << 21) | ((buf[7] & 0x7f) << 14) | ((buf[8] & 0x7f) << 7) | (buf[9] & 0x7f)) + 10; + if ((buf[5] & 16)) + id3v2size += 10; /* footer */ + return id3v2size; + } +#endif + return 0; +} + +static void mp3dec_skip_id3(const uint8_t **pbuf, size_t *pbuf_size) +{ + uint8_t *buf = (uint8_t *)(*pbuf); + size_t buf_size = *pbuf_size; + size_t id3v2size = mp3dec_skip_id3v2(buf, buf_size); + if (id3v2size) + { + if (id3v2size >= buf_size) + id3v2size = buf_size; + buf += id3v2size; + buf_size -= id3v2size; + } + mp3dec_skip_id3v1(buf, &buf_size); + *pbuf = (const uint8_t *)buf; + *pbuf_size = buf_size; +} + +static int mp3dec_check_vbrtag(const uint8_t *frame, int frame_size, uint32_t *frames, int *delay, int *padding) +{ + static const char g_xing_tag[4] = { 'X', 'i', 'n', 'g' }; + static const char g_info_tag[4] = { 'I', 'n', 'f', 'o' }; +#define FRAMES_FLAG 1 +#define BYTES_FLAG 2 +#define TOC_FLAG 4 +#define VBR_SCALE_FLAG 8 + /* Side info offsets after header: + / Mono Stereo + / MPEG1 17 32 + / MPEG2 & 2.5 9 17*/ + bs_t bs[1]; + L3_gr_info_t gr_info[4]; + bs_init(bs, frame + HDR_SIZE, frame_size - HDR_SIZE); + if (HDR_IS_CRC(frame)) + get_bits(bs, 16); + if (L3_read_side_info(bs, gr_info, frame) < 0) + return 0; /* side info corrupted */ + + const uint8_t *tag = frame + HDR_SIZE + bs->pos/8; + if (memcmp(g_xing_tag, tag, 4) && memcmp(g_info_tag, tag, 4)) + return 0; + int flags = tag[7]; + if (!((flags & FRAMES_FLAG))) + return -1; + tag += 8; + *frames = (uint32_t)(tag[0] << 24) | (tag[1] << 16) | (tag[2] << 8) | tag[3]; + tag += 4; + if (flags & BYTES_FLAG) + tag += 4; + if (flags & TOC_FLAG) + tag += 100; + if (flags & VBR_SCALE_FLAG) + tag += 4; + *delay = *padding = 0; + if (*tag) + { /* extension, LAME, Lavc, etc. Should be the same structure. */ + tag += 21; + if (tag - frame + 14 >= frame_size) + return 0; + *delay = ((tag[0] << 4) | (tag[1] >> 4)) + (528 + 1); + *padding = (((tag[1] & 0xF) << 8) | tag[2]) - (528 + 1); + } + return 1; +} + +int mp3dec_detect_buf(const uint8_t *buf, size_t buf_size) +{ + return mp3dec_detect_cb(0, (uint8_t *)buf, buf_size); +} + +int mp3dec_detect_cb(mp3dec_io_t *io, uint8_t *buf, size_t buf_size) +{ + if (!buf || (size_t)-1 == buf_size || (io && buf_size < MINIMP3_BUF_SIZE)) + return MP3D_E_PARAM; + size_t filled = buf_size; + if (io) + { + if (io->seek(0, io->seek_data)) + return MP3D_E_IOERROR; + filled = io->read(buf, MINIMP3_ID3_DETECT_SIZE, io->read_data); + if (filled > MINIMP3_ID3_DETECT_SIZE) + return MP3D_E_IOERROR; + } + if (filled < MINIMP3_ID3_DETECT_SIZE) + return MP3D_E_USER; /* too small, can't be mp3/mpa */ + if (mp3dec_skip_id3v2(buf, filled)) + return 0; /* id3v2 tag is enough evidence */ + if (io) + { + size_t readed = io->read(buf + MINIMP3_ID3_DETECT_SIZE, buf_size - MINIMP3_ID3_DETECT_SIZE, io->read_data); + if (readed > (buf_size - MINIMP3_ID3_DETECT_SIZE)) + return MP3D_E_IOERROR; + filled += readed; + if (filled < MINIMP3_BUF_SIZE) + mp3dec_skip_id3v1(buf, &filled); + } else + { + mp3dec_skip_id3v1(buf, &filled); + if (filled > MINIMP3_BUF_SIZE) + filled = MINIMP3_BUF_SIZE; + } + int free_format_bytes, frame_size; + mp3d_find_frame(buf, filled, &free_format_bytes, &frame_size); + if (frame_size) + return 0; /* MAX_FRAME_SYNC_MATCHES consecutive frames found */ + return MP3D_E_USER; +} + +int mp3dec_load_buf(mp3dec_t *dec, const uint8_t *buf, size_t buf_size, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data) +{ + return mp3dec_load_cb(dec, 0, (uint8_t *)buf, buf_size, info, progress_cb, user_data); +} + +int mp3dec_load_cb(mp3dec_t *dec, mp3dec_io_t *io, uint8_t *buf, size_t buf_size, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data) +{ + if (!dec || !buf || !info || (size_t)-1 == buf_size || (io && buf_size < MINIMP3_BUF_SIZE)) + return MP3D_E_PARAM; + uint64_t detected_samples = 0; + size_t orig_buf_size = buf_size; + int to_skip = 0; + mp3dec_frame_info_t frame_info; + memset(info, 0, sizeof(*info)); + memset(&frame_info, 0, sizeof(frame_info)); + + /* skip id3 */ + size_t filled = 0, consumed = 0; + int eof = 0, ret = 0; + if (io) + { + if (io->seek(0, io->seek_data)) + return MP3D_E_IOERROR; + filled = io->read(buf, MINIMP3_ID3_DETECT_SIZE, io->read_data); + if (filled > MINIMP3_ID3_DETECT_SIZE) + return MP3D_E_IOERROR; + if (MINIMP3_ID3_DETECT_SIZE != filled) + return 0; + size_t id3v2size = mp3dec_skip_id3v2(buf, filled); + if (id3v2size) + { + if (io->seek(id3v2size, io->seek_data)) + return MP3D_E_IOERROR; + filled = io->read(buf, buf_size, io->read_data); + if (filled > buf_size) + return MP3D_E_IOERROR; + } else + { + size_t readed = io->read(buf + MINIMP3_ID3_DETECT_SIZE, buf_size - MINIMP3_ID3_DETECT_SIZE, io->read_data); + if (readed > (buf_size - MINIMP3_ID3_DETECT_SIZE)) + return MP3D_E_IOERROR; + filled += readed; + } + if (filled < MINIMP3_BUF_SIZE) + mp3dec_skip_id3v1(buf, &filled); + } else + { + mp3dec_skip_id3((const uint8_t **)&buf, &buf_size); + if (!buf_size) + return 0; + } + /* try to make allocation size assumption by first frame or vbr tag */ + mp3dec_init(dec); + int samples; + do + { + uint32_t frames; + int i, delay, padding, free_format_bytes = 0, frame_size = 0; + const uint8_t *hdr; + if (io) + { + if (!eof && filled - consumed < MINIMP3_BUF_SIZE) + { /* keep minimum 10 consecutive mp3 frames (~16KB) worst case */ + memmove(buf, buf + consumed, filled - consumed); + filled -= consumed; + consumed = 0; + size_t readed = io->read(buf + filled, buf_size - filled, io->read_data); + if (readed > (buf_size - filled)) + return MP3D_E_IOERROR; + if (readed != (buf_size - filled)) + eof = 1; + filled += readed; + if (eof) + mp3dec_skip_id3v1(buf, &filled); + } + i = mp3d_find_frame(buf + consumed, filled - consumed, &free_format_bytes, &frame_size); + consumed += i; + hdr = buf + consumed; + } else + { + i = mp3d_find_frame(buf, buf_size, &free_format_bytes, &frame_size); + buf += i; + buf_size -= i; + hdr = buf; + } + if (i && !frame_size) + continue; + if (!frame_size) + return 0; + frame_info.channels = HDR_IS_MONO(hdr) ? 1 : 2; + frame_info.hz = hdr_sample_rate_hz(hdr); + frame_info.layer = 4 - HDR_GET_LAYER(hdr); + frame_info.bitrate_kbps = hdr_bitrate_kbps(hdr); + frame_info.frame_bytes = frame_size; + samples = hdr_frame_samples(hdr)*frame_info.channels; + if (3 != frame_info.layer) + break; + int ret = mp3dec_check_vbrtag(hdr, frame_size, &frames, &delay, &padding); + if (ret > 0) + { + padding *= frame_info.channels; + to_skip = delay*frame_info.channels; + detected_samples = samples*(uint64_t)frames; + if (detected_samples >= (uint64_t)to_skip) + detected_samples -= to_skip; + if (padding > 0 && detected_samples >= (uint64_t)padding) + detected_samples -= padding; + if (!detected_samples) + return 0; + } + if (ret) + { + if (io) + { + consumed += frame_size; + } else + { + buf += frame_size; + buf_size -= frame_size; + } + } + break; + } while(1); + size_t allocated = MINIMP3_MAX_SAMPLES_PER_FRAME*sizeof(mp3d_sample_t); + if (detected_samples) + allocated += detected_samples*sizeof(mp3d_sample_t); + else + allocated += (buf_size/frame_info.frame_bytes)*samples*sizeof(mp3d_sample_t); + info->buffer = (mp3d_sample_t*)malloc(allocated); + if (!info->buffer) + return MP3D_E_MEMORY; + /* save info */ + info->channels = frame_info.channels; + info->hz = frame_info.hz; + info->layer = frame_info.layer; + /* decode all frames */ + size_t avg_bitrate_kbps = 0, frames = 0; + do + { + if ((allocated - info->samples*sizeof(mp3d_sample_t)) < MINIMP3_MAX_SAMPLES_PER_FRAME*sizeof(mp3d_sample_t)) + { + allocated *= 2; + mp3d_sample_t *alloc_buf = (mp3d_sample_t*)realloc(info->buffer, allocated); + if (!alloc_buf) + return MP3D_E_MEMORY; + info->buffer = alloc_buf; + } + if (io) + { + if (!eof && filled - consumed < MINIMP3_BUF_SIZE) + { /* keep minimum 10 consecutive mp3 frames (~16KB) worst case */ + memmove(buf, buf + consumed, filled - consumed); + filled -= consumed; + consumed = 0; + size_t readed = io->read(buf + filled, buf_size - filled, io->read_data); + if (readed != (buf_size - filled)) + eof = 1; + filled += readed; + if (eof) + mp3dec_skip_id3v1(buf, &filled); + } + samples = mp3dec_decode_frame(dec, buf + consumed, filled - consumed, info->buffer + info->samples, &frame_info); + consumed += frame_info.frame_bytes; + } else + { + samples = mp3dec_decode_frame(dec, buf, MINIMP3_MIN(buf_size, (size_t)INT_MAX), info->buffer + info->samples, &frame_info); + buf += frame_info.frame_bytes; + buf_size -= frame_info.frame_bytes; + } + if (samples) + { + if (info->hz != frame_info.hz || info->layer != frame_info.layer) + { + ret = MP3D_E_DECODE; + break; + } + if (info->channels && info->channels != frame_info.channels) + { +#ifdef MINIMP3_ALLOW_MONO_STEREO_TRANSITION + info->channels = 0; /* mark file with mono-stereo transition */ +#else + ret = MP3D_E_DECODE; + break; +#endif + } + samples *= frame_info.channels; + if (to_skip) + { + size_t skip = MINIMP3_MIN(samples, to_skip); + to_skip -= skip; + samples -= skip; + memmove(info->buffer, info->buffer + skip, samples*sizeof(mp3d_sample_t)); + } + info->samples += samples; + avg_bitrate_kbps += frame_info.bitrate_kbps; + frames++; + if (progress_cb) + { + ret = progress_cb(user_data, orig_buf_size, orig_buf_size - buf_size, &frame_info); + if (ret) + break; + } + } + } while (frame_info.frame_bytes); + if (detected_samples && info->samples > detected_samples) + info->samples = detected_samples; /* cut padding */ + /* reallocate to normal buffer size */ + if (allocated != info->samples*sizeof(mp3d_sample_t)) + { + mp3d_sample_t *alloc_buf = (mp3d_sample_t*)realloc(info->buffer, info->samples*sizeof(mp3d_sample_t)); + if (!alloc_buf && info->samples) + return MP3D_E_MEMORY; + info->buffer = alloc_buf; + } + if (frames) + info->avg_bitrate_kbps = avg_bitrate_kbps/frames; + return ret; +} + +int mp3dec_iterate_buf(const uint8_t *buf, size_t buf_size, MP3D_ITERATE_CB callback, void *user_data) +{ + const uint8_t *orig_buf = buf; + if (!buf || (size_t)-1 == buf_size || !callback) + return MP3D_E_PARAM; + /* skip id3 */ + mp3dec_skip_id3(&buf, &buf_size); + if (!buf_size) + return 0; + mp3dec_frame_info_t frame_info; + memset(&frame_info, 0, sizeof(frame_info)); + do + { + int free_format_bytes = 0, frame_size = 0, ret; + int i = mp3d_find_frame(buf, buf_size, &free_format_bytes, &frame_size); + buf += i; + buf_size -= i; + if (i && !frame_size) + continue; + if (!frame_size) + break; + const uint8_t *hdr = buf; + frame_info.channels = HDR_IS_MONO(hdr) ? 1 : 2; + frame_info.hz = hdr_sample_rate_hz(hdr); + frame_info.layer = 4 - HDR_GET_LAYER(hdr); + frame_info.bitrate_kbps = hdr_bitrate_kbps(hdr); + frame_info.frame_bytes = frame_size; + + if (callback) + { + if ((ret = callback(user_data, hdr, frame_size, free_format_bytes, buf_size, hdr - orig_buf, &frame_info))) + return ret; + } + buf += frame_size; + buf_size -= frame_size; + } while (1); + return 0; +} + +int mp3dec_iterate_cb(mp3dec_io_t *io, uint8_t *buf, size_t buf_size, MP3D_ITERATE_CB callback, void *user_data) +{ + if (!io || !buf || (size_t)-1 == buf_size || buf_size < MINIMP3_BUF_SIZE || !callback) + return MP3D_E_PARAM; + size_t filled = io->read(buf, MINIMP3_ID3_DETECT_SIZE, io->read_data), consumed = 0; + uint64_t readed = 0; + mp3dec_frame_info_t frame_info; + int eof = 0; + memset(&frame_info, 0, sizeof(frame_info)); + if (filled > MINIMP3_ID3_DETECT_SIZE) + return MP3D_E_IOERROR; + if (MINIMP3_ID3_DETECT_SIZE != filled) + return 0; + size_t id3v2size = mp3dec_skip_id3v2(buf, filled); + if (id3v2size) + { + if (io->seek(id3v2size, io->seek_data)) + return MP3D_E_IOERROR; + filled = io->read(buf, buf_size, io->read_data); + if (filled > buf_size) + return MP3D_E_IOERROR; + readed += id3v2size; + } else + { + size_t readed = io->read(buf + MINIMP3_ID3_DETECT_SIZE, buf_size - MINIMP3_ID3_DETECT_SIZE, io->read_data); + if (readed > (buf_size - MINIMP3_ID3_DETECT_SIZE)) + return MP3D_E_IOERROR; + filled += readed; + } + if (filled < MINIMP3_BUF_SIZE) + mp3dec_skip_id3v1(buf, &filled); + do + { + int free_format_bytes = 0, frame_size = 0, ret; + int i = mp3d_find_frame(buf + consumed, filled - consumed, &free_format_bytes, &frame_size); + if (i && !frame_size) + { + consumed += i; + continue; + } + if (!frame_size) + break; + const uint8_t *hdr = buf + consumed + i; + frame_info.channels = HDR_IS_MONO(hdr) ? 1 : 2; + frame_info.hz = hdr_sample_rate_hz(hdr); + frame_info.layer = 4 - HDR_GET_LAYER(hdr); + frame_info.bitrate_kbps = hdr_bitrate_kbps(hdr); + frame_info.frame_bytes = frame_size; + + readed += i; + if (callback) + { + if ((ret = callback(user_data, hdr, frame_size, free_format_bytes, filled - consumed, readed, &frame_info))) + return ret; + } + readed += frame_size; + consumed += i + frame_size; + if (!eof && filled - consumed < MINIMP3_BUF_SIZE) + { /* keep minimum 10 consecutive mp3 frames (~16KB) worst case */ + memmove(buf, buf + consumed, filled - consumed); + filled -= consumed; + consumed = 0; + size_t readed = io->read(buf + filled, buf_size - filled, io->read_data); + if (readed > (buf_size - filled)) + return MP3D_E_IOERROR; + if (readed != (buf_size - filled)) + eof = 1; + filled += readed; + if (eof) + mp3dec_skip_id3v1(buf, &filled); + } + } while (1); + return 0; +} + +static int mp3dec_load_index(void *user_data, const uint8_t *frame, int frame_size, int free_format_bytes, size_t buf_size, uint64_t offset, mp3dec_frame_info_t *info) +{ + mp3dec_frame_t *idx_frame; + mp3dec_ex_t *dec = (mp3dec_ex_t *)user_data; + if (!dec->index.frames && !dec->start_offset) + { /* detect VBR tag and try to avoid full scan */ + uint32_t frames; + int delay, padding; + dec->info = *info; + dec->start_offset = dec->offset = offset; + dec->end_offset = offset + buf_size; + dec->free_format_bytes = free_format_bytes; /* should not change */ + if (3 == dec->info.layer) + { + int ret = mp3dec_check_vbrtag(frame, frame_size, &frames, &delay, &padding); + if (ret) + dec->start_offset = dec->offset = offset + frame_size; + if (ret > 0) + { + padding *= info->channels; + dec->start_delay = dec->to_skip = delay*info->channels; + dec->samples = hdr_frame_samples(frame)*info->channels*(uint64_t)frames; + if (dec->samples >= (uint64_t)dec->start_delay) + dec->samples -= dec->start_delay; + if (padding > 0 && dec->samples >= (uint64_t)padding) + dec->samples -= padding; + dec->detected_samples = dec->samples; + dec->vbr_tag_found = 1; + return MP3D_E_USER; + } else if (ret < 0) + return 0; + } + } + if (dec->flags & MP3D_DO_NOT_SCAN) + return MP3D_E_USER; + if (dec->index.num_frames + 1 > dec->index.capacity) + { + if (!dec->index.capacity) + dec->index.capacity = 4096; + else + dec->index.capacity *= 2; + mp3dec_frame_t *alloc_buf = (mp3dec_frame_t *)realloc((void*)dec->index.frames, sizeof(mp3dec_frame_t)*dec->index.capacity); + if (!alloc_buf) + return MP3D_E_MEMORY; + dec->index.frames = alloc_buf; + } + idx_frame = &dec->index.frames[dec->index.num_frames++]; + idx_frame->offset = offset; + idx_frame->sample = dec->samples; + if (!dec->buffer_samples && dec->index.num_frames < 256) + { /* for some cutted mp3 frames, bit-reservoir not filled and decoding can't be started from first frames */ + /* try to decode up to 255 first frames till samples starts to decode */ + dec->buffer_samples = mp3dec_decode_frame(&dec->mp3d, frame, MINIMP3_MIN(buf_size, (size_t)INT_MAX), dec->buffer, info); + dec->samples += dec->buffer_samples*info->channels; + } else + dec->samples += hdr_frame_samples(frame)*info->channels; + return 0; +} + +int mp3dec_ex_open_buf(mp3dec_ex_t *dec, const uint8_t *buf, size_t buf_size, int flags) +{ + if (!dec || !buf || (size_t)-1 == buf_size || (flags & (~MP3D_FLAGS_MASK))) + return MP3D_E_PARAM; + memset(dec, 0, sizeof(*dec)); + dec->file.buffer = buf; + dec->file.size = buf_size; + dec->flags = flags; + mp3dec_init(&dec->mp3d); + int ret = mp3dec_iterate_buf(dec->file.buffer, dec->file.size, mp3dec_load_index, dec); + if (ret && MP3D_E_USER != ret) + return ret; + mp3dec_init(&dec->mp3d); + dec->buffer_samples = 0; + dec->indexes_built = !(dec->vbr_tag_found || (flags & MP3D_DO_NOT_SCAN)); + dec->flags &= (~MP3D_DO_NOT_SCAN); + return 0; +} + +#ifndef MINIMP3_SEEK_IDX_LINEAR_SEARCH +static size_t mp3dec_idx_binary_search(mp3dec_index_t *idx, uint64_t position) +{ + size_t end = idx->num_frames, start = 0, index = 0; + while (start <= end) + { + size_t mid = (start + end) / 2; + if (idx->frames[mid].sample >= position) + { /* move left side. */ + if (idx->frames[mid].sample == position) + return mid; + end = mid - 1; + } else + { /* move to right side */ + index = mid; + start = mid + 1; + if (start == idx->num_frames) + break; + } + } + return index; +} +#endif + +int mp3dec_ex_seek(mp3dec_ex_t *dec, uint64_t position) +{ + size_t i; + if (!dec) + return MP3D_E_PARAM; + if (!(dec->flags & MP3D_SEEK_TO_SAMPLE)) + { + if (dec->io) + { + dec->offset = position; + } else + { + dec->offset = MINIMP3_MIN(position, dec->file.size); + } + dec->cur_sample = 0; + goto do_exit; + } + dec->cur_sample = position; + position += dec->start_delay; + if (0 == position) + { /* optimize seek to zero, no index needed */ +seek_zero: + dec->offset = dec->start_offset; + dec->to_skip = 0; + goto do_exit; + } + if (!dec->indexes_built) + { /* no index created yet (vbr tag used to calculate track length or MP3D_DO_NOT_SCAN open flag used) */ + dec->indexes_built = 1; + dec->samples = 0; + dec->buffer_samples = 0; + if (dec->io) + { + if (dec->io->seek(dec->start_offset, dec->io->seek_data)) + return MP3D_E_IOERROR; + int ret = mp3dec_iterate_cb(dec->io, (uint8_t *)dec->file.buffer, dec->file.size, mp3dec_load_index, dec); + if (ret && MP3D_E_USER != ret) + return ret; + } else + { + int ret = mp3dec_iterate_buf(dec->file.buffer + dec->start_offset, dec->file.size - dec->start_offset, mp3dec_load_index, dec); + if (ret && MP3D_E_USER != ret) + return ret; + } + for (i = 0; i < dec->index.num_frames; i++) + dec->index.frames[i].offset += dec->start_offset; + dec->samples = dec->detected_samples; + } + if (!dec->index.frames) + goto seek_zero; /* no frames in file - seek to zero */ +#ifdef MINIMP3_SEEK_IDX_LINEAR_SEARCH + for (i = 0; i < dec->index.num_frames; i++) + { + if (dec->index.frames[i].sample >= position) + break; + } +#else + i = mp3dec_idx_binary_search(&dec->index, position); +#endif + if (i) + { + int to_fill_bytes = 511; + int skip_frames = MINIMP3_PREDECODE_FRAMES +#ifdef MINIMP3_SEEK_IDX_LINEAR_SEARCH + + ((dec->index.frames[i].sample == position) ? 0 : 1) +#endif + ; + i -= MINIMP3_MIN(i, (size_t)skip_frames); + if (3 == dec->info.layer) + { + while (i && to_fill_bytes) + { /* make sure bit-reservoir is filled when we start decoding */ + bs_t bs[1]; + L3_gr_info_t gr_info[4]; + int frame_bytes, frame_size; + const uint8_t *hdr; + if (dec->io) + { + hdr = dec->file.buffer; + if (dec->io->seek(dec->index.frames[i - 1].offset, dec->io->seek_data)) + return MP3D_E_IOERROR; + size_t readed = dec->io->read((uint8_t *)hdr, HDR_SIZE, dec->io->read_data); + if (readed != HDR_SIZE) + return MP3D_E_IOERROR; + frame_size = hdr_frame_bytes(hdr, dec->free_format_bytes) + hdr_padding(hdr); + readed = dec->io->read((uint8_t *)hdr + HDR_SIZE, frame_size - HDR_SIZE, dec->io->read_data); + if (readed != (size_t)(frame_size - HDR_SIZE)) + return MP3D_E_IOERROR; + bs_init(bs, hdr + HDR_SIZE, frame_size - HDR_SIZE); + } else + { + hdr = dec->file.buffer + dec->index.frames[i - 1].offset; + frame_size = hdr_frame_bytes(hdr, dec->free_format_bytes) + hdr_padding(hdr); + bs_init(bs, hdr + HDR_SIZE, frame_size - HDR_SIZE); + } + if (HDR_IS_CRC(hdr)) + get_bits(bs, 16); + i--; + if (L3_read_side_info(bs, gr_info, hdr) < 0) + break; /* frame not decodable, we can start from here */ + frame_bytes = (bs->limit - bs->pos)/8; + to_fill_bytes -= MINIMP3_MIN(to_fill_bytes, frame_bytes); + } + } + } + dec->offset = dec->index.frames[i].offset; + dec->to_skip = position - dec->index.frames[i].sample; + while ((i + 1) < dec->index.num_frames && !dec->index.frames[i].sample && !dec->index.frames[i + 1].sample) + { /* skip not decodable first frames */ + const uint8_t *hdr; + if (dec->io) + { + hdr = dec->file.buffer; + if (dec->io->seek(dec->index.frames[i].offset, dec->io->seek_data)) + return MP3D_E_IOERROR; + size_t readed = dec->io->read((uint8_t *)hdr, HDR_SIZE, dec->io->read_data); + if (readed != HDR_SIZE) + return MP3D_E_IOERROR; + } else + hdr = dec->file.buffer + dec->index.frames[i].offset; + dec->to_skip += hdr_frame_samples(hdr)*dec->info.channels; + i++; + } +do_exit: + if (dec->io) + { + if (dec->io->seek(dec->offset, dec->io->seek_data)) + return MP3D_E_IOERROR; + } + dec->buffer_samples = 0; + dec->buffer_consumed = 0; + dec->input_consumed = 0; + dec->input_filled = 0; + dec->last_error = 0; + mp3dec_init(&dec->mp3d); + return 0; +} + +size_t mp3dec_ex_read_frame(mp3dec_ex_t *dec, mp3d_sample_t **buf, mp3dec_frame_info_t *frame_info, size_t max_samples) +{ + if (!dec || !buf || !frame_info) + { + if (dec) + dec->last_error = MP3D_E_PARAM; + return 0; + } + if (dec->detected_samples && dec->cur_sample >= dec->detected_samples) + return 0; /* at end of stream */ + if (dec->last_error) + return 0; /* error eof state, seek can reset it */ + *buf = NULL; + uint64_t end_offset = dec->end_offset ? dec->end_offset : dec->file.size; + int eof = 0; + while (dec->buffer_consumed == dec->buffer_samples) + { + const uint8_t *dec_buf; + if (dec->io) + { + if (!eof && (dec->input_filled - dec->input_consumed) < MINIMP3_BUF_SIZE) + { /* keep minimum 10 consecutive mp3 frames (~16KB) worst case */ + memmove((uint8_t*)dec->file.buffer, (uint8_t*)dec->file.buffer + dec->input_consumed, dec->input_filled - dec->input_consumed); + dec->input_filled -= dec->input_consumed; + dec->input_consumed = 0; + size_t readed = dec->io->read((uint8_t*)dec->file.buffer + dec->input_filled, dec->file.size - dec->input_filled, dec->io->read_data); + if (readed > (dec->file.size - dec->input_filled)) + { + dec->last_error = MP3D_E_IOERROR; + readed = 0; + } + if (readed != (dec->file.size - dec->input_filled)) + eof = 1; + dec->input_filled += readed; + if (eof) + mp3dec_skip_id3v1((uint8_t*)dec->file.buffer, &dec->input_filled); + } + dec_buf = dec->file.buffer + dec->input_consumed; + if (!(dec->input_filled - dec->input_consumed)) + return 0; + dec->buffer_samples = mp3dec_decode_frame(&dec->mp3d, dec_buf, dec->input_filled - dec->input_consumed, dec->buffer, frame_info); + dec->input_consumed += frame_info->frame_bytes; + } else + { + dec_buf = dec->file.buffer + dec->offset; + uint64_t buf_size = end_offset - dec->offset; + if (!buf_size) + return 0; + dec->buffer_samples = mp3dec_decode_frame(&dec->mp3d, dec_buf, MINIMP3_MIN(buf_size, (uint64_t)INT_MAX), dec->buffer, frame_info); + } + dec->buffer_consumed = 0; + if (dec->info.hz != frame_info->hz || dec->info.layer != frame_info->layer) + { +return_e_decode: + dec->last_error = MP3D_E_DECODE; + return 0; + } + if (dec->buffer_samples) + { + dec->buffer_samples *= frame_info->channels; + if (dec->to_skip) + { + size_t skip = MINIMP3_MIN(dec->buffer_samples, dec->to_skip); + dec->buffer_consumed += skip; + dec->to_skip -= skip; + } + if ( +#ifdef MINIMP3_ALLOW_MONO_STEREO_TRANSITION + !(dec->flags & MP3D_ALLOW_MONO_STEREO_TRANSITION) && +#endif + dec->buffer_consumed != dec->buffer_samples && dec->info.channels != frame_info->channels) + { + goto return_e_decode; + } + } else if (dec->to_skip) + { /* In mp3 decoding not always can start decode from any frame because of bit reservoir, + count skip samples for such frames */ + int frame_samples = hdr_frame_samples(dec_buf)*frame_info->channels; + dec->to_skip -= MINIMP3_MIN(frame_samples, dec->to_skip); + } + dec->offset += frame_info->frame_bytes; + } + size_t out_samples = MINIMP3_MIN((size_t)(dec->buffer_samples - dec->buffer_consumed), max_samples); + if (dec->detected_samples) + { /* count decoded samples to properly cut padding */ + if (dec->cur_sample + out_samples >= dec->detected_samples) + out_samples = dec->detected_samples - dec->cur_sample; + } + dec->cur_sample += out_samples; + *buf = dec->buffer + dec->buffer_consumed; + dec->buffer_consumed += out_samples; + return out_samples; +} + +size_t mp3dec_ex_read(mp3dec_ex_t *dec, mp3d_sample_t *buf, size_t samples) +{ + if (!dec || !buf) + { + if (dec) + dec->last_error = MP3D_E_PARAM; + return 0; + } + mp3dec_frame_info_t frame_info; + memset(&frame_info, 0, sizeof(frame_info)); + size_t samples_requested = samples; + while (samples) + { + mp3d_sample_t *buf_frame = NULL; + size_t read_samples = mp3dec_ex_read_frame(dec, &buf_frame, &frame_info, samples); + if (!read_samples) + { + break; + } + memcpy(buf, buf_frame, read_samples * sizeof(mp3d_sample_t)); + buf += read_samples; + samples -= read_samples; + } + return samples_requested - samples; +} + +int mp3dec_ex_open_cb(mp3dec_ex_t *dec, mp3dec_io_t *io, int flags) +{ + if (!dec || !io || (flags & (~MP3D_FLAGS_MASK))) + return MP3D_E_PARAM; + memset(dec, 0, sizeof(*dec)); +#ifdef MINIMP3_HAVE_RING + int ret; + if (ret = mp3dec_open_ring(&dec->file, MINIMP3_IO_SIZE)) + return ret; +#else + dec->file.size = MINIMP3_IO_SIZE; + dec->file.buffer = (const uint8_t*)malloc(dec->file.size); + if (!dec->file.buffer) + return MP3D_E_MEMORY; +#endif + dec->flags = flags; + dec->io = io; + mp3dec_init(&dec->mp3d); + if (io->seek(0, io->seek_data)) + return MP3D_E_IOERROR; + int ret = mp3dec_iterate_cb(io, (uint8_t *)dec->file.buffer, dec->file.size, mp3dec_load_index, dec); + if (ret && MP3D_E_USER != ret) + return ret; + if (dec->io->seek(dec->start_offset, dec->io->seek_data)) + return MP3D_E_IOERROR; + mp3dec_init(&dec->mp3d); + dec->buffer_samples = 0; + dec->indexes_built = !(dec->vbr_tag_found || (flags & MP3D_DO_NOT_SCAN)); + dec->flags &= (~MP3D_DO_NOT_SCAN); + return 0; +} + + +#ifndef MINIMP3_NO_STDIO + +#if defined(__linux__) || defined(__FreeBSD__) +#include <errno.h> +#include <sys/mman.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <unistd.h> +#include <fcntl.h> +#if !defined(_GNU_SOURCE) +#include <sys/ipc.h> +#include <sys/shm.h> +#endif +#if !defined(MAP_POPULATE) && defined(__linux__) +#define MAP_POPULATE 0x08000 +#elif !defined(MAP_POPULATE) +#define MAP_POPULATE 0 +#endif + +static void mp3dec_close_file(mp3dec_map_info_t *map_info) +{ + if (map_info->buffer && MAP_FAILED != map_info->buffer) + munmap((void *)map_info->buffer, map_info->size); + map_info->buffer = 0; + map_info->size = 0; +} + +static int mp3dec_open_file(const char *file_name, mp3dec_map_info_t *map_info) +{ + if (!file_name) + return MP3D_E_PARAM; + int file; + struct stat st; + memset(map_info, 0, sizeof(*map_info)); +retry_open: + file = open(file_name, O_RDONLY); + if (file < 0 && (errno == EAGAIN || errno == EINTR)) + goto retry_open; + if (file < 0 || fstat(file, &st) < 0) + { + close(file); + return MP3D_E_IOERROR; + } + + map_info->size = st.st_size; +retry_mmap: + map_info->buffer = (const uint8_t *)mmap(NULL, st.st_size, PROT_READ, MAP_PRIVATE | MAP_POPULATE, file, 0); + if (MAP_FAILED == map_info->buffer && (errno == EAGAIN || errno == EINTR)) + goto retry_mmap; + close(file); + if (MAP_FAILED == map_info->buffer) + return MP3D_E_IOERROR; + return 0; +} + +#if MINIMP3_ENABLE_RING && defined(__linux__) && defined(_GNU_SOURCE) +#define MINIMP3_HAVE_RING +static void mp3dec_close_ring(mp3dec_map_info_t *map_info) +{ +#if defined(__linux__) && defined(_GNU_SOURCE) + if (map_info->buffer && MAP_FAILED != map_info->buffer) + munmap((void *)map_info->buffer, map_info->size*2); +#else + if (map_info->buffer) + { + shmdt(map_info->buffer); + shmdt(map_info->buffer + map_info->size); + } +#endif + map_info->buffer = 0; + map_info->size = 0; +} + +static int mp3dec_open_ring(mp3dec_map_info_t *map_info, size_t size) +{ + int memfd, page_size; +#if defined(__linux__) && defined(_GNU_SOURCE) + void *buffer; + int res; +#endif + memset(map_info, 0, sizeof(*map_info)); + +#ifdef _SC_PAGESIZE + page_size = sysconf(_SC_PAGESIZE); +#else + page_size = getpagesize(); +#endif + map_info->size = (size + page_size - 1)/page_size*page_size; + +#if defined(__linux__) && defined(_GNU_SOURCE) + memfd = memfd_create("mp3_ring", 0); + if (memfd < 0) + return MP3D_E_MEMORY; + +retry_ftruncate: + res = ftruncate(memfd, map_info->size); + if (res && (errno == EAGAIN || errno == EINTR)) + goto retry_ftruncate; + if (res) + goto error; + +retry_mmap: + map_info->buffer = (const uint8_t *)mmap(NULL, map_info->size*2, PROT_NONE, MAP_ANONYMOUS | MAP_PRIVATE, -1, 0); + if (MAP_FAILED == map_info->buffer && (errno == EAGAIN || errno == EINTR)) + goto retry_mmap; + if (MAP_FAILED == map_info->buffer || !map_info->buffer) + goto error; +retry_mmap2: + buffer = mmap((void *)map_info->buffer, map_info->size, PROT_READ | PROT_WRITE, MAP_FIXED | MAP_SHARED, memfd, 0); + if (MAP_FAILED == map_info->buffer && (errno == EAGAIN || errno == EINTR)) + goto retry_mmap2; + if (MAP_FAILED == map_info->buffer || buffer != (void *)map_info->buffer) + goto error; +retry_mmap3: + buffer = mmap((void *)map_info->buffer + map_info->size, map_info->size, PROT_READ | PROT_WRITE, MAP_FIXED | MAP_SHARED, memfd, 0); + if (MAP_FAILED == map_info->buffer && (errno == EAGAIN || errno == EINTR)) + goto retry_mmap3; + if (MAP_FAILED == map_info->buffer || buffer != (void *)(map_info->buffer + map_info->size)) + goto error; + + close(memfd); + return 0; +error: + close(memfd); + mp3dec_close_ring(map_info); + return MP3D_E_MEMORY; +#else + memfd = shmget(IPC_PRIVATE, map_info->size, IPC_CREAT | 0700); + if (memfd < 0) + return MP3D_E_MEMORY; +retry_mmap: + map_info->buffer = (const uint8_t *)mmap(NULL, map_info->size*2, PROT_NONE, MAP_PRIVATE, -1, 0); + if (MAP_FAILED == map_info->buffer && (errno == EAGAIN || errno == EINTR)) + goto retry_mmap; + if (MAP_FAILED == map_info->buffer) + goto error; + if (map_info->buffer != shmat(memfd, map_info->buffer, 0)) + goto error; + if ((map_info->buffer + map_info->size) != shmat(memfd, map_info->buffer + map_info->size, 0)) + goto error; + if (shmctl(memfd, IPC_RMID, NULL) < 0) + return MP3D_E_MEMORY; + return 0; +error: + shmctl(memfd, IPC_RMID, NULL); + mp3dec_close_ring(map_info); + return MP3D_E_MEMORY; +#endif +} +#endif /*MINIMP3_ENABLE_RING*/ +#elif defined(_WIN32) +#include <windows.h> + +static void mp3dec_close_file(mp3dec_map_info_t *map_info) +{ + if (map_info->buffer) + UnmapViewOfFile(map_info->buffer); + map_info->buffer = 0; + map_info->size = 0; +} + +static int mp3dec_open_file_h(HANDLE file, mp3dec_map_info_t *map_info) +{ + memset(map_info, 0, sizeof(*map_info)); + + HANDLE mapping = NULL; + LARGE_INTEGER s; + s.LowPart = GetFileSize(file, (DWORD*)&s.HighPart); + if (s.LowPart == INVALID_FILE_SIZE && GetLastError() != NO_ERROR) + goto error; + map_info->size = s.QuadPart; + + mapping = CreateFileMapping(file, NULL, PAGE_READONLY, 0, 0, NULL); + if (!mapping) + goto error; + map_info->buffer = (const uint8_t*)MapViewOfFile(mapping, FILE_MAP_READ, 0, 0, s.QuadPart); + CloseHandle(mapping); + if (!map_info->buffer) + goto error; + + CloseHandle(file); + return 0; +error: + mp3dec_close_file(map_info); + CloseHandle(file); + return MP3D_E_IOERROR; +} + +static int mp3dec_open_file(const char *file_name, mp3dec_map_info_t *map_info) +{ + if (!file_name) + return MP3D_E_PARAM; + HANDLE file = CreateFileA(file_name, GENERIC_READ, FILE_SHARE_READ, 0, OPEN_EXISTING, 0, 0); + if (INVALID_HANDLE_VALUE == file) + return MP3D_E_IOERROR; + return mp3dec_open_file_h(file, map_info); +} + +static int mp3dec_open_file_w(const wchar_t *file_name, mp3dec_map_info_t *map_info) +{ + if (!file_name) + return MP3D_E_PARAM; + HANDLE file = CreateFileW(file_name, GENERIC_READ, FILE_SHARE_READ, 0, OPEN_EXISTING, 0, 0); + if (INVALID_HANDLE_VALUE == file) + return MP3D_E_IOERROR; + return mp3dec_open_file_h(file, map_info); +} +#else +#include <stdio.h> + +static void mp3dec_close_file(mp3dec_map_info_t *map_info) +{ + if (map_info->buffer) + free((void *)map_info->buffer); + map_info->buffer = 0; + map_info->size = 0; +} + +static int mp3dec_open_file(const char *file_name, mp3dec_map_info_t *map_info) +{ + if (!file_name) + return MP3D_E_PARAM; + memset(map_info, 0, sizeof(*map_info)); + FILE *file = fopen(file_name, "rb"); + if (!file) + return MP3D_E_IOERROR; + int res = MP3D_E_IOERROR; + long size = -1; + if (fseek(file, 0, SEEK_END)) + goto error; + size = ftell(file); + if (size < 0) + goto error; + map_info->size = (size_t)size; + if (fseek(file, 0, SEEK_SET)) + goto error; + map_info->buffer = (uint8_t *)malloc(map_info->size); + if (!map_info->buffer) + { + res = MP3D_E_MEMORY; + goto error; + } + if (fread((void *)map_info->buffer, 1, map_info->size, file) != map_info->size) + goto error; + fclose(file); + return 0; +error: + mp3dec_close_file(map_info); + fclose(file); + return res; +} +#endif + +static int mp3dec_detect_mapinfo(mp3dec_map_info_t *map_info) +{ + int ret = mp3dec_detect_buf(map_info->buffer, map_info->size); + mp3dec_close_file(map_info); + return ret; +} + +static int mp3dec_load_mapinfo(mp3dec_t *dec, mp3dec_map_info_t *map_info, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data) +{ + int ret = mp3dec_load_buf(dec, map_info->buffer, map_info->size, info, progress_cb, user_data); + mp3dec_close_file(map_info); + return ret; +} + +static int mp3dec_iterate_mapinfo(mp3dec_map_info_t *map_info, MP3D_ITERATE_CB callback, void *user_data) +{ + int ret = mp3dec_iterate_buf(map_info->buffer, map_info->size, callback, user_data); + mp3dec_close_file(map_info); + return ret; +} + +static int mp3dec_ex_open_mapinfo(mp3dec_ex_t *dec, int flags) +{ + int ret = mp3dec_ex_open_buf(dec, dec->file.buffer, dec->file.size, flags); + dec->is_file = 1; + if (ret) + mp3dec_ex_close(dec); + return ret; +} + +int mp3dec_detect(const char *file_name) +{ + int ret; + mp3dec_map_info_t map_info; + if ((ret = mp3dec_open_file(file_name, &map_info))) + return ret; + return mp3dec_detect_mapinfo(&map_info); +} + +int mp3dec_load(mp3dec_t *dec, const char *file_name, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data) +{ + int ret; + mp3dec_map_info_t map_info; + if ((ret = mp3dec_open_file(file_name, &map_info))) + return ret; + return mp3dec_load_mapinfo(dec, &map_info, info, progress_cb, user_data); +} + +int mp3dec_iterate(const char *file_name, MP3D_ITERATE_CB callback, void *user_data) +{ + int ret; + mp3dec_map_info_t map_info; + if ((ret = mp3dec_open_file(file_name, &map_info))) + return ret; + return mp3dec_iterate_mapinfo(&map_info, callback, user_data); +} + +int mp3dec_ex_open(mp3dec_ex_t *dec, const char *file_name, int flags) +{ + int ret; + if (!dec) + return MP3D_E_PARAM; + if ((ret = mp3dec_open_file(file_name, &dec->file))) + return ret; + return mp3dec_ex_open_mapinfo(dec, flags); +} + +void mp3dec_ex_close(mp3dec_ex_t *dec) +{ +#ifdef MINIMP3_HAVE_RING + if (dec->io) + mp3dec_close_ring(&dec->file); +#else + if (dec->io && dec->file.buffer) + free((void*)dec->file.buffer); +#endif + if (dec->is_file) + mp3dec_close_file(&dec->file); + if (dec->index.frames) + free(dec->index.frames); + memset(dec, 0, sizeof(*dec)); +} + +#ifdef _WIN32 +int mp3dec_detect_w(const wchar_t *file_name) +{ + int ret; + mp3dec_map_info_t map_info; + if ((ret = mp3dec_open_file_w(file_name, &map_info))) + return ret; + return mp3dec_detect_mapinfo(&map_info); +} + +int mp3dec_load_w(mp3dec_t *dec, const wchar_t *file_name, mp3dec_file_info_t *info, MP3D_PROGRESS_CB progress_cb, void *user_data) +{ + int ret; + mp3dec_map_info_t map_info; + if ((ret = mp3dec_open_file_w(file_name, &map_info))) + return ret; + return mp3dec_load_mapinfo(dec, &map_info, info, progress_cb, user_data); +} + +int mp3dec_iterate_w(const wchar_t *file_name, MP3D_ITERATE_CB callback, void *user_data) +{ + int ret; + mp3dec_map_info_t map_info; + if ((ret = mp3dec_open_file_w(file_name, &map_info))) + return ret; + return mp3dec_iterate_mapinfo(&map_info, callback, user_data); +} + +int mp3dec_ex_open_w(mp3dec_ex_t *dec, const wchar_t *file_name, int flags) +{ + int ret; + if ((ret = mp3dec_open_file_w(file_name, &dec->file))) + return ret; + return mp3dec_ex_open_mapinfo(dec, flags); +} +#endif +#else /* MINIMP3_NO_STDIO */ +void mp3dec_ex_close(mp3dec_ex_t *dec) +{ +#ifdef MINIMP3_HAVE_RING + if (dec->io) + mp3dec_close_ring(&dec->file); +#else + if (dec->io && dec->file.buffer) + free((void*)dec->file.buffer); +#endif + if (dec->index.frames) + free(dec->index.frames); + memset(dec, 0, sizeof(*dec)); +} +#endif + +#endif /*MINIMP3_IMPLEMENTATION*/ diff --git a/thirdparty/misc/easing_equations.cpp b/thirdparty/misc/easing_equations.cpp index bc84564b19..af48aaf079 100644 --- a/thirdparty/misc/easing_equations.cpp +++ b/thirdparty/misc/easing_equations.cpp @@ -188,7 +188,8 @@ static real_t out_in(real_t t, real_t b, real_t c, real_t d) { /////////////////////////////////////////////////////////////////////////// namespace cubic { static real_t in(real_t t, real_t b, real_t c, real_t d) { - return c * (t /= d) * t * t + b; + t /= d; + return c * t * t * t + b; } static real_t out(real_t t, real_t b, real_t c, real_t d) { @@ -197,8 +198,10 @@ static real_t out(real_t t, real_t b, real_t c, real_t d) { } static real_t in_out(real_t t, real_t b, real_t c, real_t d) { - if ((t /= d / 2) < 1) return c / 2 * t * t * t + b; - return c / 2 * ((t -= 2) * t * t + 2) + b; + t /= d / 2; + if (t < 1) return c / 2 * t * t * t + b; + t -= 2; + return c / 2 * (t * t * t + 2) + b; } static real_t out_in(real_t t, real_t b, real_t c, real_t d) { @@ -210,16 +213,22 @@ static real_t out_in(real_t t, real_t b, real_t c, real_t d) { /////////////////////////////////////////////////////////////////////////// namespace circ { static real_t in(real_t t, real_t b, real_t c, real_t d) { - return -c * (sqrt(1 - (t /= d) * t) - 1) + b; // TODO: ehrich: operation with t is undefined + t /= d; + return -c * (sqrt(1 - t * t) - 1) + b; } static real_t out(real_t t, real_t b, real_t c, real_t d) { - return c * sqrt(1 - (t = t / d - 1) * t) + b; // TODO: ehrich: operation with t is undefined + t = t / d - 1; + return c * sqrt(1 - t * t) + b; } static real_t in_out(real_t t, real_t b, real_t c, real_t d) { - if ((t /= d / 2) < 1) return -c / 2 * (sqrt(1 - t * t) - 1) + b; - return c / 2 * (sqrt(1 - t * (t -= 2)) + 1) + b; // TODO: ehrich: operation with t is undefined + t /= d / 2; + if (t < 1) { + return -c / 2 * (sqrt(1 - t * t) - 1) + b; + } + t -= 2; + return c / 2 * (sqrt(1 - t * t) + 1) + b; } static real_t out_in(real_t t, real_t b, real_t c, real_t d) { @@ -271,14 +280,16 @@ static real_t in(real_t t, real_t b, real_t c, real_t d) { static real_t out(real_t t, real_t b, real_t c, real_t d) { float s = 1.70158f; - return c * ((t = t / d - 1) * t * ((s + 1) * t + s) + 1) + b; // TODO: ehrich: operation with t is undefined + t = t / d - 1; + return c * (t * t * ((s + 1) * t + s) + 1) + b; } static real_t in_out(real_t t, real_t b, real_t c, real_t d) { - float s = 1.70158f; - if ((t /= d / 2) < 1) return c / 2 * (t * t * (((s *= (1.525f)) + 1) * t - s)) + b; // TODO: ehrich: operation with s is undefined - float postFix = t -= 2; - return c / 2 * ((postFix)*t * (((s *= (1.525f)) + 1) * t + s) + 2) + b; // TODO: ehrich: operation with s is undefined + float s = 1.70158f * 1.525f; + t /= d / 2; + if (t < 1) return c / 2 * (t * t * ((s + 1) * t - s)) + b; + t -= 2; + return c / 2 * (t * t * ((s + 1) * t + s) + 2) + b; } static real_t out_in(real_t t, real_t b, real_t c, real_t d) { diff --git a/thirdparty/misc/open-simplex-noise.c b/thirdparty/misc/open-simplex-noise.c index 88fbd3e51d..44a072cad1 100644 --- a/thirdparty/misc/open-simplex-noise.c +++ b/thirdparty/misc/open-simplex-noise.c @@ -100,27 +100,27 @@ static const signed char gradients4D[] = { -3, -1, -1, -1, -1, -3, -1, -1, -1, -1, -3, -1, -1, -1, -1, -3, }; -static double extrapolate2(struct osn_context *ctx, int xsb, int ysb, double dx, double dy) +static double extrapolate2(const struct osn_context *ctx, int xsb, int ysb, double dx, double dy) { - int16_t *perm = ctx->perm; + const int16_t *perm = ctx->perm; int index = perm[(perm[xsb & 0xFF] + ysb) & 0xFF] & 0x0E; return gradients2D[index] * dx + gradients2D[index + 1] * dy; } -static double extrapolate3(struct osn_context *ctx, int xsb, int ysb, int zsb, double dx, double dy, double dz) +static double extrapolate3(const struct osn_context *ctx, int xsb, int ysb, int zsb, double dx, double dy, double dz) { - int16_t *perm = ctx->perm; - int16_t *permGradIndex3D = ctx->permGradIndex3D; + const int16_t *perm = ctx->perm; + const int16_t *permGradIndex3D = ctx->permGradIndex3D; int index = permGradIndex3D[(perm[(perm[xsb & 0xFF] + ysb) & 0xFF] + zsb) & 0xFF]; return gradients3D[index] * dx + gradients3D[index + 1] * dy + gradients3D[index + 2] * dz; } -static double extrapolate4(struct osn_context *ctx, int xsb, int ysb, int zsb, int wsb, double dx, double dy, double dz, double dw) +static double extrapolate4(const struct osn_context *ctx, int xsb, int ysb, int zsb, int wsb, double dx, double dy, double dz, double dw) { - int16_t *perm = ctx->perm; + const int16_t *perm = ctx->perm; int index = perm[(perm[(perm[(perm[xsb & 0xFF] + ysb) & 0xFF] + zsb) & 0xFF] + wsb) & 0xFF] & 0xFC; return gradients4D[index] * dx + gradients4D[index + 1] * dy @@ -227,7 +227,7 @@ void open_simplex_noise_free(struct osn_context *ctx) // -- GODOT end -- /* 2D OpenSimplex (Simplectic) Noise. */ -double open_simplex_noise2(struct osn_context *ctx, double x, double y) +double open_simplex_noise2(const struct osn_context *ctx, double x, double y) { /* Place input coordinates onto grid. */ @@ -355,7 +355,7 @@ double open_simplex_noise2(struct osn_context *ctx, double x, double y) /* * 3D OpenSimplex (Simplectic) Noise */ -double open_simplex_noise3(struct osn_context *ctx, double x, double y, double z) +double open_simplex_noise3(const struct osn_context *ctx, double x, double y, double z) { /* Place input coordinates on simplectic honeycomb. */ @@ -928,7 +928,7 @@ double open_simplex_noise3(struct osn_context *ctx, double x, double y, double z /* * 4D OpenSimplex (Simplectic) Noise. */ -double open_simplex_noise4(struct osn_context *ctx, double x, double y, double z, double w) +double open_simplex_noise4(const struct osn_context *ctx, double x, double y, double z, double w) { double uins; double dx1, dy1, dz1, dw1; diff --git a/thirdparty/misc/open-simplex-noise.h b/thirdparty/misc/open-simplex-noise.h index 89e0df8218..fd9248c3a1 100644 --- a/thirdparty/misc/open-simplex-noise.h +++ b/thirdparty/misc/open-simplex-noise.h @@ -47,9 +47,9 @@ int open_simplex_noise(int64_t seed, struct osn_context *ctx); //int open_simplex_noise_init_perm(struct osn_context *ctx, int16_t p[], int nelements); // -- GODOT end -- void open_simplex_noise_free(struct osn_context *ctx); -double open_simplex_noise2(struct osn_context *ctx, double x, double y); -double open_simplex_noise3(struct osn_context *ctx, double x, double y, double z); -double open_simplex_noise4(struct osn_context *ctx, double x, double y, double z, double w); +double open_simplex_noise2(const struct osn_context *ctx, double x, double y); +double open_simplex_noise3(const struct osn_context *ctx, double x, double y, double z); +double open_simplex_noise4(const struct osn_context *ctx, double x, double y, double z, double w); #ifdef __cplusplus } diff --git a/thirdparty/rvo2/src/API.h b/thirdparty/rvo2/API.h index c64efb452c..c64efb452c 100644 --- a/thirdparty/rvo2/src/API.h +++ b/thirdparty/rvo2/API.h diff --git a/thirdparty/rvo2/src/Agent.cpp b/thirdparty/rvo2/Agent.cpp index 851d780758..851d780758 100644 --- a/thirdparty/rvo2/src/Agent.cpp +++ b/thirdparty/rvo2/Agent.cpp diff --git a/thirdparty/rvo2/src/Agent.h b/thirdparty/rvo2/Agent.h index 16f75a08f6..16f75a08f6 100644 --- a/thirdparty/rvo2/src/Agent.h +++ b/thirdparty/rvo2/Agent.h diff --git a/thirdparty/rvo2/src/Definitions.h b/thirdparty/rvo2/Definitions.h index a73aca9908..a73aca9908 100644 --- a/thirdparty/rvo2/src/Definitions.h +++ b/thirdparty/rvo2/Definitions.h diff --git a/thirdparty/rvo2/src/KdTree.cpp b/thirdparty/rvo2/KdTree.cpp index bc224614f0..bc224614f0 100644 --- a/thirdparty/rvo2/src/KdTree.cpp +++ b/thirdparty/rvo2/KdTree.cpp diff --git a/thirdparty/rvo2/src/KdTree.h b/thirdparty/rvo2/KdTree.h index 1dbad00ea4..1dbad00ea4 100644 --- a/thirdparty/rvo2/src/KdTree.h +++ b/thirdparty/rvo2/KdTree.h diff --git a/thirdparty/rvo2/src/Vector3.h b/thirdparty/rvo2/Vector3.h index 8c8835c865..8c8835c865 100644 --- a/thirdparty/rvo2/src/Vector3.h +++ b/thirdparty/rvo2/Vector3.h diff --git a/thirdparty/xatlas/xatlas.cpp b/thirdparty/xatlas/xatlas.cpp index 43aec33a9f..9f66ae0067 100644 --- a/thirdparty/xatlas/xatlas.cpp +++ b/thirdparty/xatlas/xatlas.cpp @@ -33,19 +33,25 @@ https://github.com/brandonpelfrey/Fast-BVH MIT License Copyright (c) 2012 Brandon Pelfrey */ -#include <atomic> -#include <condition_variable> -#include <mutex> -#include <thread> +#include "xatlas.h" +#ifndef XATLAS_C_API +#define XATLAS_C_API 0 +#endif +#if XATLAS_C_API +#include "xatlas_c.h" +#endif #include <assert.h> #include <float.h> // FLT_MAX #include <limits.h> #include <math.h> +#include <atomic> +#include <condition_variable> +#include <mutex> +#include <thread> #define __STDC_LIMIT_MACROS #include <stdint.h> #include <stdio.h> #include <string.h> -#include "xatlas.h" #ifndef XA_DEBUG #ifdef NDEBUG @@ -59,7 +65,7 @@ Copyright (c) 2012 Brandon Pelfrey #define XA_PROFILE 0 #endif #if XA_PROFILE -#include <time.h> +#include <chrono> #endif #ifndef XA_MULTITHREADED @@ -70,7 +76,10 @@ Copyright (c) 2012 Brandon Pelfrey #define XA_XSTR(x) XA_STR(x) #ifndef XA_ASSERT -#define XA_ASSERT(exp) if (!(exp)) { XA_PRINT_WARNING("\rASSERT: %s %s %d\n", XA_XSTR(exp), __FILE__, __LINE__); } +#define XA_ASSERT(exp) \ + if (!(exp)) { \ + XA_PRINT_WARNING("\rASSERT: %s %s %d\n", XA_XSTR(exp), __FILE__, __LINE__); \ + } #endif #ifndef XA_DEBUG_ASSERT @@ -78,13 +87,13 @@ Copyright (c) 2012 Brandon Pelfrey #endif #ifndef XA_PRINT -#define XA_PRINT(...) \ +#define XA_PRINT(...) \ if (xatlas::internal::s_print && xatlas::internal::s_printVerbose) \ xatlas::internal::s_print(__VA_ARGS__); #endif #ifndef XA_PRINT_WARNING -#define XA_PRINT_WARNING(...) \ +#define XA_PRINT_WARNING(...) \ if (xatlas::internal::s_print) \ xatlas::internal::s_print(__VA_ARGS__); #endif @@ -116,9 +125,9 @@ Copyright (c) 2012 Brandon Pelfrey #define XA_MERGE_CHARTS 1 #define XA_MERGE_CHARTS_MIN_NORMAL_DEVIATION 0.5f #define XA_RECOMPUTE_CHARTS 1 -#define XA_CLOSE_HOLES_CHECK_EDGE_INTERSECTION 0 -#define XA_FIX_INTERNAL_BOUNDARY_LOOPS 1 -#define XA_PRINT_CHART_WARNINGS 0 +#define XA_CHECK_PARAM_WINDING 0 +#define XA_CHECK_PIECEWISE_CHART_QUALITY 0 +#define XA_CHECK_T_JUNCTIONS 0 #define XA_DEBUG_HEAP 0 #define XA_DEBUG_SINGLE_CHART 0 @@ -131,25 +140,19 @@ Copyright (c) 2012 Brandon Pelfrey #define XA_DEBUG_EXPORT_OBJ_CHART_GROUPS 0 #define XA_DEBUG_EXPORT_OBJ_PLANAR_REGIONS 0 #define XA_DEBUG_EXPORT_OBJ_CHARTS 0 -#define XA_DEBUG_EXPORT_OBJ_BEFORE_FIX_TJUNCTION 0 -#define XA_DEBUG_EXPORT_OBJ_CLOSE_HOLES_ERROR 0 +#define XA_DEBUG_EXPORT_OBJ_TJUNCTION 0 // XA_CHECK_T_JUNCTIONS must also be set #define XA_DEBUG_EXPORT_OBJ_CHARTS_AFTER_PARAMETERIZATION 0 #define XA_DEBUG_EXPORT_OBJ_INVALID_PARAMETERIZATION 0 #define XA_DEBUG_EXPORT_OBJ_RECOMPUTED_CHARTS 0 -#define XA_DEBUG_EXPORT_OBJ (0 \ - || XA_DEBUG_EXPORT_OBJ_FACE_GROUPS \ - || XA_DEBUG_EXPORT_OBJ_CHART_GROUPS \ - || XA_DEBUG_EXPORT_OBJ_PLANAR_REGIONS \ - || XA_DEBUG_EXPORT_OBJ_CHARTS \ - || XA_DEBUG_EXPORT_OBJ_BEFORE_FIX_TJUNCTION \ - || XA_DEBUG_EXPORT_OBJ_CLOSE_HOLES_ERROR \ - || XA_DEBUG_EXPORT_OBJ_CHARTS_AFTER_PARAMETERIZATION \ - || XA_DEBUG_EXPORT_OBJ_INVALID_PARAMETERIZATION \ - || XA_DEBUG_EXPORT_OBJ_RECOMPUTED_CHARTS) +#define XA_DEBUG_EXPORT_OBJ (0 || XA_DEBUG_EXPORT_OBJ_FACE_GROUPS || XA_DEBUG_EXPORT_OBJ_CHART_GROUPS || XA_DEBUG_EXPORT_OBJ_PLANAR_REGIONS || XA_DEBUG_EXPORT_OBJ_CHARTS || XA_DEBUG_EXPORT_OBJ_TJUNCTION || XA_DEBUG_EXPORT_OBJ_CHARTS_AFTER_PARAMETERIZATION || XA_DEBUG_EXPORT_OBJ_INVALID_PARAMETERIZATION || XA_DEBUG_EXPORT_OBJ_RECOMPUTED_CHARTS) #ifdef _MSC_VER -#define XA_FOPEN(_file, _filename, _mode) { if (fopen_s(&_file, _filename, _mode) != 0) _file = NULL; } +#define XA_FOPEN(_file, _filename, _mode) \ + { \ + if (fopen_s(&_file, _filename, _mode) != 0) \ + _file = NULL; \ + } #define XA_SPRINTF(_buffer, _size, _format, ...) sprintf_s(_buffer, _size, _format, __VA_ARGS__) #else #define XA_FOPEN(_file, _filename, _mode) _file = fopen(_filename, _mode) @@ -165,74 +168,76 @@ static PrintFunc s_print = printf; static bool s_printVerbose = false; #if XA_PROFILE -#define XA_PROFILE_START(var) const clock_t var##Start = clock(); -#define XA_PROFILE_END(var) internal::s_profile.var += clock() - var##Start; -#define XA_PROFILE_PRINT_AND_RESET(label, var) XA_PRINT("%s%.2f seconds (%g ms)\n", label, internal::clockToSeconds(internal::s_profile.var), internal::clockToMs(internal::s_profile.var)); internal::s_profile.var = 0; +typedef uint64_t Duration; + +#define XA_PROFILE_START(var) const std::chrono::time_point<std::chrono::high_resolution_clock> var##Start = std::chrono::high_resolution_clock::now(); +#define XA_PROFILE_END(var) internal::s_profile.var += uint64_t(std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - var##Start).count()); +#define XA_PROFILE_PRINT_AND_RESET(label, var) \ + XA_PRINT("%s%.2f seconds (%g ms)\n", label, internal::durationToSeconds(internal::s_profile.var), internal::durationToMs(internal::s_profile.var)); \ + internal::s_profile.var = 0u; #define XA_PROFILE_ALLOC 0 -struct ProfileData -{ +struct ProfileData { #if XA_PROFILE_ALLOC - std::atomic<clock_t> alloc; + std::atomic<Duration> alloc; #endif - clock_t addMeshReal; - clock_t addMeshCopyData; - std::atomic<clock_t> addMeshThread; - std::atomic<clock_t> addMeshCreateColocals; - clock_t computeChartsReal; - std::atomic<clock_t> computeChartsThread; - std::atomic<clock_t> createFaceGroups; - std::atomic<clock_t> extractInvalidMeshGeometry; - std::atomic<clock_t> chartGroupComputeChartsReal; - std::atomic<clock_t> chartGroupComputeChartsThread; - std::atomic<clock_t> createChartGroupMesh; - std::atomic<clock_t> createChartGroupMeshColocals; - std::atomic<clock_t> createChartGroupMeshBoundaries; - std::atomic<clock_t> buildAtlas; - std::atomic<clock_t> buildAtlasInit; - std::atomic<clock_t> planarCharts; - std::atomic<clock_t> clusteredCharts; - std::atomic<clock_t> clusteredChartsPlaceSeeds; - std::atomic<clock_t> clusteredChartsPlaceSeedsBoundaryIntersection; - std::atomic<clock_t> clusteredChartsRelocateSeeds; - std::atomic<clock_t> clusteredChartsReset; - std::atomic<clock_t> clusteredChartsGrow; - std::atomic<clock_t> clusteredChartsGrowBoundaryIntersection; - std::atomic<clock_t> clusteredChartsMerge; - std::atomic<clock_t> clusteredChartsFillHoles; - std::atomic<clock_t> copyChartFaces; - clock_t parameterizeChartsReal; - std::atomic<clock_t> parameterizeChartsThread; - std::atomic<clock_t> createChartMesh; - std::atomic<clock_t> fixChartMeshTJunctions; - std::atomic<clock_t> closeChartMeshHoles; - std::atomic<clock_t> parameterizeChartsOrthogonal; - std::atomic<clock_t> parameterizeChartsLSCM; - std::atomic<clock_t> parameterizeChartsRecompute; - std::atomic<clock_t> parameterizeChartsPiecewise; - std::atomic<clock_t> parameterizeChartsPiecewiseBoundaryIntersection; - std::atomic<clock_t> parameterizeChartsEvaluateQuality; - clock_t packCharts; - clock_t packChartsAddCharts; - std::atomic<clock_t> packChartsAddChartsThread; - std::atomic<clock_t> packChartsAddChartsRestoreTexcoords; - clock_t packChartsRasterize; - clock_t packChartsDilate; - clock_t packChartsFindLocation; - clock_t packChartsBlit; - clock_t buildOutputMeshes; + std::chrono::time_point<std::chrono::high_resolution_clock> addMeshRealStart; + Duration addMeshReal; + Duration addMeshCopyData; + std::atomic<Duration> addMeshThread; + std::atomic<Duration> addMeshCreateColocals; + Duration computeChartsReal; + std::atomic<Duration> computeChartsThread; + std::atomic<Duration> createFaceGroups; + std::atomic<Duration> extractInvalidMeshGeometry; + std::atomic<Duration> chartGroupComputeChartsReal; + std::atomic<Duration> chartGroupComputeChartsThread; + std::atomic<Duration> createChartGroupMesh; + std::atomic<Duration> createChartGroupMeshColocals; + std::atomic<Duration> createChartGroupMeshBoundaries; + std::atomic<Duration> buildAtlas; + std::atomic<Duration> buildAtlasInit; + std::atomic<Duration> planarCharts; + std::atomic<Duration> originalUvCharts; + std::atomic<Duration> clusteredCharts; + std::atomic<Duration> clusteredChartsPlaceSeeds; + std::atomic<Duration> clusteredChartsPlaceSeedsBoundaryIntersection; + std::atomic<Duration> clusteredChartsRelocateSeeds; + std::atomic<Duration> clusteredChartsReset; + std::atomic<Duration> clusteredChartsGrow; + std::atomic<Duration> clusteredChartsGrowBoundaryIntersection; + std::atomic<Duration> clusteredChartsMerge; + std::atomic<Duration> clusteredChartsFillHoles; + std::atomic<Duration> copyChartFaces; + std::atomic<Duration> createChartMeshAndParameterizeReal; + std::atomic<Duration> createChartMeshAndParameterizeThread; + std::atomic<Duration> createChartMesh; + std::atomic<Duration> parameterizeCharts; + std::atomic<Duration> parameterizeChartsOrthogonal; + std::atomic<Duration> parameterizeChartsLSCM; + std::atomic<Duration> parameterizeChartsRecompute; + std::atomic<Duration> parameterizeChartsPiecewise; + std::atomic<Duration> parameterizeChartsPiecewiseBoundaryIntersection; + std::atomic<Duration> parameterizeChartsEvaluateQuality; + Duration packCharts; + Duration packChartsAddCharts; + std::atomic<Duration> packChartsAddChartsThread; + std::atomic<Duration> packChartsAddChartsRestoreTexcoords; + Duration packChartsRasterize; + Duration packChartsDilate; + Duration packChartsFindLocation; + Duration packChartsBlit; + Duration buildOutputMeshes; }; static ProfileData s_profile; -static double clockToMs(clock_t c) -{ - return c * 1000.0 / CLOCKS_PER_SEC; +static double durationToMs(Duration c) { + return (double)c * 0.001; } -static double clockToSeconds(clock_t c) -{ - return c / (double)CLOCKS_PER_SEC; +static double durationToSeconds(Duration c) { + return (double)c * 0.000001; } #else #define XA_PROFILE_START(var) @@ -241,10 +246,8 @@ static double clockToSeconds(clock_t c) #define XA_PROFILE_ALLOC 0 #endif -struct MemTag -{ - enum - { +struct MemTag { + enum { Default, BitImage, BVH, @@ -267,8 +270,7 @@ struct MemTag }; #if XA_DEBUG_HEAP -struct AllocHeader -{ +struct AllocHeader { size_t size; const char *file; int line; @@ -281,11 +283,10 @@ struct AllocHeader static std::mutex s_allocMutex; static AllocHeader *s_allocRoot = nullptr; static size_t s_allocTotalCount = 0, s_allocTotalSize = 0, s_allocPeakSize = 0, s_allocCount[MemTag::Count] = { 0 }, s_allocTotalTagSize[MemTag::Count] = { 0 }, s_allocPeakTagSize[MemTag::Count] = { 0 }; -static uint32_t s_allocId =0 ; +static uint32_t s_allocId = 0; static constexpr uint32_t kAllocRedzone = 0x12345678; -static void *Realloc(void *ptr, size_t size, int tag, const char *file, int line) -{ +static void *Realloc(void *ptr, size_t size, int tag, const char *file, int line) { std::unique_lock<std::mutex> lock(s_allocMutex); if (!size && !ptr) return nullptr; @@ -346,8 +347,7 @@ static void *Realloc(void *ptr, size_t size, int tag, const char *file, int line return newPtr + sizeof(AllocHeader); } -static void ReportLeaks() -{ +static void ReportLeaks() { printf("Checking for memory leaks...\n"); bool anyLeaks = false; AllocHeader *header = s_allocRoot; @@ -375,8 +375,7 @@ static void ReportLeaks() s_allocTotalTagSize[i] = s_allocPeakTagSize[i] = 0; } -static void PrintMemoryUsage() -{ +static void PrintMemoryUsage() { XA_PRINT("Total allocations: %zu\n", s_allocTotalCount); XA_PRINT("Memory usage: %0.2fMB current, %0.2fMB peak\n", internal::s_allocTotalSize / 1024.0f / 1024.0f, internal::s_allocPeakSize / 1024.0f / 1024.0f); static const char *labels[] = { // Sync with MemTag @@ -405,8 +404,7 @@ static void PrintMemoryUsage() #define XA_PRINT_MEM_USAGE internal::PrintMemoryUsage(); #else -static void *Realloc(void *ptr, size_t size, int /*tag*/, const char * /*file*/, int /*line*/) -{ +static void *Realloc(void *ptr, size_t size, int /*tag*/, const char * /*file*/, int /*line*/) { if (size == 0 && !ptr) return nullptr; if (size == 0 && s_free) { @@ -432,89 +430,75 @@ static constexpr float kEpsilon = 0.0001f; static constexpr float kAreaEpsilon = FLT_EPSILON; static constexpr float kNormalEpsilon = 0.001f; -static int align(int x, int a) -{ +static int align(int x, int a) { return (x + a - 1) & ~(a - 1); } template <typename T> -static T max(const T &a, const T &b) -{ +static T max(const T &a, const T &b) { return a > b ? a : b; } template <typename T> -static T min(const T &a, const T &b) -{ +static T min(const T &a, const T &b) { return a < b ? a : b; } template <typename T> -static T max3(const T &a, const T &b, const T &c) -{ +static T max3(const T &a, const T &b, const T &c) { return max(a, max(b, c)); } /// Return the maximum of the three arguments. template <typename T> -static T min3(const T &a, const T &b, const T &c) -{ +static T min3(const T &a, const T &b, const T &c) { return min(a, min(b, c)); } /// Clamp between two values. template <typename T> -static T clamp(const T &x, const T &a, const T &b) -{ +static T clamp(const T &x, const T &a, const T &b) { return min(max(x, a), b); } template <typename T> -static void swap(T &a, T &b) -{ +static void swap(T &a, T &b) { T temp = a; a = b; b = temp; } -union FloatUint32 -{ +union FloatUint32 { float f; uint32_t u; }; -static bool isFinite(float f) -{ +static bool isFinite(float f) { FloatUint32 fu; fu.f = f; return fu.u != 0x7F800000u && fu.u != 0x7F800001u; } -static bool isNan(float f) -{ +static bool isNan(float f) { return f != f; } // Robust floating point comparisons: // http://realtimecollisiondetection.net/blog/?p=89 -static bool equal(const float f0, const float f1, const float epsilon) -{ +static bool equal(const float f0, const float f1, const float epsilon) { //return fabs(f0-f1) <= epsilon; return fabs(f0 - f1) <= epsilon * max3(1.0f, fabsf(f0), fabsf(f1)); } -static int ftoi_ceil(float val) -{ +static int ftoi_ceil(float val) { return (int)ceilf(val); } -static bool isZero(const float f, const float epsilon) -{ +static bool isZero(const float f, const float epsilon) { return fabs(f) <= epsilon; } -static float square(float f) -{ +static float square(float f) { return f * f; } @@ -524,9 +508,8 @@ static float square(float f) * @note isPowerOfTwo(x) == true -> nextPowerOfTwo(x) == x * @note nextPowerOfTwo(x) = 2 << log2(x-1) */ -static uint32_t nextPowerOfTwo(uint32_t x) -{ - XA_DEBUG_ASSERT( x != 0 ); +static uint32_t nextPowerOfTwo(uint32_t x) { + XA_DEBUG_ASSERT(x != 0); // On modern CPUs this is supposed to be as fast as using the bsr instruction. x--; x |= x >> 1; @@ -537,38 +520,34 @@ static uint32_t nextPowerOfTwo(uint32_t x) return x + 1; } -class Vector2 -{ +class Vector2 { public: Vector2() {} - explicit Vector2(float f) : x(f), y(f) {} - Vector2(float x, float y): x(x), y(y) {} + explicit Vector2(float f) : + x(f), y(f) {} + Vector2(float _x, float _y) : + x(_x), y(_y) {} - Vector2 operator-() const - { + Vector2 operator-() const { return Vector2(-x, -y); } - void operator+=(const Vector2 &v) - { + void operator+=(const Vector2 &v) { x += v.x; y += v.y; } - void operator-=(const Vector2 &v) - { + void operator-=(const Vector2 &v) { x -= v.x; y -= v.y; } - void operator*=(float s) - { + void operator*=(float s) { x *= s; y *= s; } - void operator*=(const Vector2 &v) - { + void operator*=(const Vector2 &v) { x *= v.x; y *= v.y; } @@ -576,13 +555,11 @@ public: float x, y; }; -static bool operator==(const Vector2 &a, const Vector2 &b) -{ +static bool operator==(const Vector2 &a, const Vector2 &b) { return a.x == b.x && a.y == b.y; } -static bool operator!=(const Vector2 &a, const Vector2 &b) -{ +static bool operator!=(const Vector2 &a, const Vector2 &b) { return a.x != b.x || a.y != b.y; } @@ -591,78 +568,64 @@ static bool operator!=(const Vector2 &a, const Vector2 &b) return Vector2(a.x + b.x, a.y + b.y); }*/ -static Vector2 operator-(const Vector2 &a, const Vector2 &b) -{ +static Vector2 operator-(const Vector2 &a, const Vector2 &b) { return Vector2(a.x - b.x, a.y - b.y); } -static Vector2 operator*(const Vector2 &v, float s) -{ +static Vector2 operator*(const Vector2 &v, float s) { return Vector2(v.x * s, v.y * s); } -static float dot(const Vector2 &a, const Vector2 &b) -{ +static float dot(const Vector2 &a, const Vector2 &b) { return a.x * b.x + a.y * b.y; } -static float lengthSquared(const Vector2 &v) -{ +static float lengthSquared(const Vector2 &v) { return v.x * v.x + v.y * v.y; } -static float length(const Vector2 &v) -{ +static float length(const Vector2 &v) { return sqrtf(lengthSquared(v)); } #if XA_DEBUG -static bool isNormalized(const Vector2 &v, float epsilon = kNormalEpsilon) -{ +static bool isNormalized(const Vector2 &v, float epsilon = kNormalEpsilon) { return equal(length(v), 1, epsilon); } #endif -static Vector2 normalize(const Vector2 &v, float epsilon) -{ - float l = length(v); - XA_DEBUG_ASSERT(!isZero(l, epsilon)); - XA_UNUSED(epsilon); - Vector2 n = v * (1.0f / l); +static Vector2 normalize(const Vector2 &v) { + const float l = length(v); + XA_DEBUG_ASSERT(l > 0.0f); // Never negative. + const Vector2 n = v * (1.0f / l); XA_DEBUG_ASSERT(isNormalized(n)); return n; } -static Vector2 normalizeSafe(const Vector2 &v, const Vector2 &fallback, float epsilon) -{ - float l = length(v); - if (isZero(l, epsilon)) - return fallback; - return v * (1.0f / l); +static Vector2 normalizeSafe(const Vector2 &v, const Vector2 &fallback) { + const float l = length(v); + if (l > 0.0f) // Never negative. + return v * (1.0f / l); + return fallback; } -static bool equal(const Vector2 &v1, const Vector2 &v2, float epsilon) -{ +static bool equal(const Vector2 &v1, const Vector2 &v2, float epsilon) { return equal(v1.x, v2.x, epsilon) && equal(v1.y, v2.y, epsilon); } -static Vector2 min(const Vector2 &a, const Vector2 &b) -{ +static Vector2 min(const Vector2 &a, const Vector2 &b) { return Vector2(min(a.x, b.x), min(a.y, b.y)); } -static Vector2 max(const Vector2 &a, const Vector2 &b) -{ +static Vector2 max(const Vector2 &a, const Vector2 &b) { return Vector2(max(a.x, b.x), max(a.y, b.y)); } -static bool isFinite(const Vector2 &v) -{ +static bool isFinite(const Vector2 &v) { return isFinite(v.x) && isFinite(v.y); } -static float triangleArea(const Vector2 &a, const Vector2 &b, const Vector2 &c) -{ +static float triangleArea(const Vector2 &a, const Vector2 &b, const Vector2 &c) { // IC: While it may be appealing to use the following expression: //return (c.x * a.y + a.x * b.y + b.x * c.y - b.x * a.y - c.x * b.y - a.x * c.y) * 0.5f; // That's actually a terrible idea. Small triangles far from the origin can end up producing fairly large floating point @@ -676,8 +639,7 @@ static float triangleArea(const Vector2 &a, const Vector2 &b, const Vector2 &c) return (v0.x * v1.y - v0.y * v1.x) * 0.5f; } -static bool linesIntersect(const Vector2 &a1, const Vector2 &a2, const Vector2 &b1, const Vector2 &b2, float epsilon) -{ +static bool linesIntersect(const Vector2 &a1, const Vector2 &a2, const Vector2 &b1, const Vector2 &b2, float epsilon) { const Vector2 v0 = a2 - a1; const Vector2 v1 = b2 - b1; const float denom = -v1.x * v0.y + v0.x * v1.y; @@ -685,76 +647,70 @@ static bool linesIntersect(const Vector2 &a1, const Vector2 &a2, const Vector2 & return false; const float s = (-v0.y * (a1.x - b1.x) + v0.x * (a1.y - b1.y)) / denom; if (s > epsilon && s < 1.0f - epsilon) { - const float t = ( v1.x * (a1.y - b1.y) - v1.y * (a1.x - b1.x)) / denom; + const float t = (v1.x * (a1.y - b1.y) - v1.y * (a1.x - b1.x)) / denom; return t > epsilon && t < 1.0f - epsilon; } return false; } -struct Vector2i -{ +struct Vector2i { Vector2i() {} - Vector2i(int32_t x, int32_t y) : x(x), y(y) {} + Vector2i(int32_t _x, int32_t _y) : + x(_x), y(_y) {} int32_t x, y; }; -class Vector3 -{ +class Vector3 { public: Vector3() {} - explicit Vector3(float f) : x(f), y(f), z(f) {} - Vector3(float x, float y, float z) : x(x), y(y), z(z) {} - Vector3(const Vector2 &v, float z) : x(v.x), y(v.y), z(z) {} - - Vector2 xy() const - { + explicit Vector3(float f) : + x(f), y(f), z(f) {} + Vector3(float _x, float _y, float _z) : + x(_x), y(_y), z(_z) {} + Vector3(const Vector2 &v, float _z) : + x(v.x), y(v.y), z(_z) {} + + Vector2 xy() const { return Vector2(x, y); } - Vector3 operator-() const - { + Vector3 operator-() const { return Vector3(-x, -y, -z); } - void operator+=(const Vector3 &v) - { + void operator+=(const Vector3 &v) { x += v.x; y += v.y; z += v.z; } - void operator-=(const Vector3 &v) - { + void operator-=(const Vector3 &v) { x -= v.x; y -= v.y; z -= v.z; } - void operator*=(float s) - { + void operator*=(float s) { x *= s; y *= s; z *= s; } - void operator/=(float s) - { + void operator/=(float s) { float is = 1.0f / s; x *= is; y *= is; z *= is; } - void operator*=(const Vector3 &v) - { + void operator*=(const Vector3 &v) { x *= v.x; y *= v.y; z *= v.z; } - void operator/=(const Vector3 &v) - { + void operator/=(const Vector3 &v) { x /= v.x; y /= v.y; z /= v.z; @@ -763,260 +719,151 @@ public: float x, y, z; }; -static Vector3 operator+(const Vector3 &a, const Vector3 &b) -{ +static Vector3 operator+(const Vector3 &a, const Vector3 &b) { return Vector3(a.x + b.x, a.y + b.y, a.z + b.z); } -static Vector3 operator-(const Vector3 &a, const Vector3 &b) -{ +static Vector3 operator-(const Vector3 &a, const Vector3 &b) { return Vector3(a.x - b.x, a.y - b.y, a.z - b.z); } -static Vector3 cross(const Vector3 &a, const Vector3 &b) -{ +static bool operator==(const Vector3 &a, const Vector3 &b) { + return a.x == b.x && a.y == b.y && a.z == b.z; +} + +static Vector3 cross(const Vector3 &a, const Vector3 &b) { return Vector3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); } -static Vector3 operator*(const Vector3 &v, float s) -{ +static Vector3 operator*(const Vector3 &v, float s) { return Vector3(v.x * s, v.y * s, v.z * s); } -static Vector3 operator/(const Vector3 &v, float s) -{ +static Vector3 operator/(const Vector3 &v, float s) { return v * (1.0f / s); } -static float dot(const Vector3 &a, const Vector3 &b) -{ +static float dot(const Vector3 &a, const Vector3 &b) { return a.x * b.x + a.y * b.y + a.z * b.z; } -static float lengthSquared(const Vector3 &v) -{ +static float lengthSquared(const Vector3 &v) { return v.x * v.x + v.y * v.y + v.z * v.z; } -static float length(const Vector3 &v) -{ +static float length(const Vector3 &v) { return sqrtf(lengthSquared(v)); } -static bool isNormalized(const Vector3 &v, float epsilon = kNormalEpsilon) -{ - return equal(length(v), 1, epsilon); +static bool isNormalized(const Vector3 &v, float epsilon = kNormalEpsilon) { + return equal(length(v), 1.0f, epsilon); } -static Vector3 normalize(const Vector3 &v, float epsilon) -{ - float l = length(v); - XA_DEBUG_ASSERT(!isZero(l, epsilon)); - XA_UNUSED(epsilon); - Vector3 n = v * (1.0f / l); +static Vector3 normalize(const Vector3 &v) { + const float l = length(v); + XA_DEBUG_ASSERT(l > 0.0f); // Never negative. + const Vector3 n = v * (1.0f / l); XA_DEBUG_ASSERT(isNormalized(n)); return n; } -static Vector3 normalizeSafe(const Vector3 &v, const Vector3 &fallback, float epsilon) -{ - float l = length(v); - if (isZero(l, epsilon)) { - return fallback; - } - return v * (1.0f / l); +static Vector3 normalizeSafe(const Vector3 &v, const Vector3 &fallback) { + const float l = length(v); + if (l > 0.0f) // Never negative. + return v * (1.0f / l); + return fallback; } -static bool equal(const Vector3 &v0, const Vector3 &v1, float epsilon) -{ +static bool equal(const Vector3 &v0, const Vector3 &v1, float epsilon) { return fabs(v0.x - v1.x) <= epsilon && fabs(v0.y - v1.y) <= epsilon && fabs(v0.z - v1.z) <= epsilon; } -static Vector3 min(const Vector3 &a, const Vector3 &b) -{ +static Vector3 min(const Vector3 &a, const Vector3 &b) { return Vector3(min(a.x, b.x), min(a.y, b.y), min(a.z, b.z)); } -static Vector3 max(const Vector3 &a, const Vector3 &b) -{ +static Vector3 max(const Vector3 &a, const Vector3 &b) { return Vector3(max(a.x, b.x), max(a.y, b.y), max(a.z, b.z)); } #if XA_DEBUG -bool isFinite(const Vector3 &v) -{ +bool isFinite(const Vector3 &v) { return isFinite(v.x) && isFinite(v.y) && isFinite(v.z); } #endif -struct Extents2 -{ +struct Extents2 { Vector2 min, max; Extents2() {} - - Extents2(Vector2 p1, Vector2 p2) - { + + Extents2(Vector2 p1, Vector2 p2) { min = xatlas::internal::min(p1, p2); max = xatlas::internal::max(p1, p2); } - void reset() - { + void reset() { min.x = min.y = FLT_MAX; max.x = max.y = -FLT_MAX; } - void add(Vector2 p) - { + void add(Vector2 p) { min = xatlas::internal::min(min, p); max = xatlas::internal::max(max, p); } - Vector2 midpoint() const - { + Vector2 midpoint() const { return Vector2(min.x + (max.x - min.x) * 0.5f, min.y + (max.y - min.y) * 0.5f); } - static bool intersect(const Extents2 &e1, const Extents2 &e2) - { + static bool intersect(const Extents2 &e1, const Extents2 &e2) { return e1.min.x <= e2.max.x && e1.max.x >= e2.min.x && e1.min.y <= e2.max.y && e1.max.y >= e2.min.y; } }; -struct Plane -{ - Plane() = default; - - Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3) - { - normal = cross(p2 - p1, p3 - p1); - dist = dot(normal, p1); - } - - float distance(const Vector3 &p) const - { - return dot(normal, p) - dist; - } - - void normalize() - { - const float len = length(normal); - if (len > 0.0f) { - const float il = 1.0f / len; - normal *= il; - dist *= il; - } - } - - Vector3 normal; - float dist; -}; - -static bool lineIntersectsPoint(const Vector3 &point, const Vector3 &lineStart, const Vector3 &lineEnd, float *t, float epsilon) -{ - float tt; - if (!t) - t = &tt; - *t = 0.0f; - if (equal(lineStart, point, epsilon) || equal(lineEnd, point, epsilon)) - return false; // Vertex lies on either line vertices. - const Vector3 v01 = point - lineStart; - const Vector3 v21 = lineEnd - lineStart; - const float l = length(v21); - const float d = length(cross(v01, v21)) / l; - if (!isZero(d, epsilon)) - return false; - *t = dot(v01, v21) / (l * l); - return *t > kEpsilon && *t < 1.0f - kEpsilon; -} - -static bool sameSide(const Vector3 &p1, const Vector3 &p2, const Vector3 &a, const Vector3 &b) -{ - const Vector3 &ab = b - a; - return dot(cross(ab, p1 - a), cross(ab, p2 - a)) >= 0.0f; -} - -// http://blackpawn.com/texts/pointinpoly/default.html -static bool pointInTriangle(const Vector3 &p, const Vector3 &a, const Vector3 &b, const Vector3 &c) -{ - return sameSide(p, a, b, c) && sameSide(p, b, a, c) && sameSide(p, c, a, b); -} - -#if XA_CLOSE_HOLES_CHECK_EDGE_INTERSECTION -// https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm -static bool rayIntersectsTriangle(const Vector3 &rayOrigin, const Vector3 &rayDir, const Vector3 *tri, float *t) -{ - *t = 0.0f; - const Vector3 &edge1 = tri[1] - tri[0]; - const Vector3 &edge2 = tri[2] - tri[0]; - const Vector3 h = cross(rayDir, edge2); - const float a = dot(edge1, h); - if (a > -kEpsilon && a < kEpsilon) - return false; // This ray is parallel to this triangle. - const float f = 1.0f / a; - const Vector3 s = rayOrigin - tri[0]; - const float u = f * dot(s, h); - if (u < 0.0f || u > 1.0f) - return false; - const Vector3 q = cross(s, edge1); - const float v = f * dot(rayDir, q); - if (v < 0.0f || u + v > 1.0f) - return false; - // At this stage we can compute t to find out where the intersection point is on the line. - *t = f * dot(edge2, q); - if (*t > kEpsilon && *t < 1.0f - kEpsilon) - return true; - // This means that there is a line intersection but not a ray intersection. - return false; -} -#endif - // From Fast-BVH -struct AABB -{ - AABB() : min(FLT_MAX, FLT_MAX, FLT_MAX), max(-FLT_MAX, -FLT_MAX, -FLT_MAX) {} - AABB(const Vector3 &min, const Vector3 &max) : min(min), max(max) { } - AABB(const Vector3 &p, float radius = 0.0f) : min(p), max(p) { if (radius > 0.0f) expand(radius); } - - bool intersect(const AABB &other) const - { +struct AABB { + AABB() : + min(FLT_MAX, FLT_MAX, FLT_MAX), max(-FLT_MAX, -FLT_MAX, -FLT_MAX) {} + AABB(const Vector3 &_min, const Vector3 &_max) : + min(_min), max(_max) {} + AABB(const Vector3 &p, float radius = 0.0f) : + min(p), max(p) { + if (radius > 0.0f) + expand(radius); + } + + bool intersect(const AABB &other) const { return min.x <= other.max.x && max.x >= other.min.x && min.y <= other.max.y && max.y >= other.min.y && min.z <= other.max.z && max.z >= other.min.z; } - void expandToInclude(const Vector3 &p) - { + void expandToInclude(const Vector3 &p) { min = internal::min(min, p); max = internal::max(max, p); } - void expandToInclude(const AABB &aabb) - { + void expandToInclude(const AABB &aabb) { min = internal::min(min, aabb.min); max = internal::max(max, aabb.max); } - void expand(float amount) - { + void expand(float amount) { min -= Vector3(amount); max += Vector3(amount); } - Vector3 centroid() const - { + Vector3 centroid() const { return min + (max - min) * 0.5f; } - uint32_t maxDimension() const - { + uint32_t maxDimension() const { const Vector3 extent = max - min; uint32_t result = 0; if (extent.y > extent.x) { result = 1; if (extent.z > extent.y) result = 2; - } - else if(extent.z > extent.x) + } else if (extent.z > extent.x) result = 2; return result; } @@ -1024,10 +871,9 @@ struct AABB Vector3 min, max; }; -struct ArrayBase -{ - ArrayBase(uint32_t elementSize, int memTag = MemTag::Default) : buffer(nullptr), elementSize(elementSize), size(0), capacity(0) - { +struct ArrayBase { + ArrayBase(uint32_t _elementSize, int memTag = MemTag::Default) : + buffer(nullptr), elementSize(_elementSize), size(0), capacity(0) { #if XA_DEBUG_HEAP this->memTag = memTag; #else @@ -1035,31 +881,31 @@ struct ArrayBase #endif } - ~ArrayBase() - { + ~ArrayBase() { XA_FREE(buffer); } - XA_INLINE void clear() - { + XA_INLINE void clear() { size = 0; } - void copyFrom(const uint8_t *data, uint32_t length) - { + void copyFrom(const uint8_t *data, uint32_t length) { + XA_DEBUG_ASSERT(data); + XA_DEBUG_ASSERT(length > 0); resize(length, true); - memcpy(buffer, data, length * elementSize); + if (buffer && data && length > 0) + memcpy(buffer, data, length * elementSize); } - void copyTo(ArrayBase &other) const - { + void copyTo(ArrayBase &other) const { XA_DEBUG_ASSERT(elementSize == other.elementSize); + XA_DEBUG_ASSERT(size > 0); other.resize(size, true); - memcpy(other.buffer, buffer, size * elementSize); + if (other.buffer && buffer && size > 0) + memcpy(other.buffer, buffer, size * elementSize); } - void destroy() - { + void destroy() { size = 0; XA_FREE(buffer); buffer = nullptr; @@ -1068,17 +914,18 @@ struct ArrayBase } // Insert the given element at the given index shifting all the elements up. - void insertAt(uint32_t index, const uint8_t *value) - { + void insertAt(uint32_t index, const uint8_t *value) { XA_DEBUG_ASSERT(index >= 0 && index <= size); + XA_DEBUG_ASSERT(value); resize(size + 1, false); - if (index < size - 1) + XA_DEBUG_ASSERT(buffer); + if (buffer && index < size - 1) memmove(buffer + elementSize * (index + 1), buffer + elementSize * index, elementSize * (size - 1 - index)); - memcpy(&buffer[index * elementSize], value, elementSize); + if (buffer && value) + memcpy(&buffer[index * elementSize], value, elementSize); } - void moveTo(ArrayBase &other) - { + void moveTo(ArrayBase &other) { XA_DEBUG_ASSERT(elementSize == other.elementSize); other.destroy(); other.buffer = buffer; @@ -1092,55 +939,61 @@ struct ArrayBase elementSize = size = capacity = 0; } - void pop_back() - { + void pop_back() { XA_DEBUG_ASSERT(size > 0); resize(size - 1, false); } - void push_back(const uint8_t *value) - { + void push_back(const uint8_t *value) { XA_DEBUG_ASSERT(value < buffer || value >= buffer + size); + XA_DEBUG_ASSERT(value); resize(size + 1, false); - memcpy(&buffer[(size - 1) * elementSize], value, elementSize); + XA_DEBUG_ASSERT(buffer); + if (buffer && value) + memcpy(&buffer[(size - 1) * elementSize], value, elementSize); } - void push_back(const ArrayBase &other) - { + void push_back(const ArrayBase &other) { XA_DEBUG_ASSERT(elementSize == other.elementSize); - if (other.size == 0) - return; - const uint32_t oldSize = size; - resize(size + other.size, false); - memcpy(buffer + oldSize * elementSize, other.buffer, other.size * other.elementSize); + if (other.size > 0) { + const uint32_t oldSize = size; + resize(size + other.size, false); + XA_DEBUG_ASSERT(buffer); + if (buffer) + memcpy(buffer + oldSize * elementSize, other.buffer, other.size * other.elementSize); + } } // Remove the element at the given index. This is an expensive operation! - void removeAt(uint32_t index) - { + void removeAt(uint32_t index) { XA_DEBUG_ASSERT(index >= 0 && index < size); - if (size != 1) - memmove(buffer + elementSize * index, buffer + elementSize * (index + 1), elementSize * (size - 1 - index)); - size--; + XA_DEBUG_ASSERT(buffer); + if (buffer) { + if (size > 1) + memmove(buffer + elementSize * index, buffer + elementSize * (index + 1), elementSize * (size - 1 - index)); + if (size > 0) + size--; + } } // Element at index is swapped with the last element, then the array length is decremented. - void removeAtFast(uint32_t index) - { + void removeAtFast(uint32_t index) { XA_DEBUG_ASSERT(index >= 0 && index < size); - if (size != 1 && index != size - 1) - memcpy(buffer + elementSize * index, buffer + elementSize * (size - 1), elementSize); - size--; + XA_DEBUG_ASSERT(buffer); + if (buffer) { + if (size > 1 && index != size - 1) + memcpy(buffer + elementSize * index, buffer + elementSize * (size - 1), elementSize); + if (size > 0) + size--; + } } - void reserve(uint32_t desiredSize) - { + void reserve(uint32_t desiredSize) { if (desiredSize > capacity) setArrayCapacity(desiredSize); } - void resize(uint32_t newSize, bool exact) - { + void resize(uint32_t newSize, bool exact) { size = newSize; if (size > capacity) { // First allocation is always exact. Otherwise, following allocations grow array to 150% of desired size. @@ -1153,8 +1006,7 @@ struct ArrayBase } } - void setArrayCapacity(uint32_t newCapacity) - { + void setArrayCapacity(uint32_t newCapacity) { XA_DEBUG_ASSERT(newCapacity >= size); if (newCapacity == 0) { // free the buffer. @@ -1174,8 +1026,7 @@ struct ArrayBase } #if XA_DEBUG_HEAP - void setMemTag(int _memTag) - { + void setMemTag(int _memTag) { this->memTag = _memTag; } #endif @@ -1189,28 +1040,27 @@ struct ArrayBase #endif }; -template<typename T> -class Array -{ +template <typename T> +class Array { public: - Array(int memTag = MemTag::Default) : m_base(sizeof(T), memTag) {} - Array(const Array&) = delete; + Array(int memTag = MemTag::Default) : + m_base(sizeof(T), memTag) {} + Array(const Array &) = delete; Array &operator=(const Array &) = delete; - XA_INLINE const T &operator[](uint32_t index) const - { + XA_INLINE const T &operator[](uint32_t index) const { XA_DEBUG_ASSERT(index < m_base.size); + XA_DEBUG_ASSERT(m_base.buffer); return ((const T *)m_base.buffer)[index]; } - XA_INLINE T &operator[](uint32_t index) - { + XA_INLINE T &operator[](uint32_t index) { XA_DEBUG_ASSERT(index < m_base.size); + XA_DEBUG_ASSERT(m_base.buffer); return ((T *)m_base.buffer)[index]; } - XA_INLINE const T &back() const - { + XA_INLINE const T &back() const { XA_DEBUG_ASSERT(!isEmpty()); return ((const T *)m_base.buffer)[m_base.size - 1]; } @@ -1218,8 +1068,7 @@ public: XA_INLINE T *begin() { return (T *)m_base.buffer; } XA_INLINE void clear() { m_base.clear(); } - bool contains(const T &value) const - { + bool contains(const T &value) const { for (uint32_t i = 0; i < m_base.size; i++) { if (((const T *)m_base.buffer)[i] == value) return true; @@ -1244,28 +1093,25 @@ public: void reserve(uint32_t desiredSize) { m_base.reserve(desiredSize); } void resize(uint32_t newSize) { m_base.resize(newSize, true); } - void runCtors() - { + void runCtors() { for (uint32_t i = 0; i < m_base.size; i++) new (&((T *)m_base.buffer)[i]) T; } - void runDtors() - { + void runDtors() { for (uint32_t i = 0; i < m_base.size; i++) ((T *)m_base.buffer)[i].~T(); } - void fill(const T &value) - { + void fill(const T &value) { auto buffer = (T *)m_base.buffer; for (uint32_t i = 0; i < m_base.size; i++) buffer[i] = value; } - void fillBytes(uint8_t value) - { - memset(m_base.buffer, (int)value, m_base.size * m_base.elementSize); + void fillBytes(uint8_t value) { + if (m_base.buffer && m_base.size > 0) + memset(m_base.buffer, (int)value, m_base.size * m_base.elementSize); } #if XA_DEBUG_HEAP @@ -1273,41 +1119,67 @@ public: #endif XA_INLINE uint32_t size() const { return m_base.size; } - XA_INLINE void zeroOutMemory() { memset(m_base.buffer, 0, m_base.elementSize * m_base.size); } + + XA_INLINE void zeroOutMemory() { + if (m_base.buffer && m_base.size > 0) + memset(m_base.buffer, 0, m_base.elementSize * m_base.size); + } private: ArrayBase m_base; }; -template<typename T> -struct ArrayView -{ - ArrayView() : data(nullptr), length(0) {} - ArrayView(Array<T> &a) : data(a.data()), length(a.size()) {} - ArrayView(T *data, uint32_t length) : data(data), length(length) {} - ArrayView &operator=(Array<T> &a) { data = a.data(); length = a.size(); return *this; } - XA_INLINE const T &operator[](uint32_t index) const { XA_DEBUG_ASSERT(index < length); return data[index]; } +template <typename T> +struct ArrayView { + ArrayView() : + data(nullptr), length(0) {} + ArrayView(Array<T> &a) : + data(a.data()), length(a.size()) {} + ArrayView(T *_data, uint32_t _length) : + data(_data), length(_length) {} + ArrayView &operator=(Array<T> &a) { + data = a.data(); + length = a.size(); + return *this; + } + XA_INLINE const T &operator[](uint32_t index) const { + XA_DEBUG_ASSERT(index < length); + return data[index]; + } + XA_INLINE T &operator[](uint32_t index) { + XA_DEBUG_ASSERT(index < length); + return data[index]; + } T *data; uint32_t length; }; -template<typename T> -struct ConstArrayView -{ - ConstArrayView() : data(nullptr), length(0) {} - ConstArrayView(const Array<T> &a) : data(a.data()), length(a.size()) {} - ConstArrayView(const T *data, uint32_t length) : data(data), length(length) {} - ConstArrayView &operator=(const Array<T> &a) { data = a.data(); length = a.size(); return *this; } - XA_INLINE const T &operator[](uint32_t index) const { XA_DEBUG_ASSERT(index < length); return data[index]; } +template <typename T> +struct ConstArrayView { + ConstArrayView() : + data(nullptr), length(0) {} + ConstArrayView(const Array<T> &a) : + data(a.data()), length(a.size()) {} + ConstArrayView(ArrayView<T> av) : + data(av.data), length(av.length) {} + ConstArrayView(const T *_data, uint32_t _length) : + data(_data), length(_length) {} + ConstArrayView &operator=(const Array<T> &a) { + data = a.data(); + length = a.size(); + return *this; + } + XA_INLINE const T &operator[](uint32_t index) const { + XA_DEBUG_ASSERT(index < length); + return data[index]; + } const T *data; uint32_t length; }; /// Basis class to compute tangent space basis, ortogonalizations and to transform vectors from one space to another. -struct Basis -{ - XA_NODISCARD static Vector3 computeTangent(const Vector3 &normal) - { +struct Basis { + XA_NODISCARD static Vector3 computeTangent(const Vector3 &normal) { XA_ASSERT(isNormalized(normal)); // Choose minimum axis. Vector3 tangent; @@ -1319,12 +1191,11 @@ struct Basis tangent = Vector3(0, 0, 1); // Ortogonalize tangent -= normal * dot(normal, tangent); - tangent = normalize(tangent, kEpsilon); + tangent = normalize(tangent); return tangent; } - XA_NODISCARD static Vector3 computeBitangent(const Vector3 &normal, const Vector3 &tangent) - { + XA_NODISCARD static Vector3 computeBitangent(const Vector3 &normal, const Vector3 &tangent) { return cross(normal, tangent); } @@ -1334,42 +1205,36 @@ struct Basis }; // Simple bit array. -class BitArray -{ +class BitArray { public: - BitArray() : m_size(0) {} + BitArray() : + m_size(0) {} - BitArray(uint32_t sz) - { + BitArray(uint32_t sz) { resize(sz); } - void resize(uint32_t new_size) - { + void resize(uint32_t new_size) { m_size = new_size; m_wordArray.resize((m_size + 31) >> 5); } - bool get(uint32_t index) const - { + bool get(uint32_t index) const { XA_DEBUG_ASSERT(index < m_size); return (m_wordArray[index >> 5] & (1 << (index & 31))) != 0; } - void set(uint32_t index) - { + void set(uint32_t index) { XA_DEBUG_ASSERT(index < m_size); m_wordArray[index >> 5] |= (1 << (index & 31)); } - void unset(uint32_t index) - { + void unset(uint32_t index) { XA_DEBUG_ASSERT(index < m_size); m_wordArray[index >> 5] &= ~(1 << (index & 31)); } - void zeroOutMemory() - { + void zeroOutMemory() { m_wordArray.zeroOutMemory(); } @@ -1378,13 +1243,13 @@ private: Array<uint32_t> m_wordArray; }; -class BitImage -{ +class BitImage { public: - BitImage() : m_width(0), m_height(0), m_rowStride(0), m_data(MemTag::BitImage) {} + BitImage() : + m_width(0), m_height(0), m_rowStride(0), m_data(MemTag::BitImage) {} - BitImage(uint32_t w, uint32_t h) : m_width(w), m_height(h), m_data(MemTag::BitImage) - { + BitImage(uint32_t w, uint32_t h) : + m_width(w), m_height(h), m_data(MemTag::BitImage) { m_rowStride = (m_width + 63) >> 6; m_data.resize(m_rowStride * m_height); m_data.zeroOutMemory(); @@ -1395,16 +1260,14 @@ public: uint32_t width() const { return m_width; } uint32_t height() const { return m_height; } - void copyTo(BitImage &other) - { + void copyTo(BitImage &other) { other.m_width = m_width; other.m_height = m_height; other.m_rowStride = m_rowStride; m_data.copyTo(other.m_data); } - void resize(uint32_t w, uint32_t h, bool discard) - { + void resize(uint32_t w, uint32_t h, bool discard) { const uint32_t rowStride = (w + 63) >> 6; if (discard) { m_data.resize(rowStride * h); @@ -1428,28 +1291,24 @@ public: m_rowStride = rowStride; } - bool get(uint32_t x, uint32_t y) const - { + bool get(uint32_t x, uint32_t y) const { XA_DEBUG_ASSERT(x < m_width && y < m_height); const uint32_t index = (x >> 6) + y * m_rowStride; return (m_data[index] & (UINT64_C(1) << (uint64_t(x) & UINT64_C(63)))) != 0; } - void set(uint32_t x, uint32_t y) - { + void set(uint32_t x, uint32_t y) { XA_DEBUG_ASSERT(x < m_width && y < m_height); const uint32_t index = (x >> 6) + y * m_rowStride; m_data[index] |= UINT64_C(1) << (uint64_t(x) & UINT64_C(63)); XA_DEBUG_ASSERT(get(x, y)); } - void zeroOutMemory() - { + void zeroOutMemory() { m_data.zeroOutMemory(); } - bool canBlit(const BitImage &image, uint32_t offsetX, uint32_t offsetY) const - { + bool canBlit(const BitImage &image, uint32_t offsetX, uint32_t offsetY) const { for (uint32_t y = 0; y < image.m_height; y++) { const uint32_t thisY = y + offsetY; if (thisY >= m_height) @@ -1473,8 +1332,7 @@ public: return true; } - void dilate(uint32_t padding) - { + void dilate(uint32_t padding) { BitImage tmp(m_width, m_height); for (uint32_t p = 0; p < padding; p++) { tmp.zeroOutMemory(); @@ -1484,15 +1342,21 @@ public: if (!b) { if (x > 0) { b |= get(x - 1, y); - if (y > 0) b |= get(x - 1, y - 1); - if (y < m_height - 1) b |= get(x - 1, y + 1); + if (y > 0) + b |= get(x - 1, y - 1); + if (y < m_height - 1) + b |= get(x - 1, y + 1); } - if (y > 0) b |= get(x, y - 1); - if (y < m_height - 1) b |= get(x, y + 1); + if (y > 0) + b |= get(x, y - 1); + if (y < m_height - 1) + b |= get(x, y + 1); if (x < m_width - 1) { b |= get(x + 1, y); - if (y > 0) b |= get(x + 1, y - 1); - if (y < m_height - 1) b |= get(x + 1, y + 1); + if (y > 0) + b |= get(x + 1, y - 1); + if (y < m_height - 1) + b |= get(x + 1, y + 1); } } if (b) @@ -1511,11 +1375,10 @@ private: }; // From Fast-BVH -class BVH -{ +class BVH { public: - BVH(const Array<AABB> &objectAabbs, uint32_t leafSize = 4) : m_objectIds(MemTag::BVH), m_nodes(MemTag::BVH) - { + BVH(const Array<AABB> &objectAabbs, uint32_t leafSize = 4) : + m_objectIds(MemTag::BVH), m_nodes(MemTag::BVH) { m_objectAabbs = &objectAabbs; if (m_objectAabbs->isEmpty()) return; @@ -1535,7 +1398,7 @@ public: Node node; m_nodes.reserve(objectAabbs.size() * 2); uint32_t nNodes = 0; - while(stackptr > 0) { + while (stackptr > 0) { // Pop the next item off of the stack const BuildEntry &bnode = todo[--stackptr]; const uint32_t start = bnode.start; @@ -1548,7 +1411,7 @@ public: // Calculate the bounding box for this node AABB bb(objectAabbs[m_objectIds[start]]); AABB bc(objectAabbs[m_objectIds[start]].centroid()); - for(uint32_t p = start + 1; p < end; ++p) { + for (uint32_t p = start + 1; p < end; ++p) { bb.expandToInclude(objectAabbs[m_objectIds[p]]); bc.expandToInclude(objectAabbs[m_objectIds[p]].centroid()); } @@ -1564,7 +1427,7 @@ public: m_nodes[bnode.parent].rightOffset--; // When this is the second touch, this is the right child. // The right child sets up the offset for the flat tree. - if (m_nodes[bnode.parent].rightOffset == kTouchedTwice ) + if (m_nodes[bnode.parent].rightOffset == kTouchedTwice) m_nodes[bnode.parent].rightOffset = nNodes - 1 - bnode.parent; } // If this is a leaf, no need to subdivide. @@ -1599,21 +1462,20 @@ public: } } - void query(const AABB &queryAabb, Array<uint32_t> &result) const - { + void query(const AABB &queryAabb, Array<uint32_t> &result) const { result.clear(); // Working set uint32_t todo[64]; int32_t stackptr = 0; // "Push" on the root node to the working set todo[stackptr] = 0; - while(stackptr >= 0) { + while (stackptr >= 0) { // Pop off the next node to work on. const int ni = todo[stackptr--]; const Node &node = m_nodes[ni]; // Is leaf -> Intersect if (node.rightOffset == 0) { - for(uint32_t o = 0; o < node.nPrims; ++o) { + for (uint32_t o = 0; o < node.nPrims; ++o) { const uint32_t obj = node.start + o; if (queryAabb.intersect((*m_objectAabbs)[m_objectIds[obj]])) result.push_back(m_objectIds[obj]); @@ -1630,14 +1492,12 @@ public: } private: - struct BuildEntry - { + struct BuildEntry { uint32_t parent; // If non-zero then this is the index of the parent. (used in offsets) uint32_t start, end; // The range of objects in the object list covered by this node. }; - struct Node - { + struct Node { AABB aabb; uint32_t start, nPrims, rightOffset; }; @@ -1647,16 +1507,14 @@ private: Array<Node> m_nodes; }; -struct Fit -{ - static bool computeBasis(const Vector3 *points, uint32_t pointsCount, Basis *basis) - { - if (computeLeastSquaresNormal(points, pointsCount, &basis->normal)) { +struct Fit { + static bool computeBasis(ConstArrayView<Vector3> points, Basis *basis) { + if (computeLeastSquaresNormal(points, &basis->normal)) { basis->tangent = Basis::computeTangent(basis->normal); basis->bitangent = Basis::computeBitangent(basis->normal, basis->tangent); return true; } - return computeEigen(points, pointsCount, basis); + return computeEigen(points, basis); } private: @@ -1664,21 +1522,20 @@ private: // Fast, and accurate to within a few degrees. // Returns None if the points do not span a plane. // https://www.ilikebigbits.com/2015_03_04_plane_from_points.html - static bool computeLeastSquaresNormal(const Vector3 *points, uint32_t pointsCount, Vector3 *normal) - { - XA_DEBUG_ASSERT(pointsCount >= 3); - if (pointsCount == 3) { - *normal = normalize(cross(points[2] - points[0], points[1] - points[0]), kEpsilon); + static bool computeLeastSquaresNormal(ConstArrayView<Vector3> points, Vector3 *normal) { + XA_DEBUG_ASSERT(points.length >= 3); + if (points.length == 3) { + *normal = normalize(cross(points[2] - points[0], points[1] - points[0])); return true; } - const float invN = 1.0f / float(pointsCount); + const float invN = 1.0f / float(points.length); Vector3 centroid(0.0f); - for (uint32_t i = 0; i < pointsCount; i++) + for (uint32_t i = 0; i < points.length; i++) centroid += points[i]; centroid *= invN; // Calculate full 3x3 covariance matrix, excluding symmetries: float xx = 0.0f, xy = 0.0f, xz = 0.0f, yy = 0.0f, yz = 0.0f, zz = 0.0f; - for (uint32_t i = 0; i < pointsCount; i++) { + for (uint32_t i = 0; i < points.length; i++) { Vector3 r = points[i] - centroid; xx += r.x * r.x; xy += r.x * r.y; @@ -1730,7 +1587,7 @@ private: // Pick path with best conditioning: Vector3 dir(0.0f); if (det_max == det_x) - dir = Vector3(det_x,xz * yz - xy * zz,xy * yz - xz * yy); + dir = Vector3(det_x, xz * yz - xy * zz, xy * yz - xz * yy); else if (det_max == det_y) dir = Vector3(xz * yz - xy * zz, det_y, xy * xz - yz * xx); else if (det_max == det_z) @@ -1743,41 +1600,37 @@ private: return isNormalized(*normal); } - static bool computeEigen(const Vector3 *points, uint32_t pointsCount, Basis *basis) - { + static bool computeEigen(ConstArrayView<Vector3> points, Basis *basis) { float matrix[6]; - computeCovariance(pointsCount, points, matrix); + computeCovariance(points, matrix); if (matrix[0] == 0 && matrix[3] == 0 && matrix[5] == 0) return false; float eigenValues[3]; Vector3 eigenVectors[3]; if (!eigenSolveSymmetric3(matrix, eigenValues, eigenVectors)) return false; - basis->normal = normalize(eigenVectors[2], kEpsilon); - basis->tangent = normalize(eigenVectors[0], kEpsilon); - basis->bitangent = normalize(eigenVectors[1], kEpsilon); + basis->normal = normalize(eigenVectors[2]); + basis->tangent = normalize(eigenVectors[0]); + basis->bitangent = normalize(eigenVectors[1]); return true; } - static Vector3 computeCentroid(int n, const Vector3 * points) - { + static Vector3 computeCentroid(ConstArrayView<Vector3> points) { Vector3 centroid(0.0f); - for (int i = 0; i < n; i++) { + for (uint32_t i = 0; i < points.length; i++) centroid += points[i]; - } - centroid /= float(n); + centroid /= float(points.length); return centroid; } - static Vector3 computeCovariance(int n, const Vector3 * points, float * covariance) - { + static Vector3 computeCovariance(ConstArrayView<Vector3> points, float *covariance) { // compute the centroid - Vector3 centroid = computeCentroid(n, points); + Vector3 centroid = computeCentroid(points); // compute covariance matrix for (int i = 0; i < 6; i++) { covariance[i] = 0.0f; } - for (int i = 0; i < n; i++) { + for (uint32_t i = 0; i < points.length; i++) { Vector3 v = points[i] - centroid; covariance[0] += v.x * v.x; covariance[1] += v.x * v.y; @@ -1792,8 +1645,7 @@ private: // Tridiagonal solver from Charles Bloom. // Householder transforms followed by QL decomposition. // Seems to be based on the code from Numerical Recipes in C. - static bool eigenSolveSymmetric3(const float matrix[6], float eigenValues[3], Vector3 eigenVectors[3]) - { + static bool eigenSolveSymmetric3(const float matrix[6], float eigenValues[3], Vector3 eigenVectors[3]) { XA_DEBUG_ASSERT(matrix != nullptr && eigenValues != nullptr && eigenVectors != nullptr); float subd[3]; float diag[3]; @@ -1818,7 +1670,7 @@ private: // eigenvectors are the columns; make them the rows : for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - (&eigenVectors[j].x)[i] = (float) work[i][j]; + (&eigenVectors[j].x)[i] = (float)work[i][j]; } } // shuffle to sort by singular value : @@ -1840,8 +1692,7 @@ private: } private: - static void EigenSolver3_Tridiagonal(float mat[3][3], float *diag, float *subd) - { + static void EigenSolver3_Tridiagonal(float mat[3][3], float *diag, float *subd) { // Householder reduction T = Q^t M Q // Input: // mat, symmetric 3x3 matrix M @@ -1893,8 +1744,7 @@ private: } } - static bool EigenSolver3_QLAlgorithm(float mat[3][3], float *diag, float *subd) - { + static bool EigenSolver3_QLAlgorithm(float mat[3][3], float *diag, float *subd) { // QL iteration with implicit shifting to reduce matrix from tridiagonal // to diagonal const int maxiter = 32; @@ -1904,21 +1754,21 @@ private: int m; for (m = ell; m <= 1; m++) { float dd = fabsf(diag[m]) + fabsf(diag[m + 1]); - if ( fabsf(subd[m]) + dd == dd ) + if (fabsf(subd[m]) + dd == dd) break; } - if ( m == ell ) + if (m == ell) break; float g = (diag[ell + 1] - diag[ell]) / (2 * subd[ell]); float r = sqrtf(g * g + 1); - if ( g < 0 ) + if (g < 0) g = diag[m] - diag[ell] + subd[ell] / (g - r); else g = diag[m] - diag[ell] + subd[ell] / (g + r); float s = 1, c = 1, p = 0; for (int i = m - 1; i >= ell; i--) { float f = s * subd[i], b = c * subd[i]; - if ( fabsf(f) >= fabsf(g) ) { + if (fabsf(f) >= fabsf(g)) { c = g / f; r = sqrtf(c * c + 1); subd[i + 1] = f * r; @@ -1944,7 +1794,7 @@ private: subd[ell] = g; subd[m] = 0; } - if ( iter == maxiter ) + if (iter == maxiter) // should not get here under normal circumstances return false; } @@ -1952,56 +1802,48 @@ private: } }; -static uint32_t sdbmHash(const void *data_in, uint32_t size, uint32_t h = 5381) -{ - const uint8_t *data = (const uint8_t *) data_in; +static uint32_t sdbmHash(const void *data_in, uint32_t size, uint32_t h = 5381) { + const uint8_t *data = (const uint8_t *)data_in; uint32_t i = 0; while (i < size) { - h = (h << 16) + (h << 6) - h + (uint32_t ) data[i++]; + h = (h << 16) + (h << 6) - h + (uint32_t)data[i++]; } return h; } template <typename T> -static uint32_t hash(const T &t, uint32_t h = 5381) -{ +static uint32_t hash(const T &t, uint32_t h = 5381) { return sdbmHash(&t, sizeof(T), h); } template <typename Key> -struct Hash -{ +struct Hash { uint32_t operator()(const Key &k) const { return hash(k); } }; template <typename Key> -struct PassthroughHash -{ +struct PassthroughHash { uint32_t operator()(const Key &k) const { return (uint32_t)k; } }; template <typename Key> -struct Equal -{ +struct Equal { bool operator()(const Key &k0, const Key &k1) const { return k0 == k1; } }; -template<typename Key, typename H = Hash<Key>, typename E = Equal<Key> > -class HashMap -{ +template <typename Key, typename H = Hash<Key>, typename E = Equal<Key>> +class HashMap { public: - HashMap(int memTag, uint32_t size) : m_memTag(memTag), m_size(size), m_numSlots(0), m_slots(nullptr), m_keys(memTag), m_next(memTag) - { + HashMap(int memTag, uint32_t size) : + m_memTag(memTag), m_size(size), m_numSlots(0), m_slots(nullptr), m_keys(memTag), m_next(memTag) { } - ~HashMap() - { + ~HashMap() { if (m_slots) XA_FREE(m_slots); } - void destroy() - { + void destroy() { if (m_slots) { XA_FREE(m_slots); m_slots = nullptr; @@ -2010,8 +1852,7 @@ public: m_next.destroy(); } - uint32_t add(const Key &key) - { + uint32_t add(const Key &key) { if (!m_slots) alloc(); const uint32_t hash = computeHash(key); @@ -2021,36 +1862,18 @@ public: return m_keys.size() - 1; } - uint32_t get(const Key &key) const - { + uint32_t get(const Key &key) const { if (!m_slots) return UINT32_MAX; - const uint32_t hash = computeHash(key); - uint32_t i = m_slots[hash]; - E equal; - while (i != UINT32_MAX) { - if (equal(m_keys[i], key)) - return i; - i = m_next[i]; - } - return UINT32_MAX; + return find(key, m_slots[computeHash(key)]); } - uint32_t getNext(uint32_t current) const - { - uint32_t i = m_next[current]; - E equal; - while (i != UINT32_MAX) { - if (equal(m_keys[i], m_keys[current])) - return i; - i = m_next[i]; - } - return UINT32_MAX; + uint32_t getNext(const Key &key, uint32_t current) const { + return find(key, m_next[current]); } private: - void alloc() - { + void alloc() { XA_DEBUG_ASSERT(m_size > 0); m_numSlots = nextPowerOfTwo(m_size); auto minNumSlots = uint32_t(m_size * 1.3); @@ -2063,12 +1886,21 @@ private: m_next.reserve(m_size); } - uint32_t computeHash(const Key &key) const - { + uint32_t computeHash(const Key &key) const { H hash; return hash(key) & (m_numSlots - 1); } + uint32_t find(const Key &key, uint32_t current) const { + E equal; + while (current != UINT32_MAX) { + if (equal(m_keys[current], key)) + return current; + current = m_next[current]; + } + return current; + } + int m_memTag; uint32_t m_size; uint32_t m_numSlots; @@ -2077,9 +1909,8 @@ private: Array<uint32_t> m_next; }; -template<typename T> -static void insertionSort(T *data, uint32_t length) -{ +template <typename T> +static void insertionSort(T *data, uint32_t length) { for (int32_t i = 1; i < (int32_t)length; i++) { T x = data[i]; int32_t j = i - 1; @@ -2091,21 +1922,18 @@ static void insertionSort(T *data, uint32_t length) } } -class KISSRng -{ +class KISSRng { public: KISSRng() { reset(); } - void reset() - { + void reset() { x = 123456789; y = 362436000; z = 521288629; c = 7654321; } - uint32_t getRange(uint32_t range) - { + uint32_t getRange(uint32_t range) { if (range == 0) return 0; x = 69069 * x + 12345; @@ -2124,12 +1952,10 @@ private: // Based on Pierre Terdiman's and Michael Herf's source code. // http://www.codercorner.com/RadixSortRevisited.htm // http://www.stereopsis.com/radix.html -class RadixSort -{ +class RadixSort { public: - void sort(const float *input, uint32_t count) - { - if (input == nullptr || count == 0) { + void sort(ConstArrayView<float> input) { + if (input.length == 0) { m_buffer1.clear(); m_buffer2.clear(); m_ranks = m_buffer1.data(); @@ -2137,33 +1963,27 @@ public: return; } // Resize lists if needed - m_buffer1.resize(count); - m_buffer2.resize(count); + m_buffer1.resize(input.length); + m_buffer2.resize(input.length); m_ranks = m_buffer1.data(); m_ranks2 = m_buffer2.data(); m_validRanks = false; - if (count < 32) - insertionSort(input, count); + if (input.length < 32) + insertionSort(input); else { // @@ Avoid touching the input multiple times. - for (uint32_t i = 0; i < count; i++) { + for (uint32_t i = 0; i < input.length; i++) { floatFlip((uint32_t &)input[i]); } - radixSort<uint32_t>((const uint32_t *)input, count); - for (uint32_t i = 0; i < count; i++) { + radixSort(ConstArrayView<uint32_t>((const uint32_t *)input.data, input.length)); + for (uint32_t i = 0; i < input.length; i++) { ifloatFlip((uint32_t &)input[i]); } } } - void sort(const Array<float> &input) - { - sort(input.data(), input.size()); - } - // Access to results. m_ranks is a list of indices in sorted order, i.e. in the order you may further process your data - const uint32_t *ranks() const - { + const uint32_t *ranks() const { XA_DEBUG_ASSERT(m_validRanks); return m_ranks; } @@ -2171,54 +1991,40 @@ public: private: uint32_t *m_ranks, *m_ranks2; Array<uint32_t> m_buffer1, m_buffer2; - bool m_validRanks; + bool m_validRanks = false; - void floatFlip(uint32_t &f) - { + void floatFlip(uint32_t &f) { int32_t mask = (int32_t(f) >> 31) | 0x80000000; // Warren Hunt, Manchor Ko. f ^= mask; } - void ifloatFlip(uint32_t &f) - { + void ifloatFlip(uint32_t &f) { uint32_t mask = ((f >> 31) - 1) | 0x80000000; // Michael Herf. f ^= mask; } - template<typename T> - void createHistograms(const T *buffer, uint32_t count, uint32_t *histogram) - { - const uint32_t bucketCount = sizeof(T); // (8 * sizeof(T)) / log2(radix) + void createHistograms(ConstArrayView<uint32_t> input, uint32_t *histogram) { + const uint32_t bucketCount = sizeof(uint32_t); // Init bucket pointers. uint32_t *h[bucketCount]; for (uint32_t i = 0; i < bucketCount; i++) { h[i] = histogram + 256 * i; } // Clear histograms. - memset(histogram, 0, 256 * bucketCount * sizeof(uint32_t )); + memset(histogram, 0, 256 * bucketCount * sizeof(uint32_t)); // @@ Add support for signed integers. // Build histograms. - const uint8_t *p = (const uint8_t *)buffer; // @@ Does this break aliasing rules? - const uint8_t *pe = p + count * sizeof(T); + const uint8_t *p = (const uint8_t *)input.data; // @@ Does this break aliasing rules? + const uint8_t *pe = p + input.length * sizeof(uint32_t); while (p != pe) { h[0][*p++]++, h[1][*p++]++, h[2][*p++]++, h[3][*p++]++; -#ifdef _MSC_VER -#pragma warning(push) -#pragma warning(disable : 4127) -#endif - if (bucketCount == 8) h[4][*p++]++, h[5][*p++]++, h[6][*p++]++, h[7][*p++]++; -#ifdef _MSC_VER -#pragma warning(pop) -#endif } } - template <typename T> - void insertionSort(const T *input, uint32_t count) - { + void insertionSort(ConstArrayView<float> input) { if (!m_validRanks) { m_ranks[0] = 0; - for (uint32_t i = 1; i != count; ++i) { + for (uint32_t i = 1; i != input.length; ++i) { int rank = m_ranks[i] = i; uint32_t j = i; while (j != 0 && input[rank] < input[m_ranks[j - 1]]) { @@ -2231,7 +2037,7 @@ private: } m_validRanks = true; } else { - for (uint32_t i = 1; i != count; ++i) { + for (uint32_t i = 1; i != input.length; ++i) { int rank = m_ranks[i]; uint32_t j = i; while (j != 0 && input[rank] < input[m_ranks[j - 1]]) { @@ -2245,35 +2051,34 @@ private: } } - template <typename T> - void radixSort(const T *input, uint32_t count) - { - const uint32_t P = sizeof(T); // pass count + void radixSort(ConstArrayView<uint32_t> input) { + const uint32_t P = sizeof(uint32_t); // pass count // Allocate histograms & offsets on the stack uint32_t histogram[256 * P]; uint32_t *link[256]; - createHistograms(input, count, histogram); + createHistograms(input, histogram); // Radix sort, j is the pass number (0=LSB, P=MSB) for (uint32_t j = 0; j < P; j++) { // Pointer to this bucket. const uint32_t *h = &histogram[j * 256]; - const uint8_t *inputBytes = (const uint8_t *)input; // @@ Is this aliasing legal? + auto inputBytes = (const uint8_t *)input.data; // @@ Is this aliasing legal? inputBytes += j; - if (h[inputBytes[0]] == count) { + if (h[inputBytes[0]] == input.length) { // Skip this pass, all values are the same. continue; } // Create offsets link[0] = m_ranks2; - for (uint32_t i = 1; i < 256; i++) link[i] = link[i - 1] + h[i - 1]; + for (uint32_t i = 1; i < 256; i++) + link[i] = link[i - 1] + h[i - 1]; // Perform Radix Sort if (!m_validRanks) { - for (uint32_t i = 0; i < count; i++) { + for (uint32_t i = 0; i < input.length; i++) { *link[inputBytes[i * P]]++ = i; } m_validRanks = true; } else { - for (uint32_t i = 0; i < count; i++) { + for (uint32_t i = 0; i < input.length; i++) { const uint32_t idx = m_ranks[i]; *link[inputBytes[idx * P]]++ = idx; } @@ -2283,7 +2088,7 @@ private: } // All values were equal, generate linear ranks. if (!m_validRanks) { - for (uint32_t i = 0; i < count; i++) + for (uint32_t i = 0; i < input.length; i++) m_ranks[i] = i; m_validRanks = true; } @@ -2291,30 +2096,25 @@ private: }; // Wrapping this in a class allows temporary arrays to be re-used. -class BoundingBox2D -{ +class BoundingBox2D { public: Vector2 majorAxis, minorAxis, minCorner, maxCorner; - void clear() - { + void clear() { m_boundaryVertices.clear(); } - void appendBoundaryVertex(Vector2 v) - { + void appendBoundaryVertex(Vector2 v) { m_boundaryVertices.push_back(v); } // This should compute convex hull and use rotating calipers to find the best box. Currently it uses a brute force method. - // If vertices is null or vertexCount is 0, the boundary vertices are used. - void compute(const Vector2 *vertices = nullptr, uint32_t vertexCount = 0) - { - if (!vertices || vertexCount == 0) { - vertices = m_boundaryVertices.data(); - vertexCount = m_boundaryVertices.size(); - } - convexHull(m_boundaryVertices.data(), m_boundaryVertices.size(), m_hull, 0.00001f); + // If vertices are empty, the boundary vertices are used. + void compute(ConstArrayView<Vector2> vertices = ConstArrayView<Vector2>()) { + XA_DEBUG_ASSERT(!m_boundaryVertices.isEmpty()); + if (vertices.length == 0) + vertices = m_boundaryVertices; + convexHull(m_boundaryVertices, m_hull, 0.00001f); // @@ Ideally I should use rotating calipers to find the best box. Using brute force for now. float best_area = FLT_MAX; Vector2 best_min(0); @@ -2324,13 +2124,13 @@ public: for (uint32_t i = 0, j = hullCount - 1; i < hullCount; j = i, i++) { if (equal(m_hull[i], m_hull[j], kEpsilon)) continue; - Vector2 axis = normalize(m_hull[i] - m_hull[j], 0.0f); + Vector2 axis = normalize(m_hull[i] - m_hull[j]); XA_DEBUG_ASSERT(isFinite(axis)); // Compute bounding box. Vector2 box_min(FLT_MAX, FLT_MAX); Vector2 box_max(-FLT_MAX, -FLT_MAX); // Consider all points, not only boundary points, in case the input chart is malformed. - for (uint32_t v = 0; v < vertexCount; v++) { + for (uint32_t v = 0; v < vertices.length; v++) { const Vector2 &point = vertices[v]; const float x = dot(axis, point); const float y = dot(Vector2(-axis.y, axis.x), point); @@ -2357,28 +2157,27 @@ public: private: // Compute the convex hull using Graham Scan. - void convexHull(const Vector2 *input, uint32_t inputCount, Array<Vector2> &output, float epsilon) - { - m_coords.resize(inputCount); - for (uint32_t i = 0; i < inputCount; i++) + void convexHull(ConstArrayView<Vector2> input, Array<Vector2> &output, float epsilon) { + m_coords.resize(input.length); + for (uint32_t i = 0; i < input.length; i++) m_coords[i] = input[i].x; m_radix.sort(m_coords); const uint32_t *ranks = m_radix.ranks(); m_top.clear(); m_bottom.clear(); - m_top.reserve(inputCount); - m_bottom.reserve(inputCount); + m_top.reserve(input.length); + m_bottom.reserve(input.length); Vector2 P = input[ranks[0]]; - Vector2 Q = input[ranks[inputCount - 1]]; + Vector2 Q = input[ranks[input.length - 1]]; float topy = max(P.y, Q.y); float boty = min(P.y, Q.y); - for (uint32_t i = 0; i < inputCount; i++) { + for (uint32_t i = 0; i < input.length; i++) { Vector2 p = input[ranks[i]]; if (p.y >= boty) m_top.push_back(p); } - for (uint32_t i = 0; i < inputCount; i++) { - Vector2 p = input[ranks[inputCount - 1 - i]]; + for (uint32_t i = 0; i < input.length; i++) { + Vector2 p = input[ranks[input.length - 1 - i]]; if (p.y <= topy) m_bottom.push_back(p); } @@ -2387,7 +2186,7 @@ private: XA_DEBUG_ASSERT(m_top.size() >= 2); output.push_back(m_top[0]); output.push_back(m_top[1]); - for (uint32_t i = 2; i < m_top.size(); ) { + for (uint32_t i = 2; i < m_top.size();) { Vector2 a = output[output.size() - 2]; Vector2 b = output[output.size() - 1]; Vector2 c = m_top[i]; @@ -2403,7 +2202,7 @@ private: XA_DEBUG_ASSERT(m_bottom.size() >= 2); output.push_back(m_bottom[1]); // Filter bottom list. - for (uint32_t i = 2; i < m_bottom.size(); ) { + for (uint32_t i = 2; i < m_bottom.size();) { Vector2 a = output[output.size() - 2]; Vector2 b = output[output.size() - 1]; Vector2 c = m_bottom[i]; @@ -2426,32 +2225,45 @@ private: RadixSort m_radix; }; -static uint32_t meshEdgeFace(uint32_t edge) { return edge / 3; } -static uint32_t meshEdgeIndex0(uint32_t edge) { return edge; } +struct EdgeKey { + EdgeKey(const EdgeKey &k) : + v0(k.v0), v1(k.v1) {} + EdgeKey(uint32_t _v0, uint32_t _v1) : + v0(_v0), v1(_v1) {} + bool operator==(const EdgeKey &k) const { return v0 == k.v0 && v1 == k.v1; } -static uint32_t meshEdgeIndex1(uint32_t edge) -{ + uint32_t v0; + uint32_t v1; +}; + +struct EdgeHash { + uint32_t operator()(const EdgeKey &k) const { return k.v0 * 32768u + k.v1; } +}; + +static uint32_t meshEdgeFace(uint32_t edge) { + return edge / 3; +} +static uint32_t meshEdgeIndex0(uint32_t edge) { + return edge; +} + +static uint32_t meshEdgeIndex1(uint32_t edge) { const uint32_t faceFirstEdge = edge / 3 * 3; return faceFirstEdge + (edge - faceFirstEdge + 1) % 3; } -struct MeshFlags -{ - enum - { - HasIgnoredFaces = 1<<0, - HasNormals = 1<<1 +struct MeshFlags { + enum { + HasIgnoredFaces = 1 << 0, + HasNormals = 1 << 1, + HasMaterials = 1 << 2 }; }; -class Mesh; -static void meshGetBoundaryLoops(const Mesh &mesh, Array<uint32_t> &boundaryLoops); - -class Mesh -{ +class Mesh { public: - Mesh(float epsilon, uint32_t approxVertexCount, uint32_t approxFaceCount, uint32_t flags = 0, uint32_t id = UINT32_MAX) : m_epsilon(epsilon), m_flags(flags), m_id(id), m_faceIgnore(MemTag::Mesh), m_indices(MemTag::MeshIndices), m_positions(MemTag::MeshPositions), m_normals(MemTag::MeshNormals), m_texcoords(MemTag::MeshTexcoords), m_nextColocalVertex(MemTag::MeshColocals), m_boundaryEdges(MemTag::MeshBoundaries), m_oppositeEdges(MemTag::MeshBoundaries), m_nextBoundaryEdges(MemTag::MeshBoundaries), m_edgeMap(MemTag::MeshEdgeMap, approxFaceCount * 3) - { + Mesh(float epsilon, uint32_t approxVertexCount, uint32_t approxFaceCount, uint32_t flags = 0, uint32_t id = UINT32_MAX) : + m_epsilon(epsilon), m_flags(flags), m_id(id), m_faceIgnore(MemTag::Mesh), m_faceMaterials(MemTag::Mesh), m_indices(MemTag::MeshIndices), m_positions(MemTag::MeshPositions), m_normals(MemTag::MeshNormals), m_texcoords(MemTag::MeshTexcoords), m_nextColocalVertex(MemTag::MeshColocals), m_firstColocalVertex(MemTag::MeshColocals), m_boundaryEdges(MemTag::MeshBoundaries), m_oppositeEdges(MemTag::MeshBoundaries), m_edgeMap(MemTag::MeshEdgeMap, approxFaceCount * 3) { m_indices.reserve(approxFaceCount * 3); m_positions.reserve(approxVertexCount); m_texcoords.reserve(approxVertexCount); @@ -2459,13 +2271,14 @@ public: m_faceIgnore.reserve(approxFaceCount); if (m_flags & MeshFlags::HasNormals) m_normals.reserve(approxVertexCount); + if (m_flags & MeshFlags::HasMaterials) + m_faceMaterials.reserve(approxFaceCount); } uint32_t flags() const { return m_flags; } uint32_t id() const { return m_id; } - void addVertex(const Vector3 &pos, const Vector3 &normal = Vector3(0.0f), const Vector2 &texcoord = Vector2(0.0f)) - { + void addVertex(const Vector3 &pos, const Vector3 &normal = Vector3(0.0f), const Vector2 &texcoord = Vector2(0.0f)) { XA_DEBUG_ASSERT(isFinite(pos)); m_positions.push_back(pos); if (m_flags & MeshFlags::HasNormals) @@ -2473,45 +2286,22 @@ public: m_texcoords.push_back(texcoord); } - struct AddFaceResult - { - enum Enum - { - OK, - DuplicateEdge = 1 - }; - }; - - AddFaceResult::Enum addFace(uint32_t v0, uint32_t v1, uint32_t v2, bool ignore = false) - { - uint32_t indexArray[3]; - indexArray[0] = v0; - indexArray[1] = v1; - indexArray[2] = v2; - return addFace(indexArray, ignore); - } - - AddFaceResult::Enum addFace(const uint32_t *indices, bool ignore = false) - { - AddFaceResult::Enum result = AddFaceResult::OK; + void addFace(const uint32_t *indices, bool ignore = false, uint32_t material = UINT32_MAX) { if (m_flags & MeshFlags::HasIgnoredFaces) m_faceIgnore.push_back(ignore); + if (m_flags & MeshFlags::HasMaterials) + m_faceMaterials.push_back(material); const uint32_t firstIndex = m_indices.size(); for (uint32_t i = 0; i < 3; i++) m_indices.push_back(indices[i]); for (uint32_t i = 0; i < 3; i++) { const uint32_t vertex0 = m_indices[firstIndex + i]; const uint32_t vertex1 = m_indices[firstIndex + (i + 1) % 3]; - const EdgeKey key(vertex0, vertex1); - if (m_edgeMap.get(key) != UINT32_MAX) - result = AddFaceResult::DuplicateEdge; - m_edgeMap.add(key); + m_edgeMap.add(EdgeKey(vertex0, vertex1)); } - return result; } - void createColocals() - { + void createColocalsBVH() { const uint32_t vertexCount = m_positions.size(); Array<AABB> aabbs(MemTag::BVH); aabbs.resize(vertexCount); @@ -2522,6 +2312,8 @@ public: Array<uint32_t> potential(MemTag::MeshColocals); m_nextColocalVertex.resize(vertexCount); m_nextColocalVertex.fillBytes(0xff); + m_firstColocalVertex.resize(vertexCount); + m_firstColocalVertex.fillBytes(0xff); for (uint32_t i = 0; i < vertexCount; i++) { if (m_nextColocalVertex[i] != UINT32_MAX) continue; // Already linked. @@ -2537,18 +2329,65 @@ public: if (colocals.size() == 1) { // No colocals for this vertex. m_nextColocalVertex[i] = i; - continue; + m_firstColocalVertex[i] = i; + continue; } // Link in ascending order. insertionSort(colocals.data(), colocals.size()); - for (uint32_t j = 0; j < colocals.size(); j++) + for (uint32_t j = 0; j < colocals.size(); j++) { m_nextColocalVertex[colocals[j]] = colocals[(j + 1) % colocals.size()]; + m_firstColocalVertex[colocals[j]] = colocals[0]; + } XA_DEBUG_ASSERT(m_nextColocalVertex[i] != UINT32_MAX); } } - void createBoundaries() - { + void createColocalsHash() { + const uint32_t vertexCount = m_positions.size(); + HashMap<Vector3> positionToVertexMap(MemTag::Default, vertexCount); + for (uint32_t i = 0; i < vertexCount; i++) + positionToVertexMap.add(m_positions[i]); + Array<uint32_t> colocals(MemTag::MeshColocals); + m_nextColocalVertex.resize(vertexCount); + m_nextColocalVertex.fillBytes(0xff); + m_firstColocalVertex.resize(vertexCount); + m_firstColocalVertex.fillBytes(0xff); + for (uint32_t i = 0; i < vertexCount; i++) { + if (m_nextColocalVertex[i] != UINT32_MAX) + continue; // Already linked. + // Find other vertices colocal to this one. + colocals.clear(); + colocals.push_back(i); // Always add this vertex. + uint32_t otherVertex = positionToVertexMap.get(m_positions[i]); + while (otherVertex != UINT32_MAX) { + if (otherVertex != i && equal(m_positions[i], m_positions[otherVertex], m_epsilon) && m_nextColocalVertex[otherVertex] == UINT32_MAX) + colocals.push_back(otherVertex); + otherVertex = positionToVertexMap.getNext(m_positions[i], otherVertex); + } + if (colocals.size() == 1) { + // No colocals for this vertex. + m_nextColocalVertex[i] = i; + m_firstColocalVertex[i] = i; + continue; + } + // Link in ascending order. + insertionSort(colocals.data(), colocals.size()); + for (uint32_t j = 0; j < colocals.size(); j++) { + m_nextColocalVertex[colocals[j]] = colocals[(j + 1) % colocals.size()]; + m_firstColocalVertex[colocals[j]] = colocals[0]; + } + XA_DEBUG_ASSERT(m_nextColocalVertex[i] != UINT32_MAX); + } + } + + void createColocals() { + if (m_epsilon <= FLT_EPSILON) + createColocalsHash(); + else + createColocalsBVH(); + } + + void createBoundaries() { const uint32_t edgeCount = m_indices.size(); const uint32_t vertexCount = m_positions.size(); m_oppositeEdges.resize(edgeCount); @@ -2578,151 +2417,54 @@ public: } } - void linkBoundaries() - { - const uint32_t edgeCount = m_indices.size(); - HashMap<uint32_t> vertexToEdgeMap(MemTag::Mesh, edgeCount); // Edge is index / 2 - for (uint32_t i = 0; i < edgeCount; i++) { - vertexToEdgeMap.add(m_indices[meshEdgeIndex0(i)]); - vertexToEdgeMap.add(m_indices[meshEdgeIndex1(i)]); - } - m_nextBoundaryEdges.resize(edgeCount); - for (uint32_t i = 0; i < edgeCount; i++) - m_nextBoundaryEdges[i] = UINT32_MAX; - uint32_t numBoundaryLoops = 0, numUnclosedBoundaries = 0; - BitArray linkedEdges(edgeCount); - linkedEdges.zeroOutMemory(); - for (;;) { - // Find the first boundary edge that hasn't been linked yet. - uint32_t firstEdge = UINT32_MAX; - for (uint32_t i = 0; i < edgeCount; i++) { - if (m_oppositeEdges[i] == UINT32_MAX && !linkedEdges.get(i)) { - firstEdge = i; - break; - } - } - if (firstEdge == UINT32_MAX) - break; - uint32_t currentEdge = firstEdge; - for (;;) { - // Find the next boundary edge. The first vertex will be the same as (or colocal to) the current edge second vertex. - const uint32_t startVertex = m_indices[meshEdgeIndex1(currentEdge)]; - uint32_t bestNextEdge = UINT32_MAX; - for (ColocalVertexIterator it(this, startVertex); !it.isDone(); it.advance()) { - uint32_t mapIndex = vertexToEdgeMap.get(it.vertex()); - while (mapIndex != UINT32_MAX) { - const uint32_t otherEdge = mapIndex / 2; // Two vertices added per edge. - if (m_oppositeEdges[otherEdge] != UINT32_MAX) - goto next; // Not a boundary edge. - if (linkedEdges.get(otherEdge)) - goto next; // Already linked. - if (m_indices[meshEdgeIndex0(otherEdge)] != it.vertex()) - goto next; // Edge contains the vertex, but it's the wrong one. - // First edge (closing the boundary loop) has the highest priority. - // Non-colocal vertex has the next highest. - if (bestNextEdge != firstEdge && (bestNextEdge == UINT32_MAX || it.vertex() == startVertex)) - bestNextEdge = otherEdge; - next: - mapIndex = vertexToEdgeMap.getNext(mapIndex); - } - } - if (bestNextEdge == UINT32_MAX) { - numUnclosedBoundaries++; - if (currentEdge == firstEdge) - linkedEdges.set(firstEdge); // Only 1 edge in this boundary "loop". - break; // Can't find a next edge. - } - m_nextBoundaryEdges[currentEdge] = bestNextEdge; - linkedEdges.set(bestNextEdge); - currentEdge = bestNextEdge; - if (currentEdge == firstEdge) { - numBoundaryLoops++; - break; // Closed the boundary loop. - } - } - } -#if XA_FIX_INTERNAL_BOUNDARY_LOOPS - // Find internal boundary loops and separate them. - // Detect by finding two edges in a boundary loop that have a colocal end vertex. - // Fix by swapping their next boundary edge. - // Need to start over after every fix since known boundary loops have changed. - Array<uint32_t> boundaryLoops; - fixInternalBoundary: - meshGetBoundaryLoops(*this, boundaryLoops); - for (uint32_t loop = 0; loop < boundaryLoops.size(); loop++) { - linkedEdges.zeroOutMemory(); - for (Mesh::BoundaryLoopEdgeIterator it1(this, boundaryLoops[loop]); !it1.isDone(); it1.advance()) { - const uint32_t e1 = it1.edge(); - if (linkedEdges.get(e1)) - continue; - for (Mesh::BoundaryLoopEdgeIterator it2(this, boundaryLoops[loop]); !it2.isDone(); it2.advance()) { - const uint32_t e2 = it2.edge(); - if (e1 == e2 || !isBoundaryEdge(e2) || linkedEdges.get(e2)) - continue; - if (!areColocal(m_indices[meshEdgeIndex1(e1)], m_indices[meshEdgeIndex1(e2)])) - continue; - swap(m_nextBoundaryEdges[e1], m_nextBoundaryEdges[e2]); - linkedEdges.set(e1); - linkedEdges.set(e2); - goto fixInternalBoundary; // start over - } - } - } -#endif - } - /// Find edge, test all colocals. - uint32_t findEdge(uint32_t vertex0, uint32_t vertex1) const - { - uint32_t result = UINT32_MAX; - if (m_nextColocalVertex.isEmpty()) { + uint32_t findEdge(uint32_t vertex0, uint32_t vertex1) const { + // Try to find exact vertex match first. + { EdgeKey key(vertex0, vertex1); uint32_t edge = m_edgeMap.get(key); while (edge != UINT32_MAX) { // Don't find edges of ignored faces. - if (!isFaceIgnored(meshEdgeFace(edge))) { - //XA_DEBUG_ASSERT(m_id != UINT32_MAX || (m_id == UINT32_MAX && result == UINT32_MAX)); // duplicate edge - ignore on initial meshes - result = edge; -#if !XA_DEBUG - return result; -#endif - } - edge = m_edgeMap.getNext(edge); + if (!isFaceIgnored(meshEdgeFace(edge))) + return edge; + edge = m_edgeMap.getNext(key, edge); } - } else { - for (ColocalVertexIterator it0(this, vertex0); !it0.isDone(); it0.advance()) { - for (ColocalVertexIterator it1(this, vertex1); !it1.isDone(); it1.advance()) { - EdgeKey key(it0.vertex(), it1.vertex()); + } + // If colocals were created, try every permutation. + if (!m_nextColocalVertex.isEmpty()) { + uint32_t colocalVertex0 = vertex0; + for (;;) { + uint32_t colocalVertex1 = vertex1; + for (;;) { + EdgeKey key(colocalVertex0, colocalVertex1); uint32_t edge = m_edgeMap.get(key); while (edge != UINT32_MAX) { // Don't find edges of ignored faces. - if (!isFaceIgnored(meshEdgeFace(edge))) { - XA_DEBUG_ASSERT(m_id != UINT32_MAX || (m_id == UINT32_MAX && result == UINT32_MAX)); // duplicate edge - ignore on initial meshes - result = edge; -#if !XA_DEBUG - return result; -#endif - } - edge = m_edgeMap.getNext(edge); + if (!isFaceIgnored(meshEdgeFace(edge))) + return edge; + edge = m_edgeMap.getNext(key, edge); } + colocalVertex1 = m_nextColocalVertex[colocalVertex1]; + if (colocalVertex1 == vertex1) + break; // Back to start. } + colocalVertex0 = m_nextColocalVertex[colocalVertex0]; + if (colocalVertex0 == vertex0) + break; // Back to start. } } - return result; + return UINT32_MAX; } // Edge map can be destroyed when no longer used to reduce memory usage. It's used by: // * Mesh::createBoundaries() - // * Mesh::ColocalEdgeIterator (used by MeshFaceGroups) - // * meshCloseHole() - void destroyEdgeMap() - { + // * Mesh::edgeMap() (used by MeshFaceGroups) + void destroyEdgeMap() { m_edgeMap.destroy(); } #if XA_DEBUG_EXPORT_OBJ - void writeObjVertices(FILE *file) const - { + void writeObjVertices(FILE *file) const { for (uint32_t i = 0; i < m_positions.size(); i++) fprintf(file, "v %g %g %g\n", m_positions[i].x, m_positions[i].y, m_positions[i].z); if (m_flags & MeshFlags::HasNormals) { @@ -2733,8 +2475,7 @@ public: fprintf(file, "vt %g %g\n", m_texcoords[i].x, m_texcoords[i].y); } - void writeObjFace(FILE *file, uint32_t face, uint32_t offset = 0) const - { + void writeObjFace(FILE *file, uint32_t face, uint32_t offset = 0) const { fprintf(file, "f "); for (uint32_t j = 0; j < 3; j++) { const uint32_t index = m_indices[face * 3 + j] + 1 + offset; // 1-indexed @@ -2742,8 +2483,7 @@ public: } } - void writeObjBoundaryEges(FILE *file) const - { + void writeObjBoundaryEges(FILE *file) const { if (m_oppositeEdges.isEmpty()) return; // Boundaries haven't been created. fprintf(file, "o boundary_edges\n"); @@ -2754,31 +2494,7 @@ public: } } - void writeObjLinkedBoundaries(FILE *file) const - { - if (m_oppositeEdges.isEmpty() || m_nextBoundaryEdges.isEmpty()) - return; // Boundaries haven't been created and/or linked. - Array<uint32_t> boundaryLoops; - meshGetBoundaryLoops(*this, boundaryLoops); - for (uint32_t i = 0; i < boundaryLoops.size(); i++) { - uint32_t edge = boundaryLoops[i]; - fprintf(file, "o boundary_%04d\n", i); - fprintf(file, "l"); - for (;;) { - const uint32_t vertex0 = m_indices[meshEdgeIndex0(edge)]; - const uint32_t vertex1 = m_indices[meshEdgeIndex1(edge)]; - fprintf(file, " %d", vertex0 + 1); // 1-indexed - edge = m_nextBoundaryEdges[edge]; - if (edge == boundaryLoops[i] || edge == UINT32_MAX) { - fprintf(file, " %d\n", vertex1 + 1); // 1-indexed - break; - } - } - } - } - - void writeObjFile(const char *filename) const - { + void writeObjFile(const char *filename) const { FILE *file; XA_FOPEN(file, filename, "w"); if (!file) @@ -2789,13 +2505,11 @@ public: for (uint32_t i = 0; i < faceCount(); i++) writeObjFace(file, i); writeObjBoundaryEges(file); - writeObjLinkedBoundaries(file); fclose(file); } #endif - float computeSurfaceArea() const - { + float computeSurfaceArea() const { float area = 0; for (uint32_t f = 0; f < faceCount(); f++) area += computeFaceArea(f); @@ -2804,24 +2518,21 @@ public: } // Returned value is always positive, even if some triangles are flipped. - float computeParametricArea() const - { + float computeParametricArea() const { float area = 0; for (uint32_t f = 0; f < faceCount(); f++) area += fabsf(computeFaceParametricArea(f)); // May be negative, depends on texcoord winding. - return area; + return area; } - float computeFaceArea(uint32_t face) const - { + float computeFaceArea(uint32_t face) const { const Vector3 &p0 = m_positions[m_indices[face * 3 + 0]]; const Vector3 &p1 = m_positions[m_indices[face * 3 + 1]]; const Vector3 &p2 = m_positions[m_indices[face * 3 + 2]]; return length(cross(p1 - p0, p2 - p0)) * 0.5f; } - Vector3 computeFaceCentroid(uint32_t face) const - { + Vector3 computeFaceCentroid(uint32_t face) const { Vector3 sum(0.0f); for (uint32_t i = 0; i < 3; i++) sum += m_positions[m_indices[face * 3 + i]]; @@ -2830,8 +2541,7 @@ public: // Average of the edge midpoints weighted by the edge length. // I want a point inside the triangle, but closer to the cirumcenter. - Vector3 computeFaceCenter(uint32_t face) const - { + Vector3 computeFaceCenter(uint32_t face) const { const Vector3 &p0 = m_positions[m_indices[face * 3 + 0]]; const Vector3 &p1 = m_positions[m_indices[face * 3 + 1]]; const Vector3 &p2 = m_positions[m_indices[face * 3 + 2]]; @@ -2844,28 +2554,25 @@ public: return m0 + m1 + m2; } - Vector3 computeFaceNormal(uint32_t face) const - { + Vector3 computeFaceNormal(uint32_t face) const { const Vector3 &p0 = m_positions[m_indices[face * 3 + 0]]; const Vector3 &p1 = m_positions[m_indices[face * 3 + 1]]; const Vector3 &p2 = m_positions[m_indices[face * 3 + 2]]; const Vector3 e0 = p2 - p0; const Vector3 e1 = p1 - p0; const Vector3 normalAreaScaled = cross(e0, e1); - return normalizeSafe(normalAreaScaled, Vector3(0, 0, 1), 0.0f); + return normalizeSafe(normalAreaScaled, Vector3(0, 0, 1)); } - float computeFaceParametricArea(uint32_t face) const - { + float computeFaceParametricArea(uint32_t face) const { const Vector2 &t0 = m_texcoords[m_indices[face * 3 + 0]]; const Vector2 &t1 = m_texcoords[m_indices[face * 3 + 1]]; const Vector2 &t2 = m_texcoords[m_indices[face * 3 + 2]]; return triangleArea(t0, t1, t2); } - + // @@ This is not exactly accurate, we should compare the texture coordinates... - bool isSeam(uint32_t edge) const - { + bool isSeam(uint32_t edge) const { const uint32_t oppositeEdge = m_oppositeEdges[edge]; if (oppositeEdge == UINT32_MAX) return false; // boundary edge @@ -2876,8 +2583,7 @@ public: return m_indices[e0] != m_indices[oe1] || m_indices[e1] != m_indices[oe0]; } - bool isTextureSeam(uint32_t edge) const - { + bool isTextureSeam(uint32_t edge) const { const uint32_t oppositeEdge = m_oppositeEdges[edge]; if (oppositeEdge == UINT32_MAX) return false; // boundary edge @@ -2888,26 +2594,9 @@ public: return m_texcoords[m_indices[e0]] != m_texcoords[m_indices[oe1]] || m_texcoords[m_indices[e1]] != m_texcoords[m_indices[oe0]]; } - uint32_t firstColocal(uint32_t vertex) const - { - for (ColocalVertexIterator it(this, vertex); !it.isDone(); it.advance()) { - if (it.vertex() < vertex) - vertex = it.vertex(); - } - return vertex; - } - - bool areColocal(uint32_t vertex0, uint32_t vertex1) const - { - if (vertex0 == vertex1) - return true; - if (m_nextColocalVertex.isEmpty()) - return false; - for (ColocalVertexIterator it(this, vertex0); !it.isDone(); it.advance()) { - if (it.vertex() == vertex1) - return true; - } - return false; + uint32_t firstColocalVertex(uint32_t vertex) const { + XA_DEBUG_ASSERT(m_firstColocalVertex.size() == m_positions.size()); + return m_firstColocalVertex[vertex]; } XA_INLINE float epsilon() const { return m_epsilon; } @@ -2919,23 +2608,28 @@ public: XA_INLINE uint32_t vertexCount() const { return m_positions.size(); } XA_INLINE uint32_t vertexAt(uint32_t i) const { return m_indices[i]; } XA_INLINE const Vector3 &position(uint32_t vertex) const { return m_positions[vertex]; } - XA_INLINE const Vector3 *positions() const { return m_positions.data(); } - XA_INLINE const Vector3 &normal(uint32_t vertex) const { XA_DEBUG_ASSERT(m_flags & MeshFlags::HasNormals); return m_normals[vertex]; } + XA_INLINE ConstArrayView<Vector3> positions() const { return m_positions; } + XA_INLINE const Vector3 &normal(uint32_t vertex) const { + XA_DEBUG_ASSERT(m_flags & MeshFlags::HasNormals); + return m_normals[vertex]; + } XA_INLINE const Vector2 &texcoord(uint32_t vertex) const { return m_texcoords[vertex]; } XA_INLINE Vector2 &texcoord(uint32_t vertex) { return m_texcoords[vertex]; } - XA_INLINE const Vector2 *texcoords() const { return m_texcoords.data(); } - XA_INLINE Vector2 *texcoords() { return m_texcoords.data(); } + XA_INLINE const ConstArrayView<Vector2> texcoords() const { return m_texcoords; } + XA_INLINE ArrayView<Vector2> texcoords() { return m_texcoords; } XA_INLINE uint32_t faceCount() const { return m_indices.size() / 3; } - XA_INLINE const uint32_t *indices() const { return m_indices.data(); } + XA_INLINE ConstArrayView<uint32_t> indices() const { return m_indices; } XA_INLINE uint32_t indexCount() const { return m_indices.size(); } XA_INLINE bool isFaceIgnored(uint32_t face) const { return (m_flags & MeshFlags::HasIgnoredFaces) && m_faceIgnore[face]; } + XA_INLINE uint32_t faceMaterial(uint32_t face) const { return (m_flags & MeshFlags::HasMaterials) ? m_faceMaterials[face] : UINT32_MAX; } + XA_INLINE const HashMap<EdgeKey, EdgeHash> &edgeMap() const { return m_edgeMap; } private: - float m_epsilon; uint32_t m_flags; uint32_t m_id; Array<bool> m_faceIgnore; + Array<uint32_t> m_faceMaterials; Array<uint32_t> m_indices; Array<Vector3> m_positions; Array<Vector3> m_normals; @@ -2943,205 +2637,31 @@ private: // Populated by createColocals Array<uint32_t> m_nextColocalVertex; // In: vertex index. Out: the vertex index of the next colocal position. + Array<uint32_t> m_firstColocalVertex; // Populated by createBoundaries BitArray m_isBoundaryVertex; Array<uint32_t> m_boundaryEdges; Array<uint32_t> m_oppositeEdges; // In: edge index. Out: the index of the opposite edge (i.e. wound the opposite direction). UINT32_MAX if the input edge is a boundary edge. - // Populated by linkBoundaries - Array<uint32_t> m_nextBoundaryEdges; // The index of the next boundary edge. UINT32_MAX if the edge is not a boundary edge. - - struct EdgeKey - { - EdgeKey(const EdgeKey &k) : v0(k.v0), v1(k.v1) {} - EdgeKey(uint32_t v0, uint32_t v1) : v0(v0), v1(v1) {} - bool operator==(const EdgeKey &k) const { return v0 == k.v0 && v1 == k.v1; } - - uint32_t v0; - uint32_t v1; - }; - - struct EdgeHash - { - uint32_t operator()(const EdgeKey &k) const { return k.v0 * 32768u + k.v1; } - }; - HashMap<EdgeKey, EdgeHash> m_edgeMap; public: - class BoundaryLoopEdgeIterator - { + class FaceEdgeIterator { public: - BoundaryLoopEdgeIterator(const Mesh *mesh, uint32_t edge) : m_mesh(mesh), m_first(UINT32_MAX), m_current(edge) {} - - void advance() - { - if (m_first == UINT32_MAX) - m_first = m_current; - m_current = m_mesh->m_nextBoundaryEdges[m_current]; - } - - bool isDone() const - { - return m_first == m_current || m_current == UINT32_MAX; - } - - uint32_t edge() const - { - return m_current; - } - - uint32_t nextEdge() const - { - return m_mesh->m_nextBoundaryEdges[m_current]; - } - - private: - const Mesh *m_mesh; - uint32_t m_first; - uint32_t m_current; - }; - - class ColocalVertexIterator - { - public: - ColocalVertexIterator(const Mesh *mesh, uint32_t v) : m_mesh(mesh), m_first(UINT32_MAX), m_current(v) {} - - void advance() - { - if (m_first == UINT32_MAX) - m_first = m_current; - if (!m_mesh->m_nextColocalVertex.isEmpty()) - m_current = m_mesh->m_nextColocalVertex[m_current]; - } - - bool isDone() const - { - return m_first == m_current; - } - - uint32_t vertex() const - { - return m_current; - } - - const Vector3 *pos() const - { - return &m_mesh->m_positions[m_current]; - } - - private: - const Mesh *m_mesh; - uint32_t m_first; - uint32_t m_current; - }; - - class ColocalEdgeIterator - { - public: - ColocalEdgeIterator(const Mesh *mesh, uint32_t vertex0, uint32_t vertex1) : m_mesh(mesh), m_vertex0It(mesh, vertex0), m_vertex1It(mesh, vertex1), m_vertex1(vertex1) - { - do { - if (!resetElement()) { - advanceVertex1(); - } - else { - break; - } - } while (!isDone()); - } - - void advance() - { - advanceElement(); - } - - bool isDone() const - { - return m_vertex0It.isDone() && m_vertex1It.isDone() && m_edge == UINT32_MAX; - } - - uint32_t edge() const - { - return m_edge; - } - - private: - bool resetElement() - { - m_edge = m_mesh->m_edgeMap.get(Mesh::EdgeKey(m_vertex0It.vertex(), m_vertex1It.vertex())); - while (m_edge != UINT32_MAX) { - if (!isIgnoredFace()) - break; - m_edge = m_mesh->m_edgeMap.getNext(m_edge); - } - if (m_edge == UINT32_MAX) { - return false; - } - return true; - } - - void advanceElement() - { - for (;;) { - m_edge = m_mesh->m_edgeMap.getNext(m_edge); - if (m_edge == UINT32_MAX) - break; - if (!isIgnoredFace()) - break; - } - if (m_edge == UINT32_MAX) - advanceVertex1(); - } - - void advanceVertex1() - { - auto successful = false; - while (!successful) { - m_vertex1It.advance(); - if (m_vertex1It.isDone()) { - if (!m_vertex0It.isDone()) { - m_vertex0It.advance(); - m_vertex1It = ColocalVertexIterator(m_mesh, m_vertex1); - } - else { - return; - } - } - successful = resetElement(); - } - } - - bool isIgnoredFace() const - { - return m_mesh->m_faceIgnore[meshEdgeFace(m_edge)]; - } - - const Mesh *m_mesh; - ColocalVertexIterator m_vertex0It, m_vertex1It; - const uint32_t m_vertex1; - uint32_t m_edge; - }; - - class FaceEdgeIterator - { - public: - FaceEdgeIterator (const Mesh *mesh, uint32_t face) : m_mesh(mesh), m_face(face), m_relativeEdge(0) - { + FaceEdgeIterator(const Mesh *mesh, uint32_t face) : + m_mesh(mesh), m_face(face), m_relativeEdge(0) { m_edge = m_face * 3; } - void advance() - { + void advance() { if (m_relativeEdge < 3) { m_edge++; m_relativeEdge++; } } - bool isDone() const - { + bool isDone() const { return m_relativeEdge == 3; } @@ -3152,9 +2672,8 @@ public: uint32_t relativeEdge() const { return m_relativeEdge; } uint32_t face() const { return m_face; } uint32_t oppositeEdge() const { return m_mesh->m_oppositeEdges[m_edge]; } - - uint32_t oppositeFace() const - { + + uint32_t oppositeFace() const { const uint32_t oedge = m_mesh->m_oppositeEdges[m_edge]; if (oedge == UINT32_MAX) return UINT32_MAX; @@ -3178,19 +2697,18 @@ public: }; }; -struct MeshFaceGroups -{ +struct MeshFaceGroups { typedef uint32_t Handle; static constexpr Handle kInvalid = UINT32_MAX; - MeshFaceGroups(const Mesh *mesh) : m_mesh(mesh), m_groups(MemTag::Mesh), m_firstFace(MemTag::Mesh), m_nextFace(MemTag::Mesh), m_faceCount(MemTag::Mesh) {} + MeshFaceGroups(const Mesh *mesh) : + m_mesh(mesh), m_groups(MemTag::Mesh), m_firstFace(MemTag::Mesh), m_nextFace(MemTag::Mesh), m_faceCount(MemTag::Mesh) {} XA_INLINE Handle groupAt(uint32_t face) const { return m_groups[face]; } XA_INLINE uint32_t groupCount() const { return m_faceCount.size(); } XA_INLINE uint32_t nextFace(uint32_t face) const { return m_nextFace[face]; } XA_INLINE uint32_t faceCount(uint32_t group) const { return m_faceCount[group]; } - void compute() - { + void compute() { m_groups.resize(m_mesh->faceCount()); m_groups.fillBytes(0xff); // Set all faces to kInvalid uint32_t firstUnassignedFace = 0; @@ -3222,57 +2740,25 @@ struct MeshFaceGroups break; const uint32_t f = growFaces.back(); growFaces.pop_back(); + const uint32_t material = m_mesh->faceMaterial(f); for (Mesh::FaceEdgeIterator edgeIt(m_mesh, f); !edgeIt.isDone(); edgeIt.advance()) { - // Iterate opposite edges. There may be more than one - non-manifold geometry can have duplicate edges. - // Prioritize the one with exact vertex match, not just colocal. - // If *any* of the opposite edges are already assigned to this group, don't do anything. - bool alreadyAssignedToThisGroup = false; - uint32_t bestConnectedFace = UINT32_MAX; - for (Mesh::ColocalEdgeIterator oppositeEdgeIt(m_mesh, edgeIt.vertex1(), edgeIt.vertex0()); !oppositeEdgeIt.isDone(); oppositeEdgeIt.advance()) { - const uint32_t oppositeEdge = oppositeEdgeIt.edge(); - const uint32_t oppositeFace = meshEdgeFace(oppositeEdge); -#if 0 - // Reject opposite face if dihedral angle >= 90 degrees. - { - Vector3 a = m_mesh->computeFaceNormal(f); - Vector3 b = m_mesh->computeFaceNormal(oppositeFace); - if (dot(a, b) <= 0.0f) - continue; - } -#endif - if (m_mesh->isFaceIgnored(oppositeFace)) - continue; // Don't add ignored faces to group. - if (m_groups[oppositeFace] == group) { - alreadyAssignedToThisGroup = true; - break; - } - if (m_groups[oppositeFace] != kInvalid) - continue; // Connected face is already assigned to another group. - if (faceDuplicatesGroupEdge(group, oppositeFace)) - continue; // Don't want duplicate edges in a group. - const uint32_t oppositeVertex0 = m_mesh->vertexAt(meshEdgeIndex0(oppositeEdge)); - const uint32_t oppositeVertex1 = m_mesh->vertexAt(meshEdgeIndex1(oppositeEdge)); - if (bestConnectedFace == UINT32_MAX || (oppositeVertex0 == edgeIt.vertex1() && oppositeVertex1 == edgeIt.vertex0())) - bestConnectedFace = oppositeFace; -#if 0 - else { - // Choose the opposite face with the smallest dihedral angle. - const float d1 = 1.0f - dot(computeFaceNormal(f), computeFaceNormal(bestConnectedFace)); - const float d2 = 1.0f - dot(computeFaceNormal(f), computeFaceNormal(oppositeFace)); - if (d2 < d1) - bestConnectedFace = oppositeFace; - } -#endif - } - if (!alreadyAssignedToThisGroup && bestConnectedFace != UINT32_MAX) { - m_groups[bestConnectedFace] = group; - m_nextFace[bestConnectedFace] = UINT32_MAX; - if (prevFace != UINT32_MAX) - m_nextFace[prevFace] = bestConnectedFace; - prevFace = bestConnectedFace; - groupFaceCount++; - growFaces.push_back(bestConnectedFace); - } + const uint32_t oppositeEdge = m_mesh->findEdge(edgeIt.vertex1(), edgeIt.vertex0()); + if (oppositeEdge == UINT32_MAX) + continue; // Boundary edge. + const uint32_t oppositeFace = meshEdgeFace(oppositeEdge); + if (m_mesh->isFaceIgnored(oppositeFace)) + continue; // Don't add ignored faces to group. + if (m_mesh->faceMaterial(oppositeFace) != material) + continue; // Different material. + if (m_groups[oppositeFace] != kInvalid) + continue; // Connected face is already assigned to another group. + m_groups[oppositeFace] = group; + m_nextFace[oppositeFace] = UINT32_MAX; + if (prevFace != UINT32_MAX) + m_nextFace[prevFace] = oppositeFace; + prevFace = oppositeFace; + groupFaceCount++; + growFaces.push_back(oppositeFace); } } m_faceCount.push_back(groupFaceCount); @@ -3281,27 +2767,23 @@ struct MeshFaceGroups } } - class Iterator - { + class Iterator { public: - Iterator(const MeshFaceGroups *meshFaceGroups, Handle group) : m_meshFaceGroups(meshFaceGroups) - { + Iterator(const MeshFaceGroups *meshFaceGroups, Handle group) : + m_meshFaceGroups(meshFaceGroups) { XA_DEBUG_ASSERT(group != kInvalid); m_current = m_meshFaceGroups->m_firstFace[group]; } - void advance() - { + void advance() { m_current = m_meshFaceGroups->m_nextFace[m_current]; } - bool isDone() const - { + bool isDone() const { return m_current == UINT32_MAX; } - uint32_t face() const - { + uint32_t face() const { return m_current; } @@ -3311,18 +2793,6 @@ struct MeshFaceGroups }; private: - // Check if the face duplicates any edges of any face already in the group. - bool faceDuplicatesGroupEdge(Handle group, uint32_t face) const - { - for (Mesh::FaceEdgeIterator edgeIt(m_mesh, face); !edgeIt.isDone(); edgeIt.advance()) { - for (Mesh::ColocalEdgeIterator colocalEdgeIt(m_mesh, edgeIt.vertex0(), edgeIt.vertex1()); !colocalEdgeIt.isDone(); colocalEdgeIt.advance()) { - if (m_groups[meshEdgeFace(colocalEdgeIt.edge())] == group) - return true; - } - } - return false; - } - const Mesh *m_mesh; Array<Handle> m_groups; Array<uint32_t> m_firstFace; @@ -3332,243 +2802,27 @@ private: constexpr MeshFaceGroups::Handle MeshFaceGroups::kInvalid; -static bool meshCloseHole(Mesh *mesh, const Array<uint32_t> &holeVertices, const Vector3 &normal) -{ -#if XA_CLOSE_HOLES_CHECK_EDGE_INTERSECTION - const uint32_t faceCount = mesh->faceCount(); -#endif - const bool compareNormal = equal(normal, Vector3(0.0f), FLT_EPSILON); - uint32_t frontCount = holeVertices.size(); - Array<uint32_t> frontVertices; - Array<Vector3> frontPoints; - Array<float> frontAngles; - frontVertices.resize(frontCount); - frontPoints.resize(frontCount); - for (uint32_t i = 0; i < frontCount; i++) { - frontVertices[i] = holeVertices[i]; - frontPoints[i] = mesh->position(frontVertices[i]); - } - while (frontCount >= 3) { - frontAngles.resize(frontCount); - float smallestAngle = kPi2, smallestAngleIgnoringNormal = kPi2; - uint32_t smallestAngleIndex = UINT32_MAX, smallestAngleIndexIgnoringNormal = UINT32_MAX; - for (uint32_t i = 0; i < frontCount; i++) { - const uint32_t i1 = i == 0 ? frontCount - 1 : i - 1; - const uint32_t i2 = i; - const uint32_t i3 = (i + 1) % frontCount; - const Vector3 edge1 = frontPoints[i1] - frontPoints[i2]; - const Vector3 edge2 = frontPoints[i3] - frontPoints[i2]; - frontAngles[i] = atan2f(length(cross(edge1, edge2)), dot(edge1, edge2)); - if (frontAngles[i] >= smallestAngle || isNan(frontAngles[i])) - continue; - // Don't duplicate edges. - if (mesh->findEdge(frontVertices[i1], frontVertices[i2]) != UINT32_MAX) - continue; - if (mesh->findEdge(frontVertices[i2], frontVertices[i3]) != UINT32_MAX) - continue; - if (mesh->findEdge(frontVertices[i3], frontVertices[i1]) != UINT32_MAX) - continue; - /* - Make sure he new edge that would be formed by (i3, i1) doesn't intersect any vertices. This often happens when fixing t-junctions. - - i2 - * - / \ - / \ - i1 *--*--* i3 - \ | / - \|/ - * - */ - bool intersection = false; - for (uint32_t j = 0; j < frontCount; j++) { - if (j == i1 || j == i2 || j == i3) - continue; - if (lineIntersectsPoint(frontPoints[j], frontPoints[i3], frontPoints[i1], nullptr, mesh->epsilon())) { - intersection = true; - break; - } - } - if (intersection) - continue; - // Don't add the triangle if a boundary point lies on the same plane as the triangle, and is inside it. - intersection = false; - const Plane plane(frontPoints[i1], frontPoints[i2], frontPoints[i3]); - for (uint32_t j = 0; j < frontCount; j++) { - if (j == i1 || j == i2 || j == i3) - continue; - if (!isZero(plane.distance(frontPoints[j]), mesh->epsilon())) - continue; - if (pointInTriangle(frontPoints[j], frontPoints[i1], frontPoints[i2], frontPoints[i3])) { - intersection = true; - break; - } - } - if (intersection) - continue; -#if XA_CLOSE_HOLES_CHECK_EDGE_INTERSECTION - // Don't add the triangle if the new edge (i3, i1), intersects any other triangle that isn't part of the filled hole. - intersection = false; - const Vector3 newEdgeVector = frontPoints[i1] - frontPoints[i3]; - for (uint32_t f = 0; f < faceCount; f++) { - Vector3 tri[3]; - for (uint32_t j = 0; j < 3; j++) - tri[j] = mesh->position(mesh->vertexAt(f * 3 + j)); - float t; - if (rayIntersectsTriangle(frontPoints[i3], newEdgeVector, tri, &t)) { - intersection = true; - break; - } - } - if (intersection) - continue; -#endif - // Skip backwards facing triangles. - if (compareNormal) { - if (frontAngles[i] < smallestAngleIgnoringNormal) { - smallestAngleIgnoringNormal = frontAngles[i]; - smallestAngleIndexIgnoringNormal = i; - } - const Vector3 e0 = frontPoints[i3] - frontPoints[i1]; - const Vector3 e1 = frontPoints[i2] - frontPoints[i1]; - const Vector3 triNormal = normalizeSafe(cross(e0, e1), Vector3(0.0f), mesh->epsilon()); - if (dot(normal, triNormal) <= 0.0f) - continue; - } - smallestAngle = smallestAngleIgnoringNormal = frontAngles[i]; - smallestAngleIndex = smallestAngleIndexIgnoringNormal = i; - } - // Closing holes failed if we don't have a smallest angle. - // Fallback to ignoring the backwards facing normal test if possible. - if (smallestAngleIndex == UINT32_MAX || smallestAngle <= 0.0f || smallestAngle >= kPi) { - if (smallestAngleIgnoringNormal == UINT32_MAX || smallestAngleIgnoringNormal <= 0.0f || smallestAngleIgnoringNormal >= kPi) - return false; - else - smallestAngleIndex = smallestAngleIndexIgnoringNormal; - } - const uint32_t i1 = smallestAngleIndex == 0 ? frontCount - 1 : smallestAngleIndex - 1; - const uint32_t i2 = smallestAngleIndex; - const uint32_t i3 = (smallestAngleIndex + 1) % frontCount; - const Mesh::AddFaceResult::Enum result = mesh->addFace(frontVertices[i1], frontVertices[i2], frontVertices[i3]); - XA_DEBUG_ASSERT(result == Mesh::AddFaceResult::OK); // Shouldn't happen due to the findEdge calls above. - XA_UNUSED(result); - frontVertices.removeAt(i2); - frontPoints.removeAt(i2); - frontCount = frontVertices.size(); - } - return true; -} - -static bool meshCloseHoles(Mesh *mesh, const Array<uint32_t> &boundaryLoops, const Vector3 &normal, uint32_t *holeCount, Array<uint32_t> *holeFaceCounts) -{ - if (holeFaceCounts) - holeFaceCounts->clear(); - // Compute lengths. - const uint32_t boundaryCount = boundaryLoops.size(); - Array<float> boundaryLengths; - Array<uint32_t> boundaryEdgeCounts; - boundaryEdgeCounts.resize(boundaryCount); - for (uint32_t i = 0; i < boundaryCount; i++) { - float boundaryLength = 0.0f; - boundaryEdgeCounts[i] = 0; - for (Mesh::BoundaryLoopEdgeIterator it(mesh, boundaryLoops[i]); !it.isDone(); it.advance()) { - const Vector3 &t0 = mesh->position(mesh->vertexAt(meshEdgeIndex0(it.edge()))); - const Vector3 &t1 = mesh->position(mesh->vertexAt(meshEdgeIndex1(it.edge()))); - boundaryLength += length(t1 - t0); - boundaryEdgeCounts[i]++; - } - boundaryLengths.push_back(boundaryLength); - } - // Find disk boundary. - uint32_t diskBoundary = 0; - float maxLength = boundaryLengths[0]; - for (uint32_t i = 1; i < boundaryCount; i++) { - if (boundaryLengths[i] > maxLength) { - maxLength = boundaryLengths[i]; - diskBoundary = i; - } - } - // Close holes. - Array<uint32_t> holeVertices; - Array<Vector3> holePoints; - bool result = true; - for (uint32_t i = 0; i < boundaryCount; i++) { - if (diskBoundary == i) - continue; // Skip disk boundary. - holeVertices.resize(boundaryEdgeCounts[i]); - holePoints.resize(boundaryEdgeCounts[i]); - // Winding is backwards for internal boundaries. - uint32_t e = 0; - for (Mesh::BoundaryLoopEdgeIterator it(mesh, boundaryLoops[i]); !it.isDone(); it.advance()) { - const uint32_t vertex = mesh->vertexAt(meshEdgeIndex0(it.edge())); - holeVertices[boundaryEdgeCounts[i] - 1 - e] = vertex; - holePoints[boundaryEdgeCounts[i] - 1 - e] = mesh->position(vertex); - e++; - } - const uint32_t oldFaceCount = mesh->faceCount(); - if (!meshCloseHole(mesh, holeVertices, normal)) - result = false; // Return false if any hole failed to close, but keep trying to close other holes. - if (holeCount) - (*holeCount)++; - if (holeFaceCounts) - holeFaceCounts->push_back(mesh->faceCount() - oldFaceCount); - } - return result; -} - -static bool meshIsPlanar(const Mesh &mesh) -{ - const Vector3 p1 = mesh.position(mesh.vertexAt(0)); - const Vector3 p2 = mesh.position(mesh.vertexAt(1)); - const Vector3 p3 = mesh.position(mesh.vertexAt(2)); - const Plane plane(p1, p2, p3); - const uint32_t vertexCount = mesh.vertexCount(); - for (uint32_t v = 0; v < vertexCount; v++) { - const float d = plane.distance(mesh.position(v)); - if (!isZero(d, mesh.epsilon())) - return false; - } - return true; -} - -/* -Fixing T-junctions. - -- Find T-junctions. Find vertices that are on an edge. -- This test is approximate. -- Insert edges on a spatial index to speedup queries. -- Consider only open edges, that is edges that have no pairs. -- Consider only vertices on boundaries. -- Close T-junction. -- Split edge. - -*/ -struct SplitEdge -{ - uint32_t edge; - float t; - uint32_t vertex; - - bool operator<(const SplitEdge &other) const - { - if (edge < other.edge) - return true; - else if (edge == other.edge) { - if (t < other.t) - return true; - } +#if XA_CHECK_T_JUNCTIONS +static bool lineIntersectsPoint(const Vector3 &point, const Vector3 &lineStart, const Vector3 &lineEnd, float *t, float epsilon) { + float tt; + if (!t) + t = &tt; + *t = 0.0f; + if (equal(lineStart, point, epsilon) || equal(lineEnd, point, epsilon)) + return false; // Vertex lies on either line vertices. + const Vector3 v01 = point - lineStart; + const Vector3 v21 = lineEnd - lineStart; + const float l = length(v21); + const float d = length(cross(v01, v21)) / l; + if (!isZero(d, epsilon)) return false; - } -}; + *t = dot(v01, v21) / (l * l); + return *t > kEpsilon && *t < 1.0f - kEpsilon; +} -// Returns nullptr if there were no t-junctions to fix. -static Mesh *meshFixTJunctions(const Mesh &inputMesh, bool *duplicatedEdge, bool *failed, uint32_t *fixedTJunctionsCount) -{ - if (duplicatedEdge) - *duplicatedEdge = false; - if (failed) - *failed = false; - Array<SplitEdge> splitEdges; +// Returns the number of T-junctions found. +static int meshCheckTJunctions(const Mesh &inputMesh) { + int count = 0; const uint32_t vertexCount = inputMesh.vertexCount(); const uint32_t edgeCount = inputMesh.edgeCount(); for (uint32_t v = 0; v < vertexCount; v++) { @@ -3582,155 +2836,130 @@ static Mesh *meshFixTJunctions(const Mesh &inputMesh, bool *duplicatedEdge, bool const Vector3 &edgePos1 = inputMesh.position(inputMesh.vertexAt(meshEdgeIndex0(e))); const Vector3 &edgePos2 = inputMesh.position(inputMesh.vertexAt(meshEdgeIndex1(e))); float t; - if (!lineIntersectsPoint(pos, edgePos1, edgePos2, &t, inputMesh.epsilon())) - continue; - SplitEdge splitEdge; - splitEdge.edge = e; - splitEdge.t = t; - splitEdge.vertex = v; - splitEdges.push_back(splitEdge); + if (lineIntersectsPoint(pos, edgePos1, edgePos2, &t, inputMesh.epsilon())) + count++; } } - if (splitEdges.isEmpty()) - return nullptr; - const uint32_t faceCount = inputMesh.faceCount(); - Mesh *mesh = XA_NEW_ARGS(MemTag::Mesh, Mesh, inputMesh.epsilon(), vertexCount + splitEdges.size(), faceCount); - for (uint32_t v = 0; v < vertexCount; v++) - mesh->addVertex(inputMesh.position(v)); - Array<uint32_t> indexArray; - indexArray.reserve(4); - Array<SplitEdge> faceSplitEdges; - faceSplitEdges.reserve(4); - for (uint32_t f = 0; f < faceCount; f++) { - // Find t-junctions in this face. - faceSplitEdges.clear(); - for (uint32_t i = 0; i < splitEdges.size(); i++) { - if (meshEdgeFace(splitEdges[i].edge) == f) - faceSplitEdges.push_back(splitEdges[i]); - } - if (!faceSplitEdges.isEmpty()) { - // Need to split edges in winding order when a single edge has multiple t-junctions. - insertionSort(faceSplitEdges.data(), faceSplitEdges.size()); - indexArray.clear(); - for (Mesh::FaceEdgeIterator it(&inputMesh, f); !it.isDone(); it.advance()) { - indexArray.push_back(it.vertex0()); - for (uint32_t se = 0; se < faceSplitEdges.size(); se++) { - const SplitEdge &splitEdge = faceSplitEdges[se]; - if (splitEdge.edge == it.edge()) - indexArray.push_back(splitEdge.vertex); + return count; +} +#endif + +// References invalid faces and vertices in a mesh. +struct InvalidMeshGeometry { + // If meshFaceGroups is not null, invalid faces have the face group MeshFaceGroups::kInvalid. + // If meshFaceGroups is null, invalid faces are Mesh::isFaceIgnored. + void extract(const Mesh *mesh, const MeshFaceGroups *meshFaceGroups) { + // Copy invalid faces. + m_faces.clear(); + const uint32_t meshFaceCount = mesh->faceCount(); + for (uint32_t f = 0; f < meshFaceCount; f++) { + if ((meshFaceGroups && meshFaceGroups->groupAt(f) == MeshFaceGroups::kInvalid) || (!meshFaceGroups && mesh->isFaceIgnored(f))) + m_faces.push_back(f); + } + // Create *unique* list of vertices of invalid faces. + const uint32_t faceCount = m_faces.size(); + m_indices.resize(faceCount * 3); + const uint32_t approxVertexCount = min(faceCount * 3, mesh->vertexCount()); + m_vertexToSourceVertexMap.clear(); + m_vertexToSourceVertexMap.reserve(approxVertexCount); + HashMap<uint32_t, PassthroughHash<uint32_t>> sourceVertexToVertexMap(MemTag::Mesh, approxVertexCount); + for (uint32_t f = 0; f < faceCount; f++) { + const uint32_t face = m_faces[f]; + for (uint32_t i = 0; i < 3; i++) { + const uint32_t vertex = mesh->vertexAt(face * 3 + i); + uint32_t newVertex = sourceVertexToVertexMap.get(vertex); + if (newVertex == UINT32_MAX) { + newVertex = sourceVertexToVertexMap.add(vertex); + m_vertexToSourceVertexMap.push_back(vertex); } - } - if (!meshCloseHole(mesh, indexArray, Vector3(0.0f))) { - if (failed) - *failed = true; - } - } else { - // No t-junctions in this face. Copy from input mesh. - if (mesh->addFace(&inputMesh.indices()[f * 3]) == Mesh::AddFaceResult::DuplicateEdge) { - if (duplicatedEdge) - *duplicatedEdge = true; + m_indices[f * 3 + i] = newVertex; } } } - if (fixedTJunctionsCount) - *fixedTJunctionsCount = splitEdges.size(); - return mesh; -} -// boundaryLoops are the first edges for each boundary loop. -static void meshGetBoundaryLoops(const Mesh &mesh, Array<uint32_t> &boundaryLoops) -{ - const uint32_t edgeCount = mesh.edgeCount(); - BitArray bitFlags(edgeCount); - bitFlags.zeroOutMemory(); - boundaryLoops.clear(); - // Search for boundary edges. Mark all the edges that belong to the same boundary. - for (uint32_t e = 0; e < edgeCount; e++) { - if (bitFlags.get(e) || !mesh.isBoundaryEdge(e)) - continue; - for (Mesh::BoundaryLoopEdgeIterator it(&mesh, e); !it.isDone(); it.advance()) - bitFlags.set(it.edge()); - boundaryLoops.push_back(e); - } -} + ConstArrayView<uint32_t> faces() const { return m_faces; } + ConstArrayView<uint32_t> indices() const { return m_indices; } + ConstArrayView<uint32_t> vertices() const { return m_vertexToSourceVertexMap; } -struct Progress -{ - Progress(ProgressCategory::Enum category, ProgressFunc func, void *userData, uint32_t maxValue) : value(0), cancel(false), m_category(category), m_func(func), m_userData(userData), m_maxValue(maxValue), m_progress(0) - { +private: + Array<uint32_t> m_faces, m_indices; + Array<uint32_t> m_vertexToSourceVertexMap; // Map face vertices to vertices of the source mesh. +}; + +struct Progress { + Progress(ProgressCategory category, ProgressFunc func, void *userData, uint32_t maxValue) : + cancel(false), m_category(category), m_func(func), m_userData(userData), m_value(0), m_maxValue(maxValue), m_percent(0) { if (m_func) { if (!m_func(category, 0, userData)) cancel = true; } } - ~Progress() - { + ~Progress() { if (m_func) { if (!m_func(m_category, 100, m_userData)) cancel = true; } } - void update() - { - if (!m_func) - return; - m_mutex.lock(); - const uint32_t newProgress = uint32_t(ceilf(value.load() / (float)m_maxValue * 100.0f)); - if (newProgress != m_progress && newProgress < 100) { - m_progress = newProgress; - if (!m_func(m_category, m_progress, m_userData)) - cancel = true; - } - m_mutex.unlock(); + void increment(uint32_t value) { + m_value += value; + update(); } - void setMaxValue(uint32_t maxValue) - { - m_mutex.lock(); + void setMaxValue(uint32_t maxValue) { m_maxValue = maxValue; - m_mutex.unlock(); + update(); } - std::atomic<uint32_t> value; std::atomic<bool> cancel; private: - ProgressCategory::Enum m_category; + void update() { + if (!m_func) + return; + const uint32_t newPercent = uint32_t(ceilf(m_value.load() / (float)m_maxValue.load() * 100.0f)); + if (newPercent != m_percent) { + // Atomic max. + uint32_t oldPercent = m_percent; + while (oldPercent < newPercent && !m_percent.compare_exchange_weak(oldPercent, newPercent)) { + } + if (!m_func(m_category, m_percent, m_userData)) + cancel = true; + } + } + + ProgressCategory m_category; ProgressFunc m_func; void *m_userData; - uint32_t m_maxValue; - uint32_t m_progress; - std::mutex m_mutex; + std::atomic<uint32_t> m_value, m_maxValue, m_percent; }; -struct Spinlock -{ - void lock() { while(m_lock.test_and_set(std::memory_order_acquire)) {} } +struct Spinlock { + void lock() { + while (m_lock.test_and_set(std::memory_order_acquire)) { + } + } void unlock() { m_lock.clear(std::memory_order_release); } private: std::atomic_flag m_lock = ATOMIC_FLAG_INIT; }; -struct TaskGroupHandle -{ +struct TaskGroupHandle { uint32_t value = UINT32_MAX; }; -struct Task -{ - void (*func)(void *userData); - void *userData; +struct Task { + void (*func)(void *groupUserData, void *taskUserData); + void *userData; // Passed to func as taskUserData. }; #if XA_MULTITHREADED -class TaskScheduler -{ +class TaskScheduler { public: - TaskScheduler() : m_shutdown(false) - { + TaskScheduler() : + m_shutdown(false) { m_threadIndex = 0; // Max with current task scheduler usage is 1 per thread + 1 deep nesting, but allow for some slop. m_maxGroups = std::thread::hardware_concurrency() * 4; @@ -3739,6 +2968,7 @@ public: new (&m_groups[i]) TaskGroup(); m_groups[i].free = true; m_groups[i].ref = 0; + m_groups[i].userData = nullptr; } m_workers.resize(std::thread::hardware_concurrency() <= 1 ? 1 : std::thread::hardware_concurrency() - 1); for (uint32_t i = 0; i < m_workers.size(); i++) { @@ -3748,8 +2978,7 @@ public: } } - ~TaskScheduler() - { + ~TaskScheduler() { m_shutdown = true; for (uint32_t i = 0; i < m_workers.size(); i++) { Worker &worker = m_workers[i]; @@ -3767,13 +2996,12 @@ public: XA_FREE(m_groups); } - uint32_t threadCount() const - { + uint32_t threadCount() const { return max(1u, std::thread::hardware_concurrency()); // Including the main thread. } - TaskGroupHandle createTaskGroup(uint32_t reserveSize = 0) - { + // userData is passed to Task::func as groupUserData. + TaskGroupHandle createTaskGroup(void *userData = nullptr, uint32_t reserveSize = 0) { // Claim the first free group. for (uint32_t i = 0; i < m_maxGroups; i++) { TaskGroup &group = m_groups[i]; @@ -3785,6 +3013,8 @@ public: group.queue.clear(); group.queue.reserve(reserveSize); group.queueLock.unlock(); + group.userData = userData; + group.ref = 0; TaskGroupHandle handle; handle.value = i; return handle; @@ -3795,8 +3025,7 @@ public: return handle; } - void run(TaskGroupHandle handle, const Task &task) - { + void run(TaskGroupHandle handle, const Task &task) { XA_DEBUG_ASSERT(handle.value != UINT32_MAX); TaskGroup &group = m_groups[handle.value]; group.queueLock.lock(); @@ -3810,8 +3039,7 @@ public: } } - void wait(TaskGroupHandle *handle) - { + void wait(TaskGroupHandle *handle) { if (handle->value == UINT32_MAX) { XA_DEBUG_ASSERT(false); return; @@ -3826,7 +3054,7 @@ public: group.queueLock.unlock(); if (!task) break; - task->func(task->userData); + task->func(group.userData, task->userData); group.ref--; } // Even though the task queue is empty, workers can still be running tasks. @@ -3839,17 +3067,16 @@ public: static uint32_t currentThreadIndex() { return m_threadIndex; } private: - struct TaskGroup - { + struct TaskGroup { std::atomic<bool> free; Array<Task> queue; // Items are never removed. queueHead is incremented to pop items. uint32_t queueHead = 0; Spinlock queueLock; std::atomic<uint32_t> ref; // Increment when a task is enqueued, decrement when a task finishes. + void *userData; }; - struct Worker - { + struct Worker { std::thread *thread = nullptr; std::mutex mutex; std::condition_variable cv; @@ -3862,12 +3089,11 @@ private: uint32_t m_maxGroups; static thread_local uint32_t m_threadIndex; - static void workerThread(TaskScheduler *scheduler, Worker *worker, uint32_t threadIndex) - { + static void workerThread(TaskScheduler *scheduler, Worker *worker, uint32_t threadIndex) { m_threadIndex = threadIndex; std::unique_lock<std::mutex> lock(worker->mutex); for (;;) { - worker->cv.wait(lock, [=]{ return worker->wakeup.load(); }); + worker->cv.wait(lock, [=] { return worker->wakeup.load(); }); worker->wakeup = false; for (;;) { if (scheduler->m_shutdown) @@ -3889,7 +3115,7 @@ private: } if (!task) break; - task->func(task->userData); + task->func(group->userData, task->userData); group->ref--; } } @@ -3898,44 +3124,39 @@ private: thread_local uint32_t TaskScheduler::m_threadIndex; #else -class TaskScheduler -{ +class TaskScheduler { public: - ~TaskScheduler() - { + ~TaskScheduler() { for (uint32_t i = 0; i < m_groups.size(); i++) destroyGroup({ i }); } - uint32_t threadCount() const - { + uint32_t threadCount() const { return 1; } - TaskGroupHandle createTaskGroup(uint32_t reserveSize = 0) - { + TaskGroupHandle createTaskGroup(void *userData = nullptr, uint32_t reserveSize = 0) { TaskGroup *group = XA_NEW(MemTag::Default, TaskGroup); group->queue.reserve(reserveSize); + group->userData = userData; m_groups.push_back(group); TaskGroupHandle handle; handle.value = m_groups.size() - 1; return handle; } - void run(TaskGroupHandle handle, Task task) - { + void run(TaskGroupHandle handle, Task task) { m_groups[handle.value]->queue.push_back(task); } - void wait(TaskGroupHandle *handle) - { + void wait(TaskGroupHandle *handle) { if (handle->value == UINT32_MAX) { XA_DEBUG_ASSERT(false); return; } TaskGroup *group = m_groups[handle->value]; for (uint32_t i = 0; i < group->queue.size(); i++) - group->queue[i].func(group->queue[i].userData); + group->queue[i].func(group->userData, group->queue[i].userData); group->queue.clear(); destroyGroup(*handle); handle->value = UINT32_MAX; @@ -3944,8 +3165,7 @@ public: static uint32_t currentThreadIndex() { return 0; } private: - void destroyGroup(TaskGroupHandle handle) - { + void destroyGroup(TaskGroupHandle handle) { TaskGroup *group = m_groups[handle.value]; if (group) { group->~TaskGroup(); @@ -3954,9 +3174,9 @@ private: } } - struct TaskGroup - { + struct TaskGroup { Array<Task> queue; + void *userData; }; Array<TaskGroup *> m_groups; @@ -3968,8 +3188,7 @@ const uint8_t TGA_TYPE_RGB = 2; const uint8_t TGA_ORIGIN_UPPER = 0x20; #pragma pack(push, 1) -struct TgaHeader -{ +struct TgaHeader { uint8_t id_length; uint8_t colormap_type; uint8_t image_type; @@ -3986,8 +3205,7 @@ struct TgaHeader }; #pragma pack(pop) -static void WriteTga(const char *filename, const uint8_t *data, uint32_t width, uint32_t height) -{ +static void WriteTga(const char *filename, const uint8_t *data, uint32_t width, uint32_t height) { XA_DEBUG_ASSERT(sizeof(TgaHeader) == TgaHeader::Size); FILE *f; XA_FOPEN(f, filename, "wb"); @@ -4012,12 +3230,10 @@ static void WriteTga(const char *filename, const uint8_t *data, uint32_t width, } #endif -template<typename T> -class ThreadLocal -{ +template <typename T> +class ThreadLocal { public: - ThreadLocal() - { + ThreadLocal() { #if XA_MULTITHREADED const uint32_t n = std::thread::hardware_concurrency(); #else @@ -4028,8 +3244,7 @@ public: new (&m_array[i]) T; } - ~ThreadLocal() - { + ~ThreadLocal() { #if XA_MULTITHREADED const uint32_t n = std::thread::hardware_concurrency(); #else @@ -4040,8 +3255,7 @@ public: XA_FREE(m_array); } - T &get() const - { + T &get() const { return m_array[TaskScheduler::currentThreadIndex()]; } @@ -4049,11 +3263,104 @@ private: T *m_array; }; -class UniformGrid2 -{ +// Implemented as a struct so the temporary arrays can be reused. +struct Triangulator { + // This is doing a simple ear-clipping algorithm that skips invalid triangles. Ideally, we should + // also sort the ears by angle, start with the ones that have the smallest angle and proceed in order. + void triangulatePolygon(ConstArrayView<Vector3> vertices, ConstArrayView<uint32_t> inputIndices, Array<uint32_t> &outputIndices) { + m_polygonVertices.clear(); + m_polygonVertices.reserve(inputIndices.length); + outputIndices.clear(); + if (inputIndices.length == 3) { + // Simple case for triangles. + outputIndices.push_back(inputIndices[0]); + outputIndices.push_back(inputIndices[1]); + outputIndices.push_back(inputIndices[2]); + } else { + // Build 2D polygon projecting vertices onto normal plane. + // Faces are not necesarily planar, this is for example the case, when the face comes from filling a hole. In such cases + // it's much better to use the best fit plane. + Basis basis; + basis.normal = normalize(cross(vertices[inputIndices[1]] - vertices[inputIndices[0]], vertices[inputIndices[2]] - vertices[inputIndices[1]])); + basis.tangent = basis.computeTangent(basis.normal); + basis.bitangent = basis.computeBitangent(basis.normal, basis.tangent); + const uint32_t edgeCount = inputIndices.length; + m_polygonPoints.clear(); + m_polygonPoints.reserve(edgeCount); + m_polygonAngles.clear(); + m_polygonAngles.reserve(edgeCount); + for (uint32_t i = 0; i < inputIndices.length; i++) { + m_polygonVertices.push_back(inputIndices[i]); + const Vector3 &pos = vertices[inputIndices[i]]; + m_polygonPoints.push_back(Vector2(dot(basis.tangent, pos), dot(basis.bitangent, pos))); + } + m_polygonAngles.resize(edgeCount); + while (m_polygonVertices.size() > 2) { + const uint32_t size = m_polygonVertices.size(); + // Update polygon angles. @@ Update only those that have changed. + float minAngle = kPi2; + uint32_t bestEar = 0; // Use first one if none of them is valid. + bool bestIsValid = false; + for (uint32_t i = 0; i < size; i++) { + uint32_t i0 = i; + uint32_t i1 = (i + 1) % size; // Use Sean's polygon interation trick. + uint32_t i2 = (i + 2) % size; + Vector2 p0 = m_polygonPoints[i0]; + Vector2 p1 = m_polygonPoints[i1]; + Vector2 p2 = m_polygonPoints[i2]; + float d = clamp(dot(p0 - p1, p2 - p1) / (length(p0 - p1) * length(p2 - p1)), -1.0f, 1.0f); + float angle = acosf(d); + float area = triangleArea(p0, p1, p2); + if (area < 0.0f) + angle = kPi2 - angle; + m_polygonAngles[i1] = angle; + if (angle < minAngle || !bestIsValid) { + // Make sure this is a valid ear, if not, skip this point. + bool valid = true; + for (uint32_t j = 0; j < size; j++) { + if (j == i0 || j == i1 || j == i2) + continue; + Vector2 p = m_polygonPoints[j]; + if (pointInTriangle(p, p0, p1, p2)) { + valid = false; + break; + } + } + if (valid || !bestIsValid) { + minAngle = angle; + bestEar = i1; + bestIsValid = valid; + } + } + } + // Clip best ear: + const uint32_t i0 = (bestEar + size - 1) % size; + const uint32_t i1 = (bestEar + 0) % size; + const uint32_t i2 = (bestEar + 1) % size; + outputIndices.push_back(m_polygonVertices[i0]); + outputIndices.push_back(m_polygonVertices[i1]); + outputIndices.push_back(m_polygonVertices[i2]); + m_polygonVertices.removeAt(i1); + m_polygonPoints.removeAt(i1); + m_polygonAngles.removeAt(i1); + } + } + } + +private: + static bool pointInTriangle(const Vector2 &p, const Vector2 &a, const Vector2 &b, const Vector2 &c) { + return triangleArea(a, b, p) >= kAreaEpsilon && triangleArea(b, c, p) >= kAreaEpsilon && triangleArea(c, a, p) >= kAreaEpsilon; + } + + Array<int> m_polygonVertices; + Array<float> m_polygonAngles; + Array<Vector2> m_polygonPoints; +}; + +class UniformGrid2 { public: - void reset(const Vector2 *positions, const uint32_t *indices = nullptr, uint32_t reserveEdgeCount = 0) - { + // indices are optional. + void reset(ConstArrayView<Vector2> positions, ConstArrayView<uint32_t> indices = ConstArrayView<uint32_t>(), uint32_t reserveEdgeCount = 0) { m_edges.clear(); if (reserveEdgeCount > 0) m_edges.reserve(reserveEdgeCount); @@ -4062,14 +3369,12 @@ public: m_cellDataOffsets.clear(); } - void append(uint32_t edge) - { + void append(uint32_t edge) { XA_DEBUG_ASSERT(m_cellDataOffsets.isEmpty()); m_edges.push_back(edge); } - bool intersect(Vector2 v1, Vector2 v2, float epsilon) - { + bool intersect(Vector2 v1, Vector2 v2, float epsilon) { const uint32_t edgeCount = m_edges.size(); bool bruteForce = edgeCount <= 20; if (!bruteForce && m_cellDataOffsets.isEmpty()) @@ -4096,8 +3401,7 @@ public: } // If edges is empty, checks for intersection with all edges in the grid. - bool intersect(float epsilon, ConstArrayView<uint32_t> edges = ConstArrayView<uint32_t>(), ConstArrayView<uint32_t> ignoreEdges = ConstArrayView<uint32_t>()) - { + bool intersect(float epsilon, ConstArrayView<uint32_t> edges = ConstArrayView<uint32_t>(), ConstArrayView<uint32_t> ignoreEdges = ConstArrayView<uint32_t>()) { bool bruteForce = m_edges.size() <= 20; if (!bruteForce && m_cellDataOffsets.isEmpty()) bruteForce = !createGrid(); @@ -4167,8 +3471,7 @@ public: } #if XA_DEBUG_EXPORT_BOUNDARY_GRID - void debugExport(const char *filename) - { + void debugExport(const char *filename) { Array<uint8_t> image; image.resize(m_gridWidth * m_gridHeight * 3); for (uint32_t y = 0; y < m_gridHeight; y++) { @@ -4190,8 +3493,7 @@ public: #endif private: - bool createGrid() - { + bool createGrid() { // Compute edge extents. Min will be the grid origin. const uint32_t edgeCount = m_edges.size(); Extents2 edgeExtents; @@ -4202,14 +3504,14 @@ private: edgeExtents.add(edgePosition1(edge)); } m_gridOrigin = edgeExtents.min; - // Size grid to approximately one edge per cell. + // Size grid to approximately one edge per cell in the largest dimension. const Vector2 extentsSize(edgeExtents.max - edgeExtents.min); - m_cellSize = min(extentsSize.x, extentsSize.y) / sqrtf((float)edgeCount); + m_cellSize = max(extentsSize.x, extentsSize.y) / (float)clamp(edgeCount, 32u, 512u); if (m_cellSize <= 0.0f) return false; m_gridWidth = uint32_t(ceilf(extentsSize.x / m_cellSize)); m_gridHeight = uint32_t(ceilf(extentsSize.y / m_cellSize)); - if (m_gridWidth == 0 || m_gridHeight == 0) + if (m_gridWidth <= 1 || m_gridHeight <= 1) return false; // Insert edges into cells. m_cellDataOffsets.resize(m_gridWidth * m_gridHeight); @@ -4243,8 +3545,7 @@ private: return true; } - void computePotentialEdges(Vector2 p1, Vector2 p2) - { + void computePotentialEdges(Vector2 p1, Vector2 p2) { m_potentialEdges.clear(); traverse(p1, p2); for (uint32_t j = 0; j < m_traversedCellOffsets.size(); j++) { @@ -4262,10 +3563,9 @@ private: } // "A Fast Voxel Traversal Algorithm for Ray Tracing" - void traverse(Vector2 p1, Vector2 p2) - { + void traverse(Vector2 p1, Vector2 p2) { const Vector2 dir = p2 - p1; - const Vector2 normal = normalizeSafe(dir, Vector2(0.0f), kEpsilon); + const Vector2 normal = normalizeSafe(dir, Vector2(0.0f)); const int stepX = dir.x >= 0 ? 1 : -1; const int stepY = dir.y >= 0 ? 1 : -1; const uint32_t firstCell[2] = { cellX(p1.x), cellY(p1.y) }; @@ -4284,14 +3584,12 @@ private: if (normal.x > kEpsilon || normal.x < -kEpsilon) { tMaxX = (distToNextCellX * stepX) / normal.x; tDeltaX = (m_cellSize * stepX) / normal.x; - } - else + } else tMaxX = tDeltaX = FLT_MAX; if (normal.y > kEpsilon || normal.y < -kEpsilon) { tMaxY = (distToNextCellY * stepY) / normal.y; tDeltaY = (m_cellSize * stepY) / normal.y; - } - else + } else tMaxY = tDeltaY = FLT_MAX; m_traversedCellOffsets.clear(); m_traversedCellOffsets.push_back(firstCell[0] + firstCell[1] * m_gridWidth); @@ -4318,34 +3616,29 @@ private: } } - uint32_t cellX(float x) const - { + uint32_t cellX(float x) const { return min((uint32_t)max(0.0f, (x - m_gridOrigin.x) / m_cellSize), m_gridWidth - 1u); } - uint32_t cellY(float y) const - { + uint32_t cellY(float y) const { return min((uint32_t)max(0.0f, (y - m_gridOrigin.y) / m_cellSize), m_gridHeight - 1u); } - Vector2 edgePosition0(uint32_t edge) const - { + Vector2 edgePosition0(uint32_t edge) const { return m_positions[vertexAt(meshEdgeIndex0(edge))]; } - Vector2 edgePosition1(uint32_t edge) const - { + Vector2 edgePosition1(uint32_t edge) const { return m_positions[vertexAt(meshEdgeIndex1(edge))]; } - uint32_t vertexAt(uint32_t index) const - { - return m_indices ? m_indices[index] : index; + uint32_t vertexAt(uint32_t index) const { + return m_indices.length > 0 ? m_indices[index] : index; } Array<uint32_t> m_edges; - const Vector2 *m_positions; - const uint32_t *m_indices; // Optional + ConstArrayView<Vector2> m_positions; + ConstArrayView<uint32_t> m_indices; // Optional. Empty if unused. float m_cellSize; Vector2 m_gridOrigin; uint32_t m_gridWidth, m_gridHeight; // in cells @@ -4355,26 +3648,25 @@ private: Array<uint32_t> m_traversedCellOffsets; }; -struct UvMeshChart -{ +struct UvMeshChart { Array<uint32_t> faces; Array<uint32_t> indices; uint32_t material; }; -struct UvMesh -{ +struct UvMesh { UvMeshDecl decl; + BitArray faceIgnore; + Array<uint32_t> faceMaterials; Array<uint32_t> indices; + Array<Vector2> texcoords; // Copied from input and never modified, UvMeshInstance::texcoords are. Used to restore UvMeshInstance::texcoords so packing can be run multiple times. Array<UvMeshChart *> charts; Array<uint32_t> vertexToChartMap; }; -struct UvMeshInstance -{ +struct UvMeshInstance { UvMesh *mesh; Array<Vector2> texcoords; - bool rotateCharts; }; /* @@ -4420,27 +3712,30 @@ struct UvMeshInstance * FRANCE */ namespace opennl { -#define NL_NEW(T) XA_ALLOC(MemTag::OpenNL, T) -#define NL_NEW_ARRAY(T,NB) XA_ALLOC_ARRAY(MemTag::OpenNL, T, NB) -#define NL_RENEW_ARRAY(T,x,NB) XA_REALLOC(MemTag::OpenNL, x, T, NB) -#define NL_DELETE(x) XA_FREE(x); x = nullptr -#define NL_DELETE_ARRAY(x) XA_FREE(x); x = nullptr -#define NL_CLEAR(x, T) memset(x, 0, sizeof(T)); -#define NL_CLEAR_ARRAY(T,x,NB) memset(x, 0, (size_t)(NB)*sizeof(T)) -#define NL_NEW_VECTOR(dim) XA_ALLOC_ARRAY(MemTag::OpenNL, double, dim) -#define NL_DELETE_VECTOR(ptr) XA_FREE(ptr) +#define NL_NEW(T) XA_ALLOC(MemTag::OpenNL, T) +#define NL_NEW_ARRAY(T, NB) XA_ALLOC_ARRAY(MemTag::OpenNL, T, NB) +#define NL_RENEW_ARRAY(T, x, NB) XA_REALLOC(MemTag::OpenNL, x, T, NB) +#define NL_DELETE(x) \ + XA_FREE(x); \ + x = nullptr +#define NL_DELETE_ARRAY(x) \ + XA_FREE(x); \ + x = nullptr +#define NL_CLEAR(x, T) memset(x, 0, sizeof(T)); +#define NL_CLEAR_ARRAY(T, x, NB) memset(x, 0, (size_t)(NB) * sizeof(T)) +#define NL_NEW_VECTOR(dim) XA_ALLOC_ARRAY(MemTag::OpenNL, double, dim) +#define NL_DELETE_VECTOR(ptr) XA_FREE(ptr) struct NLMatrixStruct; -typedef NLMatrixStruct * NLMatrix; +typedef NLMatrixStruct *NLMatrix; typedef void (*NLDestroyMatrixFunc)(NLMatrix M); -typedef void (*NLMultMatrixVectorFunc)(NLMatrix M, const double* x, double* y); +typedef void (*NLMultMatrixVectorFunc)(NLMatrix M, const double *x, double *y); #define NL_MATRIX_SPARSE_DYNAMIC 0x1001 -#define NL_MATRIX_CRS 0x1002 -#define NL_MATRIX_OTHER 0x1006 +#define NL_MATRIX_CRS 0x1002 +#define NL_MATRIX_OTHER 0x1006 -struct NLMatrixStruct -{ +struct NLMatrixStruct { uint32_t m; uint32_t n; uint32_t type; @@ -4450,39 +3745,35 @@ struct NLMatrixStruct /* Dynamic arrays for sparse row/columns */ -struct NLCoeff -{ +struct NLCoeff { uint32_t index; double value; }; -struct NLRowColumn -{ +struct NLRowColumn { uint32_t size; uint32_t capacity; - NLCoeff* coeff; + NLCoeff *coeff; }; /* Compressed Row Storage */ -struct NLCRSMatrix -{ +struct NLCRSMatrix { uint32_t m; uint32_t n; uint32_t type; NLDestroyMatrixFunc destroy_func; NLMultMatrixVectorFunc mult_func; - double* val; - uint32_t* rowptr; - uint32_t* colind; + double *val; + uint32_t *rowptr; + uint32_t *colind; uint32_t nslices; - uint32_t* sliceptr; + uint32_t *sliceptr; }; /* SparseMatrix data structure */ -struct NLSparseMatrix -{ +struct NLSparseMatrix { uint32_t m; uint32_t n; uint32_t type; @@ -4490,25 +3781,23 @@ struct NLSparseMatrix NLMultMatrixVectorFunc mult_func; uint32_t diag_size; uint32_t diag_capacity; - NLRowColumn* row; - NLRowColumn* column; - double* diag; + NLRowColumn *row; + NLRowColumn *column; + double *diag; uint32_t row_capacity; uint32_t column_capacity; }; /* NLContext data structure */ -struct NLBufferBinding -{ - void* base_address; +struct NLBufferBinding { + void *base_address; uint32_t stride; }; -#define NL_BUFFER_ITEM(B,i) *(double*)((void*)((char*)((B).base_address)+((i)*(B).stride))) +#define NL_BUFFER_ITEM(B, i) *(double *)((void *)((char *)((B).base_address) + ((i) * (B).stride))) -struct NLContext -{ +struct NLContext { NLBufferBinding *variable_buffer; double *variable_value; bool *variable_is_locked; @@ -4532,35 +3821,30 @@ struct NLContext double error; }; -static void nlDeleteMatrix(NLMatrix M) -{ +static void nlDeleteMatrix(NLMatrix M) { if (!M) return; M->destroy_func(M); NL_DELETE(M); } -static void nlMultMatrixVector(NLMatrix M, const double* x, double* y) -{ +static void nlMultMatrixVector(NLMatrix M, const double *x, double *y) { M->mult_func(M, x, y); } -static void nlRowColumnConstruct(NLRowColumn* c) -{ +static void nlRowColumnConstruct(NLRowColumn *c) { c->size = 0; c->capacity = 0; c->coeff = nullptr; } -static void nlRowColumnDestroy(NLRowColumn* c) -{ +static void nlRowColumnDestroy(NLRowColumn *c) { NL_DELETE_ARRAY(c->coeff); c->size = 0; c->capacity = 0; } -static void nlRowColumnGrow(NLRowColumn* c) -{ +static void nlRowColumnGrow(NLRowColumn *c) { if (c->capacity != 0) { c->capacity = 2 * c->capacity; c->coeff = NL_RENEW_ARRAY(NLCoeff, c->coeff, c->capacity); @@ -4571,8 +3855,7 @@ static void nlRowColumnGrow(NLRowColumn* c) } } -static void nlRowColumnAdd(NLRowColumn* c, uint32_t index, double value) -{ +static void nlRowColumnAdd(NLRowColumn *c, uint32_t index, double value) { for (uint32_t i = 0; i < c->size; i++) { if (c->coeff[i].index == index) { c->coeff[i].value += value; @@ -4587,8 +3870,7 @@ static void nlRowColumnAdd(NLRowColumn* c, uint32_t index, double value) } /* Does not check whether the index already exists */ -static void nlRowColumnAppend(NLRowColumn* c, uint32_t index, double value) -{ +static void nlRowColumnAppend(NLRowColumn *c, uint32_t index, double value) { if (c->size == c->capacity) nlRowColumnGrow(c); c->coeff[c->size].index = index; @@ -4596,32 +3878,27 @@ static void nlRowColumnAppend(NLRowColumn* c, uint32_t index, double value) c->size++; } -static void nlRowColumnZero(NLRowColumn* c) -{ +static void nlRowColumnZero(NLRowColumn *c) { c->size = 0; } -static void nlRowColumnClear(NLRowColumn* c) -{ +static void nlRowColumnClear(NLRowColumn *c) { c->size = 0; c->capacity = 0; NL_DELETE_ARRAY(c->coeff); } -static int nlCoeffCompare(const void* p1, const void* p2) -{ - return (((NLCoeff*)(p2))->index < ((NLCoeff*)(p1))->index); +static int nlCoeffCompare(const void *p1, const void *p2) { + return (((NLCoeff *)(p2))->index < ((NLCoeff *)(p1))->index); } -static void nlRowColumnSort(NLRowColumn* c) -{ +static void nlRowColumnSort(NLRowColumn *c) { qsort(c->coeff, c->size, sizeof(NLCoeff), nlCoeffCompare); } /* CRSMatrix data structure */ -static void nlCRSMatrixDestroy(NLCRSMatrix* M) -{ +static void nlCRSMatrixDestroy(NLCRSMatrix *M) { NL_DELETE_ARRAY(M->val); NL_DELETE_ARRAY(M->rowptr); NL_DELETE_ARRAY(M->colind); @@ -4631,8 +3908,7 @@ static void nlCRSMatrixDestroy(NLCRSMatrix* M) M->nslices = 0; } -static void nlCRSMatrixMultSlice(NLCRSMatrix* M, const double* x, double* y, uint32_t Ibegin, uint32_t Iend) -{ +static void nlCRSMatrixMultSlice(NLCRSMatrix *M, const double *x, double *y, uint32_t Ibegin, uint32_t Iend) { for (uint32_t i = Ibegin; i < Iend; ++i) { double sum = 0.0; for (uint32_t j = M->rowptr[i]; j < M->rowptr[i + 1]; ++j) @@ -4641,15 +3917,13 @@ static void nlCRSMatrixMultSlice(NLCRSMatrix* M, const double* x, double* y, uin } } -static void nlCRSMatrixMult(NLCRSMatrix* M, const double* x, double* y) -{ +static void nlCRSMatrixMult(NLCRSMatrix *M, const double *x, double *y) { int nslices = (int)(M->nslices); for (int slice = 0; slice < nslices; ++slice) nlCRSMatrixMultSlice(M, x, y, M->sliceptr[slice], M->sliceptr[slice + 1]); } -static void nlCRSMatrixConstruct(NLCRSMatrix* M, uint32_t m, uint32_t n, uint32_t nnz, uint32_t nslices) -{ +static void nlCRSMatrixConstruct(NLCRSMatrix *M, uint32_t m, uint32_t n, uint32_t nnz, uint32_t nslices) { M->m = m; M->n = n; M->type = NL_MATRIX_CRS; @@ -4668,22 +3942,19 @@ static void nlCRSMatrixConstruct(NLCRSMatrix* M, uint32_t m, uint32_t n, uint32_ /* SparseMatrix data structure */ -static void nlSparseMatrixDestroyRowColumns(NLSparseMatrix* M) -{ +static void nlSparseMatrixDestroyRowColumns(NLSparseMatrix *M) { for (uint32_t i = 0; i < M->m; i++) nlRowColumnDestroy(&(M->row[i])); NL_DELETE_ARRAY(M->row); } -static void nlSparseMatrixDestroy(NLSparseMatrix* M) -{ +static void nlSparseMatrixDestroy(NLSparseMatrix *M) { XA_DEBUG_ASSERT(M->type == NL_MATRIX_SPARSE_DYNAMIC); nlSparseMatrixDestroyRowColumns(M); NL_DELETE_ARRAY(M->diag); } -static void nlSparseMatrixAdd(NLSparseMatrix* M, uint32_t i, uint32_t j, double value) -{ +static void nlSparseMatrixAdd(NLSparseMatrix *M, uint32_t i, uint32_t j, double value) { XA_DEBUG_ASSERT(i >= 0 && i <= M->m - 1); XA_DEBUG_ASSERT(j >= 0 && j <= M->n - 1); if (i == j) @@ -4692,24 +3963,21 @@ static void nlSparseMatrixAdd(NLSparseMatrix* M, uint32_t i, uint32_t j, double } /* Returns the number of non-zero coefficients */ -static uint32_t nlSparseMatrixNNZ(NLSparseMatrix* M) -{ +static uint32_t nlSparseMatrixNNZ(NLSparseMatrix *M) { uint32_t nnz = 0; for (uint32_t i = 0; i < M->m; i++) nnz += M->row[i].size; return nnz; } -static void nlSparseMatrixSort(NLSparseMatrix* M) -{ +static void nlSparseMatrixSort(NLSparseMatrix *M) { for (uint32_t i = 0; i < M->m; i++) nlRowColumnSort(&(M->row[i])); } /* SparseMatrix x Vector routines, internal helper routines */ -static void nlSparseMatrix_mult_rows(NLSparseMatrix* A, const double* x, double* y) -{ +static void nlSparseMatrix_mult_rows(NLSparseMatrix *A, const double *x, double *y) { /* * Note: OpenMP does not like unsigned ints * (causes some floating point exceptions), @@ -4717,8 +3985,8 @@ static void nlSparseMatrix_mult_rows(NLSparseMatrix* A, const double* x, double* * indices. */ int m = (int)(A->m); - NLCoeff* c = nullptr; - NLRowColumn* Ri = nullptr; + NLCoeff *c = nullptr; + NLRowColumn *Ri = nullptr; for (int i = 0; i < m; i++) { Ri = &(A->row[i]); y[i] = 0; @@ -4729,14 +3997,12 @@ static void nlSparseMatrix_mult_rows(NLSparseMatrix* A, const double* x, double* } } -static void nlSparseMatrixMult(NLSparseMatrix* A, const double* x, double* y) -{ +static void nlSparseMatrixMult(NLSparseMatrix *A, const double *x, double *y) { XA_DEBUG_ASSERT(A->type == NL_MATRIX_SPARSE_DYNAMIC); nlSparseMatrix_mult_rows(A, x, y); } -static void nlSparseMatrixConstruct(NLSparseMatrix* M, uint32_t m, uint32_t n) -{ +static void nlSparseMatrixConstruct(NLSparseMatrix *M, uint32_t m, uint32_t n) { M->m = m; M->n = n; M->type = NL_MATRIX_SPARSE_DYNAMIC; @@ -4756,24 +4022,23 @@ static void nlSparseMatrixConstruct(NLSparseMatrix* M, uint32_t m, uint32_t n) NL_CLEAR_ARRAY(double, M->diag, M->diag_size); } -static NLMatrix nlCRSMatrixNewFromSparseMatrix(NLSparseMatrix* M) -{ +static NLMatrix nlCRSMatrixNewFromSparseMatrix(NLSparseMatrix *M) { uint32_t nnz = nlSparseMatrixNNZ(M); uint32_t nslices = 8; /* TODO: get number of cores */ uint32_t slice, cur_bound, cur_NNZ, cur_row; uint32_t k; uint32_t slice_size = nnz / nslices; - NLCRSMatrix* CRS = NL_NEW(NLCRSMatrix); + NLCRSMatrix *CRS = NL_NEW(NLCRSMatrix); NL_CLEAR(CRS, NLCRSMatrix); nlCRSMatrixConstruct(CRS, M->m, M->n, nnz, nslices); nlSparseMatrixSort(M); /* Convert matrix to CRS format */ k = 0; for (uint32_t i = 0; i < M->m; ++i) { - NLRowColumn* Ri = &(M->row[i]); + NLRowColumn *Ri = &(M->row[i]); CRS->rowptr[i] = k; for (uint32_t ij = 0; ij < Ri->size; ij++) { - NLCoeff* c = &(Ri->coeff[ij]); + NLCoeff *c = &(Ri->coeff[ij]); CRS->val[k] = c->value; CRS->colind[k] = c->index; ++k; @@ -4799,19 +4064,17 @@ static NLMatrix nlCRSMatrixNewFromSparseMatrix(NLSparseMatrix* M) return (NLMatrix)CRS; } -static void nlMatrixCompress(NLMatrix* M) -{ +static void nlMatrixCompress(NLMatrix *M) { NLMatrix CRS = nullptr; if ((*M)->type != NL_MATRIX_SPARSE_DYNAMIC) return; - CRS = nlCRSMatrixNewFromSparseMatrix((NLSparseMatrix*)*M); + CRS = nlCRSMatrixNewFromSparseMatrix((NLSparseMatrix *)*M); nlDeleteMatrix(*M); *M = CRS; } -static NLContext *nlNewContext() -{ - NLContext* result = NL_NEW(NLContext); +static NLContext *nlNewContext() { + NLContext *result = NL_NEW(NLContext); NL_CLEAR(result, NLContext); result->max_iterations = 100; result->threshold = 1e-6; @@ -4820,8 +4083,7 @@ static NLContext *nlNewContext() return result; } -static void nlDeleteContext(NLContext *context) -{ +static void nlDeleteContext(NLContext *context) { nlDeleteMatrix(context->M); context->M = nullptr; nlDeleteMatrix(context->P); @@ -4839,22 +4101,19 @@ static void nlDeleteContext(NLContext *context) NL_DELETE(context); } -static double ddot(int n, const double *x, const double *y) -{ +static double ddot(int n, const double *x, const double *y) { double sum = 0.0; for (int i = 0; i < n; i++) sum += x[i] * y[i]; return sum; } -static void daxpy(int n, double a, const double *x, double *y) -{ +static void daxpy(int n, double a, const double *x, double *y) { for (int i = 0; i < n; i++) y[i] = a * x[i] + y[i]; } -static void dscal(int n, double a, double *x) -{ +static void dscal(int n, double a, double *x) { for (int i = 0; i < n; i++) x[i] *= a; } @@ -4877,17 +4136,16 @@ static void dscal(int n, double a, double *x) * versions of matrix x vector product (CPU/GPU, sparse/dense ...) */ -static uint32_t nlSolveSystem_PRE_CG(NLMatrix M, NLMatrix P, double* b, double* x, double eps, uint32_t max_iter, double *sq_bnorm, double *sq_rnorm) -{ - int N = (int)M->n; - double* r = NL_NEW_VECTOR(N); - double* d = NL_NEW_VECTOR(N); - double* h = NL_NEW_VECTOR(N); +static uint32_t nlSolveSystem_PRE_CG(NLMatrix M, NLMatrix P, double *b, double *x, double eps, uint32_t max_iter, double *sq_bnorm, double *sq_rnorm) { + int N = (int)M->n; + double *r = NL_NEW_VECTOR(N); + double *d = NL_NEW_VECTOR(N); + double *h = NL_NEW_VECTOR(N); double *Ad = h; uint32_t its = 0; double rh, alpha, beta; double b_square = ddot(N, b, b); - double err = eps * eps*b_square; + double err = eps * eps * b_square; double curr_err; nlMultMatrixVector(M, x, r); daxpy(N, -1., b, r); @@ -4917,13 +4175,12 @@ static uint32_t nlSolveSystem_PRE_CG(NLMatrix M, NLMatrix P, double* b, double* return its; } -static uint32_t nlSolveSystemIterative(NLContext *context, NLMatrix M, NLMatrix P, double* b_in, double* x_in, double eps, uint32_t max_iter) -{ +static uint32_t nlSolveSystemIterative(NLContext *context, NLMatrix M, NLMatrix P, double *b_in, double *x_in, double eps, uint32_t max_iter) { uint32_t result = 0; double rnorm = 0.0; double bnorm = 0.0; - double* b = b_in; - double* x = x_in; + double *b = b_in; + double *x = x_in; XA_DEBUG_ASSERT(M->m == M->n); double sq_bnorm, sq_rnorm; result = nlSolveSystem_PRE_CG(M, P, b, x, eps, max_iter, &sq_bnorm, &sq_rnorm); @@ -4938,10 +4195,9 @@ static uint32_t nlSolveSystemIterative(NLContext *context, NLMatrix M, NLMatrix return result; } -static bool nlSolveIterative(NLContext *context) -{ - double* b = context->b; - double* x = context->x; +static bool nlSolveIterative(NLContext *context) { + double *b = context->b; + double *x = context->x; uint32_t n = context->n; NLMatrix M = context->M; NLMatrix P = context->P; @@ -4953,34 +4209,30 @@ static bool nlSolveIterative(NLContext *context) return true; } -struct NLJacobiPreconditioner -{ +struct NLJacobiPreconditioner { uint32_t m; uint32_t n; uint32_t type; NLDestroyMatrixFunc destroy_func; NLMultMatrixVectorFunc mult_func; - double* diag_inv; + double *diag_inv; }; -static void nlJacobiPreconditionerDestroy(NLJacobiPreconditioner* M) -{ +static void nlJacobiPreconditionerDestroy(NLJacobiPreconditioner *M) { NL_DELETE_ARRAY(M->diag_inv); } -static void nlJacobiPreconditionerMult(NLJacobiPreconditioner* M, const double* x, double* y) -{ +static void nlJacobiPreconditionerMult(NLJacobiPreconditioner *M, const double *x, double *y) { for (uint32_t i = 0; i < M->n; ++i) y[i] = x[i] * M->diag_inv[i]; } -static NLMatrix nlNewJacobiPreconditioner(NLMatrix M_in) -{ - NLSparseMatrix* M = nullptr; - NLJacobiPreconditioner* result = nullptr; +static NLMatrix nlNewJacobiPreconditioner(NLMatrix M_in) { + NLSparseMatrix *M = nullptr; + NLJacobiPreconditioner *result = nullptr; XA_DEBUG_ASSERT(M_in->type == NL_MATRIX_SPARSE_DYNAMIC); XA_DEBUG_ASSERT(M_in->m == M_in->n); - M = (NLSparseMatrix*)M_in; + M = (NLSparseMatrix *)M_in; result = NL_NEW(NLJacobiPreconditioner); NL_CLEAR(result, NLJacobiPreconditioner); result->m = M->m; @@ -4998,8 +4250,7 @@ static NLMatrix nlNewJacobiPreconditioner(NLMatrix M_in) #define NL_NB_VARIABLES 0x101 #define NL_MAX_ITERATIONS 0x103 -static void nlSolverParameteri(NLContext *context, uint32_t pname, int param) -{ +static void nlSolverParameteri(NLContext *context, uint32_t pname, int param) { if (pname == NL_NB_VARIABLES) { XA_DEBUG_ASSERT(param > 0); context->nb_variables = (uint32_t)param; @@ -5010,26 +4261,22 @@ static void nlSolverParameteri(NLContext *context, uint32_t pname, int param) } } -static void nlSetVariable(NLContext *context, uint32_t index, double value) -{ +static void nlSetVariable(NLContext *context, uint32_t index, double value) { XA_DEBUG_ASSERT(index >= 0 && index <= context->nb_variables - 1); NL_BUFFER_ITEM(context->variable_buffer[0], index) = value; } -static double nlGetVariable(NLContext *context, uint32_t index) -{ +static double nlGetVariable(NLContext *context, uint32_t index) { XA_DEBUG_ASSERT(index >= 0 && index <= context->nb_variables - 1); return NL_BUFFER_ITEM(context->variable_buffer[0], index); } -static void nlLockVariable(NLContext *context, uint32_t index) -{ +static void nlLockVariable(NLContext *context, uint32_t index) { XA_DEBUG_ASSERT(index >= 0 && index <= context->nb_variables - 1); context->variable_is_locked[index] = true; } -static void nlVariablesToVector(NLContext *context) -{ +static void nlVariablesToVector(NLContext *context) { uint32_t n = context->n; XA_DEBUG_ASSERT(context->x); for (uint32_t k = 0; k < context->nb_systems; ++k) { @@ -5044,8 +4291,7 @@ static void nlVariablesToVector(NLContext *context) } } -static void nlVectorToVariables(NLContext *context) -{ +static void nlVectorToVariables(NLContext *context) { uint32_t n = context->n; XA_DEBUG_ASSERT(context->x); for (uint32_t k = 0; k < context->nb_systems; ++k) { @@ -5060,8 +4306,7 @@ static void nlVectorToVariables(NLContext *context) } } -static void nlCoefficient(NLContext *context, uint32_t index, double value) -{ +static void nlCoefficient(NLContext *context, uint32_t index, double value) { XA_DEBUG_ASSERT(index >= 0 && index <= context->nb_variables - 1); if (context->variable_is_locked[index]) { /* @@ -5078,12 +4323,11 @@ static void nlCoefficient(NLContext *context, uint32_t index, double value) } } -#define NL_SYSTEM 0x0 -#define NL_MATRIX 0x1 -#define NL_ROW 0x2 +#define NL_SYSTEM 0x0 +#define NL_MATRIX 0x1 +#define NL_ROW 0x2 -static void nlBegin(NLContext *context, uint32_t prim) -{ +static void nlBegin(NLContext *context, uint32_t prim) { if (prim == NL_SYSTEM) { XA_DEBUG_ASSERT(context->nb_variables > 0); context->variable_buffer = NL_NEW_ARRAY(NLBufferBinding, context->nb_systems); @@ -5092,8 +4336,8 @@ static void nlBegin(NLContext *context, uint32_t prim) NL_CLEAR_ARRAY(double, context->variable_value, context->nb_variables * context->nb_systems); for (uint32_t k = 0; k < context->nb_systems; ++k) { context->variable_buffer[k].base_address = - context->variable_value + - k * context->nb_variables; + context->variable_value + + k * context->nb_variables; context->variable_buffer[k].stride = sizeof(double); } context->variable_is_locked = NL_NEW_ARRAY(bool, context->nb_variables); @@ -5116,11 +4360,11 @@ static void nlBegin(NLContext *context, uint32_t prim) context->max_iterations = n * 5; context->M = (NLMatrix)(NL_NEW(NLSparseMatrix)); NL_CLEAR(context->M, NLSparseMatrix); - nlSparseMatrixConstruct((NLSparseMatrix*)(context->M), n, n); - context->x = NL_NEW_ARRAY(double, n*context->nb_systems); - NL_CLEAR_ARRAY(double, context->x, n*context->nb_systems); - context->b = NL_NEW_ARRAY(double, n*context->nb_systems); - NL_CLEAR_ARRAY(double, context->b, n*context->nb_systems); + nlSparseMatrixConstruct((NLSparseMatrix *)(context->M), n, n); + context->x = NL_NEW_ARRAY(double, n * context->nb_systems); + NL_CLEAR_ARRAY(double, context->x, n * context->nb_systems); + context->b = NL_NEW_ARRAY(double, n * context->nb_systems); + NL_CLEAR_ARRAY(double, context->b, n * context->nb_systems); nlVariablesToVector(context); nlRowColumnConstruct(&context->af); nlRowColumnConstruct(&context->al); @@ -5131,16 +4375,15 @@ static void nlBegin(NLContext *context, uint32_t prim) } } -static void nlEnd(NLContext *context, uint32_t prim) -{ +static void nlEnd(NLContext *context, uint32_t prim) { if (prim == NL_MATRIX) { nlRowColumnClear(&context->af); nlRowColumnClear(&context->al); } else if (prim == NL_ROW) { - NLRowColumn* af = &context->af; - NLRowColumn* al = &context->al; - NLSparseMatrix* M = (NLSparseMatrix*)context->M; - double* b = context->b; + NLRowColumn *af = &context->af; + NLRowColumn *al = &context->al; + NLSparseMatrix *M = (NLSparseMatrix *)context->M; + double *b = context->b; uint32_t nf = af->size; uint32_t nl = al->size; uint32_t n = context->n; @@ -5161,14 +4404,13 @@ static void nlEnd(NLContext *context, uint32_t prim) S += al->coeff[jj].value * NL_BUFFER_ITEM(context->variable_buffer[k], j); } for (uint32_t jj = 0; jj < nf; jj++) - b[k*n + af->coeff[jj].index] -= af->coeff[jj].value * S; + b[k * n + af->coeff[jj].index] -= af->coeff[jj].value * S; } context->current_row++; } } -static bool nlSolve(NLContext *context) -{ +static bool nlSolve(NLContext *context) { nlDeleteMatrix(context->P); context->P = nlNewJacobiPreconditioner(context->M); nlMatrixCompress(&context->M); @@ -5179,11 +4421,9 @@ static bool nlSolve(NLContext *context) } // namespace opennl namespace raster { -class ClippedTriangle -{ +class ClippedTriangle { public: - ClippedTriangle(const Vector2 &a, const Vector2 &b, const Vector2 &c) - { + ClippedTriangle(const Vector2 &a, const Vector2 &b, const Vector2 &c) { m_numVertices = 3; m_activeVertexBuffer = 0; m_verticesA[0] = a; @@ -5194,20 +4434,20 @@ public: m_area = 0; } - void clipHorizontalPlane(float offset, float clipdirection) - { - Vector2 *v = m_vertexBuffers[m_activeVertexBuffer]; + void clipHorizontalPlane(float offset, float clipdirection) { + Vector2 *v = m_vertexBuffers[m_activeVertexBuffer]; m_activeVertexBuffer ^= 1; Vector2 *v2 = m_vertexBuffers[m_activeVertexBuffer]; v[m_numVertices] = v[0]; - float dy2, dy1 = offset - v[0].y; - int dy2in, dy1in = clipdirection * dy1 >= 0; - uint32_t p = 0; + float dy2, dy1 = offset - v[0].y; + int dy2in, dy1in = clipdirection * dy1 >= 0; + uint32_t p = 0; for (uint32_t k = 0; k < m_numVertices; k++) { - dy2 = offset - v[k + 1].y; + dy2 = offset - v[k + 1].y; dy2in = clipdirection * dy2 >= 0; - if (dy1in) v2[p++] = v[k]; - if ( dy1in + dy2in == 1 ) { // not both in/out + if (dy1in) + v2[p++] = v[k]; + if (dy1in + dy2in == 1) { // not both in/out float dx = v[k + 1].x - v[k].x; float dy = v[k + 1].y - v[k].y; v2[p++] = Vector2(v[k].x + dy1 * (dx / dy), offset); @@ -5218,20 +4458,20 @@ public: m_numVertices = p; } - void clipVerticalPlane(float offset, float clipdirection) - { - Vector2 *v = m_vertexBuffers[m_activeVertexBuffer]; + void clipVerticalPlane(float offset, float clipdirection) { + Vector2 *v = m_vertexBuffers[m_activeVertexBuffer]; m_activeVertexBuffer ^= 1; Vector2 *v2 = m_vertexBuffers[m_activeVertexBuffer]; v[m_numVertices] = v[0]; - float dx2, dx1 = offset - v[0].x; - int dx2in, dx1in = clipdirection * dx1 >= 0; - uint32_t p = 0; + float dx2, dx1 = offset - v[0].x; + int dx2in, dx1in = clipdirection * dx1 >= 0; + uint32_t p = 0; for (uint32_t k = 0; k < m_numVertices; k++) { dx2 = offset - v[k + 1].x; dx2in = clipdirection * dx2 >= 0; - if (dx1in) v2[p++] = v[k]; - if ( dx1in + dx2in == 1 ) { // not both in/out + if (dx1in) + v2[p++] = v[k]; + if (dx1in + dx2in == 1) { // not both in/out float dx = v[k + 1].x - v[k].x; float dy = v[k + 1].y - v[k].y; v2[p++] = Vector2(offset, v[k].y + dx1 * (dy / dx)); @@ -5242,9 +4482,8 @@ public: m_numVertices = p; } - void computeArea() - { - Vector2 *v = m_vertexBuffers[m_activeVertexBuffer]; + void computeArea() { + Vector2 *v = m_vertexBuffers[m_activeVertexBuffer]; v[m_numVertices] = v[0]; m_area = 0; float centroidx = 0, centroidy = 0; @@ -5258,8 +4497,7 @@ public: m_area = 0.5f * fabsf(m_area); } - void clipAABox(float x0, float y0, float x1, float y1) - { + void clipAABox(float x0, float y0, float x1, float y1) { clipVerticalPlane(x0, -1); clipHorizontalPlane(y0, -1); clipVerticalPlane(x1, 1); @@ -5267,8 +4505,7 @@ public: computeArea(); } - float area() const - { + float area() const { return m_area; } @@ -5285,10 +4522,9 @@ private: typedef bool (*SamplingCallback)(void *param, int x, int y); /// A triangle for rasterization. -struct Triangle -{ - Triangle(const Vector2 &_v0, const Vector2 &_v1, const Vector2 &_v2) : v1(_v0), v2(_v2), v3(_v1) - { +struct Triangle { + Triangle(const Vector2 &_v0, const Vector2 &_v1, const Vector2 &_v2) : + v1(_v0), v2(_v2), v3(_v1), n1(0.0f), n2(0.0f), n3(0.0f) { // make sure every triangle is front facing. flipBackface(); // Compute deltas. @@ -5296,8 +4532,7 @@ struct Triangle computeUnitInwardNormals(); } - bool isValid() - { + bool isValid() { const Vector2 e0 = v3 - v1; const Vector2 e1 = v2 - v1; const float area = e0.y * e1.x - e1.y * e0.x; @@ -5305,18 +4540,17 @@ struct Triangle } // extents has to be multiple of BK_SIZE!! - bool drawAA(const Vector2 &extents, SamplingCallback cb, void *param) - { - const float PX_INSIDE = 1.0f/sqrtf(2.0f); - const float PX_OUTSIDE = -1.0f/sqrtf(2.0f); + bool drawAA(const Vector2 &extents, SamplingCallback cb, void *param) { + const float PX_INSIDE = 1.0f / sqrtf(2.0f); + const float PX_OUTSIDE = -1.0f / sqrtf(2.0f); const float BK_SIZE = 8; - const float BK_INSIDE = sqrtf(BK_SIZE*BK_SIZE/2.0f); - const float BK_OUTSIDE = -sqrtf(BK_SIZE*BK_SIZE/2.0f); + const float BK_INSIDE = sqrtf(BK_SIZE * BK_SIZE / 2.0f); + const float BK_OUTSIDE = -sqrtf(BK_SIZE * BK_SIZE / 2.0f); // Bounding rectangle float minx = floorf(max(min3(v1.x, v2.x, v3.x), 0.0f)); float miny = floorf(max(min3(v1.y, v2.y, v3.y), 0.0f)); - float maxx = ceilf( min(max3(v1.x, v2.x, v3.x), extents.x - 1.0f)); - float maxy = ceilf( min(max3(v1.y, v2.y, v3.y), extents.y - 1.0f)); + float maxx = ceilf(min(max3(v1.x, v2.x, v3.x), extents.x - 1.0f)); + float maxy = ceilf(min(max3(v1.y, v2.y, v3.y), extents.y - 1.0f)); // There's no reason to align the blocks to the viewport, instead we align them to the origin of the triangle bounds. minx = floorf(minx); miny = floorf(miny); @@ -5341,9 +4575,10 @@ struct Triangle float bC = C2 + n2.x * xc + n2.y * yc; float cC = C3 + n3.x * xc + n3.y * yc; // Skip block when outside an edge - if ( (aC <= BK_OUTSIDE) || (bC <= BK_OUTSIDE) || (cC <= BK_OUTSIDE) ) continue; + if ((aC <= BK_OUTSIDE) || (bC <= BK_OUTSIDE) || (cC <= BK_OUTSIDE)) + continue; // Accept whole block when totally covered - if ( (aC >= BK_INSIDE) && (bC >= BK_INSIDE) && (cC >= BK_INSIDE) ) { + if ((aC >= BK_INSIDE) && (bC >= BK_INSIDE) && (cC >= BK_INSIDE)) { for (float y = y0; y < y0 + BK_SIZE; y++) { for (float x = x0; x < x0 + BK_SIZE; x++) { if (!cb(param, (int)x, (int)y)) @@ -5386,10 +4621,9 @@ struct Triangle } private: - void flipBackface() - { + void flipBackface() { // check if triangle is backfacing, if so, swap two vertices - if ( ((v3.x - v1.x) * (v2.y - v1.y) - (v3.y - v1.y) * (v2.x - v1.x)) < 0 ) { + if (((v3.x - v1.x) * (v2.y - v1.y) - (v3.y - v1.y) * (v2.x - v1.x)) < 0) { Vector2 hv = v1; v1 = v2; v2 = hv; // swap pos @@ -5397,8 +4631,7 @@ private: } // compute unit inward normals for each edge. - void computeUnitInwardNormals() - { + void computeUnitInwardNormals() { n1 = v1 - v2; n1 = Vector2(-n1.y, n1.x); n1 = n1 * (1.0f / sqrtf(dot(n1, n1))); @@ -5416,8 +4649,7 @@ private: }; // Process the given triangle. Returns false if rasterization was interrupted by the callback. -static bool drawTriangle(const Vector2 &extents, const Vector2 v[3], SamplingCallback cb, void *param) -{ +static bool drawTriangle(const Vector2 &extents, const Vector2 v[3], SamplingCallback cb, void *param) { Triangle tri(v[0], v[1], v[2]); // @@ It would be nice to have a conservative drawing mode that enlarges the triangle extents by one texel and is able to handle degenerate triangles. // @@ Maybe the simplest thing to do would be raster triangle edges. @@ -5432,22 +4664,19 @@ namespace segment { // - Insertion is o(n) // - Smallest element goes at the end, so that popping it is o(1). -struct CostQueue -{ - CostQueue(uint32_t size = UINT32_MAX) : m_maxSize(size), m_pairs(MemTag::SegmentAtlasChartCandidates) {} +struct CostQueue { + CostQueue(uint32_t size = UINT32_MAX) : + m_maxSize(size), m_pairs(MemTag::SegmentAtlasChartCandidates) {} - float peekCost() const - { + float peekCost() const { return m_pairs.back().cost; } - uint32_t peekFace() const - { + uint32_t peekFace() const { return m_pairs.back().face; } - void push(float cost, uint32_t face) - { + void push(float cost, uint32_t face) { const Pair p = { cost, face }; if (m_pairs.isEmpty() || cost < peekCost()) m_pairs.push_back(p); @@ -5464,29 +4693,25 @@ struct CostQueue } } - uint32_t pop() - { + uint32_t pop() { XA_DEBUG_ASSERT(!m_pairs.isEmpty()); uint32_t f = m_pairs.back().face; m_pairs.pop_back(); return f; } - XA_INLINE void clear() - { + XA_INLINE void clear() { m_pairs.clear(); } - XA_INLINE uint32_t count() const - { + XA_INLINE uint32_t count() const { return m_pairs.size(); } private: const uint32_t m_maxSize; - struct Pair - { + struct Pair { float cost; uint32_t face; }; @@ -5494,25 +4719,27 @@ private: Array<Pair> m_pairs; }; -struct AtlasData -{ +struct AtlasData { ChartOptions options; const Mesh *mesh = nullptr; Array<float> edgeDihedralAngles; Array<float> edgeLengths; Array<float> faceAreas; + Array<float> faceUvAreas; // Can be negative. Array<Vector3> faceNormals; BitArray isFaceInChart; - AtlasData() : edgeDihedralAngles(MemTag::SegmentAtlasMeshData), edgeLengths(MemTag::SegmentAtlasMeshData), faceAreas(MemTag::SegmentAtlasMeshData), faceNormals(MemTag::SegmentAtlasMeshData) {} + AtlasData() : + edgeDihedralAngles(MemTag::SegmentAtlasMeshData), edgeLengths(MemTag::SegmentAtlasMeshData), faceAreas(MemTag::SegmentAtlasMeshData), faceNormals(MemTag::SegmentAtlasMeshData) {} - void compute() - { + void compute() { const uint32_t faceCount = mesh->faceCount(); const uint32_t edgeCount = mesh->edgeCount(); edgeDihedralAngles.resize(edgeCount); edgeLengths.resize(edgeCount); faceAreas.resize(faceCount); + if (options.useInputMeshUvs) + faceUvAreas.resize(faceCount); faceNormals.resize(faceCount); isFaceInChart.resize(faceCount); isFaceInChart.zeroOutMemory(); @@ -5526,6 +4753,8 @@ struct AtlasData } faceAreas[f] = mesh->computeFaceArea(f); XA_DEBUG_ASSERT(faceAreas[f] > 0.0f); + if (options.useInputMeshUvs) + faceUvAreas[f] = mesh->computeFaceParametricArea(f); faceNormals[f] = mesh->computeFaceNormal(f); } for (uint32_t face = 0; face < faceCount; face++) { @@ -5543,19 +4772,109 @@ struct AtlasData } }; +// If MeshDecl::vertexUvData is set on input meshes, find charts by floodfilling faces in world/model space without crossing UV seams. +struct OriginalUvCharts { + OriginalUvCharts(AtlasData &data) : + m_data(data) {} + uint32_t chartCount() const { return m_charts.size(); } + const Basis &chartBasis(uint32_t chartIndex) const { return m_chartBasis[chartIndex]; } + + ConstArrayView<uint32_t> chartFaces(uint32_t chartIndex) const { + const Chart &chart = m_charts[chartIndex]; + return ConstArrayView<uint32_t>(&m_chartFaces[chart.firstFace], chart.faceCount); + } + + void compute() { + m_charts.clear(); + m_chartFaces.clear(); + const Mesh *mesh = m_data.mesh; + const uint32_t faceCount = mesh->faceCount(); + for (uint32_t f = 0; f < faceCount; f++) { + if (m_data.isFaceInChart.get(f)) + continue; + if (isZero(m_data.faceUvAreas[f], kAreaEpsilon)) + continue; // Face must have valid UVs. + // Found an unassigned face, create a new chart. + Chart chart; + chart.firstFace = m_chartFaces.size(); + chart.faceCount = 1; + m_chartFaces.push_back(f); + m_data.isFaceInChart.set(f); + floodfillFaces(chart); + m_charts.push_back(chart); + } + // Compute basis for each chart. + m_chartBasis.resize(m_charts.size()); + for (uint32_t c = 0; c < m_charts.size(); c++) { + const Chart &chart = m_charts[c]; + m_tempPoints.resize(chart.faceCount * 3); + for (uint32_t f = 0; f < chart.faceCount; f++) { + const uint32_t face = m_chartFaces[chart.firstFace + f]; + for (uint32_t i = 0; i < 3; i++) + m_tempPoints[f * 3 + i] = m_data.mesh->position(m_data.mesh->vertexAt(face * 3 + i)); + } + Fit::computeBasis(m_tempPoints, &m_chartBasis[c]); + } + } + +private: + struct Chart { + uint32_t firstFace, faceCount; + }; + + void floodfillFaces(Chart &chart) { + const bool isFaceAreaNegative = m_data.faceUvAreas[m_chartFaces[chart.firstFace]] < 0.0f; + for (;;) { + bool newFaceAdded = false; + const uint32_t faceCount = chart.faceCount; + for (uint32_t f = 0; f < faceCount; f++) { + const uint32_t sourceFace = m_chartFaces[chart.firstFace + f]; + for (Mesh::FaceEdgeIterator edgeIt(m_data.mesh, sourceFace); !edgeIt.isDone(); edgeIt.advance()) { + const uint32_t face = edgeIt.oppositeFace(); + if (face == UINT32_MAX) + continue; // Boundary edge. + if (m_data.isFaceInChart.get(face)) + continue; // Already assigned to a chart. + if (isZero(m_data.faceUvAreas[face], kAreaEpsilon)) + continue; // Face must have valid UVs. + if ((m_data.faceUvAreas[face] < 0.0f) != isFaceAreaNegative) + continue; // Face winding is opposite of the first chart face. + const Vector2 &uv0 = m_data.mesh->texcoord(edgeIt.vertex0()); + const Vector2 &uv1 = m_data.mesh->texcoord(edgeIt.vertex1()); + const Vector2 &ouv0 = m_data.mesh->texcoord(m_data.mesh->vertexAt(meshEdgeIndex0(edgeIt.oppositeEdge()))); + const Vector2 &ouv1 = m_data.mesh->texcoord(m_data.mesh->vertexAt(meshEdgeIndex1(edgeIt.oppositeEdge()))); + if (!equal(uv0, ouv1, m_data.mesh->epsilon()) || !equal(uv1, ouv0, m_data.mesh->epsilon())) + continue; // UVs must match exactly. + m_chartFaces.push_back(face); + chart.faceCount++; + m_data.isFaceInChart.set(face); + newFaceAdded = true; + } + } + if (!newFaceAdded) + break; + } + } + + AtlasData &m_data; + Array<Chart> m_charts; + Array<Basis> m_chartBasis; + Array<uint32_t> m_chartFaces; + Array<Vector3> m_tempPoints; +}; + #if XA_DEBUG_EXPORT_OBJ_PLANAR_REGIONS static uint32_t s_planarRegionsCurrentRegion; static uint32_t s_planarRegionsCurrentVertex; #endif -struct PlanarCharts -{ - PlanarCharts(AtlasData &data) : m_data(data), m_nextRegionFace(MemTag::SegmentAtlasPlanarRegions), m_faceToRegionId(MemTag::SegmentAtlasPlanarRegions) {} +struct PlanarCharts { + PlanarCharts(AtlasData &data) : + m_data(data), m_nextRegionFace(MemTag::SegmentAtlasPlanarRegions), m_faceToRegionId(MemTag::SegmentAtlasPlanarRegions) {} const Basis &chartBasis(uint32_t chartIndex) const { return m_chartBasis[chartIndex]; } uint32_t chartCount() const { return m_charts.size(); } - - ConstArrayView<uint32_t> chartFaces(uint32_t chartIndex) const - { + + ConstArrayView<uint32_t> chartFaces(uint32_t chartIndex) const { const Chart &chart = m_charts[chartIndex]; return ConstArrayView<uint32_t>(&m_chartFaces[chart.firstFace], chart.faceCount); } @@ -5564,8 +4883,7 @@ struct PlanarCharts uint32_t nextRegionFace(uint32_t face) const { return m_nextRegionFace[face]; } float regionArea(uint32_t region) const { return m_regionAreas[region]; } - void compute() - { + void compute() { const uint32_t faceCount = m_data.mesh->faceCount(); // Precompute regions of coplanar incident faces. m_regionFirstFace.clear(); @@ -5581,6 +4899,8 @@ struct PlanarCharts for (uint32_t f = 0; f < faceCount; f++) { if (m_nextRegionFace[f] != f) continue; // Already assigned. + if (m_data.isFaceInChart.get(f)) + continue; // Already in a chart. faceStack.clear(); faceStack.push_back(f); for (;;) { @@ -5595,6 +4915,8 @@ struct PlanarCharts continue; if (m_nextRegionFace[oface] != oface) continue; // Already assigned. + if (m_data.isFaceInChart.get(oface)) + continue; // Already in a chart. if (!equal(dot(m_data.faceNormals[face], m_data.faceNormals[oface]), 1.0f, kEpsilon)) continue; // Not coplanar. const uint32_t next = m_nextRegionFace[face]; @@ -5632,8 +4954,11 @@ struct PlanarCharts // Precompute planar region areas. m_regionAreas.resize(regionCount); m_regionAreas.zeroOutMemory(); - for (uint32_t f = 0; f < faceCount; f++) + for (uint32_t f = 0; f < faceCount; f++) { + if (m_faceToRegionId[f] == UINT32_MAX) + continue; m_regionAreas[m_faceToRegionId[f]] += m_data.faceAreas[f]; + } // Create charts from suitable planar regions. // The dihedral angle of all boundary edges must be >= 90 degrees. m_charts.clear(); @@ -5658,8 +4983,7 @@ struct PlanarCharts if (!createChart) break; face = m_nextRegionFace[face]; - } - while (face != firstRegionFace); + } while (face != firstRegionFace); // Create a chart. if (createChart) { Chart chart; @@ -5671,15 +4995,13 @@ struct PlanarCharts m_chartFaces.push_back(face); chart.faceCount++; face = m_nextRegionFace[face]; - } - while (face != firstRegionFace); + } while (face != firstRegionFace); m_charts.push_back(chart); } } // Compute basis for each chart using the first face normal (all faces have the same normal). m_chartBasis.resize(m_charts.size()); - for (uint32_t c = 0; c < m_charts.size(); c++) - { + for (uint32_t c = 0; c < m_charts.size(); c++) { const uint32_t face = m_chartFaces[m_charts[c].firstFace]; Basis &basis = m_chartBasis[c]; basis.normal = m_data.faceNormals[face]; @@ -5689,8 +5011,7 @@ struct PlanarCharts } private: - struct Chart - { + struct Chart { uint32_t firstFace, faceCount; }; @@ -5704,12 +5025,11 @@ private: Array<Basis> m_chartBasis; }; -struct ClusteredCharts -{ - ClusteredCharts(AtlasData &data, const PlanarCharts &planarCharts) : m_data(data), m_planarCharts(planarCharts), m_texcoords(MemTag::SegmentAtlasMeshData), m_bestTriangles(10), m_placingSeeds(false) {} +struct ClusteredCharts { + ClusteredCharts(AtlasData &data, const PlanarCharts &planarCharts) : + m_data(data), m_planarCharts(planarCharts), m_texcoords(MemTag::SegmentAtlasMeshData), m_bestTriangles(10), m_placingSeeds(false) {} - ~ClusteredCharts() - { + ~ClusteredCharts() { const uint32_t chartCount = m_charts.size(); for (uint32_t i = 0; i < chartCount; i++) { m_charts[i]->~Chart(); @@ -5721,8 +5041,7 @@ struct ClusteredCharts ConstArrayView<uint32_t> chartFaces(uint32_t chartIndex) const { return m_charts[chartIndex]->faces; } const Basis &chartBasis(uint32_t chartIndex) const { return m_charts[chartIndex]->basis; } - void compute() - { + void compute() { const uint32_t faceCount = m_data.mesh->faceCount(); m_facesLeft = 0; for (uint32_t i = 0; i < faceCount; i++) { @@ -5768,9 +5087,9 @@ struct ClusteredCharts } private: - struct Chart - { - Chart() : faces(MemTag::SegmentAtlasChartFaces) {} + struct Chart { + Chart() : + faces(MemTag::SegmentAtlasChartFaces) {} int id = -1; Basis basis; // Best fit normal. @@ -5784,8 +5103,7 @@ private: uint32_t seed; }; - void placeSeeds(float threshold) - { + void placeSeeds(float threshold) { XA_PROFILE_START(clusteredChartsPlaceSeeds) m_placingSeeds = true; // Instead of using a predefiened number of seeds: @@ -5801,8 +5119,7 @@ private: } // Returns true if any of the charts can grow more. - void growCharts(float threshold) - { + void growCharts(float threshold) { XA_PROFILE_START(clusteredChartsGrow) for (;;) { if (m_facesLeft == 0) @@ -5848,8 +5165,7 @@ private: XA_PROFILE_END(clusteredChartsGrow) } - void resetCharts() - { + void resetCharts() { XA_PROFILE_START(clusteredChartsReset) const uint32_t faceCount = m_data.mesh->faceCount(); for (uint32_t i = 0; i < faceCount; i++) { @@ -5880,8 +5196,7 @@ private: XA_PROFILE_END(clusteredChartsReset) } - bool relocateSeeds() - { + bool relocateSeeds() { XA_PROFILE_START(clusteredChartsRelocateSeeds) bool anySeedChanged = false; const uint32_t chartCount = m_charts.size(); @@ -5894,8 +5209,7 @@ private: return anySeedChanged; } - void fillHoles(float threshold) - { + void fillHoles(float threshold) { XA_PROFILE_START(clusteredChartsFillHoles) while (m_facesLeft > 0) createChart(threshold); @@ -5903,8 +5217,7 @@ private: } #if XA_MERGE_CHARTS - void mergeCharts() - { + void mergeCharts() { XA_PROFILE_START(clusteredChartsMerge) const uint32_t chartCount = m_charts.size(); // Merge charts progressively until there's none left to merge. @@ -5964,7 +5277,7 @@ private: // Merge if chart2 has a single face. // chart1 must have more than 1 face. // chart2 area must be <= 10% of chart1 area. - if (m_sharedBoundaryLengthsNoSeams[cc] > 0.0f && chart->faces.size() > 1 && chart2->faces.size() == 1 && chart2->area <= chart->area * 0.1f) + if (m_sharedBoundaryLengthsNoSeams[cc] > 0.0f && chart->faces.size() > 1 && chart2->faces.size() == 1 && chart2->area <= chart->area * 0.1f) goto merge; // Merge if chart2 has two faces (probably a quad), and chart1 bounds at least 2 of its edges. if (chart2->faces.size() == 2 && m_sharedBoundaryEdgeCountNoSeams[cc] >= 2) @@ -5972,8 +5285,8 @@ private: // Merge if chart2 is wholely inside chart1, ignoring seams. if (m_sharedBoundaryLengthsNoSeams[cc] > 0.0f && equal(m_sharedBoundaryLengthsNoSeams[cc], chart2->boundaryLength, kEpsilon)) goto merge; - if (m_sharedBoundaryLengths[cc] > 0.2f * max(0.0f, chart->boundaryLength - externalBoundaryLength) || - m_sharedBoundaryLengths[cc] > 0.75f * chart2->boundaryLength) + if (m_sharedBoundaryLengths[cc] > 0.2f * max(0.0f, chart->boundaryLength - externalBoundaryLength) || + m_sharedBoundaryLengths[cc] > 0.75f * chart2->boundaryLength) goto merge; continue; merge: @@ -6011,8 +5324,7 @@ private: #endif private: - void createChart(float threshold) - { + void createChart(float threshold) { Chart *chart = XA_NEW(MemTag::Default, Chart); chart->id = (int)m_charts.size(); m_charts.push_back(chart); @@ -6043,15 +5355,13 @@ private: } } - bool isChartBoundaryEdge(const Chart *chart, uint32_t edge) const - { + bool isChartBoundaryEdge(const Chart *chart, uint32_t edge) const { const uint32_t oppositeEdge = m_data.mesh->oppositeEdge(edge); const uint32_t oppositeFace = meshEdgeFace(oppositeEdge); return oppositeEdge == UINT32_MAX || m_faceCharts[oppositeFace] != chart->id; } - bool computeChartBasis(Chart *chart, Basis *basis) - { + bool computeChartBasis(Chart *chart, Basis *basis) { const uint32_t faceCount = chart->faces.size(); m_tempPoints.resize(chart->faces.size() * 3); for (uint32_t i = 0; i < faceCount; i++) { @@ -6059,11 +5369,10 @@ private: for (uint32_t j = 0; j < 3; j++) m_tempPoints[i * 3 + j] = m_data.mesh->position(m_data.mesh->vertexAt(f * 3 + j)); } - return Fit::computeBasis(m_tempPoints.data(), m_tempPoints.size(), basis); + return Fit::computeBasis(m_tempPoints, basis); } - bool isFaceFlipped(uint32_t face) const - { + bool isFaceFlipped(uint32_t face) const { const Vector2 &v1 = m_texcoords[face * 3 + 0]; const Vector2 &v2 = m_texcoords[face * 3 + 1]; const Vector2 &v3 = m_texcoords[face * 3 + 2]; @@ -6071,8 +5380,7 @@ private: return parametricArea < 0.0f; } - void parameterizeChart(const Chart *chart) - { + void parameterizeChart(const Chart *chart) { const uint32_t faceCount = chart->faces.size(); for (uint32_t i = 0; i < faceCount; i++) { const uint32_t face = chart->faces[i]; @@ -6085,8 +5393,7 @@ private: } // m_faceCharts for the chart faces must be set to the chart ID. Needed to compute boundary edges. - bool isChartParameterizationValid(const Chart *chart) - { + bool isChartParameterizationValid(const Chart *chart) { const uint32_t faceCount = chart->faces.size(); // Check for flipped faces in the parameterization. OK if all are flipped. uint32_t flippedFaceCount = 0; @@ -6099,7 +5406,7 @@ private: // Check for boundary intersection in the parameterization. XA_PROFILE_START(clusteredChartsPlaceSeedsBoundaryIntersection) XA_PROFILE_START(clusteredChartsGrowBoundaryIntersection) - m_boundaryGrid.reset(m_texcoords.data()); + m_boundaryGrid.reset(m_texcoords); for (uint32_t i = 0; i < faceCount; i++) { const uint32_t f = chart->faces[i]; for (uint32_t j = 0; j < 3; j++) { @@ -6120,15 +5427,14 @@ private: return true; } - bool addFaceToChart(Chart *chart, uint32_t face) - { + bool addFaceToChart(Chart *chart, uint32_t face) { XA_DEBUG_ASSERT(!m_data.isFaceInChart.get(face)); const uint32_t oldFaceCount = chart->faces.size(); const bool firstFace = oldFaceCount == 0; // Append the face and any coplanar connected faces to the chart faces array. chart->faces.push_back(face); uint32_t coplanarFace = m_planarCharts.nextRegionFace(face); - while (coplanarFace != face) { + while (coplanarFace != face) { XA_DEBUG_ASSERT(!m_data.isFaceInChart.get(coplanarFace)); chart->faces.push_back(coplanarFace); coplanarFace = m_planarCharts.nextRegionFace(coplanarFace); @@ -6140,7 +5446,7 @@ private: // Use the first face normal. // Use any edge as the tangent vector. basis.normal = m_data.faceNormals[face]; - basis.tangent = normalize(m_data.mesh->position(m_data.mesh->vertexAt(face * 3 + 0)) - m_data.mesh->position(m_data.mesh->vertexAt(face * 3 + 1)), kEpsilon); + basis.tangent = normalize(m_data.mesh->position(m_data.mesh->vertexAt(face * 3 + 0)) - m_data.mesh->position(m_data.mesh->vertexAt(face * 3 + 1))); basis.bitangent = cross(basis.normal, basis.tangent); } else { // Use best fit normal. @@ -6199,8 +5505,7 @@ private: } // Returns true if the seed has changed. - bool relocateSeed(Chart *chart) - { + bool relocateSeed(Chart *chart) { // Find the first N triangles that fit the proxy best. const uint32_t faceCount = chart->faces.size(); m_bestTriangles.clear(); @@ -6230,8 +5535,7 @@ private: } // Cost is combined metrics * weights. - float computeCost(Chart *chart, uint32_t face) const - { + float computeCost(Chart *chart, uint32_t face) const { // Estimate boundary length and area: const float newChartArea = computeArea(chart, face); const float newBoundaryLength = computeBoundaryLength(chart, face); @@ -6267,28 +5571,25 @@ private: // Returns a value in [0-1]. // 0 if face normal is coplanar to the chart's best fit normal. // 1 if face normal is perpendicular. - float computeNormalDeviationMetric(Chart *chart, uint32_t face) const - { + float computeNormalDeviationMetric(Chart *chart, uint32_t face) const { // All faces in coplanar regions have the same normal, can use any face. const Vector3 faceNormal = m_data.faceNormals[face]; // Use plane fitting metric for now: return min(1.0f - dot(faceNormal, chart->basis.normal), 1.0f); // @@ normal deviations should be weighted by face area } - float computeRoundnessMetric(Chart *chart, float newBoundaryLength, float newChartArea) const - { + float computeRoundnessMetric(Chart *chart, float newBoundaryLength, float newChartArea) const { const float oldRoundness = square(chart->boundaryLength) / chart->area; const float newRoundness = square(newBoundaryLength) / newChartArea; return 1.0f - oldRoundness / newRoundness; } - float computeStraightnessMetric(Chart *chart, uint32_t firstFace) const - { + float computeStraightnessMetric(Chart *chart, uint32_t firstFace) const { float l_out = 0.0f; // Length of firstFace planar region boundary that doesn't border the chart. float l_in = 0.0f; // Length that does border the chart. const uint32_t planarRegionId = m_planarCharts.regionIdFromFace(firstFace); uint32_t face = firstFace; - for (;;) { + for (;;) { for (Mesh::FaceEdgeIterator it(m_data.mesh, face); !it.isDone(); it.advance()) { const float l = m_data.edgeLengths[it.edge()]; if (it.isBoundary()) { @@ -6305,7 +5606,6 @@ private: break; } #if 1 - XA_DEBUG_ASSERT(l_in != 0.0f); // Candidate face must be adjacent to chart. @@ This is not true if the input mesh has zero-length edges. float ratio = (l_out - l_in) / (l_out + l_in); return min(ratio, 0.0f); // Only use the straightness metric to close gaps. #else @@ -6313,8 +5613,7 @@ private: #endif } - bool isNormalSeam(uint32_t edge) const - { + bool isNormalSeam(uint32_t edge) const { const uint32_t oppositeEdge = m_data.mesh->oppositeEdge(edge); if (oppositeEdge == UINT32_MAX) return false; // boundary edge @@ -6334,11 +5633,10 @@ private: return !equal(m_data.faceNormals[f0], m_data.faceNormals[f1], kNormalEpsilon); } - float computeNormalSeamMetric(Chart *chart, uint32_t firstFace) const - { + float computeNormalSeamMetric(Chart *chart, uint32_t firstFace) const { float seamFactor = 0.0f, totalLength = 0.0f; uint32_t face = firstFace; - for (;;) { + for (;;) { for (Mesh::FaceEdgeIterator it(m_data.mesh, face); !it.isDone(); it.advance()) { if (it.isBoundary()) continue; @@ -6375,11 +5673,10 @@ private: return seamFactor / totalLength; } - float computeTextureSeamMetric(Chart *chart, uint32_t firstFace) const - { + float computeTextureSeamMetric(Chart *chart, uint32_t firstFace) const { float seamLength = 0.0f, totalLength = 0.0f; uint32_t face = firstFace; - for (;;) { + for (;;) { for (Mesh::FaceEdgeIterator it(m_data.mesh, face); !it.isDone(); it.advance()) { if (it.isBoundary()) continue; @@ -6402,11 +5699,10 @@ private: return seamLength / totalLength; } - float computeArea(Chart *chart, uint32_t firstFace) const - { + float computeArea(Chart *chart, uint32_t firstFace) const { float area = chart->area; uint32_t face = firstFace; - for (;;) { + for (;;) { area += m_data.faceAreas[face]; face = m_planarCharts.nextRegionFace(face); if (face == firstFace) @@ -6415,13 +5711,12 @@ private: return area; } - float computeBoundaryLength(Chart *chart, uint32_t firstFace) const - { + float computeBoundaryLength(Chart *chart, uint32_t firstFace) const { float boundaryLength = chart->boundaryLength; // Add new edges, subtract edges shared with the chart. const uint32_t planarRegionId = m_planarCharts.regionIdFromFace(firstFace); uint32_t face = firstFace; - for (;;) { + for (;;) { for (Mesh::FaceEdgeIterator it(m_data.mesh, face); !it.isDone(); it.advance()) { const float edgeLength = m_data.edgeLengths[it.edge()]; if (it.isBoundary()) { @@ -6437,11 +5732,10 @@ private: if (face == firstFace) break; } - return max(0.0f, boundaryLength); // @@ Hack! + return max(0.0f, boundaryLength); // @@ Hack! } - bool mergeChart(Chart *owner, Chart *chart, float sharedBoundaryLength) - { + bool mergeChart(Chart *owner, Chart *chart, float sharedBoundaryLength) { const uint32_t oldOwnerFaceCount = owner->faces.size(); const uint32_t chartFaceCount = chart->faces.size(); owner->faces.push_back(chart->faces); @@ -6499,33 +5793,53 @@ private: bool m_placingSeeds; }; -struct Atlas -{ - Atlas() : m_planarCharts(m_data), m_clusteredCharts(m_data, m_planarCharts) {} +struct ChartGeneratorType { + enum Enum { + OriginalUv, + Planar, + Clustered, + Piecewise + }; +}; - uint32_t chartCount() const - { - return m_planarCharts.chartCount() + m_clusteredCharts.chartCount(); +struct Atlas { + Atlas() : + m_originalUvCharts(m_data), m_planarCharts(m_data), m_clusteredCharts(m_data, m_planarCharts) {} + + uint32_t chartCount() const { + return m_originalUvCharts.chartCount() + m_planarCharts.chartCount() + m_clusteredCharts.chartCount(); } - ConstArrayView<uint32_t> chartFaces(uint32_t chartIndex) const - { + ConstArrayView<uint32_t> chartFaces(uint32_t chartIndex) const { + if (chartIndex < m_originalUvCharts.chartCount()) + return m_originalUvCharts.chartFaces(chartIndex); + chartIndex -= m_originalUvCharts.chartCount(); if (chartIndex < m_planarCharts.chartCount()) return m_planarCharts.chartFaces(chartIndex); chartIndex -= m_planarCharts.chartCount(); return m_clusteredCharts.chartFaces(chartIndex); } - const Basis &chartBasis(uint32_t chartIndex) const - { + const Basis &chartBasis(uint32_t chartIndex) const { + if (chartIndex < m_originalUvCharts.chartCount()) + return m_originalUvCharts.chartBasis(chartIndex); + chartIndex -= m_originalUvCharts.chartCount(); if (chartIndex < m_planarCharts.chartCount()) return m_planarCharts.chartBasis(chartIndex); chartIndex -= m_planarCharts.chartCount(); return m_clusteredCharts.chartBasis(chartIndex); } - void reset(const Mesh *mesh, const ChartOptions &options) - { + ChartGeneratorType::Enum chartGeneratorType(uint32_t chartIndex) const { + if (chartIndex < m_originalUvCharts.chartCount()) + return ChartGeneratorType::OriginalUv; + chartIndex -= m_originalUvCharts.chartCount(); + if (chartIndex < m_planarCharts.chartCount()) + return ChartGeneratorType::Planar; + return ChartGeneratorType::Clustered; + } + + void reset(const Mesh *mesh, const ChartOptions &options) { XA_PROFILE_START(buildAtlasInit) m_data.options = options; m_data.mesh = mesh; @@ -6533,8 +5847,12 @@ struct Atlas XA_PROFILE_END(buildAtlasInit) } - void compute() - { + void compute() { + if (m_data.options.useInputMeshUvs) { + XA_PROFILE_START(originalUvCharts) + m_originalUvCharts.compute(); + XA_PROFILE_END(originalUvCharts) + } XA_PROFILE_START(planarCharts) m_planarCharts.compute(); XA_PROFILE_END(planarCharts) @@ -6545,17 +5863,143 @@ struct Atlas private: AtlasData m_data; + OriginalUvCharts m_originalUvCharts; PlanarCharts m_planarCharts; ClusteredCharts m_clusteredCharts; }; +struct ComputeUvMeshChartsTaskArgs { + UvMesh *mesh; + Progress *progress; +}; + +// Charts are found by floodfilling faces without crossing UV seams. +struct ComputeUvMeshChartsTask { + ComputeUvMeshChartsTask(ComputeUvMeshChartsTaskArgs *args) : + m_mesh(args->mesh), m_progress(args->progress), m_uvToEdgeMap(MemTag::Default, m_mesh->indices.size()), m_faceAssigned(m_mesh->indices.size() / 3) {} + + void run() { + const uint32_t vertexCount = m_mesh->texcoords.size(); + const uint32_t indexCount = m_mesh->indices.size(); + const uint32_t faceCount = indexCount / 3; + // A vertex can only be assigned to one chart. + m_mesh->vertexToChartMap.resize(vertexCount); + m_mesh->vertexToChartMap.fill(UINT32_MAX); + // Map vertex UV to edge. Face is then edge / 3. + for (uint32_t i = 0; i < indexCount; i++) + m_uvToEdgeMap.add(m_mesh->texcoords[m_mesh->indices[i]]); + // Find charts. + m_faceAssigned.zeroOutMemory(); + for (uint32_t f = 0; f < faceCount; f++) { + if (m_progress->cancel) + return; + m_progress->increment(1); + // Found an unassigned face, see if it can be added. + const uint32_t chartIndex = m_mesh->charts.size(); + if (!canAddFaceToChart(chartIndex, f)) + continue; + // Face is OK, create a new chart with the face. + UvMeshChart *chart = XA_NEW(MemTag::Default, UvMeshChart); + m_mesh->charts.push_back(chart); + chart->material = m_mesh->faceMaterials.isEmpty() ? 0 : m_mesh->faceMaterials[f]; + addFaceToChart(chartIndex, f); + // Walk incident faces and assign them to the chart. + uint32_t f2 = 0; + for (;;) { + bool newFaceAssigned = false; + const uint32_t faceCount2 = chart->faces.size(); + for (; f2 < faceCount2; f2++) { + const uint32_t face = chart->faces[f2]; + for (uint32_t i = 0; i < 3; i++) { + // Add any valid faces with colocal UVs to the chart. + const Vector2 &uv = m_mesh->texcoords[m_mesh->indices[face * 3 + i]]; + uint32_t edge = m_uvToEdgeMap.get(uv); + while (edge != UINT32_MAX) { + const uint32_t newFace = edge / 3; + if (canAddFaceToChart(chartIndex, newFace)) { + addFaceToChart(chartIndex, newFace); + newFaceAssigned = true; + } + edge = m_uvToEdgeMap.getNext(uv, edge); + } + } + } + if (!newFaceAssigned) + break; + } + } + } + +private: + // The chart at chartIndex doesn't have to exist yet. + bool canAddFaceToChart(uint32_t chartIndex, uint32_t face) const { + if (m_faceAssigned.get(face)) + return false; // Already assigned to a chart. + if (m_mesh->faceIgnore.get(face)) + return false; // Face is ignored (zero area or nan UVs). + if (!m_mesh->faceMaterials.isEmpty() && chartIndex < m_mesh->charts.size()) { + if (m_mesh->faceMaterials[face] != m_mesh->charts[chartIndex]->material) + return false; // Materials don't match. + } + for (uint32_t i = 0; i < 3; i++) { + const uint32_t vertex = m_mesh->indices[face * 3 + i]; + if (m_mesh->vertexToChartMap[vertex] != UINT32_MAX && m_mesh->vertexToChartMap[vertex] != chartIndex) + return false; // Vertex already assigned to another chart. + } + return true; + } + + void addFaceToChart(uint32_t chartIndex, uint32_t face) { + UvMeshChart *chart = m_mesh->charts[chartIndex]; + m_faceAssigned.set(face); + chart->faces.push_back(face); + for (uint32_t i = 0; i < 3; i++) { + const uint32_t vertex = m_mesh->indices[face * 3 + i]; + m_mesh->vertexToChartMap[vertex] = chartIndex; + chart->indices.push_back(vertex); + } + } + + UvMesh *const m_mesh; + Progress *const m_progress; + HashMap<Vector2> m_uvToEdgeMap; // Face is edge / 3. + BitArray m_faceAssigned; +}; + +static void runComputeUvMeshChartsTask(void * /*groupUserData*/, void *taskUserData) { + XA_PROFILE_START(computeChartsThread) + ComputeUvMeshChartsTask task((ComputeUvMeshChartsTaskArgs *)taskUserData); + task.run(); + XA_PROFILE_END(computeChartsThread) +} + +static bool computeUvMeshCharts(TaskScheduler *taskScheduler, ArrayView<UvMesh *> meshes, ProgressFunc progressFunc, void *progressUserData) { + uint32_t totalFaceCount = 0; + for (uint32_t i = 0; i < meshes.length; i++) + totalFaceCount += meshes[i]->indices.size() / 3; + Progress progress(ProgressCategory::ComputeCharts, progressFunc, progressUserData, totalFaceCount); + TaskGroupHandle taskGroup = taskScheduler->createTaskGroup(nullptr, meshes.length); + Array<ComputeUvMeshChartsTaskArgs> taskArgs; + taskArgs.resize(meshes.length); + for (uint32_t i = 0; i < meshes.length; i++) { + ComputeUvMeshChartsTaskArgs &args = taskArgs[i]; + args.mesh = meshes[i]; + args.progress = &progress; + Task task; + task.userData = &args; + task.func = runComputeUvMeshChartsTask; + taskScheduler->run(taskGroup, task); + } + taskScheduler->wait(&taskGroup); + return !progress.cancel; +} + } // namespace segment namespace param { // Fast sweep in 3 directions -static bool findApproximateDiameterVertices(Mesh *mesh, uint32_t *a, uint32_t *b) -{ +static bool findApproximateDiameterVertices(Mesh *mesh, uint32_t *a, uint32_t *b) { XA_DEBUG_ASSERT(a != nullptr); XA_DEBUG_ASSERT(b != nullptr); const uint32_t vertexCount = mesh->vertexCount(); @@ -6612,10 +6056,9 @@ static bool findApproximateDiameterVertices(Mesh *mesh, uint32_t *a, uint32_t *b // From OpenNL LSCM example. // Computes the coordinates of the vertices of a triangle in a local 2D orthonormal basis of the triangle's plane. -static void projectTriangle(Vector3 p0, Vector3 p1, Vector3 p2, Vector2 *z0, Vector2 *z1, Vector2 *z2, float epsilon) -{ - Vector3 X = normalize(p1 - p0, epsilon); - Vector3 Z = normalize(cross(X, p2 - p0), epsilon); +static void projectTriangle(Vector3 p0, Vector3 p1, Vector3 p2, Vector2 *z0, Vector2 *z1, Vector2 *z2) { + Vector3 X = normalize(p1 - p0); + Vector3 Z = normalize(cross(X, p2 - p0)); Vector3 Y = cross(Z, X); Vector3 &O = p0; *z0 = Vector2(0, 0); @@ -6623,8 +6066,83 @@ static void projectTriangle(Vector3 p0, Vector3 p1, Vector3 p2, Vector2 *z0, Vec *z2 = Vector2(dot(p2 - O, X), dot(p2 - O, Y)); } -static bool computeLeastSquaresConformalMap(Mesh *mesh) -{ +// Conformal relations from Brecht Van Lommel (based on ABF): + +static float vec_angle_cos(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) { + Vector3 d1 = v1 - v2; + Vector3 d2 = v3 - v2; + return clamp(dot(d1, d2) / (length(d1) * length(d2)), -1.0f, 1.0f); +} + +static float vec_angle(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) { + float dot = vec_angle_cos(v1, v2, v3); + return acosf(dot); +} + +static void triangle_angles(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3, float *a1, float *a2, float *a3) { + *a1 = vec_angle(v3, v1, v2); + *a2 = vec_angle(v1, v2, v3); + *a3 = kPi - *a2 - *a1; +} + +static bool setup_abf_relations(opennl::NLContext *context, int id0, int id1, int id2, const Vector3 &p0, const Vector3 &p1, const Vector3 &p2) { + // @@ IC: Wouldn't it be more accurate to return cos and compute 1-cos^2? + // It does indeed seem to be a little bit more robust. + // @@ Need to revisit this more carefully! + float a0, a1, a2; + triangle_angles(p0, p1, p2, &a0, &a1, &a2); + if (a0 == 0.0f || a1 == 0.0f || a2 == 0.0f) + return false; + float s0 = sinf(a0); + float s1 = sinf(a1); + float s2 = sinf(a2); + if (s1 > s0 && s1 > s2) { + swap(s1, s2); + swap(s0, s1); + swap(a1, a2); + swap(a0, a1); + swap(id1, id2); + swap(id0, id1); + } else if (s0 > s1 && s0 > s2) { + swap(s0, s2); + swap(s0, s1); + swap(a0, a2); + swap(a0, a1); + swap(id0, id2); + swap(id0, id1); + } + float c0 = cosf(a0); + float ratio = (s2 == 0.0f) ? 1.0f : s1 / s2; + float cosine = c0 * ratio; + float sine = s0 * ratio; + // Note : 2*id + 0 --> u + // 2*id + 1 --> v + int u0_id = 2 * id0 + 0; + int v0_id = 2 * id0 + 1; + int u1_id = 2 * id1 + 0; + int v1_id = 2 * id1 + 1; + int u2_id = 2 * id2 + 0; + int v2_id = 2 * id2 + 1; + // Real part + opennl::nlBegin(context, NL_ROW); + opennl::nlCoefficient(context, u0_id, cosine - 1.0f); + opennl::nlCoefficient(context, v0_id, -sine); + opennl::nlCoefficient(context, u1_id, -cosine); + opennl::nlCoefficient(context, v1_id, sine); + opennl::nlCoefficient(context, u2_id, 1); + opennl::nlEnd(context, NL_ROW); + // Imaginary part + opennl::nlBegin(context, NL_ROW); + opennl::nlCoefficient(context, u0_id, sine); + opennl::nlCoefficient(context, v0_id, cosine - 1.0f); + opennl::nlCoefficient(context, u1_id, -sine); + opennl::nlCoefficient(context, v1_id, -cosine); + opennl::nlCoefficient(context, v2_id, 1); + opennl::nlEnd(context, NL_ROW); + return true; +} + +static bool computeLeastSquaresConformalMap(Mesh *mesh) { uint32_t lockedVertex0, lockedVertex1; if (!findApproximateDiameterVertices(mesh, &lockedVertex0, &lockedVertex1)) { // Mesh has no boundaries. @@ -6635,55 +6153,57 @@ static bool computeLeastSquaresConformalMap(Mesh *mesh) opennl::nlSolverParameteri(context, NL_NB_VARIABLES, int(2 * vertexCount)); opennl::nlSolverParameteri(context, NL_MAX_ITERATIONS, int(5 * vertexCount)); opennl::nlBegin(context, NL_SYSTEM); - const Vector2 *texcoords = mesh->texcoords(); + ArrayView<Vector2> texcoords = mesh->texcoords(); for (uint32_t i = 0; i < vertexCount; i++) { opennl::nlSetVariable(context, 2 * i, texcoords[i].x); opennl::nlSetVariable(context, 2 * i + 1, texcoords[i].y); if (i == lockedVertex0 || i == lockedVertex1) { opennl::nlLockVariable(context, 2 * i); opennl::nlLockVariable(context, 2 * i + 1); - } + } } opennl::nlBegin(context, NL_MATRIX); const uint32_t faceCount = mesh->faceCount(); - const Vector3 *positions = mesh->positions(); - const uint32_t *indices = mesh->indices(); + ConstArrayView<Vector3> positions = mesh->positions(); + ConstArrayView<uint32_t> indices = mesh->indices(); for (uint32_t f = 0; f < faceCount; f++) { const uint32_t v0 = indices[f * 3 + 0]; const uint32_t v1 = indices[f * 3 + 1]; const uint32_t v2 = indices[f * 3 + 2]; - Vector2 z0, z1, z2; - projectTriangle(positions[v0], positions[v1], positions[v2], &z0, &z1, &z2, mesh->epsilon()); - double a = z1.x - z0.x; - double b = z1.y - z0.y; - double c = z2.x - z0.x; - double d = z2.y - z0.y; - XA_DEBUG_ASSERT(b == 0.0); - // Note : 2*id + 0 --> u - // 2*id + 1 --> v - uint32_t u0_id = 2 * v0; - uint32_t v0_id = 2 * v0 + 1; - uint32_t u1_id = 2 * v1; - uint32_t v1_id = 2 * v1 + 1; - uint32_t u2_id = 2 * v2; - uint32_t v2_id = 2 * v2 + 1; - // Note : b = 0 - // Real part - opennl::nlBegin(context, NL_ROW); - opennl::nlCoefficient(context, u0_id, -a+c) ; - opennl::nlCoefficient(context, v0_id, b-d) ; - opennl::nlCoefficient(context, u1_id, -c) ; - opennl::nlCoefficient(context, v1_id, d) ; - opennl::nlCoefficient(context, u2_id, a); - opennl::nlEnd(context, NL_ROW); - // Imaginary part - opennl::nlBegin(context, NL_ROW); - opennl::nlCoefficient(context, u0_id, -b+d); - opennl::nlCoefficient(context, v0_id, -a+c); - opennl::nlCoefficient(context, u1_id, -d); - opennl::nlCoefficient(context, v1_id, -c); - opennl::nlCoefficient(context, v2_id, a); - opennl::nlEnd(context, NL_ROW); + if (!setup_abf_relations(context, v0, v1, v2, positions[v0], positions[v1], positions[v2])) { + Vector2 z0, z1, z2; + projectTriangle(positions[v0], positions[v1], positions[v2], &z0, &z1, &z2); + double a = z1.x - z0.x; + double b = z1.y - z0.y; + double c = z2.x - z0.x; + double d = z2.y - z0.y; + XA_DEBUG_ASSERT(b == 0.0); + // Note : 2*id + 0 --> u + // 2*id + 1 --> v + uint32_t u0_id = 2 * v0; + uint32_t v0_id = 2 * v0 + 1; + uint32_t u1_id = 2 * v1; + uint32_t v1_id = 2 * v1 + 1; + uint32_t u2_id = 2 * v2; + uint32_t v2_id = 2 * v2 + 1; + // Note : b = 0 + // Real part + opennl::nlBegin(context, NL_ROW); + opennl::nlCoefficient(context, u0_id, -a + c); + opennl::nlCoefficient(context, v0_id, b - d); + opennl::nlCoefficient(context, u1_id, -c); + opennl::nlCoefficient(context, v1_id, d); + opennl::nlCoefficient(context, u2_id, a); + opennl::nlEnd(context, NL_ROW); + // Imaginary part + opennl::nlBegin(context, NL_ROW); + opennl::nlCoefficient(context, u0_id, -b + d); + opennl::nlCoefficient(context, v0_id, -a + c); + opennl::nlCoefficient(context, u1_id, -d); + opennl::nlCoefficient(context, v1_id, -c); + opennl::nlCoefficient(context, v2_id, a); + opennl::nlEnd(context, NL_ROW); + } } opennl::nlEnd(context, NL_MATRIX); opennl::nlEnd(context, NL_SYSTEM); @@ -6694,7 +6214,7 @@ static bool computeLeastSquaresConformalMap(Mesh *mesh) for (uint32_t i = 0; i < vertexCount; i++) { const double u = opennl::nlGetVariable(context, 2 * i); const double v = opennl::nlGetVariable(context, 2 * i + 1); - mesh->texcoord(i) = Vector2((float)u, (float)v); + texcoords[i] = Vector2((float)u, (float)v); XA_DEBUG_ASSERT(!isNan(mesh->texcoord(i).x)); XA_DEBUG_ASSERT(!isNan(mesh->texcoord(i).y)); } @@ -6702,30 +6222,26 @@ static bool computeLeastSquaresConformalMap(Mesh *mesh) return true; } -#if XA_RECOMPUTE_CHARTS -struct PiecewiseParam -{ - void reset(const Mesh *mesh, uint32_t faceCount) - { +struct PiecewiseParam { + void reset(const Mesh *mesh) { m_mesh = mesh; - m_faceCount = faceCount; + const uint32_t faceCount = m_mesh->faceCount(); const uint32_t vertexCount = m_mesh->vertexCount(); m_texcoords.resize(vertexCount); - m_patch.reserve(m_faceCount); - m_candidates.reserve(m_faceCount); - m_faceInAnyPatch.resize(m_faceCount); + m_patch.reserve(faceCount); + m_candidates.reserve(faceCount); + m_faceInAnyPatch.resize(faceCount); m_faceInAnyPatch.zeroOutMemory(); - m_faceInvalid.resize(m_faceCount); - m_faceInPatch.resize(m_faceCount); + m_faceInvalid.resize(faceCount); + m_faceInPatch.resize(faceCount); m_vertexInPatch.resize(vertexCount); - m_faceToCandidate.resize(m_faceCount); + m_faceToCandidate.resize(faceCount); } ConstArrayView<uint32_t> chartFaces() const { return m_patch; } - const Vector2 *texcoords() const { return m_texcoords.data(); } + ConstArrayView<Vector2> texcoords() const { return m_texcoords; } - bool computeChart() - { + bool computeChart() { // Clear per-patch state. m_patch.clear(); m_candidates.clear(); @@ -6734,8 +6250,9 @@ struct PiecewiseParam m_faceInPatch.zeroOutMemory(); m_vertexInPatch.zeroOutMemory(); // Add the seed face (first unassigned face) to the patch. + const uint32_t faceCount = m_mesh->faceCount(); uint32_t seed = UINT32_MAX; - for (uint32_t f = 0; f < m_faceCount; f++) { + for (uint32_t f = 0; f < faceCount; f++) { if (m_faceInAnyPatch.get(f)) continue; seed = f; @@ -6749,7 +6266,7 @@ struct PiecewiseParam } addFaceToPatch(seed); // Initialize the boundary grid. - m_boundaryGrid.reset(m_texcoords.data(), m_mesh->indices()); + m_boundaryGrid.reset(m_texcoords, m_mesh->indices()); for (Mesh::FaceEdgeIterator it(m_mesh, seed); !it.isDone(); it.advance()) m_boundaryGrid.append(it.edge()); break; @@ -6793,22 +6310,34 @@ struct PiecewiseParam break; } } + // Check for zero area and flipped faces (using area). + for (CandidateIterator it(bestCandidate); !it.isDone(); it.advance()) { + const Vector2 a = m_texcoords[m_mesh->vertexAt(it.current()->face * 3 + 0)]; + const Vector2 b = m_texcoords[m_mesh->vertexAt(it.current()->face * 3 + 1)]; + const Vector2 c = m_texcoords[m_mesh->vertexAt(it.current()->face * 3 + 2)]; + const float area = triangleArea(a, b, c); + if (area <= 0.0f) { + invalid = true; + break; + } + } // Check for boundary intersection. if (!invalid) { XA_PROFILE_START(parameterizeChartsPiecewiseBoundaryIntersection) // Test candidate edges that would form part of the new patch boundary. // Ignore boundary edges that would become internal if the candidate faces were added to the patch. - Array<uint32_t> newBoundaryEdges, ignoreEdges; + m_newBoundaryEdges.clear(); + m_ignoreBoundaryEdges.clear(); for (CandidateIterator candidateIt(bestCandidate); !candidateIt.isDone(); candidateIt.advance()) { for (Mesh::FaceEdgeIterator it(m_mesh, candidateIt.current()->face); !it.isDone(); it.advance()) { const uint32_t oface = it.oppositeFace(); - if (oface == UINT32_MAX || oface >= m_faceCount || !m_faceInPatch.get(oface)) - newBoundaryEdges.push_back(it.edge()); - if (oface != UINT32_MAX && oface < m_faceCount && m_faceInPatch.get(oface)) - ignoreEdges.push_back(it.oppositeEdge()); + if (oface == UINT32_MAX || !m_faceInPatch.get(oface)) + m_newBoundaryEdges.push_back(it.edge()); + if (oface != UINT32_MAX && m_faceInPatch.get(oface)) + m_ignoreBoundaryEdges.push_back(it.oppositeEdge()); } } - invalid = m_boundaryGrid.intersect(m_mesh->epsilon(), newBoundaryEdges, ignoreEdges); + invalid = m_boundaryGrid.intersect(m_mesh->epsilon(), m_newBoundaryEdges, m_ignoreBoundaryEdges); XA_PROFILE_END(parameterizeChartsPiecewiseBoundaryIntersection) } if (invalid) { @@ -6826,11 +6355,11 @@ struct PiecewiseParam removeLinkedCandidates(bestCandidate); // Reset the grid with all edges on the patch boundary. XA_PROFILE_START(parameterizeChartsPiecewiseBoundaryIntersection) - m_boundaryGrid.reset(m_texcoords.data(), m_mesh->indices()); + m_boundaryGrid.reset(m_texcoords, m_mesh->indices()); for (uint32_t i = 0; i < m_patch.size(); i++) { for (Mesh::FaceEdgeIterator it(m_mesh, m_patch[i]); !it.isDone(); it.advance()) { const uint32_t oface = it.oppositeFace(); - if (oface == UINT32_MAX || oface >= m_faceCount || !m_faceInPatch.get(oface)) + if (oface == UINT32_MAX || !m_faceInPatch.get(oface)) m_boundaryGrid.append(it.edge()); } } @@ -6841,8 +6370,7 @@ struct PiecewiseParam } private: - struct Candidate - { + struct Candidate { uint32_t face, vertex; Candidate *prev, *next; // The previous/next candidate with the same vertex. Vector2 position; @@ -6852,10 +6380,14 @@ private: float patchVertexOrient; }; - struct CandidateIterator - { - CandidateIterator(Candidate *head) : m_current(head) { XA_DEBUG_ASSERT(!head->prev); } - void advance() { if (m_current != nullptr) { m_current = m_current->next; } } + struct CandidateIterator { + CandidateIterator(Candidate *head) : + m_current(head) { XA_DEBUG_ASSERT(!head->prev); } + void advance() { + if (m_current != nullptr) { + m_current = m_current->next; + } + } bool isDone() const { return !m_current; } Candidate *current() { return m_current; } @@ -6864,7 +6396,6 @@ private: }; const Mesh *m_mesh; - uint32_t m_faceCount; Array<Vector2> m_texcoords; BitArray m_faceInAnyPatch; // Face is in a previous chart patch or the current patch. Array<Candidate *> m_candidates; // Incident faces to the patch. @@ -6873,9 +6404,9 @@ private: BitArray m_faceInPatch, m_vertexInPatch; // Face/vertex is in the current patch. BitArray m_faceInvalid; // Face cannot be added to the patch - flipped, cost too high or causes boundary intersection. UniformGrid2 m_boundaryGrid; + Array<uint32_t> m_newBoundaryEdges, m_ignoreBoundaryEdges; // Temp arrays used when testing for boundary intersection. - void addFaceToPatch(uint32_t face) - { + void addFaceToPatch(uint32_t face) { XA_DEBUG_ASSERT(!m_faceInPatch.get(face)); XA_DEBUG_ASSERT(!m_faceInAnyPatch.get(face)); m_patch.push_back(face); @@ -6884,7 +6415,7 @@ private: // Find new candidate faces on the patch incident to the newly added face. for (Mesh::FaceEdgeIterator it(m_mesh, face); !it.isDone(); it.advance()) { const uint32_t oface = it.oppositeFace(); - if (oface == UINT32_MAX || oface >= m_faceCount || m_faceInAnyPatch.get(oface) || m_faceToCandidate[oface]) + if (oface == UINT32_MAX || m_faceInAnyPatch.get(oface) || m_faceToCandidate[oface]) continue; // Found an active edge on the patch front. // Find the free vertex (the vertex that isn't on the active edge). @@ -6900,12 +6431,14 @@ private: } } XA_DEBUG_ASSERT(freeVertex != UINT32_MAX); - // If the free vertex is already in the patch, the face is enclosed by the patch. Add the face to the patch - don't need to assign texcoords. - /*if (m_vertexInPatch.get(freeVertex)) { + if (m_vertexInPatch.get(freeVertex)) { +#if 0 + // If the free vertex is already in the patch, the face is enclosed by the patch. Add the face to the patch - don't need to assign texcoords. freeVertex = UINT32_MAX; - addFaceToPatch(oface, false); + addFaceToPatch(oface); +#endif continue; - }*/ + } // Check this here rather than above so faces enclosed by the patch are always added. if (m_faceInvalid.get(oface)) continue; @@ -6913,8 +6446,7 @@ private: } } - void addCandidateFace(uint32_t patchEdge, float patchVertexOrient, uint32_t face, uint32_t edge, uint32_t freeVertex) - { + void addCandidateFace(uint32_t patchEdge, float patchVertexOrient, uint32_t face, uint32_t edge, uint32_t freeVertex) { XA_DEBUG_ASSERT(!m_faceToCandidate[face]); Vector2 texcoords[3]; orthoProjectFace(face, texcoords); @@ -6960,8 +6492,10 @@ private: uv.x = x + texcoords[localVertex0].x; uv.y = y + texcoords[localVertex0].y; } - if (isNan(texcoords[localFreeVertex].x) || isNan(texcoords[localFreeVertex].y)) + if (isNan(texcoords[localFreeVertex].x) || isNan(texcoords[localFreeVertex].y)) { + m_faceInvalid.set(face); return; + } // Check for local overlap (flipped triangle). // The patch face vertex that isn't on the active edge and the free vertex should be oriented on opposite sides to the active edge. const float freeVertexOrient = orientToEdge(m_texcoords[vertex0], m_texcoords[vertex1], texcoords[localFreeVertex]); @@ -6975,12 +6509,10 @@ private: return; } const float cost = fabsf(stretch - 1.0f); -#if 0 - if (cost > 0.25f) { + if (cost > 0.5f) { m_faceInvalid.set(face); return; } -#endif // Add the candidate. Candidate *candidate = XA_ALLOC(MemTag::Default, Candidate); candidate->face = face; @@ -7017,8 +6549,7 @@ private: it.current()->maxCost = maxCost; } - Candidate *linkedCandidateHead(Candidate *candidate) - { + Candidate *linkedCandidateHead(Candidate *candidate) { Candidate *current = candidate; for (;;) { if (!current->prev) @@ -7028,8 +6559,7 @@ private: return current; } - void removeLinkedCandidates(Candidate *head) - { + void removeLinkedCandidates(Candidate *head) { XA_DEBUG_ASSERT(!head->prev); Candidate *current = head; while (current) { @@ -7046,10 +6576,9 @@ private: } } - void orthoProjectFace(uint32_t face, Vector2 *texcoords) const - { - const Vector3 normal = m_mesh->computeFaceNormal(face); - const Vector3 tangent = normalize(m_mesh->position(m_mesh->vertexAt(face * 3 + 1)) - m_mesh->position(m_mesh->vertexAt(face * 3 + 0)), kEpsilon); + void orthoProjectFace(uint32_t face, Vector2 *texcoords) const { + const Vector3 normal = -m_mesh->computeFaceNormal(face); + const Vector3 tangent = normalize(m_mesh->position(m_mesh->vertexAt(face * 3 + 1)) - m_mesh->position(m_mesh->vertexAt(face * 3 + 0))); const Vector3 bitangent = cross(normal, tangent); for (uint32_t i = 0; i < 3; i++) { const Vector3 &pos = m_mesh->position(m_mesh->vertexAt(face * 3 + i)); @@ -7057,16 +6586,14 @@ private: } } - float parametricArea(const Vector2 *texcoords) const - { + float parametricArea(const Vector2 *texcoords) const { const Vector2 &v1 = texcoords[0]; const Vector2 &v2 = texcoords[1]; const Vector2 &v3 = texcoords[2]; return ((v2.x - v1.x) * (v3.y - v1.y) - (v3.x - v1.x) * (v2.y - v1.y)) * 0.5f; } - float computeStretch(Vector3 p1, Vector3 p2, Vector3 p3, Vector2 t1, Vector2 t2, Vector2 t3) const - { + float computeStretch(Vector3 p1, Vector3 p2, Vector3 p3, Vector2 t1, Vector2 t2, Vector2 t3) const { float parametricArea = ((t2.y - t1.y) * (t3.x - t1.x) - (t3.y - t1.y) * (t2.x - t1.x)) * 0.5f; if (isZero(parametricArea, kAreaEpsilon)) return FLT_MAX; @@ -7080,16 +6607,13 @@ private: } // Return value is positive if the point is one side of the edge, negative if on the other side. - float orientToEdge(Vector2 edgeVertex0, Vector2 edgeVertex1, Vector2 point) const - { + float orientToEdge(Vector2 edgeVertex0, Vector2 edgeVertex1, Vector2 point) const { return (edgeVertex0.x - point.x) * (edgeVertex1.y - point.y) - (edgeVertex0.y - point.y) * (edgeVertex1.x - point.x); } }; -#endif // Estimate quality of existing parameterization. -struct Quality -{ +struct Quality { // computeBoundaryIntersection bool boundaryIntersection = false; @@ -7106,8 +6630,7 @@ struct Quality float conformalMetric = 0.0f; float authalicMetric = 0.0f; - void computeBoundaryIntersection(const Mesh *mesh, UniformGrid2 &boundaryGrid) - { + void computeBoundaryIntersection(const Mesh *mesh, UniformGrid2 &boundaryGrid) { const Array<uint32_t> &boundaryEdges = mesh->boundaryEdges(); const uint32_t boundaryEdgeCount = boundaryEdges.size(); boundaryGrid.reset(mesh->texcoords(), mesh->indices(), boundaryEdgeCount); @@ -7123,11 +6646,11 @@ struct Quality #endif } - void computeFlippedFaces(const Mesh *mesh, uint32_t faceCount, Array<uint32_t> *flippedFaces) - { + void computeFlippedFaces(const Mesh *mesh, Array<uint32_t> *flippedFaces) { totalTriangleCount = flippedTriangleCount = zeroAreaTriangleCount = 0; if (flippedFaces) flippedFaces->clear(); + const uint32_t faceCount = mesh->faceCount(); for (uint32_t f = 0; f < faceCount; f++) { Vector2 texcoord[3]; for (int i = 0; i < 3; i++) { @@ -7159,8 +6682,7 @@ struct Quality flippedFaces->clear(); flippedTriangleCount = 0; } - if (flippedTriangleCount > totalTriangleCount / 2) - { + if (flippedTriangleCount > totalTriangleCount / 2) { // If more than half the triangles are flipped, reverse the flipped / not flipped classification. flippedTriangleCount = totalTriangleCount - flippedTriangleCount; if (flippedFaces) { @@ -7182,10 +6704,10 @@ struct Quality } } - void computeMetrics(const Mesh *mesh, uint32_t faceCount) - { + void computeMetrics(const Mesh *mesh) { totalGeometricArea = totalParametricArea = 0.0f; stretchMetric = maxStretchMetric = conformalMetric = authalicMetric = 0.0f; + const uint32_t faceCount = mesh->faceCount(); for (uint32_t f = 0; f < faceCount; f++) { Vector3 pos[3]; Vector2 texcoord[3]; @@ -7214,7 +6736,7 @@ struct Quality const float a = dot(Ss, Ss); // E const float b = dot(Ss, St); // F const float c = dot(St, St); // G - // Compute eigen-values of the first fundamental form: + // Compute eigen-values of the first fundamental form: const float sigma1 = sqrtf(0.5f * max(0.0f, a + c - sqrtf(square(a - c) + 4 * square(b)))); // gamma uppercase, min eigenvalue. const float sigma2 = sqrtf(0.5f * max(0.0f, a + c + sqrtf(square(a - c) + 4 * square(b)))); // gamma lowercase, max eigenvalue. XA_ASSERT(sigma2 > sigma1 || equal(sigma1, sigma2, kEpsilon)); @@ -7245,347 +6767,261 @@ struct Quality if (totalGeometricArea > 0.0f) { const float normFactor = sqrtf(totalParametricArea / totalGeometricArea); stretchMetric = sqrtf(stretchMetric / totalGeometricArea) * normFactor; - maxStretchMetric *= normFactor; + maxStretchMetric *= normFactor; conformalMetric = sqrtf(conformalMetric / totalGeometricArea); authalicMetric = sqrtf(authalicMetric / totalGeometricArea); } } }; -struct ChartWarningFlags -{ - enum Enum - { - CloseHolesFailed = 1<<1, - FixTJunctionsDuplicatedEdge = 1<<2, - FixTJunctionsFailed = 1<<3, - TriangulateDuplicatedEdge = 1<<4, - }; -}; - -struct ChartCtorBuffers -{ +struct ChartCtorBuffers { Array<uint32_t> chartMeshIndices; Array<uint32_t> unifiedMeshIndices; - Array<uint32_t> boundaryLoops; }; -class Chart -{ +class Chart { public: - Chart(ChartCtorBuffers &buffers, const ParameterizeOptions &options, const Basis &basis, ConstArrayView<uint32_t> faces, const Mesh *sourceMesh, uint32_t chartGroupId, uint32_t chartId) : m_basis(basis), m_mesh(nullptr), m_unifiedMesh(nullptr), m_unmodifiedUnifiedMesh(nullptr), m_type(ChartType::LSCM), m_warningFlags(0), m_closedHolesCount(0), m_fixedTJunctionsCount(0), m_isInvalid(false) - { + Chart(const Basis &basis, segment::ChartGeneratorType::Enum generatorType, ConstArrayView<uint32_t> faces, const Mesh *sourceMesh, uint32_t chartGroupId, uint32_t chartId) : + m_basis(basis), m_unifiedMesh(nullptr), m_type(ChartType::LSCM), m_generatorType(generatorType), m_tjunctionCount(0), m_originalVertexCount(0), m_isInvalid(false) { XA_UNUSED(chartGroupId); XA_UNUSED(chartId); m_faceToSourceFaceMap.copyFrom(faces.data, faces.length); const uint32_t approxVertexCount = min(faces.length * 3, sourceMesh->vertexCount()); - m_mesh = XA_NEW_ARGS(MemTag::Mesh, Mesh, sourceMesh->epsilon(), approxVertexCount, faces.length); m_unifiedMesh = XA_NEW_ARGS(MemTag::Mesh, Mesh, sourceMesh->epsilon(), approxVertexCount, faces.length); HashMap<uint32_t, PassthroughHash<uint32_t>> sourceVertexToUnifiedVertexMap(MemTag::Mesh, approxVertexCount), sourceVertexToChartVertexMap(MemTag::Mesh, approxVertexCount); - // Add vertices. - const uint32_t faceCount = m_initialFaceCount = faces.length; + m_originalIndices.resize(faces.length * 3); + // Add geometry. + const uint32_t faceCount = faces.length; for (uint32_t f = 0; f < faceCount; f++) { + uint32_t unifiedIndices[3]; for (uint32_t i = 0; i < 3; i++) { const uint32_t sourceVertex = sourceMesh->vertexAt(m_faceToSourceFaceMap[f] * 3 + i); - const uint32_t sourceUnifiedVertex = sourceMesh->firstColocal(sourceVertex); + uint32_t sourceUnifiedVertex = sourceMesh->firstColocalVertex(sourceVertex); + if (m_generatorType == segment::ChartGeneratorType::OriginalUv && sourceVertex != sourceUnifiedVertex) { + // Original UVs: don't unify vertices with different UVs; we want to preserve UVs. + if (!equal(sourceMesh->texcoord(sourceVertex), sourceMesh->texcoord(sourceUnifiedVertex), sourceMesh->epsilon())) + sourceUnifiedVertex = sourceVertex; + } uint32_t unifiedVertex = sourceVertexToUnifiedVertexMap.get(sourceUnifiedVertex); if (unifiedVertex == UINT32_MAX) { unifiedVertex = sourceVertexToUnifiedVertexMap.add(sourceUnifiedVertex); - m_unifiedMesh->addVertex(sourceMesh->position(sourceVertex)); + m_unifiedMesh->addVertex(sourceMesh->position(sourceVertex), Vector3(0.0f), sourceMesh->texcoord(sourceVertex)); } if (sourceVertexToChartVertexMap.get(sourceVertex) == UINT32_MAX) { sourceVertexToChartVertexMap.add(sourceVertex); m_vertexToSourceVertexMap.push_back(sourceVertex); m_chartVertexToUnifiedVertexMap.push_back(unifiedVertex); - m_mesh->addVertex(sourceMesh->position(sourceVertex), Vector3(0.0f), sourceMesh->texcoord(sourceVertex)); + m_originalVertexCount++; } - } - } - // Add faces. - for (uint32_t f = 0; f < faceCount; f++) { - uint32_t indices[3], unifiedIndices[3]; - for (uint32_t i = 0; i < 3; i++) { - const uint32_t sourceVertex = sourceMesh->vertexAt(m_faceToSourceFaceMap[f] * 3 + i); - const uint32_t sourceUnifiedVertex = sourceMesh->firstColocal(sourceVertex); - indices[i] = sourceVertexToChartVertexMap.get(sourceVertex); - XA_DEBUG_ASSERT(indices[i] != UINT32_MAX); + m_originalIndices[f * 3 + i] = sourceVertexToChartVertexMap.get(sourceVertex); + ; + XA_DEBUG_ASSERT(m_originalIndices[f * 3 + i] != UINT32_MAX); unifiedIndices[i] = sourceVertexToUnifiedVertexMap.get(sourceUnifiedVertex); XA_DEBUG_ASSERT(unifiedIndices[i] != UINT32_MAX); } - Mesh::AddFaceResult::Enum result = m_mesh->addFace(indices); - XA_UNUSED(result); - XA_DEBUG_ASSERT(result == Mesh::AddFaceResult::OK); -#if XA_DEBUG - // Unifying colocals may create degenerate edges. e.g. if two triangle vertices are colocal. - for (int i = 0; i < 3; i++) { - const uint32_t index1 = unifiedIndices[i]; - const uint32_t index2 = unifiedIndices[(i + 1) % 3]; - XA_DEBUG_ASSERT(index1 != index2); - } -#endif - result = m_unifiedMesh->addFace(unifiedIndices); - XA_UNUSED(result); - XA_DEBUG_ASSERT(result == Mesh::AddFaceResult::OK); + m_unifiedMesh->addFace(unifiedIndices); } - m_mesh->createBoundaries(); // For AtlasPacker::computeBoundingBox - m_mesh->destroyEdgeMap(); // Only needed it for createBoundaries. m_unifiedMesh->createBoundaries(); - if (meshIsPlanar(*m_unifiedMesh)) { + if (m_generatorType == segment::ChartGeneratorType::Planar) { m_type = ChartType::Planar; return; } - m_unifiedMesh->linkBoundaries(); -#if XA_DEBUG_EXPORT_OBJ_BEFORE_FIX_TJUNCTION - m_unifiedMesh->writeObjFile("debug_before_fix_tjunction.obj"); -#endif - bool duplicatedEdge = false, failed = false; - if (options.fixTJunctions) { - XA_PROFILE_START(fixChartMeshTJunctions) - Mesh *fixedUnifiedMesh = meshFixTJunctions(*m_unifiedMesh, &duplicatedEdge, &failed, &m_fixedTJunctionsCount); - XA_PROFILE_END(fixChartMeshTJunctions) - if (fixedUnifiedMesh) { - if (duplicatedEdge) - m_warningFlags |= ChartWarningFlags::FixTJunctionsDuplicatedEdge; - if (failed) - m_warningFlags |= ChartWarningFlags::FixTJunctionsFailed; - m_unmodifiedUnifiedMesh = m_unifiedMesh; - m_unifiedMesh = fixedUnifiedMesh; - m_unifiedMesh->createBoundaries(); - m_unifiedMesh->linkBoundaries(); - m_initialFaceCount = m_unifiedMesh->faceCount(); // Fixing t-junctions rewrites faces. - } - } - if (options.closeHoles) { - // See if there are any holes that need closing. - Array<uint32_t> &boundaryLoops = buffers.boundaryLoops; - meshGetBoundaryLoops(*m_unifiedMesh, boundaryLoops); - if (boundaryLoops.size() > 1) { -#if XA_DEBUG_EXPORT_OBJ_CLOSE_HOLES_ERROR - const uint32_t faceCountBeforeHolesClosed = m_unifiedMesh->faceCount(); +#if XA_CHECK_T_JUNCTIONS + m_tjunctionCount = meshCheckTJunctions(*m_unifiedMesh); +#if XA_DEBUG_EXPORT_OBJ_TJUNCTION + if (m_tjunctionCount > 0) { + char filename[256]; + XA_SPRINTF(filename, sizeof(filename), "debug_mesh_%03u_chartgroup_%03u_chart_%03u_tjunction.obj", sourceMesh->id(), chartGroupId, chartId); + m_unifiedMesh->writeObjFile(filename); + } #endif - // Closing the holes is not always the best solution and does not fix all the problems. - // We need to do some analysis of the holes and the genus to: - // - Find cuts that reduce genus. - // - Find cuts to connect holes. - // - Use minimal spanning trees or seamster. - XA_PROFILE_START(closeChartMeshHoles) - uint32_t holeCount = 0; -#if XA_DEBUG_EXPORT_OBJ_CLOSE_HOLES_ERROR - Array<uint32_t> holeFaceCounts; - failed = !meshCloseHoles(m_unifiedMesh, boundaryLoops, m_basis.normal, &holeFaceCounts); -#else - failed = !meshCloseHoles(m_unifiedMesh, boundaryLoops, m_basis.normal, &holeCount, nullptr); #endif - XA_PROFILE_END(closeChartMeshHoles) - m_unifiedMesh->createBoundaries(); - m_unifiedMesh->linkBoundaries(); - meshGetBoundaryLoops(*m_unifiedMesh, boundaryLoops); - if (failed || boundaryLoops.size() > 1) - m_warningFlags |= ChartWarningFlags::CloseHolesFailed; - m_closedHolesCount = holeCount; -#if XA_DEBUG_EXPORT_OBJ_CLOSE_HOLES_ERROR - if (m_warningFlags & ChartWarningFlags::CloseHolesFailed) { - char filename[256]; - XA_SPRINTF(filename, sizeof(filename), "debug_mesh_%03u_chartgroup_%03u_chart_%03u_close_holes_error.obj", sourceMesh->id(), chartGroupId, chartId); - FILE *file; - XA_FOPEN(file, filename, "w"); - if (file) { - m_unifiedMesh->writeObjVertices(file); - fprintf(file, "s off\n"); - fprintf(file, "o object\n"); - for (uint32_t i = 0; i < faceCountBeforeHolesClosed; i++) - m_unifiedMesh->writeObjFace(file, i); - uint32_t face = faceCountBeforeHolesClosed; - for (uint32_t i = 0; i < holeFaceCounts.size(); i++) { - fprintf(file, "s off\n"); - fprintf(file, "o hole%u\n", i); - for (uint32_t j = 0; j < holeFaceCounts[i]; j++) { - m_unifiedMesh->writeObjFace(file, face); - face++; - } - } - m_unifiedMesh->writeObjBoundaryEges(file); - m_unifiedMesh->writeObjLinkedBoundaries(file); - fclose(file); - } - } -#endif - } - } } -#if XA_RECOMPUTE_CHARTS - Chart(ChartCtorBuffers &buffers, const Chart *parent, const Mesh *parentMesh, ConstArrayView<uint32_t> faces, const Vector2 *texcoords, const Mesh *sourceMesh) : m_mesh(nullptr), m_unifiedMesh(nullptr), m_unmodifiedUnifiedMesh(nullptr), m_type(ChartType::Piecewise), m_warningFlags(0), m_closedHolesCount(0), m_fixedTJunctionsCount(0), m_isInvalid(false) - { - const uint32_t faceCount = m_initialFaceCount = faces.length; + Chart(ChartCtorBuffers &buffers, const Chart *parent, const Mesh *parentMesh, ConstArrayView<uint32_t> faces, ConstArrayView<Vector2> texcoords, const Mesh *sourceMesh) : + m_unifiedMesh(nullptr), m_type(ChartType::Piecewise), m_generatorType(segment::ChartGeneratorType::Piecewise), m_tjunctionCount(0), m_originalVertexCount(0), m_isInvalid(false) { + const uint32_t faceCount = faces.length; m_faceToSourceFaceMap.resize(faceCount); for (uint32_t i = 0; i < faceCount; i++) m_faceToSourceFaceMap[i] = parent->m_faceToSourceFaceMap[faces[i]]; // Map faces to parent chart source mesh. // Copy face indices. - m_mesh = XA_NEW_ARGS(MemTag::Mesh, Mesh, sourceMesh->epsilon(), m_faceToSourceFaceMap.size() * 3, m_faceToSourceFaceMap.size()); Array<uint32_t> &chartMeshIndices = buffers.chartMeshIndices; chartMeshIndices.resize(sourceMesh->vertexCount()); chartMeshIndices.fillBytes(0xff); + m_unifiedMesh = XA_NEW_ARGS(MemTag::Mesh, Mesh, sourceMesh->epsilon(), m_faceToSourceFaceMap.size() * 3, m_faceToSourceFaceMap.size()); + HashMap<uint32_t, PassthroughHash<uint32_t>> sourceVertexToUnifiedVertexMap(MemTag::Mesh, m_faceToSourceFaceMap.size() * 3); // Add vertices. for (uint32_t f = 0; f < faceCount; f++) { for (uint32_t i = 0; i < 3; i++) { const uint32_t vertex = sourceMesh->vertexAt(m_faceToSourceFaceMap[f] * 3 + i); + const uint32_t sourceUnifiedVertex = sourceMesh->firstColocalVertex(vertex); const uint32_t parentVertex = parentMesh->vertexAt(faces[f] * 3 + i); - if (chartMeshIndices[vertex] == (uint32_t)~0) { - chartMeshIndices[vertex] = m_mesh->vertexCount(); + uint32_t unifiedVertex = sourceVertexToUnifiedVertexMap.get(sourceUnifiedVertex); + if (unifiedVertex == UINT32_MAX) { + unifiedVertex = sourceVertexToUnifiedVertexMap.add(sourceUnifiedVertex); + m_unifiedMesh->addVertex(sourceMesh->position(vertex), Vector3(0.0f), texcoords[parentVertex]); + } + if (chartMeshIndices[vertex] == UINT32_MAX) { + chartMeshIndices[vertex] = m_originalVertexCount; + m_originalVertexCount++; m_vertexToSourceVertexMap.push_back(vertex); - m_mesh->addVertex(sourceMesh->position(vertex), Vector3(0.0f), texcoords[parentVertex]); + m_chartVertexToUnifiedVertexMap.push_back(unifiedVertex); } } } // Add faces. + m_originalIndices.resize(faceCount * 3); for (uint32_t f = 0; f < faceCount; f++) { - uint32_t indices[3]; + uint32_t unifiedIndices[3]; for (uint32_t i = 0; i < 3; i++) { const uint32_t vertex = sourceMesh->vertexAt(m_faceToSourceFaceMap[f] * 3 + i); - indices[i] = chartMeshIndices[vertex]; + m_originalIndices[f * 3 + i] = chartMeshIndices[vertex]; + const uint32_t unifiedVertex = sourceMesh->firstColocalVertex(vertex); + unifiedIndices[i] = sourceVertexToUnifiedVertexMap.get(unifiedVertex); } - Mesh::AddFaceResult::Enum result = m_mesh->addFace(indices); - XA_UNUSED(result); - XA_DEBUG_ASSERT(result == Mesh::AddFaceResult::OK); + m_unifiedMesh->addFace(unifiedIndices); } - m_mesh->createBoundaries(); // For AtlasPacker::computeBoundingBox - m_mesh->destroyEdgeMap(); // Only needed it for createBoundaries. + m_unifiedMesh->createBoundaries(); // Need to store texcoords for backup/restore so packing can be run multiple times. backupTexcoords(); } -#endif - ~Chart() - { - if (m_mesh) { - m_mesh->~Mesh(); - XA_FREE(m_mesh); + ~Chart() { + if (m_unifiedMesh) { + m_unifiedMesh->~Mesh(); + XA_FREE(m_unifiedMesh); + m_unifiedMesh = nullptr; } - destroyUnifiedMesh(); } bool isInvalid() const { return m_isInvalid; } - ChartType::Enum type() const { return m_type; } - uint32_t warningFlags() const { return m_warningFlags; } - uint32_t closedHolesCount() const { return m_closedHolesCount; } - uint32_t fixedTJunctionsCount() const { return m_fixedTJunctionsCount; } + ChartType type() const { return m_type; } + segment::ChartGeneratorType::Enum generatorType() const { return m_generatorType; } + uint32_t tjunctionCount() const { return m_tjunctionCount; } const Quality &quality() const { return m_quality; } - uint32_t initialFaceCount() const { return m_initialFaceCount; } #if XA_DEBUG_EXPORT_OBJ_INVALID_PARAMETERIZATION const Array<uint32_t> ¶mFlippedFaces() const { return m_paramFlippedFaces; } #endif uint32_t mapFaceToSourceFace(uint32_t i) const { return m_faceToSourceFaceMap[i]; } uint32_t mapChartVertexToSourceVertex(uint32_t i) const { return m_vertexToSourceVertexMap[i]; } - const Mesh *mesh() const { return m_mesh; } - Mesh *mesh() { return m_mesh; } const Mesh *unifiedMesh() const { return m_unifiedMesh; } - const Mesh *unmodifiedUnifiedMesh() const { return m_unmodifiedUnifiedMesh; } + Mesh *unifiedMesh() { return m_unifiedMesh; } - void parameterize(const ParameterizeOptions &options, UniformGrid2 &boundaryGrid) - { - XA_PROFILE_START(parameterizeChartsOrthogonal) - { + // Vertex count of the chart mesh before unifying vertices. + uint32_t originalVertexCount() const { return m_originalVertexCount; } + + uint32_t originalVertexToUnifiedVertex(uint32_t v) const { return m_chartVertexToUnifiedVertexMap[v]; } + + ConstArrayView<uint32_t> originalVertices() const { return m_originalIndices; } + + void parameterize(const ChartOptions &options, UniformGrid2 &boundaryGrid) { + const uint32_t unifiedVertexCount = m_unifiedMesh->vertexCount(); + if (m_generatorType == segment::ChartGeneratorType::OriginalUv) { + } else { // Project vertices to plane. - const uint32_t vertexCount = m_unifiedMesh->vertexCount(); - for (uint32_t i = 0; i < vertexCount; i++) + XA_PROFILE_START(parameterizeChartsOrthogonal) + for (uint32_t i = 0; i < unifiedVertexCount; i++) m_unifiedMesh->texcoord(i) = Vector2(dot(m_basis.tangent, m_unifiedMesh->position(i)), dot(m_basis.bitangent, m_unifiedMesh->position(i))); - } - XA_PROFILE_END(parameterizeChartsOrthogonal) - // Computing charts checks for flipped triangles and boundary intersection. Don't need to do that again here if chart is planar. - if (m_type != ChartType::Planar) { - XA_PROFILE_START(parameterizeChartsEvaluateQuality) - m_quality.computeBoundaryIntersection(m_unifiedMesh, boundaryGrid); - m_quality.computeFlippedFaces(m_unifiedMesh, m_initialFaceCount, nullptr); - m_quality.computeMetrics(m_unifiedMesh, m_initialFaceCount); - XA_PROFILE_END(parameterizeChartsEvaluateQuality) - // Use orthogonal parameterization if quality is acceptable. - if (!m_quality.boundaryIntersection && m_quality.flippedTriangleCount == 0 && m_quality.totalGeometricArea > 0.0f && m_quality.stretchMetric <= 1.1f && m_quality.maxStretchMetric <= 1.25f) - m_type = ChartType::Ortho; - } - if (m_type == ChartType::LSCM) { - XA_PROFILE_START(parameterizeChartsLSCM) - if (options.func) { - options.func(&m_unifiedMesh->position(0).x, &m_unifiedMesh->texcoord(0).x, m_unifiedMesh->vertexCount(), m_unifiedMesh->indices(), m_unifiedMesh->indexCount()); - } - else - computeLeastSquaresConformalMap(m_unifiedMesh); - XA_PROFILE_END(parameterizeChartsLSCM) - XA_PROFILE_START(parameterizeChartsEvaluateQuality) - m_quality.computeBoundaryIntersection(m_unifiedMesh, boundaryGrid); + XA_PROFILE_END(parameterizeChartsOrthogonal) + // Computing charts checks for flipped triangles and boundary intersection. Don't need to do that again here if chart is planar. + if (m_type != ChartType::Planar && m_generatorType != segment::ChartGeneratorType::OriginalUv) { + XA_PROFILE_START(parameterizeChartsEvaluateQuality) + m_quality.computeBoundaryIntersection(m_unifiedMesh, boundaryGrid); + m_quality.computeFlippedFaces(m_unifiedMesh, nullptr); + m_quality.computeMetrics(m_unifiedMesh); + XA_PROFILE_END(parameterizeChartsEvaluateQuality) + // Use orthogonal parameterization if quality is acceptable. + if (!m_quality.boundaryIntersection && m_quality.flippedTriangleCount == 0 && m_quality.zeroAreaTriangleCount == 0 && m_quality.totalGeometricArea > 0.0f && m_quality.stretchMetric <= 1.1f && m_quality.maxStretchMetric <= 1.25f) + m_type = ChartType::Ortho; + } + if (m_type == ChartType::LSCM) { + XA_PROFILE_START(parameterizeChartsLSCM) + if (options.paramFunc) { + options.paramFunc(&m_unifiedMesh->position(0).x, &m_unifiedMesh->texcoord(0).x, m_unifiedMesh->vertexCount(), m_unifiedMesh->indices().data, m_unifiedMesh->indexCount()); + } else + computeLeastSquaresConformalMap(m_unifiedMesh); + XA_PROFILE_END(parameterizeChartsLSCM) + XA_PROFILE_START(parameterizeChartsEvaluateQuality) + m_quality.computeBoundaryIntersection(m_unifiedMesh, boundaryGrid); #if XA_DEBUG_EXPORT_OBJ_INVALID_PARAMETERIZATION - m_quality.computeFlippedFaces(m_unifiedMesh, m_initialFaceCount, &m_paramFlippedFaces); + m_quality.computeFlippedFaces(m_unifiedMesh, &m_paramFlippedFaces); #else - m_quality.computeFlippedFaces(m_unifiedMesh, m_initialFaceCount, nullptr); + m_quality.computeFlippedFaces(m_unifiedMesh, nullptr); #endif - // Don't need to call computeMetrics here, that's only used in evaluateOrthoQuality to determine if quality is acceptable enough to use ortho projection. - if (m_quality.boundaryIntersection || m_quality.flippedTriangleCount > 0) - m_isInvalid = true; - XA_PROFILE_END(parameterizeChartsEvaluateQuality) + // Don't need to call computeMetrics here, that's only used in evaluateOrthoQuality to determine if quality is acceptable enough to use ortho projection. + if (m_quality.boundaryIntersection || m_quality.flippedTriangleCount > 0 || m_quality.zeroAreaTriangleCount > 0) + m_isInvalid = true; + XA_PROFILE_END(parameterizeChartsEvaluateQuality) + } } + if (options.fixWinding && m_unifiedMesh->computeFaceParametricArea(0) < 0.0f) { + for (uint32_t i = 0; i < unifiedVertexCount; i++) + m_unifiedMesh->texcoord(i).x *= -1.0f; + } +#if XA_CHECK_PARAM_WINDING + const uint32_t faceCount = m_unifiedMesh->faceCount(); + uint32_t flippedCount = 0; + for (uint32_t i = 0; i < faceCount; i++) { + const float area = m_unifiedMesh->computeFaceParametricArea(i); + if (area < 0.0f) + flippedCount++; + } + if (flippedCount == faceCount) { + XA_PRINT_WARNING("param: all faces flipped\n"); + } else if (flippedCount > 0) { + XA_PRINT_WARNING("param: %u / %u faces flipped\n", flippedCount, faceCount); + } +#endif + #if XA_DEBUG_ALL_CHARTS_INVALID m_isInvalid = true; #endif - // Transfer parameterization from unified mesh to chart mesh. - const uint32_t vertexCount = m_mesh->vertexCount(); - for (uint32_t v = 0; v < vertexCount; v++) - m_mesh->texcoord(v) = m_unifiedMesh->texcoord(m_chartVertexToUnifiedVertexMap[v]); - // Can destroy unified mesh now. - // But not if the parameterization is invalid, the unified mesh will be needed for PiecewiseParameterization. - if (!m_isInvalid) - destroyUnifiedMesh(); // Need to store texcoords for backup/restore so packing can be run multiple times. backupTexcoords(); } - Vector2 computeParametricBounds() const - { + Vector2 computeParametricBounds() const { Vector2 minCorner(FLT_MAX, FLT_MAX); Vector2 maxCorner(-FLT_MAX, -FLT_MAX); - const uint32_t vertexCount = m_mesh->vertexCount(); + const uint32_t vertexCount = m_unifiedMesh->vertexCount(); for (uint32_t v = 0; v < vertexCount; v++) { - minCorner = min(minCorner, m_mesh->texcoord(v)); - maxCorner = max(maxCorner, m_mesh->texcoord(v)); + minCorner = min(minCorner, m_unifiedMesh->texcoord(v)); + maxCorner = max(maxCorner, m_unifiedMesh->texcoord(v)); } return (maxCorner - minCorner) * 0.5f; } - void restoreTexcoords() - { - memcpy(m_mesh->texcoords(), m_backupTexcoords.data(), m_mesh->vertexCount() * sizeof(Vector2)); +#if XA_CHECK_PIECEWISE_CHART_QUALITY + void evaluateQuality(UniformGrid2 &boundaryGrid) { + m_quality.computeBoundaryIntersection(m_unifiedMesh, boundaryGrid); +#if XA_DEBUG_EXPORT_OBJ_INVALID_PARAMETERIZATION + m_quality.computeFlippedFaces(m_unifiedMesh, &m_paramFlippedFaces); +#else + m_quality.computeFlippedFaces(m_unifiedMesh, nullptr); +#endif + if (m_quality.boundaryIntersection || m_quality.flippedTriangleCount > 0 || m_quality.zeroAreaTriangleCount > 0) + m_isInvalid = true; } +#endif -private: - void backupTexcoords() - { - m_backupTexcoords.resize(m_mesh->vertexCount()); - memcpy(m_backupTexcoords.data(), m_mesh->texcoords(), m_mesh->vertexCount() * sizeof(Vector2)); + void restoreTexcoords() { + memcpy(m_unifiedMesh->texcoords().data, m_backupTexcoords.data(), m_unifiedMesh->vertexCount() * sizeof(Vector2)); } - void destroyUnifiedMesh() - { - if (m_unifiedMesh) { - m_unifiedMesh->~Mesh(); - XA_FREE(m_unifiedMesh); - m_unifiedMesh = nullptr; - } - if (m_unmodifiedUnifiedMesh) { - m_unmodifiedUnifiedMesh->~Mesh(); - XA_FREE(m_unmodifiedUnifiedMesh); - m_unmodifiedUnifiedMesh = nullptr; - } - // Don't need this when unified meshes are destroyed. - m_chartVertexToUnifiedVertexMap.destroy(); +private: + void backupTexcoords() { + m_backupTexcoords.resize(m_unifiedMesh->vertexCount()); + memcpy(m_backupTexcoords.data(), m_unifiedMesh->texcoords().data, m_unifiedMesh->vertexCount() * sizeof(Vector2)); } Basis m_basis; - Mesh *m_mesh; Mesh *m_unifiedMesh; - Mesh *m_unmodifiedUnifiedMesh; // Unified mesh before fixing t-junctions. Null if no t-junctions were fixed - ChartType::Enum m_type; - uint32_t m_warningFlags; - uint32_t m_initialFaceCount; // Before fixing T-junctions and/or closing holes. - uint32_t m_closedHolesCount, m_fixedTJunctionsCount; + ChartType m_type; + segment::ChartGeneratorType::Enum m_generatorType; + uint32_t m_tjunctionCount; + + uint32_t m_originalVertexCount; + Array<uint32_t> m_originalIndices; // List of faces of the source mesh that belong to this chart. Array<uint32_t> m_faceToSourceFaceMap; @@ -7604,47 +7040,46 @@ private: bool m_isInvalid; }; -struct CreateAndParameterizeChartTaskArgs -{ - const Basis *basis; +struct CreateAndParameterizeChartTaskGroupArgs { + Progress *progress; ThreadLocal<UniformGrid2> *boundaryGrid; + ThreadLocal<ChartCtorBuffers> *chartBuffers; + const ChartOptions *options; + ThreadLocal<PiecewiseParam> *pp; +}; + +struct CreateAndParameterizeChartTaskArgs { + const Basis *basis; Chart *chart; // output Array<Chart *> charts; // output (if more than one chart) - ThreadLocal<ChartCtorBuffers> *chartBuffers; + segment::ChartGeneratorType::Enum chartGeneratorType; const Mesh *mesh; - const ParameterizeOptions *options; -#if XA_RECOMPUTE_CHARTS - ThreadLocal<PiecewiseParam> *pp; -#endif ConstArrayView<uint32_t> faces; uint32_t chartGroupId; uint32_t chartId; }; -static void runCreateAndParameterizeChartTask(void *userData) -{ - auto args = (CreateAndParameterizeChartTaskArgs *)userData; +static void runCreateAndParameterizeChartTask(void *groupUserData, void *taskUserData) { + XA_PROFILE_START(createChartMeshAndParameterizeThread) + auto groupArgs = (CreateAndParameterizeChartTaskGroupArgs *)groupUserData; + auto args = (CreateAndParameterizeChartTaskArgs *)taskUserData; XA_PROFILE_START(createChartMesh) - args->chart = XA_NEW_ARGS(MemTag::Default, Chart, args->chartBuffers->get(), *args->options, *args->basis, args->faces, args->mesh, args->chartGroupId, args->chartId); + args->chart = XA_NEW_ARGS(MemTag::Default, Chart, *args->basis, args->chartGeneratorType, args->faces, args->mesh, args->chartGroupId, args->chartId); XA_PROFILE_END(createChartMesh) - args->chart->parameterize(*args->options, args->boundaryGrid->get()); + XA_PROFILE_START(parameterizeCharts) + args->chart->parameterize(*groupArgs->options, groupArgs->boundaryGrid->get()); + XA_PROFILE_END(parameterizeCharts) #if XA_RECOMPUTE_CHARTS - if (!args->chart->isInvalid()) + if (!args->chart->isInvalid()) { + XA_PROFILE_END(createChartMeshAndParameterizeThread) return; + } // Recompute charts with invalid parameterizations. XA_PROFILE_START(parameterizeChartsRecompute) Chart *invalidChart = args->chart; - // Fixing t-junctions rewrites unified mesh faces, and we need to map faces back to input mesh. So use the unmodified unified mesh. - const Mesh *invalidMesh = invalidChart->unmodifiedUnifiedMesh(); - uint32_t faceCount = 0; - if (invalidMesh) { - faceCount = invalidMesh->faceCount(); - } else { - invalidMesh = invalidChart->unifiedMesh(); - faceCount = invalidChart->initialFaceCount(); // Not invalidMesh->faceCount(). Don't want faces added by hole closing. - } - PiecewiseParam &pp = args->pp->get(); - pp.reset(invalidMesh, faceCount); + const Mesh *invalidMesh = invalidChart->unifiedMesh(); + PiecewiseParam &pp = groupArgs->pp->get(); + pp.reset(invalidMesh); #if XA_DEBUG_EXPORT_OBJ_RECOMPUTED_CHARTS char filename[256]; XA_SPRINTF(filename, sizeof(filename), "debug_mesh_%03u_chartgroup_%03u_chart_%03u_recomputed.obj", args->mesh->id(), args->chartGroupId, args->chartId); @@ -7658,7 +7093,10 @@ static void runCreateAndParameterizeChartTask(void *userData) XA_PROFILE_END(parameterizeChartsPiecewise) if (!facesRemaining) break; - Chart *chart = XA_NEW_ARGS(MemTag::Default, Chart, args->chartBuffers->get(), invalidChart, invalidMesh, pp.chartFaces(), pp.texcoords(), args->mesh); + Chart *chart = XA_NEW_ARGS(MemTag::Default, Chart, groupArgs->chartBuffers->get(), invalidChart, invalidMesh, pp.chartFaces(), pp.texcoords(), args->mesh); +#if XA_CHECK_PIECEWISE_CHART_QUALITY + chart->evaluateQuality(args->boundaryGrid->get()); +#endif args->charts.push_back(chart); #if XA_DEBUG_EXPORT_OBJ_RECOMPUTED_CHARTS if (file) { @@ -7686,50 +7124,63 @@ static void runCreateAndParameterizeChartTask(void *userData) #endif XA_PROFILE_END(parameterizeChartsRecompute) #endif // XA_RECOMPUTE_CHARTS + XA_PROFILE_END(createChartMeshAndParameterizeThread) + // Update progress. + groupArgs->progress->increment(args->faces.length); } // Set of charts corresponding to mesh faces in the same face group. -class ChartGroup -{ +class ChartGroup { public: - ChartGroup(uint32_t id, const Mesh *sourceMesh, const MeshFaceGroups *sourceMeshFaceGroups, MeshFaceGroups::Handle faceGroup) : m_id(id), m_sourceMesh(sourceMesh), m_sourceMeshFaceGroups(sourceMeshFaceGroups), m_faceGroup(faceGroup), m_faceCount(0), m_paramAddedChartsCount(0), m_paramDeletedChartsCount(0) - { + ChartGroup(uint32_t id, const Mesh *sourceMesh, const MeshFaceGroups *sourceMeshFaceGroups, MeshFaceGroups::Handle faceGroup) : + m_id(id), m_sourceMesh(sourceMesh), m_sourceMeshFaceGroups(sourceMeshFaceGroups), m_faceGroup(faceGroup) { } - ~ChartGroup() - { + ~ChartGroup() { for (uint32_t i = 0; i < m_charts.size(); i++) { m_charts[i]->~Chart(); XA_FREE(m_charts[i]); } } - uint32_t segmentChartCount() const { return m_chartBasis.size(); } uint32_t chartCount() const { return m_charts.size(); } Chart *chartAt(uint32_t i) const { return m_charts[i]; } - uint32_t faceCount() const { return m_faceCount; } - uint32_t paramAddedChartsCount() const { return m_paramAddedChartsCount; } - uint32_t paramDeletedChartsCount() const { return m_paramDeletedChartsCount; } + uint32_t faceCount() const { return m_sourceMeshFaceGroups->faceCount(m_faceGroup); } - void computeChartFaces(const ChartOptions &options, segment::Atlas &atlas) - { + void computeCharts(TaskScheduler *taskScheduler, const ChartOptions &options, Progress *progress, segment::Atlas &atlas, ThreadLocal<UniformGrid2> *boundaryGrid, ThreadLocal<ChartCtorBuffers> *chartBuffers, ThreadLocal<PiecewiseParam> *piecewiseParam) { + // This function may be called multiple times, so destroy existing charts. + for (uint32_t i = 0; i < m_charts.size(); i++) { + m_charts[i]->~Chart(); + XA_FREE(m_charts[i]); + } // Create mesh from source mesh, using only the faces in this face group. XA_PROFILE_START(createChartGroupMesh) Mesh *mesh = createMesh(); XA_PROFILE_END(createChartGroupMesh) // Segment mesh into charts (arrays of faces). #if XA_DEBUG_SINGLE_CHART - m_chartBasis.resize(1); - Fit::computeBasis(&mesh->position(0), mesh->vertexCount(), &m_chartBasis[0]); - m_chartFaces.resize(1 + mesh->faceCount()); - m_chartFaces[0] = mesh->faceCount(); - for (uint32_t i = 0; i < m_chartFaces.size(); i++) - m_chartFaces[i + 1] = i; + XA_UNUSED(options); + XA_UNUSED(atlas); + const uint32_t chartCount = 1; + uint32_t offset; + Basis chartBasis; + Fit::computeBasis(&mesh->position(0), mesh->vertexCount(), &chartBasis); + Array<uint32_t> chartFaces; + chartFaces.resize(1 + mesh->faceCount()); + chartFaces[0] = mesh->faceCount(); + for (uint32_t i = 0; i < chartFaces.size() - 1; i++) + chartFaces[i + 1] = m_faceToSourceFaceMap[i]; + // Destroy mesh. + const uint32_t faceCount = mesh->faceCount(); + mesh->~Mesh(); + XA_FREE(mesh); #else XA_PROFILE_START(buildAtlas) atlas.reset(mesh, options); atlas.compute(); XA_PROFILE_END(buildAtlas) + // Update progress. + progress->increment(faceCount()); #if XA_DEBUG_EXPORT_OBJ_CHARTS char filename[256]; XA_SPRINTF(filename, sizeof(filename), "debug_mesh_%03u_chartgroup_%03u_charts.obj", m_sourceMesh->id(), m_id); @@ -7745,7 +7196,6 @@ public: mesh->writeObjFace(file, faces[f]); } mesh->writeObjBoundaryEges(file); - mesh->writeObjLinkedBoundaries(file); fclose(file); } #endif @@ -7754,65 +7204,57 @@ public: mesh->~Mesh(); XA_FREE(mesh); XA_PROFILE_START(copyChartFaces) - // Copy basis. - const uint32_t chartCount = atlas.chartCount(); - m_chartBasis.resize(chartCount); - for (uint32_t i = 0; i < chartCount; i++) - m_chartBasis[i] = atlas.chartBasis(i); + if (progress->cancel) + return; // Copy faces from segment::Atlas to m_chartFaces array with <chart 0 face count> <face 0> <face n> <chart 1 face count> etc. encoding. // segment::Atlas faces refer to the chart group mesh. Map them to the input mesh instead. - m_chartFaces.resize(chartCount + faceCount); + const uint32_t chartCount = atlas.chartCount(); + Array<uint32_t> chartFaces; + chartFaces.resize(chartCount + faceCount); uint32_t offset = 0; for (uint32_t i = 0; i < chartCount; i++) { ConstArrayView<uint32_t> faces = atlas.chartFaces(i); - m_chartFaces[offset++] = faces.length; + chartFaces[offset++] = faces.length; for (uint32_t j = 0; j < faces.length; j++) - m_chartFaces[offset++] = m_faceToSourceFaceMap[faces[j]]; + chartFaces[offset++] = m_faceToSourceFaceMap[faces[j]]; } XA_PROFILE_END(copyChartFaces) #endif - } - -#if XA_RECOMPUTE_CHARTS - void parameterizeCharts(TaskScheduler *taskScheduler, const ParameterizeOptions &options, ThreadLocal<UniformGrid2> *boundaryGrid, ThreadLocal<ChartCtorBuffers> *chartBuffers, ThreadLocal<PiecewiseParam> *piecewiseParam) -#else - void parameterizeCharts(TaskScheduler* taskScheduler, const ParameterizeOptions &options, ThreadLocal<UniformGrid2>* boundaryGrid, ThreadLocal<ChartCtorBuffers>* chartBuffers) -#endif - { - // This function may be called multiple times, so destroy existing charts. - for (uint32_t i = 0; i < m_charts.size(); i++) { - m_charts[i]->~Chart(); - XA_FREE(m_charts[i]); - } - m_paramAddedChartsCount = 0; - const uint32_t chartCount = m_chartBasis.size(); + XA_PROFILE_START(createChartMeshAndParameterizeReal) + CreateAndParameterizeChartTaskGroupArgs groupArgs; + groupArgs.progress = progress; + groupArgs.boundaryGrid = boundaryGrid; + groupArgs.chartBuffers = chartBuffers; + groupArgs.options = &options; + groupArgs.pp = piecewiseParam; + TaskGroupHandle taskGroup = taskScheduler->createTaskGroup(&groupArgs, chartCount); Array<CreateAndParameterizeChartTaskArgs> taskArgs; taskArgs.resize(chartCount); taskArgs.runCtors(); // Has Array member. - TaskGroupHandle taskGroup = taskScheduler->createTaskGroup(chartCount); - uint32_t offset = 0; + offset = 0; for (uint32_t i = 0; i < chartCount; i++) { CreateAndParameterizeChartTaskArgs &args = taskArgs[i]; - args.basis = &m_chartBasis[i]; - args.boundaryGrid = boundaryGrid; +#if XA_DEBUG_SINGLE_CHART + args.basis = &chartBasis; + args.isPlanar = false; +#else + args.basis = &atlas.chartBasis(i); + args.chartGeneratorType = atlas.chartGeneratorType(i); +#endif args.chart = nullptr; args.chartGroupId = m_id; args.chartId = i; - args.chartBuffers = chartBuffers; - const uint32_t faceCount = m_chartFaces[offset++]; - args.faces = ConstArrayView<uint32_t>(&m_chartFaces[offset], faceCount); - offset += faceCount; + const uint32_t chartFaceCount = chartFaces[offset++]; + args.faces = ConstArrayView<uint32_t>(&chartFaces[offset], chartFaceCount); + offset += chartFaceCount; args.mesh = m_sourceMesh; - args.options = &options; -#if XA_RECOMPUTE_CHARTS - args.pp = piecewiseParam; -#endif Task task; task.userData = &args; task.func = runCreateAndParameterizeChartTask; taskScheduler->run(taskGroup, task); } taskScheduler->wait(&taskGroup); + XA_PROFILE_END(createChartMeshAndParameterizeReal) #if XA_RECOMPUTE_CHARTS // Count charts. Skip invalid ones and include new ones added by recomputing. uint32_t newChartCount = 0; @@ -7830,7 +7272,6 @@ public: if (chart->isInvalid()) { chart->~Chart(); XA_FREE(chart); - m_paramDeletedChartsCount++; continue; } m_charts[current++] = chart; @@ -7838,10 +7279,8 @@ public: // Now add new charts. for (uint32_t i = 0; i < chartCount; i++) { CreateAndParameterizeChartTaskArgs &args = taskArgs[i]; - for (uint32_t j = 0; j < args.charts.size(); j++) { + for (uint32_t j = 0; j < args.charts.size(); j++) m_charts[current++] = args.charts[j]; - m_paramAddedChartsCount++; - } } #else // XA_RECOMPUTE_CHARTS m_charts.resize(chartCount); @@ -7852,15 +7291,14 @@ public: } private: - Mesh *createMesh() - { + Mesh *createMesh() { XA_DEBUG_ASSERT(m_faceGroup != MeshFaceGroups::kInvalid); // Create new mesh from the source mesh, using faces that belong to this group. m_faceToSourceFaceMap.reserve(m_sourceMeshFaceGroups->faceCount(m_faceGroup)); for (MeshFaceGroups::Iterator it(m_sourceMeshFaceGroups, m_faceGroup); !it.isDone(); it.advance()) m_faceToSourceFaceMap.push_back(it.face()); // Only initial meshes has ignored faces. The only flag we care about is HasNormals. - const uint32_t faceCount = m_faceCount = m_faceToSourceFaceMap.size(); + const uint32_t faceCount = m_faceToSourceFaceMap.size(); XA_DEBUG_ASSERT(faceCount > 0); const uint32_t approxVertexCount = min(faceCount * 3, m_sourceMesh->vertexCount()); Mesh *mesh = XA_NEW_ARGS(MemTag::Mesh, Mesh, m_sourceMesh->epsilon(), approxVertexCount, faceCount, m_sourceMesh->flags() & MeshFlags::HasNormals); @@ -7889,9 +7327,7 @@ private: XA_DEBUG_ASSERT(indices[i] != UINT32_MAX); } // Don't copy flags - ignored faces aren't used by chart groups, they are handled by InvalidMeshGeometry. - Mesh::AddFaceResult::Enum result = mesh->addFace(indices); - XA_UNUSED(result); - XA_DEBUG_ASSERT(result == Mesh::AddFaceResult::OK); + mesh->addFace(indices); } XA_PROFILE_START(createChartGroupMeshColocals) mesh->createColocals(); @@ -7909,98 +7345,57 @@ private: } const uint32_t m_id; - const Mesh * const m_sourceMesh; - const MeshFaceGroups * const m_sourceMeshFaceGroups; + const Mesh *const m_sourceMesh; + const MeshFaceGroups *const m_sourceMeshFaceGroups; const MeshFaceGroups::Handle m_faceGroup; Array<uint32_t> m_faceToSourceFaceMap; // List of faces of the source mesh that belong to this chart group. - Array<Basis> m_chartBasis; // Copied from segment::Atlas. - Array<uint32_t> m_chartFaces; // Copied from segment::Atlas. Encoding: <chart 0 face count> <face 0> <face n> <chart 1 face count> etc. Array<Chart *> m_charts; - uint32_t m_faceCount; // Set by createMesh(). Used for sorting. - uint32_t m_paramAddedChartsCount; // Number of new charts added by recomputing charts with invalid parameterizations. - uint32_t m_paramDeletedChartsCount; // Number of charts with invalid parameterizations that were deleted, after charts were recomputed. -}; - -// References invalid faces and vertices in a mesh. -struct InvalidMeshGeometry -{ - // Invalid faces have the face groups MeshFaceGroups::kInvalid. - void extract(const Mesh *mesh, const MeshFaceGroups *meshFaceGroups) - { - // Copy invalid faces. - m_faces.clear(); - const uint32_t meshFaceCount = mesh->faceCount(); - for (uint32_t f = 0; f < meshFaceCount; f++) { - if (meshFaceGroups->groupAt(f) == MeshFaceGroups::kInvalid) - m_faces.push_back(f); - } - // Create *unique* list of vertices of invalid faces. - const uint32_t faceCount = m_faces.size(); - m_indices.resize(faceCount * 3); - const uint32_t approxVertexCount = min(faceCount * 3, mesh->vertexCount()); - m_vertexToSourceVertexMap.clear(); - m_vertexToSourceVertexMap.reserve(approxVertexCount); - HashMap<uint32_t, PassthroughHash<uint32_t>> sourceVertexToVertexMap(MemTag::Mesh, approxVertexCount); - for (uint32_t f = 0; f < faceCount; f++) { - const uint32_t face = m_faces[f]; - for (uint32_t i = 0; i < 3; i++) { - const uint32_t vertex = mesh->vertexAt(face * 3 + i); - uint32_t newVertex = sourceVertexToVertexMap.get(vertex); - if (newVertex == UINT32_MAX) { - newVertex = sourceVertexToVertexMap.add(vertex); - m_vertexToSourceVertexMap.push_back(vertex); - } - m_indices[f * 3 + i] = newVertex; - } - } - } - - ConstArrayView<uint32_t> faces() const { return m_faces; } - ConstArrayView<uint32_t> indices() const { return m_indices; } - ConstArrayView<uint32_t> vertices() const { return m_vertexToSourceVertexMap; } - -private: - Array<uint32_t> m_faces, m_indices; - Array<uint32_t> m_vertexToSourceVertexMap; // Map face vertices to vertices of the source mesh. }; -struct ChartGroupComputeChartFacesTaskArgs -{ +struct ChartGroupComputeChartsTaskGroupArgs { ThreadLocal<segment::Atlas> *atlas; - ChartGroup *chartGroup; const ChartOptions *options; Progress *progress; + TaskScheduler *taskScheduler; + ThreadLocal<UniformGrid2> *boundaryGrid; + ThreadLocal<ChartCtorBuffers> *chartBuffers; + ThreadLocal<PiecewiseParam> *piecewiseParam; }; -static void runChartGroupComputeChartFacesJob(void *userData) -{ - auto args = (ChartGroupComputeChartFacesTaskArgs *)userData; +static void runChartGroupComputeChartsTask(void *groupUserData, void *taskUserData) { + auto args = (ChartGroupComputeChartsTaskGroupArgs *)groupUserData; + auto chartGroup = (ChartGroup *)taskUserData; if (args->progress->cancel) return; XA_PROFILE_START(chartGroupComputeChartsThread) - args->chartGroup->computeChartFaces(*args->options, args->atlas->get()); + chartGroup->computeCharts(args->taskScheduler, *args->options, args->progress, args->atlas->get(), args->boundaryGrid, args->chartBuffers, args->piecewiseParam); XA_PROFILE_END(chartGroupComputeChartsThread) } -struct MeshComputeChartFacesTaskArgs -{ - Array<ChartGroup *> *chartGroups; // output - InvalidMeshGeometry *invalidMeshGeometry; // output +struct MeshComputeChartsTaskGroupArgs { ThreadLocal<segment::Atlas> *atlas; const ChartOptions *options; Progress *progress; - const Mesh *sourceMesh; TaskScheduler *taskScheduler; + ThreadLocal<UniformGrid2> *boundaryGrid; + ThreadLocal<ChartCtorBuffers> *chartBuffers; + ThreadLocal<PiecewiseParam> *piecewiseParam; +}; + +struct MeshComputeChartsTaskArgs { + const Mesh *sourceMesh; + Array<ChartGroup *> *chartGroups; // output + InvalidMeshGeometry *invalidMeshGeometry; // output }; #if XA_DEBUG_EXPORT_OBJ_FACE_GROUPS static uint32_t s_faceGroupsCurrentVertex = 0; #endif -static void runMeshComputeChartFacesJob(void *userData) -{ - auto args = (MeshComputeChartFacesTaskArgs *)userData; - if (args->progress->cancel) +static void runMeshComputeChartsTask(void *groupUserData, void *taskUserData) { + auto groupArgs = (MeshComputeChartsTaskGroupArgs *)groupUserData; + auto args = (MeshComputeChartsTaskArgs *)taskUserData; + if (groupArgs->progress->cancel) return; XA_PROFILE_START(computeChartsThread) // Create face groups. @@ -8009,7 +7404,7 @@ static void runMeshComputeChartFacesJob(void *userData) meshFaceGroups->compute(); const uint32_t chartGroupCount = meshFaceGroups->groupCount(); XA_PROFILE_END(createFaceGroups) - if (args->progress->cancel) + if (groupArgs->progress->cancel) goto cleanup; #if XA_DEBUG_EXPORT_OBJ_FACE_GROUPS { @@ -8053,33 +7448,41 @@ static void runMeshComputeChartFacesJob(void *userData) for (uint32_t i = 0; i < chartGroupCount; i++) (*args->chartGroups)[i] = XA_NEW_ARGS(MemTag::Default, ChartGroup, i, args->sourceMesh, meshFaceGroups, MeshFaceGroups::Handle(i)); // Extract invalid geometry via the invalid face group (MeshFaceGroups::kInvalid). - XA_PROFILE_START(extractInvalidMeshGeometry) - args->invalidMeshGeometry->extract(args->sourceMesh, meshFaceGroups); - XA_PROFILE_END(extractInvalidMeshGeometry) - // One task for each chart group - compute chart faces. + { + XA_PROFILE_START(extractInvalidMeshGeometry) + args->invalidMeshGeometry->extract(args->sourceMesh, meshFaceGroups); + XA_PROFILE_END(extractInvalidMeshGeometry) + } + // One task for each chart group - compute charts. { XA_PROFILE_START(chartGroupComputeChartsReal) - Array<ChartGroupComputeChartFacesTaskArgs> taskArgs; - taskArgs.resize(chartGroupCount); - for (uint32_t i = 0; i < chartGroupCount; i++) { - taskArgs[i].atlas = args->atlas; - taskArgs[i].chartGroup = (*args->chartGroups)[i]; - taskArgs[i].options = args->options; - taskArgs[i].progress = args->progress; - } - TaskGroupHandle taskGroup = args->taskScheduler->createTaskGroup(chartGroupCount); + // Sort chart groups by face count. + Array<float> chartGroupSortData; + chartGroupSortData.resize(chartGroupCount); + for (uint32_t i = 0; i < chartGroupCount; i++) + chartGroupSortData[i] = (float)(*args->chartGroups)[i]->faceCount(); + RadixSort chartGroupSort; + chartGroupSort.sort(chartGroupSortData); + // Larger chart groups are added first to reduce the chance of thread starvation. + ChartGroupComputeChartsTaskGroupArgs taskGroupArgs; + taskGroupArgs.atlas = groupArgs->atlas; + taskGroupArgs.options = groupArgs->options; + taskGroupArgs.progress = groupArgs->progress; + taskGroupArgs.taskScheduler = groupArgs->taskScheduler; + taskGroupArgs.boundaryGrid = groupArgs->boundaryGrid; + taskGroupArgs.chartBuffers = groupArgs->chartBuffers; + taskGroupArgs.piecewiseParam = groupArgs->piecewiseParam; + TaskGroupHandle taskGroup = groupArgs->taskScheduler->createTaskGroup(&taskGroupArgs, chartGroupCount); for (uint32_t i = 0; i < chartGroupCount; i++) { Task task; - task.userData = &taskArgs[i]; - task.func = runChartGroupComputeChartFacesJob; - args->taskScheduler->run(taskGroup, task); + task.userData = (*args->chartGroups)[chartGroupCount - i - 1]; + task.func = runChartGroupComputeChartsTask; + groupArgs->taskScheduler->run(taskGroup, task); } - args->taskScheduler->wait(&taskGroup); + groupArgs->taskScheduler->wait(&taskGroup); XA_PROFILE_END(chartGroupComputeChartsReal) } XA_PROFILE_END(computeChartsThread) - args->progress->value++; - args->progress->update(); cleanup: if (meshFaceGroups) { meshFaceGroups->~MeshFaceGroups(); @@ -8087,43 +7490,13 @@ cleanup: } } -struct ParameterizeChartsTaskArgs -{ - TaskScheduler *taskScheduler; - ChartGroup *chartGroup; - const ParameterizeOptions *options; - ThreadLocal<UniformGrid2> *boundaryGrid; - ThreadLocal<ChartCtorBuffers> *chartBuffers; -#if XA_RECOMPUTE_CHARTS - ThreadLocal<PiecewiseParam> *piecewiseParam; -#endif - Progress *progress; -}; - -static void runParameterizeChartsJob(void *userData) -{ - auto args = (ParameterizeChartsTaskArgs *)userData; - if (args->progress->cancel) - return; - XA_PROFILE_START(parameterizeChartsThread) -#if XA_RECOMPUTE_CHARTS - args->chartGroup->parameterizeCharts(args->taskScheduler, *args->options, args->boundaryGrid, args->chartBuffers, args->piecewiseParam); -#else - args->chartGroup->parameterizeCharts(args->taskScheduler, *args->options, args->boundaryGrid, args->chartBuffers); -#endif - XA_PROFILE_END(parameterizeChartsThread) - args->progress->value++; - args->progress->update(); -} - /// An atlas is a set of chart groups. -class Atlas -{ +class Atlas { public: - Atlas() : m_chartsComputed(false), m_chartsParameterized(false) {} + Atlas() : + m_chartsComputed(false) {} - ~Atlas() - { + ~Atlas() { for (uint32_t i = 0; i < m_meshChartGroups.size(); i++) { for (uint32_t j = 0; j < m_meshChartGroups[i].size(); j++) { m_meshChartGroups[i][j]->~ChartGroup(); @@ -8137,22 +7510,25 @@ public: uint32_t meshCount() const { return m_meshes.size(); } const InvalidMeshGeometry &invalidMeshGeometry(uint32_t meshIndex) const { return m_invalidMeshGeometry[meshIndex]; } bool chartsComputed() const { return m_chartsComputed; } - bool chartsParameterized() const { return m_chartsParameterized; } uint32_t chartGroupCount(uint32_t mesh) const { return m_meshChartGroups[mesh].size(); } const ChartGroup *chartGroupAt(uint32_t mesh, uint32_t group) const { return m_meshChartGroups[mesh][group]; } - void addMesh(const Mesh *mesh) - { + void addMesh(const Mesh *mesh) { m_meshes.push_back(mesh); } - bool computeCharts(TaskScheduler *taskScheduler, const ChartOptions &options, ProgressFunc progressFunc, void *progressUserData) - { + bool computeCharts(TaskScheduler *taskScheduler, const ChartOptions &options, ProgressFunc progressFunc, void *progressUserData) { + XA_PROFILE_START(computeChartsReal) #if XA_DEBUG_EXPORT_OBJ_PLANAR_REGIONS segment::s_planarRegionsCurrentRegion = segment::s_planarRegionsCurrentVertex = 0; #endif + // Progress is per-face x 2 (1 for chart faces, 1 for parameterized chart faces). + const uint32_t meshCount = m_meshes.size(); + uint32_t totalFaceCount = 0; + for (uint32_t i = 0; i < meshCount; i++) + totalFaceCount += m_meshes[i]->faceCount(); + Progress progress(ProgressCategory::ComputeCharts, progressFunc, progressUserData, totalFaceCount * 2); m_chartsComputed = false; - m_chartsParameterized = false; // Clear chart groups, since this function may be called multiple times. if (!m_meshChartGroups.isEmpty()) { for (uint32_t i = 0; i < m_meshChartGroups.size(); i++) { @@ -8162,27 +7538,20 @@ public: } m_meshChartGroups[i].clear(); } - XA_ASSERT(m_meshChartGroups.size() == m_meshes.size()); // The number of meshes shouldn't have changed. + XA_ASSERT(m_meshChartGroups.size() == meshCount); // The number of meshes shouldn't have changed. } - m_meshChartGroups.resize(m_meshes.size()); + m_meshChartGroups.resize(meshCount); m_meshChartGroups.runCtors(); - m_invalidMeshGeometry.resize(m_meshes.size()); + m_invalidMeshGeometry.resize(meshCount); m_invalidMeshGeometry.runCtors(); // One task per mesh. - const uint32_t meshCount = m_meshes.size(); - Progress progress(ProgressCategory::ComputeCharts, progressFunc, progressUserData, meshCount); - ThreadLocal<segment::Atlas> atlas; - Array<MeshComputeChartFacesTaskArgs> taskArgs; + Array<MeshComputeChartsTaskArgs> taskArgs; taskArgs.resize(meshCount); for (uint32_t i = 0; i < meshCount; i++) { - MeshComputeChartFacesTaskArgs &args = taskArgs[i]; - args.atlas = &atlas; + MeshComputeChartsTaskArgs &args = taskArgs[i]; + args.sourceMesh = m_meshes[i]; args.chartGroups = &m_meshChartGroups[i]; args.invalidMeshGeometry = &m_invalidMeshGeometry[i]; - args.options = &options; - args.progress = &progress; - args.sourceMesh = m_meshes[i]; - args.taskScheduler = taskScheduler; } // Sort meshes by indexCount. Array<float> meshSortData; @@ -8192,105 +7561,53 @@ public: RadixSort meshSort; meshSort.sort(meshSortData); // Larger meshes are added first to reduce the chance of thread starvation. - TaskGroupHandle taskGroup = taskScheduler->createTaskGroup(meshCount); - for (uint32_t i = 0; i < meshCount; i++) { - Task task; - task.userData = &taskArgs[meshSort.ranks()[meshCount - i - 1]]; - task.func = runMeshComputeChartFacesJob; - taskScheduler->run(taskGroup, task); - } - taskScheduler->wait(&taskGroup); - if (progress.cancel) - return false; - m_chartsComputed = true; - return true; - } - - bool parameterizeCharts(TaskScheduler *taskScheduler, const ParameterizeOptions &options, ProgressFunc progressFunc, void *progressUserData) - { - m_chartsParameterized = false; - uint32_t chartGroupCount = 0; - for (uint32_t i = 0; i < m_meshChartGroups.size(); i++) - chartGroupCount += m_meshChartGroups[i].size(); - Progress progress(ProgressCategory::ParameterizeCharts, progressFunc, progressUserData, chartGroupCount); + ThreadLocal<segment::Atlas> atlas; ThreadLocal<UniformGrid2> boundaryGrid; // For Quality boundary intersection. ThreadLocal<ChartCtorBuffers> chartBuffers; -#if XA_RECOMPUTE_CHARTS ThreadLocal<PiecewiseParam> piecewiseParam; -#endif - Array<ParameterizeChartsTaskArgs> taskArgs; - taskArgs.resize(chartGroupCount); - { - uint32_t k = 0; - for (uint32_t i = 0; i < m_meshChartGroups.size(); i++) { - const uint32_t count = m_meshChartGroups[i].size(); - for (uint32_t j = 0; j < count; j++) { - ParameterizeChartsTaskArgs &args = taskArgs[k]; - args.taskScheduler = taskScheduler; - args.chartGroup = m_meshChartGroups[i][j]; - args.options = &options; - args.boundaryGrid = &boundaryGrid; - args.chartBuffers = &chartBuffers; -#if XA_RECOMPUTE_CHARTS - args.piecewiseParam = &piecewiseParam; -#endif - args.progress = &progress; - k++; - } - } - } - // Sort chart groups by face count. - Array<float> chartGroupSortData; - chartGroupSortData.resize(chartGroupCount); - { - uint32_t k = 0; - for (uint32_t i = 0; i < m_meshChartGroups.size(); i++) { - const uint32_t count = m_meshChartGroups[i].size(); - for (uint32_t j = 0; j < count; j++) { - chartGroupSortData[k++] = (float)m_meshChartGroups[i][j]->faceCount(); - } - } - } - RadixSort chartGroupSort; - chartGroupSort.sort(chartGroupSortData); - // Larger chart groups are added first to reduce the chance of thread starvation. - TaskGroupHandle taskGroup = taskScheduler->createTaskGroup(chartGroupCount); - for (uint32_t i = 0; i < chartGroupCount; i++) { + MeshComputeChartsTaskGroupArgs taskGroupArgs; + taskGroupArgs.atlas = &atlas; + taskGroupArgs.options = &options; + taskGroupArgs.progress = &progress; + taskGroupArgs.taskScheduler = taskScheduler; + taskGroupArgs.boundaryGrid = &boundaryGrid; + taskGroupArgs.chartBuffers = &chartBuffers; + taskGroupArgs.piecewiseParam = &piecewiseParam; + TaskGroupHandle taskGroup = taskScheduler->createTaskGroup(&taskGroupArgs, meshCount); + for (uint32_t i = 0; i < meshCount; i++) { Task task; - task.userData = &taskArgs[chartGroupSort.ranks()[chartGroupCount - i - 1]]; - task.func = runParameterizeChartsJob; + task.userData = &taskArgs[meshSort.ranks()[meshCount - i - 1]]; + task.func = runMeshComputeChartsTask; taskScheduler->run(taskGroup, task); } taskScheduler->wait(&taskGroup); + XA_PROFILE_END(computeChartsReal) if (progress.cancel) return false; - m_chartsParameterized = true; + m_chartsComputed = true; return true; } private: Array<const Mesh *> m_meshes; Array<InvalidMeshGeometry> m_invalidMeshGeometry; // 1 per mesh. - Array<Array<ChartGroup *> > m_meshChartGroups; + Array<Array<ChartGroup *>> m_meshChartGroups; bool m_chartsComputed; - bool m_chartsParameterized; }; } // namespace param namespace pack { -class AtlasImage -{ +class AtlasImage { public: - AtlasImage(uint32_t width, uint32_t height) : m_width(width), m_height(height) - { + AtlasImage(uint32_t width, uint32_t height) : + m_width(width), m_height(height) { m_data.resize(m_width * m_height); memset(m_data.data(), 0, sizeof(uint32_t) * m_data.size()); } - void resize(uint32_t width, uint32_t height) - { + void resize(uint32_t width, uint32_t height) { Array<uint32_t> data; data.resize(width * height); memset(data.data(), 0, sizeof(uint32_t) * data.size()); @@ -8301,8 +7618,7 @@ public: data.moveTo(m_data); } - void addChart(uint32_t chartIndex, const BitImage *image, const BitImage *imageBilinear, const BitImage *imagePadding, int atlas_w, int atlas_h, int offset_x, int offset_y) - { + void addChart(uint32_t chartIndex, const BitImage *image, const BitImage *imageBilinear, const BitImage *imagePadding, int atlas_w, int atlas_h, int offset_x, int offset_y) { const int w = image->width(); const int h = image->height(); for (int y = 0; y < h; y++) { @@ -8328,15 +7644,13 @@ public: } } - void copyTo(uint32_t *dest, uint32_t destWidth, uint32_t destHeight, int padding) const - { + void copyTo(uint32_t *dest, uint32_t destWidth, uint32_t destHeight, int padding) const { for (uint32_t y = 0; y < destHeight; y++) memcpy(&dest[y * destWidth], &m_data[padding + (y + padding) * m_width], destWidth * sizeof(uint32_t)); } #if XA_DEBUG_EXPORT_ATLAS_IMAGES - void writeTga(const char *filename, uint32_t width, uint32_t height) const - { + void writeTga(const char *filename, uint32_t width, uint32_t height) const { Array<uint8_t> image; image.resize(width * height * 3); for (uint32_t y = 0; y < height; y++) { @@ -8378,18 +7692,14 @@ private: Array<uint32_t> m_data; }; -struct Chart -{ +struct Chart { int32_t atlasIndex; uint32_t material; - uint32_t indexCount; - const uint32_t *indices; + ConstArrayView<uint32_t> indices; float parametricArea; float surfaceArea; - Vector2 *vertices; - uint32_t vertexCount; + ArrayView<Vector2> vertices; Array<uint32_t> uniqueVertices; - bool allowRotate; // bounding box Vector2 majorAxis, minorAxis, minCorner, maxCorner; // Mesh only @@ -8398,29 +7708,26 @@ struct Chart Array<uint32_t> faces; Vector2 &uniqueVertexAt(uint32_t v) { return uniqueVertices.isEmpty() ? vertices[v] : vertices[uniqueVertices[v]]; } - uint32_t uniqueVertexCount() const { return uniqueVertices.isEmpty() ? vertexCount : uniqueVertices.size(); } + uint32_t uniqueVertexCount() const { return uniqueVertices.isEmpty() ? vertices.length : uniqueVertices.size(); } }; -struct AddChartTaskArgs -{ - ThreadLocal<BoundingBox2D> *boundingBox; +struct AddChartTaskArgs { param::Chart *paramChart; Chart *chart; // out }; -static void runAddChartTask(void *userData) -{ +static void runAddChartTask(void *groupUserData, void *taskUserData) { XA_PROFILE_START(packChartsAddChartsThread) - auto args = (AddChartTaskArgs *)userData; + auto boundingBox = (ThreadLocal<BoundingBox2D> *)groupUserData; + auto args = (AddChartTaskArgs *)taskUserData; param::Chart *paramChart = args->paramChart; XA_PROFILE_START(packChartsAddChartsRestoreTexcoords) paramChart->restoreTexcoords(); XA_PROFILE_END(packChartsAddChartsRestoreTexcoords) - Mesh *mesh = paramChart->mesh(); + Mesh *mesh = paramChart->unifiedMesh(); Chart *chart = args->chart = XA_NEW(MemTag::Default, Chart); chart->atlasIndex = -1; chart->material = 0; - chart->indexCount = mesh->indexCount(); chart->indices = mesh->indices(); chart->parametricArea = mesh->computeParametricArea(); if (chart->parametricArea < kAreaEpsilon) { @@ -8430,17 +7737,15 @@ static void runAddChartTask(void *userData) } chart->surfaceArea = mesh->computeSurfaceArea(); chart->vertices = mesh->texcoords(); - chart->vertexCount = mesh->vertexCount(); - chart->allowRotate = true; chart->boundaryEdges = &mesh->boundaryEdges(); // Compute bounding box of chart. - BoundingBox2D &bb = args->boundingBox->get(); + BoundingBox2D &bb = boundingBox->get(); bb.clear(); - for (uint32_t v = 0; v < chart->vertexCount; v++) { + for (uint32_t v = 0; v < chart->vertices.length; v++) { if (mesh->isBoundaryVertex(v)) bb.appendBoundaryVertex(mesh->texcoord(v)); } - bb.compute(mesh->texcoords(), mesh->vertexCount()); + bb.compute(mesh->texcoords()); chart->majorAxis = bb.majorAxis; chart->minorAxis = bb.minorAxis; chart->minCorner = bb.minCorner; @@ -8448,10 +7753,8 @@ static void runAddChartTask(void *userData) XA_PROFILE_END(packChartsAddChartsThread) } -struct Atlas -{ - ~Atlas() - { +struct Atlas { + ~Atlas() { for (uint32_t i = 0; i < m_atlasImages.size(); i++) { m_atlasImages[i]->~AtlasImage(); XA_FREE(m_atlasImages[i]); @@ -8475,8 +7778,7 @@ struct Atlas const Array<AtlasImage *> &getImages() const { return m_atlasImages; } float getUtilization(uint32_t atlas) const { return m_utilization[atlas]; } - void addCharts(TaskScheduler *taskScheduler, param::Atlas *paramAtlas) - { + void addCharts(TaskScheduler *taskScheduler, param::Atlas *paramAtlas) { // Count charts. uint32_t chartCount = 0; for (uint32_t i = 0; i < paramAtlas->meshCount(); i++) { @@ -8489,11 +7791,11 @@ struct Atlas if (chartCount == 0) return; // Run one task per chart. + ThreadLocal<BoundingBox2D> boundingBox; + TaskGroupHandle taskGroup = taskScheduler->createTaskGroup(&boundingBox, chartCount); Array<AddChartTaskArgs> taskArgs; taskArgs.resize(chartCount); - TaskGroupHandle taskGroup = taskScheduler->createTaskGroup(chartCount); uint32_t chartIndex = 0; - ThreadLocal<BoundingBox2D> boundingBox; for (uint32_t i = 0; i < paramAtlas->meshCount(); i++) { const uint32_t chartGroupsCount = paramAtlas->chartGroupCount(i); for (uint32_t j = 0; j < chartGroupsCount; j++) { @@ -8501,7 +7803,6 @@ struct Atlas const uint32_t count = chartGroup->chartCount(); for (uint32_t k = 0; k < count; k++) { AddChartTaskArgs &args = taskArgs[chartIndex]; - args.boundingBox = &boundingBox; args.paramChart = chartGroup->chartAt(k); Task task; task.userData = &taskArgs[chartIndex]; @@ -8518,8 +7819,10 @@ struct Atlas m_charts[i] = taskArgs[i].chart; } - void addUvMeshCharts(UvMeshInstance *mesh) - { + void addUvMeshCharts(UvMeshInstance *mesh) { + // Copy texcoords from mesh. + mesh->texcoords.resize(mesh->mesh->texcoords.size()); + memcpy(mesh->texcoords.data(), mesh->mesh->texcoords.data(), mesh->texcoords.size() * sizeof(Vector2)); BitArray vertexUsed(mesh->texcoords.size()); BoundingBox2D boundingBox; for (uint32_t c = 0; c < mesh->mesh->charts.size(); c++) { @@ -8527,17 +7830,14 @@ struct Atlas Chart *chart = XA_NEW(MemTag::Default, Chart); chart->atlasIndex = -1; chart->material = uvChart->material; - chart->indexCount = uvChart->indices.size(); - chart->indices = uvChart->indices.data(); - chart->vertices = mesh->texcoords.data(); - chart->vertexCount = mesh->texcoords.size(); - chart->allowRotate = mesh->rotateCharts; + chart->indices = uvChart->indices; + chart->vertices = mesh->texcoords; chart->boundaryEdges = nullptr; chart->faces.resize(uvChart->faces.size()); memcpy(chart->faces.data(), uvChart->faces.data(), sizeof(uint32_t) * uvChart->faces.size()); // Find unique vertices. vertexUsed.zeroOutMemory(); - for (uint32_t i = 0; i < chart->indexCount; i++) { + for (uint32_t i = 0; i < chart->indices.length; i++) { const uint32_t vertex = chart->indices[i]; if (!vertexUsed.get(vertex)) { vertexUsed.set(vertex); @@ -8546,14 +7846,13 @@ struct Atlas } // Compute parametric and surface areas. chart->parametricArea = 0.0f; - for (uint32_t f = 0; f < chart->indexCount / 3; f++) { + for (uint32_t f = 0; f < chart->indices.length / 3; f++) { const Vector2 &v1 = chart->vertices[chart->indices[f * 3 + 0]]; const Vector2 &v2 = chart->vertices[chart->indices[f * 3 + 1]]; const Vector2 &v3 = chart->vertices[chart->indices[f * 3 + 2]]; chart->parametricArea += fabsf(triangleArea(v1, v2, v3)); } chart->parametricArea *= 0.5f; - chart->surfaceArea = chart->parametricArea; // Identical for UV meshes. if (chart->parametricArea < kAreaEpsilon) { // When the parametric area is too small we use a rough approximation to prevent divisions by very small numbers. Vector2 minCorner(FLT_MAX, FLT_MAX); @@ -8565,6 +7864,9 @@ struct Atlas const Vector2 bounds = (maxCorner - minCorner) * 0.5f; chart->parametricArea = bounds.x * bounds.y; } + XA_DEBUG_ASSERT(isFinite(chart->parametricArea)); + XA_DEBUG_ASSERT(!isNan(chart->parametricArea)); + chart->surfaceArea = chart->parametricArea; // Identical for UV meshes. // Compute bounding box of chart. // Using all unique vertices for simplicity, can compute real boundaries if this is too slow. boundingBox.clear(); @@ -8580,8 +7882,7 @@ struct Atlas } // Pack charts in the smallest possible rectangle. - bool packCharts(const PackOptions &options, ProgressFunc progressFunc, void *progressUserData) - { + bool packCharts(const PackOptions &options, ProgressFunc progressFunc, void *progressUserData) { if (progressFunc) { if (!progressFunc(ProgressCategory::PackCharts, 0, progressUserData)) return false; @@ -8627,19 +7928,19 @@ struct Atlas // Compute chart scale float scale = 1.0f; if (chart->parametricArea != 0.0f) { - scale = (chart->surfaceArea / chart->parametricArea) * m_texelsPerUnit; + scale = sqrtf(chart->surfaceArea / chart->parametricArea) * m_texelsPerUnit; XA_ASSERT(isFinite(scale)); } // Translate, rotate and scale vertices. Compute extents. Vector2 minCorner(FLT_MAX, FLT_MAX); - if (!chart->allowRotate) { + if (!options.rotateChartsToAxis) { for (uint32_t i = 0; i < chart->uniqueVertexCount(); i++) minCorner = min(minCorner, chart->uniqueVertexAt(i)); } Vector2 extents(0.0f); for (uint32_t i = 0; i < chart->uniqueVertexCount(); i++) { Vector2 &texcoord = chart->uniqueVertexAt(i); - if (chart->allowRotate) { + if (options.rotateChartsToAxis) { const float x = dot(texcoord, chart->majorAxis); const float y = dot(texcoord, chart->minorAxis); texcoord.x = x; @@ -8750,27 +8051,27 @@ struct Atlas // Resize and clear (discard = true) chart images. // Leave room for padding at extents. chartImage.resize(ftoi_ceil(chartExtents[c].x) + options.padding, ftoi_ceil(chartExtents[c].y) + options.padding, true); - if (chart->allowRotate) + if (options.rotateCharts) chartImageRotated.resize(chartImage.height(), chartImage.width(), true); if (options.bilinear) { chartImageBilinear.resize(chartImage.width(), chartImage.height(), true); - if (chart->allowRotate) + if (options.rotateCharts) chartImageBilinearRotated.resize(chartImage.height(), chartImage.width(), true); } // Rasterize chart faces. - const uint32_t faceCount = chart->indexCount / 3; + const uint32_t faceCount = chart->indices.length / 3; for (uint32_t f = 0; f < faceCount; f++) { Vector2 vertices[3]; for (uint32_t v = 0; v < 3; v++) vertices[v] = chart->vertices[chart->indices[f * 3 + v]]; DrawTriangleCallbackArgs args; args.chartBitImage = &chartImage; - args.chartBitImageRotated = chart->allowRotate ? &chartImageRotated : nullptr; + args.chartBitImageRotated = options.rotateCharts ? &chartImageRotated : nullptr; raster::drawTriangle(Vector2((float)chartImage.width(), (float)chartImage.height()), vertices, drawTriangleCallback, &args); } // Expand chart by pixels sampled by bilinear interpolation. if (options.bilinear) - bilinearExpand(chart, &chartImage, &chartImageBilinear, chart->allowRotate ? &chartImageBilinearRotated : nullptr, boundaryEdgeGrid); + bilinearExpand(chart, &chartImage, &chartImageBilinear, options.rotateCharts ? &chartImageBilinearRotated : nullptr, boundaryEdgeGrid); // Expand chart by padding pixels (dilation). if (options.padding > 0) { // Copy into the same BitImage instances for every chart to avoid reallocating BitImage buffers (largest chart is packed first). @@ -8780,7 +8081,7 @@ struct Atlas else chartImage.copyTo(chartImagePadding); chartImagePadding.dilate(options.padding); - if (chart->allowRotate) { + if (options.rotateCharts) { if (options.bilinear) chartImageBilinearRotated.copyTo(chartImagePaddingRotated); else @@ -8815,23 +8116,25 @@ struct Atlas int best_x = 0, best_y = 0; int best_cw = 0, best_ch = 0; int best_r = 0; - for (;;) - { + for (;;) { +#if XA_DEBUG bool firstChartInBitImage = false; - XA_UNUSED(firstChartInBitImage); +#endif if (currentAtlas + 1 > m_bitImages.size()) { // Chart doesn't fit in the current bitImage, create a new one. BitImage *bi = XA_NEW_ARGS(MemTag::Default, BitImage, resolution, resolution); m_bitImages.push_back(bi); atlasSizes.push_back(Vector2i(0, 0)); +#if XA_DEBUG firstChartInBitImage = true; +#endif if (createImage) m_atlasImages.push_back(XA_NEW_ARGS(MemTag::Default, AtlasImage, resolution, resolution)); // Start positions are per-atlas, so create a new one of those too. chartStartPositions.push_back(Vector2i(0, 0)); } XA_PROFILE_START(packChartsFindLocation) - const bool foundLocation = findChartLocation(chartStartPositions[currentAtlas], options.bruteForce, m_bitImages[currentAtlas], chartImageToPack, chartImageToPackRotated, atlasSizes[currentAtlas].x, atlasSizes[currentAtlas].y, &best_x, &best_y, &best_cw, &best_ch, &best_r, options.blockAlign, maxResolution, chart->allowRotate); + const bool foundLocation = findChartLocation(options, chartStartPositions[currentAtlas], m_bitImages[currentAtlas], chartImageToPack, chartImageToPackRotated, atlasSizes[currentAtlas].x, atlasSizes[currentAtlas].y, &best_x, &best_y, &best_cw, &best_ch, &best_r, maxResolution); XA_PROFILE_END(packChartsFindLocation) XA_DEBUG_ASSERT(!(firstChartInBitImage && !foundLocation)); // Chart doesn't fit in an empty, newly allocated bitImage. Shouldn't happen, since charts are resized if they are too big to fit in the atlas. if (maxResolution == 0) { @@ -8849,8 +8152,7 @@ struct Atlas if (best_x + best_cw > atlasSizes[currentAtlas].x || best_y + best_ch > atlasSizes[currentAtlas].y) { for (uint32_t j = 0; j < chartStartPositions.size(); j++) chartStartPositions[j] = Vector2i(0, 0); - } - else { + } else { chartStartPositions[currentAtlas] = Vector2i(best_x, best_y); } } @@ -8897,7 +8199,7 @@ struct Atlas Vector2 &texcoord = chart->uniqueVertexAt(v); Vector2 t = texcoord; if (best_r) { - XA_DEBUG_ASSERT(chart->allowRotate); + XA_DEBUG_ASSERT(options.rotateCharts); swap(t.x, t.y); } texcoord.x = best_x + t.x; @@ -8938,8 +8240,7 @@ struct Atlas } if (m_utilization.size() > 1) { XA_PRINT(" %u: %f%% utilization\n", i, m_utilization[i] * 100.0f); - } - else { + } else { XA_PRINT(" %f%% utilization\n", m_utilization[i] * 100.0f); } } @@ -8958,28 +8259,22 @@ struct Atlas } private: - // IC: Brute force is slow, and random may take too much time to converge. We start inserting large charts in a small atlas. Using brute force is lame, because most of the space - // is occupied at this point. At the end we have many small charts and a large atlas with sparse holes. Finding those holes randomly is slow. A better approach would be to - // start stacking large charts as if they were tetris pieces. Once charts get small try to place them randomly. It may be interesting to try a intermediate strategy, first try - // along one axis and then try exhaustively along that axis. - bool findChartLocation(const Vector2i &startPosition, bool bruteForce, const BitImage *atlasBitImage, const BitImage *chartBitImage, const BitImage *chartBitImageRotated, int w, int h, int *best_x, int *best_y, int *best_w, int *best_h, int *best_r, bool blockAligned, uint32_t maxResolution, bool allowRotate) - { + bool findChartLocation(const PackOptions &options, const Vector2i &startPosition, const BitImage *atlasBitImage, const BitImage *chartBitImage, const BitImage *chartBitImageRotated, int w, int h, int *best_x, int *best_y, int *best_w, int *best_h, int *best_r, uint32_t maxResolution) { const int attempts = 4096; - if (bruteForce || attempts >= w * h) - return findChartLocation_bruteForce(startPosition, atlasBitImage, chartBitImage, chartBitImageRotated, w, h, best_x, best_y, best_w, best_h, best_r, blockAligned, maxResolution, allowRotate); - return findChartLocation_random(atlasBitImage, chartBitImage, chartBitImageRotated, w, h, best_x, best_y, best_w, best_h, best_r, attempts, blockAligned, maxResolution, allowRotate); + if (options.bruteForce || attempts >= w * h) + return findChartLocation_bruteForce(options, startPosition, atlasBitImage, chartBitImage, chartBitImageRotated, w, h, best_x, best_y, best_w, best_h, best_r, maxResolution); + return findChartLocation_random(options, atlasBitImage, chartBitImage, chartBitImageRotated, w, h, best_x, best_y, best_w, best_h, best_r, attempts, maxResolution); } - bool findChartLocation_bruteForce(const Vector2i &startPosition, const BitImage *atlasBitImage, const BitImage *chartBitImage, const BitImage *chartBitImageRotated, int w, int h, int *best_x, int *best_y, int *best_w, int *best_h, int *best_r, bool blockAligned, uint32_t maxResolution, bool allowRotate) - { - const int stepSize = blockAligned ? 4 : 1; + bool findChartLocation_bruteForce(const PackOptions &options, const Vector2i &startPosition, const BitImage *atlasBitImage, const BitImage *chartBitImage, const BitImage *chartBitImageRotated, int w, int h, int *best_x, int *best_y, int *best_w, int *best_h, int *best_r, uint32_t maxResolution) { + const int stepSize = options.blockAlign ? 4 : 1; int best_metric = INT_MAX; // Try two different orientations. for (int r = 0; r < 2; r++) { int cw = chartBitImage->width(); int ch = chartBitImage->height(); if (r == 1) { - if (allowRotate) + if (options.rotateCharts) swap(cw, ch); else break; @@ -9016,15 +8311,14 @@ private: return best_metric != INT_MAX; } - bool findChartLocation_random(const BitImage *atlasBitImage, const BitImage *chartBitImage, const BitImage *chartBitImageRotated, int w, int h, int *best_x, int *best_y, int *best_w, int *best_h, int *best_r, int minTrialCount, bool blockAligned, uint32_t maxResolution, bool allowRotate) - { + bool findChartLocation_random(const PackOptions &options, const BitImage *atlasBitImage, const BitImage *chartBitImage, const BitImage *chartBitImageRotated, int w, int h, int *best_x, int *best_y, int *best_w, int *best_h, int *best_r, int attempts, uint32_t maxResolution) { bool result = false; const int BLOCK_SIZE = 4; int best_metric = INT_MAX; - for (int i = 0; i < minTrialCount; i++) { + for (int i = 0; i < attempts; i++) { int cw = chartBitImage->width(); int ch = chartBitImage->height(); - int r = allowRotate ? m_rand.getRange(1) : 0; + int r = options.rotateCharts ? m_rand.getRange(1) : 0; if (r == 1) swap(cw, ch); // + 1 to extend atlas in case atlas full. We may want to use a higher number to increase probability of extending atlas. @@ -9037,7 +8331,7 @@ private: } int x = m_rand.getRange(xRange); int y = m_rand.getRange(yRange); - if (blockAligned) { + if (options.blockAlign) { x = align(x, BLOCK_SIZE); y = align(y, BLOCK_SIZE); if (maxResolution > 0 && (x > (int)maxResolution - cw || y > (int)maxResolution - ch)) @@ -9062,7 +8356,7 @@ private: *best_y = y; *best_w = cw; *best_h = ch; - *best_r = allowRotate ? r : 0; + *best_r = options.rotateCharts ? r : 0; if (area == w * h) { // Chart is completely inside, do not look at any other location. break; @@ -9072,8 +8366,7 @@ private: return result; } - void addChart(BitImage *atlasBitImage, const BitImage *chartBitImage, const BitImage *chartBitImageRotated, int atlas_w, int atlas_h, int offset_x, int offset_y, int r) - { + void addChart(BitImage *atlasBitImage, const BitImage *chartBitImage, const BitImage *chartBitImageRotated, int atlas_w, int atlas_h, int offset_x, int offset_y, int r) { XA_DEBUG_ASSERT(r == 0 || r == 1); const BitImage *image = r == 0 ? chartBitImage : chartBitImageRotated; const int w = image->width(); @@ -9096,15 +8389,14 @@ private: } } - void bilinearExpand(const Chart *chart, BitImage *source, BitImage *dest, BitImage *destRotated, UniformGrid2 &boundaryEdgeGrid) const - { + void bilinearExpand(const Chart *chart, BitImage *source, BitImage *dest, BitImage *destRotated, UniformGrid2 &boundaryEdgeGrid) const { boundaryEdgeGrid.reset(chart->vertices, chart->indices); if (chart->boundaryEdges) { const uint32_t edgeCount = chart->boundaryEdges->size(); for (uint32_t i = 0; i < edgeCount; i++) boundaryEdgeGrid.append((*chart->boundaryEdges)[i]); } else { - for (uint32_t i = 0; i < chart->indexCount; i++) + for (uint32_t i = 0; i < chart->indices.length; i++) boundaryEdgeGrid.append(i); } const int xOffsets[] = { -1, 0, 1, -1, 1, -1, 0, 1 }; @@ -9152,13 +8444,11 @@ private: } } - struct DrawTriangleCallbackArgs - { + struct DrawTriangleCallbackArgs { BitImage *chartBitImage, *chartBitImageRotated; }; - static bool drawTriangleCallback(void *param, int x, int y) - { + static bool drawTriangleCallback(void *param, int x, int y) { auto args = (DrawTriangleCallbackArgs *)param; args->chartBitImage->set(x, y); if (args->chartBitImageRotated) @@ -9180,8 +8470,14 @@ private: } // namespace pack } // namespace internal -struct Context -{ +// Used to map triangulated polygons back to polygons. +struct MeshPolygonMapping { + internal::Array<uint8_t> faceVertexCount; // Copied from MeshDecl::faceVertexCount. + internal::Array<uint32_t> triangleToPolygonMap; // Triangle index (mesh face index) to polygon index. + internal::Array<uint32_t> triangleToPolygonIndicesMap; // Triangle indices to polygon indices. +}; + +struct Context { Atlas atlas; internal::Progress *addMeshProgress = nullptr; internal::TaskGroupHandle addMeshTaskGroup; @@ -9190,20 +8486,20 @@ struct Context void *progressUserData = nullptr; internal::TaskScheduler *taskScheduler; internal::Array<internal::Mesh *> meshes; + internal::Array<MeshPolygonMapping *> meshPolygonMappings; internal::Array<internal::UvMesh *> uvMeshes; internal::Array<internal::UvMeshInstance *> uvMeshInstances; + bool uvMeshChartsComputed = false; }; -Atlas *Create() -{ +Atlas *Create() { Context *ctx = XA_NEW(internal::MemTag::Default, Context); memset(&ctx->atlas, 0, sizeof(Atlas)); ctx->taskScheduler = XA_NEW(internal::MemTag::Default, internal::TaskScheduler); return &ctx->atlas; } -static void DestroyOutputMeshes(Context *ctx) -{ +static void DestroyOutputMeshes(Context *ctx) { if (!ctx->atlas.meshes) return; for (int i = 0; i < (int)ctx->atlas.meshCount; i++) { @@ -9224,8 +8520,7 @@ static void DestroyOutputMeshes(Context *ctx) ctx->atlas.meshes = nullptr; } -void Destroy(Atlas *atlas) -{ +void Destroy(Atlas *atlas) { XA_DEBUG_ASSERT(atlas); Context *ctx = (Context *)atlas; if (atlas->utilization) @@ -9244,6 +8539,13 @@ void Destroy(Atlas *atlas) mesh->~Mesh(); XA_FREE(mesh); } + for (uint32_t i = 0; i < ctx->meshPolygonMappings.size(); i++) { + MeshPolygonMapping *mapping = ctx->meshPolygonMappings[i]; + if (mapping) { + mapping->~MeshPolygonMapping(); + XA_FREE(mapping); + } + } for (uint32_t i = 0; i < ctx->uvMeshes.size(); i++) { internal::UvMesh *mesh = ctx->uvMeshes[i]; for (uint32_t j = 0; j < mesh->charts.size(); j++) { @@ -9265,66 +8567,52 @@ void Destroy(Atlas *atlas) #endif } -struct AddMeshTaskArgs -{ - Context *ctx; - internal::Mesh *mesh; -}; - -static void runAddMeshTask(void *userData) -{ +static void runAddMeshTask(void *groupUserData, void *taskUserData) { XA_PROFILE_START(addMeshThread) - auto args = (AddMeshTaskArgs *)userData; // Responsible for freeing this. - internal::Mesh *mesh = args->mesh; - internal::Progress *progress = args->ctx->addMeshProgress; - if (progress->cancel) - goto cleanup; - { - XA_PROFILE_START(addMeshCreateColocals) - mesh->createColocals(); - XA_PROFILE_END(addMeshCreateColocals) + auto ctx = (Context *)groupUserData; + auto mesh = (internal::Mesh *)taskUserData; + internal::Progress *progress = ctx->addMeshProgress; + if (progress->cancel) { + XA_PROFILE_END(addMeshThread) + return; } - if (progress->cancel) - goto cleanup; - progress->value++; - progress->update(); -cleanup: - args->~AddMeshTaskArgs(); - XA_FREE(args); + XA_PROFILE_START(addMeshCreateColocals) + mesh->createColocals(); + XA_PROFILE_END(addMeshCreateColocals) + if (progress->cancel) { + XA_PROFILE_END(addMeshThread) + return; + } + progress->increment(1); XA_PROFILE_END(addMeshThread) } -static internal::Vector3 DecodePosition(const MeshDecl &meshDecl, uint32_t index) -{ +static internal::Vector3 DecodePosition(const MeshDecl &meshDecl, uint32_t index) { XA_DEBUG_ASSERT(meshDecl.vertexPositionData); XA_DEBUG_ASSERT(meshDecl.vertexPositionStride > 0); return *((const internal::Vector3 *)&((const uint8_t *)meshDecl.vertexPositionData)[meshDecl.vertexPositionStride * index]); } -static internal::Vector3 DecodeNormal(const MeshDecl &meshDecl, uint32_t index) -{ +static internal::Vector3 DecodeNormal(const MeshDecl &meshDecl, uint32_t index) { XA_DEBUG_ASSERT(meshDecl.vertexNormalData); XA_DEBUG_ASSERT(meshDecl.vertexNormalStride > 0); return *((const internal::Vector3 *)&((const uint8_t *)meshDecl.vertexNormalData)[meshDecl.vertexNormalStride * index]); } -static internal::Vector2 DecodeUv(const MeshDecl &meshDecl, uint32_t index) -{ +static internal::Vector2 DecodeUv(const MeshDecl &meshDecl, uint32_t index) { XA_DEBUG_ASSERT(meshDecl.vertexUvData); XA_DEBUG_ASSERT(meshDecl.vertexUvStride > 0); return *((const internal::Vector2 *)&((const uint8_t *)meshDecl.vertexUvData)[meshDecl.vertexUvStride * index]); } -static uint32_t DecodeIndex(IndexFormat::Enum format, const void *indexData, int32_t offset, uint32_t i) -{ +static uint32_t DecodeIndex(IndexFormat format, const void *indexData, int32_t offset, uint32_t i) { XA_DEBUG_ASSERT(indexData); if (format == IndexFormat::UInt16) return uint16_t((int32_t)((const uint16_t *)indexData)[i] + offset); return uint32_t((int32_t)((const uint32_t *)indexData)[i] + offset); } -AddMeshError::Enum AddMesh(Atlas *atlas, const MeshDecl &meshDecl, uint32_t meshCountHint) -{ +AddMeshError AddMesh(Atlas *atlas, const MeshDecl &meshDecl, uint32_t meshCountHint) { XA_DEBUG_ASSERT(atlas); if (!atlas) { XA_PRINT_WARNING("AddMesh: atlas is null.\n"); @@ -9337,33 +8625,36 @@ AddMeshError::Enum AddMesh(Atlas *atlas, const MeshDecl &meshDecl, uint32_t mesh } #if XA_PROFILE if (ctx->meshes.isEmpty()) - internal::s_profile.addMeshReal = clock(); + internal::s_profile.addMeshRealStart = std::chrono::high_resolution_clock::now(); #endif // Don't know how many times AddMesh will be called, so progress needs to adjusted each time. if (!ctx->addMeshProgress) { ctx->addMeshProgress = XA_NEW_ARGS(internal::MemTag::Default, internal::Progress, ProgressCategory::AddMesh, ctx->progressFunc, ctx->progressUserData, 1); - } - else { + } else { ctx->addMeshProgress->setMaxValue(internal::max(ctx->meshes.size() + 1, meshCountHint)); } XA_PROFILE_START(addMeshCopyData) const bool hasIndices = meshDecl.indexCount > 0; const uint32_t indexCount = hasIndices ? meshDecl.indexCount : meshDecl.vertexCount; - XA_PRINT("Adding mesh %d: %u vertices, %u triangles\n", ctx->meshes.size(), meshDecl.vertexCount, indexCount / 3); - // Expecting triangle faces. - if ((indexCount % 3) != 0) - return AddMeshError::InvalidIndexCount; - if (hasIndices) { - // Check if any index is out of range. - for (uint32_t i = 0; i < indexCount; i++) { - const uint32_t index = DecodeIndex(meshDecl.indexFormat, meshDecl.indexData, meshDecl.indexOffset, i); - if (index >= meshDecl.vertexCount) - return AddMeshError::IndexOutOfRange; + uint32_t faceCount = indexCount / 3; + if (meshDecl.faceVertexCount) { + faceCount = meshDecl.faceCount; + XA_PRINT("Adding mesh %d: %u vertices, %u polygons\n", ctx->meshes.size(), meshDecl.vertexCount, faceCount); + for (uint32_t f = 0; f < faceCount; f++) { + if (meshDecl.faceVertexCount[f] < 3) + return AddMeshError::InvalidFaceVertexCount; } + } else { + XA_PRINT("Adding mesh %d: %u vertices, %u triangles\n", ctx->meshes.size(), meshDecl.vertexCount, faceCount); + // Expecting triangle faces unless otherwise specified. + if ((indexCount % 3) != 0) + return AddMeshError::InvalidIndexCount; } uint32_t meshFlags = internal::MeshFlags::HasIgnoredFaces; if (meshDecl.vertexNormalData) meshFlags |= internal::MeshFlags::HasNormals; + if (meshDecl.faceMaterialData) + meshFlags |= internal::MeshFlags::HasMaterials; internal::Mesh *mesh = XA_NEW_ARGS(internal::MemTag::Mesh, internal::Mesh, meshDecl.epsilon, meshDecl.vertexCount, indexCount / 3, meshFlags, ctx->meshes.size()); for (uint32_t i = 0; i < meshDecl.vertexCount; i++) { internal::Vector3 normal(0.0f); @@ -9374,17 +8665,42 @@ AddMeshError::Enum AddMesh(Atlas *atlas, const MeshDecl &meshDecl, uint32_t mesh texcoord = DecodeUv(meshDecl, i); mesh->addVertex(DecodePosition(meshDecl, i), normal, texcoord); } + MeshPolygonMapping *meshPolygonMapping = nullptr; + if (meshDecl.faceVertexCount) { + meshPolygonMapping = XA_NEW(internal::MemTag::Default, MeshPolygonMapping); + // Copy MeshDecl::faceVertexCount so it can be used later when building output meshes. + meshPolygonMapping->faceVertexCount.copyFrom(meshDecl.faceVertexCount, meshDecl.faceCount); + // There should be at least as many triangles as polygons. + meshPolygonMapping->triangleToPolygonMap.reserve(meshDecl.faceCount); + meshPolygonMapping->triangleToPolygonIndicesMap.reserve(meshDecl.indexCount); + } const uint32_t kMaxWarnings = 50; uint32_t warningCount = 0; - for (uint32_t i = 0; i < indexCount / 3; i++) { - uint32_t tri[3]; - for (int j = 0; j < 3; j++) - tri[j] = hasIndices ? DecodeIndex(meshDecl.indexFormat, meshDecl.indexData, meshDecl.indexOffset, i * 3 + j) : i * 3 + j; + internal::Array<uint32_t> triIndices; + uint32_t firstFaceIndex = 0; + internal::Triangulator triangulator; + for (uint32_t face = 0; face < faceCount; face++) { + // Decode face indices. + const uint32_t faceVertexCount = meshDecl.faceVertexCount ? (uint32_t)meshDecl.faceVertexCount[face] : 3; + uint32_t polygon[UINT8_MAX]; + for (uint32_t i = 0; i < faceVertexCount; i++) { + if (hasIndices) { + polygon[i] = DecodeIndex(meshDecl.indexFormat, meshDecl.indexData, meshDecl.indexOffset, face * faceVertexCount + i); + // Check if any index is out of range. + if (polygon[i] >= meshDecl.vertexCount) { + mesh->~Mesh(); + XA_FREE(mesh); + return AddMeshError::IndexOutOfRange; + } + } else { + polygon[i] = face * faceVertexCount + i; + } + } + // Ignore faces with degenerate or zero length edges. bool ignore = false; - // Check for degenerate or zero length edges. - for (int j = 0; j < 3; j++) { - const uint32_t index1 = tri[j]; - const uint32_t index2 = tri[(j + 1) % 3]; + for (uint32_t i = 0; i < faceVertexCount; i++) { + const uint32_t index1 = polygon[i]; + const uint32_t index2 = polygon[(i + 1) % 3]; if (index1 == index2) { ignore = true; if (++warningCount <= kMaxWarnings) @@ -9402,119 +8718,136 @@ AddMeshError::Enum AddMesh(Atlas *atlas, const MeshDecl &meshDecl, uint32_t mesh } // Ignore faces with any nan vertex attributes. if (!ignore) { - for (int j = 0; j < 3; j++) { - const internal::Vector3 &pos = mesh->position(tri[j]); + for (uint32_t i = 0; i < faceVertexCount; i++) { + const internal::Vector3 &pos = mesh->position(polygon[i]); if (internal::isNan(pos.x) || internal::isNan(pos.y) || internal::isNan(pos.z)) { if (++warningCount <= kMaxWarnings) - XA_PRINT(" NAN position in face: %d\n", i); + XA_PRINT(" NAN position in face: %d\n", face); ignore = true; break; } if (meshDecl.vertexNormalData) { - const internal::Vector3 &normal = mesh->normal(tri[j]); + const internal::Vector3 &normal = mesh->normal(polygon[i]); if (internal::isNan(normal.x) || internal::isNan(normal.y) || internal::isNan(normal.z)) { if (++warningCount <= kMaxWarnings) - XA_PRINT(" NAN normal in face: %d\n", i); + XA_PRINT(" NAN normal in face: %d\n", face); ignore = true; break; } } if (meshDecl.vertexUvData) { - const internal::Vector2 &uv = mesh->texcoord(tri[j]); + const internal::Vector2 &uv = mesh->texcoord(polygon[i]); if (internal::isNan(uv.x) || internal::isNan(uv.y)) { if (++warningCount <= kMaxWarnings) - XA_PRINT(" NAN texture coordinate in face: %d\n", i); + XA_PRINT(" NAN texture coordinate in face: %d\n", face); ignore = true; break; } } } } - const internal::Vector3 &a = mesh->position(tri[0]); - const internal::Vector3 &b = mesh->position(tri[1]); - const internal::Vector3 &c = mesh->position(tri[2]); - // Check for zero area faces. - float area = 0.0f; - if (!ignore) { - area = internal::length(internal::cross(b - a, c - a)) * 0.5f; - if (area <= internal::kAreaEpsilon) { - ignore = true; - if (++warningCount <= kMaxWarnings) - XA_PRINT(" Zero area face: %d, indices (%d %d %d), area is %f\n", i, tri[0], tri[1], tri[2], area); - } + // Triangulate if necessary. + triIndices.clear(); + if (faceVertexCount == 3) { + triIndices.push_back(polygon[0]); + triIndices.push_back(polygon[1]); + triIndices.push_back(polygon[2]); + } else { + triangulator.triangulatePolygon(mesh->positions(), internal::ConstArrayView<uint32_t>(polygon, faceVertexCount), triIndices); } + // Check for zero area faces. if (!ignore) { - if (internal::equal(a, b, meshDecl.epsilon) || internal::equal(a, c, meshDecl.epsilon) || internal::equal(b, c, meshDecl.epsilon)) { - ignore = true; - if (++warningCount <= kMaxWarnings) - XA_PRINT(" Degenerate face: %d, area is %f\n", i, area); + for (uint32_t i = 0; i < triIndices.size(); i += 3) { + const internal::Vector3 &a = mesh->position(triIndices[i + 0]); + const internal::Vector3 &b = mesh->position(triIndices[i + 1]); + const internal::Vector3 &c = mesh->position(triIndices[i + 2]); + const float area = internal::length(internal::cross(b - a, c - a)) * 0.5f; + if (area <= internal::kAreaEpsilon) { + ignore = true; + if (++warningCount <= kMaxWarnings) + XA_PRINT(" Zero area face: %d, area is %f\n", face, area); + break; + } } } - if (meshDecl.faceIgnoreData && meshDecl.faceIgnoreData[i]) + // User face ignore. + if (meshDecl.faceIgnoreData && meshDecl.faceIgnoreData[face]) ignore = true; - mesh->addFace(tri[0], tri[1], tri[2], ignore); + // User material. + uint32_t material = UINT32_MAX; + if (meshDecl.faceMaterialData) + material = meshDecl.faceMaterialData[face]; + // Add the face(s). + for (uint32_t i = 0; i < triIndices.size(); i += 3) { + mesh->addFace(&triIndices[i], ignore, material); + if (meshPolygonMapping) + meshPolygonMapping->triangleToPolygonMap.push_back(face); + } + if (meshPolygonMapping) { + for (uint32_t i = 0; i < triIndices.size(); i++) + meshPolygonMapping->triangleToPolygonIndicesMap.push_back(triIndices[i]); + } + firstFaceIndex += faceVertexCount; } if (warningCount > kMaxWarnings) XA_PRINT(" %u additional warnings truncated\n", warningCount - kMaxWarnings); XA_PROFILE_END(addMeshCopyData) ctx->meshes.push_back(mesh); + ctx->meshPolygonMappings.push_back(meshPolygonMapping); ctx->paramAtlas.addMesh(mesh); if (ctx->addMeshTaskGroup.value == UINT32_MAX) - ctx->addMeshTaskGroup = ctx->taskScheduler->createTaskGroup(); - AddMeshTaskArgs *taskArgs = XA_NEW(internal::MemTag::Default, AddMeshTaskArgs); // The task frees this. - taskArgs->ctx = ctx; - taskArgs->mesh = mesh; + ctx->addMeshTaskGroup = ctx->taskScheduler->createTaskGroup(ctx); internal::Task task; - task.userData = taskArgs; + task.userData = mesh; task.func = runAddMeshTask; ctx->taskScheduler->run(ctx->addMeshTaskGroup, task); return AddMeshError::Success; } -void AddMeshJoin(Atlas *atlas) -{ +void AddMeshJoin(Atlas *atlas) { XA_DEBUG_ASSERT(atlas); if (!atlas) { XA_PRINT_WARNING("AddMeshJoin: atlas is null.\n"); return; } Context *ctx = (Context *)atlas; - if (!ctx->addMeshProgress) - return; - ctx->taskScheduler->wait(&ctx->addMeshTaskGroup); - ctx->addMeshProgress->~Progress(); - XA_FREE(ctx->addMeshProgress); - ctx->addMeshProgress = nullptr; + if (!ctx->uvMeshes.isEmpty()) { #if XA_PROFILE - XA_PRINT("Added %u meshes\n", ctx->meshes.size()); - internal::s_profile.addMeshReal = clock() - internal::s_profile.addMeshReal; + XA_PRINT("Added %u UV meshes\n", ctx->uvMeshes.size()); + internal::s_profile.addMeshReal = uint64_t(std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - internal::s_profile.addMeshRealStart).count()); #endif - XA_PROFILE_PRINT_AND_RESET(" Total (real): ", addMeshReal) - XA_PROFILE_PRINT_AND_RESET(" Copy data: ", addMeshCopyData) - XA_PROFILE_PRINT_AND_RESET(" Total (thread): ", addMeshThread) - XA_PROFILE_PRINT_AND_RESET(" Create colocals: ", addMeshCreateColocals) + XA_PROFILE_PRINT_AND_RESET(" Total: ", addMeshReal) + XA_PROFILE_PRINT_AND_RESET(" Copy data: ", addMeshCopyData) #if XA_PROFILE_ALLOC - XA_PROFILE_PRINT_AND_RESET(" Alloc: ", alloc) + XA_PROFILE_PRINT_AND_RESET(" Alloc: ", alloc) #endif - XA_PRINT_MEM_USAGE + XA_PRINT_MEM_USAGE + } else { + if (!ctx->addMeshProgress) + return; + ctx->taskScheduler->wait(&ctx->addMeshTaskGroup); + ctx->addMeshProgress->~Progress(); + XA_FREE(ctx->addMeshProgress); + ctx->addMeshProgress = nullptr; +#if XA_PROFILE + XA_PRINT("Added %u meshes\n", ctx->meshes.size()); + internal::s_profile.addMeshReal = uint64_t(std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - internal::s_profile.addMeshRealStart).count()); +#endif + XA_PROFILE_PRINT_AND_RESET(" Total (real): ", addMeshReal) + XA_PROFILE_PRINT_AND_RESET(" Copy data: ", addMeshCopyData) + XA_PROFILE_PRINT_AND_RESET(" Total (thread): ", addMeshThread) + XA_PROFILE_PRINT_AND_RESET(" Create colocals: ", addMeshCreateColocals) +#if XA_PROFILE_ALLOC + XA_PROFILE_PRINT_AND_RESET(" Alloc: ", alloc) +#endif + XA_PRINT_MEM_USAGE #if XA_DEBUG_EXPORT_OBJ_FACE_GROUPS - internal::param::s_faceGroupsCurrentVertex = 0; + internal::param::s_faceGroupsCurrentVertex = 0; #endif + } } -struct EdgeKey -{ - EdgeKey() {} - EdgeKey(const EdgeKey &k) : v0(k.v0), v1(k.v1) {} - EdgeKey(uint32_t v0, uint32_t v1) : v0(v0), v1(v1) {} - bool operator==(const EdgeKey &k) const { return v0 == k.v0 && v1 == k.v1; } - - uint32_t v0; - uint32_t v1; -}; - -AddMeshError::Enum AddUvMesh(Atlas *atlas, const UvMeshDecl &decl) -{ +AddMeshError AddUvMesh(Atlas *atlas, const UvMeshDecl &decl) { XA_DEBUG_ASSERT(atlas); if (!atlas) { XA_PRINT_WARNING("AddUvMesh: atlas is null.\n"); @@ -9525,13 +8858,18 @@ AddMeshError::Enum AddUvMesh(Atlas *atlas, const UvMeshDecl &decl) XA_PRINT_WARNING("AddUvMesh: Meshes and UV meshes cannot be added to the same atlas.\n"); return AddMeshError::Error; } - const bool decoded = (decl.indexCount <= 0); - const uint32_t indexCount = decoded ? decl.vertexCount : decl.indexCount; +#if XA_PROFILE + if (ctx->uvMeshInstances.isEmpty()) + internal::s_profile.addMeshRealStart = std::chrono::high_resolution_clock::now(); +#endif + XA_PROFILE_START(addMeshCopyData) + const bool hasIndices = decl.indexCount > 0; + const uint32_t indexCount = hasIndices ? decl.indexCount : decl.vertexCount; XA_PRINT("Adding UV mesh %d: %u vertices, %u triangles\n", ctx->uvMeshes.size(), decl.vertexCount, indexCount / 3); // Expecting triangle faces. if ((indexCount % 3) != 0) return AddMeshError::InvalidIndexCount; - if (!decoded) { + if (hasIndices) { // Check if any index is out of range. for (uint32_t i = 0; i < indexCount; i++) { const uint32_t index = DecodeIndex(decl.indexFormat, decl.indexData, decl.indexOffset, i); @@ -9539,319 +8877,266 @@ AddMeshError::Enum AddUvMesh(Atlas *atlas, const UvMeshDecl &decl) return AddMeshError::IndexOutOfRange; } } + // Create a mesh instance. internal::UvMeshInstance *meshInstance = XA_NEW(internal::MemTag::Default, internal::UvMeshInstance); - meshInstance->texcoords.resize(decl.vertexCount); - for (uint32_t i = 0; i < decl.vertexCount; i++) { - internal::Vector2 texcoord = *((const internal::Vector2 *)&((const uint8_t *)decl.vertexUvData)[decl.vertexStride * i]); - // Set nan values to 0. - if (internal::isNan(texcoord.x) || internal::isNan(texcoord.y)) - texcoord.x = texcoord.y = 0.0f; - meshInstance->texcoords[i] = texcoord; - } - meshInstance->rotateCharts = decl.rotateCharts; + meshInstance->mesh = nullptr; + ctx->uvMeshInstances.push_back(meshInstance); // See if this is an instance of an already existing mesh. internal::UvMesh *mesh = nullptr; for (uint32_t m = 0; m < ctx->uvMeshes.size(); m++) { if (memcmp(&ctx->uvMeshes[m]->decl, &decl, sizeof(UvMeshDecl)) == 0) { - meshInstance->mesh = mesh = ctx->uvMeshes[m]; + mesh = ctx->uvMeshes[m]; + XA_PRINT(" instance of a previous UV mesh\n"); break; } } if (!mesh) { // Copy geometry to mesh. - meshInstance->mesh = mesh = XA_NEW(internal::MemTag::Default, internal::UvMesh); + mesh = XA_NEW(internal::MemTag::Default, internal::UvMesh); + ctx->uvMeshes.push_back(mesh); mesh->decl = decl; + if (decl.faceMaterialData) { + mesh->faceMaterials.resize(decl.indexCount / 3); + memcpy(mesh->faceMaterials.data(), decl.faceMaterialData, mesh->faceMaterials.size() * sizeof(uint32_t)); + } mesh->indices.resize(decl.indexCount); for (uint32_t i = 0; i < indexCount; i++) - mesh->indices[i] = decoded ? i : DecodeIndex(decl.indexFormat, decl.indexData, decl.indexOffset, i); - mesh->vertexToChartMap.resize(decl.vertexCount); - for (uint32_t i = 0; i < mesh->vertexToChartMap.size(); i++) - mesh->vertexToChartMap[i] = UINT32_MAX; - // Calculate charts (incident faces). - internal::HashMap<internal::Vector2> vertexToFaceMap(internal::MemTag::Default, indexCount); // Face is index / 3 - const uint32_t faceCount = indexCount / 3; - for (uint32_t i = 0; i < indexCount; i++) - vertexToFaceMap.add(meshInstance->texcoords[mesh->indices[i]]); - internal::BitArray faceAssigned(faceCount); - faceAssigned.zeroOutMemory(); - for (uint32_t f = 0; f < faceCount; f++) { - if (faceAssigned.get(f)) - continue; - // Found an unassigned face, create a new chart. - internal::UvMeshChart *chart = XA_NEW(internal::MemTag::Default, internal::UvMeshChart); - chart->material = decl.faceMaterialData ? decl.faceMaterialData[f] : 0; - // Walk incident faces and assign them to the chart. - faceAssigned.set(f); - chart->faces.push_back(f); - for (;;) { - bool newFaceAssigned = false; - const uint32_t faceCount2 = chart->faces.size(); - for (uint32_t f2 = 0; f2 < faceCount2; f2++) { - const uint32_t face = chart->faces[f2]; - for (uint32_t i = 0; i < 3; i++) { - const internal::Vector2 &texcoord = meshInstance->texcoords[meshInstance->mesh->indices[face * 3 + i]]; - uint32_t mapIndex = vertexToFaceMap.get(texcoord); - while (mapIndex != UINT32_MAX) { - const uint32_t face2 = mapIndex / 3; // 3 vertices added per face. - // Materials must match. - if (!faceAssigned.get(face2) && (!decl.faceMaterialData || decl.faceMaterialData[face] == decl.faceMaterialData[face2])) { - faceAssigned.set(face2); - chart->faces.push_back(face2); - newFaceAssigned = true; - } - mapIndex = vertexToFaceMap.getNext(mapIndex); - } - } - } - if (!newFaceAssigned) + mesh->indices[i] = hasIndices ? DecodeIndex(decl.indexFormat, decl.indexData, decl.indexOffset, i) : i; + mesh->texcoords.resize(decl.vertexCount); + for (uint32_t i = 0; i < decl.vertexCount; i++) + mesh->texcoords[i] = *((const internal::Vector2 *)&((const uint8_t *)decl.vertexUvData)[decl.vertexStride * i]); + // Validate. + mesh->faceIgnore.resize(decl.indexCount / 3); + mesh->faceIgnore.zeroOutMemory(); + const uint32_t kMaxWarnings = 50; + uint32_t warningCount = 0; + for (uint32_t f = 0; f < indexCount / 3; f++) { + bool ignore = false; + uint32_t tri[3]; + for (uint32_t i = 0; i < 3; i++) + tri[i] = mesh->indices[f * 3 + i]; + // Check for nan UVs. + for (uint32_t i = 0; i < 3; i++) { + const uint32_t vertex = tri[i]; + if (internal::isNan(mesh->texcoords[vertex].x) || internal::isNan(mesh->texcoords[vertex].y)) { + ignore = true; + if (++warningCount <= kMaxWarnings) + XA_PRINT(" NAN texture coordinate in vertex %u\n", vertex); break; + } } - for (uint32_t i = 0; i < chart->faces.size(); i++) { - for (uint32_t j = 0; j < 3; j++) { - const uint32_t vertex = meshInstance->mesh->indices[chart->faces[i] * 3 + j]; - chart->indices.push_back(vertex); - mesh->vertexToChartMap[vertex] = mesh->charts.size(); + // Check for zero area faces. + if (!ignore) { + const internal::Vector2 &v1 = mesh->texcoords[tri[0]]; + const internal::Vector2 &v2 = mesh->texcoords[tri[1]]; + const internal::Vector2 &v3 = mesh->texcoords[tri[2]]; + const float area = fabsf(((v2.x - v1.x) * (v3.y - v1.y) - (v3.x - v1.x) * (v2.y - v1.y)) * 0.5f); + if (area <= internal::kAreaEpsilon) { + ignore = true; + if (++warningCount <= kMaxWarnings) + XA_PRINT(" Zero area face: %d, indices (%d %d %d), area is %f\n", f, tri[0], tri[1], tri[2], area); } } - mesh->charts.push_back(chart); + if (ignore) + mesh->faceIgnore.set(f); } - ctx->uvMeshes.push_back(mesh); - } else { - XA_PRINT(" instance of a previous UV mesh\n"); + if (warningCount > kMaxWarnings) + XA_PRINT(" %u additional warnings truncated\n", warningCount - kMaxWarnings); } - XA_PRINT(" %u charts\n", meshInstance->mesh->charts.size()); - ctx->uvMeshInstances.push_back(meshInstance); + meshInstance->mesh = mesh; + XA_PROFILE_END(addMeshCopyData) return AddMeshError::Success; } -void ComputeCharts(Atlas *atlas, ChartOptions options) -{ +void ComputeCharts(Atlas *atlas, ChartOptions options) { if (!atlas) { XA_PRINT_WARNING("ComputeCharts: atlas is null.\n"); return; } Context *ctx = (Context *)atlas; - if (!ctx->uvMeshInstances.isEmpty()) { - XA_PRINT_WARNING("ComputeCharts: This function should not be called with UV meshes.\n"); - return; - } AddMeshJoin(atlas); - if (ctx->meshes.isEmpty()) { - XA_PRINT_WARNING("ComputeCharts: No meshes. Call AddMesh first.\n"); - return; - } - XA_PRINT("Computing charts\n"); - XA_PROFILE_START(computeChartsReal) - if (!ctx->paramAtlas.computeCharts(ctx->taskScheduler, options, ctx->progressFunc, ctx->progressUserData)) { - XA_PRINT(" Cancelled by user\n"); - return; - } - XA_PROFILE_END(computeChartsReal) - // Count charts. - uint32_t chartCount = 0; - const uint32_t meshCount = ctx->meshes.size(); - for (uint32_t i = 0; i < meshCount; i++) { - for (uint32_t j = 0; j < ctx->paramAtlas.chartGroupCount(i); j++) { - const internal::param::ChartGroup *chartGroup = ctx->paramAtlas.chartGroupAt(i, j); - chartCount += chartGroup->segmentChartCount(); - } - } - XA_PRINT(" %u charts\n", chartCount); -#if XA_PROFILE - XA_PRINT(" Chart groups\n"); - uint32_t chartGroupCount = 0; - for (uint32_t i = 0; i < meshCount; i++) { - XA_PRINT(" Mesh %u: %u chart groups\n", i, ctx->paramAtlas.chartGroupCount(i)); - chartGroupCount += ctx->paramAtlas.chartGroupCount(i); - } - XA_PRINT(" %u total\n", chartGroupCount); -#endif - XA_PROFILE_PRINT_AND_RESET(" Total (real): ", computeChartsReal) - XA_PROFILE_PRINT_AND_RESET(" Total (thread): ", computeChartsThread) - XA_PROFILE_PRINT_AND_RESET(" Create face groups: ", createFaceGroups) - XA_PROFILE_PRINT_AND_RESET(" Extract invalid mesh geometry: ", extractInvalidMeshGeometry) - XA_PROFILE_PRINT_AND_RESET(" Chart group compute charts (real): ", chartGroupComputeChartsReal) - XA_PROFILE_PRINT_AND_RESET(" Chart group compute charts (thread): ", chartGroupComputeChartsThread) - XA_PROFILE_PRINT_AND_RESET(" Create chart group mesh: ", createChartGroupMesh) - XA_PROFILE_PRINT_AND_RESET(" Create colocals: ", createChartGroupMeshColocals) - XA_PROFILE_PRINT_AND_RESET(" Create boundaries: ", createChartGroupMeshBoundaries) - XA_PROFILE_PRINT_AND_RESET(" Build atlas: ", buildAtlas) - XA_PROFILE_PRINT_AND_RESET(" Init: ", buildAtlasInit) - XA_PROFILE_PRINT_AND_RESET(" Planar charts: ", planarCharts) - XA_PROFILE_PRINT_AND_RESET(" Clustered charts: ", clusteredCharts) - XA_PROFILE_PRINT_AND_RESET(" Place seeds: ", clusteredChartsPlaceSeeds) - XA_PROFILE_PRINT_AND_RESET(" Boundary intersection: ", clusteredChartsPlaceSeedsBoundaryIntersection) - XA_PROFILE_PRINT_AND_RESET(" Relocate seeds: ", clusteredChartsRelocateSeeds) - XA_PROFILE_PRINT_AND_RESET(" Reset: ", clusteredChartsReset) - XA_PROFILE_PRINT_AND_RESET(" Grow: ", clusteredChartsGrow) - XA_PROFILE_PRINT_AND_RESET(" Boundary intersection: ", clusteredChartsGrowBoundaryIntersection) - XA_PROFILE_PRINT_AND_RESET(" Merge: ", clusteredChartsMerge) - XA_PROFILE_PRINT_AND_RESET(" Fill holes: ", clusteredChartsFillHoles) - XA_PROFILE_PRINT_AND_RESET(" Copy chart faces: ", copyChartFaces) -#if XA_PROFILE_ALLOC - XA_PROFILE_PRINT_AND_RESET(" Alloc: ", alloc) -#endif - XA_PRINT_MEM_USAGE -} - -void ParameterizeCharts(Atlas *atlas, ParameterizeOptions options) -{ - if (!atlas) { - XA_PRINT_WARNING("ParameterizeCharts: atlas is null.\n"); - return; - } - Context *ctx = (Context *)atlas; - if (!ctx->uvMeshInstances.isEmpty()) { - XA_PRINT_WARNING("ParameterizeCharts: This function should not be called with UV meshes.\n"); - return; - } - if (!ctx->paramAtlas.chartsComputed()) { - XA_PRINT_WARNING("ParameterizeCharts: ComputeCharts must be called first.\n"); + if (ctx->meshes.isEmpty() && ctx->uvMeshInstances.isEmpty()) { + XA_PRINT_WARNING("ComputeCharts: No meshes. Call AddMesh or AddUvMesh first.\n"); return; } - atlas->atlasCount = 0; - atlas->height = 0; - atlas->texelsPerUnit = 0; - atlas->width = 0; - if (atlas->utilization) { + // Reset atlas state. This function may be called multiple times, or again after PackCharts. + if (atlas->utilization) XA_FREE(atlas->utilization); - atlas->utilization = nullptr; - } - if (atlas->image) { + if (atlas->image) XA_FREE(atlas->image); - atlas->image = nullptr; - } DestroyOutputMeshes(ctx); - XA_PRINT("Parameterizing charts\n"); - XA_PROFILE_START(parameterizeChartsReal) - if (!ctx->paramAtlas.parameterizeCharts(ctx->taskScheduler, options, ctx->progressFunc, ctx->progressUserData)) { - XA_PRINT(" Cancelled by user\n"); + memset(&ctx->atlas, 0, sizeof(Atlas)); + XA_PRINT("Computing charts\n"); + if (!ctx->meshes.isEmpty()) { + if (!ctx->paramAtlas.computeCharts(ctx->taskScheduler, options, ctx->progressFunc, ctx->progressUserData)) { + XA_PRINT(" Cancelled by user\n"); return; - } - XA_PROFILE_END(parameterizeChartsReal) - const uint32_t meshCount = ctx->meshes.size(); - uint32_t chartCount = 0, chartsWithHolesCount = 0, holesCount = 0, chartsWithTJunctionsCount = 0, tJunctionsCount = 0, orthoChartsCount = 0, planarChartsCount = 0, lscmChartsCount = 0, piecewiseChartsCount = 0, chartsAddedCount = 0, chartsDeletedCount = 0; - for (uint32_t i = 0; i < meshCount; i++) { - for (uint32_t j = 0; j < ctx->paramAtlas.chartGroupCount(i); j++) { - const internal::param::ChartGroup *chartGroup = ctx->paramAtlas.chartGroupAt(i, j); - for (uint32_t k = 0; k < chartGroup->chartCount(); k++) { - const internal::param::Chart *chart = chartGroup->chartAt(k); -#if XA_PRINT_CHART_WARNINGS - if (chart->warningFlags() & internal::param::ChartWarningFlags::CloseHolesFailed) - XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u): failed to close holes\n", chartCount, i, j, k); - if (chart->warningFlags() & internal::param::ChartWarningFlags::FixTJunctionsDuplicatedEdge) - XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u): fixing t-junctions created non-manifold geometry\n", chartCount, i, j, k); - if (chart->warningFlags() & internal::param::ChartWarningFlags::FixTJunctionsFailed) - XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u): fixing t-junctions failed\n", chartCount, i, j, k); - if (chart->warningFlags() & internal::param::ChartWarningFlags::TriangulateDuplicatedEdge) - XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u): triangulation created non-manifold geometry\n", chartCount, i, j, k); -#endif - holesCount += chart->closedHolesCount(); - if (chart->closedHolesCount() > 0) - chartsWithHolesCount++; - tJunctionsCount += chart->fixedTJunctionsCount(); - if (chart->fixedTJunctionsCount() > 0) - chartsWithTJunctionsCount++; - if (chart->type() == ChartType::Planar) - planarChartsCount++; - else if (chart->type() == ChartType::Ortho) - orthoChartsCount++; - else if (chart->type() == ChartType::LSCM) - lscmChartsCount++; - else if (chart->type() == ChartType::Piecewise) - piecewiseChartsCount++; - } - chartCount += chartGroup->chartCount(); - chartsAddedCount += chartGroup->paramAddedChartsCount(); - chartsDeletedCount += chartGroup->paramDeletedChartsCount(); - } - } - if (holesCount > 0) - XA_PRINT(" %u holes closed in %u charts\n", holesCount, chartsWithHolesCount); - if (tJunctionsCount > 0) - XA_PRINT(" %u t-junctions fixed in %u charts\n", tJunctionsCount, chartsWithTJunctionsCount); - XA_PRINT(" %u planar charts, %u ortho charts, %u LSCM charts, %u piecewise charts\n", planarChartsCount, orthoChartsCount, lscmChartsCount, piecewiseChartsCount); - if (chartsDeletedCount > 0) { - XA_PRINT(" %u charts with invalid parameterizations replaced with %u new charts\n", chartsDeletedCount, chartsAddedCount); + } + uint32_t chartsWithTJunctionsCount = 0, tJunctionCount = 0, orthoChartsCount = 0, planarChartsCount = 0, lscmChartsCount = 0, piecewiseChartsCount = 0, originalUvChartsCount = 0; + uint32_t chartCount = 0; + const uint32_t meshCount = ctx->meshes.size(); + for (uint32_t i = 0; i < meshCount; i++) { + for (uint32_t j = 0; j < ctx->paramAtlas.chartGroupCount(i); j++) { + const internal::param::ChartGroup *chartGroup = ctx->paramAtlas.chartGroupAt(i, j); + for (uint32_t k = 0; k < chartGroup->chartCount(); k++) { + const internal::param::Chart *chart = chartGroup->chartAt(k); + tJunctionCount += chart->tjunctionCount(); + if (chart->tjunctionCount() > 0) + chartsWithTJunctionsCount++; + if (chart->type() == ChartType::Planar) + planarChartsCount++; + else if (chart->type() == ChartType::Ortho) + orthoChartsCount++; + else if (chart->type() == ChartType::LSCM) + lscmChartsCount++; + else if (chart->type() == ChartType::Piecewise) + piecewiseChartsCount++; + if (chart->generatorType() == internal::segment::ChartGeneratorType::OriginalUv) + originalUvChartsCount++; + } + chartCount += chartGroup->chartCount(); + } + } + if (tJunctionCount > 0) + XA_PRINT(" %u t-junctions found in %u charts\n", tJunctionCount, chartsWithTJunctionsCount); XA_PRINT(" %u charts\n", chartCount); - } - uint32_t chartIndex = 0, invalidParamCount = 0; - for (uint32_t i = 0; i < meshCount; i++) { - for (uint32_t j = 0; j < ctx->paramAtlas.chartGroupCount(i); j++) { - const internal::param::ChartGroup *chartGroup = ctx->paramAtlas.chartGroupAt(i, j); - for (uint32_t k = 0; k < chartGroup->chartCount(); k++) { - internal::param::Chart *chart = chartGroup->chartAt(k); - const internal::param::Quality &quality = chart->quality(); + XA_PRINT(" %u planar, %u ortho, %u LSCM, %u piecewise\n", planarChartsCount, orthoChartsCount, lscmChartsCount, piecewiseChartsCount); + if (originalUvChartsCount > 0) + XA_PRINT(" %u with original UVs\n", originalUvChartsCount); + uint32_t chartIndex = 0, invalidParamCount = 0; + for (uint32_t i = 0; i < meshCount; i++) { + for (uint32_t j = 0; j < ctx->paramAtlas.chartGroupCount(i); j++) { + const internal::param::ChartGroup *chartGroup = ctx->paramAtlas.chartGroupAt(i, j); + for (uint32_t k = 0; k < chartGroup->chartCount(); k++) { + internal::param::Chart *chart = chartGroup->chartAt(k); + const internal::param::Quality &quality = chart->quality(); #if XA_DEBUG_EXPORT_OBJ_CHARTS_AFTER_PARAMETERIZATION - { - char filename[256]; - XA_SPRINTF(filename, sizeof(filename), "debug_chart_%03u_after_parameterization.obj", chartIndex); - chart->unifiedMesh()->writeObjFile(filename); - } -#endif - const char *type = "LSCM"; - if (chart->type() == ChartType::Planar) - type = "planar"; - else if (chart->type() == ChartType::Ortho) - type = "ortho"; - else if (chart->type() == ChartType::Piecewise) - type = "piecewise"; - if (chart->isInvalid()) { - if (quality.boundaryIntersection) { - XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u) (%s): invalid parameterization, self-intersecting boundary.\n", chartIndex, i, j, k, type); - } - if (quality.flippedTriangleCount > 0) { - XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u) (%s): invalid parameterization, %u / %u flipped triangles.\n", chartIndex, i, j, k, type, quality.flippedTriangleCount, quality.totalTriangleCount); + { + char filename[256]; + XA_SPRINTF(filename, sizeof(filename), "debug_chart_%03u_after_parameterization.obj", chartIndex); + chart->unifiedMesh()->writeObjFile(filename); } - invalidParamCount++; +#endif + const char *type = "LSCM"; + if (chart->type() == ChartType::Planar) + type = "planar"; + else if (chart->type() == ChartType::Ortho) + type = "ortho"; + else if (chart->type() == ChartType::Piecewise) + type = "piecewise"; + if (chart->isInvalid()) { + if (quality.boundaryIntersection) { + XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u) (%s): invalid parameterization, self-intersecting boundary.\n", chartIndex, i, j, k, type); + } + if (quality.flippedTriangleCount > 0) { + XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u) (%s): invalid parameterization, %u / %u flipped triangles.\n", chartIndex, i, j, k, type, quality.flippedTriangleCount, quality.totalTriangleCount); + } + if (quality.zeroAreaTriangleCount > 0) { + XA_PRINT_WARNING(" Chart %u (mesh %u, group %u, id %u) (%s): invalid parameterization, %u / %u zero area triangles.\n", chartIndex, i, j, k, type, quality.zeroAreaTriangleCount, quality.totalTriangleCount); + } + invalidParamCount++; #if XA_DEBUG_EXPORT_OBJ_INVALID_PARAMETERIZATION - char filename[256]; - XA_SPRINTF(filename, sizeof(filename), "debug_chart_%03u_invalid_parameterization.obj", chartIndex); - const internal::Mesh *mesh = chart->unifiedMesh(); - FILE *file; - XA_FOPEN(file, filename, "w"); - if (file) { - mesh->writeObjVertices(file); - fprintf(file, "s off\n"); - fprintf(file, "o object\n"); - for (uint32_t f = 0; f < mesh->faceCount(); f++) - mesh->writeObjFace(file, f); - if (!chart->paramFlippedFaces().isEmpty()) { - fprintf(file, "o flipped_faces\n"); - for (uint32_t f = 0; f < chart->paramFlippedFaces().size(); f++) - mesh->writeObjFace(file, chart->paramFlippedFaces()[f]); + char filename[256]; + XA_SPRINTF(filename, sizeof(filename), "debug_chart_%03u_invalid_parameterization.obj", chartIndex); + const internal::Mesh *mesh = chart->unifiedMesh(); + FILE *file; + XA_FOPEN(file, filename, "w"); + if (file) { + mesh->writeObjVertices(file); + fprintf(file, "s off\n"); + fprintf(file, "o object\n"); + for (uint32_t f = 0; f < mesh->faceCount(); f++) + mesh->writeObjFace(file, f); + if (!chart->paramFlippedFaces().isEmpty()) { + fprintf(file, "o flipped_faces\n"); + for (uint32_t f = 0; f < chart->paramFlippedFaces().size(); f++) + mesh->writeObjFace(file, chart->paramFlippedFaces()[f]); + } + mesh->writeObjBoundaryEges(file); + fclose(file); } - mesh->writeObjBoundaryEges(file); - mesh->writeObjLinkedBoundaries(file); - fclose(file); - } #endif + } + chartIndex++; } - chartIndex++; } } + if (invalidParamCount > 0) + XA_PRINT_WARNING(" %u charts with invalid parameterizations\n", invalidParamCount); +#if XA_PROFILE + XA_PRINT(" Chart groups\n"); + uint32_t chartGroupCount = 0; + for (uint32_t i = 0; i < meshCount; i++) { +#if 0 + XA_PRINT(" Mesh %u: %u chart groups\n", i, ctx->paramAtlas.chartGroupCount(i)); +#endif + chartGroupCount += ctx->paramAtlas.chartGroupCount(i); + } + XA_PRINT(" %u total\n", chartGroupCount); +#endif + XA_PROFILE_PRINT_AND_RESET(" Compute charts total (real): ", computeChartsReal) + XA_PROFILE_PRINT_AND_RESET(" Compute charts total (thread): ", computeChartsThread) + XA_PROFILE_PRINT_AND_RESET(" Create face groups: ", createFaceGroups) + XA_PROFILE_PRINT_AND_RESET(" Extract invalid mesh geometry: ", extractInvalidMeshGeometry) + XA_PROFILE_PRINT_AND_RESET(" Chart group compute charts (real): ", chartGroupComputeChartsReal) + XA_PROFILE_PRINT_AND_RESET(" Chart group compute charts (thread): ", chartGroupComputeChartsThread) + XA_PROFILE_PRINT_AND_RESET(" Create chart group mesh: ", createChartGroupMesh) + XA_PROFILE_PRINT_AND_RESET(" Create colocals: ", createChartGroupMeshColocals) + XA_PROFILE_PRINT_AND_RESET(" Create boundaries: ", createChartGroupMeshBoundaries) + XA_PROFILE_PRINT_AND_RESET(" Build atlas: ", buildAtlas) + XA_PROFILE_PRINT_AND_RESET(" Init: ", buildAtlasInit) + XA_PROFILE_PRINT_AND_RESET(" Planar charts: ", planarCharts) + if (options.useInputMeshUvs) { + XA_PROFILE_PRINT_AND_RESET(" Original UV charts: ", originalUvCharts) + } + XA_PROFILE_PRINT_AND_RESET(" Clustered charts: ", clusteredCharts) + XA_PROFILE_PRINT_AND_RESET(" Place seeds: ", clusteredChartsPlaceSeeds) + XA_PROFILE_PRINT_AND_RESET(" Boundary intersection: ", clusteredChartsPlaceSeedsBoundaryIntersection) + XA_PROFILE_PRINT_AND_RESET(" Relocate seeds: ", clusteredChartsRelocateSeeds) + XA_PROFILE_PRINT_AND_RESET(" Reset: ", clusteredChartsReset) + XA_PROFILE_PRINT_AND_RESET(" Grow: ", clusteredChartsGrow) + XA_PROFILE_PRINT_AND_RESET(" Boundary intersection: ", clusteredChartsGrowBoundaryIntersection) + XA_PROFILE_PRINT_AND_RESET(" Merge: ", clusteredChartsMerge) + XA_PROFILE_PRINT_AND_RESET(" Fill holes: ", clusteredChartsFillHoles) + XA_PROFILE_PRINT_AND_RESET(" Copy chart faces: ", copyChartFaces) + XA_PROFILE_PRINT_AND_RESET(" Create chart mesh and parameterize (real): ", createChartMeshAndParameterizeReal) + XA_PROFILE_PRINT_AND_RESET(" Create chart mesh and parameterize (thread): ", createChartMeshAndParameterizeThread) + XA_PROFILE_PRINT_AND_RESET(" Create chart mesh: ", createChartMesh) + XA_PROFILE_PRINT_AND_RESET(" Parameterize charts: ", parameterizeCharts) + XA_PROFILE_PRINT_AND_RESET(" Orthogonal: ", parameterizeChartsOrthogonal) + XA_PROFILE_PRINT_AND_RESET(" LSCM: ", parameterizeChartsLSCM) + XA_PROFILE_PRINT_AND_RESET(" Recompute: ", parameterizeChartsRecompute) + XA_PROFILE_PRINT_AND_RESET(" Piecewise: ", parameterizeChartsPiecewise) + XA_PROFILE_PRINT_AND_RESET(" Boundary intersection: ", parameterizeChartsPiecewiseBoundaryIntersection) + XA_PROFILE_PRINT_AND_RESET(" Evaluate quality: ", parameterizeChartsEvaluateQuality) +#if XA_PROFILE_ALLOC + XA_PROFILE_PRINT_AND_RESET(" Alloc: ", alloc) +#endif + XA_PRINT_MEM_USAGE + } else { + XA_PROFILE_START(computeChartsReal) + if (!internal::segment::computeUvMeshCharts(ctx->taskScheduler, ctx->uvMeshes, ctx->progressFunc, ctx->progressUserData)) { + XA_PRINT(" Cancelled by user\n"); + return; + } + XA_PROFILE_END(computeChartsReal) + ctx->uvMeshChartsComputed = true; + // Count charts. + uint32_t chartCount = 0; + const uint32_t meshCount = ctx->uvMeshes.size(); + for (uint32_t i = 0; i < meshCount; i++) + chartCount += ctx->uvMeshes[i]->charts.size(); + XA_PRINT(" %u charts\n", chartCount); + XA_PROFILE_PRINT_AND_RESET(" Total (real): ", computeChartsReal) + XA_PROFILE_PRINT_AND_RESET(" Total (thread): ", computeChartsThread) } - if (invalidParamCount > 0) - XA_PRINT_WARNING(" %u charts with invalid parameterizations\n", invalidParamCount); - XA_PROFILE_PRINT_AND_RESET(" Total (real): ", parameterizeChartsReal) - XA_PROFILE_PRINT_AND_RESET(" Total (thread): ", parameterizeChartsThread) - XA_PROFILE_PRINT_AND_RESET(" Create chart mesh: ", createChartMesh) - XA_PROFILE_PRINT_AND_RESET(" Fix t-junctions: ", fixChartMeshTJunctions) - XA_PROFILE_PRINT_AND_RESET(" Close holes: ", closeChartMeshHoles) - XA_PROFILE_PRINT_AND_RESET(" Orthogonal: ", parameterizeChartsOrthogonal) - XA_PROFILE_PRINT_AND_RESET(" LSCM: ", parameterizeChartsLSCM) - XA_PROFILE_PRINT_AND_RESET(" Recompute: ", parameterizeChartsRecompute) - XA_PROFILE_PRINT_AND_RESET(" Piecewise: ", parameterizeChartsPiecewise) - XA_PROFILE_PRINT_AND_RESET(" Boundary intersection: ", parameterizeChartsPiecewiseBoundaryIntersection) - XA_PROFILE_PRINT_AND_RESET(" Evaluate quality: ", parameterizeChartsEvaluateQuality) #if XA_PROFILE_ALLOC XA_PROFILE_PRINT_AND_RESET(" Alloc: ", alloc) #endif XA_PRINT_MEM_USAGE } -void PackCharts(Atlas *atlas, PackOptions packOptions) -{ +void PackCharts(Atlas *atlas, PackOptions packOptions) { // Validate arguments and context state. if (!atlas) { XA_PRINT_WARNING("PackCharts: atlas is null.\n"); @@ -9867,10 +9152,9 @@ void PackCharts(Atlas *atlas, PackOptions packOptions) XA_PRINT_WARNING("PackCharts: ComputeCharts must be called first.\n"); return; } - if (!ctx->paramAtlas.chartsParameterized()) { - XA_PRINT_WARNING("PackCharts: ParameterizeCharts must be called first.\n"); - return; - } + } else if (!ctx->uvMeshChartsComputed) { + XA_PRINT_WARNING("PackCharts: ComputeCharts must be called first.\n"); + return; } if (packOptions.texelsPerUnit < 0.0f) { XA_PRINT_WARNING("PackCharts: PackOptions::texelsPerUnit is negative.\n"); @@ -9893,8 +9177,7 @@ void PackCharts(Atlas *atlas, PackOptions packOptions) if (!ctx->uvMeshInstances.isEmpty()) { for (uint32_t i = 0; i < ctx->uvMeshInstances.size(); i++) packAtlas.addUvMeshCharts(ctx->uvMeshInstances[i]); - } - else + } else packAtlas.addCharts(ctx->taskScheduler, &ctx->paramAtlas); XA_PROFILE_END(packChartsAddCharts) XA_PROFILE_START(packCharts) @@ -9946,16 +9229,35 @@ void PackCharts(Atlas *atlas, PackOptions packOptions) uint32_t chartIndex = 0; for (uint32_t i = 0; i < atlas->meshCount; i++) { Mesh &outputMesh = atlas->meshes[i]; + MeshPolygonMapping *meshPolygonMapping = ctx->meshPolygonMappings[i]; + // One polygon can have many triangles. Don't want to process the same polygon more than once when counting indices, building chart faces etc. + internal::BitArray polygonTouched; + if (meshPolygonMapping) { + polygonTouched.resize(meshPolygonMapping->faceVertexCount.size()); + polygonTouched.zeroOutMemory(); + } // Count and alloc arrays. - const internal::param::InvalidMeshGeometry &invalid = ctx->paramAtlas.invalidMeshGeometry(i); + const internal::InvalidMeshGeometry &invalid = ctx->paramAtlas.invalidMeshGeometry(i); outputMesh.vertexCount += invalid.vertices().length; outputMesh.indexCount += invalid.faces().length * 3; for (uint32_t cg = 0; cg < ctx->paramAtlas.chartGroupCount(i); cg++) { const internal::param::ChartGroup *chartGroup = ctx->paramAtlas.chartGroupAt(i, cg); for (uint32_t c = 0; c < chartGroup->chartCount(); c++) { const internal::param::Chart *chart = chartGroup->chartAt(c); - outputMesh.vertexCount += chart->mesh()->vertexCount(); - outputMesh.indexCount += chart->mesh()->faceCount() * 3; + outputMesh.vertexCount += chart->originalVertexCount(); + const uint32_t faceCount = chart->unifiedMesh()->faceCount(); + if (meshPolygonMapping) { + // Map triangles back to polygons and count the polygon vertices. + for (uint32_t f = 0; f < faceCount; f++) { + const uint32_t polygon = meshPolygonMapping->triangleToPolygonMap[chart->mapFaceToSourceFace(f)]; + if (!polygonTouched.get(polygon)) { + polygonTouched.set(polygon); + outputMesh.indexCount += meshPolygonMapping->faceVertexCount[polygon]; + } + } + } else { + outputMesh.indexCount += faceCount * 3; + } outputMesh.chartCount++; } } @@ -9966,7 +9268,7 @@ void PackCharts(Atlas *atlas, PackOptions packOptions) // Copy mesh data. uint32_t firstVertex = 0; { - const internal::param::InvalidMeshGeometry &mesh = ctx->paramAtlas.invalidMeshGeometry(i); + const internal::InvalidMeshGeometry &mesh = ctx->paramAtlas.invalidMeshGeometry(i); internal::ConstArrayView<uint32_t> faces = mesh.faces(); internal::ConstArrayView<uint32_t> indices = mesh.indices(); internal::ConstArrayView<uint32_t> vertices = mesh.vertices(); @@ -9991,23 +9293,50 @@ void PackCharts(Atlas *atlas, PackOptions packOptions) const internal::param::ChartGroup *chartGroup = ctx->paramAtlas.chartGroupAt(i, cg); for (uint32_t c = 0; c < chartGroup->chartCount(); c++) { const internal::param::Chart *chart = chartGroup->chartAt(c); - const internal::Mesh *mesh = chart->mesh(); + const internal::Mesh *unifiedMesh = chart->unifiedMesh(); + const uint32_t faceCount = unifiedMesh->faceCount(); +#if XA_CHECK_PARAM_WINDING + uint32_t flippedCount = 0; + for (uint32_t f = 0; f < faceCount; f++) { + const float area = mesh->computeFaceParametricArea(f); + if (area < 0.0f) + flippedCount++; + } + const char *type = "LSCM"; + if (chart->type() == ChartType::Planar) + type = "planar"; + else if (chart->type() == ChartType::Ortho) + type = "ortho"; + else if (chart->type() == ChartType::Piecewise) + type = "piecewise"; + if (flippedCount > 0) { + if (flippedCount == faceCount) { + XA_PRINT_WARNING("chart %u (%s): all face flipped\n", chartIndex, type); + } else { + XA_PRINT_WARNING("chart %u (%s): %u / %u faces flipped\n", chartIndex, type, flippedCount, faceCount); + } + } +#endif // Vertices. - for (uint32_t v = 0; v < mesh->vertexCount(); v++) { + for (uint32_t v = 0; v < chart->originalVertexCount(); v++) { Vertex &vertex = outputMesh.vertexArray[firstVertex + v]; vertex.atlasIndex = packAtlas.getChart(chartIndex)->atlasIndex; XA_DEBUG_ASSERT(vertex.atlasIndex >= 0); vertex.chartIndex = (int32_t)chartIndex; - const internal::Vector2 &uv = mesh->texcoord(v); + const internal::Vector2 &uv = unifiedMesh->texcoord(chart->originalVertexToUnifiedVertex(v)); vertex.uv[0] = internal::max(0.0f, uv.x); vertex.uv[1] = internal::max(0.0f, uv.y); vertex.xref = chart->mapChartVertexToSourceVertex(v); } // Indices. - for (uint32_t f = 0; f < mesh->faceCount(); f++) { + for (uint32_t f = 0; f < faceCount; f++) { const uint32_t indexOffset = chart->mapFaceToSourceFace(f) * 3; - for (uint32_t j = 0; j < 3; j++) - outputMesh.indexArray[indexOffset + j] = firstVertex + mesh->vertexAt(f * 3 + j); + for (uint32_t j = 0; j < 3; j++) { + uint32_t outIndex = indexOffset + j; + if (meshPolygonMapping) + outIndex = meshPolygonMapping->triangleToPolygonIndicesMap[outIndex]; + outputMesh.indexArray[outIndex] = firstVertex + chart->originalVertices()[f * 3 + j]; + } } // Charts. Chart *outputChart = &outputMesh.chartArray[meshChartIndex]; @@ -10015,14 +9344,38 @@ void PackCharts(Atlas *atlas, PackOptions packOptions) XA_DEBUG_ASSERT(atlasIndex >= 0); outputChart->atlasIndex = (uint32_t)atlasIndex; outputChart->type = chart->isInvalid() ? ChartType::Invalid : chart->type(); - outputChart->faceCount = mesh->faceCount(); - outputChart->faceArray = XA_ALLOC_ARRAY(internal::MemTag::Default, uint32_t, outputChart->faceCount); - for (uint32_t f = 0; f < outputChart->faceCount; f++) - outputChart->faceArray[f] = chart->mapFaceToSourceFace(f); + if (meshPolygonMapping) { + // Count polygons. + polygonTouched.zeroOutMemory(); + outputChart->faceCount = 0; + for (uint32_t f = 0; f < faceCount; f++) { + const uint32_t polygon = meshPolygonMapping->triangleToPolygonMap[chart->mapFaceToSourceFace(f)]; + if (!polygonTouched.get(polygon)) { + polygonTouched.set(polygon); + outputChart->faceCount++; + } + } + // Write polygons. + outputChart->faceArray = XA_ALLOC_ARRAY(internal::MemTag::Default, uint32_t, outputChart->faceCount); + polygonTouched.zeroOutMemory(); + uint32_t of = 0; + for (uint32_t f = 0; f < faceCount; f++) { + const uint32_t polygon = meshPolygonMapping->triangleToPolygonMap[chart->mapFaceToSourceFace(f)]; + if (!polygonTouched.get(polygon)) { + polygonTouched.set(polygon); + outputChart->faceArray[of++] = polygon; + } + } + } else { + outputChart->faceCount = faceCount; + outputChart->faceArray = XA_ALLOC_ARRAY(internal::MemTag::Default, uint32_t, outputChart->faceCount); + for (uint32_t f = 0; f < outputChart->faceCount; f++) + outputChart->faceArray[f] = chart->mapFaceToSourceFace(f); + } outputChart->material = 0; meshChartIndex++; chartIndex++; - firstVertex += mesh->vertexCount(); + firstVertex += chart->originalVertexCount(); } } XA_DEBUG_ASSERT(outputMesh.vertexCount == firstVertex); @@ -10102,28 +9455,21 @@ void PackCharts(Atlas *atlas, PackOptions packOptions) XA_PRINT_MEM_USAGE } -void Generate(Atlas *atlas, ChartOptions chartOptions, ParameterizeOptions parameterizeOptions, PackOptions packOptions) -{ +void Generate(Atlas *atlas, ChartOptions chartOptions, PackOptions packOptions) { if (!atlas) { XA_PRINT_WARNING("Generate: atlas is null.\n"); return; } Context *ctx = (Context *)atlas; - if (!ctx->uvMeshInstances.isEmpty()) { - XA_PRINT_WARNING("Generate: This function should not be called with UV meshes.\n"); - return; - } - if (ctx->meshes.isEmpty()) { - XA_PRINT_WARNING("Generate: No meshes. Call AddMesh first.\n"); + if (ctx->meshes.isEmpty() && ctx->uvMeshInstances.isEmpty()) { + XA_PRINT_WARNING("Generate: No meshes. Call AddMesh or AddUvMesh first.\n"); return; } ComputeCharts(atlas, chartOptions); - ParameterizeCharts(atlas, parameterizeOptions); PackCharts(atlas, packOptions); } -void SetProgressCallback(Atlas *atlas, ProgressFunc progressFunc, void *progressUserData) -{ +void SetProgressCallback(Atlas *atlas, ProgressFunc progressFunc, void *progressUserData) { if (!atlas) { XA_PRINT_WARNING("SetProgressCallback: atlas is null.\n"); return; @@ -10133,37 +9479,33 @@ void SetProgressCallback(Atlas *atlas, ProgressFunc progressFunc, void *progress ctx->progressUserData = progressUserData; } -void SetAlloc(ReallocFunc reallocFunc, FreeFunc freeFunc) -{ +void SetAlloc(ReallocFunc reallocFunc, FreeFunc freeFunc) { internal::s_realloc = reallocFunc; internal::s_free = freeFunc; } -void SetPrint(PrintFunc print, bool verbose) -{ +void SetPrint(PrintFunc print, bool verbose) { internal::s_print = print; internal::s_printVerbose = verbose; } -const char *StringForEnum(AddMeshError::Enum error) -{ +const char *StringForEnum(AddMeshError error) { if (error == AddMeshError::Error) return "Unspecified error"; if (error == AddMeshError::IndexOutOfRange) return "Index out of range"; + if (error == AddMeshError::InvalidFaceVertexCount) + return "Invalid face vertex count"; if (error == AddMeshError::InvalidIndexCount) return "Invalid index count"; return "Success"; } -const char *StringForEnum(ProgressCategory::Enum category) -{ +const char *StringForEnum(ProgressCategory category) { if (category == ProgressCategory::AddMesh) return "Adding mesh(es)"; if (category == ProgressCategory::ComputeCharts) return "Computing charts"; - if (category == ProgressCategory::ParameterizeCharts) - return "Parameterizing charts"; if (category == ProgressCategory::PackCharts) return "Packing charts"; if (category == ProgressCategory::BuildOutputMeshes) @@ -10172,3 +9514,96 @@ const char *StringForEnum(ProgressCategory::Enum category) } } // namespace xatlas + +#if XATLAS_C_API +static_assert(sizeof(xatlas::Chart) == sizeof(xatlasChart), "xatlasChart size mismatch"); +static_assert(sizeof(xatlas::Vertex) == sizeof(xatlasVertex), "xatlasVertex size mismatch"); +static_assert(sizeof(xatlas::Mesh) == sizeof(xatlasMesh), "xatlasMesh size mismatch"); +static_assert(sizeof(xatlas::Atlas) == sizeof(xatlasAtlas), "xatlasAtlas size mismatch"); +static_assert(sizeof(xatlas::MeshDecl) == sizeof(xatlasMeshDecl), "xatlasMeshDecl size mismatch"); +static_assert(sizeof(xatlas::UvMeshDecl) == sizeof(xatlasUvMeshDecl), "xatlasUvMeshDecl size mismatch"); +static_assert(sizeof(xatlas::ChartOptions) == sizeof(xatlasChartOptions), "xatlasChartOptions size mismatch"); +static_assert(sizeof(xatlas::PackOptions) == sizeof(xatlasPackOptions), "xatlasPackOptions size mismatch"); + +#ifdef __cplusplus +extern "C" { +#endif + +xatlasAtlas *xatlasCreate() { + return (xatlasAtlas *)xatlas::Create(); +} + +void xatlasDestroy(xatlasAtlas *atlas) { + xatlas::Destroy((xatlas::Atlas *)atlas); +} + +xatlasAddMeshError xatlasAddMesh(xatlasAtlas *atlas, const xatlasMeshDecl *meshDecl, uint32_t meshCountHint) { + return (xatlasAddMeshError)xatlas::AddMesh((xatlas::Atlas *)atlas, *(const xatlas::MeshDecl *)meshDecl, meshCountHint); +} + +void xatlasAddMeshJoin(xatlasAtlas *atlas) { + xatlas::AddMeshJoin((xatlas::Atlas *)atlas); +} + +xatlasAddMeshError xatlasAddUvMesh(xatlasAtlas *atlas, const xatlasUvMeshDecl *decl) { + return (xatlasAddMeshError)xatlas::AddUvMesh((xatlas::Atlas *)atlas, *(const xatlas::UvMeshDecl *)decl); +} + +void xatlasComputeCharts(xatlasAtlas *atlas, const xatlasChartOptions *chartOptions) { + xatlas::ComputeCharts((xatlas::Atlas *)atlas, chartOptions ? *(xatlas::ChartOptions *)chartOptions : xatlas::ChartOptions()); +} + +void xatlasPackCharts(xatlasAtlas *atlas, const xatlasPackOptions *packOptions) { + xatlas::PackCharts((xatlas::Atlas *)atlas, packOptions ? *(xatlas::PackOptions *)packOptions : xatlas::PackOptions()); +} + +void xatlasGenerate(xatlasAtlas *atlas, const xatlasChartOptions *chartOptions, const xatlasPackOptions *packOptions) { + xatlas::Generate((xatlas::Atlas *)atlas, chartOptions ? *(xatlas::ChartOptions *)chartOptions : xatlas::ChartOptions(), packOptions ? *(xatlas::PackOptions *)packOptions : xatlas::PackOptions()); +} + +void xatlasSetProgressCallback(xatlasAtlas *atlas, xatlasProgressFunc progressFunc, void *progressUserData) { + xatlas::ProgressFunc pf; + *(void **)&pf = (void *)progressFunc; + xatlas::SetProgressCallback((xatlas::Atlas *)atlas, pf, progressUserData); +} + +void xatlasSetAlloc(xatlasReallocFunc reallocFunc, xatlasFreeFunc freeFunc) { + xatlas::SetAlloc((xatlas::ReallocFunc)reallocFunc, (xatlas::FreeFunc)freeFunc); +} + +void xatlasSetPrint(xatlasPrintFunc print, bool verbose) { + xatlas::SetPrint((xatlas::PrintFunc)print, verbose); +} + +const char *xatlasAddMeshErrorString(xatlasAddMeshError error) { + return xatlas::StringForEnum((xatlas::AddMeshError)error); +} + +const char *xatlasProgressCategoryString(xatlasProgressCategory category) { + return xatlas::StringForEnum((xatlas::ProgressCategory)category); +} + +void xatlasMeshDeclInit(xatlasMeshDecl *meshDecl) { + xatlas::MeshDecl init; + memcpy(meshDecl, &init, sizeof(init)); +} + +void xatlasUvMeshDeclInit(xatlasUvMeshDecl *uvMeshDecl) { + xatlas::UvMeshDecl init; + memcpy(uvMeshDecl, &init, sizeof(init)); +} + +void xatlasChartOptionsInit(xatlasChartOptions *chartOptions) { + xatlas::ChartOptions init; + memcpy(chartOptions, &init, sizeof(init)); +} + +void xatlasPackOptionsInit(xatlasPackOptions *packOptions) { + xatlas::PackOptions init; + memcpy(packOptions, &init, sizeof(init)); +} + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // XATLAS_C_API diff --git a/thirdparty/xatlas/xatlas.h b/thirdparty/xatlas/xatlas.h index cc47f4837e..fc40d9d49c 100644 --- a/thirdparty/xatlas/xatlas.h +++ b/thirdparty/xatlas/xatlas.h @@ -31,35 +31,30 @@ Copyright NVIDIA Corporation 2006 -- Ignacio Castano <icastano@nvidia.com> #pragma once #ifndef XATLAS_H #define XATLAS_H +#include <stddef.h> #include <stdint.h> namespace xatlas { -struct ChartType -{ - enum Enum - { - Planar, - Ortho, - LSCM, - Piecewise, - Invalid - }; +enum class ChartType { + Planar, + Ortho, + LSCM, + Piecewise, + Invalid }; // A group of connected faces, belonging to a single atlas. -struct Chart -{ +struct Chart { uint32_t *faceArray; uint32_t atlasIndex; // Sub-atlas index. uint32_t faceCount; - ChartType::Enum type; + ChartType type; uint32_t material; }; // Output vertex. -struct Vertex -{ +struct Vertex { int32_t atlasIndex; // Sub-atlas index. -1 if the vertex doesn't exist in any atlas. int32_t chartIndex; // -1 if the vertex doesn't exist in any chart. float uv[2]; // Not normalized - values are in Atlas width and height range. @@ -67,8 +62,7 @@ struct Vertex }; // Output mesh. -struct Mesh -{ +struct Mesh { Chart *chartArray; uint32_t *indexArray; Vertex *vertexArray; @@ -83,16 +77,15 @@ static const uint32_t kImageIsBilinearBit = 0x40000000; static const uint32_t kImageIsPaddingBit = 0x20000000; // Empty on creation. Populated after charts are packed. -struct Atlas -{ +struct Atlas { uint32_t *image; Mesh *meshes; // The output meshes, corresponding to each AddMesh call. + float *utilization; // Normalized atlas texel utilization array. E.g. a value of 0.8 means 20% empty space. atlasCount in length. uint32_t width; // Atlas width in texels. uint32_t height; // Atlas height in texels. uint32_t atlasCount; // Number of sub-atlases. Equal to 0 unless PackOptions resolution is changed from default (0). uint32_t chartCount; // Total number of charts in all meshes. uint32_t meshCount; // Number of output meshes. Equal to the number of times AddMesh was called. - float *utilization; // Normalized atlas texel utilization array. E.g. a value of 0.8 means 20% empty space. atlasCount in length. float texelsPerUnit; // Equal to PackOptions texelsPerUnit if texelsPerUnit > 0, otherwise an estimated value to match PackOptions resolution. }; @@ -101,73 +94,76 @@ Atlas *Create(); void Destroy(Atlas *atlas); -struct IndexFormat -{ - enum Enum - { - UInt16, - UInt32 - }; +enum class IndexFormat { + UInt16, + UInt32 }; // Input mesh declaration. -struct MeshDecl -{ +struct MeshDecl { const void *vertexPositionData = nullptr; const void *vertexNormalData = nullptr; // optional const void *vertexUvData = nullptr; // optional. The input UVs are provided as a hint to the chart generator. const void *indexData = nullptr; // optional - - // Optional. indexCount / 3 (triangle count) in length. + + // Optional. Must be faceCount in length. // Don't atlas faces set to true. Ignored faces still exist in the output meshes, Vertex uv is set to (0, 0) and Vertex atlasIndex to -1. const bool *faceIgnoreData = nullptr; + // Optional. Must be faceCount in length. + // Only faces with the same material will be assigned to the same chart. + const uint32_t *faceMaterialData = nullptr; + + // Optional. Must be faceCount in length. + // Polygon / n-gon support. Faces are assumed to be triangles if this is null. + const uint8_t *faceVertexCount = nullptr; + uint32_t vertexCount = 0; uint32_t vertexPositionStride = 0; uint32_t vertexNormalStride = 0; // optional uint32_t vertexUvStride = 0; // optional uint32_t indexCount = 0; int32_t indexOffset = 0; // optional. Add this offset to all indices. - IndexFormat::Enum indexFormat = IndexFormat::UInt16; + uint32_t faceCount = 0; // Optional if faceVertexCount is null. Otherwise assumed to be indexCount / 3. + IndexFormat indexFormat = IndexFormat::UInt16; // Vertex positions within epsilon distance of each other are considered colocal. float epsilon = 1.192092896e-07F; }; -struct AddMeshError -{ - enum Enum - { - Success, // No error. - Error, // Unspecified error. - IndexOutOfRange, // An index is >= MeshDecl vertexCount. - InvalidIndexCount // Not evenly divisible by 3 - expecting triangles. - }; +enum class AddMeshError { + Success, // No error. + Error, // Unspecified error. + IndexOutOfRange, // An index is >= MeshDecl vertexCount. + InvalidFaceVertexCount, // Must be >= 3. + InvalidIndexCount // Not evenly divisible by 3 - expecting triangles. }; // Add a mesh to the atlas. MeshDecl data is copied, so it can be freed after AddMesh returns. -AddMeshError::Enum AddMesh(Atlas *atlas, const MeshDecl &meshDecl, uint32_t meshCountHint = 0); +AddMeshError AddMesh(Atlas *atlas, const MeshDecl &meshDecl, uint32_t meshCountHint = 0); // Wait for AddMesh async processing to finish. ComputeCharts / Generate call this internally. void AddMeshJoin(Atlas *atlas); -struct UvMeshDecl -{ +struct UvMeshDecl { const void *vertexUvData = nullptr; const void *indexData = nullptr; // optional - const uint32_t *faceMaterialData = nullptr; // Optional. Faces with different materials won't be assigned to the same chart. Must be indexCount / 3 in length. + const uint32_t *faceMaterialData = nullptr; // Optional. Overlapping UVs should be assigned a different material. Must be indexCount / 3 in length. uint32_t vertexCount = 0; uint32_t vertexStride = 0; uint32_t indexCount = 0; int32_t indexOffset = 0; // optional. Add this offset to all indices. - IndexFormat::Enum indexFormat = IndexFormat::UInt16; - bool rotateCharts = true; + IndexFormat indexFormat = IndexFormat::UInt16; }; -AddMeshError::Enum AddUvMesh(Atlas *atlas, const UvMeshDecl &decl); +AddMeshError AddUvMesh(Atlas *atlas, const UvMeshDecl &decl); + +// Custom parameterization function. texcoords initial values are an orthogonal parameterization. +typedef void (*ParameterizeFunc)(const float *positions, float *texcoords, uint32_t vertexCount, const uint32_t *indices, uint32_t indexCount); + +struct ChartOptions { + ParameterizeFunc paramFunc = nullptr; -struct ChartOptions -{ float maxChartArea = 0.0f; // Don't grow charts to be larger than this. 0 means no limit. float maxBoundaryLength = 0.0f; // Don't grow charts to have a longer boundary than this. 0 means no limit. @@ -180,26 +176,31 @@ struct ChartOptions float maxCost = 2.0f; // If total of all metrics * weights > maxCost, don't grow chart. Lower values result in more charts. uint32_t maxIterations = 1; // Number of iterations of the chart growing and seeding phases. Higher values result in better charts. + + bool useInputMeshUvs = false; // Use MeshDecl::vertexUvData for charts. + bool fixWinding = false; // Enforce consistent texture coordinate winding. }; // Call after all AddMesh calls. Can be called multiple times to recompute charts with different options. void ComputeCharts(Atlas *atlas, ChartOptions options = ChartOptions()); -// Custom parameterization function. texcoords initial values are an orthogonal parameterization. -typedef void (*ParameterizeFunc)(const float *positions, float *texcoords, uint32_t vertexCount, const uint32_t *indices, uint32_t indexCount); +struct PackOptions { + // Charts larger than this will be scaled down. 0 means no limit. + uint32_t maxChartSize = 0; -struct ParameterizeOptions -{ - ParameterizeFunc func = nullptr; - bool closeHoles = true; // If the custom parameterization function works with multiple boundaries, this can be set to false to improve performance. - bool fixTJunctions = true; // If meshes don't have T-junctions, this can be set to false to improve performance. -}; + // Number of pixels to pad charts with. + uint32_t padding = 0; + + // Unit to texel scale. e.g. a 1x1 quad with texelsPerUnit of 32 will take up approximately 32x32 texels in the atlas. + // If 0, an estimated value will be calculated to approximately match the given resolution. + // If resolution is also 0, the estimated value will approximately match a 1024x1024 atlas. + float texelsPerUnit = 0.0f; -// Call after ComputeCharts. Can be called multiple times to re-parameterize charts with a different ParameterizeFunc. -void ParameterizeCharts(Atlas *atlas, ParameterizeOptions options = ParameterizeOptions()); + // If 0, generate a single atlas with texelsPerUnit determining the final resolution. + // If not 0, and texelsPerUnit is not 0, generate one or more atlases with that exact resolution. + // If not 0, and texelsPerUnit is 0, texelsPerUnit is estimated to approximately match the resolution. + uint32_t resolution = 0; -struct PackOptions -{ // Leave space around charts for texels that would be sampled by bilinear filtering. bool bilinear = true; @@ -212,44 +213,29 @@ struct PackOptions // Create Atlas::image bool createImage = false; - // Charts larger than this will be scaled down. 0 means no limit. - uint32_t maxChartSize = 0; - - // Number of pixels to pad charts with. - uint32_t padding = 0; + // Rotate charts to the axis of their convex hull. + bool rotateChartsToAxis = true; - // Unit to texel scale. e.g. a 1x1 quad with texelsPerUnit of 32 will take up approximately 32x32 texels in the atlas. - // If 0, an estimated value will be calculated to approximately match the given resolution. - // If resolution is also 0, the estimated value will approximately match a 1024x1024 atlas. - float texelsPerUnit = 0.0f; - - // If 0, generate a single atlas with texelsPerUnit determining the final resolution. - // If not 0, and texelsPerUnit is not 0, generate one or more atlases with that exact resolution. - // If not 0, and texelsPerUnit is 0, texelsPerUnit is estimated to approximately match the resolution. - uint32_t resolution = 0; + // Rotate charts to improve packing. + bool rotateCharts = true; }; -// Call after ParameterizeCharts. Can be called multiple times to re-pack charts with different options. +// Call after ComputeCharts. Can be called multiple times to re-pack charts with different options. void PackCharts(Atlas *atlas, PackOptions packOptions = PackOptions()); -// Equivalent to calling ComputeCharts, ParameterizeCharts and PackCharts in sequence. Can be called multiple times to regenerate with different options. -void Generate(Atlas *atlas, ChartOptions chartOptions = ChartOptions(), ParameterizeOptions parameterizeOptions = ParameterizeOptions(), PackOptions packOptions = PackOptions()); +// Equivalent to calling ComputeCharts and PackCharts in sequence. Can be called multiple times to regenerate with different options. +void Generate(Atlas *atlas, ChartOptions chartOptions = ChartOptions(), PackOptions packOptions = PackOptions()); // Progress tracking. -struct ProgressCategory -{ - enum Enum - { - AddMesh, - ComputeCharts, - ParameterizeCharts, - PackCharts, - BuildOutputMeshes - }; +enum class ProgressCategory { + AddMesh, + ComputeCharts, + PackCharts, + BuildOutputMeshes }; // May be called from any thread. Return false to cancel. -typedef bool (*ProgressFunc)(ProgressCategory::Enum category, int progress, void *userData); +typedef bool (*ProgressFunc)(ProgressCategory category, int progress, void *userData); void SetProgressCallback(Atlas *atlas, ProgressFunc progressFunc = nullptr, void *progressUserData = nullptr); @@ -263,8 +249,8 @@ typedef int (*PrintFunc)(const char *, ...); void SetPrint(PrintFunc print, bool verbose); // Helper functions for error messages. -const char *StringForEnum(AddMeshError::Enum error); -const char *StringForEnum(ProgressCategory::Enum category); +const char *StringForEnum(AddMeshError error); +const char *StringForEnum(ProgressCategory category); } // namespace xatlas |