maxmustermann
/
godot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

bullet: Sync with upstream 3.17 

Stop include Bullet headers using `-isystem` for GCC/Clang as it misleads
SCons into not properly rebuilding all files when headers change.

This means we also need to make sure Bullet builds without warning, and
current version fares fairly well, there were just a couple to fix (patch
included).

Increase minimum version for distro packages to 2.90 (this was never released
as the "next" version after 2.89 was 3.05... but that covers it too).
This commit is contained in:
Rémi Verschelde 2021-09-29 15:47:08 +02:00
parent 9b4e62d78f
commit b7901c773c
No known key found for this signature in database
GPG Key ID: C3336907360768E1
107 changed files with 10806 additions and 6111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
modules/bullet/SCsub
View File

@ -10,7 +10,7 @@ env_bullet = env_modules.Clone()
thirdparty_obj = []
if env["builtin_bullet"]:
# Build only version 2 for now (as of 2.89)
# Build only "Bullet2" API (not "Bullet3" folders).
# Sync file list with relevant upstream CMakeLists.txt for each folder.
thirdparty_dir = "#thirdparty/bullet/"
@ -177,6 +177,7 @@ if env["builtin_bullet"]:
"BulletSoftBody/btDeformableContactProjection.cpp",
"BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp",
"BulletSoftBody/btDeformableContactConstraint.cpp",
"BulletSoftBody/poly34.cpp",
# clew
"clew/clew.c",
# LinearMath
@ -186,6 +187,7 @@ if env["builtin_bullet"]:
"LinearMath/btGeometryUtil.cpp",
"LinearMath/btPolarDecomposition.cpp",
"LinearMath/btQuickprof.cpp",
"LinearMath/btReducedVector.cpp",
"LinearMath/btSerializer.cpp",
"LinearMath/btSerializer64.cpp",
"LinearMath/btThreads.cpp",
@ -197,13 +199,7 @@ if env["builtin_bullet"]:
thirdparty_sources = [thirdparty_dir + file for file in bullet2_src]
# Treat Bullet headers as system headers to avoid raising warnings. Not supported on MSVC.
if not env.msvc:
env_bullet.Append(CPPFLAGS=["-isystem", Dir(thirdparty_dir).path])
else:
env_bullet.Prepend(CPPPATH=[thirdparty_dir])
# if env['target'] == "debug" or env['target'] == "release_debug":
# env_bullet.Append(CPPDEFINES=['BT_DEBUG'])
env_bullet.Prepend(CPPPATH=[thirdparty_dir])
env_bullet.Append(CPPDEFINES=["BT_USE_OLD_DAMPING_METHOD"])

8
platform/server/detect.py
View File

@ -149,15 +149,17 @@ def configure(env):
env.ParseConfig("pkg-config libpng16 --cflags --libs")
if not env["builtin_bullet"]:
# We need at least version 2.89
# We need at least version 2.90
min_bullet_version = "2.90"
import subprocess
bullet_version = subprocess.check_output(["pkg-config", "bullet", "--modversion"]).strip()
if str(bullet_version) < "2.89":
if str(bullet_version) < min_bullet_version:
# Abort as system bullet was requested but too old
print(
"Bullet: System version {0} does not match minimal requirements ({1}). Aborting.".format(
bullet_version, "2.89"
bullet_version, min_bullet_version
)
)
sys.exit(255)

8
platform/x11/detect.py
View File

@ -239,15 +239,17 @@ def configure(env):
env.ParseConfig("pkg-config libpng16 --cflags --libs")
if not env["builtin_bullet"]:
# We need at least version 2.89
# We need at least version 2.90
min_bullet_version = "2.90"
import subprocess
bullet_version = subprocess.check_output(["pkg-config", "bullet", "--modversion"]).strip()
if str(bullet_version) < "2.89":
if str(bullet_version) < min_bullet_version:
# Abort as system bullet was requested but too old
print(
"Bullet: System version {0} does not match minimal requirements ({1}). Aborting.".format(
bullet_version, "2.89"
bullet_version, min_bullet_version
)
)
sys.exit(255)

8
thirdparty/README.md vendored
View File

@ -8,13 +8,15 @@ readability.
## bullet
- Upstream: https://github.com/bulletphysics/bullet3
- Version: 2.89 (830f0a9565b1829a07e21e2f16be2aa9966bd28c, 2019)
- Version: 3.17 (ebe1916b90acae8b13cd8c6b637d8327cdc64e94, 2021)
- License: zlib
Files extracted from upstream source:
- src/* apart from CMakeLists.txt and premake4.lua files
- LICENSE.txt
- `src/*` apart from CMakeLists.txt and premake4.lua files
- `LICENSE.txt`, and `VERSION` as `VERSION.txt`
Includes a warning fix which should be upstreamed soon (see patch in `patches`).
## certs

34
thirdparty/bullet/0001-old-damping-def.patch vendored
View File

@ -1,34 +0,0 @@
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
index 9e8705b001..f1b50b39c8 100644
--- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -136,8 +136,13 @@ void btRigidBody::setGravity(const btVector3& acceleration)
void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
{
- m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
- m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+ m_linearDamping = btMax(lin_damping, btScalar(0.0));
+ m_angularDamping = btMax(ang_damping, btScalar(0.0));
+#else
+ m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
+ m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
+#endif
}
///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
@@ -146,10 +151,9 @@ void btRigidBody::applyDamping(btScalar timeStep)
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
-//#define USE_OLD_DAMPING_METHOD 1
-#ifdef USE_OLD_DAMPING_METHOD
- m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
- m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+#ifdef BT_USE_OLD_DAMPING_METHOD
+ m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
+ m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
#else
m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);

9
thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp vendored
View File

@ -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);

6
thirdparty/bullet/Bullet3Serialize/Bullet2FileLoader/b3File.cpp vendored
View File

@ -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);

2
thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvt.h vendored
View File

@ -203,8 +203,8 @@ struct btDbvntNode
btDbvntNode(const btDbvtNode* n)
: volume(n->volume)
, angle(0)
, normal(0,0,0)
, angle(0)
, data(n->data)
{
childs[0] = 0;

11
thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h vendored
View File

@ -61,7 +61,8 @@ public:
virtual void cleanOverlappingPair(btBroadphasePair& pair, btDispatcher* dispatcher) = 0;
virtual int getNumOverlappingPairs() const = 0;
virtual bool needsBroadphaseCollision(btBroadphaseProxy * proxy0, btBroadphaseProxy * proxy1) const = 0;
virtual btOverlapFilterCallback* getOverlapFilterCallback() = 0;
virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy, btDispatcher* dispatcher) = 0;
virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
@ -380,6 +381,14 @@ public:
{
}
bool needsBroadphaseCollision(btBroadphaseProxy*, btBroadphaseProxy*) const
{
return true;
}
btOverlapFilterCallback* getOverlapFilterCallback()
{
return 0;
}
virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
{
}

14
thirdparty/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp vendored
View File

@ -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;
}
/*
@ -468,7 +464,7 @@ void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCall
#ifdef RAYAABB2
btVector3 rayDir = (rayTarget - raySource);
rayDir.normalize();
rayDir.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17
lambda_max = rayDir.dot(rayTarget - raySource);
///what about division by zero? --> just set rayDirection[i] to 1.0
btVector3 rayDirectionInverse;
@ -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
@ -554,7 +548,7 @@ void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback*
#ifdef RAYAABB2
btVector3 rayDirection = (rayTarget - raySource);
rayDirection.normalize();
rayDirection.safeNormalize();// stephengold changed normalize to safeNormalize 2020-02-17
lambda_max = rayDirection.dot(rayTarget - raySource);
///what about division by zero? --> just set rayDirection[i] to 1.0
rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0];
@ -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

6
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h vendored
View File

@ -46,8 +46,6 @@ protected:
btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr;
btManifoldResult m_defaultManifoldResult;
btNearCallback m_nearCallback;
btPoolAllocator* m_collisionAlgorithmPoolAllocator;
@ -95,11 +93,15 @@ public:
btPersistentManifold* getManifoldByIndexInternal(int index)
{
btAssert(index>=0);
btAssert(index<m_manifoldsPtr.size());
return m_manifoldsPtr[index];
}
const btPersistentManifold* getManifoldByIndexInternal(int index) const
{
btAssert(index>=0);
btAssert(index<m_manifoldsPtr.size());
return m_manifoldsPtr[index];
}

22
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp vendored
View File

@ -28,6 +28,7 @@ subject to the following restrictions:
btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize)
: btCollisionDispatcher(config)
{
m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads());
m_batchUpdating = false;
m_grainSize = grainSize; // iterations per task
}
@ -65,6 +66,10 @@ btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionO
manifold->m_index1a = m_manifoldsPtr.size();
m_manifoldsPtr.push_back(manifold);
}
else
{
m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold);
}
return manifold;
}
@ -121,7 +126,7 @@ struct CollisionDispatcherUpdater : public btIParallelForBody
void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher)
{
int pairCount = pairCache->getNumOverlappingPairs();
const int pairCount = pairCache->getNumOverlappingPairs();
if (pairCount == 0)
{
return;
@ -136,16 +141,17 @@ void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache*
btParallelFor(0, pairCount, m_grainSize, updater);
m_batchUpdating = false;
// reconstruct the manifolds array to ensure determinism
m_manifoldsPtr.resizeNoInitialize(0);
btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
for (int i = 0; i < pairCount; ++i)
// merge new manifolds, if any
for (int i = 0; i < m_batchManifoldsPtr.size(); ++i)
{
if (btCollisionAlgorithm* algo = pairs[i].m_algorithm)
btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchManifoldsPtr[i];
for (int j = 0; j < batchManifoldsPtr.size(); ++j)
{
algo->getAllContactManifolds(m_manifoldsPtr);
m_manifoldsPtr.push_back(batchManifoldsPtr[j]);
}
batchManifoldsPtr.resizeNoInitialize(0);
}
// update the indices (used when releasing manifolds)

1
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h vendored
View File

@ -30,6 +30,7 @@ public:
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE;
protected:
btAlignedObjectArray<btAlignedObjectArray<btPersistentManifold*> > m_batchManifoldsPtr;
bool m_batchUpdating;
int m_grainSize;
};

14
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h vendored
View File

@ -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)

6
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp vendored
View File

@ -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);
}
};

7
thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp vendored
View File

@ -139,7 +139,12 @@ public:
if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
{
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, childTrans, -1, index);
btTransform preTransform = childTrans;
if (this->m_compoundColObjWrap->m_preTransform)
{
preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform));
}
btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index);
btCollisionAlgorithm* algo = 0;
bool allocatedAlgorithm = false;

8
thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp vendored
View File

@ -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);

18
thirdparty/bullet/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp vendored
View File

@ -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)
{

13
thirdparty/bullet/BulletCollision/CollisionShapes/btCollisionShape.h vendored
View File

@ -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)

141
thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp vendored
View File

@ -17,27 +17,73 @@ 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();
}
}

57
thirdparty/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h vendored
View File

@ -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
#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H

9
thirdparty/bullet/BulletCollision/CollisionShapes/btOptimizedBvh.cpp vendored
View File

@ -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);

5
thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp vendored
View File

@ -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;

10
thirdparty/bullet/BulletCollision/Gimpact/btGImpactShape.h vendored
View File

@ -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

3
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp vendored
View File

@ -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
{

2
thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp vendored
View File

@ -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);

10
thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h vendored
View File

@ -46,7 +46,9 @@ struct btContactSolverInfoData
btScalar m_sor; //successive over-relaxation term
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_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
@ -67,6 +69,7 @@ struct btContactSolverInfoData
bool m_jointFeedbackInWorldSpace;
bool m_jointFeedbackInJointFrame;
int m_reportSolverAnalytics;
int m_numNonContactInnerIterations;
};
struct btContactSolverInfo : public btContactSolverInfoData
@ -82,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.);
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.);
@ -104,6 +109,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_jointFeedbackInWorldSpace = false;
m_jointFeedbackInJointFrame = false;
m_reportSolverAnalytics = 0;
m_numNonContactInnerIterations = 1; // the number of inner iterations for solving motor constraint in a single iteration of the constraint solve
}
};

5
thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp vendored
View File

@ -876,7 +876,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
// will we not request a velocity with the wrong direction ?
// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
// so the sign of the force that is really matters
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
else
info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
btScalar minf = f < fd ? f : fd;
btScalar maxf = f < fd ? fd : f;

1
thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h vendored
View File

@ -265,6 +265,7 @@ enum bt6DofFlags2
BT_6DOF_FLAGS_ERP_STOP2 = 2,
BT_6DOF_FLAGS_CFM_MOTO2 = 4,
BT_6DOF_FLAGS_ERP_MOTO2 = 8,
BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16)
};
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis

12
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp vendored
View File

@ -14,7 +14,9 @@ subject to the following restrictions:
*/
//#define COMPUTE_IMPULSE_DENOM 1
//#define BT_ADDITIONAL_DEBUG
#ifdef BT_DEBUG
# define BT_ADDITIONAL_DEBUG
#endif
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
@ -690,8 +692,10 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
{
#if BT_THREADSAFE
int solverBodyId = -1;
bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
if (isRigidBodyType && !body.isStaticOrKinematicObject())
const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
const bool isKinematic = body.isKinematicObject();
if (isRigidBodyType && !isStaticOrKinematic)
{
// dynamic body
// Dynamic bodies can only be in one island, so it's safe to write to the companionId
@ -704,7 +708,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
body.setCompanionId(solverBodyId);
}
}
else if (isRigidBodyType && body.isKinematicObject())
else if (isRigidBodyType && isKinematic)
{
//
// NOTE: must test for kinematic before static because some kinematic objects also

8
thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp vendored
View File

@ -800,6 +800,14 @@ public:
///don't do CCD when the collision filters are not matching
if (!ClosestConvexResultCallback::needsCollision(proxy0))
return false;
if (m_pairCache->getOverlapFilterCallback()) {
btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle();
bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1);
if (!collides)
{
return false;
}
}
btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject;

3
thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp vendored
View File

@ -384,6 +384,9 @@ void btRigidBody::integrateVelocities(btScalar step)
{
m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
}
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_angularVelocity);
#endif
}
btQuaternion btRigidBody::getOrientation() const

53
thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.h vendored
View File

@ -305,6 +305,9 @@ public:
void applyTorque(const btVector3& torque)
{
m_totalTorque += torque * m_angularFactor;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_totalTorque);
#endif
}
void applyForce(const btVector3& force, const btVector3& rel_pos)
@ -316,11 +319,17 @@ public:
void applyCentralImpulse(const btVector3& impulse)
{
m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_linearVelocity);
#endif
}
void applyTorqueImpulse(const btVector3& torque)
{
m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_angularVelocity);
#endif
}
void applyImpulse(const btVector3& impulse, const btVector3& rel_pos)
@ -347,12 +356,12 @@ public:
}
}
btVector3 getPushVelocity()
btVector3 getPushVelocity() const
{
return m_pushVelocity;
}
btVector3 getTurnVelocity()
btVector3 getTurnVelocity() const
{
return m_turnVelocity;
}
@ -361,20 +370,46 @@ public:
{
m_pushVelocity = v;
}
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
void clampVelocity(btVector3& v) const {
v.setX(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
);
v.setY(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
);
v.setZ(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
);
}
#endif
void setTurnVelocity(const btVector3& v)
{
m_turnVelocity = v;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_turnVelocity);
#endif
}
void applyCentralPushImpulse(const btVector3& impulse)
{
m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_pushVelocity);
#endif
}
void applyTorqueTurnImpulse(const btVector3& torque)
{
m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_turnVelocity);
#endif
}
void clearForces()
@ -408,12 +443,18 @@ public:
{
m_updateRevision++;
m_linearVelocity = lin_vel;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_linearVelocity);
#endif
}
inline void setAngularVelocity(const btVector3& ang_vel)
{
m_updateRevision++;
m_angularVelocity = ang_vel;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
clampVelocity(m_angularVelocity);
#endif
}
btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
@ -424,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)
{

2
thirdparty/bullet/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp vendored
View File

@ -171,6 +171,8 @@ void btSimulationIslandManagerMt::initIslandPools()
btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
{
btAssert(id >= 0);
btAssert(id < m_lookupIslandFromId.size());
Island* island = m_lookupIslandFromId[id];
if (island == NULL)
{

627
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.cpp vendored
View File

@ -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);
@ -583,52 +589,6 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
}
}
btScalar btMultiBody::getKineticEnergy() const
{
int num_links = getNumLinks();
// TODO: would be better not to allocate memory here
btAlignedObjectArray<btVector3> omega;
omega.resize(num_links + 1);
btAlignedObjectArray<btVector3> vel;
vel.resize(num_links + 1);
compTreeLinkVelocities(&omega[0], &vel[0]);
// we will do the factor of 0.5 at the end
btScalar result = m_baseMass * vel[0].dot(vel[0]);
result += omega[0].dot(m_baseInertia * omega[0]);
for (int i = 0; i < num_links; ++i)
{
result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
}
return 0.5f * result;
}
btVector3 btMultiBody::getAngularMomentum() const
{
int num_links = getNumLinks();
// TODO: would be better not to allocate memory here
btAlignedObjectArray<btVector3> omega;
omega.resize(num_links + 1);
btAlignedObjectArray<btVector3> vel;
vel.resize(num_links + 1);
btAlignedObjectArray<btQuaternion> rot_from_world;
rot_from_world.resize(num_links + 1);
compTreeLinkVelocities(&omega[0], &vel[0]);
rot_from_world[0] = m_baseQuat;
btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));
for (int i = 0; i < num_links; ++i)
{
rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
}
return result;
}
void btMultiBody::clearConstraintForces()
{
@ -717,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(
@ -842,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();
}
@ -918,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)
@ -955,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());
@ -981,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;
@ -1093,7 +1085,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
// Second 'upward' loop
// (part of TreeForwardDynamics in Mirtich)
if (m_fixedBase)
if (isBaseStaticOrKinematic())
{
spatAcc[0].setZero();
}
@ -1127,22 +1119,24 @@ 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);
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];
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];
}
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];
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)
{
m_internalNeedsJointFeedback = true;
@ -1420,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++)
{
@ -1478,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();
}
@ -1497,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;
@ -1540,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();
}
@ -1553,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;
@ -1596,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)
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)
// 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];
// 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)
@ -1663,26 +1664,29 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
//
btScalar *pBaseQuat;
if(!isBaseKinematic())
{
btScalar *pBaseQuat;
// reset to current orientation
for (int i = 0; i < 4; ++i)
{
m_baseQuat_interpolate[i] = m_baseQuat[i];
}
pBaseQuat = m_baseQuat_interpolate;
// 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();
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)
@ -1690,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)
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[0] = m_links[i].m_jointPos[0];
break;
}
case btMultibodyLink::eSpherical:
{
for (int j = 0; j < 4; ++j)
{
pJointPos[j] = m_links[i].m_jointPos[j];
}
break;
}
case btMultibodyLink::ePlanar:
{
for (int j = 0; j < 3; ++j)
{
pJointPos[j] = m_links[i].m_jointPos[j];
}
break;
}
default:
break;
}
}
else
{
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
btScalar *pJointVel = getJointVelMultiDof(i);
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:
switch (m_links[i].m_jointType)
{
for (int j = 0; j < 3; ++j)
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:
{
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:
{
}
}
@ -1749,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)
@ -1809,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();
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());
//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;
@ -1834,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:
{
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
btScalar *pJointPos;
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
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:
switch (m_links[i].m_jointType)
{
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;
break;
}
default:
{
}
}
}
@ -2061,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();
}
@ -2181,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())
{
@ -2328,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();
}

114
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBody.h vendored
View File

@ -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];
@ -307,13 +342,6 @@ public:
//
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
//
// calculate kinetic energy and angular momentum
// useful for debugging.
//
btScalar getKineticEnergy() const;
btVector3 getAngularMomentum() const;
//
// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
@ -404,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);
@ -497,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
@ -660,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:
@ -681,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
@ -718,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;
@ -736,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;
@ -758,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

8
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp vendored
View File

@ -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;

25
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.h vendored
View File

@ -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) {}

31
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp vendored
View File

@ -30,23 +30,28 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
//solve featherstone non-contact constraints
btScalar nonContactResidual = 0;
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
{
int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
// reset the nonContactResdual to 0 at start of each inner iteration
nonContactResidual = 0;
for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
{
int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
nonContactResidual = btMax(nonContactResidual, residual * residual);
if (constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
if (constraint.m_multiBodyB)
constraint.m_multiBodyB->setPosUpdated(false);
if (constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
if (constraint.m_multiBodyB)
constraint.m_multiBodyB->setPosUpdated(false);
}
}
leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
//solve featherstone normal contact
for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
@ -1250,7 +1255,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
{
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
@ -1270,7 +1275,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
// return;
//only a single rollingFriction per manifold
int rollingFriction = 1;
int rollingFriction = 4;
for (int j = 0; j < manifold->getNumContacts(); j++)
{

23
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp vendored
View File

@ -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)
//{

2
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h vendored
View File

@ -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

4
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp vendored
View File

@ -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),

2
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp vendored
View File

@ -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),

2
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp vendored
View File

@ -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)
{

16
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h vendored
View File

@ -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

4
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp vendored
View File

@ -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.),

3
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLink.h vendored
View File

@ -295,6 +295,9 @@ struct btMultibodyLink
}
}
}
};
#endif //BT_MULTIBODY_LINK_H

17
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h vendored
View File

@ -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)

4
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp vendored
View File

@ -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),

4
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp vendored
View File

@ -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),

22
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp vendored
View File

@ -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;

49
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h vendored
View File

@ -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)

4
thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp vendored
View File

@ -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

9
thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h vendored
View File

@ -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);

144
thirdparty/bullet/BulletSoftBody/btCGProjection.h vendored
View File

@ -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);
}
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;
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()
{
}
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 */

231
thirdparty/bullet/BulletSoftBody/btConjugateGradient.h vendored
View File

@ -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;
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;
}
typedef btAlignedObjectArray<btVector3> TVStack;
typedef btKrylovSolver<MatrixX> Base;
TVStack r, p, z, temp;
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;
}
public:
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;
}
void reinitialize(const TVStack& b)
{
r.resize(b.size());
p.resize(b.size());
z.resize(b.size());
temp.resize(b.size());
}
};
#endif /* btConjugateGradient_h */

112
thirdparty/bullet/BulletSoftBody/btConjugateResidual.h vendored Normal file
View File

@ -0,0 +1,112 @@
/*
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_CONJUGATE_RESIDUAL_H
#define BT_CONJUGATE_RESIDUAL_H
#include "btKrylovSolver.h"
template <class MatrixX>
class btConjugateResidual : public btKrylovSolver<MatrixX>
{
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)
: 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 */

350
thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp vendored
View File

@ -17,181 +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_preconditioner = new MassPreconditioner(m_softBodies);
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_preconditioner;
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;
}
}
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);
}
}
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()
void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
{
m_projection.setConstraints();
m_projection.setConstraints(infoGlobal);
}
void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
{
m_projection.applyDynamicFriction(r);
m_projection.applyDynamicFriction(r);
}

258
thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h vendored
View File

@ -15,11 +15,12 @@
#ifndef BT_BACKWARD_EULER_OBJECTIVE_H
#define BT_BACKWARD_EULER_OBJECTIVE_H
#include "btConjugateGradient.h"
//#include "btConjugateGradient.h"
#include "btDeformableLagrangianForce.h"
#include "btDeformableMassSpringForce.h"
#include "btDeformableGravityForce.h"
#include "btDeformableCorotatedForce.h"
#include "btDeformableMousePickingForce.h"
#include "btDeformableLinearElasticityForce.h"
#include "btDeformableNeoHookeanForce.h"
#include "btDeformableContactProjection.h"
@ -30,105 +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;
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();
// 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);
}
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v);
// 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;
}
virtual ~btDeformableBackwardEulerObjective();
// Calculate the total potential energy in the system
btScalar totalEnergy(btScalar dt);
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 */

755
thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp vendored
View File

@ -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_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
, m_lineSearch(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);
computeStep(m_dv, m_residual);
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)
{
m_cg.solve(*m_objective, ddv, residual);
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);
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();
}
if (dt > 0)
{
m_dt = dt;
}
m_objective->reinitialize(nodeUpdated, dt);
updateSoftBodies();
}
void btDeformableBodySolver::setConstraints()
void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("setConstraint");
m_objective->setConstraints();
BT_PROFILE("setConstraint");
m_objective->setConstraints(infoGlobal);
}
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveContactConstraints");
btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
return maxSquaredResidual;
}
btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveSplitImpulse");
return m_objective->m_projection.solveSplitImpulse(infoGlobal);
}
void btDeformableBodySolver::splitImpulseSetup(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] + psb->m_nodes[j].m_vsplit;
++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)
{
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;
}
BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
int i, ni;
/* Nodes */
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
{
btSoftBody::Node& n = psb->m_nodes[i];
btVector3 points[2] = {n.m_x, n.m_q};
vol = btDbvtVolume::FromPoints(points, 2);
vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
psb->m_ndbvt.update(n.m_leaf, vol);
}
/* 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();
}
}
if (!psb->m_fdbvt.empty())
{
for (int i = 0; i < psb->m_faces.size(); ++i)
{
btSoftBody::Face& f = psb->m_faces[i];
btVector3 points[6] = {f.m_n[0]->m_x, f.m_n[0]->m_q,
f.m_n[1]->m_x, f.m_n[1]->m_q,
f.m_n[2]->m_x, f.m_n[2]->m_q};
vol = btDbvtVolume::FromPoints(points, 6);
vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
psb->m_fdbvt.update(f.m_leaf, vol);
}
}
/* 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);
/* 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;
}

246
thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h vendored
View File

@ -16,149 +16,145 @@
#ifndef BT_DEFORMABLE_BODY_SOLVERS_H
#define BT_DEFORMABLE_BODY_SOLVERS_H
#include "btSoftBodySolvers.h"
#include "btDeformableBackwardEulerObjective.h"
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "btConjugateResidual.h"
#include "btConjugateGradient.h"
struct btCollisionObjectWrapper;
class btDeformableBackwardEulerObjective;
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
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;
btDeformableBodySolver();
virtual ~btDeformableBodySolver();
virtual SolverTypes getSolverType() const
{
return DEFORMABLE_SOLVER;
}
// handles data related to objective function
btDeformableBackwardEulerObjective* m_objective;
bool m_useProjection;
// update soft body normals
virtual void updateSoftBodies();
// solve the momentum equation
virtual void solveDeformableConstraints(btScalar solverdt);
// solve the contact between deformable and rigid as well as among deformables
btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies);
// solve the position error between deformable and rigid as well as among deformables;
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
// set up the position error in split impulse
void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
btDeformableBodySolver();
// resize/clear data structures
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
// set up contact constraints
void setConstraints();
// 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 ~btDeformableBodySolver();
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
virtual SolverTypes getSolverType() const
{
return DEFORMABLE_SOLVER;
}
// 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);
}
// update soft body normals
virtual void updateSoftBodies();
// 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) {}
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 */

1061
thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp vendored
View File

File diff suppressed because it is too large Load Diff

450
thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h vendored
View File

@ -21,55 +21,49 @@
class btDeformableContactConstraint
{
public:
// True if the friction is static
// False if the friction is dynamic
bool m_static;
// normal of the contact
btVector3 m_normal;
btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal)
{
}
btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal)
{
}
btDeformableContactConstraint(const btDeformableContactConstraint& other)
: m_static(other.m_static)
, m_normal(other.m_normal)
{
}
btDeformableContactConstraint(){}
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() = 0;
// solve the position error by applying an inelastic impulse that changes only the position (not velocity)
virtual btScalar solveSplitImpulse(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;
// apply position based impulse to the soft body node and/or face involved
virtual void applySplitImpulse(const btVector3& impulse) = 0;
// scale the penetration depth by erp
virtual void setPenetrationScale(btScalar scale) = 0;
// 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(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal) : m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
{
}
btDeformableContactConstraint() {}
btDeformableContactConstraint(const btDeformableContactConstraint& other)
: 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;
};
//
@ -77,51 +71,41 @@ public:
class btDeformableStaticConstraint : public btDeformableContactConstraint
{
public:
const btSoftBody::Node* m_node;
btDeformableStaticConstraint(){}
btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0))
{
}
btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
: m_node(other.m_node)
, btDeformableContactConstraint(other)
{
}
virtual ~btDeformableStaticConstraint(){}
virtual btScalar solveConstraint()
{
return 0;
}
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
return 0;
}
btSoftBody::Node* m_node;
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 applySplitImpulse(const btVector3& impulse){}
virtual void setPenetrationScale(btScalar scale){}
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) {}
};
//
@ -129,65 +113,67 @@ public:
class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
{
public:
const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
btDeformableNodeAnchorConstraint(){}
btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
virtual ~btDeformableNodeAnchorConstraint()
{
}
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
// todo xuchenhan@
return 0;
}
// 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 applySplitImpulse(const btVector3& impulse)
{
// todo xuchenhan@
};
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(){}
btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c);
btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
virtual ~btDeformableRigidContactConstraint()
{
}
// object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const;
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(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;
};
//
@ -195,31 +181,34 @@ public:
class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint
{
public:
// the deformable node in contact
const btSoftBody::Node* m_node;
btDeformableNodeRigidContactConstraint(){}
btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact);
btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
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);
virtual void applySplitImpulse(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);
};
//
@ -227,29 +216,33 @@ public:
class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
{
public:
const btSoftBody::Face* m_face;
btDeformableFaceRigidContactConstraint(){}
btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
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);
virtual void applySplitImpulse(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);
};
//
@ -257,46 +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(){}
btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact);
virtual ~btDeformableFaceNodeContactConstraint(){}
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
// todo: xuchenhan@
return 0;
}
// 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 applySplitImpulse(const btVector3& impulse)
{
// todo xuchenhan@
}
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 */

518
thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp vendored
View File

@ -17,7 +17,7 @@
#include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm>
#include <cmath>
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{
btScalar residualSquare = 0;
for (int i = 0; i < numDeformableBodies; ++i)
@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
{
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
{
btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
{
btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
@ -58,57 +58,36 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
return residualSquare;
}
void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
{
for (int i = 0; i < m_softBodies.size(); ++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)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
}
}
}
btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
{
btScalar residualSquare = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
for (int i = 0; i < numDeformableBodies; ++i)
{
// node constraints
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
for (int j = 0; j < m_softBodies.size(); ++j)
{
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
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);
}
}
// anchor constraints
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{
btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face constraints
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
return residualSquare;
}
void btDeformableContactProjection::setConstraints()
void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("setConstraints");
for (int i = 0; i < m_softBodies.size(); ++i)
@ -124,11 +103,11 @@ void btDeformableContactProjection::setConstraints()
{
if (psb->m_nodes[j].m_im == 0)
{
btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
m_staticConstraints[i].push_back(static_constraint);
}
}
// set up deformable anchors
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{
@ -139,10 +118,10 @@ void btDeformableContactProjection::setConstraints()
continue;
}
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
btDeformableNodeAnchorConstraint constraint(anchor);
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)
{
@ -152,18 +131,10 @@ void btDeformableContactProjection::setConstraints()
{
continue;
}
btDeformableNodeRigidContactConstraint constraint(contact);
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);
}
btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
m_nodeRigidConstraints[i].push_back(constraint);
}
// set Deformable Face vs. Rigid constraint
for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
{
@ -173,38 +144,15 @@ void btDeformableContactProjection::setConstraints()
{
continue;
}
btDeformableFaceRigidContactConstraint constraint(contact);
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);
}
}
// set Deformable Face vs. Deformable Node constraint
for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
{
const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
btDeformableFaceNodeContactConstraint constraint(contact);
btVector3 va = constraint.getVa();
btVector3 vb = constraint.getVb();
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, contact.m_normal);
if (dn > -SIMD_EPSILON)
{
m_deformableConstraints[i].push_back(constraint);
}
btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
m_faceRigidConstraints[i].push_back(constraint);
}
}
}
void btDeformableContactProjection::project(TVStack& x)
{
#ifndef USE_MGS
const int dim = 3;
for (int index = 0; index < m_projectionsDict.size(); ++index)
{
@ -224,7 +172,6 @@ void btDeformableContactProjection::project(TVStack& x)
if (free_dir.safeNorm() < SIMD_EPSILON)
{
x[i] -= x[i].dot(dir0) * dir0;
x[i] -= x[i].dot(dir1) * dir1;
}
else
{
@ -239,14 +186,27 @@ void btDeformableContactProjection::project(TVStack& x)
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];
}
#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));
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];
@ -257,6 +217,7 @@ void btDeformableContactProjection::setProjection()
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);
@ -273,6 +234,7 @@ void btDeformableContactProjection::setProjection()
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);
@ -289,42 +251,53 @@ void btDeformableContactProjection::setProjection()
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
int index = m_nodeRigidConstraints[i][j].m_node->index;
if (m_nodeRigidConstraints[i][j].m_static)
m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
if (m_nodeRigidConstraints[i][j].m_binding)
{
if (m_projectionsDict.find(index) == NULL)
if (m_nodeRigidConstraints[i][j].m_static)
{
m_projectionsDict.insert(index, units);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
if (m_projectionsDict.find(index) == NULL)
{
projections.push_back(units[k]);
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);
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)
{
const btSoftBody::Node* node = face->m_n[k];
btSoftBody::Node* node = face->m_n[k];
int index = node->index;
if (m_faceRigidConstraints[i][j].m_static)
{
@ -335,9 +308,9 @@ void btDeformableContactProjection::setProjection()
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
for (int l = 0; l < 3; ++l)
{
projections.push_back(units[k]);
projections.push_back(units[l]);
}
}
}
@ -357,80 +330,236 @@ void btDeformableContactProjection::setProjection()
}
}
}
for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
{
const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face;
for (int k = 0; k < 3; ++k)
{
const btSoftBody::Node* node = face->m_n[k];
int index = node->index;
if (m_deformableConstraints[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_deformableConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_deformableConstraints[i][j].m_normal);
}
}
}
const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node;
int index = node->index;
if (m_deformableConstraints[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_deformableConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_deformableConstraints[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;
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);
}
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);
v[l] = bary[k];
rv.m_vecs.push_back(v);
rv.sort();
}
m_projections.push_back(rv);
}
}
else
{
btReducedVector rv(dof);
for (int k = 0; k < 3; ++k)
{
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();
}
m_projections.push_back(rv);
}
}
}
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]);
// 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_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)
{
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)
{
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);
}
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);
}
}
}
//
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{
for (int i = 0; i < m_softBodies.size(); ++i)
@ -442,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)
@ -455,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);
}
}
}
@ -467,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)
{
@ -475,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);
}
}
}
@ -492,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();
@ -502,8 +630,10 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
m_faceRigidConstraints[i].clear();
m_deformableConstraints[i].clear();
}
#ifndef USE_MGS
m_projectionsDict.clear();
#else
m_projections.clear();
#endif
m_lagrangeMultipliers.clear();
}

111
thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h vendored
View File

@ -21,30 +21,36 @@
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "btDeformableContactConstraint.h"
#include "LinearMath/btHashMap.h"
#include "LinearMath/btReducedVector.h"
#include "LinearMath/btModifiedGramSchmidt.h"
#include <vector>
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;
};
class btDeformableContactProjection
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
// // map from node index to static constraint
// btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// // map from node index to node rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// // map from node index to face rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// // map from node index to deformable constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// // map from node index to node anchor constraint
// btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
// map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
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;
#else
btAlignedObjectArray<btReducedVector> m_projections;
#endif
btAlignedObjectArray<LagrangeMultiplier> m_lagrangeMultipliers;
// map from node index to static constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
// map from node index to node rigid constraint
@ -55,36 +61,39 @@ public:
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
// map from node index to node anchor constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
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);
// solve the position error using split impulse
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
virtual void setConstraints();
// 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);
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 */

187
thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h vendored
View File

@ -21,105 +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 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 */

158
thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h vendored
View File

@ -21,85 +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 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 */

662
thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h vendored
View File

@ -22,343 +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_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;
// 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 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]);
}
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody*> m_softBodies;
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
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();
}
btDeformableLagrangianForce()
{
}
//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);
}
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 */

748
thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h vendored
View File

@ -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 */

498
thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h vendored
View File

@ -20,236 +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;
}
}
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 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;
virtual void addScaledForces(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
addScaledElasticForce(scale, force);
}
// 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;
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
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 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 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;
}
};
#endif /* btMassSpring_h */

162
thirdparty/bullet/BulletSoftBody/btDeformableMousePickingForce.h vendored Normal file
View File

@ -0,0 +1,162 @@
/*
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_MOUSE_PICKING_FORCE_H
#define BT_MOUSE_PICKING_FORCE_H
#include "btDeformableLagrangianForce.h"
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;
public:
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 */

207
thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp vendored
View File

@ -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
// 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);
// 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)))
{
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)))
{
#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;
}
}
}
}
}

46
thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h vendored
View File

@ -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;
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);
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, 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);
// 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);
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 */

1153
thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp vendored
View File

File diff suppressed because it is too large Load Diff

383
thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h vendored
View File

@ -36,130 +36,281 @@ 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_contact_iterations;
bool m_implicit;
bool m_lineSearch;
bool m_selfCollision;
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 removeSoftBody(btSoftBody* body);
void removeCollisionObject(btCollisionObject* collisionObject);
int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; }
void setupConstraints();
void solveMultiBodyConstraints();
void solveContactConstraints();
void sortConstraints();
void softBodySelfCollision();
void setImplicit(bool implicit)
{
m_implicit = implicit;
}
void setLineSearch(bool lineSearch)
{
m_lineSearch = lineSearch;
}
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;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
#else
//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
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);
#ifndef USE_BRUTEFORCE_RAYBROADPHASE
m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB);
#else
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);
}
}
};
#endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H

733
thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h vendored
View File

@ -23,353 +23,398 @@ subject to the following restrictions:
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda;
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;
}
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;
}
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();
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();
}
// 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);
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_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)
{
m_mu_damp = damping * m_mu;
m_lambda_damp = damping * m_lambda;
}
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_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;
}
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;
// 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)
{
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;
}
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;
}
// 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 */

107
thirdparty/bullet/BulletSoftBody/btKrylovSolver.h vendored Normal file
View File

@ -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 */

296
thirdparty/bullet/BulletSoftBody/btPreconditioner.h vendored
View File

@ -19,61 +19,267 @@
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 < b.size(); ++i)
{
b[i] = x[i] * m_inv_mass[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;
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
#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];
}
}
#else
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_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 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 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];
}
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];
}
}
#endif
};
#endif /* BT_PRECONDITIONER_H */

1727
thirdparty/bullet/BulletSoftBody/btSoftBody.cpp vendored
View File

File diff suppressed because it is too large Load Diff

540
thirdparty/bullet/BulletSoftBody/btSoftBody.h vendored
View File

@ -35,6 +35,8 @@ subject to the following restrictions:
//#else
#define btSoftBodyData btSoftBodyFloatData
#define btSoftBodyDataName "btSoftBodyFloatData"
static const btScalar OVERLAP_REDUCTION_FACTOR = 0.1;
static unsigned long seed = 243703;
//#endif //BT_USE_DOUBLE_PRECISION
class btBroadphaseInterface;
@ -161,14 +163,18 @@ public:
RVSmask = 0x000f, ///Rigid versus soft mask
SDF_RS = 0x0001, ///SDF based rigid vs soft
CL_RS = 0x0002, ///Cluster vs convex rigid vs soft
SDF_RD = 0x0003, ///DF based rigid vs deformable
SDF_RDF = 0x0004, ///DF based rigid vs deformable faces
SDF_RD = 0x0004, ///rigid vs deformable
SVSmask = 0x00F0, ///Rigid versus soft mask
SVSmask = 0x00f0, ///Rigid versus soft mask
VF_SS = 0x0010, ///Vertex vs face soft vs soft handling
CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling
CL_SELF = 0x0040, ///Cluster soft body self collision
VF_DD = 0x0050, ///Vertex vs face soft vs soft handling
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
/* presets */
Default = SDF_RS,
END
@ -220,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 */
@ -252,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_vsplit; // Temporary Velocity in addintion to velocity used in split impulse
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
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)
@ -281,39 +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
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
};
/* 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
};
/* RContact */
struct RContact
{
@ -324,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
{
@ -711,12 +735,21 @@ 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)
{
}
btScalar sdt; // dt*timescale
btScalar isdt; // 1/sdt
btScalar velmrg; // velocity margin
@ -753,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;
@ -775,43 +810,47 @@ 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
btDbvt m_cdbvt; // Clusters tree
tClusterArray m_clusters; // Clusters
btScalar m_dampingCoefficient; // Damping Coefficient
btScalar m_sleepingThreshold;
btScalar m_maxSpeedSquared;
bool m_useFaceContact;
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
bool m_useSelfCollision;
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
btScalar m_repulsionStiffness;
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
bool m_useSelfCollision;
bool m_softSoftCollision;
btAlignedObjectArray<bool> m_clusterConnectivity; //cluster connectivity, for self-collision
btTransform m_initialWorldTransform;
btVector3 m_windVelocity;
btScalar m_restLengthScale;
@ -838,16 +877,11 @@ public:
{
return m_worldInfo;
}
void setDampingCoefficient(btScalar damping_coeff)
{
m_dampingCoefficient = damping_coeff;
}
void setUseFaceContact(bool useFaceContact)
{
m_useFaceContact = false;
}
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)
@ -908,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());
@ -957,6 +992,16 @@ public:
void setVolumeMass(btScalar mass);
/* Set volume density (using tetrahedrons) */
void setVolumeDensity(btScalar density);
/* Get the linear velocity of the center of mass */
btVector3 getLinearVelocity();
/* Set the linear velocity of the center of mass */
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);
/* Transform */
void transform(const btTransform& trs);
/* Translate */
@ -1023,6 +1068,11 @@ public:
bool rayTest(const btVector3& rayFrom,
const btVector3& rayTo,
sRayCast& results);
bool rayFaceTest(const btVector3& rayFrom,
const btVector3& rayTo,
sRayCast& results);
int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
btScalar& mint, int& index) const;
/* Solver presets */
void setSolver(eSolverPresets::_ preset);
/* predictMotion */
@ -1040,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.
@ -1120,10 +1170,11 @@ public:
int rayTest(const btVector3& rayFrom, const btVector3& rayTo,
btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
void initializeFaceTree();
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();
@ -1137,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);
@ -1152,9 +1205,188 @@ public:
static void VSolve_Links(btSoftBody* psb, btScalar kst);
static psolver_t getSolver(ePSolver::_ solver);
static vsolver_t getSolver(eVSolver::_ solver);
void geometricCollisionHandler(btSoftBody* psb);
#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
if (use_velocity)
{
btVector3 points[2] = {n->m_x, n->m_x + m_sst.sdt * n->m_v};
vol = btDbvtVolume::FromPoints(points, 2);
vol.Expand(btVector3(pad, pad, pad));
}
else
{
vol = btDbvtVolume::FromCR(n->m_x, pad);
}
node->volume = vol;
return;
}
else
{
updateNode(node->childs[0], use_velocity, margin);
updateNode(node->childs[1], use_velocity, margin);
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
node->volume = vol;
}
}
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
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;
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};
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};
vol = btDbvtVolume::FromPoints(points, 3);
}
vol.Expand(btVector3(pad, pad, pad));
node->volume = vol;
return;
}
else
{
updateFace(node->childs[0], use_velocity, margin);
updateFace(node->childs[1], use_velocity, margin);
ATTRIBUTE_ALIGNED16(btDbvtVolume)
vol;
Merge(node->childs[0]->volume, node->childs[1]->volume, vol);
node->volume = vol;
}
}
void updateFaceTree(bool use_velocity, bool margin)
{
if (m_fdbvt.m_root)
updateFace(m_fdbvt.m_root, use_velocity, margin);
if (m_fdbvnt)
updateFace(m_fdbvnt, use_velocity, margin);
}
template <typename T>
static inline T BaryEval(const T& a,
const T& b,
const T& c,
const btVector3& coord)
{
return (a * coord.x() + b * coord.y() + c * coord.z());
}
void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
{
btAlignedObjectArray<int> indices;
{
// randomize the order of repulsive force
indices.resize(m_faceNodeContacts.size());
for (int i = 0; i < m_faceNodeContacts.size(); ++i)
indices[i] = i;
#define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
int i, ni;
for (i = 0, ni = indices.size(); i < ni; ++i)
{
btSwap(indices[i], indices[NEXTRAND % ni]);
}
}
for (int k = 0; k < m_faceNodeContacts.size(); ++k)
{
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);
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
if (vn > OVERLAP_REDUCTION_FACTOR * d / timeStep)
continue;
btVector3 vt = vr - vn * n;
btScalar I = 0;
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;
int face_penetration = 0, node_penetration = node->m_constrained;
for (int i = 0; i < 3; ++i)
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;
}
if (node_penetration <= 0)
{
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);
vt.safeNormalize();
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 (node_penetration <= 0)
{
node->m_v -= I_tilde * node->m_im * vt;
}
}
}
}
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;
};

683
thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.cpp vendored
View File

@ -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,302 +1245,419 @@ 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)
{
ss.ignore(128, ' '); // ignore "4"
Index tet;
tet.resize(4);
for (size_t i = 0; i < 4; i++)
{
ss >> tet[i];
}
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());
std::ifstream fs;
fs.open(vtk_file);
btAssert(fs);
fs.close();
return psb;
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)
{
printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
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;
}
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::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::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;
}
// 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_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;
bool found = false;
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;
}
}

26
thirdparty/bullet/BulletSoftBody/btSoftBodyHelpers.h vendored
View File

@ -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,17 +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 interpolateBarycentricWeights(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,

1626
thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h vendored
View File

File diff suppressed because it is too large Load Diff

2
thirdparty/bullet/BulletSoftBody/btSoftBodySolvers.h vendored
View File

@ -36,7 +36,7 @@ public:
CL_SIMD_SOLVER,
DX_SOLVER,
DX_SIMD_SOLVER,
DEFORMABLE_SOLVER
DEFORMABLE_SOLVER
};
protected:

5
thirdparty/bullet/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp vendored
View File

@ -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
// ///////////////////////////////
}

3
thirdparty/bullet/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp vendored
View File

@ -48,9 +48,10 @@ btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm()
}
#include <stdio.h>
#include "LinearMath/btQuickprof.h"
void btSoftRigidCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
BT_PROFILE("btSoftRigidCollisionAlgorithm::processCollision");
(void)dispatchInfo;
(void)resultOut;
//printf("btSoftRigidCollisionAlgorithm\n");

68
thirdparty/bullet/BulletSoftBody/btSparseSDF.h vendored
View File

@ -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;
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;
}
/* 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;
}

447
thirdparty/bullet/BulletSoftBody/poly34.cpp vendored Normal file
View File

@ -0,0 +1,447 @@
// poly34.cpp : solution of cubic and quartic equation
// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
// khash2 (at) gmail.com
// Thanks to Alexandr Rakhmanin <rakhmanin (at) gmail.com>
// public domain
//
#include <math.h>
#include "poly34.h" // solution of cubic and quartic equation
#define TwoPi 6.28318530717958648
const btScalar eps = SIMD_EPSILON;
//=============================================================================
// _root3, root3 from http://prografix.narod.ru
//=============================================================================
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 SIMD_FORCE_INLINE root3(btScalar x)
{
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;
}
//---------------------------------------------------------------------------
// x - array of size 3
// In case 3 real roots: => x[0], x[1], x[2], return 3
// 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) {
//---------------------------------------------------------------------------
// a>=0!
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;
}
}
//---------------------------------------------------------------------------
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
//---------------------------------------------------------------------------
#define SWAP(a, b) \
{ \
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
}
}
//---------------------------------------------------------------------------
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
//-----------------------------------------------------------------------------
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;
}
//-----------------------------------------------------------------------------
// 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 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
{
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
{
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
//-----------------------------------------------------------------------------

38
thirdparty/bullet/BulletSoftBody/poly34.h vendored Normal file
View File

@ -0,0 +1,38 @@
// poly34.h : solution of cubic and quartic equation
// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
// khash2 (at) gmail.com
#ifndef POLY_34
#define POLY_34
#include "LinearMath/btScalar.h"
// 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
// 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
// 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
// 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
//-----------------------------------------------------------------------------
// 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
#endif

4
thirdparty/bullet/LinearMath/btAlignedAllocator.cpp vendored
View File

@ -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;

4
thirdparty/bullet/LinearMath/btAlignedAllocator.h vendored
View File

@ -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);

6
thirdparty/bullet/LinearMath/btConvexHullComputer.cpp vendored
View File

@ -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)
{

3
thirdparty/bullet/LinearMath/btConvexHullComputer.h vendored
View File

@ -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;

6
thirdparty/bullet/LinearMath/btIDebugDraw.h vendored
View File

@ -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
{

4
thirdparty/bullet/LinearMath/btImplicitQRSVD.h vendored
View File

@ -41,7 +41,7 @@
#ifndef btImplicitQRSVD_h
#define btImplicitQRSVD_h
#include <limits>
#include "btMatrix3x3.h"
class btMatrix2x2
{
@ -753,7 +753,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A,
btMatrix3x3& V,
btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
{
using std::fabs;
// using std::fabs;
btMatrix3x3 B = A;
U.setIdentity();
V.setIdentity();

16
thirdparty/bullet/LinearMath/btMatrix3x3.h vendored
View File

@ -26,10 +26,12 @@ subject to the following restrictions:
#endif
#if defined(BT_USE_SSE)
#define v0000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f))
#define v1000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f))
#define v0100 (_mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f))
#define v0010 (_mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f))
#elif defined(BT_USE_NEON)
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0000) = {0.0f, 0.0f, 0.0f, 0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
@ -330,6 +332,20 @@ public:
btScalar(0.0), btScalar(0.0), btScalar(1.0));
#endif
}
/**@brief Set the matrix to the identity */
void setZero()
{
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
m_el[0] = v0000;
m_el[1] = v0000;
m_el[2] = v0000;
#else
setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0),
btScalar(0.0), btScalar(0.0), btScalar(0.0),
btScalar(0.0), btScalar(0.0), btScalar(0.0));
#endif
}
static const btMatrix3x3& getIdentity()
{

3
thirdparty/bullet/LinearMath/btMatrixX.h vendored
View File

@ -346,10 +346,9 @@ struct btMatrixX
T dotProd = 0;
{
{
int r = rows();
int c = cols();
for (int k = 0; k < cols(); k++)
for (int k = 0; k < c; k++)
{
T w = (*this)(i, k);
if (other(k, j) != 0.f)

83
thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h vendored Normal file
View File

@ -0,0 +1,83 @@
//
// btModifiedGramSchmidt.h
// LinearMath
//
// Created by Xuchen Han on 4/4/20.
//
#ifndef btModifiedGramSchmidt_h
#define btModifiedGramSchmidt_h
#include "btReducedVector.h"
#include "btAlignedObjectArray.h"
#include <iostream>
#include <cmath>
template<class TV>
class btModifiedGramSchmidt
{
public:
btAlignedObjectArray<TV> m_in;
btAlignedObjectArray<TV> m_out;
btModifiedGramSchmidt(const btAlignedObjectArray<TV>& vecs): m_in(vecs)
{
m_out.resize(0);
}
void solve()
{
m_out.resize(m_in.size());
for (int i = 0; i < m_in.size(); ++i)
{
// printf("========= starting %d ==========\n", i);
TV v(m_in[i]);
// v.print();
for (int j = 0; j < i; ++j)
{
v = v - v.proj(m_out[j]);
// v.print();
}
v.normalize();
m_out[i] = v;
// v.print();
}
}
void test()
{
std::cout << SIMD_EPSILON << std::endl;
printf("=======inputs=========\n");
for (int i = 0; i < m_out.size(); ++i)
{
m_in[i].print();
}
printf("=======output=========\n");
for (int i = 0; i < m_out.size(); ++i)
{
m_out[i].print();
}
btScalar eps = SIMD_EPSILON;
for (int i = 0; i < m_out.size(); ++i)
{
for (int j = 0; j < m_out.size(); ++j)
{
if (i == j)
{
if (std::abs(1.0-m_out[i].dot(m_out[j])) > eps)// && std::abs(m_out[i].dot(m_out[j])) > eps)
{
printf("vec[%d] is not unit, norm squared = %f\n", i,m_out[i].dot(m_out[j]));
}
}
else
{
if (std::abs(m_out[i].dot(m_out[j])) > eps)
{
printf("vec[%d] and vec[%d] is not orthogonal, dot product = %f\n", i, j, m_out[i].dot(m_out[j]));
}
}
}
}
}
};
template class btModifiedGramSchmidt<btReducedVector>;
#endif /* btModifiedGramSchmidt_h */

3
thirdparty/bullet/LinearMath/btQuickprof.cpp vendored
View File

@ -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

170
thirdparty/bullet/LinearMath/btReducedVector.cpp vendored Normal file
View File

@ -0,0 +1,170 @@
//
// btReducedVector.cpp
// LinearMath
//
// Created by Xuchen Han on 4/4/20.
//
#include <stdio.h>
#include "btReducedVector.h"
#include <cmath>
// returns the projection of this onto other
btReducedVector btReducedVector::proj(const btReducedVector& other) const
{
btReducedVector ret(m_sz);
btScalar other_length2 = other.length2();
if (other_length2 < SIMD_EPSILON)
{
return ret;
}
return other*(this->dot(other))/other_length2;
}
void btReducedVector::normalize()
{
if (this->length2() < SIMD_EPSILON)
{
m_indices.clear();
m_vecs.clear();
return;
}
*this /= std::sqrt(this->length2());
}
bool btReducedVector::testAdd() const
{
int sz = 5;
btAlignedObjectArray<int> id1;
id1.push_back(1);
id1.push_back(3);
btAlignedObjectArray<btVector3> v1;
v1.push_back(btVector3(1,0,1));
v1.push_back(btVector3(3,1,5));
btAlignedObjectArray<int> id2;
id2.push_back(2);
id2.push_back(3);
id2.push_back(5);
btAlignedObjectArray<btVector3> v2;
v2.push_back(btVector3(2,3,1));
v2.push_back(btVector3(3,4,9));
v2.push_back(btVector3(0,4,0));
btAlignedObjectArray<int> id3;
id3.push_back(1);
id3.push_back(2);
id3.push_back(3);
id3.push_back(5);
btAlignedObjectArray<btVector3> v3;
v3.push_back(btVector3(1,0,1));
v3.push_back(btVector3(2,3,1));
v3.push_back(btVector3(6,5,14));
v3.push_back(btVector3(0,4,0));
btReducedVector rv1(sz, id1, v1);
btReducedVector rv2(sz, id2, v2);
btReducedVector ans(sz, id3, v3);
bool ret = ((ans == rv1+rv2) && (ans == rv2+rv1));
if (!ret)
printf("btReducedVector testAdd failed\n");
return ret;
}
bool btReducedVector::testMinus() const
{
int sz = 5;
btAlignedObjectArray<int> id1;
id1.push_back(1);
id1.push_back(3);
btAlignedObjectArray<btVector3> v1;
v1.push_back(btVector3(1,0,1));
v1.push_back(btVector3(3,1,5));
btAlignedObjectArray<int> id2;
id2.push_back(2);
id2.push_back(3);
id2.push_back(5);
btAlignedObjectArray<btVector3> v2;
v2.push_back(btVector3(2,3,1));
v2.push_back(btVector3(3,4,9));
v2.push_back(btVector3(0,4,0));
btAlignedObjectArray<int> id3;
id3.push_back(1);
id3.push_back(2);
id3.push_back(3);
id3.push_back(5);
btAlignedObjectArray<btVector3> v3;
v3.push_back(btVector3(-1,-0,-1));
v3.push_back(btVector3(2,3,1));
v3.push_back(btVector3(0,3,4));
v3.push_back(btVector3(0,4,0));
btReducedVector rv1(sz, id1, v1);
btReducedVector rv2(sz, id2, v2);
btReducedVector ans(sz, id3, v3);
bool ret = (ans == rv2-rv1);
if (!ret)
printf("btReducedVector testMinus failed\n");
return ret;
}
bool btReducedVector::testDot() const
{
int sz = 5;
btAlignedObjectArray<int> id1;
id1.push_back(1);
id1.push_back(3);
btAlignedObjectArray<btVector3> v1;
v1.push_back(btVector3(1,0,1));
v1.push_back(btVector3(3,1,5));
btAlignedObjectArray<int> id2;
id2.push_back(2);
id2.push_back(3);
id2.push_back(5);
btAlignedObjectArray<btVector3> v2;
v2.push_back(btVector3(2,3,1));
v2.push_back(btVector3(3,4,9));
v2.push_back(btVector3(0,4,0));
btReducedVector rv1(sz, id1, v1);
btReducedVector rv2(sz, id2, v2);
btScalar ans = 58;
bool ret = (ans == rv2.dot(rv1) && ans == rv1.dot(rv2));
ans = 14+16+9+16+81;
ret &= (ans==rv2.dot(rv2));
if (!ret)
printf("btReducedVector testDot failed\n");
return ret;
}
bool btReducedVector::testMultiply() const
{
int sz = 5;
btAlignedObjectArray<int> id1;
id1.push_back(1);
id1.push_back(3);
btAlignedObjectArray<btVector3> v1;
v1.push_back(btVector3(1,0,1));
v1.push_back(btVector3(3,1,5));
btScalar s = 2;
btReducedVector rv1(sz, id1, v1);
btAlignedObjectArray<int> id2;
id2.push_back(1);
id2.push_back(3);
btAlignedObjectArray<btVector3> v2;
v2.push_back(btVector3(2,0,2));
v2.push_back(btVector3(6,2,10));
btReducedVector ans(sz, id2, v2);
bool ret = (ans == rv1*s);
if (!ret)
printf("btReducedVector testMultiply failed\n");
return ret;
}
void btReducedVector::test() const
{
bool ans = testAdd() && testMinus() && testDot() && testMultiply();
if (ans)
{
printf("All tests passed\n");
}
else
{
printf("Tests failed\n");
}
}

Some files were not shown because too many files have changed in this diff Show More