Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #2373 from xhan0619/DeformableImprovement

Browse Source 
Deformable improvement
This commit is contained in:
erwincoumans
2019-08-24 18:09:47 -07:00
committed by GitHub
parent ceee3e075b 908cf69f06
commit 25cc1fa386
56 changed files with 68011 additions and 1125 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h
View File

@@ -27,13 +27,19 @@ 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)
{
}
SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; }
SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; }

2
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
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;

8
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -338,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();

4
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
View File

@@ -57,9 +57,11 @@ public:
};
void buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback);
void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld);
void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback);
bool getSplitIslands()
{
return m_splitIslands;

10
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -1707,9 +1707,9 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
{
//reset to current pos
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j)
{
pJointPos[i] = m_links[i].m_jointPos[i];
pJointPos[j] = m_links[i].m_jointPos[j];
}
btVector3 jointVel;
@@ -1725,9 +1725,9 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
}
case btMultibodyLink::ePlanar:
{
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
{
pJointPos[i] = m_links[i].m_jointPos[i];
pJointPos[j] = m_links[i].m_jointPos[j];
}
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
@@ -2142,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++)
@@ -2170,6 +2171,7 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setInterpolationWorldTransform(tr);
}
}
}

5
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -273,6 +273,11 @@ public:
{
return &m_realBuf[0];
}
const btScalar *getDeltaVelocityVector() const
{
return &m_deltaV[0];
}
/* btScalar * getVelocityVector()
{
return &real_buf[0];

317
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -169,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;
@@ -441,56 +229,53 @@ void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& sol
{
/// 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();
}
{
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)
@@ -588,15 +373,10 @@ void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverIn
{
if (!bod->isUsingRK4Integration())
{
const btScalar linearDamp = bod->getLinearDamping();
// const btScalar angularDamp = bod->getAngularDamping();
bod->setLinearDamping(0);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
bod->setLinearDamping(linearDamp);
// bod->setAngularDamping(angularDamp);
}
else
{
@@ -1087,3 +867,8 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer)
}
}
}
//
//void btMultiBodyDynamicsWorld::setSplitIslands(bool split)
//{
// m_islandManager->setSplitIslands(split);
//}

3
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
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
@@ -57,6 +58,7 @@ 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);
@@ -118,6 +120,5 @@ public:
virtual void solveExternalForces(btContactSolverInfo& solverInfo);
virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
void buildIslands();
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

235
src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h Normal file
View File

@@ -0,0 +1,235 @@
/*
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<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++)
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");
}
}
}
}
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_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};
#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */

6
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -157,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),
@@ -179,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);
@@ -195,7 +197,7 @@ struct btMultibodyLink
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
btQuaternion& cachedRot = m_cachedRotParentToThis;
btVector3& cachedVector =m_cachedRVector;
btVector3& cachedVector = m_cachedRVector;
switch (m_jointType)
{
case eRevolute:
@@ -239,6 +241,8 @@ struct btMultibodyLink
btAssert(0);
}
}
m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis;
m_cachedRVector_interpolate = m_cachedRVector;
}
void updateInterpolationCacheMultiDof()

8
src/BulletSoftBody/CMakeLists.txt
View File

@@ -16,11 +16,12 @@ SET(BulletSoftBody_SRCS
btSoftMultiBodyDynamicsWorld.cpp
btSoftSoftCollisionAlgorithm.cpp
btDefaultSoftBodySolver.cpp
btDeformableMultiBodyConstraintSolver.cpp
btDeformableBackwardEulerObjective.cpp
btDeformableBodySolver.cpp
btDeformableContactProjection.cpp
btDeformableRigidDynamicsWorld.cpp
btDeformableMultiBodyDynamicsWorld.cpp
)
@@ -43,13 +44,16 @@ SET(BulletSoftBody_HDRS
btConjugateGradient.h
btDeformableGravityForce.h
btDeformableMassSpringForce.h
btDeformableCorotatedForce.h
btDeformableNeoHookeanForce.h
btDeformableLagrangianForce.h
btPreconditioner.h
btDeformableBackwardEulerObjective.h
btDeformableBodySolver.h
btDeformableMultiBodyConstraintSolver.h
btDeformableContactProjection.h
btDeformableRigidDynamicsWorld.h
btDeformableMultiBodyDynamicsWorld.h
btSoftBodySolverVertexBuffer.h
)

87
src/BulletSoftBody/btCGProjection.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -18,43 +20,34 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
class btDeformableRigidDynamicsWorld;
//class btDeformableMultiBodyDynamicsWorld;
struct DeformableContactConstraint
{
const btSoftBody::Node* m_node;
btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
btAlignedObjectArray<btVector3> m_direction;
btAlignedObjectArray<btScalar> m_value;
// the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
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)
DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node)
{
append(rcontact);
}
DeformableContactConstraint(const btVector3& dir)
DeformableContactConstraint(): m_node(NULL)
{
m_contact.push_back(NULL);
m_direction.push_back(dir);
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
}
DeformableContactConstraint()
{
m_contact.push_back(NULL);
m_direction.push_back(btVector3(0,0,0));
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
}
void append(const btSoftBody::RContact& rcontact)
{
m_contact.push_back(&rcontact);
m_direction.push_back(rcontact.m_cti.m_normal);
m_value.push_back(0);
m_accumulated_normal_impulse.push_back(0);
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);
}
~DeformableContactConstraint()
@@ -62,49 +55,6 @@ struct DeformableContactConstraint
}
};
struct DeformableFrictionConstraint
{
btAlignedObjectArray<bool> m_static; // whether the friction is static
btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels
btAlignedObjectArray<btScalar> m_dv; // the dv magnitude of the node
btAlignedObjectArray<btVector3> m_direction; // the direction of the friction for the node
btAlignedObjectArray<bool> m_static_prev;
btAlignedObjectArray<btScalar> m_impulse_prev;
btAlignedObjectArray<btScalar> m_dv_prev;
btAlignedObjectArray<btVector3> m_direction_prev;
btAlignedObjectArray<bool> m_released; // whether the contact is released
// the total impulse the node applied to the rb in the tangential direction in the cg solve
btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
DeformableFrictionConstraint()
{
append();
}
void append()
{
m_static.push_back(false);
m_static_prev.push_back(false);
m_direction_prev.push_back(btVector3(0,0,0));
m_direction.push_back(btVector3(0,0,0));
m_impulse.push_back(0);
m_impulse_prev.push_back(0);
m_dv.push_back(0);
m_dv_prev.push_back(0);
m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
m_released.push_back(false);
}
};
class btCGProjection
{
public:
@@ -112,8 +62,6 @@ public:
typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
btDeformableRigidDynamicsWorld* m_world;
// const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
const btScalar& m_dt;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
@@ -132,16 +80,11 @@ public:
virtual void setConstraints() = 0;
// update the constraints
virtual void update() = 0;
virtual btScalar update() = 0;
virtual void reinitialize(bool nodeUpdated)
{
}
virtual void setWorld(btDeformableRigidDynamicsWorld* world)
{
m_world = world;
}
};

78
src/BulletSoftBody/btConjugateGradient.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -33,63 +35,57 @@ public:
virtual ~btConjugateGradient(){}
// return the number of iterations taken
int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance, 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);
A.enforceConstraint(x);
btScalar r_norm = std::sqrt(squaredNorm(r));
if (r_norm < tolerance) {
std::cout << "Iteration = 0" << std::endl;
std::cout << "Two norm of the residual = " << r_norm << std::endl;
return 0;
}
// z = M^(-1) * r
A.precondition(r, z);
p = z;
// temp = A*p
A.multiply(p, temp);
btScalar r_dot_z = dot(z,r), r_dot_z_new;
// alpha = r^T * z / (p^T * A * p)
btScalar alpha = r_dot_z / dot(p, temp), beta;
for (int k = 1; k < max_iterations; k++) {
// x += alpha * p;
// r -= alpha * temp;
multAndAddTo(alpha, p, x);
multAndAddTo(-alpha, temp, r);
// zero out the dofs of r
A.project(r);
A.enforceConstraint(x);
r_norm = std::sqrt(squaredNorm(r));
if (r_norm < tolerance) {
std::cout << "ConjugateGradient iterations " << k << std::endl;
return k;
A.project(z);
btScalar r_dot_z = dot(z,r);
if (r_dot_z < tolerance) {
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);
// 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_new = dot(r,z);
beta = r_dot_z_new/ r_dot_z;
r_dot_z = r_dot_z_new;
// p = z + beta * p;
r_dot_z_new = dot(r,z);
if (r_dot_z_new < tolerance) {
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);
// temp = A * p;
A.multiply(p, temp);
// alpha = r^T * z / (p^T * A * p)
alpha = r_dot_z / dot(p, temp);
}
std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
if (verbose)
{
std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
}
return max_iterations;
}

4
src/BulletSoftBody/btDefaultSoftBodySolver.cpp
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
src/BulletSoftBody/btDefaultSoftBodySolver.h
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);

24
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -22,10 +24,18 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
m_preconditioner = new DefaultPreconditioner();
}
btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
{
delete m_preconditioner;
}
void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
{
BT_PROFILE("reinitialize");
setDt(dt);
if (dt > 0)
{
setDt(dt);
}
if(nodeUpdated)
{
updateId();
@@ -117,6 +127,11 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual
void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
m_softBodies[i]->updateDeformation();
}
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledExplicitForce(m_dt, force);
@@ -141,7 +156,10 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
//set constraints as projections
void btDeformableBackwardEulerObjective::setConstraints()
{
// build islands for multibody solve
m_world->btMultiBodyDynamicsWorld::buildIslands();
projection.setConstraints();
}
void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
{
projection.applyDynamicFriction(r);
}

22
src/BulletSoftBody/btDeformableBackwardEulerObjective.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -17,18 +19,18 @@
#include "btDeformableLagrangianForce.h"
#include "btDeformableMassSpringForce.h"
#include "btDeformableGravityForce.h"
#include "btDeformableCorotatedForce.h"
#include "btDeformableNeoHookeanForce.h"
#include "btDeformableContactProjection.h"
#include "btPreconditioner.h"
#include "btDeformableRigidDynamicsWorld.h"
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "LinearMath/btQuickprof.h"
class btDeformableRigidDynamicsWorld;
class btDeformableBackwardEulerObjective
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_dt;
btDeformableRigidDynamicsWorld* m_world;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
Preconditioner* m_preconditioner;
@@ -37,7 +39,7 @@ public:
btAlignedObjectArray<btSoftBody::Node* > m_nodes;
btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
virtual ~btDeformableBackwardEulerObjective() {}
virtual ~btDeformableBackwardEulerObjective();
void initialize(){}
@@ -72,9 +74,10 @@ public:
{
BT_PROFILE("enforceConstraint");
projection.enforceConstraint(x);
updateVelocity(x);
}
void applyDynamicFriction(TVStack& r);
// add dv to velocity
void updateVelocity(const TVStack& dv);
@@ -85,7 +88,6 @@ public:
void project(TVStack& r)
{
BT_PROFILE("project");
projection.update();
projection.project(r);
}
@@ -94,13 +96,7 @@ public:
{
m_preconditioner->operator()(x,b);
}
virtual void setWorld(btDeformableRigidDynamicsWorld* world)
{
m_world = world;
projection.setWorld(world);
}
virtual void updateId()
{
size_t id = 0;

33
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -18,7 +20,7 @@
btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0)
, m_cg(10)
, m_cg(50)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
}
@@ -28,24 +30,19 @@ btDeformableBodySolver::~btDeformableBodySolver()
delete m_objective;
}
void btDeformableBodySolver::solveConstraints(float solverdt)
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
BT_PROFILE("solveConstraints");
// add constraints to the solver
setConstraints();
// save v_{n+1}^* velocity after explicit forces
backupVelocity();
m_objective->computeResidual(solverdt, m_residual);
m_objective->applyDynamicFriction(m_residual);
computeStep(m_dv, m_residual);
updateVelocity();
}
void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
{
btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * m_objective->computeNorm(residual);
btScalar tolerance = std::numeric_limits<float>::epsilon() * 16 * m_objective->computeNorm(residual);
m_cg.solve(*m_objective, dv, residual, tolerance);
}
@@ -77,11 +74,16 @@ void btDeformableBodySolver::setConstraints()
m_objective->setConstraints();
}
void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
btScalar btDeformableBodySolver::solveContactConstraints()
{
m_objective->setWorld(world);
BT_PROFILE("setConstraint");
btScalar maxSquaredResidual = m_objective->projection.update();
m_objective->enforceConstraint(m_dv);
m_objective->updateVelocity(m_dv);
return maxSquaredResidual;
}
void btDeformableBodySolver::updateVelocity()
{
int counter = 0;
@@ -90,6 +92,11 @@ void btDeformableBodySolver::updateVelocity()
btSoftBody* psb = m_softBodySet[i];
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];
++counter;
}
@@ -136,7 +143,7 @@ bool btDeformableBodySolver::updateNodes()
}
void btDeformableBodySolver::predictMotion(float solverdt)
void btDeformableBodySolver::predictMotion(btScalar solverdt)
{
for (int i = 0; i < m_softBodySet.size(); ++i)
{

20
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -17,13 +19,13 @@
#include "btSoftBodySolvers.h"
#include "btDeformableBackwardEulerObjective.h"
#include "btDeformableRigidDynamicsWorld.h"
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
struct btCollisionObjectWrapper;
class btDeformableBackwardEulerObjective;
class btDeformableRigidDynamicsWorld;
class btDeformableMultiBodyDynamicsWorld;
class btDeformableBodySolver : public btSoftBodySolver
{
@@ -37,6 +39,7 @@ protected:
btAlignedObjectArray<btVector3> m_backupVelocity;
btScalar m_dt;
btScalar m_contact_iterations;
btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
@@ -55,10 +58,12 @@ public:
virtual void updateSoftBodies();
virtual void copyBackToSoftBodies(bool bMove = true) {}
void extracted(float solverdt);
virtual void solveConstraints(float solverdt);
virtual void solveDeformableConstraints(btScalar solverdt);
btScalar solveContactConstraints();
virtual void solveConstraints(btScalar dt){}
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
@@ -74,7 +79,7 @@ public:
void computeStep(TVStack& dv, const TVStack& residual);
virtual void predictMotion(float solverdt);
virtual void predictMotion(btScalar solverdt);
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
@@ -87,8 +92,9 @@ public:
softBody->defaultCollisionHandler(otherSoftBody);
}
virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
virtual bool checkInitialized(){return true;}
virtual void setWorld(btDeformableRigidDynamicsWorld* world);
};
#endif /* btDeformableBodySolver_h */

504
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -12,214 +14,150 @@
*/
#include "btDeformableContactProjection.h"
#include "btDeformableRigidDynamicsWorld.h"
#include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm>
#include <cmath>
static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
btMultiBodyJacobianData& jacobianData,
const btVector3& contact_point,
const btVector3& dir)
btScalar btDeformableContactProjection::update()
{
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;
}
void btDeformableContactProjection::update()
{
///solve rigid body constraints
m_world->getSolverInfo().m_numIterations = 1;
m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
btScalar residualSquare = 0;
// loop through constraints to set constrained values
for (int index = 0; index < m_constraints.size(); ++index)
{
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
const btSoftBody::Node* node = constraint.m_node;
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
DeformableContactConstraint& constraint = constraints[i];
DeformableFrictionConstraint& friction = frictions[i];
for (int j = 0; j < constraint.m_contact.size(); ++j)
if (constraint.m_contact[j] == NULL)
{
if (constraint.m_contact[j] == NULL)
{
// nothing needs to be done for dirichelet constraints
continue;
}
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
// nothing needs to be done for dirichelet constraints
continue;
}
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
const btScalar* deltaV_normal;
if (cti.m_colObj->hasContactResponse())
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
const btScalar* deltaV_normal;
// 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(c->m_c1)) * m_dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : 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 = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_n[k];
}
va = cti.m_normal * vel * m_dt;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t1[k];
}
va += c->t1 * vel * m_dt;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t2[k];
}
va += c->t2 * vel * m_dt;
vel += (local_v[k]+local_dv[k]) * J_n[k];
}
}
const btVector3 vb = c->m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
btVector3 impulse = c->m_c0 * vr;
const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
const btVector3 impulse_tangent = impulse - impulse_normal;
// start friction handling
// copy old data
friction.m_impulse_prev[j] = friction.m_impulse[j];
friction.m_dv_prev[j] = friction.m_dv[j];
friction.m_static_prev[j] = friction.m_static[j];
// get the current tangent direction
btScalar local_tangent_norm = impulse_tangent.norm();
btVector3 local_tangent_dir = btVector3(0,0,0);
if (local_tangent_norm > SIMD_EPSILON)
local_tangent_dir = impulse_tangent.normalized();
// accumulated impulse on the rb in this and all prev cg iterations
constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j];
// the total tangential impulse required to stop sliding
btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent;
btScalar tangent_norm = tangent.norm();
if (accumulated_normal < 0)
{
friction.m_direction[j] = -local_tangent_dir;
// do not allow switching from static friction to dynamic friction
// it causes cg to explode
if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
va = cti.m_normal * vel * m_dt;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
friction.m_static[j] = false;
friction.m_impulse[j] = -accumulated_normal*c->m_c3;
vel += (local_v[k]+local_dv[k]) * J_t1[k];
}
va += c->t1 * vel * m_dt;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t2[k];
}
va += c->t2 * vel * m_dt;
}
}
const btVector3 vb = c->m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
btVector3 impulse = c->m_c0 * vr;
const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
btVector3 old_total_tangent_dv = constraint.m_total_tangent_dv[j];
constraint.m_total_normal_dv[j] -= impulse_normal * node->m_im;
constraint.m_total_tangent_dv[j] -= impulse_tangent * node->m_im;
if (constraint.m_total_normal_dv[j].dot(cti.m_normal) < 0)
{
// separating in the normal direction
constraint.m_static[j] = false;
constraint.m_can_be_dynamic[j] = false;
constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
{
if (constraint.m_can_be_dynamic[j] && constraint.m_total_normal_dv[j].norm() * c->m_c3 < constraint.m_total_tangent_dv[j].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.
constraint.m_static[j] = false;
constraint.m_can_be_dynamic[j] = true;
if (constraint.m_total_tangent_dv[j].norm() < SIMD_EPSILON)
{
constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
}
else
{
friction.m_static[j] = true;
friction.m_impulse[j] = tangent_norm;
constraint.m_total_tangent_dv[j] = constraint.m_total_tangent_dv[j].normalized() * constraint.m_total_normal_dv[j].norm() * c->m_c3;
}
impulse_tangent = -btScalar(1)/node->m_im * (constraint.m_total_tangent_dv[j] - old_total_tangent_dv);
}
else
{
friction.m_released[j] = true;
friction.m_static[j] = false;
friction.m_impulse[j] = 0;
friction.m_direction[j] = btVector3(0,0,0);
// static friction
constraint.m_static[j] = true;
constraint.m_can_be_dynamic[j] = false;
}
friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt;
friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j];
// the incremental impulse applied to rb in the tangential direction
btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
// dv = new_impulse + accumulated velocity change in previous CG iterations
// so we have the invariant node->m_v = backupVelocity + dv;
btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
// the following is equivalent
/*
btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)];
btScalar dvn = dv.dot(cti.m_normal);
*/
constraint.m_value[j] = dvn;
// the incremental impulse:
// in the normal direction it's the normal component of "impulse"
// in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
impulse = impulse_normal + incremental_tangent;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
}
impulse = impulse_normal + impulse_tangent;
// dn is the normal component of velocity diffrerence. Approximates the residual.
residualSquare = btMax(residualSquare, dn*dn);
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
{
if (rigidCol)
rigidCol->applyImpulse(impulse, c->m_c1);
rigidCol->applyImpulse(impulse, c->m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
if (multibodyLinkCol)
{
if (multibodyLinkCol)
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
if (impulse_tangent.norm() > SIMD_EPSILON)
{
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
if (incremental_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2));
}
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(c->t2));
}
}
}
}
}
}
return residualSquare;
}
void btDeformableContactProjection::setConstraints()
@@ -233,19 +171,7 @@ void btDeformableContactProjection::setConstraints()
{
if (psb->m_nodes[j].m_im == 0)
{
btAlignedObjectArray<DeformableContactConstraint> c;
c.reserve(3);
c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
m_constraints.insert(psb->m_nodes[j].index, c);
btAlignedObjectArray<DeformableFrictionConstraint> f;
f.reserve(3);
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
m_frictions.insert(psb->m_nodes[j].index, f);
m_constraints.insert(psb->m_nodes[j].index, DeformableContactConstraint());
}
}
}
@@ -300,41 +226,12 @@ void btDeformableContactProjection::setConstraints()
if (m_constraints.find(c.m_node->index) == NULL)
{
btAlignedObjectArray<DeformableContactConstraint> constraints;
constraints.push_back(DeformableContactConstraint(c));
m_constraints.insert(c.m_node->index,constraints);
btAlignedObjectArray<DeformableFrictionConstraint> frictions;
frictions.push_back(DeformableFrictionConstraint());
m_frictions.insert(c.m_node->index,frictions);
m_constraints.insert(c.m_node->index, DeformableContactConstraint(c));
}
else
{
// group colinear constraints into one
const btScalar angle_epsilon = 0.015192247; // less than 10 degree
bool merged = false;
btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints[c.m_node->index];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[c.m_node->index];
for (int j = 0; j < constraints.size(); ++j)
{
const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
btScalar dot_prod = dirs[0].dot(cti.m_normal);
if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
{
// group the constraints
constraints[j].append(c);
// push in an empty friction
frictions[j].append();
merged = true;
break;
}
}
const int dim = 3;
// hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim)
{
constraints.push_back(DeformableContactConstraint(c));
frictions.push_back(DeformableFrictionConstraint());
}
DeformableContactConstraint& constraints = *m_constraints[c.m_node->index];
constraints.append(c);
}
}
}
@@ -344,62 +241,15 @@ void btDeformableContactProjection::setConstraints()
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
const int dim = 3;
for (int index = 0; index < m_constraints.size(); ++index)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
x[i].setZero();
for (int j = 0; j < constraints.m_total_normal_dv.size(); ++j)
{
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
for (int j = 0; j < constraints[0].m_direction.size(); ++j)
x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
}
else if (constraints.size() == 2)
{
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
else
{
x[i].setZero();
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
// apply friction if the node is not constrained in all directions
if (constraints.size() < 3)
{
for (int f = 0; f < frictions.size(); ++f)
{
const DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// add the friction constraint
if (friction.m_static[j] == true)
{
x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
x[i] += friction.m_direction[j] * friction.m_dv[j];
}
}
}
x[i] += constraints.m_total_normal_dv[j];
x[i] += constraints.m_total_tangent_dv[j];
}
}
}
@@ -409,59 +259,80 @@ void btDeformableContactProjection::project(TVStack& x)
const int dim = 3;
for (int index = 0; index < m_constraints.size(); ++index)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
if (constraints.m_contact[0] == NULL)
{
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
// static node
x[i].setZero();
continue;
}
else if (constraints.size() == 2)
bool has_static = false;
for (int j = 0; j < constraints.m_static.size(); ++j)
{
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
has_static = has_static || constraints.m_static[j];
}
// static friction => fully constrained
if (has_static)
{
x[i].setZero();
}
else if (constraints.m_total_normal_dv.size() >= dim)
{
x[i].setZero();
}
else if (constraints.m_total_normal_dv.size() == 2)
{
btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
btVector3 dir1 = (constraints.m_total_normal_dv[1].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[1].normalized() : btVector3(0,0,0);
btVector3 free_dir = btCross(dir0, dir1);
if (free_dir.norm() < 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
x[i].setZero();
// apply friction if the node is not constrained in all directions
if (constraints.size() < 3)
{
bool has_static_constraint = false;
for (int f = 0; f < frictions.size(); ++f)
btAssert(constraints.m_total_normal_dv.size() == 1);
btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
x[i] -= x[i].dot(dir0) * dir0;
}
}
}
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{
for (int index = 0; index < m_constraints.size(); ++index)
{
const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
const btSoftBody::Node* node = constraint.m_node;
if (node == NULL)
continue;
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
bool has_static_constraint = false;
// apply dynamic friction force (scaled by dt) if the node does not have static friction constraint
for (int j = 0; j < constraint.m_static.size(); ++j)
{
if (constraint.m_static[j])
{
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
has_static_constraint = true;
break;
}
for (int f = 0; f < frictions.size(); ++f)
}
for (int j = 0; j < constraint.m_total_tangent_dv.size(); ++j)
{
btVector3 friction_force = constraint.m_total_tangent_dv[j] * (1./node->m_im);
if (!has_static_constraint)
{
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
}
// only add to the rhs if there is no static friction constraint on the node
if (friction.m_static[j] == false)
{
if (!has_static_constraint)
x[i] += friction.m_direction[j] * friction.m_impulse[j];
}
else
{
// otherwise clear the constraint in the friction direction
x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
}
}
f[i] += friction_force;
}
}
}
@@ -471,7 +342,6 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
btCGProjection::reinitialize(nodeUpdated);
m_constraints.clear();
m_frictions.clear();
}

9
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -22,8 +24,7 @@ class btDeformableContactProjection : public btCGProjection
{
public:
// map from node index to constraints
btHashMap<btHashInt, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
btHashMap<btHashInt, btAlignedObjectArray<DeformableFrictionConstraint> >m_frictions;
btHashMap<btHashInt, DeformableContactConstraint> m_constraints;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: btCGProjection(softBodies, dt)
@@ -36,12 +37,14 @@ public:
// apply the constraints to the rhs
virtual void project(TVStack& x);
// add to friction
virtual void applyDynamicFriction(TVStack& f);
// apply constraints to x in Ax=b
virtual void enforceConstraint(TVStack& x);
// update the constraints
virtual void update();
virtual btScalar update();
virtual void setConstraints();

120
src/BulletSoftBody/btDeformableCorotatedForce.h Normal file
View File

@@ -0,0 +1,120 @@
/*
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 PolarDecompose(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 addScaledImplicitForce(btScalar scale, TVStack& 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() * (m_lambda * (J-1));
if (m_mu > SIMD_EPSILON)
{
btMatrix3x3 R,S;
if (J < 1024 * SIMD_EPSILON)
R.setIdentity();
else
PolarDecompose(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 addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_COROTATED_FORCE;
}
};
#endif /* btCorotated_h */

2
src/BulletSoftBody/btDeformableGravityForce.h
View File

@@ -1,4 +1,6 @@
/*
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.

7
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -20,13 +22,14 @@
enum btDeformableLagrangianForceType
{
BT_GRAVITY_FORCE = 1,
BT_MASSSPRING_FORCE = 2
BT_MASSSPRING_FORCE = 2,
BT_COROTATED_FORCE = 3,
BT_NEOHOOKEAN_FORCE = 4
};
class btDeformableLagrangianForce
{
public:
// using TVStack = btAlignedObjectArray<btVector3>;
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *> m_softBodies;
const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;

37
src/BulletSoftBody/btDeformableMassSpringForce.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -18,10 +20,14 @@
class btDeformableMassSpringForce : public btDeformableLagrangianForce
{
bool m_momentum_conserving;
btScalar m_elasticStiffness, m_dampingStiffness;
public:
// using TVStack = btDeformableLagrangianForce::TVStack;
typedef btAlignedObjectArray<btVector3> TVStack;
btDeformableMassSpringForce()
btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
{
}
btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d)
{
}
@@ -52,8 +58,15 @@ public:
// damping force
btVector3 v_diff = (node2->m_v - node1->m_v);
btScalar k_damp = psb->m_dampingCoefficient;
btVector3 scaled_force = scale * v_diff * k_damp;
btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
if (m_momentum_conserving)
{
if ((node2->m_q - node1->m_q).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;
}
@@ -72,7 +85,6 @@ public:
const btSoftBody::Link& link = psb->m_links[j];
btSoftBody::Node* node1 = link.m_n[0];
btSoftBody::Node* node2 = link.m_n[1];
btScalar kLST = link.Feature::m_material->m_kLST;
btScalar r = link.m_rl;
size_t id1 = node1->index;
size_t id2 = node2->index;
@@ -80,8 +92,8 @@ public:
// elastic force
// explicit elastic force
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = dir.normalized();
btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r);
btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
force[id1] += scaled_force;
force[id2] -= scaled_force;
}
@@ -94,7 +106,7 @@ public:
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
btScalar scaled_k_damp = psb->m_dampingCoefficient * scale;
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];
@@ -102,7 +114,16 @@ public:
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_q - node1->m_q).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;
}

53
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp Normal file
View File

@@ -0,0 +1,53 @@
/*
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::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,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++)
{
m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
solverBodyWriteBack(infoGlobal);
m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints());
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;
}

107
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h Normal file
View File

@@ -0,0 +1,107 @@
/*
Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H
#include "btDeformableBodySolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
class btDeformableBodySolver;
ATTRIBUTE_ALIGNED16(class)
btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
{
btDeformableBodySolver* m_deformableSolver;
protected:
// override the iterations method to include deformable/multibody contact
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
void 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 writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
{
btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
m_tmpSolverContactConstraintPool,
m_tmpSolverNonContactConstraintPool,
m_tmpSolverContactFrictionConstraintPool,
m_tmpSolverContactRollingFrictionConstraintPool,
m_orderTmpConstraintPool,
m_orderNonContactConstraintPool,
m_orderFrictionConstraintPool,
m_tmpConstraintSizesPool,
m_resolveSingleConstraintRowGeneric,
m_resolveSingleConstraintRowLowerLimit,
m_resolveSplitPenetrationImpulse,
m_kinematicBodyUniqueIdToSolverBodyTable,
m_btSeed2,
m_fixedBodyId,
m_maxOverrideNumSolverIterations);
for (int i = 0; i < numBodies; i++)
{
int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body && body->getInvMass())
{
btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId];
solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
}
}
}
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
void setDeformableSolver(btDeformableBodySolver* deformableSolver)
{
m_deformableSolver = deformableSolver;
}
virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, 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
solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
// inherited from MultiBodyConstraintSolver
solveGroupCacheFriendlyFinish(bodies, numBodies, info);
m_tmpMultiBodyConstraints = 0;
m_tmpNumMultiBodyConstraints = 0;
}
};
#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

426
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp Normal file
View File

@@ -0,0 +1,426 @@
/*
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:
1. 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 collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
3. 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}.
4. Contact constraints are solved as 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.
5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
6. Apply position correction to prevent numerical drift.
The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
*/
#include <stdio.h>
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "btDeformableBodySolver.h"
#include "LinearMath/btQuickprof.h"
void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
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();
btMultiBodyDynamicsWorld::calculateSimulationIslands();
beforeSolverCallbacks(timeStep);
///solve deformable bodies constraints
solveConstraints(timeStep);
afterSolverCallbacks(timeStep);
integrateTransforms(timeStep);
///update vehicle simulation
btMultiBodyDynamicsWorld::updateActions(timeStep);
btMultiBodyDynamicsWorld::updateActivationState(timeStep);
// End solver-wise simulation step
// ///////////////////////////////
}
void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
{
// perform position correction for all constraints
BT_PROFILE("positionCorrection");
for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
{
DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
const btSoftBody::RContact* c = constraint.m_contact[j];
// skip anchor points
if (c == NULL || c->m_node->m_im == 0)
continue;
const btSoftBody::sCti& cti = c->m_cti;
btVector3 va(0, 0, 0);
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_n[k];
}
va = cti.m_normal * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t1[k];
}
va += c->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t2[k];
}
va += c->t2 * vel;
}
}
else
{
// The object interacting with deformable node is not supported for position correction
btAssert(false);
}
if (cti.m_colObj->hasContactResponse())
{
btScalar dp = cti.m_offset;
// only perform position correction when penetrating
if (dp < 0)
{
c->m_node->m_v -= dp * cti.m_normal / timeStep;
}
}
}
}
}
void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
m_deformableBodySolver->backupVelocity();
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_q + timeStep * node.m_v;
}
}
m_deformableBodySolver->revertVelocity();
}
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();
solveMultiBodyRelatedConstraints();
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_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
// build islands
m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
// write the constraint information of each island to the callback, and also setup the constraints in solver
}
}
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::solveMultiBodyRelatedConstraints()
{
// process constraints on each island
m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
// process deferred
m_solverMultiBodyIslandCallback->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);
}
}
}
}
}
}
// todo : may not be necessary
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->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::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
clearForces();
clearMultiBodyForces();
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" );
}
}
}
}
}
}
clearForces();
clearMultiBodyForces();
}
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));
}

36
src/BulletSoftBody/btDeformableRigidDynamicsWorld.h → src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -18,15 +20,20 @@
#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;
class btDeformableMultiBodyConstraintSolver;
typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
{
typedef btAlignedObjectArray<btVector3> TVStack;
// using TVStack = btAlignedObjectArray<btVector3>;
@@ -38,10 +45,10 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
bool m_drawFaceTree;
bool m_drawClusterTree;
btSoftBodyWorldInfo m_sbi;
bool m_ownsSolver;
btScalar m_internalTime;
int m_contact_iterations;
typedef void (*btSolverCallback)(btScalar time, btDeformableRigidDynamicsWorld* world);
typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
btSolverCallback m_solverCallback;
protected:
@@ -49,13 +56,13 @@ protected:
virtual void integrateTransforms(btScalar timeStep);
void positionCorrection(btScalar dt);
void positionCorrection(btScalar timeStep);
void solveDeformableBodiesConstraints(btScalar timeStep);
void solveConstraints(btScalar timeStep);
public:
btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
: btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
{
m_drawFlags = fDrawFlags::Std;
@@ -65,6 +72,7 @@ public:
m_sbi.m_broadphase = pairCache;
m_sbi.m_dispatcher = dispatcher;
m_sbi.m_sparsesdf.Initialize();
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025);
m_sbi.m_sparsesdf.Reset();
m_sbi.air_density = (btScalar)1.2;
@@ -72,8 +80,6 @@ public:
m_sbi.water_offset = 0;
m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0);
m_sbi.m_sparsesdf.Initialize();
m_internalTime = 0.0;
}
@@ -82,7 +88,7 @@ public:
m_solverCallback = cb;
}
virtual ~btDeformableRigidDynamicsWorld()
virtual ~btDeformableMultiBodyDynamicsWorld()
{
}
@@ -135,8 +141,18 @@ public:
void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
void removeSoftBody(btSoftBody* body);
int getDrawFlags() const { return (m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags = f; }
void setupConstraints();
void solveMultiBodyConstraints();
void solveMultiBodyRelatedConstraints();
void sortConstraints();
};
#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H

136
src/BulletSoftBody/btDeformableNeoHookeanForce.h Normal file
View File

@@ -0,0 +1,136 @@
/*
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"
class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btScalar m_mu, m_lambda;
btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
{
}
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
{
}
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
{
addScaledDampingForce(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 C = F.transpose()*F;
btScalar J = F.determinant();
btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint() * (m_lambda * (J - 1) - 0.75 * m_mu);
}
virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
}
void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& G, btMatrix3x3& P)
{
btScalar J = F.determinant();
addScaledCofactorMatrixDifferential(F, G, m_lambda*(J-1), P);
P += F.adjoint() * m_lambda * DotProduct(F.adjoint(), G);
}
btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
{
btScalar ans = 0;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
ans += A[i][j] * B[i][j];
}
}
return ans;
}
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 */

266
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -1,266 +0,0 @@
/*
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:
1. 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 collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
3. 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}.
4. Contact constraints are solved as 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.
5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
6. Apply position correction to prevent numerical drift.
The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
*/
#include <stdio.h>
#include "btDeformableRigidDynamicsWorld.h"
#include "btDeformableBodySolver.h"
#include "LinearMath/btQuickprof.h"
void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
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();
btMultiBodyDynamicsWorld::calculateSimulationIslands();
beforeSolverCallbacks(timeStep);
///solve deformable bodies constraints
solveDeformableBodiesConstraints(timeStep);
afterSolverCallbacks(timeStep);
integrateTransforms(timeStep);
///update vehicle simulation
btMultiBodyDynamicsWorld::updateActions(timeStep);
btMultiBodyDynamicsWorld::updateActivationState(timeStep);
// End solver-wise simulation step
// ///////////////////////////////
}
void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
{
// perform position correction for all constraints
BT_PROFILE("positionCorrection");
for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
{
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)];
btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
DeformableContactConstraint& constraint = constraints[i];
DeformableFrictionConstraint& friction = frictions[i];
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
const btSoftBody::RContact* c = constraint.m_contact[j];
// skip anchor points
if (c == NULL || c->m_node->m_im == 0)
continue;
const btSoftBody::sCti& cti = c->m_cti;
btVector3 va(0, 0, 0);
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_n[k];
}
va = cti.m_normal * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t1[k];
}
va += c->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t2[k];
}
va += c->t2 * vel;
}
}
else
{
// The object interacting with deformable node is not supported for position correction
btAssert(false);
}
if (cti.m_colObj->hasContactResponse())
{
btScalar dp = cti.m_offset;
// only perform position correction when penetrating
if (dp < 0)
{
if (friction.m_static[j] == true)
{
c->m_node->m_v = va;
}
c->m_node->m_v -= dp * cti.m_normal / dt;
}
}
}
}
}
}
void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
{
BT_PROFILE("integrateTransforms");
m_deformableBodySolver->backupVelocity();
positionCorrection(dt);
btMultiBodyDynamicsWorld::integrateTransforms(dt);
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_x = node.m_q + dt * node.m_v;
}
}
m_deformableBodySolver->revertVelocity();
}
void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep)
{
m_deformableBodySolver->solveConstraints(timeStep);
}
void btDeformableRigidDynamicsWorld::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 btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
BT_PROFILE("predictUnconstraintMotion");
btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
m_deformableBodySolver->predictMotion(timeStep);
}
void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep)
{
m_internalTime += timeStep;
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 btDeformableRigidDynamicsWorld::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
clearForces();
clearMultiBodyForces();
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
btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo());
clearForces();
clearMultiBodyForces();
}
void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
{
if (0 != m_internalTickCallback)
{
(*m_internalTickCallback)(this, timeStep);
}
if (0 != m_solverCallback)
{
(*m_solverCallback)(m_internalTime, this);
}
}
void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
{
if (0 != m_solverCallback)
{
(*m_solverCallback)(m_internalTime, this);
}
}
void btDeformableRigidDynamicsWorld::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);
}
}

7
src/BulletSoftBody/btPreconditioner.h
View File

@@ -1,4 +1,6 @@
/*
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.
@@ -18,9 +20,9 @@ class Preconditioner
{
public:
typedef btAlignedObjectArray<btVector3> TVStack;
// using TVStack = btAlignedObjectArray<btVector3>;
virtual void operator()(const TVStack& x, TVStack& b) = 0;
virtual void reinitialize(bool nodeUpdated) = 0;
virtual ~Preconditioner(){}
};
class DefaultPreconditioner : public Preconditioner
@@ -34,8 +36,9 @@ public:
}
virtual void reinitialize(bool nodeUpdated)
{
}
virtual ~DefaultPreconditioner(){}
};
class MassPreconditioner : public Preconditioner

41
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -20,7 +20,6 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
//
btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: m_softBodySolver(0), m_worldInfo(worldInfo)
@@ -854,6 +853,7 @@ void btSoftBody::scale(const btVector3& scl)
updateNormals();
updateBounds();
updateConstants();
initializeDmInverse();
}
//
@@ -2300,8 +2300,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
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
const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
btTransform wtr = (predict) ?
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
: colObjWrap->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(x),
@@ -2811,6 +2812,40 @@ void btSoftBody::setSpringStiffness(btScalar 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_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 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)
{
// updateDeformation is called before predictMotion where m_q is sync'd.
// So m_x is the current position of the node.
btSoftBody::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 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;
}
}
//
void btSoftBody::Joint::Prepare(btScalar dt, int)
{

5
src/BulletSoftBody/btSoftBody.h
View File

@@ -292,6 +292,9 @@ 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;
};
/* RContact */
struct RContact
@@ -1023,6 +1026,8 @@ public:
void applyClusters(bool drift);
void dampClusters();
void setSpringStiffness(btScalar k);
void initializeDmInverse();
void updateDeformation();
void applyForces();
static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);

107
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@@ -16,6 +16,9 @@ subject to the following restrictions:
#include "btSoftBodyInternals.h"
#include <stdio.h>
#include <string>
#include <iostream>
#include <sstream>
#include <string.h>
#include "btSoftBodyHelpers.h"
#include "LinearMath/btConvexHull.h"
@@ -721,7 +724,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 +745,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 +1231,102 @@ if(face&&face[0])
}
}
}
psb->initializeDmInverse();
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);
}
}
psb->initializeDmInverse();
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;
}

9
src/BulletSoftBody/btSoftBodyHelpers.h
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,6 +142,9 @@ struct btSoftBodyHelpers
bool bfacelinks,
bool btetralinks,
bool bfacesfromtetras);
static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
/// 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

4
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -998,7 +998,7 @@ struct btSoftColliders
if (!n.m_battach)
{
// check for collision at x_{n+1}^*
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true))
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true))
{
const btScalar ima = n.m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
@@ -1006,7 +1006,7 @@ struct btSoftColliders
if (ms > 0)
{
// resolve contact at x_n
psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false);
psb->checkDeformableContact(m_colObj1Wrap, n.m_q, 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();

4
src/BulletSoftBody/btSoftBodySolvers.h
View File

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

10
src/BulletSoftBody/btSparseSDF.h
View File

@@ -70,6 +70,7 @@ struct btSparseSdf
btAlignedObjectArray<Cell*> cells;
btScalar voxelsz;
btScalar m_defaultVoxelsz;
int puid;
int ncells;
int m_clampCells;
@@ -87,9 +88,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 +111,7 @@ struct btSparseSdf
pc = pn;
}
}
voxelsz = 0.25;
voxelsz = m_defaultVoxelsz;
puid = 0;
ncells = 0;
nprobes = 1;

2
src/BulletSoftBody/premake4.lua
View File

@@ -11,4 +11,4 @@
files {
"**.cpp",
"**.h"
}
}