diff options
Diffstat (limited to 'thirdparty/bullet')
83 files changed, 9106 insertions, 7914 deletions
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..dbe82fd61f 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -24,6 +24,7 @@ subject to the following restrictions: #define WANTS_DEACTIVATION 3 #define DISABLE_DEACTIVATION 4 #define DISABLE_SIMULATION 5 +#define FIXED_BASE_MULTI_BODY 6 struct btBroadphaseProxy; class btCollisionShape; @@ -127,6 +128,7 @@ public: enum CollisionFlags { + CF_DYNAMIC_OBJECT = 0, CF_STATIC_OBJECT = 1, CF_KINEMATIC_OBJECT = 2, CF_NO_CONTACT_RESPONSE = 4, @@ -251,6 +253,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); @@ -293,7 +305,7 @@ public: SIMD_FORCE_INLINE bool isActive() const { - return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); + return ((getActivationState() != FIXED_BASE_MULTI_BODY) && (getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } void setRestitution(btScalar rest) diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 71184f36ac..f74dcabc54 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1037,7 +1037,7 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback m_castShape(castShape) { btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin() - m_convexFromTrans.getOrigin()); - btVector3 rayDir = unnormalizedRayDir.normalized(); + btVector3 rayDir = unnormalizedRayDir.fuzzyZero() ? btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0)) : unnormalizedRayDir.normalized(); ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; @@ -1294,9 +1294,7 @@ public: btVector3 normalColor(1, 1, 0); m_debugDrawer->drawLine(center, center + normal, normalColor); } - m_debugDrawer->drawLine(wv0, wv1, m_color); - m_debugDrawer->drawLine(wv1, wv2, m_color); - m_debugDrawer->drawLine(wv2, wv0, m_color); + m_debugDrawer->drawTriangle(wv0, wv1, wv2, m_color, 1.0); } }; 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..01bf7f67f5 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -18,26 +18,72 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const float* heightfieldData, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges) + : m_userValue3(0), m_triangleInfoMap(0) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_FLOAT, + flipQuadEdges); +} + +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const double* heightfieldData, + btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges) + : m_userValue3(0), m_triangleInfoMap(0) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + /*heightScale=*/1, minHeight, maxHeight, upAxis, PHY_DOUBLE, + flipQuadEdges); +} + +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const short* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges) + : m_userValue3(0), m_triangleInfoMap(0) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + heightScale, minHeight, maxHeight, upAxis, PHY_SHORT, + flipQuadEdges); +} + +btHeightfieldTerrainShape::btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, const unsigned char* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, bool flipQuadEdges) + : m_userValue3(0), m_triangleInfoMap(0) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + heightScale, minHeight, maxHeight, upAxis, PHY_UCHAR, + flipQuadEdges); +} + +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) { + // legacy constructor: Assumes PHY_FLOAT means btScalar. +#ifdef BT_USE_DOUBLE_PRECISION + if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE; +#endif initialize(heightStickWidth, heightStickLength, heightfieldData, heightScale, minHeight, maxHeight, upAxis, hdt, flipQuadEdges); } 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, - // and min height is zero + // legacy constructor: support only btScalar or unsigned char data, + // and min height is zero. PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR; +#ifdef BT_USE_DOUBLE_PRECISION + if (hdt == PHY_FLOAT) hdt = PHY_DOUBLE; +#endif btScalar minHeight = 0.0f; // previously, height = uchar * maxHeight / 65535. @@ -61,7 +107,7 @@ void btHeightfieldTerrainShape::initialize( // btAssert(heightScale) -- do we care? Trust caller here btAssert(minHeight <= maxHeight); // && "bad min/max height"); btAssert(upAxis >= 0 && upAxis < 3); // && "bad upAxis--should be in range [0,2]"); - btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT); // && "Bad height data type enum"); + btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_DOUBLE || hdt != PHY_SHORT); // && "Bad height data type enum"); // initialize member variables m_shapeType = TERRAIN_SHAPE_PROXYTYPE; @@ -154,6 +200,12 @@ btHeightfieldTerrainShape::getRawHeightFieldValue(int x, int y) const break; } + case PHY_DOUBLE: + { + val = m_heightfieldDataDouble[(y * m_heightStickWidth) + x]; + break; + } + case PHY_UCHAR: { unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y * m_heightStickWidth) + x]; @@ -234,6 +286,30 @@ getQuantized( return (int)(x + 0.5); } +// Equivalent to std::minmax({a, b, c}). +// Performs at most 3 comparisons. +static btHeightfieldTerrainShape::Range minmaxRange(btScalar a, btScalar b, btScalar c) +{ + if (a > b) + { + if (b > c) + return btHeightfieldTerrainShape::Range(c, a); + else if (a > c) + return btHeightfieldTerrainShape::Range(b, a); + else + return btHeightfieldTerrainShape::Range(b, c); + } + else + { + if (a > c) + return btHeightfieldTerrainShape::Range(c, b); + else if (b > c) + return btHeightfieldTerrainShape::Range(a, b); + else + return btHeightfieldTerrainShape::Range(a, c); + } +} + /// given input vector, return quantized version /** This routine is basically determining the gridpoint indices for a given @@ -336,7 +412,8 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback } // TODO If m_vboundsGrid is available, use it to determine if we really need to process this area - + + const Range aabbUpRange(aabbMin[m_upAxis], aabbMax[m_upAxis]); for (int j = startJ; j < endJ; j++) { for (int x = startX; x < endX; x++) @@ -351,29 +428,51 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j + x) & 1)) || (m_useZigzagSubdivision && !(j & 1))) { - //first triangle getVertex(x, j, vertices[indices[0]]); getVertex(x, j + 1, vertices[indices[1]]); getVertex(x + 1, j + 1, vertices[indices[2]]); - callback->processTriangle(vertices, 2 * x, j); - //second triangle - // getVertex(x,j,vertices[0]);//already got this vertex before, thanks to Danny Chapman - getVertex(x + 1, j + 1, vertices[indices[1]]); + + // Skip triangle processing if the triangle is out-of-AABB. + Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]); + + if (upRange.overlaps(aabbUpRange)) + callback->processTriangle(vertices, 2 * x, j); + + // already set: getVertex(x, j, vertices[indices[0]]) + + // equivalent to: getVertex(x + 1, j + 1, vertices[indices[1]]); + vertices[indices[1]] = vertices[indices[2]]; + getVertex(x + 1, j, vertices[indices[2]]); - callback->processTriangle(vertices, 2 * x+1, j); + upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]); + upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]); + + if (upRange.overlaps(aabbUpRange)) + callback->processTriangle(vertices, 2 * x + 1, j); } else { - //first triangle getVertex(x, j, vertices[indices[0]]); getVertex(x, j + 1, vertices[indices[1]]); getVertex(x + 1, j, vertices[indices[2]]); - callback->processTriangle(vertices, 2 * x, j); - //second triangle - getVertex(x + 1, j, vertices[indices[0]]); - //getVertex(x,j+1,vertices[1]); + + // Skip triangle processing if the triangle is out-of-AABB. + Range upRange = minmaxRange(vertices[0][m_upAxis], vertices[1][m_upAxis], vertices[2][m_upAxis]); + + if (upRange.overlaps(aabbUpRange)) + callback->processTriangle(vertices, 2 * x, j); + + // already set: getVertex(x, j + 1, vertices[indices[1]]); + + // equivalent to: getVertex(x + 1, j, vertices[indices[0]]); + vertices[indices[0]] = vertices[indices[2]]; + getVertex(x + 1, j + 1, vertices[indices[2]]); - callback->processTriangle(vertices, 2 * x+1, j); + upRange.min = btMin(upRange.min, vertices[indices[2]][m_upAxis]); + upRange.max = btMax(upRange.max, vertices[indices[2]][m_upAxis]); + + if (upRange.overlaps(aabbUpRange)) + callback->processTriangle(vertices, 2 * x + 1, j); } } } @@ -848,4 +947,4 @@ void btHeightfieldTerrainShape::buildAccelerator(int chunkSize) void btHeightfieldTerrainShape::clearAccelerator() { m_vboundsGrid.clear(); -}
\ No newline at end of file +} diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 8dea98fc6b..7e251fa71e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -50,17 +50,15 @@ subject to the following restrictions: The heightfield heights are determined from the data type used for the heightfieldData array. - - PHY_UCHAR: height at a point is the uchar value at the + - unsigned char: height at a point is the uchar value at the grid point, multipled by heightScale. uchar isn't recommended because of its inability to deal with negative values, and low resolution (8-bit). - - PHY_SHORT: height at a point is the short int value at that grid + - short: height at a point is the short int value at that grid point, multipled by heightScale. - - PHY_FLOAT: height at a point is the float value at that grid - point. heightScale is ignored when using the float heightfield - data type. + - float or dobule: height at a point is the value at that grid point. Whatever the caller specifies as minHeight and maxHeight will be honored. The class will not inspect the heightfield to discover the actual minimum @@ -75,6 +73,14 @@ btHeightfieldTerrainShape : public btConcaveShape public: struct Range { + Range() {} + Range(btScalar min, btScalar max) : min(min), max(max) {} + + bool overlaps(const Range& other) const + { + return !(min > other.max || max < other.min); + } + btScalar min; btScalar max; }; @@ -95,7 +101,8 @@ protected: union { const unsigned char* m_heightfieldDataUnsignedChar; const short* m_heightfieldDataShort; - const btScalar* m_heightfieldDataFloat; + const float* m_heightfieldDataFloat; + const double* m_heightfieldDataDouble; const void* m_heightfieldDataUnknown; }; @@ -114,7 +121,7 @@ protected: int m_vboundsGridLength; int m_vboundsChunkSize; - int m_userIndex2; + btScalar m_userValue3; struct btTriangleInfoMap* m_triangleInfoMap; @@ -135,11 +142,33 @@ protected: public: BT_DECLARE_ALIGNED_ALLOCATOR(); - /// preferred constructor + /// preferred constructors + btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const float* heightfieldData, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges); + btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const double* heightfieldData, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges); + btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const short* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges); + btHeightfieldTerrainShape( + int heightStickWidth, int heightStickLength, + const unsigned char* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, + int upAxis, bool flipQuadEdges); + + /// legacy constructor /** This constructor supports a range of heightfield data types, and allows for a non-zero minimum height value. heightScale is needed for any integer-based heightfield data types. + + This legacy constructor considers `PHY_FLOAT` to mean `btScalar`. + With `BT_USE_DOUBLE_PRECISION`, it will expect `heightfieldData` + to be double-precision. */ btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar heightScale, @@ -150,7 +179,7 @@ public: /// legacy constructor /** The legacy constructor assumes the heightfield has a minimum height - of zero. Only unsigned char or floats are supported. For legacy + of zero. Only unsigned char or btScalar data are supported. For legacy compatibility reasons, heightScale is calculated as maxHeight / 65535 (and is only used when useFloatData = false). */ @@ -192,14 +221,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; @@ -226,4 +247,4 @@ public: } }; -#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
\ No newline at end of file +#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H 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/ConstraintSolver/btSliderConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index cac5302a73..cac5302a73 100755..100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 75ca34e978..75ca34e978 100755..100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h 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..d7588aedc8 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp @@ -33,8 +33,8 @@ namespace { -const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) -const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds +const btScalar INITIAL_SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) +const btScalar INITIAL_SLEEP_TIMEOUT = btScalar(2); // in seconds } // namespace void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame @@ -110,6 +110,9 @@ btMultiBody::btMultiBody(int n_links, m_canSleep(canSleep), m_canWakeup(true), m_sleepTimer(0), + m_sleepEpsilon(INITIAL_SLEEP_EPSILON), + m_sleepTimeout(INITIAL_SLEEP_TIMEOUT), + m_userObjectPointer(0), m_userIndex2(-1), m_userIndex(-1), @@ -125,7 +128,8 @@ btMultiBody::btMultiBody(int n_links, m_posVarCnt(0), m_useRK4(false), m_useGlobalVelocities(false), - m_internalNeedsJointFeedback(false) + m_internalNeedsJointFeedback(false), + m_kinematic_calculate_velocity(false) { m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); @@ -344,6 +348,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 +677,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 +826,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 +902,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 +961,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 +971,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 +1085,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1081,21 +1119,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) { @@ -1374,7 +1414,7 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV } } -void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const +void btMultiBody::mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const { for (int row = 0; row < rowsA; row++) { @@ -1432,7 +1472,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 +1491,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 +1536,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 +1549,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 +1594,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 +1664,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 +1694,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: + { + for (int j = 0; j < 4; ++j) + { + pJointPos[j] = m_links[i].m_jointPos[j]; + } + break; + } + case btMultibodyLink::ePlanar: { - pJointPos[j] = m_links[i].m_jointPos[j]; + for (int j = 0; j < 3; ++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; + 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 +1786,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 +1849,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 +1877,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: + { + } } } @@ -2015,10 +2107,10 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) motion += m_realBuf[i] * m_realBuf[i]; } - if (motion < SLEEP_EPSILON) + if (motion < m_sleepEpsilon) { m_sleepTimer += timestep; - if (m_sleepTimer > SLEEP_TIMEOUT) + if (m_sleepTimer > m_sleepTimeout) { goToSleep(); } @@ -2135,8 +2227,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 +2381,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 (m_kinematic_calculate_velocity && 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..345970d261 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); @@ -490,19 +545,30 @@ public: { m_canWakeup = canWakeup; } - bool isAwake() const { return m_awake; } + bool isAwake() const + { + return m_awake; + } void wakeUp(); 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,6 +719,25 @@ 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; + + void setSleepThreshold(btScalar sleepThreshold) + { + m_sleepEpsilon = sleepThreshold; + } + + void setSleepTimeout(btScalar sleepTimeout) + { + this->m_sleepTimeout = sleepTimeout; + } private: @@ -674,7 +759,7 @@ private: } } - void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; + void mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; private: btMultiBodyLinkCollider *m_baseCollider; //can be NULL @@ -711,6 +796,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; @@ -729,6 +815,8 @@ private: bool m_canSleep; bool m_canWakeup; btScalar m_sleepTimer; + btScalar m_sleepEpsilon; + btScalar m_sleepTimeout; void *m_userObjectPointer; int m_userIndex2; @@ -751,6 +839,9 @@ private: ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only bool m_internalNeedsJointFeedback; + + //If enabled, calculate the velocity based on kinematic transform changes. Currently only implemented for the base. + bool m_kinematic_calculate_velocity; }; struct btMultiBodyLinkDoubleData diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index d7ed05ce57..00d5fd5609 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), @@ -60,7 +61,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra btScalar lowerLimit, btScalar upperLimit, bool angConstraint, btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip, + btScalar damping) { solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyB = m_bodyB; @@ -347,7 +349,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra { btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel; // * damping; + btScalar velocityError = (desiredVelocity - rel_vel) * damping; btScalar erp = infoGlobal.m_erp2; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 5c15f3e851..1aaa07b69e 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; @@ -77,17 +94,21 @@ protected: bool angConstraint = false, btScalar relaxation = 1.f, - bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0); + bool isFriction = false, btScalar desiredVelocity = 0, btScalar cfmSlip = 0, btScalar damping = 1.0); 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..e7af332eb3 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -137,7 +137,14 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) btMultiBodyLinkCollider* col = body->getBaseCollider(); if (col && col->getActivationState() == ACTIVE_TAG) { - col->setActivationState(WANTS_DEACTIVATION); + if (body->hasFixedBase()) + { + col->setActivationState(FIXED_BASE_MULTI_BODY); + } else + { + col->setActivationState(WANTS_DEACTIVATION); + } + col->setDeactivationTime(0.f); } for (int b = 0; b < body->getNumLinks(); b++) @@ -592,6 +599,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) if (!isSleeping) { + bod->addSplitV(); int nLinks = bod->getNumLinks(); ///base + num m_links @@ -610,6 +618,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 +876,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..00a7ef3579 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -23,13 +23,16 @@ 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.), - m_kp(0.2), + m_use_multi_dof_params(false), + m_kd(1., 1., 1.), + m_kp(0.2, 0.2, 0.2), m_erp(1), - m_rhsClamp(SIMD_INFINITY) + m_rhsClamp(SIMD_INFINITY), + m_maxAppliedImpulseMultiDof(maxMotorImpulse, maxMotorImpulse, maxMotorImpulse), + m_damping(1.0, 1.0, 1.0) { m_maxAppliedImpulse = maxMotorImpulse; @@ -139,7 +142,8 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; btScalar desiredVelocity = this->m_desiredVelocity[row]; - btScalar velocityError = desiredVelocity - currentVelocity; + double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0]; + btScalar velocityError = (desiredVelocity - currentVelocity) * kd; btMatrix3x3 frameAworld; frameAworld.setIdentity(); @@ -152,12 +156,16 @@ btQuaternion relRot = currentQuat.inverse() * desiredQuat; case btMultibodyLink::eSpherical: { btVector3 constraintNormalAng = frameAworld.getColumn(row % 3); - posError = m_kp*angleDiff[row % 3]; + double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0]; + posError = kp*angleDiff[row % 3]; + double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse; fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, btVector3(0,0,0), dummy, dummy, posError, infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + -max_applied_impulse, max_applied_impulse, true, + 1.0, false, 0, 0, + m_damping[row % 3]); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; break; diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h index 621beab5a4..bdeccc2e24 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h @@ -26,10 +26,13 @@ class btMultiBodySphericalJointMotor : public btMultiBodyConstraint protected: btVector3 m_desiredVelocity; btQuaternion m_desiredPosition; - btScalar m_kd; - btScalar m_kp; + bool m_use_multi_dof_params; + btVector3 m_kd; + btVector3 m_kp; btScalar m_erp; btScalar m_rhsClamp; //maximum error + btVector3 m_maxAppliedImpulseMultiDof; + btVector3 m_damping; public: btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse); @@ -44,16 +47,32 @@ public: btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal); - virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f) + virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0) + { + m_desiredVelocity = velTarget; + m_kd = btVector3(kd, kd, kd); + m_use_multi_dof_params = false; + } + + virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0)) { m_desiredVelocity = velTarget; m_kd = kd; + m_use_multi_dof_params = true; } - virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f) + virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f) + { + m_desiredPosition = posTarget; + m_kp = btVector3(kp, kp, kp); + m_use_multi_dof_params = false; + } + + virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f)) { m_desiredPosition = posTarget; m_kp = kp; + m_use_multi_dof_params = true; } virtual void setErp(btScalar erp) @@ -68,6 +87,28 @@ public: { m_rhsClamp = rhsClamp; } + + btScalar getMaxAppliedImpulseMultiDof(int i) const + { + return m_maxAppliedImpulseMultiDof[i]; + } + + void setMaxAppliedImpulseMultiDof(const btVector3& maxImp) + { + m_maxAppliedImpulseMultiDof = maxImp; + m_use_multi_dof_params = true; + } + + btScalar getDamping(int i) const + { + return m_damping[i]; + } + + void setDamping(const btVector3& damping) + { + m_damping = damping; + } + virtual void debugDraw(class btIDebugDraw* drawer) { //todo(erwincoumans) diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 5d864f2757..ed4e0b686d 100644 --- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -532,7 +532,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) J_transpose = J.transpose(); btMatrixXu& tmp = m_scratchTmp; - + //Minv.printMatrix("Minv="); { { BT_PROFILE("J*Minv"); @@ -543,7 +543,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) m_A = tmp * J_transpose; } } - + //J.printMatrix("J"); if (1) { // add cfm to the diagonal of m_A 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..e81680f019 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]; + /* Clear contacts */ + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); + psb->m_faceNodeContacts.resize(0); + + 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); + } + /* 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..697408355c 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, const 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..dfde8fd1e4 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; { @@ -1289,66 +1317,68 @@ public: } for (int k = 0; k < m_faceNodeContacts.size(); ++k) { - int i = indices[k]; - btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i]; + int idx = indices[k]; + btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[idx]; btSoftBody::Node* node = c.m_node; btSoftBody::Face* face = c.m_face; const btVector3& w = c.m_bary; 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/btIDebugDraw.h b/thirdparty/bullet/LinearMath/btIDebugDraw.h index 82ec19a69b..df4db2ff5a 100644 --- a/thirdparty/bullet/LinearMath/btIDebugDraw.h +++ b/thirdparty/bullet/LinearMath/btIDebugDraw.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2009 Erwin Coumans 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, +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. @@ -21,7 +21,7 @@ subject to the following restrictions: ///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations. ///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld. -///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum. +///A class that implements the btIDebugDraw interface will need to provide non-empty implementations of the the drawLine and getDebugMode methods at a minimum. ///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1] class btIDebugDraw { diff --git a/thirdparty/bullet/LinearMath/btQuickprof.cpp b/thirdparty/bullet/LinearMath/btQuickprof.cpp index 86fd1d7812..33b51eb763 100644 --- a/thirdparty/bullet/LinearMath/btQuickprof.cpp +++ b/thirdparty/bullet/LinearMath/btQuickprof.cpp @@ -720,6 +720,9 @@ void btLeaveProfileZoneDefault() #define BT_HAVE_TLS 1 #elif __linux__ #define BT_HAVE_TLS 1 +#elif defined(__FreeBSD__) || defined(__NetBSD__) + // TODO: At the moment disabling purposely OpenBSD, albeit tls support exists but not fully functioning + #define BT_HAVE_TLS 1 #endif // __thread is broken on Andorid clang until r12b. See 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..b239217bb6 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 317 inline int btGetVersion() { diff --git a/thirdparty/bullet/LinearMath/btSerializer.h b/thirdparty/bullet/LinearMath/btSerializer.h index 2ee712047f..f18442f23d 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] = '1'; + buffer[11] = '7'; } virtual void startSerialization() @@ -499,7 +499,6 @@ public: writeDNA(); //if we didn't pre-allocate a buffer, we need to create a contiguous buffer now - int mysize = 0; if (!m_totalSize) { if (m_buffer) @@ -511,14 +510,12 @@ public: unsigned char* currentPtr = m_buffer; writeHeader(m_buffer); currentPtr += BT_HEADER_LENGTH; - mysize += BT_HEADER_LENGTH; for (int i = 0; i < m_chunkPtrs.size(); i++) { int curLength = sizeof(btChunk) + m_chunkPtrs[i]->m_length; memcpy(currentPtr, m_chunkPtrs[i], curLength); btAlignedFree(m_chunkPtrs[i]); currentPtr += curLength; - mysize += curLength; } } diff --git a/thirdparty/bullet/VERSION.txt b/thirdparty/bullet/VERSION.txt new file mode 100644 index 0000000000..78c8a7428a --- /dev/null +++ b/thirdparty/bullet/VERSION.txt @@ -0,0 +1 @@ +3.17 diff --git a/thirdparty/bullet/patches/bullet-fix-warnings.patch b/thirdparty/bullet/patches/bullet-fix-warnings.patch new file mode 100644 index 0000000000..69cde1b16e --- /dev/null +++ b/thirdparty/bullet/patches/bullet-fix-warnings.patch @@ -0,0 +1,42 @@ +diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.h b/thirdparty/bullet/BulletSoftBody/btSoftBody.h +index f578487b8c..dfde8fd1e4 100644 +--- a/thirdparty/bullet/BulletSoftBody/btSoftBody.h ++++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.h +@@ -1317,8 +1317,8 @@ public: + } + for (int k = 0; k < m_faceNodeContacts.size(); ++k) + { +- int i = indices[k]; +- btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i]; ++ int idx = indices[k]; ++ btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[idx]; + btSoftBody::Node* node = c.m_node; + btSoftBody::Face* face = c.m_face; + const btVector3& w = c.m_bary; +diff --git a/thirdparty/bullet/LinearMath/btSerializer.h b/thirdparty/bullet/LinearMath/btSerializer.h +index ce4fc34e20..11592d2ccd 100644 +--- a/thirdparty/bullet/LinearMath/btSerializer.h ++++ b/thirdparty/bullet/LinearMath/btSerializer.h +@@ -499,7 +499,6 @@ public: + writeDNA(); + + //if we didn't pre-allocate a buffer, we need to create a contiguous buffer now +- int mysize = 0; + if (!m_totalSize) + { + if (m_buffer) +@@ -511,14 +510,12 @@ public: + unsigned char* currentPtr = m_buffer; + writeHeader(m_buffer); + currentPtr += BT_HEADER_LENGTH; +- mysize += BT_HEADER_LENGTH; + for (int i = 0; i < m_chunkPtrs.size(); i++) + { + int curLength = sizeof(btChunk) + m_chunkPtrs[i]->m_length; + memcpy(currentPtr, m_chunkPtrs[i], curLength); + btAlignedFree(m_chunkPtrs[i]); + currentPtr += curLength; +- mysize += curLength; + } + } + |