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

bullet: Sync with upstream 2.89 

This allows distro unbundling again for distros that ship Bullet 2.89+.
This commit is contained in:
Rémi Verschelde 2020-01-08 18:05:43 +01:00
parent 98222130bf
commit 29e07dfa4e
90 changed files with 11215 additions and 3083 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
modules/bullet/SCsub
View File

@ -8,7 +8,8 @@ env_bullet = env_modules.Clone()
# Thirdparty source files
if env['builtin_bullet']:
# Build only version 2 for now (as of 2.87)
# Build only version 2 for now (as of 2.89)
# Sync file list with relevant upstream CMakeLists.txt for each folder.
thirdparty_dir = "#thirdparty/bullet/"
bullet2_src = [
@ -67,8 +68,8 @@ if env['builtin_bullet']:
, "BulletCollision/CollisionShapes/btCylinderShape.cpp"
, "BulletCollision/CollisionShapes/btEmptyShape.cpp"
, "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp"
, "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp"
, "BulletCollision/CollisionShapes/btMiniSDF.cpp"
, "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp"
, "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp"
, "BulletCollision/CollisionShapes/btMultiSphereShape.cpp"
, "BulletCollision/CollisionShapes/btOptimizedBvh.cpp"
@ -124,6 +125,8 @@ if env['builtin_bullet']:
, "BulletDynamics/ConstraintSolver/btHingeConstraint.cpp"
, "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp"
, "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp"
, "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp"
, "BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp"
, "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp"
, "BulletDynamics/ConstraintSolver/btSliderConstraint.cpp"
, "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp"
@ -138,15 +141,17 @@ if env['builtin_bullet']:
, "BulletDynamics/Vehicle/btRaycastVehicle.cpp"
, "BulletDynamics/Vehicle/btWheelInfo.cpp"
, "BulletDynamics/Featherstone/btMultiBody.cpp"
, "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"
, "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp"
, "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"
, "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
, "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"
, "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"
, "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"
, "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
, "BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"
, "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"
, "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
, "BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"
, "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp"
, "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"
, "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
, "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp"
, "BulletDynamics/MLCPSolvers/btDantzigLCP.cpp"
, "BulletDynamics/MLCPSolvers/btMLCPSolver.cpp"
, "BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp"
@ -167,6 +172,12 @@ if env['builtin_bullet']:
, "BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp"
, "BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp"
, "BulletSoftBody/btDefaultSoftBodySolver.cpp"
, "BulletSoftBody/btDeformableBackwardEulerObjective.cpp"
, "BulletSoftBody/btDeformableBodySolver.cpp"
, "BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp"
, "BulletSoftBody/btDeformableContactProjection.cpp"
, "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp"
, "BulletSoftBody/btDeformableContactConstraint.cpp"
# clew
, "clew/clew.c"
@ -182,6 +193,9 @@ if env['builtin_bullet']:
, "LinearMath/btSerializer64.cpp"
, "LinearMath/btThreads.cpp"
, "LinearMath/btVector3.cpp"
, "LinearMath/TaskScheduler/btTaskScheduler.cpp"
, "LinearMath/TaskScheduler/btThreadSupportPosix.cpp"
, "LinearMath/TaskScheduler/btThreadSupportWin32.cpp"
]
thirdparty_sources = [thirdparty_dir + file for file in bullet2_src]

2
thirdparty/README.md vendored
View File

@ -11,7 +11,7 @@
## bullet
- Upstream: https://github.com/bulletphysics/bullet3
- Version: git (5ec8339, 2019)
- Version: 2.89
- License: zlib
Files extracted from upstream source:

16
thirdparty/bullet/Bullet3Common/b3Logging.h vendored
View File

@ -14,20 +14,8 @@ extern "C"
//#define b3Printf b3OutputPrintfVarArgsInternal
//#define b3Printf(...) printf(__VA_ARGS__)
//#define b3Printf(...)
#define b3Warning(...) \
do \
{ \
b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__); \
b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); \
} while (0)
#define b3Error(...) \
do \
{ \
b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__); \
b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); \
} while (0)
#define b3Warning(...) do{ b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__);b3OutputWarningMessageVarArgsInternal(__VA_ARGS__);} while (0)
#define b3Error(...)do {b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__);b3OutputErrorMessageVarArgsInternal(__VA_ARGS__);} while (0)
#ifndef B3_NO_PROFILE
void b3EnterProfileZone(const char* name);

34
thirdparty/bullet/Bullet3Common/b3Scalar.h vendored
View File

@ -40,8 +40,12 @@ inline int b3GetVersion()
#ifdef _WIN32
#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined(_MSC_VER) && _MSC_VER < 1300)
#if defined(__GNUC__) // it should handle both MINGW and CYGWIN
#define B3_FORCE_INLINE __inline__ __attribute__((always_inline))
#define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
#define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
#define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
#elif ( defined(_MSC_VER) && _MSC_VER < 1300 )
#define B3_FORCE_INLINE inline
#define B3_ATTRIBUTE_ALIGNED16(a) a
#define B3_ATTRIBUTE_ALIGNED64(a) a
@ -67,7 +71,17 @@ inline int b3GetVersion()
#if (defined(_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined(B3_USE_DOUBLE_PRECISION))
#if (defined(_M_IX86) || defined(_M_X64))
#ifdef __clang__
//#define B3_NO_SIMD_OPERATOR_OVERLOADS
#define B3_DISABLE_SSE
#endif //__clang__
#ifndef B3_DISABLE_SSE
#define B3_USE_SSE
#endif //B3_DISABLE_SSE
#ifdef B3_USE_SSE
//B3_USE_SSE_IN_API is disabled under Windows by default, because
//it makes it harder to integrate Bullet into your application under Windows
@ -88,17 +102,7 @@ inline int b3GetVersion()
#ifdef B3_DEBUG
#ifdef _MSC_VER
#include <stdio.h>
#define b3Assert(x) \
{ \
if (!(x)) \
{ \
b3Error( \
"Assert "__FILE__ \
":%u (" #x ")\n", \
__LINE__); \
__debugbreak(); \
} \
}
#define b3Assert(x) { if(!(x)){b3Error("Assert " __FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak(); }}
#else //_MSC_VER
#include <assert.h>
#define b3Assert assert
@ -293,7 +297,7 @@ static int b3NanMask = 0x7F800001;
static int b3InfinityMask = 0x7F800000;
#define B3_INFINITY_MASK (*(float *)&b3InfinityMask)
#endif
#ifndef B3_NO_SIMD_OPERATOR_OVERLOADS
inline __m128 operator+(const __m128 A, const __m128 B)
{
return _mm_add_ps(A, B);
@ -308,7 +312,7 @@ inline __m128 operator*(const __m128 A, const __m128 B)
{
return _mm_mul_ps(A, B);
}
#endif //B3_NO_SIMD_OPERATOR_OVERLOADS
#define b3CastfTo128i(a) (_mm_castps_si128(a))
#define b3CastfTo128d(a) (_mm_castps_pd(a))
#define b3CastiTo128f(a) (_mm_castsi128_ps(a))

2
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp vendored
View File

@ -496,7 +496,7 @@ void b3GpuParallelLinearBvh::constructBinaryRadixTree()
clFinish(m_queue);
}
//Find the number of nodes seperating each internal node and the root node
//Find the number of nodes separating each internal node and the root node
//so that the AABBs can be set using the next kernel.
//Also determine the maximum number of nodes separating an internal node and the root node.
{

2
thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h vendored
View File

@ -68,7 +68,7 @@ public:
virtual void unLockReadOnlyVertexBase(int subpart) const = 0;
/// getNumSubParts returns the number of seperate subparts
/// getNumSubParts returns the number of separate subparts
/// each subpart has a continuous array of vertices and indices
virtual int getNumSubParts() const = 0;

2
thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h vendored
View File

@ -100,7 +100,7 @@ public:
virtual void unLockReadOnlyVertexBase(int subpart) const { (void)subpart; }
/// getNumSubParts returns the number of seperate subparts
/// getNumSubParts returns the number of separate subparts
/// each subpart has a continuous array of vertices and indices
virtual int getNumSubParts() const
{

4
thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp vendored
View File

@ -183,9 +183,9 @@ bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
const b3Vector3& c = m_simplexVectorW[2];
const b3Vector3& d = m_simplexVectorW[3];
bool hasSeperation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);
bool hasSeparation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);
if (hasSeperation)
if (hasSeparation)
{
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +

7
thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h vendored
View File

@ -187,13 +187,6 @@ btBroadphasePair
BT_DECLARE_ALIGNED_ALLOCATOR();
btBroadphasePair(const btBroadphasePair& other)
: m_pProxy0(other.m_pProxy0),
m_pProxy1(other.m_pProxy1),
m_algorithm(other.m_algorithm),
m_internalInfo1(other.m_internalInfo1)
{
}
btBroadphasePair(btBroadphaseProxy & proxy0, btBroadphaseProxy & proxy1)
{
//keep them sorted, so the std::set operations work

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

@ -21,7 +21,6 @@ subject to the following restrictions:
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btAabbUtil2.h"
//
// Compile time configuration
//
@ -131,6 +130,7 @@ subject to the following restrictions:
/* btDbvtAabbMm */
struct btDbvtAabbMm
{
DBVT_INLINE btDbvtAabbMm(){}
DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); }
DBVT_INLINE btVector3 Lengths() const { return (mx - mi); }
DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); }
@ -190,6 +190,36 @@ struct btDbvtNode
};
};
/* btDbv(normal)tNode */
struct btDbvntNode
{
btDbvtVolume volume;
btVector3 normal;
btScalar angle;
DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
DBVT_INLINE bool isinternal() const { return (!isleaf()); }
btDbvntNode* childs[2];
void* data;
btDbvntNode(const btDbvtNode* n)
: volume(n->volume)
, angle(0)
, normal(0,0,0)
, data(n->data)
{
childs[0] = 0;
childs[1] = 0;
}
~btDbvntNode()
{
if (childs[0])
delete childs[0];
if (childs[1])
delete childs[1];
}
};
typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
@ -225,6 +255,14 @@ struct btDbvt
btDbvtNode* parent;
sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {}
};
struct sStknNN
{
const btDbvntNode* a;
const btDbvntNode* b;
sStknNN() {}
sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {}
};
// Policies/Interfaces
/* ICollide */
@ -234,6 +272,7 @@ struct btDbvt
DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {}
DBVT_VIRTUAL void Process(const btDbvtNode*) {}
DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); }
DBVT_VIRTUAL void Process(const btDbvntNode*, const btDbvntNode*) {}
DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); }
DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); }
};
@ -306,6 +345,12 @@ struct btDbvt
void collideTT(const btDbvtNode* root0,
const btDbvtNode* root1,
DBVT_IPOLICY);
DBVT_PREFIX
void selfCollideT(const btDbvntNode* root,
DBVT_IPOLICY);
DBVT_PREFIX
void selfCollideTT(const btDbvtNode* root,
DBVT_IPOLICY);
DBVT_PREFIX
void collideTTpersistentStack(const btDbvtNode* root0,
@ -837,6 +882,135 @@ inline void btDbvt::collideTT(const btDbvtNode* root0,
}
}
//
DBVT_PREFIX
inline void btDbvt::selfCollideT(const btDbvntNode* root,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if (root)
{
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 4;
btAlignedObjectArray<sStknNN> stkStack;
stkStack.resize(DOUBLE_STACKSIZE);
stkStack[0] = sStknNN(root, root);
do
{
sStknNN p = stkStack[--depth];
if (depth > treshold)
{
stkStack.resize(stkStack.size() * 2);
treshold = stkStack.size() - 4;
}
if (p.a == p.b)
{
if (p.a->isinternal() && p.a->angle > SIMD_PI)
{
stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[0]);
stkStack[depth++] = sStknNN(p.a->childs[1], p.a->childs[1]);
stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[1]);
}
}
else if (Intersect(p.a->volume, p.b->volume))
{
if (p.a->isinternal())
{
if (p.b->isinternal())
{
stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[0]);
stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[0]);
stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[1]);
stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[1]);
}
else
{
stkStack[depth++] = sStknNN(p.a->childs[0], p.b);
stkStack[depth++] = sStknNN(p.a->childs[1], p.b);
}
}
else
{
if (p.b->isinternal())
{
stkStack[depth++] = sStknNN(p.a, p.b->childs[0]);
stkStack[depth++] = sStknNN(p.a, p.b->childs[1]);
}
else
{
policy.Process(p.a, p.b);
}
}
}
} while (depth);
}
}
//
DBVT_PREFIX
inline void btDbvt::selfCollideTT(const btDbvtNode* root,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if (root)
{
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 4;
btAlignedObjectArray<sStkNN> stkStack;
stkStack.resize(DOUBLE_STACKSIZE);
stkStack[0] = sStkNN(root, root);
do
{
sStkNN p = stkStack[--depth];
if (depth > treshold)
{
stkStack.resize(stkStack.size() * 2);
treshold = stkStack.size() - 4;
}
if (p.a == p.b)
{
if (p.a->isinternal())
{
stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
}
}
else if (Intersect(p.a->volume, p.b->volume))
{
if (p.a->isinternal())
{
if (p.b->isinternal())
{
stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
}
else
{
stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
}
}
else
{
if (p.b->isinternal())
{
stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
}
else
{
policy.Process(p.a, p.b);
}
}
}
} while (depth);
}
}
DBVT_PREFIX
inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0,
const btDbvtNode* root1,

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

@ -68,7 +68,7 @@ public:
virtual void processAllOverlappingPairs(btOverlapCallback*, btDispatcher* dispatcher) = 0;
virtual void processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo)
virtual void processAllOverlappingPairs(btOverlapCallback* callback, btDispatcher* dispatcher, const struct btDispatcherInfo& /*dispatchInfo*/)
{
processAllOverlappingPairs(callback, dispatcher);
}

8
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h vendored
View File

@ -27,11 +27,17 @@ public:
const btCollisionShape* m_shape;
const btCollisionObject* m_collisionObject;
const btTransform& m_worldTransform;
const btTransform* m_preTransform;
int m_partId;
int m_index;
btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index)
: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_partId(partId), m_index(index)
: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(NULL), m_partId(partId), m_index(index)
{
}
btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, const btTransform& preTransform, int partId, int index)
: m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(&preTransform), m_partId(partId), m_index(index)
{
}

4
thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp vendored
View File

@ -292,8 +292,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape(btCollisionSha
}
break;
}
#endif //SUPPORT_GIMPACT_SHAPE_IMPORT \
//The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API \
#endif //SUPPORT_GIMPACT_SHAPE_IMPORT
//The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API
//so deal with this
case CAPSULE_SHAPE_PROXYTYPE:
{

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

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

4
thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp vendored
View File

@ -503,8 +503,8 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw);
//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN \
//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); \
#endif //ZERO_MARGIN
//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
//if (l2>SIMD_EPSILON)
{
sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld; //gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);

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

@ -1,6 +1,8 @@
#include "btInternalEdgeUtility.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
@ -290,6 +292,39 @@ struct btConnectivityProcessor : public btTriangleCallback
}
}
};
struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback
{
btHeightfieldTerrainShape* m_heightfieldShape;
btTriangleInfoMap* m_triangleInfoMap;
b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap)
:m_heightfieldShape(heightFieldShape),
m_triangleInfoMap(triangleInfoMap)
{
}
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{
btConnectivityProcessor connectivityProcessor;
connectivityProcessor.m_partIdA = partId;
connectivityProcessor.m_triangleIndexA = triangleIndex;
connectivityProcessor.m_triangleVerticesA = triangle;
connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap;
btVector3 aabbMin, aabbMax;
aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
aabbMin.setMin(triangle[0]);
aabbMax.setMax(triangle[0]);
aabbMin.setMin(triangle[1]);
aabbMax.setMax(triangle[1]);
aabbMin.setMin(triangle[2]);
aabbMax.setMax(triangle[2]);
m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax);
}
};
/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
@ -361,6 +396,28 @@ void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangle
}
}
void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap)
{
//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
if (heightfieldShape->getTriangleInfoMap())
return;
heightfieldShape->setTriangleInfoMap(triangleInfoMap);
//get all the triangles of the heightfield
btVector3 aabbMin, aabbMax;
aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT));
b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap);
heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax);
}
// Given a point and a line segment (defined by two points), compute the closest point
// in the line. Cap the point at the endpoints of the line segment.
void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint)
@ -426,6 +483,32 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
return;
btTriangleInfoMap* triangleInfoMapPtr = 0;
if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
{
btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
triangleInfoMapPtr = heightfield->getTriangleInfoMap();
//#define USE_HEIGHTFIELD_TRIANGLES
#ifdef USE_HEIGHTFIELD_TRIANGLES
btVector3 newNormal = btVector3(0, 0, 1);
const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
btVector3 tri_normal;
tri_shape->calcNormal(tri_normal);
newNormal = tri_normal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
return;
#endif
}
btBvhTriangleMeshShape* trimesh = 0;
if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
@ -439,10 +522,12 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
}
}
if (trimesh == 0)
return;
if (trimesh)
{
triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap();
}
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap();
if (!triangleInfoMapPtr)
return;

3
thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h vendored
View File

@ -15,6 +15,7 @@ class btCollisionObject;
struct btCollisionObjectWrapper;
class btManifoldPoint;
class btIDebugDraw;
class btHeightfieldTerrainShape;
enum btInternalEdgeAdjustFlags
{
@ -26,6 +27,8 @@ enum btInternalEdgeAdjustFlags
///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo'
void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap);
void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* trimeshShape, btTriangleInfoMap* triangleInfoMap);
///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo)
///If this info map is missing, or the triangle is not store in this map, nothing will be done
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap, const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0);

9
thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp vendored
View File

@ -280,6 +280,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if (colObj0->getActivationState() == ISLAND_SLEEPING)
@ -337,13 +338,17 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
}
}
///@todo: this is random access, it can be walked 'cache friendly'!
void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
{
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
buildIslands(dispatcher, collisionWorld);
processIslands(dispatcher, collisionWorld, callback);
}
void btSimulationIslandManager::processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
{
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
int endIslandIndex = 1;
int startIslandIndex;
int numElem = getUnionFind().getNumElements();

2
thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h vendored
View File

@ -60,6 +60,8 @@ public:
void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld);
void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback);
bool getSplitIslands()
{
return m_splitIslands;

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

@ -21,6 +21,9 @@ 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_triangleInfoMap(0)
{
initialize(heightStickWidth, heightStickLength, heightfieldData,
heightScale, minHeight, maxHeight, upAxis, hdt,
@ -28,6 +31,9 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(
}
btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar maxHeight, int upAxis, bool useFloatData, bool flipQuadEdges)
:m_userIndex2(-1),
m_userValue3(0),
m_triangleInfoMap(0)
{
// legacy constructor: support only float or unsigned char,
// and min height is zero
@ -349,12 +355,12 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
getVertex(x, j, vertices[indices[0]]);
getVertex(x, j + 1, vertices[indices[1]]);
getVertex(x + 1, j + 1, vertices[indices[2]]);
callback->processTriangle(vertices, x, j);
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]]);
getVertex(x + 1, j, vertices[indices[2]]);
callback->processTriangle(vertices, x, j);
callback->processTriangle(vertices, 2 * x+1, j);
}
else
{
@ -362,12 +368,12 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
getVertex(x, j, vertices[indices[0]]);
getVertex(x, j + 1, vertices[indices[1]]);
getVertex(x + 1, j, vertices[indices[2]]);
callback->processTriangle(vertices, x, j);
callback->processTriangle(vertices, 2 * x, j);
//second triangle
getVertex(x + 1, j, vertices[indices[0]]);
//getVertex(x,j+1,vertices[1]);
getVertex(x + 1, j + 1, vertices[indices[2]]);
callback->processTriangle(vertices, x, j);
callback->processTriangle(vertices, 2 * x+1, j);
}
}
}

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

@ -114,6 +114,11 @@ protected:
int m_vboundsGridLength;
int m_vboundsChunkSize;
int m_userIndex2;
btScalar m_userValue3;
struct btTriangleInfoMap* m_triangleInfoMap;
virtual btScalar getRawHeightFieldValue(int x, int y) const;
void quantizeWithClamp(int* out, const btVector3& point, int isMax) const;
@ -185,6 +190,40 @@ public:
}
//debugging
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;
}
btScalar getUserValue3() const
{
return m_userValue3;
}
const struct btTriangleInfoMap* getTriangleInfoMap() const
{
return m_triangleInfoMap;
}
struct btTriangleInfoMap* getTriangleInfoMap()
{
return m_triangleInfoMap;
}
void setTriangleInfoMap(btTriangleInfoMap* map)
{
m_triangleInfoMap = map;
}
const unsigned char* getHeightfieldRawData() const
{
return m_heightfieldDataUnsignedChar;
}
};
#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H

2
thirdparty/bullet/BulletCollision/CollisionShapes/btStridingMeshInterface.h vendored
View File

@ -58,7 +58,7 @@ public:
virtual void unLockReadOnlyVertexBase(int subpart) const = 0;
/// getNumSubParts returns the number of seperate subparts
/// getNumSubParts returns the number of separate subparts
/// each subpart has a continuous array of vertices and indices
virtual int getNumSubParts() const = 0;

2
thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h vendored
View File

@ -100,7 +100,7 @@ public:
virtual void unLockReadOnlyVertexBase(int subpart) const { (void)subpart; }
/// getNumSubParts returns the number of seperate subparts
/// getNumSubParts returns the number of separate subparts
/// each subpart has a continuous array of vertices and indices
virtual int getNumSubParts() const
{

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

@ -1,5 +1,5 @@
/*! \file btGImpactShape.h
\author Francisco Leon Najera
\author Francisco Len Nßjera
*/
/*
This source file is part of GIMPACT Library.

8
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h vendored
View File

@ -95,11 +95,11 @@ int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate
for (;;)
//while (true)
{
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
btVector3 pInA = a.getLocalSupportWithoutMargin(separatingAxisInA);
btVector3 qInB = b.getLocalSupportWithoutMargin(separatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);

40
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp vendored
View File

@ -79,11 +79,11 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput &input, Result
static void btComputeSupport(const btConvexShape *convexA, const btTransform &localTransA, const btConvexShape *convexB, const btTransform &localTransB, const btVector3 &dir, bool check2d, btVector3 &supAworld, btVector3 &supBworld, btVector3 &aMinb)
{
btVector3 seperatingAxisInA = (dir)*localTransA.getBasis();
btVector3 seperatingAxisInB = (-dir) * localTransB.getBasis();
btVector3 separatingAxisInA = (dir)*localTransA.getBasis();
btVector3 separatingAxisInB = (-dir) * localTransB.getBasis();
btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
btVector3 pInA = pInANoMargin;
btVector3 qInB = qInBNoMargin;
@ -839,11 +839,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &inpu
for (;;)
//while (true)
{
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
btVector3 separatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
btVector3 separatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
@ -1116,11 +1116,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &inpu
btScalar d2 = 0.f;
{
btVector3 seperatingAxisInA = (-orgNormalInB) * localTransA.getBasis();
btVector3 seperatingAxisInB = orgNormalInB * localTransB.getBasis();
btVector3 separatingAxisInA = (-orgNormalInB) * localTransA.getBasis();
btVector3 separatingAxisInB = orgNormalInB * localTransB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
@ -1130,11 +1130,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &inpu
btScalar d1 = 0;
{
btVector3 seperatingAxisInA = (normalInB)*localTransA.getBasis();
btVector3 seperatingAxisInB = -normalInB * localTransB.getBasis();
btVector3 separatingAxisInA = (normalInB)*localTransA.getBasis();
btVector3 separatingAxisInB = -normalInB * localTransB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);
@ -1143,11 +1143,11 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput &inpu
}
btScalar d0 = 0.f;
{
btVector3 seperatingAxisInA = (-normalInB) * input.m_transformA.getBasis();
btVector3 seperatingAxisInB = normalInB * input.m_transformB.getBasis();
btVector3 separatingAxisInA = (-normalInB) * input.m_transformA.getBasis();
btVector3 separatingAxisInB = normalInB * input.m_transformB.getBasis();
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
btVector3 pWorld = localTransA(pInA);
btVector3 qWorld = localTransB(qInB);

4
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h vendored
View File

@ -64,9 +64,9 @@ public:
{
m_minkowskiB = minkB;
}
void setCachedSeperatingAxis(const btVector3& seperatingAxis)
void setCachedSeparatingAxis(const btVector3& separatingAxis)
{
m_cachedSeparatingAxis = seperatingAxis;
m_cachedSeparatingAxis = separatingAxis;
}
const btVector3& getCachedSeparatingAxis() const

3
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h vendored
View File

@ -55,6 +55,7 @@ public:
: m_userPersistentData(0),
m_contactPointFlags(0),
m_appliedImpulse(0.f),
m_prevRHS(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
@ -79,6 +80,7 @@ public:
m_userPersistentData(0),
m_contactPointFlags(0),
m_appliedImpulse(0.f),
m_prevRHS(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
@ -114,6 +116,7 @@ public:
int m_contactPointFlags;
btScalar m_appliedImpulse;
btScalar m_prevRHS;
btScalar m_appliedImpulseLateral1;
btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1;

36
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp vendored
View File

@ -65,7 +65,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
btScalar minProj = btScalar(BT_LARGE_FLOAT);
btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
btVector3 minA, minB;
btVector3 seperatingAxisInA, seperatingAxisInB;
btVector3 separatingAxisInA, separatingAxisInB;
btVector3 pInA, qInB, pWorld, qWorld, w;
#ifndef __SPU__
@ -75,8 +75,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
btVector3 separatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
btVector3 separatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int i;
int numSampleDirections = NUM_UNITSPHERE_POINTS;
@ -84,8 +84,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
for (i = 0; i < numSampleDirections; i++)
{
btVector3 norm = getPenetrationDirections()[i];
seperatingAxisInABatch[i] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[i] = norm * transB.getBasis();
separatingAxisInABatch[i] = (-norm) * transA.getBasis();
separatingAxisInBBatch[i] = norm * transB.getBasis();
}
{
@ -98,8 +98,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
convexA->getPreferredPenetrationDirection(i, norm);
norm = transA.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
numSampleDirections++;
}
}
@ -115,15 +115,15 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
convexB->getPreferredPenetrationDirection(i, norm);
norm = transB.getBasis() * norm;
getPenetrationDirections()[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
numSampleDirections++;
}
}
}
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (i = 0; i < numSampleDirections; i++)
{
@ -134,8 +134,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
}
if (norm.length2() > 0.01)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
separatingAxisInA = separatingAxisInABatch[i];
separatingAxisInB = separatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
@ -199,10 +199,10 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
for (int i = 0; i < numSampleDirections; i++)
{
const btVector3& norm = getPenetrationDirections()[i];
seperatingAxisInA = (-norm) * transA.getBasis();
seperatingAxisInB = norm * transB.getBasis();
pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
separatingAxisInA = (-norm) * transA.getBasis();
separatingAxisInB = norm * transB.getBasis();
pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
pWorld = transA(pInA);
qWorld = transB(qInB);
w = qWorld - pWorld;
@ -259,7 +259,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); //minProj;
btIntermediateResult res;
gjkdet.setCachedSeperatingAxis(-minNorm);
gjkdet.setCachedSeparatingAxis(-minNorm);
gjkdet.getClosestPoints(input, res, debugDraw);
btScalar correctedMinNorm = minProj - res.m_depth;

10
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btMprPenetration.h vendored
View File

@ -309,11 +309,11 @@ inline void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b,
const btMprCollisionDescription &colDesc,
const btVector3 &dir, btMprSupport_t *supp)
{
btVector3 seperatingAxisInA = dir * a.getWorldTransform().getBasis();
btVector3 seperatingAxisInB = -dir * b.getWorldTransform().getBasis();
btVector3 separatingAxisInA = dir * a.getWorldTransform().getBasis();
btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis();
btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA);
btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB);
btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA);
btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB);
supp->v1 = a.getWorldTransform()(pInA);
supp->v2 = b.getWorldTransform()(qInB);
@ -467,7 +467,7 @@ static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b,
for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
//while (1)
{
// compute direction outside the portal (from v0 throught v1,v2,v3
// compute direction outside the portal (from v0 through v1,v2,v3
// face)
btPortalDir(portal, &dir);

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

@ -325,6 +325,7 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma
{
const btManifoldPoint& pt = manifold->getContactPoint(i);
dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS;
dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
@ -371,6 +372,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleDa
btManifoldPoint& pt = m_pointCache[i];
pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
@ -416,6 +418,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat
btManifoldPoint& pt = m_pointCache[i];
pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);

5
thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h vendored
View File

@ -173,6 +173,7 @@ public:
//get rid of duplicated userPersistentData pointer
m_pointCache[lastUsedIndex].m_userPersistentData = 0;
m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
m_pointCache[lastUsedIndex].m_prevRHS = 0.f;
m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
@ -195,6 +196,7 @@ public:
#ifdef MAINTAIN_PERSISTENCY
int lifeTime = m_pointCache[insertIndex].getLifeTime();
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS;
btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
@ -223,6 +225,7 @@ public:
m_pointCache[insertIndex] = newPoint;
m_pointCache[insertIndex].m_userPersistentData = cache;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_prevRHS = prevRHS;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
}
@ -276,6 +279,7 @@ struct btPersistentManifoldDoubleData
btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
double m_pointCacheDistance[4];
double m_pointCacheAppliedImpulse[4];
double m_pointCachePrevRHS[4];
double m_pointCacheCombinedFriction[4];
double m_pointCacheCombinedRollingFriction[4];
double m_pointCacheCombinedSpinningFriction[4];
@ -322,6 +326,7 @@ struct btPersistentManifoldFloatData
btVector3FloatData m_pointCacheLateralFrictionDir2[4];
float m_pointCacheDistance[4];
float m_pointCacheAppliedImpulse[4];
float m_pointCachePrevRHS[4];
float m_pointCacheCombinedFriction[4];
float m_pointCacheCombinedRollingFriction[4];
float m_pointCacheCombinedSpinningFriction[4];

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

@ -890,6 +890,8 @@ static void setupSpatialGridBatchesMt(
btVector3 gridExtent = bboxMax - bboxMin;
gridExtent.setMax(btVector3(btScalar(1), btScalar(1), btScalar(1)));
btVector3 gridCellSize = consExtent;
int gridDim[3];
gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x());

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

@ -30,7 +30,8 @@ enum btSolverMode
SOLVER_SIMD = 256,
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024,
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048,
SOLVER_USE_ARTICULATED_WARMSTARTING = 4096,
};
struct btContactSolverInfoData
@ -45,6 +46,7 @@ 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_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
@ -54,7 +56,7 @@ struct btContactSolverInfoData
btScalar m_splitImpulseTurnErp;
btScalar m_linearSlop;
btScalar m_warmstartingFactor;
btScalar m_articulatedWarmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
@ -80,6 +82,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_numIterations = 10;
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.2);
m_deformable_erp = btScalar(0.);
m_globalCfm = btScalar(0.);
m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default
m_frictionCFM = btScalar(0.);
@ -89,6 +92,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_splitImpulseTurnErp = 0.1f;
m_linearSlop = btScalar(0.0);
m_warmstartingFactor = btScalar(0.85);
m_articulatedWarmstartingFactor = btScalar(0.85);
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD; // | SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2; //unused as of 2.81
@ -120,6 +124,7 @@ struct btContactSolverInfoDoubleData
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_articulatedWarmstartingFactor;
double m_maxGyroscopicForce; ///it is only used for 'explicit' version of gyroscopic force
double m_singleAxisRollingFrictionThreshold;
@ -150,16 +155,17 @@ struct btContactSolverInfoFloatData
float m_linearSlop;
float m_warmstartingFactor;
float m_articulatedWarmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;
float m_singleAxisRollingFrictionThreshold;
int m_numIterations;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
#endif //BT_CONTACT_SOLVER_INFO

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

@ -32,7 +32,7 @@ Cons:
/*
2007-09-09
btGeneric6DofConstraint Refactored by Francisco Leon
btGeneric6DofConstraint Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
@ -311,9 +311,9 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
case RO_XYZ:
{
//Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
//The two planes are non-homologous, so this is a Tait-Bryan angle formalism and not a proper Euler
//The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler
//Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait-Bryan angles)
//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait Bryan angles)
// x' = Nperp = N.cross(axis2)
// y' = N = axis2.cross(axis0)
// z' = z

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

File diff suppressed because it is too large Load Diff

124
thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h vendored
View File

@ -29,68 +29,6 @@ class btCollisionObject;
typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
struct btSISolverSingleIterationData
{
btAlignedObjectArray<btSolverBody>& m_tmpSolverBodyPool;
btConstraintArray& m_tmpSolverContactConstraintPool;
btConstraintArray& m_tmpSolverNonContactConstraintPool;
btConstraintArray& m_tmpSolverContactFrictionConstraintPool;
btConstraintArray& m_tmpSolverContactRollingFrictionConstraintPool;
btAlignedObjectArray<int>& m_orderTmpConstraintPool;
btAlignedObjectArray<int>& m_orderNonContactConstraintPool;
btAlignedObjectArray<int>& m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& m_tmpConstraintSizesPool;
unsigned long& m_seed;
btSingleConstraintRowSolver& m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver& m_resolveSingleConstraintRowLowerLimit;
btSingleConstraintRowSolver& m_resolveSplitPenetrationImpulse;
btAlignedObjectArray<int>& m_kinematicBodyUniqueIdToSolverBodyTable;
int& m_fixedBodyId;
int& m_maxOverrideNumSolverIterations;
int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
int getSolverBody(btCollisionObject& body) const;
btSISolverSingleIterationData(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
btConstraintArray& tmpSolverContactConstraintPool,
btConstraintArray& tmpSolverNonContactConstraintPool,
btConstraintArray& tmpSolverContactFrictionConstraintPool,
btConstraintArray& tmpSolverContactRollingFrictionConstraintPool,
btAlignedObjectArray<int>& orderTmpConstraintPool,
btAlignedObjectArray<int>& orderNonContactConstraintPool,
btAlignedObjectArray<int>& orderFrictionConstraintPool,
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& tmpConstraintSizesPool,
btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric,
btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit,
btSingleConstraintRowSolver& resolveSplitPenetrationImpulse,
btAlignedObjectArray<int>& kinematicBodyUniqueIdToSolverBodyTable,
unsigned long& seed,
int& fixedBodyId,
int& maxOverrideNumSolverIterations
)
:m_tmpSolverBodyPool(tmpSolverBodyPool),
m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool),
m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool),
m_tmpSolverContactFrictionConstraintPool(tmpSolverContactFrictionConstraintPool),
m_tmpSolverContactRollingFrictionConstraintPool(tmpSolverContactRollingFrictionConstraintPool),
m_orderTmpConstraintPool(orderTmpConstraintPool),
m_orderNonContactConstraintPool(orderNonContactConstraintPool),
m_orderFrictionConstraintPool(orderFrictionConstraintPool),
m_tmpConstraintSizesPool(tmpConstraintSizesPool),
m_seed(seed),
m_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric),
m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit),
m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse),
m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable),
m_fixedBodyId(fixedBodyId),
m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations)
{
}
};
struct btSolverAnalyticsData
{
btSolverAnalyticsData()
@ -178,7 +116,6 @@ protected:
virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
@ -204,8 +141,7 @@ protected:
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
}
public:
protected:
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
@ -213,7 +149,6 @@ public:
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
@ -225,51 +160,12 @@ public:
virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
static btScalar solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation,
const btVector3& rel_pos1, const btVector3& rel_pos2);
static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.);
static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity, btScalar cfmSlip);
static void setupFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip);
static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
static void setFrictionConstraintImpulseInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool,
btSolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
static void convertJointInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
int& maxOverrideNumSolverIterations,
btSolverConstraint* currentConstraintRow,
btTypedConstraint* constraint,
const btTypedConstraint::btConstraintInfo1& info1,
int solverBodyIdA,
int solverBodyIdB,
const btContactSolverInfo& infoGlobal);
static btScalar solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
static void writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void writeBackBodiesInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
///clear internal cached data and reset random seed
virtual void reset();
unsigned long btRand2();
int btRandInt2(int n);
static unsigned long btRand2a(unsigned long& seed);
static int btRandInt2a(int n, unsigned long& seed);
int btRandInt2(int n);
void setRandSeed(unsigned long seed)
{
@ -305,18 +201,14 @@ public:
///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
btSolverAnalyticsData m_analyticsData;
};

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

@ -1436,8 +1436,6 @@ void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize
worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse;
// Fill padding with zeros to appease msan.
memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding));
#ifdef BT_USE_DOUBLE_PRECISION
const char* structType = "btDynamicsWorldDoubleData";

16
thirdparty/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h vendored
View File

@ -17,7 +17,6 @@ subject to the following restrictions:
#define BT_DISCRETE_DYNAMICS_WORLD_H
#include "btDynamicsWorld.h"
class btDispatcher;
class btOverlappingPairCache;
class btConstraintSolver;
@ -26,6 +25,7 @@ class btTypedConstraint;
class btActionInterface;
class btPersistentManifold;
class btIDebugDraw;
struct InplaceSolverIslandCallback;
#include "LinearMath/btAlignedObjectArray.h"
@ -76,7 +76,7 @@ protected:
virtual void calculateSimulationIslands();
virtual void solveConstraints(btContactSolverInfo & solverInfo);
virtual void updateActivationState(btScalar timeStep);
@ -107,6 +107,8 @@ public:
///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
virtual void solveConstraints(btContactSolverInfo & solverInfo);
virtual void synchronizeMotionStates();
///this can be useful to synchronize a single rigid body -> graphics object
@ -227,6 +229,16 @@ public:
{
return m_latencyMotionStateInterpolation;
}
btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies()
{
return m_nonStaticRigidBodies;
}
const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const
{
return m_nonStaticRigidBodies;
}
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H

3
thirdparty/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h vendored
View File

@ -34,7 +34,8 @@ enum btDynamicsWorldType
BT_CONTINUOUS_DYNAMICS_WORLD = 3,
BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
BT_GPU_DYNAMICS_WORLD = 5,
BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6
BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6,
BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7
};
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.

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

@ -206,6 +206,14 @@ void btRigidBody::applyGravity()
applyCentralForce(m_gravity);
}
void btRigidBody::clearGravity()
{
if (isStaticOrKinematicObject())
return;
applyCentralForce(-m_gravity);
}
void btRigidBody::proceedToTransform(const btTransform& newTrans)
{
setCenterOfMassTransform(newTrans);

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

@ -206,6 +206,8 @@ public:
void applyGravity();
void clearGravity();
void setGravity(const btVector3& acceleration);
const btVector3& getGravity() const
@ -259,6 +261,7 @@ public:
m_invMass = m_linearFactor * m_inverseMass;
}
btScalar getInvMass() const { return m_inverseMass; }
btScalar getMass() const { return m_inverseMass == btScalar(0.) ? btScalar(0.) : btScalar(1.0) / m_inverseMass; }
const btMatrix3x3& getInvInertiaTensorWorld() const
{
return m_invInertiaTensorWorld;
@ -332,6 +335,48 @@ public:
}
}
void applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass != btScalar(0.))
{
applyCentralPushImpulse(impulse);
if (m_angularFactor)
{
applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor));
}
}
}
btVector3 getPushVelocity()
{
return m_pushVelocity;
}
btVector3 getTurnVelocity()
{
return m_turnVelocity;
}
void setPushVelocity(const btVector3& v)
{
m_pushVelocity = v;
}
void setTurnVelocity(const btVector3& v)
{
m_turnVelocity = v;
}
void applyCentralPushImpulse(const btVector3& impulse)
{
m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
}
void applyTorqueTurnImpulse(const btVector3& torque)
{
m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
}
void clearForces()
{
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));

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

@ -100,6 +100,8 @@ btMultiBody::btMultiBody(int n_links,
m_baseName(0),
m_basePos(0, 0, 0),
m_baseQuat(0, 0, 0, 1),
m_basePos_interpolate(0, 0, 0),
m_baseQuat_interpolate(0, 0, 0, 1),
m_baseMass(mass),
m_baseInertia(inertia),
@ -449,6 +451,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
return m_links[i].m_cachedRotParentToThis;
}
const btVector3 &btMultiBody::getInterpolateRVector(int i) const
{
return m_links[i].m_cachedRVector_interpolate;
}
const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const
{
return m_links[i].m_cachedRotParentToThis_interpolate;
}
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
{
btAssert(i >= -1);
@ -1581,6 +1593,158 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
//printf("]\n");
/////////////////
}
void btMultiBody::predictPositionsMultiDof(btScalar dt)
{
int num_links = getNumLinks();
// step position by adding dt * velocity
//btVector3 v = getBaseVel();
//m_basePos += dt * v;
//
btScalar *pBasePos;
btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
// reset to current position
for (int i = 0; i < 3; ++i)
{
m_basePos_interpolate[i] = m_basePos[i];
}
pBasePos = m_basePos_interpolate;
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)
struct
{
//"exponential map" based on btTransformUtil::integrateTransform(..)
void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
{
//baseBody => quat is alias and omega is global coor
//!baseBody => quat is alibi and omega is local coor
btVector3 axis;
btVector3 angvel;
if (!baseBody)
angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
else
angvel = omega;
btScalar fAngle = angvel.length();
//limit the angular motion
if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
{
fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
}
if (fAngle < btScalar(0.001))
{
// use Taylor's expansions of sync function
axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
}
if (!baseBody)
quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
else
quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
quat.normalize();
}
} pQuatUpdateFun;
///////////////////////////////
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
//
btScalar *pBaseQuat;
// reset to current orientation
for (int i = 0; i < 4; ++i)
{
m_baseQuat_interpolate[i] = m_baseQuat[i];
}
pBaseQuat = m_baseQuat_interpolate;
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)
{
btScalar *pJointPos;
pJointPos = &m_links[i].m_jointPos_interpolate[0];
btScalar *pJointVel = getJointVelMultiDof(i);
switch (m_links[i].m_jointType)
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
//reset to current pos
pJointPos[0] = m_links[i].m_jointPos[0];
btScalar jointVel = pJointVel[0];
pJointPos[0] += dt * jointVel;
break;
}
case btMultibodyLink::eSpherical:
{
//reset to current pos
for (int j = 0; j < 4; ++j)
{
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:
{
}
}
m_links[i].updateInterpolationCacheMultiDof();
}
}
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
{
@ -1591,7 +1755,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
//
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];
@ -1670,7 +1834,9 @@ 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 = (pq ? pq : &m_links[i].m_jointPos[0]);
btScalar *pJointPos;
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
switch (m_links[i].m_jointType)
@ -1678,12 +1844,14 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
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;
@ -1974,6 +2142,7 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getBaseCollider()->setWorldTransform(tr);
getBaseCollider()->setInterpolationWorldTransform(tr);
}
for (int k = 0; k < getNumLinks(); k++)
@ -2002,6 +2171,58 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setInterpolationWorldTransform(tr);
}
}
}
void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
{
world_to_local.resize(getNumLinks() + 1);
local_origin.resize(getNumLinks() + 1);
world_to_local[0] = getInterpolateWorldToBaseRot();
local_origin[0] = getInterpolateBasePos();
if (getBaseCollider())
{
btVector3 posr = local_origin[0];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getBaseCollider()->setInterpolationWorldTransform(tr);
}
for (int k = 0; k < getNumLinks(); k++)
{
const int parent = getParent(k);
world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
}
for (int m = 0; m < getNumLinks(); m++)
{
btMultiBodyLinkCollider *col = getLink(m).m_collider;
if (col)
{
int link = col->m_link;
btAssert(link == m);
int index = link + 1;
btVector3 posr = local_origin[index];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setInterpolationWorldTransform(tr);
}
}
}

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

@ -193,12 +193,24 @@ public:
const btQuaternion &getWorldToBaseRot() const
{
return m_baseQuat;
} // rotates world vectors into base frame
}
const btVector3 &getInterpolateBasePos() const
{
return m_basePos_interpolate;
} // in world frame
const btQuaternion &getInterpolateWorldToBaseRot() const
{
return m_baseQuat_interpolate;
}
// rotates world vectors into base frame
btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame
void setBasePos(const btVector3 &pos)
{
m_basePos = pos;
m_basePos_interpolate = pos;
}
void setBaseWorldTransform(const btTransform &tr)
@ -224,6 +236,7 @@ public:
void setWorldToBaseRot(const btQuaternion &rot)
{
m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
m_baseQuat_interpolate = rot;
}
void setBaseOmega(const btVector3 &omega)
{
@ -260,6 +273,11 @@ public:
{
return &m_realBuf[0];
}
const btScalar *getDeltaVelocityVector() const
{
return &m_deltaV[0];
}
/* btScalar * getVelocityVector()
{
return &real_buf[0];
@ -273,6 +291,8 @@ public:
const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
//
// transform vectors in local frame of link i to world frame (or vice versa)
@ -422,6 +442,9 @@ public:
// timestep the positions (given current velocities).
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
// predict the positions
void predictPositionsMultiDof(btScalar dt);
//
// contacts
//
@ -581,6 +604,7 @@ public:
void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
virtual int calculateSerializeBufferSize() const;
@ -664,7 +688,9 @@ private:
const char *m_baseName; //memory needs to be manager by user!
btVector3 m_basePos; // position of COM of base (world frame)
btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame)
btQuaternion m_baseQuat; // rotates world points into base frame
btQuaternion m_baseQuat_interpolate;
btScalar m_baseMass; // mass of the base
btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)

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

@ -342,40 +342,6 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra
solverConstraint.m_friction = 0.f; //cp.m_combinedFriction;
}
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
}
}
} else
*/
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;

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

@ -22,6 +22,8 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "LinearMath/btQuickprof.h"
#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
#include "LinearMath/btScalar.h"
btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
@ -491,11 +493,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
return deltaVel;
}
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("setupMultiBodyContactConstraint");
btVector3 rel_pos1;
@ -781,48 +779,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
}
///warm starting (or zero if disabled)
//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
if (0) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
}
else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
}
else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
}
}
}
else
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
@ -874,6 +830,54 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
}
if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
{
if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
if (solverConstraint.m_appliedImpulse < 0)
solverConstraint.m_appliedImpulse = 0;
}
else
{
solverConstraint.m_appliedImpulse = 0.f;
}
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
}
else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
}
else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
}
}
}
else
{
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
}
}
void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
@ -944,13 +948,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis0 = -constraintNormal;
btVector3 torqueAxis0 = constraintNormal;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
}
else
{
btVector3 torqueAxis0 = -constraintNormal;
btVector3 torqueAxis0 = constraintNormal;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
@ -986,13 +990,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
btVector3 torqueAxis1 = constraintNormal;
btVector3 torqueAxis1 = -constraintNormal;
solverConstraint.m_relpos2CrossNormal = torqueAxis1;
solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
}
else
{
btVector3 torqueAxis1 = constraintNormal;
btVector3 torqueAxis1 = -constraintNormal;
solverConstraint.m_relpos2CrossNormal = torqueAxis1;
solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
@ -1130,7 +1134,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
}
}
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("addMultiBodyFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
@ -1161,7 +1165,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
solverConstraint.m_originalContactPoint = &cp;
setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
return solverConstraint;
}
@ -1297,7 +1301,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
solverConstraint.m_originalContactPoint = &cp;
bool isFriction = false;
setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction);
setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
// const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB();
@ -1371,13 +1375,13 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
{
applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
@ -1388,26 +1392,27 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
}
else
{
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
//todo:
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
}
#endif //ENABLE_FRICTION
}
else
{
// Reset quantities related to warmstart as 0.
cp.m_appliedImpulse = 0;
cp.m_prevRHS = 0;
}
}
}
void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
{
//btPersistentManifold* manifold = 0;
for (int i = 0; i < numManifolds; i++)
{
btPersistentManifold* manifold = manifoldPtr[i];
@ -1434,6 +1439,51 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
}
// Warmstart for noncontact constraints
if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
{
for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
{
btMultiBodySolverConstraint& solverConstraint =
m_multiBodyNonContactConstraints[i];
solverConstraint.m_appliedImpulse =
solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
infoGlobal.m_articulatedWarmstartingFactor;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
int ndofA = multiBodyA->getNumDofs() + 6;
btScalar* deltaV =
&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
btScalar impulse = solverConstraint.m_appliedImpulse;
multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
}
if (multiBodyB)
{
int ndofB = multiBodyB->getNumDofs() + 6;
btScalar* deltaV =
&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
btScalar impulse = solverConstraint.m_appliedImpulse;
multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
}
}
}
}
else
{
for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
{
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
solverConstraint.m_appliedImpulse = 0;
}
}
}
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
@ -1556,7 +1606,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
}
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
BT_PROFILE("warm starting write back");
for (int j = 0; j < numPoolConstraints; j++)
@ -1565,6 +1615,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
pt->m_prevRHS = solverConstraint.m_rhs;
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
@ -1576,9 +1627,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
pt->m_appliedImpulseLateral2 = 0;
}
}
//do a callback here?
}
#if 0
//multibody joint feedback
{

7
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h vendored
View File

@ -49,7 +49,7 @@ protected:
void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
@ -66,7 +66,9 @@ protected:
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
const btScalar& appliedImpulse,
btManifoldPoint& cp,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
@ -82,7 +84,6 @@ protected:
void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof);
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime);

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

@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
m_multiBodies.remove(body);
}
void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
predictMultiBodyTransforms(timeStep);
}
void btMultiBodyDynamicsWorld::calculateSimulationIslands()
{
BT_PROFILE("calculateSimulationIslands");
@ -163,218 +169,6 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
btDiscreteDynamicsWorld::updateActivationState(timeStep);
}
SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
{
int islandId;
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
return islandId;
}
class btSortConstraintOnIslandPredicate2
{
public:
bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
{
int rIslandId0, lIslandId0;
rIslandId0 = btGetConstraintIslandId2(rhs);
lIslandId0 = btGetConstraintIslandId2(lhs);
return lIslandId0 < rIslandId0;
}
};
SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
{
int islandId;
int islandTagA = lhs->getIslandIdA();
int islandTagB = lhs->getIslandIdB();
islandId = islandTagA >= 0 ? islandTagA : islandTagB;
return islandId;
}
class btSortMultiBodyConstraintOnIslandPredicate
{
public:
bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
{
int rIslandId0, lIslandId0;
rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
return lIslandId0 < rIslandId0;
}
};
struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
{
btContactSolverInfo* m_solverInfo;
btMultiBodyConstraintSolver* m_solver;
btMultiBodyConstraint** m_multiBodySortedConstraints;
int m_numMultiBodyConstraints;
btTypedConstraint** m_sortedConstraints;
int m_numConstraints;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
: m_solverInfo(NULL),
m_solver(solver),
m_multiBodySortedConstraints(NULL),
m_numConstraints(0),
m_debugDrawer(NULL),
m_dispatcher(dispatcher)
{
}
MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
{
btAssert(0);
(void)other;
return *this;
}
SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
{
m_islandAnalyticsData.clear();
btAssert(solverInfo);
m_solverInfo = solverInfo;
m_multiBodySortedConstraints = sortedMultiBodyConstraints;
m_numMultiBodyConstraints = numMultiBodyConstraints;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
{
m_solver = solver;
}
virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
{
if (islandId < 0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
if (m_solverInfo->m_reportSolverAnalytics&1)
{
m_solver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
}
else
{
//also add all non-contact constraints/joints for this island
btTypedConstraint** startConstraint = 0;
btMultiBodyConstraint** startMultiBodyConstraint = 0;
int numCurConstraints = 0;
int numCurMultiBodyConstraints = 0;
int i;
//find the first constraint for this island
for (i = 0; i < m_numConstraints; i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
startConstraint = &m_sortedConstraints[i];
break;
}
}
//count the number of constraints in this island
for (; i < m_numConstraints; i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
for (i = 0; i < m_numMultiBodyConstraints; i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
break;
}
}
//count the number of multi body constraints in this island
for (; i < m_numMultiBodyConstraints; i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
numCurMultiBodyConstraints++;
}
}
//if (m_solverInfo->m_minimumSolverBatchSize<=1)
//{
// m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
//} else
{
for (i = 0; i < numBodies; i++)
m_bodies.push_back(bodies[i]);
for (i = 0; i < numManifolds; i++)
m_manifolds.push_back(manifolds[i]);
for (i = 0; i < numCurConstraints; i++)
m_constraints.push_back(startConstraint[i]);
for (i = 0; i < numCurMultiBodyConstraints; i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints(islandId);
}
else
{
//printf("deferred\n");
}
}
}
}
void processConstraints(int islandId=-1)
{
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup(bodies, m_bodies.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))
{
m_solver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};
void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
{
islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
@ -420,6 +214,71 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
}
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
solveExternalForces(solverInfo);
buildIslands();
solveInternalConstraints(solverInfo);
}
void btMultiBodyDynamicsWorld::buildIslands()
{
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
}
void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
{
/// solve all the constraints for this island
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b = 0; b < bod->getNumLinks(); b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks() + 1);
m_scratch_m.resize(bod->getNumLinks() + 1);
if (bod->internalNeedsJointFeedback())
{
if (!bod->isUsingRK4Integration())
{
if (bod->internalNeedsJointFeedback())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
}
}
}
}
}
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
{
forwardKinematics();
@ -703,67 +562,16 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
} //if (!isSleeping)
}
}
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b = 0; b < bod->getNumLinks(); b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks() + 1);
m_scratch_m.resize(bod->getNumLinks() + 1);
if (bod->internalNeedsJointFeedback())
{
if (!bod->isUsingRK4Integration())
{
if (bod->internalNeedsJointFeedback())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
}
}
}
}
}
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
integrateMultiBodyTransforms(timeStep);
}
void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
@ -787,8 +595,6 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
int nLinks = bod->getNumLinks();
///base + num m_links
{
if (!bod->isPosUpdated())
bod->stepPositionsMultiDof(timeStep);
else
@ -799,11 +605,10 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
bod->stepPositionsMultiDof(1, 0, pRealBuf);
bod->setPosUpdated(false);
}
}
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);
}
else
@ -812,6 +617,39 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
}
void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
for (int b = 0; b < m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b = 0; b < bod->getNumLinks(); b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
int nLinks = bod->getNumLinks();
bod->predictPositionsMultiDof(timeStep);
m_scratch_world_to_local.resize(nLinks + 1);
m_scratch_local_origin.resize(nLinks + 1);
bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
}
else
{
bod->clearVelocities();
}
}
}
void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
@ -1029,3 +867,8 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
}
}
}
//
//void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
//{
// m_islandManager->setSplitIslands(split);
//}

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

@ -17,6 +17,7 @@ subject to the following restrictions:
#define BT_MULTIBODY_DYNAMICS_WORLD_H
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h"
#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
@ -47,7 +48,7 @@ protected:
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void serializeMultiBodies(btSerializer* serializer);
@ -56,6 +57,8 @@ public:
virtual ~btMultiBodyDynamicsWorld();
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter);
virtual void removeMultiBody(btMultiBody* body);
@ -95,7 +98,10 @@ public:
virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
void integrateMultiBodyTransforms(btScalar timeStep);
void predictMultiBodyTransforms(btScalar timeStep);
virtual void predictUnconstraintMotion(btScalar timeStep);
virtual void debugDrawWorld();
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
@ -111,5 +117,8 @@ public:
virtual void setConstraintSolver(btConstraintSolver* solver);
virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
virtual void solveExternalForces(btContactSolverInfo& solverInfo);
virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
void buildIslands();
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

247
thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h vendored Normal file
View File

@ -0,0 +1,247 @@
/*
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_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "btMultiBodyConstraintSolver.h"
SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
{
int islandId;
const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
return islandId;
}
class btSortConstraintOnIslandPredicate2
{
public:
bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
{
int rIslandId0, lIslandId0;
rIslandId0 = btGetConstraintIslandId2(rhs);
lIslandId0 = btGetConstraintIslandId2(lhs);
return lIslandId0 < rIslandId0;
}
};
SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
{
int islandId;
int islandTagA = lhs->getIslandIdA();
int islandTagB = lhs->getIslandIdB();
islandId = islandTagA >= 0 ? islandTagA : islandTagB;
return islandId;
}
class btSortMultiBodyConstraintOnIslandPredicate
{
public:
bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
{
int rIslandId0, lIslandId0;
rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
return lIslandId0 < rIslandId0;
}
};
struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
{
btContactSolverInfo* m_solverInfo;
btMultiBodyConstraintSolver* m_solver;
btMultiBodyConstraint** m_multiBodySortedConstraints;
int m_numMultiBodyConstraints;
btTypedConstraint** m_sortedConstraints;
int m_numConstraints;
btIDebugDraw* m_debugDrawer;
btDispatcher* m_dispatcher;
btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btCollisionObject*> m_softBodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
: m_solverInfo(NULL),
m_solver(solver),
m_multiBodySortedConstraints(NULL),
m_numConstraints(0),
m_debugDrawer(NULL),
m_dispatcher(dispatcher)
{
}
MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
{
btAssert(0);
(void)other;
return *this;
}
SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
{
m_islandAnalyticsData.clear();
btAssert(solverInfo);
m_solverInfo = solverInfo;
m_multiBodySortedConstraints = sortedMultiBodyConstraints;
m_numMultiBodyConstraints = numMultiBodyConstraints;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
{
m_solver = solver;
}
virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
{
if (islandId < 0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
if (m_solverInfo->m_reportSolverAnalytics&1)
{
m_solver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
}
else
{
//also add all non-contact constraints/joints for this island
btTypedConstraint** startConstraint = 0;
btMultiBodyConstraint** startMultiBodyConstraint = 0;
int numCurConstraints = 0;
int numCurMultiBodyConstraints = 0;
int i;
//find the first constraint for this island
for (i = 0; i < m_numConstraints; i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
startConstraint = &m_sortedConstraints[i];
break;
}
}
//count the number of constraints in this island
for (; i < m_numConstraints; i++)
{
if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
for (i = 0; i < m_numMultiBodyConstraints; i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
break;
}
}
//count the number of multi body constraints in this island
for (; i < m_numMultiBodyConstraints; i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
numCurMultiBodyConstraints++;
}
}
//if (m_solverInfo->m_minimumSolverBatchSize<=1)
//{
// m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
//} else
{
for (i = 0; i < numBodies; i++)
{
bool isSoftBodyType = (bodies[i]->getInternalType() & btCollisionObject::CO_SOFT_BODY);
if (!isSoftBodyType)
{
m_bodies.push_back(bodies[i]);
}
else
{
m_softBodies.push_back(bodies[i]);
}
}
for (i = 0; i < numManifolds; i++)
m_manifolds.push_back(manifolds[i]);
for (i = 0; i < numCurConstraints; i++)
m_constraints.push_back(startConstraint[i]);
for (i = 0; i < numCurMultiBodyConstraints; i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints(islandId);
}
else
{
//printf("deferred\n");
}
}
}
}
virtual void processConstraints(int islandId=-1)
{
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
m_solver->solveMultiBodyGroup(bodies, m_bodies.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))
{
m_solver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
m_bodies.resize(0);
m_softBodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};
#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */

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

@ -112,6 +112,10 @@ struct btMultibodyLink
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
// predicted verstion
btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame
btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame.
btVector3 m_appliedForce; // In WORLD frame
btVector3 m_appliedTorque; // In WORLD frame
@ -119,6 +123,7 @@ struct btMultibodyLink
btVector3 m_appliedConstraintTorque; // In WORLD frame
btScalar m_jointPos[7];
btScalar m_jointPos_interpolate[7];
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
//It gets set to zero after each internal stepSimulation call
@ -152,6 +157,7 @@ struct btMultibodyLink
m_parent(-1),
m_zeroRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis_interpolate(0, 0, 0, 1),
m_collider(0),
m_flags(0),
m_dofCount(0),
@ -174,6 +180,7 @@ struct btMultibodyLink
m_dVector.setValue(0, 0, 0);
m_eVector.setValue(0, 0, 0);
m_cachedRVector.setValue(0, 0, 0);
m_cachedRVector_interpolate.setValue(0, 0, 0);
m_appliedForce.setValue(0, 0, 0);
m_appliedTorque.setValue(0, 0, 0);
m_appliedConstraintForce.setValue(0, 0, 0);
@ -189,41 +196,95 @@ struct btMultibodyLink
void updateCacheMultiDof(btScalar *pq = 0)
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
btQuaternion& cachedRot = m_cachedRotParentToThis;
btVector3& cachedVector = m_cachedRVector;
switch (m_jointType)
{
case eRevolute:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
break;
}
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
break;
}
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
break;
}
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
cachedRot = m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
break;
}
default:
{
//invalid type
btAssert(0);
}
}
m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis;
m_cachedRVector_interpolate = m_cachedRVector;
}
void updateInterpolationCacheMultiDof()
{
btScalar *pJointPos = &m_jointPos_interpolate[0];
btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
btVector3& cachedVector = m_cachedRVector_interpolate;
switch (m_jointType)
{
case eRevolute:
{
cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
break;
}
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
break;
}
case ePlanar:
{
cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
break;
}
case eFixed:
{
cachedRot = m_zeroRotParentToThis;
cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
break;
}

2
thirdparty/bullet/BulletDynamics/MLCPSolvers/btLemkeSolver.h vendored
View File

@ -20,7 +20,7 @@ subject to the following restrictions:
#include "btMLCPSolverInterface.h"
#include "btLemkeAlgorithm.h"
///The btLemkeSolver is based on "Fast Implementation of Lemke's Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
class btLemkeSolver : public btMLCPSolverInterface

46
thirdparty/bullet/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h vendored Normal file
View File

@ -0,0 +1,46 @@
//
// DeformableBodyInplaceSolverIslandCallback.h
// BulletSoftBody
//
// Created by Xuchen Han on 12/16/19.
//
#ifndef DeformableBodyInplaceSolverIslandCallback_h
#define DeformableBodyInplaceSolverIslandCallback_h
struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolverIslandCallback
{
btDeformableMultiBodyConstraintSolver* m_deformableSolver;
DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
{
}
virtual void processConstraints(int islandId=-1)
{
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
//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))
{
m_deformableSolver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
m_bodies.resize(0);
m_softBodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};
#endif /* DeformableBodyInplaceSolverIslandCallback_h */

106
thirdparty/bullet/BulletSoftBody/btCGProjection.h vendored Normal file
View File

@ -0,0 +1,106 @@
/*
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_CG_PROJECTION_H
#define BT_CG_PROJECTION_H
#include "btSoftBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
struct DeformableContactConstraint
{
const btSoftBody::Node* m_node;
btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
btAlignedObjectArray<btVector3> m_total_normal_dv;
btAlignedObjectArray<btVector3> m_total_tangent_dv;
btAlignedObjectArray<bool> m_static;
btAlignedObjectArray<bool> m_can_be_dynamic;
DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node)
{
append(rcontact);
}
DeformableContactConstraint(): m_node(NULL)
{
m_contact.push_back(NULL);
}
void append(const btSoftBody::RContact& rcontact)
{
m_contact.push_back(&rcontact);
m_total_normal_dv.push_back(btVector3(0,0,0));
m_total_tangent_dv.push_back(btVector3(0,0,0));
m_static.push_back(false);
m_can_be_dynamic.push_back(true);
}
void replace(const btSoftBody::RContact& rcontact)
{
m_contact.clear();
m_total_normal_dv.clear();
m_total_tangent_dv.clear();
m_static.clear();
m_can_be_dynamic.clear();
append(rcontact);
}
~DeformableContactConstraint()
{
}
};
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;
}
};
#endif /* btCGProjection_h */

158
thirdparty/bullet/BulletSoftBody/btConjugateGradient.h vendored Normal file
View File

@ -0,0 +1,158 @@
/*
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_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"
template <class MatrixX>
class btConjugateGradient
{
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;
}
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;
}
};
#endif /* btConjugateGradient_h */

4
thirdparty/bullet/BulletSoftBody/btDefaultSoftBodySolver.cpp vendored
View File

@ -60,7 +60,7 @@ bool btDefaultSoftBodySolver::checkInitialized()
return true;
}
void btDefaultSoftBodySolver::solveConstraints(float solverdt)
void btDefaultSoftBodySolver::solveConstraints(btScalar solverdt)
{
// Solve constraints for non-solver softbodies
for (int i = 0; i < m_softBodySet.size(); ++i)
@ -132,7 +132,7 @@ void btDefaultSoftBodySolver::processCollision(btSoftBody *softBody, const btCol
softBody->defaultCollisionHandler(collisionObjectWrap);
} // btDefaultSoftBodySolver::processCollision
void btDefaultSoftBodySolver::predictMotion(float timeStep)
void btDefaultSoftBodySolver::predictMotion(btScalar timeStep)
{
for (int i = 0; i < m_softBodySet.size(); ++i)
{

4
thirdparty/bullet/BulletSoftBody/btDefaultSoftBodySolver.h vendored
View File

@ -46,9 +46,9 @@ public:
virtual void copyBackToSoftBodies(bool bMove = true);
virtual void solveConstraints(float solverdt);
virtual void solveConstraints(btScalar solverdt);
virtual void predictMotion(float solverdt);
virtual void predictMotion(btScalar solverdt);
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer);

197
thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.cpp vendored Normal file
View File

@ -0,0 +1,197 @@
/*
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.
*/
#include "btDeformableBackwardEulerObjective.h"
#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)
{
m_preconditioner = new MassPreconditioner(m_softBodies);
}
btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
{
delete m_preconditioner;
}
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);
}
void btDeformableBackwardEulerObjective::setDt(btScalar dt)
{
m_dt = dt;
}
void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
{
BT_PROFILE("multiply");
// add in the mass term
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
const btSoftBody::Node& node = psb->m_nodes[j];
b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
++counter;
}
}
for (int i = 0; i < m_lf.size(); ++i)
{
// add damping matrix
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
if (m_implicit)
{
m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
}
}
}
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];
}
}
}
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();
}
}
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);
}
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 btDeformableBackwardEulerObjective::totalEnergy(btScalar dt)
{
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);
}
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;
}
}
}
//set constraints as projections
void btDeformableBackwardEulerObjective::setConstraints()
{
m_projection.setConstraints();
}
void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
{
m_projection.applyDynamicFriction(r);
}

134
thirdparty/bullet/BulletSoftBody/btDeformableBackwardEulerObjective.h vendored Normal file
View File

@ -0,0 +1,134 @@
/*
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_BACKWARD_EULER_OBJECTIVE_H
#define BT_BACKWARD_EULER_OBJECTIVE_H
#include "btConjugateGradient.h"
#include "btDeformableLagrangianForce.h"
#include "btDeformableMassSpringForce.h"
#include "btDeformableGravityForce.h"
#include "btDeformableCorotatedForce.h"
#include "btDeformableLinearElasticityForce.h"
#include "btDeformableNeoHookeanForce.h"
#include "btDeformableContactProjection.h"
#include "btPreconditioner.h"
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "LinearMath/btQuickprof.h"
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;
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);
}
// 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);
};
#endif /* btBackwardEulerObjective_h */

485
thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.cpp vendored Normal file
View File

@ -0,0 +1,485 @@
/*
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.
*/
#include <stdio.h>
#include <limits>
#include "btDeformableBodySolver.h"
#include "btSoftBodyInternals.h"
#include "LinearMath/btQuickprof.h"
static const int kMaxConjugateGradientIterations = 50;
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_cg(kMaxConjugateGradientIterations)
, m_maxNewtonIterations(5)
, m_newtonTolerance(1e-4)
, m_lineSearch(false)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
}
btDeformableBodySolver::~btDeformableBodySolver()
{
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();
}
}
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;
}
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];
}
}
void btDeformableBodySolver::revertDv()
{
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();
}
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;
}
void btDeformableBodySolver::updateState()
{
updateVelocity();
updateTempPosition();
}
void btDeformableBodySolver::updateDv(btScalar scale)
{
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);
}
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);
}
void btDeformableBodySolver::setConstraints()
{
BT_PROFILE("setConstraint");
m_objective->setConstraints();
}
btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
{
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);
}
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;
}
}
}
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();
}
}
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;
}
}
}
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;
}
}
}
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++];
}
}
}
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;
}
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);
}
}
}
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;
}
/* 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);
}
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);
}
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();
}
}
}
void btDeformableBodySolver::setImplicit(bool implicit)
{
m_implicit = implicit;
m_objective->setImplicit(implicit);
}
void btDeformableBodySolver::setLineSearch(bool lineSearch)
{
m_lineSearch = lineSearch;
}

164
thirdparty/bullet/BulletSoftBody/btDeformableBodySolver.h vendored Normal file
View File

@ -0,0 +1,164 @@
/*
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_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"
struct btCollisionObjectWrapper;
class btDeformableBackwardEulerObjective;
class btDeformableMultiBodyDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver
{
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
public:
// handles data related to objective function
btDeformableBackwardEulerObjective* m_objective;
btDeformableBodySolver();
virtual ~btDeformableBodySolver();
virtual SolverTypes getSolverType() const
{
return DEFORMABLE_SOLVER;
}
// 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);
// 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 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 */

591
thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp vendored Normal file
View File

@ -0,0 +1,591 @@
/*
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.
*/
#include "btDeformableContactConstraint.h"
/* ================ Deformable Node Anchor =================== */
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
: m_anchor(&a)
, btDeformableContactConstraint(a.m_cti.m_normal)
{
}
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
: m_anchor(other.m_anchor)
, btDeformableContactConstraint(other)
{
}
btVector3 btDeformableNodeAnchorConstraint::getVa() const
{
const btSoftBody::sCti& cti = m_anchor->m_cti;
btVector3 va(0, 0, 0);
if (cti.m_colObj->hasContactResponse())
{
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_n[k];
}
va = cti.m_normal * vel;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t1[k];
}
va += m_anchor->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t2[k];
}
va += m_anchor->t2 * vel;
}
}
}
return va;
}
btScalar btDeformableNodeAnchorConstraint::solveConstraint()
{
const btSoftBody::sCti& cti = m_anchor->m_cti;
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = (vb - va);
// + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
const btScalar dn = btDot(vr, cti.m_normal);
// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
btScalar residualSquare = dn*dn;
btVector3 impulse = m_anchor->m_c0 * vr;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their velocities
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyImpulse(impulse, m_anchor->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
}
}
return residualSquare;
}
btVector3 btDeformableNodeAnchorConstraint::getVb() const
{
return m_anchor->m_node->m_v;
}
void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
{
btVector3 dv = impulse * m_anchor->m_c2;
m_anchor->m_node->m_v -= dv;
}
/* ================ Deformable vs. Rigid =================== */
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
: m_contact(&c)
, btDeformableContactConstraint(c.m_cti.m_normal)
{
m_total_normal_dv.setZero();
m_total_tangent_dv.setZero();
// penetration is non-positive. The magnitude of penetration is the depth of penetration.
m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
}
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
: m_contact(other.m_contact)
, btDeformableContactConstraint(other)
, m_penetration(other.m_penetration)
{
m_total_normal_dv = other.m_total_normal_dv;
m_total_tangent_dv = other.m_total_tangent_dv;
}
btVector3 btDeformableRigidContactConstraint::getVa() const
{
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 va(0, 0, 0);
if (cti.m_colObj->hasContactResponse())
{
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_n[k];
}
va = cti.m_normal * vel;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t1[k];
}
va += m_contact->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t2[k];
}
va += m_contact->t2 * vel;
}
}
}
return va;
}
btScalar btDeformableRigidContactConstraint::solveConstraint()
{
const btSoftBody::sCti& cti = m_contact->m_cti;
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
btScalar residualSquare = dn*dn;
btVector3 impulse = m_contact->m_c0 * vr;
const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
btVector3 old_total_tangent_dv = m_total_tangent_dv;
// m_c2 is the inverse mass of the deformable node/face
m_total_normal_dv -= impulse_normal * m_contact->m_c2;
m_total_tangent_dv -= impulse_tangent * m_contact->m_c2;
if (m_total_normal_dv.dot(cti.m_normal) < 0)
{
// separating in the normal direction
m_static = false;
m_total_tangent_dv = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
{
if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
{
// dynamic friction
// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
m_static = false;
if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
{
m_total_tangent_dv = btVector3(0,0,0);
}
else
{
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
}
impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
}
else
{
// static friction
m_static = true;
}
}
impulse = impulse_normal + impulse_tangent;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their velocities
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyImpulse(impulse, m_contact->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
if (impulse_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
}
}
}
return residualSquare;
}
btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
const btSoftBody::sCti& cti = m_contact->m_cti;
const btScalar dn = m_penetration;
if (dn != 0)
{
const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
// one iteration of the position impulse corrects all the position error at this timestep
m_penetration -= dn;
// apply impulse to deformable nodes involved and change their position
applySplitImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their position
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
// todo xuchenhan@
}
return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
}
return 0;
}
/* ================ Node vs. Rigid =================== */
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
: m_node(contact.m_node)
, btDeformableRigidContactConstraint(contact)
{
}
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
: m_node(other.m_node)
, btDeformableRigidContactConstraint(other)
{
}
btVector3 btDeformableNodeRigidContactConstraint::getVb() const
{
return m_node->m_v;
}
btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
return m_total_normal_dv + m_total_tangent_dv;
}
void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
contact->m_node->m_v -= dv;
}
void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
contact->m_node->m_vsplit -= dv;
};
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
: m_face(contact.m_face)
, btDeformableRigidContactConstraint(contact)
{
}
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
: m_face(other.m_face)
, btDeformableRigidContactConstraint(other)
{
}
btVector3 btDeformableFaceRigidContactConstraint::getVb() const
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
return vb;
}
btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
if (m_face->m_n[0] == node)
{
return face_dv * contact->m_weights[0];
}
if (m_face->m_n[1] == node)
{
return face_dv * contact->m_weights[1];
}
btAssert(node == m_face->m_n[2]);
return face_dv * contact->m_weights[2];
}
void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face;
btVector3& v0 = face->m_n[0]->m_v;
btVector3& v1 = face->m_n[1]->m_v;
btVector3& v2 = face->m_n[2]->m_v;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
v0 -= dv * contact->m_weights[0];
if (im1 > 0)
v1 -= dv * contact->m_weights[1];
if (im2 > 0)
v2 -= dv * contact->m_weights[2];
// apply strain limiting to prevent undamped modes
btScalar m01 = (btScalar(1)/(im0 + im1));
btScalar m02 = (btScalar(1)/(im0 + im2));
btScalar m12 = (btScalar(1)/(im1 + im2));
btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
v0 += dv0;
v1 += dv1;
v2 += dv2;
}
void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face;
btVector3& v0 = face->m_n[0]->m_vsplit;
btVector3& v1 = face->m_n[1]->m_vsplit;
btVector3& v2 = face->m_n[2]->m_vsplit;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
v0 -= dv * contact->m_weights[0];
if (im1 > 0)
v1 -= dv * contact->m_weights[1];
if (im2 > 0)
v2 -= dv * contact->m_weights[2];
}
/* ================ Face vs. Node =================== */
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
: m_node(contact.m_node)
, m_face(contact.m_face)
, m_contact(&contact)
, btDeformableContactConstraint(contact.m_normal)
{
m_total_normal_dv.setZero();
m_total_tangent_dv.setZero();
}
btVector3 btDeformableFaceNodeContactConstraint::getVa() const
{
return m_node->m_v;
}
btVector3 btDeformableFaceNodeContactConstraint::getVb() const
{
const btSoftBody::DeformableFaceNodeContact* contact = getContact();
btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
return vb;
}
btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const
{
btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
if (n == m_node)
return dv;
const btSoftBody::DeformableFaceNodeContact* contact = getContact();
if (m_face->m_n[0] == n)
{
return dv * contact->m_weights[0];
}
if (m_face->m_n[1] == n)
{
return dv * contact->m_weights[1];
}
btAssert(n == m_face->m_n[2]);
return dv * contact->m_weights[2];
}
btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
{
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = vb - va;
const btScalar dn = btDot(vr, m_contact->m_normal);
// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
btScalar residualSquare = dn*dn;
btVector3 impulse = m_contact->m_c0 * vr;
const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
btVector3 old_total_tangent_dv = m_total_tangent_dv;
// m_c2 is the inverse mass of the deformable node/face
if (m_node->m_im > 0)
{
m_total_normal_dv -= impulse_normal * m_node->m_im;
m_total_tangent_dv -= impulse_tangent * m_node->m_im;
}
else
{
m_total_normal_dv -= impulse_normal * m_contact->m_imf;
m_total_tangent_dv -= impulse_tangent * m_contact->m_imf;
}
if (m_total_normal_dv.dot(m_contact->m_normal) > 0)
{
// separating in the normal direction
m_static = false;
m_total_tangent_dv = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
{
if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
{
// dynamic friction
// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
m_static = false;
if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
{
m_total_tangent_dv = btVector3(0,0,0);
}
else
{
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
}
impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
}
else
{
// static friction
m_static = true;
}
}
impulse = impulse_normal + impulse_tangent;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
return residualSquare;
}
void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceNodeContact* contact = getContact();
btVector3 dva = impulse * contact->m_node->m_im;
btVector3 dvb = impulse * contact->m_imf;
if (contact->m_node->m_im > 0)
{
contact->m_node->m_v += dva;
}
btSoftBody::Face* face = contact->m_face;
btVector3& v0 = face->m_n[0]->m_v;
btVector3& v1 = face->m_n[1]->m_v;
btVector3& v2 = face->m_n[2]->m_v;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
{
v0 -= dvb * contact->m_weights[0];
}
if (im1 > 0)
{
v1 -= dvb * contact->m_weights[1];
}
if (im2 > 0)
{
v2 -= dvb * contact->m_weights[2];
}
// todo: Face node constraints needs more work
// btScalar m01 = (btScalar(1)/(im0 + im1));
// btScalar m02 = (btScalar(1)/(im0 + im2));
// btScalar m12 = (btScalar(1)/(im1 + im2));
//
// btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
// btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
// btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
// v0 += dv0;
// v1 += dv1;
// v2 += dv2;
}

302
thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.h vendored Normal file
View File

@ -0,0 +1,302 @@
/*
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_DEFORMABLE_CONTACT_CONSTRAINT_H
#define BT_DEFORMABLE_CONTACT_CONSTRAINT_H
#include "btSoftBody.h"
// btDeformableContactConstraint is an abstract class specifying the method that each type of contact constraint needs to implement
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;
};
//
// Constraint that a certain node in the deformable objects cannot move
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;
}
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){}
};
//
// Anchor Constraint between rigid and deformable node
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){}
};
//
// 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;
}
};
//
// Constraint between rigid/multi body and deformable objects nodes
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);
};
//
// Constraint between rigid/multi body and deformable objects faces
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);
};
//
// Constraint between deformable objects faces and deformable objects nodes
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){}
};
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

509
thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp vendored Normal file
View File

@ -0,0 +1,509 @@
/*
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.
*/
#include "btDeformableContactProjection.h"
#include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm>
#include <cmath>
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
{
btScalar residualSquare = 0;
for (int i = 0; i < numDeformableBodies; ++i)
{
for (int j = 0; j < m_softBodies.size(); ++j)
{
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.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
{
btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
{
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
{
btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
}
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 residualSquare = 0;
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];
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()
{
BT_PROFILE("setConstraints");
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
// set Dirichlet constraint
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im == 0)
{
btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
m_staticConstraints[i].push_back(static_constraint);
}
}
// set up deformable anchors
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{
btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
// skip fixed points
if (anchor.m_node->m_im == 0)
{
continue;
}
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
btDeformableNodeAnchorConstraint constraint(anchor);
m_nodeAnchorConstraints[i].push_back(constraint);
}
// set Deformable Node vs. Rigid constraint
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
{
const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
// skip fixed points
if (contact.m_node->m_im == 0)
{
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);
}
}
// set Deformable Face vs. Rigid constraint
for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
{
const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
// skip fixed faces
if (contact.m_c2 == 0)
{
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);
}
}
}
}
void btDeformableContactProjection::project(TVStack& x)
{
const int dim = 3;
for (int index = 0; index < m_projectionsDict.size(); ++index)
{
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
if (projectionDirs.size() >= dim)
{
// static node
x[i].setZero();
continue;
}
else if (projectionDirs.size() == 2)
{
btVector3 dir0 = projectionDirs[0];
btVector3 dir1 = projectionDirs[1];
btVector3 free_dir = btCross(dir0, dir1);
if (free_dir.safeNorm() < SIMD_EPSILON)
{
x[i] -= x[i].dot(dir0) * dir0;
x[i] -= x[i].dot(dir1) * dir1;
}
else
{
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
}
}
else
{
btAssert(projectionDirs.size() == 1);
btVector3 dir0 = projectionDirs[0];
x[i] -= x[i].dot(dir0) * dir0;
}
}
}
void btDeformableContactProjection::setProjection()
{
btAlignedObjectArray<btVector3> units;
units.push_back(btVector3(1,0,0));
units.push_back(btVector3(0,1,0));
units.push_back(btVector3(0,0,1));
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{
int index = m_staticConstraints[i][j].m_node->index;
if (m_projectionsDict.find(index) == NULL)
{
m_projectionsDict.insert(index, units);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
if (m_projectionsDict.find(index) == NULL)
{
m_projectionsDict.insert(index, units);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
int index = m_nodeRigidConstraints[i][j].m_node->index;
if (m_nodeRigidConstraints[i][j].m_static)
{
if (m_projectionsDict.find(index) == NULL)
{
m_projectionsDict.insert(index, units);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
else
{
if (m_projectionsDict.find(index) == NULL)
{
btAlignedObjectArray<btVector3> projections;
projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
}
}
}
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
for (int k = 0; k < 3; ++k)
{
const btSoftBody::Node* node = face->m_n[k];
int index = node->index;
if (m_faceRigidConstraints[i][j].m_static)
{
if (m_projectionsDict.find(index) == NULL)
{
m_projectionsDict.insert(index, units);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
else
{
if (m_projectionsDict.find(index) == NULL)
{
btAlignedObjectArray<btVector3> projections;
projections.push_back(m_faceRigidConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_faceRigidConstraints[i][j].m_normal);
}
}
}
}
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);
}
}
}
}
}
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{
const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
const btSoftBody::Node* node = constraint.m_node;
if (node->m_im != 0)
{
int index = node->index;
f[index] += constraint.getDv(node)* (1./node->m_im);
}
}
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
const btSoftBody::Face* face = constraint.getContact()->m_face;
for (int k = 0; k < 3; ++k)
{
const btSoftBody::Node* node = face->m_n[k];
if (node->m_im != 0)
{
int index = node->index;
f[index] += constraint.getDv(node)* (1./node->m_im);
}
}
}
for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
{
const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j];
const btSoftBody::Face* face = constraint.getContact()->m_face;
const btSoftBody::Node* node = constraint.getContact()->m_node;
if (node->m_im != 0)
{
int index = node->index;
f[index] += constraint.getDv(node)* (1./node->m_im);
}
for (int k = 0; k < 3; ++k)
{
const btSoftBody::Node* node = face->m_n[k];
if (node->m_im != 0)
{
int index = node->index;
f[index] += constraint.getDv(node)* (1./node->m_im);
}
}
}
}
}
void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
int N = m_softBodies.size();
if (nodeUpdated)
{
m_staticConstraints.resize(N);
m_nodeAnchorConstraints.resize(N);
m_nodeRigidConstraints.resize(N);
m_faceRigidConstraints.resize(N);
m_deformableConstraints.resize(N);
}
for (int i = 0 ; i < N; ++i)
{
m_staticConstraints[i].clear();
m_nodeAnchorConstraints[i].clear();
m_nodeRigidConstraints[i].clear();
m_faceRigidConstraints[i].clear();
m_deformableConstraints[i].clear();
}
m_projectionsDict.clear();
}

90
thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.h vendored Normal file
View File

@ -0,0 +1,90 @@
/*
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_CONTACT_PROJECTION_H
#define BT_CONTACT_PROJECTION_H
#include "btCGProjection.h"
#include "btSoftBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "btDeformableContactConstraint.h"
#include "LinearMath/btHashMap.h"
#include <vector>
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;
// map from node index to static constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
// map from node index to node rigid constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// map from node index to face rigid constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
// map from node index to deformable constraint
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);
};
#endif /* btDeformableContactProjection_h */

125
thirdparty/bullet/BulletSoftBody/btDeformableCorotatedForce.h vendored Normal file
View File

@ -0,0 +1,125 @@
/*
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_COROTATED_H
#define BT_COROTATED_H
#include "btDeformableLagrangianForce.h"
#include "LinearMath/btPolarDecomposition.h"
static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& 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;
}
};
#endif /* btCorotated_h */

105
thirdparty/bullet/BulletSoftBody/btDeformableGravityForce.h vendored Normal file
View File

@ -0,0 +1,105 @@
/*
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_DEFORMABLE_GRAVITY_FORCE_H
#define BT_DEFORMABLE_GRAVITY_FORCE_H
#include "btDeformableLagrangianForce.h"
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;
}
// 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 */

364
thirdparty/bullet/BulletSoftBody/btDeformableLagrangianForce.h vendored Normal file
View File

@ -0,0 +1,364 @@
/*
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_DEFORMABLE_LAGRANGIAN_FORCE_H
#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
#include "btSoftBody.h"
#include <LinearMath/btHashMap.h>
#include <iostream>
enum btDeformableLagrangianForceType
{
BT_GRAVITY_FORCE = 1,
BT_MASSSPRING_FORCE = 2,
BT_COROTATED_FORCE = 3,
BT_NEOHOOKEAN_FORCE = 4,
BT_LINEAR_ELASTICITY_FORCE = 5
};
static inline double randomDouble(double low, double high)
{
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]);
}
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 */

340
thirdparty/bullet/BulletSoftBody/btDeformableLinearElasticityForce.h vendored Normal file
View File

@ -0,0 +1,340 @@
/*
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_LINEAR_ELASTICITY_H
#define BT_LINEAR_ELASTICITY_H
#include "btDeformableLagrangianForce.h"
#include "LinearMath/btQuickprof.h"
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);
#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();
}
}
#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;
}
};
#endif /* BT_LINEAR_ELASTICITY_H */

255
thirdparty/bullet/BulletSoftBody/btDeformableMassSpringForce.h vendored Normal file
View File

@ -0,0 +1,255 @@
/*
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_MASS_SPRING_H
#define BT_MASS_SPRING_H
#include "btDeformableLagrangianForce.h"
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;
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;
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;
// 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 */

143
thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp vendored Normal file
View File

@ -0,0 +1,143 @@
/*
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.
*/
#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)
{
{
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, 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)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
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;
}
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;
}
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);
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);
}
}
}
void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, 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))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
#endif
break;
}
}
}
}
}

61
thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h vendored Normal file
View File

@ -0,0 +1,61 @@
/*
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_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
#include "btDeformableBodySolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
class btDeformableBodySolver;
// btDeformableMultiBodyConstraintSolver extendsn btMultiBodyConstraintSolver to solve for the contact among rigid/multibody and deformable bodies. Notice that the following constraints
// 1. rigid/multibody against rigid/multibody
// 2. rigid/multibody against deforamble
// 3. deformable against deformable
// 4. deformable self collision
// 5. joint constraints
// are all coupled in this solve.
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);
// 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);
};
#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

653
thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp vendored Normal file
View File

@ -0,0 +1,653 @@
/*
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.
*/
/* ====== Overview of the Deformable Algorithm ====== */
/*
A single step of the deformable body simulation contains the following main components:
Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz).
1. Deformable maintaintenance of rest lengths and volume preservation. Forces only depend on position: Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces.
2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable.
TODO: add option for positional drift correction (using vel_target += erp * pos_error/dt
3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in
M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}.
Make sure contact constraints are not violated in step b by performing velocity projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact.
4. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
*/
#include <stdio.h>
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "DeformableBodyInplaceSolverIslandCallback.h"
#include "btDeformableBodySolver.h"
#include "LinearMath/btQuickprof.h"
#include "btSoftBodyInternals.h"
btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
{
m_drawFlags = fDrawFlags::Std;
m_drawNodeTree = true;
m_drawFaceTree = false;
m_drawClusterTree = false;
m_sbi.m_broadphase = pairCache;
m_sbi.m_dispatcher = dispatcher;
m_sbi.m_sparsesdf.Initialize();
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
m_sbi.m_sparsesdf.Reset();
m_sbi.air_density = (btScalar)1.2;
m_sbi.water_density = 0;
m_sbi.water_offset = 0;
m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0);
m_internalTime = 0.0;
m_implicit = false;
m_lineSearch = false;
m_selfCollision = true;
m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
}
void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
if (0 != m_internalPreTickCallback)
{
(*m_internalPreTickCallback)(this, timeStep);
}
reinitialize(timeStep);
// add gravity to velocity of rigid and multi bodys
applyRigidBodyGravity(timeStep);
///apply gravity and explicit force to velocity, predict motion
predictUnconstraintMotion(timeStep);
///perform collision detection
btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
if (m_selfCollision)
{
softBodySelfCollision();
}
btMultiBodyDynamicsWorld::calculateSimulationIslands();
beforeSolverCallbacks(timeStep);
///solve contact constraints and then deformable bodies momemtum equation
solveConstraints(timeStep);
afterSolverCallbacks(timeStep);
integrateTransforms(timeStep);
///update vehicle simulation
btMultiBodyDynamicsWorld::updateActions(timeStep);
updateActivationState(timeStep);
// End solver-wise simulation step
// ///////////////////////////////
}
void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
{
for (int i = 0; i < m_softBodies.size(); i++)
{
btSoftBody* psb = m_softBodies[i];
psb->updateDeactivation(timeStep);
if (psb->wantsSleeping())
{
if (psb->getActivationState() == ACTIVE_TAG)
psb->setActivationState(WANTS_DEACTIVATION);
if (psb->getActivationState() == ISLAND_SLEEPING)
{
psb->setZeroVelocity();
}
}
else
{
if (psb->getActivationState() != DISABLE_DEACTIVATION)
psb->setActivationState(ACTIVE_TAG);
}
}
btMultiBodyDynamicsWorld::updateActivationState(timeStep);
}
void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
{
m_deformableBodySolver->updateSoftBodies();
for (int i = 0; i < m_softBodies.size(); i++)
{
btSoftBody* psb = m_softBodies[i];
if (psb->isActive())
{
psb->defaultCollisionHandler(psb);
}
}
}
void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
{
// correct the position of rigid bodies with temporary velocity generated from split impulse
btContactSolverInfo infoGlobal;
btVector3 zero(0,0,0);
for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
{
btRigidBody* rb = m_nonStaticRigidBodies[i];
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btVector3 pushVelocity = rb->getPushVelocity();
btVector3 turnVelocity = rb->getTurnVelocity();
if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
{
btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
rb->setWorldTransform(newTransform);
rb->setPushVelocity(zero);
rb->setTurnVelocity(zero);
}
}
}
void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
positionCorrection(timeStep);
btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btSoftBody::Node& node = psb->m_nodes[j];
btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
btScalar clampDeltaV = maxDisplacement / timeStep;
for (int c = 0; c < 3; c++)
{
if (node.m_v[c] > clampDeltaV)
{
node.m_v[c] = clampDeltaV;
}
if (node.m_v[c] < -clampDeltaV)
{
node.m_v[c] = -clampDeltaV;
}
}
node.m_x = node.m_x + timeStep * node.m_v;
node.m_v -= node.m_vsplit;
node.m_vsplit.setZero();
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
// enforce anchor constraints
for (int j = 0; j < psb->m_deformableAnchors.size();++j)
{
btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
btSoftBody::Node* n = a.m_node;
n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
// update multibody anchor info
if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 nrm;
const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
const btTransform& wtr = multibodyLinkCol->getWorldTransform();
psb->m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(n->m_x),
shp,
nrm,
0);
a.m_cti.m_normal = wtr.getBasis() * nrm;
btVector3 normal = a.m_cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
a.jacobianData_normal = jacobianData_normal;
a.jacobianData_t1 = jacobianData_t1;
a.jacobianData_t2 = jacobianData_t2;
a.t1 = t1;
a.t2 = t2;
}
}
}
psb->interpolateRenderMesh();
}
}
void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
{
// save v_{n+1}^* velocity after explicit forces
m_deformableBodySolver->backupVelocity();
// set up constraints among multibodies and between multibodies and deformable bodies
setupConstraints();
// solve contact constraints
solveContactConstraints();
// set up the directions in which the velocity does not change in the momentum solve
m_deformableBodySolver->m_objective->m_projection.setProjection();
// for explicit scheme, m_backupVelocity = v_{n+1}^*
// for implicit scheme, m_backupVelocity = v_n
// Here, set dv = v_{n+1} - v_n for nodes in contact
m_deformableBodySolver->setupDeformableSolve(m_implicit);
// At this point, dv should be golden for nodes in contact
// proceed to solve deformable momentum equation
m_deformableBodySolver->solveDeformableConstraints(timeStep);
}
void btDeformableMultiBodyDynamicsWorld::setupConstraints()
{
// set up constraints between multibody and deformable bodies
m_deformableBodySolver->setConstraints();
// set up constraints among multibodies
{
sortConstraints();
// setup the solver callback
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
// build islands
m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
}
}
void btDeformableMultiBodyDynamicsWorld::sortConstraints()
{
m_sortedConstraints.resize(m_constraints.size());
int i;
for (i = 0; i < getNumConstraints(); i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i = 0; i < m_multiBodyConstraints.size(); i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
}
void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
{
// process constraints on each island
m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
// process deferred
m_solverDeformableBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
// write joint feedback
{
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b = 0; b < bod->getNumLinks(); b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks() + 1);
m_scratch_m.resize(bod->getNumLinks() + 1);
if (bod->internalNeedsJointFeedback())
{
if (!bod->isUsingRK4Integration())
{
if (bod->internalNeedsJointFeedback())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
}
}
}
}
}
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
{
m_softBodies.push_back(body);
// Set the soft body solver that will deal with this body
// to be the world's solver
body->setSoftBodySolver(m_deformableBodySolver);
btCollisionWorld::addCollisionObject(body,
collisionFilterGroup,
collisionFilterMask);
}
void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
BT_PROFILE("predictUnconstraintMotion");
btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
m_deformableBodySolver->predictMotion(timeStep);
}
void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
{
m_internalTime += timeStep;
m_deformableBodySolver->setImplicit(m_implicit);
m_deformableBodySolver->setLineSearch(m_lineSearch);
m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
}
void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
{
btMultiBodyDynamicsWorld::debugDrawWorld();
for (int i = 0; i < getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(psb, getDebugDrawer());
btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
}
}
}
void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
{
// Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
// so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
// when there are multiple substeps
btMultiBodyDynamicsWorld::applyGravity();
// integrate rigid body gravity
for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
{
btRigidBody* rb = m_nonStaticRigidBodies[i];
rb->integrateVelocities(timeStep);
}
// integrate multibody gravity
{
forwardKinematics();
clearMultiBodyConstraintForces();
{
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b = 0; b < bod->getNumLinks(); b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
m_scratch_r.resize(bod->getNumLinks() + 1);
m_scratch_v.resize(bod->getNumLinks() + 1);
m_scratch_m.resize(bod->getNumLinks() + 1);
bool isConstraintPass = false;
{
if (!bod->isUsingRK4Integration())
{
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
else
{
btAssert(" RK4Integration is not supported" );
}
}
}
}
}
}
clearGravity();
}
void btDeformableMultiBodyDynamicsWorld::clearGravity()
{
BT_PROFILE("btMultiBody clearGravity");
// clear rigid body gravity
for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
if (body->isActive())
{
body->clearGravity();
}
}
// clear multibody gravity
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b = 0; b < bod->getNumLinks(); b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
bod->addBaseForce(-m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j));
}
}
}
}
void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
{
if (0 != m_internalTickCallback)
{
(*m_internalTickCallback)(this, timeStep);
}
if (0 != m_solverCallback)
{
(*m_solverCallback)(m_internalTime, this);
}
}
void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
{
if (0 != m_solverCallback)
{
(*m_solverCallback)(m_internalTime, this);
}
}
void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
{
btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
bool added = false;
for (int i = 0; i < forces.size(); ++i)
{
if (forces[i]->getForceType() == force->getForceType())
{
forces[i]->addSoftBody(psb);
added = true;
break;
}
}
if (!added)
{
force->addSoftBody(psb);
force->setIndices(m_deformableBodySolver->m_objective->getIndices());
forces.push_back(force);
}
}
void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
{
m_softBodies.remove(body);
btCollisionWorld::removeCollisionObject(body);
// force a reinitialize so that node indices get updated.
m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
}
void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
{
btSoftBody* body = btSoftBody::upcast(collisionObject);
if (body)
removeSoftBody(body);
else
btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
}
int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
{
startProfiling(timeStep);
int numSimulationSubSteps = 0;
if (maxSubSteps)
{
//fixed timestep with interpolation
m_fixedTimeStep = fixedTimeStep;
m_localTime += timeStep;
if (m_localTime >= fixedTimeStep)
{
numSimulationSubSteps = int(m_localTime / fixedTimeStep);
m_localTime -= numSimulationSubSteps * fixedTimeStep;
}
}
else
{
//variable timestep
fixedTimeStep = timeStep;
m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
m_fixedTimeStep = 0;
if (btFuzzyZero(timeStep))
{
numSimulationSubSteps = 0;
maxSubSteps = 0;
}
else
{
numSimulationSubSteps = 1;
maxSubSteps = 1;
}
}
//process some debugging flags
if (getDebugDrawer())
{
btIDebugDraw* debugDrawer = getDebugDrawer();
gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
}
if (numSimulationSubSteps)
{
//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
saveKinematicState(fixedTimeStep * clampedSimulationSteps);
for (int i = 0; i < clampedSimulationSteps; i++)
{
internalSingleStepSimulation(fixedTimeStep);
synchronizeMotionStates();
}
}
else
{
synchronizeMotionStates();
}
clearForces();
#ifndef BT_NO_PROFILE
CProfileManager::Increment_Frame_Counter();
#endif //BT_NO_PROFILE
return numSimulationSubSteps;
}

165
thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h vendored Normal file
View File

@ -0,0 +1,165 @@
/*
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_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
#define BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
#include "btSoftMultiBodyDynamicsWorld.h"
#include "btDeformableLagrangianForce.h"
#include "btDeformableMassSpringForce.h"
#include "btDeformableBodySolver.h"
#include "btDeformableMultiBodyConstraintSolver.h"
#include "btSoftBodyHelpers.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include <functional>
typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableBodySolver;
class btDeformableLagrangianForce;
struct MultiBodyInplaceSolverIslandCallback;
struct DeformableBodyInplaceSolverIslandCallback;
class btDeformableMultiBodyConstraintSolver;
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;
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();
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 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;
}
};
#endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H

375
thirdparty/bullet/BulletSoftBody/btDeformableNeoHookeanForce.h vendored Normal file
View File

@ -0,0 +1,375 @@
/*
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_NEOHOOKEAN_H
#define BT_NEOHOOKEAN_H
#include "btDeformableLagrangianForce.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btImplicitQRSVD.h"
// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
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();
// 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();
}
}
#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)
{
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 */

79
thirdparty/bullet/BulletSoftBody/btPreconditioner.h vendored Normal file
View File

@ -0,0 +1,79 @@
/*
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_PRECONDITIONER_H
#define BT_PRECONDITIONER_H
class Preconditioner
{
public:
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(){}
};
class MassPreconditioner : public Preconditioner
{
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];
}
}
};
#endif /* BT_PRECONDITIONER_H */

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

@ -18,9 +18,12 @@ subject to the following restrictions:
#include "BulletSoftBody/btSoftBodySolvers.h"
#include "btSoftBodyData.h"
#include "LinearMath/btSerializer.h"
#include "LinearMath/btAlignedAllocator.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include <iostream>
//
btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: m_softBodySolver(0), m_worldInfo(worldInfo)
@ -86,6 +89,8 @@ void btSoftBody::initDefaults()
m_cfg.piterations = 1;
m_cfg.diterations = 0;
m_cfg.citerations = 4;
m_cfg.drag = 0;
m_cfg.m_maxStress = 0;
m_cfg.collisions = fCollision::Default;
m_pose.m_bvolume = false;
m_pose.m_bframe = false;
@ -110,6 +115,11 @@ void btSoftBody::initDefaults()
m_windVelocity = btVector3(0, 0, 0);
m_restLengthScale = btScalar(1.0);
m_dampingCoefficient = 1;
m_sleepingThreshold = 0.1;
m_useFaceContact = true;
m_useSelfCollision = false;
m_collisionFlags = 0;
}
//
@ -401,6 +411,98 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca
m_anchors.push_back(a);
}
//
void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
{
DeformableNodeRigidAnchor c;
btSoftBody::Node& n = m_nodes[node];
const btScalar ima = n.m_im;
const btScalar imb = body->getInvMass();
btVector3 nrm;
const btCollisionShape* shp = body->getCollisionShape();
const btTransform& wtr = body->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(m_nodes[node].m_x),
shp,
nrm,
0);
c.m_cti.m_colObj = body;
c.m_cti.m_normal = wtr.getBasis() * nrm;
c.m_cti.m_offset = dst;
c.m_node = &m_nodes[node];
const btScalar fc = m_cfg.kDF * body->getFriction();
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
c.m_node->m_battach = 1;
m_deformableAnchors.push_back(c);
}
//
void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
{
DeformableNodeRigidAnchor c;
btSoftBody::Node& n = m_nodes[node];
const btScalar ima = n.m_im;
btVector3 nrm;
const btCollisionShape* shp = link->getCollisionShape();
const btTransform& wtr = link->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(m_nodes[node].m_x),
shp,
nrm,
0);
c.m_cti.m_colObj = link;
c.m_cti.m_normal = wtr.getBasis() * nrm;
c.m_cti.m_offset = dst;
c.m_node = &m_nodes[node];
const btScalar fc = m_cfg.kDF * link->getFriction();
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
btVector3 normal = c.m_cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
findJacobian(link, jacobianData_t2, c.m_node->m_x, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = link->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
c.jacobianData_t2 = jacobianData_t2;
c.t1 = t1;
c.t2 = t2;
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c1 = ra;
c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
c.m_node->m_battach = 1;
m_deformableAnchors.push_back(c);
}
//
void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
{
@ -518,7 +620,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex
fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
// Check angle of attack
// cos(10°) = 0.98480
// cos(10º) = 0.98480
if (0 < n_dot_v && n_dot_v < 0.98480f)
fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
@ -604,7 +706,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex
fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
// Check angle of attack
// cos(10°) = 0.98480
// cos(10º) = 0.98480
if (0 < n_dot_v && n_dot_v < 0.98480f)
fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm));
@ -853,6 +955,7 @@ void btSoftBody::scale(const btVector3& scl)
updateNormals();
updateBounds();
updateConstants();
initializeDmInverse();
}
//
@ -1757,7 +1860,6 @@ void btSoftBody::setSolver(eSolverPresets::_ preset)
}
}
//
void btSoftBody::predictMotion(btScalar dt)
{
int i, ni;
@ -1866,6 +1968,7 @@ void btSoftBody::predictMotion(btScalar dt)
m_cdbvt.optimizeIncremental(1);
}
//
void btSoftBody::solveConstraints()
{
@ -2261,7 +2364,6 @@ btVector3 btSoftBody::evaluateCom() const
return (com);
}
//
bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
const btVector3& x,
btScalar margin,
@ -2290,6 +2392,166 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
return (false);
}
//
bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
const btVector3& x,
btScalar margin,
btSoftBody::sCti& cti, bool predict) const
{
btVector3 nrm;
const btCollisionShape* shp = colObjWrap->getCollisionShape();
const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
// but resolve contact at x_n
// btTransform wtr = (predict) ?
// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
// : colObjWrap->getWorldTransform();
const btTransform& wtr = colObjWrap->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(x),
shp,
nrm,
margin);
if (!predict)
{
cti.m_colObj = colObjWrap->getCollisionObject();
cti.m_normal = wtr.getBasis() * nrm;
cti.m_offset = dst;
}
if (dst < 0)
return true;
return (false);
}
//
// Compute barycentric coordinates (u, v, w) for
// point p with respect to triangle (a, b, c)
static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary)
{
btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
btScalar d00 = v0.dot(v0);
btScalar d01 = v0.dot(v1);
btScalar d11 = v1.dot(v1);
btScalar d20 = v2.dot(v0);
btScalar d21 = v2.dot(v1);
btScalar denom = d00 * d11 - d01 * d01;
bary.setY((d11 * d20 - d01 * d21) / denom);
bary.setZ((d00 * d21 - d01 * d20) / denom);
bary.setX(btScalar(1) - bary.getY() - bary.getZ());
}
//
bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
Face& f,
btVector3& contact_point,
btVector3& bary,
btScalar margin,
btSoftBody::sCti& cti, bool predict) const
{
btVector3 nrm;
const btCollisionShape* shp = colObjWrap->getCollisionShape();
const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
// but resolve contact at x_n
btTransform wtr = (predict) ?
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
: colObjWrap->getWorldTransform();
// const btTransform& wtr = colObjWrap->getWorldTransform();
btScalar dst;
//#define USE_QUADRATURE 1
//#define CACHE_PREV_COLLISION
// use the contact position of the previous collision
#ifdef CACHE_PREV_COLLISION
if (f.m_pcontact[3] != 0)
{
for (int i = 0; i < 3; ++i)
bary[i] = f.m_pcontact[i];
contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
dst = m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(contact_point),
shp,
nrm,
margin);
nrm = wtr.getBasis() * nrm;
// use cached contact point
}
else
{
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
triangle_transform.setIdentity();
triangle_transform.setOrigin(f.m_n[0]->m_x);
btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
dst = results.distance - margin;
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
nrm = results.normal;
for (int i = 0; i < 3; ++i)
f.m_pcontact[i] = bary[i];
}
#endif
// use collision quadrature point
#ifdef USE_QUADRATURE
{
dst = SIMD_INFINITY;
btVector3 local_nrm;
for (int q = 0; q < m_quads.size(); ++q)
{
btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(p),
shp,
local_nrm,
margin);
if (local_dst < dst)
{
dst = local_dst;
contact_point = p;
bary = m_quads[q];
nrm = wtr.getBasis() * local_nrm;
}
}
}
#endif
// regular face contact
{
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
triangle_transform.setIdentity();
triangle_transform.setOrigin(f.m_n[0]->m_x);
btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
dst = results.distance - margin;
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
nrm = results.normal;
for (int i = 0; i < 3; ++i)
f.m_pcontact[i] = bary[i];
}
if (!predict)
{
cti.m_colObj = colObjWrap->getCollisionObject();
cti.m_normal = nrm;
cti.m_offset = dst;
}
if (dst < 0)
return true;
return (false);
}
//
void btSoftBody::updateNormals()
{
@ -2305,7 +2567,8 @@ void btSoftBody::updateNormals()
btSoftBody::Face& f = m_faces[i];
const 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);
f.m_normal = n.normalized();
f.m_normal = n;
f.m_normal.safeNormalize();
f.m_n[0]->m_n += n;
f.m_n[1]->m_n += n;
f.m_n[2]->m_n += n;
@ -2333,15 +2596,48 @@ void btSoftBody::updateBounds()
m_bounds[1] = btVector3(1000, 1000, 1000);
} else {*/
if (m_ndbvt.m_root)
// if (m_ndbvt.m_root)
// {
// const btVector3& mins = m_ndbvt.m_root->volume.Mins();
// const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
// const btScalar csm = getCollisionShape()->getMargin();
// const btVector3 mrg = btVector3(csm,
// csm,
// csm) *
// 1; // ??? to investigate...
// m_bounds[0] = mins - mrg;
// m_bounds[1] = maxs + mrg;
// if (0 != getBroadphaseHandle())
// {
// m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
// m_bounds[0],
// m_bounds[1],
// m_worldInfo->m_dispatcher);
// }
// }
// else
// {
// m_bounds[0] =
// m_bounds[1] = btVector3(0, 0, 0);
// }
if (m_nodes.size())
{
const btVector3& mins = m_ndbvt.m_root->volume.Mins();
const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
btVector3 mins = m_nodes[0].m_x;
btVector3 maxs = m_nodes[0].m_x;
for (int i = 1; i < m_nodes.size(); ++i)
{
for (int d = 0; d < 3; ++d)
{
if (m_nodes[i].m_x[d] > maxs[d])
maxs[d] = m_nodes[i].m_x[d];
if (m_nodes[i].m_x[d] < mins[d])
mins[d] = m_nodes[i].m_x[d];
}
}
const btScalar csm = getCollisionShape()->getMargin();
const btVector3 mrg = btVector3(csm,
csm,
csm) *
1; // ??? to investigate...
csm);
m_bounds[0] = mins - mrg;
m_bounds[1] = maxs + mrg;
if (0 != getBroadphaseHandle())
@ -2357,7 +2653,6 @@ void btSoftBody::updateBounds()
m_bounds[0] =
m_bounds[1] = btVector3(0, 0, 0);
}
//}
}
//
@ -2774,6 +3069,62 @@ void btSoftBody::dampClusters()
}
}
void btSoftBody::setSpringStiffness(btScalar k)
{
for (int i = 0; i < m_links.size(); ++i)
{
m_links[i].Feature::m_material->m_kLST = k;
}
}
void btSoftBody::initializeDmInverse()
{
btScalar unit_simplex_measure = 1./6.;
for (int i = 0; i < m_tetras.size(); ++i)
{
Tetra &t = m_tetras[i];
btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
c1.getY(), c2.getY(), c3.getY(),
c1.getZ(), c2.getZ(), c3.getZ());
t.m_element_measure = Dm.determinant() * unit_simplex_measure;
t.m_Dm_inverse = Dm.inverse();
}
}
void btSoftBody::updateDeformation()
{
for (int i = 0; i < m_tetras.size(); ++i)
{
btSoftBody::Tetra& t = m_tetras[i];
btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
c1.getY(), c2.getY(), c3.getY(),
c1.getZ(), c2.getZ(), c3.getZ());
t.m_F = Ds * t.m_Dm_inverse;
btSoftBody::TetraScratch& s = m_tetraScratches[i];
s.m_F = t.m_F;
s.m_J = t.m_F.determinant();
btMatrix3x3 C = t.m_F.transpose()*t.m_F;
s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
s.m_cofF = t.m_F.adjoint().transpose();
}
}
void btSoftBody::advanceDeformation()
{
updateDeformation();
for (int i = 0; i < m_tetras.size(); ++i)
{
m_tetraScratchesTn[i] = m_tetraScratches[i];
}
}
//
void btSoftBody::Joint::Prepare(btScalar dt, int)
{
@ -3012,6 +3363,40 @@ void btSoftBody::applyForces()
}
}
//
void btSoftBody::setMaxStress(btScalar maxStress)
{
m_cfg.m_maxStress = maxStress;
}
//
void btSoftBody::interpolateRenderMesh()
{
for (int i = 0; i < m_renderNodes.size(); ++i)
{
Node& n = m_renderNodes[i];
n.m_x.setZero();
for (int j = 0; j < 4; ++j)
{
if (m_renderNodesParents[i].size())
{
n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
}
}
}
}
void btSoftBody::setCollisionQuadrature(int N)
{
for (int i = 0; i <= N; ++i)
{
for (int j = 0; i+j <= N; ++j)
{
m_quads.push_back(btVector3(btScalar(i)/btScalar(N), btScalar(j)/btScalar(N), btScalar(N-i-j)/btScalar(N)));
}
}
}
//
void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti)
{
@ -3214,6 +3599,16 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
return (0);
}
void btSoftBody::setSelfCollision(bool useSelfCollision)
{
m_useSelfCollision = useSelfCollision;
}
bool btSoftBody::useSelfCollision()
{
return m_useSelfCollision;
}
//
void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
{
@ -3252,12 +3647,99 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
collider.ProcessColObj(this, pcoWrap);
}
break;
case fCollision::SDF_RD:
{
btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
{
const btTransform wtr = pcoWrap->getWorldTransform();
// const btTransform ctr = pcoWrap->getWorldTransform();
// const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
const btScalar timemargin = 0;
const btScalar basemargin = getCollisionShape()->getMargin();
btVector3 mins;
btVector3 maxs;
ATTRIBUTE_ALIGNED16(btDbvtVolume)
volume;
pcoWrap->getCollisionShape()->getAabb(wtr,
mins,
maxs);
volume = btDbvtVolume::FromMM(mins, maxs);
volume.Expand(btVector3(basemargin, basemargin, basemargin));
btSoftColliders::CollideSDF_RD docollideNode;
docollideNode.psb = this;
docollideNode.m_colObj1Wrap = pcoWrap;
docollideNode.m_rigidBody = prb1;
docollideNode.dynmargin = basemargin + timemargin;
docollideNode.stamargin = basemargin;
m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
if (this->m_useFaceContact)
{
btSoftColliders::CollideSDF_RDF docollideFace;
docollideFace.psb = this;
docollideFace.m_colObj1Wrap = pcoWrap;
docollideFace.m_rigidBody = prb1;
docollideFace.dynmargin = basemargin + timemargin;
docollideFace.stamargin = basemargin;
m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
}
}
}
break;
}
}
static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
{
if (n == 0)
return 0;
btDbvntNode* root = new btDbvntNode(n);
if (n->isinternal())
{
btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
root->childs[0] = c0;
btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
root->childs[1] = c1;
}
return root;
}
static inline void calculateNormalCone(btDbvntNode* root)
{
if (!root)
return;
if (root->isleaf())
{
const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
root->normal = face->m_normal;
root->angle = 0;
}
else
{
btVector3 n0(0,0,0), n1(0,0,0);
btScalar a0 = 0, a1 = 0;
if (root->childs[0])
{
calculateNormalCone(root->childs[0]);
n0 = root->childs[0]->normal;
a0 = root->childs[0]->angle;
}
if (root->childs[1])
{
calculateNormalCone(root->childs[1]);
n1 = root->childs[1]->normal;
a1 = root->childs[1]->angle;
}
root->normal = (n0+n1).safeNormalize();
root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
}
}
//
void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
{
BT_PROFILE("Deformable Collision");
const int cf = m_cfg.collisions & psb->m_cfg.collisions;
switch (cf & fCollision::SVSmask)
{
@ -3294,6 +3776,60 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
docollide);
}
}
break;
case fCollision::VF_DD:
{
if (psb->isActive() || this->isActive())
{
if (this != psb)
{
btSoftColliders::CollideVF_DD docollide;
/* common */
docollide.mrg = getCollisionShape()->getMargin() +
psb->getCollisionShape()->getMargin();
/* psb0 nodes vs psb1 faces */
if (psb->m_tetras.size() > 0)
docollide.useFaceNormal = true;
else
docollide.useFaceNormal = false;
docollide.psb[0] = this;
docollide.psb[1] = psb;
docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
/* psb1 nodes vs psb0 faces */
if (this->m_tetras.size() > 0)
docollide.useFaceNormal = true;
else
docollide.useFaceNormal = false;
docollide.psb[0] = psb;
docollide.psb[1] = this;
docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
}
else
{
if (psb->useSelfCollision())
{
btSoftColliders::CollideFF_DD docollide;
docollide.mrg = getCollisionShape()->getMargin() +
psb->getCollisionShape()->getMargin();
docollide.psb[0] = this;
docollide.psb[1] = psb;
if (this->m_tetras.size() > 0)
docollide.useFaceNormal = true;
else
docollide.useFaceNormal = false;
/* psb0 faces vs psb0 faces */
btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
calculateNormalCone(root);
this->m_fdbvt.selfCollideT(root,docollide);
delete root;
}
}
}
}
break;
default:
{
@ -3434,7 +3970,7 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
for (int j = 0; j < 4; j++)
{
m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]);
memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j] ? m_tetras[j].m_n[j] - &m_nodes[0] : -1;
memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1;
}
memPtr->m_c1 = m_tetras[i].m_c1;
memPtr->m_c2 = m_tetras[i].m_c2;
@ -3697,3 +4233,47 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
return btSoftBodyDataName;
}
void btSoftBody::updateDeactivation(btScalar timeStep)
{
if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
return;
if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold)
{
m_deactivationTime += timeStep;
}
else
{
m_deactivationTime = btScalar(0.);
setActivationState(0);
}
}
void btSoftBody::setZeroVelocity()
{
for (int i = 0; i < m_nodes.size(); ++i)
{
m_nodes[i].m_v.setZero();
}
}
bool btSoftBody::wantsSleeping()
{
if (getActivationState() == DISABLE_DEACTIVATION)
return false;
//disable deactivation
if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
return false;
if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
return true;
if (m_deactivationTime > gDeactivationTime)
{
return true;
}
return false;
}

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

@ -20,13 +20,15 @@ subject to the following restrictions:
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btVector3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletCollision/CollisionShapes/btConcaveShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "btSparseSDF.h"
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
//#ifdef BT_USE_DOUBLE_PRECISION
//#define btRigidBodyData btRigidBodyDoubleData
//#define btRigidBodyDataName "btRigidBodyDoubleData"
@ -159,11 +161,14 @@ 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
SVSmask = 0x0030, ///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
/* presets */
Default = SDF_RS,
END
@ -215,6 +220,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 */
};
/* sMedium */
@ -249,14 +255,17 @@ public:
struct Node : Feature
{
btVector3 m_x; // Position
btVector3 m_q; // Previous step 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_f; // Force accumulator
btVector3 m_n; // Normal
btScalar m_im; // 1/mass
btScalar m_area; // Area
btDbvtNode* m_leaf; // Leaf data
int m_battach : 1; // Attached
int index;
};
/* Link */
ATTRIBUTE_ALIGNED16(struct)
@ -279,6 +288,8 @@ public:
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;
};
/* Tetra */
struct Tetra : Feature
@ -289,7 +300,20 @@ public:
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;
};
/* TetraScratch */
struct TetraScratch
{
btMatrix3x3 m_F; // deformation gradient F
btScalar m_trace; // trace of F^T * F
btScalar m_J; // det(F)
btMatrix3x3 m_cofF; // cofactor of F
};
/* RContact */
struct RContact
{
@ -300,7 +324,67 @@ 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;
};
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;
};
/* SContact */
struct SContact
{
@ -627,6 +711,8 @@ 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
};
/* SolverState */
struct SolverState
@ -689,11 +775,19 @@ public:
btSoftBodyWorldInfo* m_worldInfo; // World info
tNoteArray m_notes; // Notes
tNodeArray m_nodes; // Nodes
tNodeArray m_renderNodes; // Nodes
tLinkArray m_links; // Links
tFaceArray m_faces; // Faces
tFaceArray 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
@ -704,6 +798,15 @@ public:
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<bool> m_clusterConnectivity; //cluster connectivity, for self-collision
@ -736,6 +839,16 @@ public:
return m_worldInfo;
}
void setDampingCoefficient(btScalar damping_coeff)
{
m_dampingCoefficient = damping_coeff;
}
void setUseFaceContact(bool useFaceContact)
{
m_useFaceContact = false;
}
///@todo: avoid internal softbody shape hack and move collision code to collision library
virtual void setCollisionShape(btCollisionShape* collisionShape)
{
@ -795,6 +908,8 @@ public:
Material* mat = 0);
/* Append anchor */
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);
@ -862,6 +977,16 @@ public:
/* Return the volume */
btScalar getVolume() const;
/* Cluster count */
btVector3 getCenterOfMass() const
{
btVector3 com(0, 0, 0);
for (int i = 0; i < m_nodes.size(); i++)
{
com += (m_nodes[i].m_x * this->getMass(i));
}
com /= this->getTotalMass();
return com;
}
int clusterCount() const;
/* Cluster center of mass */
static btVector3 clusterCom(const Cluster* cluster);
@ -915,6 +1040,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();
//
// Functionality to deal with new accelerated solvers.
@ -991,6 +1121,8 @@ public:
btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
void initializeFaceTree();
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;
void updateNormals();
void updateBounds();
@ -1005,7 +1137,14 @@ public:
void solveClusters(btScalar sor);
void applyClusters(bool drift);
void dampClusters();
void setSpringStiffness(btScalar k);
void initializeDmInverse();
void updateDeformation();
void advanceDeformation();
void applyForces();
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);
@ -1018,8 +1157,6 @@ public:
///fills the dataBuffer and returns the struct name (and 0 on failure)
virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
//virtual void serializeSingleObject(class btSerializer* serializer) const;
};
#endif //_BT_SOFT_BODY_H

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

@ -16,12 +16,17 @@ subject to the following restrictions:
#include "btSoftBodyInternals.h"
#include <stdio.h>
#include <string>
#include <iostream>
#include <sstream>
#include <string.h>
#include <algorithm>
#include "btSoftBodyHelpers.h"
#include "LinearMath/btConvexHull.h"
#include "LinearMath/btConvexHullComputer.h"
#include <map>
#include <vector>
//
static void drawVertex(btIDebugDraw* idraw,
const btVector3& x, btScalar s, const btVector3& c)
{
@ -721,7 +726,8 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
int resx,
int resy,
int fixeds,
bool gendiags)
bool gendiags,
btScalar perturbation)
{
#define IDX(_x_, _y_) ((_y_)*rx + (_x_))
/* Create nodes */
@ -741,7 +747,13 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo, const
for (int ix = 0; ix < rx; ++ix)
{
const btScalar tx = ix / (btScalar)(rx - 1);
x[IDX(ix, iy)] = lerp(py0, py1, tx);
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;
}
}
@ -1221,9 +1233,314 @@ if(face&&face[0])
}
}
}
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());
return (psb);
}
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());
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]);
}
}
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();
}
void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
{
std::ifstream fs_read;
fs_read.open(filename);
std::string line;
btVector3 pos;
btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
while (std::getline(fs_read, line))
{
std::stringstream ss(line);
if (line[0] == 'v')
{
}
else if (line[0] == 'f')
{
ss.ignore();
int id0, id1, id2;
ss >> id0;
ss >> id1;
ss >> id2;
btAlignedObjectArray<int> new_face;
new_face.push_back(id1);
new_face.push_back(id0);
new_face.push_back(id2);
additional_faces.push_back(new_face);
}
}
fs_read.close();
std::ofstream fs_write;
fs_write.open(filename, std::ios_base::app);
for (int i = 0; i < additional_faces.size(); ++i)
{
fs_write << "f";
for (int n = 0; n < 3; n++)
{
fs_write << " " << additional_faces[i][n];
}
fs_write << "\n";
}
fs_write.close();
}
// 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);
}
// 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;
}
}

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

@ -17,7 +17,8 @@ subject to the following restrictions:
#define BT_SOFT_BODY_HELPERS_H
#include "btSoftBody.h"
#include <fstream>
#include <string>
//
// Helpers
//
@ -91,7 +92,8 @@ struct btSoftBodyHelpers
int resx,
int resy,
int fixeds,
bool gendiags);
bool gendiags,
btScalar perturbation = 0.);
/* Create a patch with UV Texture Coordinates */
static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
const btVector3& corner00,
@ -140,7 +142,17 @@ struct btSoftBodyHelpers
bool bfacelinks,
bool btetralinks,
bool bfacesfromtetras);
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);
/// 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,

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

@ -25,7 +25,43 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
#include <string.h> //for memset
#include <cmath>
// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction
static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
btMultiBodyJacobianData& jacobianData,
const btVector3& contact_point,
const btVector3& dir)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
btScalar* jac = &jacobianData.m_jacobians[0];
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
}
static btVector3 generateUnitOrthogonalVector(const btVector3& u)
{
btScalar ux = u.getX();
btScalar uy = u.getY();
btScalar uz = u.getZ();
btScalar ax = std::abs(ux);
btScalar ay = std::abs(uy);
btScalar az = std::abs(uz);
btVector3 v;
if (ax <= ay && ax <= az)
v = btVector3(0, -uz, uy);
else if (ay <= ax && ay <= az)
v = btVector3(-uz, 0, ux);
else
v = btVector3(-uy, ux, 0);
v.normalize();
return v;
}
//
// btSymMatrix
//
@ -298,6 +334,46 @@ static inline btMatrix3x3 Diagonal(btScalar x)
m[2] = btVector3(0, 0, x);
return (m);
}
static inline btMatrix3x3 Diagonal(const btVector3& v)
{
btMatrix3x3 m;
m[0] = btVector3(v.getX(), 0, 0);
m[1] = btVector3(0, v.getY(), 0);
m[2] = btVector3(0, 0, v.getZ());
return (m);
}
static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof)
{
btScalar result = 0;
for (int i = 0; i < ndof; ++i)
result += a[i] * b[i];
return result;
}
static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3,
const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
{
btMatrix3x3 m;
btScalar a11 = Dot(v1,u1,ndof);
btScalar a12 = Dot(v1,u2,ndof);
btScalar a13 = Dot(v1,u3,ndof);
btScalar a21 = Dot(v2,u1,ndof);
btScalar a22 = Dot(v2,u2,ndof);
btScalar a23 = Dot(v2,u3,ndof);
btScalar a31 = Dot(v3,u1,ndof);
btScalar a32 = Dot(v3,u2,ndof);
btScalar a33 = Dot(v3,u3,ndof);
m[0] = btVector3(a11, a12, a13);
m[1] = btVector3(a21, a22, a23);
m[2] = btVector3(a31, a32, a33);
return (m);
}
//
static inline btMatrix3x3 Add(const btMatrix3x3& a,
const btMatrix3x3& b)
@ -427,6 +503,77 @@ static inline void ProjectOrigin(const btVector3& a,
}
}
//
static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
{
btScalar a, f, u, v;
btVector3 e1 = v1 - v0;
btVector3 e2 = v2 - v0;
btVector3 h = dir.cross(e2);
a = e1.dot(h);
if (a > -0.00001 && a < 0.00001)
return (false);
f = btScalar(1) / a;
btVector3 s = origin - v0;
u = f * s.dot(h);
if (u < 0.0 || u > 1.0)
return (false);
btVector3 q = s.cross(e1);
v = f * dir.dot(q);
if (v < 0.0 || u + v > 1.0)
return (false);
// at this stage we can compute t to find out where
// the intersection point is on the line
t = f * e2.dot(q);
if (t > 0) // ray intersection
return (true);
else // this means that there is a line intersection
// but not a ray intersection
return (false);
}
static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
{
btVector3 dir = rayEnd - rayStart;
btScalar dir_norm = dir.norm();
if (dir_norm < SIMD_EPSILON)
return false;
dir.normalize();
btScalar t;
bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
if (ret)
{
if (t <= dir_norm)
{
sect = rayStart + dir * t;
}
else
{
ret = false;
}
}
if (ret)
{
btVector3 n = (p3-p1).cross(p2-p1);
n.safeNormalize();
if (n.dot(dir) < 0)
normal = n;
else
normal = -n;
}
return ret;
}
//
template <typename T>
static inline T BaryEval(const T& a,
@ -905,6 +1052,203 @@ struct btSoftColliders
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideSDF_RD
//
struct CollideSDF_RD : btDbvt::ICollide
{
void Process(const btDbvtNode* leaf)
{
btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
DoNode(*node);
}
void DoNode(btSoftBody::Node& n) const
{
const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
btSoftBody::DeformableNodeRigidContact c;
if (!n.m_battach)
{
// check for collision at x_{n+1}^* as well at x_n
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
{
const btScalar ima = n.m_im;
// todo: collision between multibody and fixed deformable node will be missed.
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
const btScalar ms = ima + imb;
if (ms > 0)
{
// resolve contact at x_n
psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
btSoftBody::sCti& cti = c.m_cti;
c.m_node = &n;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 normal = cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
c.jacobianData_t2 = jacobianData_t2;
c.t1 = t1;
c.t2 = t2;
}
}
psb->m_nodeRigidContacts.push_back(c);
}
}
}
}
btSoftBody* psb;
const btCollisionObjectWrapper* m_colObj1Wrap;
btRigidBody* m_rigidBody;
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideSDF_RDF
//
struct CollideSDF_RDF : btDbvt::ICollide
{
void Process(const btDbvtNode* leaf)
{
btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
DoNode(*face);
}
void DoNode(btSoftBody::Face& f) const
{
btSoftBody::Node* n0 = f.m_n[0];
btSoftBody::Node* n1 = f.m_n[1];
btSoftBody::Node* n2 = f.m_n[2];
const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
btSoftBody::DeformableFaceRigidContact c;
btVector3 contact_point;
btVector3 bary;
if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
{
f.m_pcontact[3] = 1;
btScalar ima = n0->m_im + n1->m_im + n2->m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
// todo: collision between multibody and fixed deformable face will be missed.
const btScalar ms = ima + imb;
if (ms > 0)
{
// resolve contact at x_n
psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
btSoftBody::sCti& cti = c.m_cti;
c.m_contactPoint = contact_point;
c.m_bary = bary;
// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
c.m_face = &f;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im;
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = contact_point - wtr.getOrigin();
// we do not scale the impulse matrix by dt
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 normal = cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
c.jacobianData_t2 = jacobianData_t2;
c.t1 = t1;
c.t2 = t2;
}
}
psb->m_faceRigidContacts.push_back(c);
}
}
else
{
f.m_pcontact[3] = 0;
}
}
btSoftBody* psb;
const btCollisionObjectWrapper* m_colObj1Wrap;
btRigidBody* m_rigidBody;
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideVF_SS
//
@ -915,6 +1259,12 @@ struct btSoftColliders
{
btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
for (int i = 0; i < 3; ++i)
{
if (face->m_n[i] == node)
continue;
}
btVector3 o = node->m_x;
btVector3 p;
btScalar d = SIMD_INFINITY;
@ -954,6 +1304,137 @@ struct btSoftColliders
btSoftBody* psb[2];
btScalar mrg;
};
//
// CollideVF_DD
//
struct CollideVF_DD : btDbvt::ICollide
{
void Process(const btDbvtNode* lnode,
const btDbvtNode* lface)
{
btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
btVector3 o = node->m_x;
btVector3 p;
btScalar d = SIMD_INFINITY;
ProjectOrigin(face->m_n[0]->m_x - o,
face->m_n[1]->m_x - o,
face->m_n[2]->m_x - o,
p, d);
const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
if (d < (m * m))
{
const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
const btScalar ma = node->m_im;
btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
if ((n[0]->m_im <= 0) ||
(n[1]->m_im <= 0) ||
(n[2]->m_im <= 0))
{
mb = 0;
}
const btScalar ms = ma + mb;
if (ms > 0)
{
btSoftBody::DeformableFaceNodeContact c;
if (useFaceNormal)
c.m_normal = face->m_normal;
else
c.m_normal = p / -btSqrt(d);
c.m_margin = mrg;
c.m_node = node;
c.m_face = face;
c.m_bary = w;
// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
c.m_c0 = btScalar(1)/(ma + c.m_imf);
psb[0]->m_faceNodeContacts.push_back(c);
}
}
}
btSoftBody* psb[2];
btScalar mrg;
bool useFaceNormal;
};
//
// CollideFF_DD
//
struct CollideFF_DD : btDbvt::ICollide
{
void Process(const btDbvntNode* lface1,
const btDbvntNode* lface2)
{
btSoftBody::Face* f = (btSoftBody::Face*)lface1->data;
btSoftBody::Face* face = (btSoftBody::Face*)lface2->data;
for (int node_id = 0; node_id < 3; ++node_id)
{
btSoftBody::Node* node = f->m_n[node_id];
bool skip = false;
for (int i = 0; i < 3; ++i)
{
if (face->m_n[i] == node)
{
skip = true;
break;
}
}
if (skip)
continue;
btVector3 o = node->m_x;
btVector3 p;
btScalar d = SIMD_INFINITY;
ProjectOrigin(face->m_n[0]->m_x - o,
face->m_n[1]->m_x - o,
face->m_n[2]->m_x - o,
p, d);
const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
if (d < (m * m))
{
const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
const btScalar ma = node->m_im;
btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
if ((n[0]->m_im <= 0) ||
(n[1]->m_im <= 0) ||
(n[2]->m_im <= 0))
{
mb = 0;
}
const btScalar ms = ma + mb;
if (ms > 0)
{
btSoftBody::DeformableFaceNodeContact c;
if (useFaceNormal)
c.m_normal = face->m_normal;
else
c.m_normal = p / -btSqrt(d);
c.m_margin = mrg;
c.m_node = node;
c.m_face = face;
c.m_bary = w;
// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
c.m_c0 = btScalar(1)/(ma + c.m_imf);
psb[0]->m_faceNodeContacts.push_back(c);
}
}
}
}
btSoftBody* psb[2];
btScalar mrg;
bool useFaceNormal;
};
};
#endif //_BT_SOFT_BODY_INTERNALS_H

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

@ -35,7 +35,8 @@ public:
CL_SOLVER,
CL_SIMD_SOLVER,
DX_SOLVER,
DX_SIMD_SOLVER
DX_SIMD_SOLVER,
DEFORMABLE_SOLVER
};
protected:
@ -71,10 +72,10 @@ public:
virtual void copyBackToSoftBodies(bool bMove = true) = 0;
/** Predict motion of soft bodies into next timestep */
virtual void predictMotion(float solverdt) = 0;
virtual void predictMotion(btScalar solverdt) = 0;
/** Solve constraints for a set of soft bodies */
virtual void solveConstraints(float solverdt) = 0;
virtual void solveConstraints(btScalar solverdt) = 0;
/** Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes */
virtual void updateSoftBodies() = 0;

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

@ -20,27 +20,38 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
// Modified Paul Hsieh hash
template <const int DWORDLEN>
unsigned int HsiehHash(const void* pdata)
{
const unsigned short* data = (const unsigned short*)pdata;
unsigned hash = DWORDLEN << 2, tmp;
for (int i = 0; i < DWORDLEN; ++i)
{
hash += data[0];
tmp = (data[1] << 11) ^ hash;
// Fast Hash
#if !defined (get16bits)
#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\
+(unsigned int)(((const unsigned char *)(d))[0]) )
#endif
//
// super hash function by Paul Hsieh
//
inline unsigned int HsiehHash (const char * data, int len) {
unsigned int hash = len, tmp;
len>>=2;
/* Main loop */
for (;len > 0; len--) {
hash += get16bits (data);
tmp = (get16bits (data+2) << 11) ^ hash;
hash = (hash << 16) ^ tmp;
data += 2;
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;
return (hash);
return hash;
}
template <const int CELLSIZE>
@ -70,12 +81,17 @@ struct btSparseSdf
btAlignedObjectArray<Cell*> cells;
btScalar voxelsz;
btScalar m_defaultVoxelsz;
int puid;
int ncells;
int m_clampCells;
int nprobes;
int nqueries;
~btSparseSdf()
{
Reset();
}
//
// Methods
//
@ -87,9 +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;
Reset();
}
//
void setDefaultVoxelsz(btScalar sz)
{
m_defaultVoxelsz = sz;
}
void Reset()
{
for (int i = 0, ni = cells.size(); i < ni; ++i)
@ -103,7 +126,7 @@ struct btSparseSdf
pc = pn;
}
}
voxelsz = 0.25;
voxelsz = m_defaultVoxelsz;
puid = 0;
ncells = 0;
nprobes = 1;
@ -197,6 +220,9 @@ 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);
c = c->next;
}
}
@ -248,7 +274,7 @@ struct btSparseSdf
Lerp(gy[2], gy[3], ix.f), iz.f));
normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f),
Lerp(gz[2], gz[3], ix.f), iy.f));
normal = normal.normalized();
normal.safeNormalize();
#else
normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized();
#endif
@ -322,19 +348,22 @@ struct btSparseSdf
{
struct btS
{
int x, y, z;
int x, y, z, w;
void* p;
};
btS myset;
//memset may be needed in case of additional (uninitialized) padding!
//memset(&myset, 0, sizeof(btS));
myset.x = x;
myset.y = y;
myset.z = z;
myset.w = 0;
myset.p = (void*)shape;
const void* ptr = &myset;
const char* ptr = (const char*)&myset;
unsigned int result = HsiehHash<sizeof(btS) / 4>(ptr);
unsigned int result = HsiehHash(ptr, sizeof(btS) );
return result;
}

916
thirdparty/bullet/LinearMath/btImplicitQRSVD.h vendored Normal file
View File

@ -0,0 +1,916 @@
/**
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.
Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran
Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
If the code is used in an article, the following paper shall be cited:
@techreport{qrsvd:2016,
title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
year={2016},
institution={University of California Los Angeles}
}
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
**/
#ifndef btImplicitQRSVD_h
#define btImplicitQRSVD_h
#include "btMatrix3x3.h"
class btMatrix2x2
{
public:
btScalar m_00, m_01, m_10, m_11;
btMatrix2x2(): m_00(0), m_10(0), m_01(0), m_11(0)
{
}
btMatrix2x2(const btMatrix2x2& other): m_00(other.m_00),m_01(other.m_01),m_10(other.m_10),m_11(other.m_11)
{}
btScalar& operator()(int i, int j)
{
if (i == 0 && j == 0)
return m_00;
if (i == 1 && j == 0)
return m_10;
if (i == 0 && j == 1)
return m_01;
if (i == 1 && j == 1)
return m_11;
btAssert(false);
return m_00;
}
const btScalar& operator()(int i, int j) const
{
if (i == 0 && j == 0)
return m_00;
if (i == 1 && j == 0)
return m_10;
if (i == 0 && j == 1)
return m_01;
if (i == 1 && j == 1)
return m_11;
btAssert(false);
return m_00;
}
void setIdentity()
{
m_00 = 1;
m_11 = 1;
m_01 = 0;
m_10 = 0;
}
};
static inline btScalar copySign(btScalar x, btScalar y) {
if ((x < 0 && y > 0) || (x > 0 && y < 0))
return -x;
return x;
}
/**
Class for givens rotation.
Row rotation G*A corresponds to something like
c -s 0
( s c 0 ) A
0 0 1
Column rotation A G' corresponds to something like
c -s 0
A ( s c 0 )
0 0 1
c and s are always computed so that
( c -s ) ( a ) = ( * )
s c b ( 0 )
Assume rowi<rowk.
*/
class GivensRotation {
public:
int rowi;
int rowk;
btScalar c;
btScalar s;
inline GivensRotation(int rowi_in, int rowk_in)
: rowi(rowi_in)
, rowk(rowk_in)
, c(1)
, s(0)
{
}
inline GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
: rowi(rowi_in)
, rowk(rowk_in)
{
compute(a, b);
}
~GivensRotation() {}
inline void transposeInPlace()
{
s = -s;
}
/**
Compute c and s from a and b so that
( c -s ) ( a ) = ( * )
s c b ( 0 )
*/
inline void compute(const btScalar a, const btScalar b)
{
btScalar d = a * a + b * b;
c = 1;
s = 0;
if (d > SIMD_EPSILON) {
btScalar sqrtd = btSqrt(d);
if (sqrtd>SIMD_EPSILON)
{
btScalar t = btScalar(1.0)/sqrtd;
c = a * t;
s = -b * t;
}
}
}
/**
This function computes c and s so that
( c -s ) ( a ) = ( 0 )
s c b ( * )
*/
inline void computeUnconventional(const btScalar a, const btScalar b)
{
btScalar d = a * a + b * b;
c = 0;
s = 1;
if (d > SIMD_EPSILON) {
btScalar t = btScalar(1.0)/btSqrt(d);
s = a * t;
c = b * t;
}
}
/**
Fill the R with the entries of this rotation
*/
inline void fill(const btMatrix3x3& R) const
{
btMatrix3x3& A = const_cast<btMatrix3x3&>(R);
A.setIdentity();
A[rowi][rowi] = c;
A[rowk][rowi] = -s;
A[rowi][rowk] = s;
A[rowk][rowk] = c;
}
inline void fill(const btMatrix2x2& R) const
{
btMatrix2x2& A = const_cast<btMatrix2x2&>(R);
A(rowi,rowi) = c;
A(rowk,rowi) = -s;
A(rowi,rowk) = s;
A(rowk,rowk) = c;
}
/**
This function does something like
c -s 0
( s c 0 ) A -> A
0 0 1
It only affects row i and row k of A.
*/
inline void rowRotation(btMatrix3x3& A) const
{
for (int j = 0; j < 3; j++) {
btScalar tau1 = A[rowi][j];
btScalar tau2 = A[rowk][j];
A[rowi][j] = c * tau1 - s * tau2;
A[rowk][j] = s * tau1 + c * tau2;
}
}
inline void rowRotation(btMatrix2x2& A) const
{
for (int j = 0; j < 2; j++) {
btScalar tau1 = A(rowi,j);
btScalar tau2 = A(rowk,j);
A(rowi,j) = c * tau1 - s * tau2;
A(rowk,j) = s * tau1 + c * tau2;
}
}
/**
This function does something like
c s 0
A ( -s c 0 ) -> A
0 0 1
It only affects column i and column k of A.
*/
inline void columnRotation(btMatrix3x3& A) const
{
for (int j = 0; j < 3; j++) {
btScalar tau1 = A[j][rowi];
btScalar tau2 = A[j][rowk];
A[j][rowi] = c * tau1 - s * tau2;
A[j][rowk] = s * tau1 + c * tau2;
}
}
inline void columnRotation(btMatrix2x2& A) const
{
for (int j = 0; j < 2; j++) {
btScalar tau1 = A(j,rowi);
btScalar tau2 = A(j,rowk);
A(j,rowi) = c * tau1 - s * tau2;
A(j,rowk) = s * tau1 + c * tau2;
}
}
/**
Multiply givens must be for same row and column
**/
inline void operator*=(const GivensRotation& A)
{
btScalar new_c = c * A.c - s * A.s;
btScalar new_s = s * A.c + c * A.s;
c = new_c;
s = new_s;
}
/**
Multiply givens must be for same row and column
**/
inline GivensRotation operator*(const GivensRotation& A) const
{
GivensRotation r(*this);
r *= A;
return r;
}
};
/**
\brief zero chasing the 3X3 matrix to bidiagonal form
original form of H: x x 0
x x x
0 0 x
after zero chase:
x x 0
0 x x
0 0 x
*/
inline void zeroChase(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
{
/**
Reduce H to of form
x x +
0 x x
0 0 x
*/
GivensRotation r1(H[0][0], H[1][0], 0, 1);
/**
Reduce H to of form
x x 0
0 x x
0 + x
Can calculate r2 without multiplying by r1 since both entries are in first two
rows thus no need to divide by sqrt(a^2+b^2)
*/
GivensRotation r2(1, 2);
if (H[1][0] != 0)
r2.compute(H[0][0] * H[0][1] + H[1][0] * H[1][1], H[0][0] * H[0][2] + H[1][0] * H[1][2]);
else
r2.compute(H[0][1], H[0][2]);
r1.rowRotation(H);
/* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
r2.columnRotation(H);
r2.columnRotation(V);
/**
Reduce H to of form
x x 0
0 x x
0 0 x
*/
GivensRotation r3(H[1][1], H[2][1], 1, 2);
r3.rowRotation(H);
// Save this till end for better cache coherency
// r1.rowRotation(u_transpose);
// r3.rowRotation(u_transpose);
r1.columnRotation(U);
r3.columnRotation(U);
}
/**
\brief make a 3X3 matrix to upper bidiagonal form
original form of H: x x x
x x x
x x x
after zero chase:
x x 0
0 x x
0 0 x
*/
inline void makeUpperBidiag(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
{
U.setIdentity();
V.setIdentity();
/**
Reduce H to of form
x x x
x x x
0 x x
*/
GivensRotation r(H[1][0], H[2][0], 1, 2);
r.rowRotation(H);
// r.rowRotation(u_transpose);
r.columnRotation(U);
// zeroChase(H, u_transpose, V);
zeroChase(H, U, V);
}
/**
\brief make a 3X3 matrix to lambda shape
original form of H: x x x
* x x x
* x x x
after :
* x 0 0
* x x 0
* x 0 x
*/
inline void makeLambdaShape(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
{
U.setIdentity();
V.setIdentity();
/**
Reduce H to of form
* x x 0
* x x x
* x x x
*/
GivensRotation r1(H[0][1], H[0][2], 1, 2);
r1.columnRotation(H);
r1.columnRotation(V);
/**
Reduce H to of form
* x x 0
* x x 0
* x x x
*/
r1.computeUnconventional(H[1][2], H[2][2]);
r1.rowRotation(H);
r1.columnRotation(U);
/**
Reduce H to of form
* x x 0
* x x 0
* x 0 x
*/
GivensRotation r2(H[2][0], H[2][1], 0, 1);
r2.columnRotation(H);
r2.columnRotation(V);
/**
Reduce H to of form
* x 0 0
* x x 0
* x 0 x
*/
r2.computeUnconventional(H[0][1], H[1][1]);
r2.rowRotation(H);
r2.columnRotation(U);
}
/**
\brief 2x2 polar decomposition.
\param[in] A matrix.
\param[out] R Robustly a rotation matrix.
\param[out] S_Sym Symmetric. Whole matrix is stored
Polar guarantees negative sign is on the small magnitude singular value.
S is guaranteed to be the closest one to identity.
R is guaranteed to be the closest rotation to A.
*/
inline void polarDecomposition(const btMatrix2x2& A,
GivensRotation& R,
const btMatrix2x2& S_Sym)
{
btScalar a = (A(0, 0) + A(1, 1)), b = (A(1, 0) - A(0, 1));
btScalar denominator = btSqrt(a*a+b*b);
R.c = (btScalar)1;
R.s = (btScalar)0;
if (denominator > SIMD_EPSILON) {
/*
No need to use a tolerance here because x(0) and x(1) always have
smaller magnitude then denominator, therefore overflow never happens.
In Bullet, we use a tolerance anyway.
*/
R.c = a / denominator;
R.s = -b / denominator;
}
btMatrix2x2& S = const_cast<btMatrix2x2&>(S_Sym);
S = A;
R.rowRotation(S);
}
inline void polarDecomposition(const btMatrix2x2& A,
const btMatrix2x2& R,
const btMatrix2x2& S_Sym)
{
GivensRotation r(0, 1);
polarDecomposition(A, r, S_Sym);
r.fill(R);
}
/**
\brief 2x2 SVD (singular value decomposition) A=USV'
\param[in] A Input matrix.
\param[out] U Robustly a rotation matrix in Givens form
\param[out] Sigma matrix of singular values sorted with decreasing magnitude. The second one can be negative.
\param[out] V Robustly a rotation matrix in Givens form
*/
inline void singularValueDecomposition(
const btMatrix2x2& A,
GivensRotation& U,
const btMatrix2x2& Sigma,
GivensRotation& V,
const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
{
btMatrix2x2& sigma = const_cast<btMatrix2x2&>(Sigma);
sigma.setIdentity();
btMatrix2x2 S_Sym;
polarDecomposition(A, U, S_Sym);
btScalar cosine, sine;
btScalar x = S_Sym(0, 0);
btScalar y = S_Sym(0, 1);
btScalar z = S_Sym(1, 1);
if (y == 0) {
// S is already diagonal
cosine = 1;
sine = 0;
sigma(0,0) = x;
sigma(1,1) = z;
}
else {
btScalar tau = 0.5 * (x - z);
btScalar val = tau * tau + y * y;
if (val > SIMD_EPSILON)
{
btScalar w = btSqrt(val);
// w > y > 0
btScalar t;
if (tau > 0) {
// tau + w > w > y > 0 ==> division is safe
t = y / (tau + w);
}
else {
// tau - w < -w < -y < 0 ==> division is safe
t = y / (tau - w);
}
cosine = btScalar(1) / btSqrt(t * t + btScalar(1));
sine = -t * cosine;
/*
V = [cosine -sine; sine cosine]
Sigma = V'SV. Only compute the diagonals for efficiency.
Also utilize symmetry of S and don't form V yet.
*/
btScalar c2 = cosine * cosine;
btScalar csy = 2 * cosine * sine * y;
btScalar s2 = sine * sine;
sigma(0,0) = c2 * x - csy + s2 * z;
sigma(1,1) = s2 * x + csy + c2 * z;
} else
{
cosine = 1;
sine = 0;
sigma(0,0) = x;
sigma(1,1) = z;
}
}
// Sorting
// Polar already guarantees negative sign is on the small magnitude singular value.
if (sigma(0,0) < sigma(1,1)) {
std::swap(sigma(0,0), sigma(1,1));
V.c = -sine;
V.s = cosine;
}
else {
V.c = cosine;
V.s = sine;
}
U *= V;
}
/**
\brief 2x2 SVD (singular value decomposition) A=USV'
\param[in] A Input matrix.
\param[out] U Robustly a rotation matrix.
\param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
\param[out] V Robustly a rotation matrix.
*/
inline void singularValueDecomposition(
const btMatrix2x2& A,
const btMatrix2x2& U,
const btMatrix2x2& Sigma,
const btMatrix2x2& V,
const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
{
GivensRotation gv(0, 1);
GivensRotation gu(0, 1);
singularValueDecomposition(A, gu, Sigma, gv);
gu.fill(U);
gv.fill(V);
}
/**
\brief compute wilkinsonShift of the block
a1 b1
b1 a2
based on the wilkinsonShift formula
mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2
*/
inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
{
btScalar d = (btScalar)0.5 * (a1 - a2);
btScalar bs = b1 * b1;
btScalar val = d * d + bs;
if (val>SIMD_EPSILON)
{
btScalar denom = btFabs(d) + btSqrt(val);
btScalar mu = a2 - copySign(bs / (denom), d);
// T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
return mu;
}
return a2;
}
/**
\brief Helper function of 3X3 SVD for processing 2X2 SVD
*/
template <int t>
inline void process(btMatrix3x3& B, btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V)
{
int other = (t == 1) ? 0 : 2;
GivensRotation u(0, 1);
GivensRotation v(0, 1);
sigma[other] = B[other][other];
btMatrix2x2 B_sub, sigma_sub;
if (t == 0)
{
B_sub.m_00 = B[0][0];
B_sub.m_10 = B[1][0];
B_sub.m_01 = B[0][1];
B_sub.m_11 = B[1][1];
sigma_sub.m_00 = sigma[0];
sigma_sub.m_11 = sigma[1];
// singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
singularValueDecomposition(B_sub, u, sigma_sub, v);
B[0][0] = B_sub.m_00;
B[1][0] = B_sub.m_10;
B[0][1] = B_sub.m_01;
B[1][1] = B_sub.m_11;
sigma[0] = sigma_sub.m_00;
sigma[1] = sigma_sub.m_11;
}
else
{
B_sub.m_00 = B[1][1];
B_sub.m_10 = B[2][1];
B_sub.m_01 = B[1][2];
B_sub.m_11 = B[2][2];
sigma_sub.m_00 = sigma[1];
sigma_sub.m_11 = sigma[2];
// singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
singularValueDecomposition(B_sub, u, sigma_sub, v);
B[1][1] = B_sub.m_00;
B[2][1] = B_sub.m_10;
B[1][2] = B_sub.m_01;
B[2][2] = B_sub.m_11;
sigma[1] = sigma_sub.m_00;
sigma[2] = sigma_sub.m_11;
}
u.rowi += t;
u.rowk += t;
v.rowi += t;
v.rowk += t;
u.columnRotation(U);
v.columnRotation(V);
}
/**
\brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma
*/
inline void flipSign(int i, btMatrix3x3& U, btVector3& sigma)
{
sigma[i] = -sigma[i];
U[0][i] = -U[0][i];
U[1][i] = -U[1][i];
U[2][i] = -U[2][i];
}
inline void flipSign(int i, btMatrix3x3& U)
{
U[0][i] = -U[0][i];
U[1][i] = -U[1][i];
U[2][i] = -U[2][i];
}
inline void swapCol(btMatrix3x3& A, int i, int j)
{
for (int d = 0; d < 3; ++d)
std::swap(A[d][i], A[d][j]);
}
/**
\brief Helper function of 3X3 SVD for sorting singular values
*/
inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t)
{
if (t == 0)
{
// Case: sigma(0) > |sigma(1)| >= |sigma(2)|
if (btFabs(sigma[1]) >= btFabs(sigma[2])) {
if (sigma[1] < 0) {
flipSign(1, U, sigma);
flipSign(2, U, sigma);
}
return;
}
//fix sign of sigma for both cases
if (sigma[2] < 0) {
flipSign(1, U, sigma);
flipSign(2, U, sigma);
}
//swap sigma(1) and sigma(2) for both cases
std::swap(sigma[1], sigma[2]);
// swap the col 1 and col 2 for U,V
swapCol(U,1,2);
swapCol(V,1,2);
// Case: |sigma(2)| >= sigma(0) > |simga(1)|
if (sigma[1] > sigma[0]) {
std::swap(sigma[0], sigma[1]);
swapCol(U,0,1);
swapCol(V,0,1);
}
// Case: sigma(0) >= |sigma(2)| > |simga(1)|
else {
flipSign(2, U);
flipSign(2, V);
}
}
else if (t == 1)
{
// Case: |sigma(0)| >= sigma(1) > |sigma(2)|
if (btFabs(sigma[0]) >= sigma[1]) {
if (sigma[0] < 0) {
flipSign(0, U, sigma);
flipSign(2, U, sigma);
}
return;
}
//swap sigma(0) and sigma(1) for both cases
std::swap(sigma[0], sigma[1]);
swapCol(U, 0, 1);
swapCol(V, 0, 1);
// Case: sigma(1) > |sigma(2)| >= |sigma(0)|
if (btFabs(sigma[1]) < btFabs(sigma[2])) {
std::swap(sigma[1], sigma[2]);
swapCol(U, 1, 2);
swapCol(V, 1, 2);
}
// Case: sigma(1) >= |sigma(0)| > |sigma(2)|
else {
flipSign(1, U);
flipSign(1, V);
}
// fix sign for both cases
if (sigma[1] < 0) {
flipSign(1, U, sigma);
flipSign(2, U, sigma);
}
}
}
/**
\brief 3X3 SVD (singular value decomposition) A=USV'
\param[in] A Input matrix.
\param[out] U is a rotation matrix.
\param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative.
\param[out] V is a rotation matrix.
*/
inline int singularValueDecomposition(const btMatrix3x3& A,
btMatrix3x3& U,
btVector3& sigma,
btMatrix3x3& V,
btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
{
using std::fabs;
btMatrix3x3 B = A;
U.setIdentity();
V.setIdentity();
makeUpperBidiag(B, U, V);
int count = 0;
btScalar mu = (btScalar)0;
GivensRotation r(0, 1);
btScalar alpha_1 = B[0][0];
btScalar beta_1 = B[0][1];
btScalar alpha_2 = B[1][1];
btScalar alpha_3 = B[2][2];
btScalar beta_2 = B[1][2];
btScalar gamma_1 = alpha_1 * beta_1;
btScalar gamma_2 = alpha_2 * beta_2;
btScalar val = alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2;
if (val > SIMD_EPSILON)
{
tol *= btMax((btScalar)0.5 * btSqrt(val), (btScalar)1);
}
/**
Do implicit shift QR until A^T A is block diagonal
*/
int max_count = 100;
while (btFabs(beta_2) > tol && btFabs(beta_1) > tol
&& btFabs(alpha_1) > tol && btFabs(alpha_2) > tol
&& btFabs(alpha_3) > tol
&& count < max_count) {
mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
r.compute(alpha_1 * alpha_1 - mu, gamma_1);
r.columnRotation(B);
r.columnRotation(V);
zeroChase(B, U, V);
alpha_1 = B[0][0];
beta_1 = B[0][1];
alpha_2 = B[1][1];
alpha_3 = B[2][2];
beta_2 = B[1][2];
gamma_1 = alpha_1 * beta_1;
gamma_2 = alpha_2 * beta_2;
count++;
}
/**
Handle the cases of one of the alphas and betas being 0
Sorted by ease of handling and then frequency
of occurrence
If B is of form
x x 0
0 x 0
0 0 x
*/
if (btFabs(beta_2) <= tol) {
process<0>(B, U, sigma, V);
sort(U, sigma, V,0);
}
/**
If B is of form
x 0 0
0 x x
0 0 x
*/
else if (btFabs(beta_1) <= tol) {
process<1>(B, U, sigma, V);
sort(U, sigma, V,1);
}
/**
If B is of form
x x 0
0 0 x
0 0 x
*/
else if (btFabs(alpha_2) <= tol) {
/**
Reduce B to
x x 0
0 0 0
0 0 x
*/
GivensRotation r1(1, 2);
r1.computeUnconventional(B[1][2], B[2][2]);
r1.rowRotation(B);
r1.columnRotation(U);
process<0>(B, U, sigma, V);
sort(U, sigma, V, 0);
}
/**
If B is of form
x x 0
0 x x
0 0 0
*/
else if (btFabs(alpha_3) <= tol) {
/**
Reduce B to
x x +
0 x 0
0 0 0
*/
GivensRotation r1(1, 2);
r1.compute(B[1][1], B[1][2]);
r1.columnRotation(B);
r1.columnRotation(V);
/**
Reduce B to
x x 0
+ x 0
0 0 0
*/
GivensRotation r2(0, 2);
r2.compute(B[0][0], B[0][2]);
r2.columnRotation(B);
r2.columnRotation(V);
process<0>(B, U, sigma, V);
sort(U, sigma, V, 0);
}
/**
If B is of form
0 x 0
0 x x
0 0 x
*/
else if (btFabs(alpha_1) <= tol) {
/**
Reduce B to
0 0 +
0 x x
0 0 x
*/
GivensRotation r1(0, 1);
r1.computeUnconventional(B[0][1], B[1][1]);
r1.rowRotation(B);
r1.columnRotation(U);
/**
Reduce B to
0 0 0
0 x x
0 + x
*/
GivensRotation r2(0, 2);
r2.computeUnconventional(B[0][2], B[2][2]);
r2.rowRotation(B);
r2.columnRotation(U);
process<1>(B, U, sigma, V);
sort(U, sigma, V, 1);
}
return count;
}
#endif /* btImplicitQRSVD_h */

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

@ -126,6 +126,13 @@ public:
return *this;
}
SIMD_FORCE_INLINE btMatrix3x3(const btVector3& v0, const btVector3& v1, const btVector3& v2)
{
m_el[0] = v0;
m_el[1] = v1;
m_el[2] = v2;
}
#endif
/** @brief Get a column of the matrix as a vector

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

@ -338,24 +338,23 @@ struct btMatrixX
btMatrixX res(rows(), other.cols());
res.setZero();
// BT_PROFILE("btMatrixX mul");
for (int j = 0; j < res.cols(); ++j)
for (int i = 0; i < rows(); ++i)
{
{
for (int i = 0; i < res.rows(); ++i)
for (int j = 0; j < other.cols(); ++j)
{
T dotProd = 0;
// T dotProd2=0;
//int waste=0,waste2=0;
{
{
int r = rows();
int c = cols();
for (int k = 0; k < cols(); k++)
{
// bool useOtherCol = true;
T w = (*this)(i, k);
if (other(k, j) != 0.f)
{
for (int v = 0; v < rows(); v++)
{
T w = (*this)(i, v);
if (other(v, j) != 0.f)
{
dotProd += w * other(v, j);
dotProd += w * other(k, j);
}
}
}

26
thirdparty/bullet/LinearMath/btScalar.h vendored
View File

@ -25,13 +25,23 @@ subject to the following restrictions:
#include <float.h>
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
#define BT_BULLET_VERSION 288
#define BT_BULLET_VERSION 289
inline int btGetVersion()
{
return BT_BULLET_VERSION;
}
inline int btIsDoublePrecision()
{
#ifdef BT_USE_DOUBLE_PRECISION
return true;
#else
return false;
#endif
}
// The following macro "BT_NOT_EMPTY_FILE" can be put into a file
// in order suppress the MS Visual C++ Linker warning 4221
//
@ -63,7 +73,12 @@ inline int btGetVersion()
#endif
#ifdef _WIN32
#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
#if defined(__GNUC__) // it should handle both MINGW and CYGWIN
#define SIMD_FORCE_INLINE __inline__ __attribute__((always_inline))
#define ATTRIBUTE_ALIGNED16(a) a __attribute__((aligned(16)))
#define ATTRIBUTE_ALIGNED64(a) a __attribute__((aligned(64)))
#define ATTRIBUTE_ALIGNED128(a) a __attribute__((aligned(128)))
#elif ( defined(_MSC_VER) && _MSC_VER < 1300 )
#define SIMD_FORCE_INLINE inline
#define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED64(a) a
@ -95,11 +110,16 @@ inline int btGetVersion()
#if defined (_M_ARM)
//Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however)
#elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
#ifdef __clang__
#define __BT_DISABLE_SSE__
#endif
#ifndef __BT_DISABLE_SSE__
#if _MSC_VER>1400
#define BT_USE_SIMD_VECTOR3
#endif
#define BT_USE_SSE
#endif//__BT_DISABLE_SSE__
#ifdef BT_USE_SSE
#if (_MSC_FULL_VER >= 170050727)//Visual Studio 2012 can compile SSE4/FMA3 (but SSE4/FMA3 is not enabled by default)

1198
thirdparty/bullet/LinearMath/btSerializer.cpp vendored
View File

File diff suppressed because it is too large Load Diff

2
thirdparty/bullet/LinearMath/btSerializer.h vendored
View File

@ -481,7 +481,7 @@ public:
buffer[9] = '2';
buffer[10] = '8';
buffer[11] = '8';
buffer[11] = '9';
}
virtual void startSerialization()

1197
thirdparty/bullet/LinearMath/btSerializer64.cpp vendored
View File

File diff suppressed because it is too large Load Diff