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

refactor contact solve

Browse Source
This commit is contained in:
Xuchen Han
2019-08-23 13:41:14 -07:00
parent ccd8c3a47c
commit 6beeac7065
18 changed files with 597 additions and 309 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/DeformableDemo/DeformableMultibody.cpp
View File

@@ -118,8 +118,9 @@ void DeformableMultibody::initPhysics()
m_broadphase = new btDbvtBroadphase(); m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
btMultiBodyConstraintSolver* sol; btDeformableMultiBodyConstraintSolver* sol;
sol = new btMultiBodyConstraintSolver; sol = new btDeformableMultiBodyConstraintSolver;
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol; m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
@@ -223,6 +224,7 @@ void DeformableMultibody::initPhysics()
psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = .1; psb->m_cfg.kDF = .1;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->setCollisionFlags(0);
getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false); btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false);

13
examples/DeformableDemo/DeformableRigid.cpp
View File

@@ -161,7 +161,8 @@ void DeformableRigid::initPhysics()
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol; m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
@@ -208,10 +209,12 @@ void DeformableRigid::initPhysics()
{ {
bool onGround = false; bool onGround = false;
const btScalar s = 4; const btScalar s = 4;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), const btScalar h = 0;
btVector3(+s, 0, -s),
btVector3(-s, 0, +s), btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
btVector3(+s, 0, +s), btVector3(+s, h, -s),
btVector3(-s, h, +s),
btVector3(+s, h, +s),
// 3,3, // 3,3,
20,20, 20,20,
1 + 2 + 4 + 8, true); 1 + 2 + 4 + 8, true);

3
examples/DeformableDemo/GraspDeformable.cpp
View File

@@ -187,7 +187,8 @@ void GraspDeformable::initPhysics()
m_broadphase = new btDbvtBroadphase(); m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol; m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);

3
examples/DeformableDemo/Pinch.cpp
View File

@@ -245,7 +245,8 @@ void Pinch::initPhysics()
m_broadphase = new btDbvtBroadphase(); m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol; m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);

3
examples/DeformableDemo/VolumetricDeformable.cpp
View File

@@ -178,7 +178,8 @@ void VolumetricDeformable::initPhysics()
m_broadphase = new btDbvtBroadphase(); m_broadphase = new btDbvtBroadphase();
btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
sol->setDeformableSolver(deformableBodySolver);
m_solver = sol; m_solver = sol;
m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);

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

@@ -169,218 +169,6 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
btDiscreteDynamicsWorld::updateActivationState(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 void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
{ {
islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData; islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
@@ -442,18 +230,47 @@ void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& sol
/// solve all the constraints for this island /// solve all the constraints for this island
m_solverMultiBodyIslandCallback->processConstraints(); m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer); m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
calculateJointForce(solverInfo); {
processDeltaVee(); BT_PROFILE("btMultiBody stepVelocities");
} for (int i = 0; i < this->m_multiBodies.size(); i++)
{
void btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld::processConstraintsAndDeltaVee() btMultiBody* bod = m_multiBodies[i];
{
m_solverMultiBodyIslandCallback->processConstraints(); bool isSleeping = false;
processDeltaVee();
} if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
void btMultiBodyDynamicsWorld::processDeltaVee() 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++) for (int i = 0; i < this->m_multiBodies.size(); i++)
{ {
btMultiBody* bod = m_multiBodies[i]; btMultiBody* bod = m_multiBodies[i];
@@ -461,51 +278,6 @@ void btMultiBodyDynamicsWorld::processDeltaVee()
} }
} }
void btMultiBodyDynamicsWorld::calculateJointForce(btContactSolverInfo& solverInfo)
{
{
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);
}
}
}
}
}
}
}
void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo) void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
{ {
forwardKinematics(); forwardKinematics();

4
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -17,6 +17,7 @@ subject to the following restrictions:
#define BT_MULTIBODY_DYNAMICS_WORLD_H #define BT_MULTIBODY_DYNAMICS_WORLD_H
#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h"
#define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY #define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
@@ -119,8 +120,5 @@ public:
virtual void solveExternalForces(btContactSolverInfo& solverInfo); virtual void solveExternalForces(btContactSolverInfo& solverInfo);
virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
void buildIslands(); void buildIslands();
void calculateJointForce(btContactSolverInfo& solverInfo);
void processDeltaVee();
void processConstraintsAndDeltaVee();
}; };
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

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

@@ -0,0 +1,229 @@
//
// btMultiBodyInplaceSolverCallback.h
// BulletDynamics
//
// Created by Xuchen Han on 8/22/19.
//
#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 */

2
src/BulletSoftBody/CMakeLists.txt
View File

@@ -16,6 +16,7 @@ SET(BulletSoftBody_SRCS
btSoftMultiBodyDynamicsWorld.cpp btSoftMultiBodyDynamicsWorld.cpp
btSoftSoftCollisionAlgorithm.cpp btSoftSoftCollisionAlgorithm.cpp
btDefaultSoftBodySolver.cpp btDefaultSoftBodySolver.cpp
btDeformableMultiBodyConstraintSolver.cpp
btDeformableBackwardEulerObjective.cpp btDeformableBackwardEulerObjective.cpp
btDeformableBodySolver.cpp btDeformableBodySolver.cpp
@@ -50,6 +51,7 @@ SET(BulletSoftBody_HDRS
btDeformableBackwardEulerObjective.h btDeformableBackwardEulerObjective.h
btDeformableBodySolver.h btDeformableBodySolver.h
btDeformableMultiBodyConstraintSolver.h
btDeformableContactProjection.h btDeformableContactProjection.h
btDeformableMultiBodyDynamicsWorld.h btDeformableMultiBodyDynamicsWorld.h

2
src/BulletSoftBody/btCGProjection.h
View File

@@ -80,7 +80,7 @@ public:
virtual void setConstraints() = 0; virtual void setConstraints() = 0;
// update the constraints // update the constraints
virtual void update() = 0; virtual btScalar update() = 0;
virtual void reinitialize(bool nodeUpdated) virtual void reinitialize(bool nodeUpdated)
{ {

7
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -74,12 +74,13 @@ void btDeformableBodySolver::setConstraints()
m_objective->setConstraints(); m_objective->setConstraints();
} }
void btDeformableBodySolver::solveContactConstraints() btScalar btDeformableBodySolver::solveContactConstraints()
{ {
BT_PROFILE("setConstraint"); BT_PROFILE("setConstraint");
m_objective->projection.update(); btScalar maxSquaredResidual = m_objective->projection.update();
m_objective->enforceConstraint(m_dv); m_objective->enforceConstraint(m_dv);
m_objective->updateVelocity(m_dv); m_objective->updateVelocity(m_dv);
return maxSquaredResidual;
} }
@@ -142,7 +143,7 @@ bool btDeformableBodySolver::updateNodes()
} }
void btDeformableBodySolver::predictMotion(btScalar solverdt) void btDeformableBodySolver::predictMotion(float solverdt)
{ {
for (int i = 0; i < m_softBodySet.size(); ++i) for (int i = 0; i < m_softBodySet.size(); ++i)
{ {

4
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -63,7 +63,7 @@ public:
virtual void solveDeformableConstraints(btScalar solverdt); virtual void solveDeformableConstraints(btScalar solverdt);
void solveContactConstraints(); btScalar solveContactConstraints();
virtual void solveConstraints(float dt){} virtual void solveConstraints(float dt){}
@@ -81,7 +81,7 @@ public:
void computeStep(TVStack& dv, const TVStack& residual); void computeStep(TVStack& dv, const TVStack& residual);
virtual void predictMotion(btScalar solverdt); virtual void predictMotion(float solverdt);
virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}

15
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -17,10 +17,9 @@
#include "btDeformableMultiBodyDynamicsWorld.h" #include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
void btDeformableContactProjection::update() btScalar btDeformableContactProjection::update()
{ {
// m_world->getSolverInfo().m_numIterations = 1; btScalar residualSquare = 0;
// loop through constraints to set constrained values // loop through constraints to set constrained values
for (int index = 0; index < m_constraints.size(); ++index) for (int index = 0; index < m_constraints.size(); ++index)
{ {
@@ -129,21 +128,28 @@ void btDeformableContactProjection::update()
} }
} }
impulse = impulse_normal + impulse_tangent; 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 (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)
{ {
multibodyLinkCol->m_multiBody->processDeltaVeeMultiDof2(); // make sure velocity is up-to-date;
// apply normal component of the impulse // apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
if (impulse_tangent.norm() > SIMD_EPSILON) if (impulse_tangent.norm() > SIMD_EPSILON)
{ {
// apply tangential component of the impulse // apply tangential component of the impulse
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1)); multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2)); multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2));
@@ -153,6 +159,7 @@ void btDeformableContactProjection::update()
} }
} }
} }
return residualSquare;
} }
void btDeformableContactProjection::setConstraints() void btDeformableContactProjection::setConstraints()

2
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -44,7 +44,7 @@ public:
virtual void enforceConstraint(TVStack& x); virtual void enforceConstraint(TVStack& x);
// update the constraints // update the constraints
virtual void update(); virtual btScalar update();
virtual void setConstraints(); virtual void setConstraints();

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

@@ -0,0 +1,44 @@
//
// btDeformableMultiBodyConstraintSolver.cpp
// BulletSoftBody
//
// Created by Xuchen Han on 8/23/19.
//
#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;
}

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

@@ -0,0 +1,98 @@
//
// btDeformableMultiBodyConstraintSolver.h
// BulletSoftBody
//
// Created by Xuchen Han on 8/22/19.
//
#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 */

145
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -33,6 +33,7 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
#include "btDeformableMultiBodyDynamicsWorld.h" #include "btDeformableMultiBodyDynamicsWorld.h"
#include "btDeformableBodySolver.h" #include "btDeformableBodySolver.h"
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{ {
BT_PROFILE("internalSingleStepSimulation"); BT_PROFILE("internalSingleStepSimulation");
@@ -176,27 +177,108 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
// save v_{n+1}^* velocity after explicit forces // save v_{n+1}^* velocity after explicit forces
m_deformableBodySolver->backupVelocity(); m_deformableBodySolver->backupVelocity();
// setup constraints among multibodies and between multibodies and deformable bodies // set up constraints among multibodies and between multibodies and deformable bodies
setupConstraints(); setupConstraints();
solveMultiBodyConstraints(); solveMultiBodyRelatedConstraints();
m_deformableBodySolver->solveDeformableConstraints(timeStep); m_deformableBodySolver->solveDeformableConstraints(timeStep);
} }
void btDeformableMultiBodyDynamicsWorld::setupConstraints() void btDeformableMultiBodyDynamicsWorld::setupConstraints()
{ {
// set up constraints between multibody and deformable bodies
m_deformableBodySolver->setConstraints(); m_deformableBodySolver->setConstraints();
m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
// 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::solveMultiBodyConstraints() void btDeformableMultiBodyDynamicsWorld::sortConstraints()
{ {
for (int i = 0; i < m_contact_iterations; ++i) m_sortedConstraints.resize(m_constraints.size());
int i;
for (i = 0; i < getNumConstraints(); i++)
{ {
m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), (btSimulationIslandManager::IslandCallback*)m_solverMultiBodyIslandCallback); m_sortedConstraints[i] = m_constraints[i];
btMultiBodyDynamicsWorld::processConstraintsAndDeltaVee(); }
m_deformableBodySolver->solveContactConstraints(); 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();
} }
// todo @xuchenhan: add joint force feedback
} }
void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
@@ -244,8 +326,51 @@ void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep
btRigidBody* rb = m_nonStaticRigidBodies[i]; btRigidBody* rb = m_nonStaticRigidBodies[i];
rb->integrateVelocities(timeStep); rb->integrateVelocities(timeStep);
} }
// integrate multibody gravity // integrate multibody gravity
btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo()); {
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(); clearForces();
clearMultiBodyForces(); clearMultiBodyForces();
} }

16
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@@ -20,6 +20,7 @@
#include "btDeformableLagrangianForce.h" #include "btDeformableLagrangianForce.h"
#include "btDeformableMassSpringForce.h" #include "btDeformableMassSpringForce.h"
#include "btDeformableBodySolver.h" #include "btDeformableBodySolver.h"
#include "btDeformableMultiBodyConstraintSolver.h"
#include "btSoftBodyHelpers.h" #include "btSoftBodyHelpers.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include <functional> #include <functional>
@@ -28,6 +29,7 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableBodySolver; class btDeformableBodySolver;
class btDeformableLagrangianForce; class btDeformableLagrangianForce;
struct MultiBodyInplaceSolverIslandCallback; struct MultiBodyInplaceSolverIslandCallback;
class btDeformableMultiBodyConstraintSolver;
typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray; typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
@@ -59,8 +61,8 @@ protected:
void solveConstraints(btScalar timeStep); void solveConstraints(btScalar timeStep);
public: public:
btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
: btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
{ {
m_drawFlags = fDrawFlags::Std; m_drawFlags = fDrawFlags::Std;
@@ -70,7 +72,7 @@ public:
m_sbi.m_broadphase = pairCache; m_sbi.m_broadphase = pairCache;
m_sbi.m_dispatcher = dispatcher; m_sbi.m_dispatcher = dispatcher;
m_sbi.m_sparsesdf.Initialize(); m_sbi.m_sparsesdf.Initialize();
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025); m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025);
m_sbi.m_sparsesdf.Reset(); m_sbi.m_sparsesdf.Reset();
m_sbi.air_density = (btScalar)1.2; m_sbi.air_density = (btScalar)1.2;
@@ -78,10 +80,7 @@ public:
m_sbi.water_offset = 0; m_sbi.water_offset = 0;
m_sbi.water_normal = btVector3(0, 0, 0); m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0); m_sbi.m_gravity.setValue(0, -10, 0);
m_sbi.m_sparsesdf.Initialize();
m_internalTime = 0.0; m_internalTime = 0.0;
m_contact_iterations = 1;
} }
void setSolverCallback(btSolverCallback cb) void setSolverCallback(btSolverCallback cb)
@@ -148,7 +147,12 @@ public:
void setDrawFlags(int f) { m_drawFlags = f; } void setDrawFlags(int f) { m_drawFlags = f; }
void setupConstraints(); void setupConstraints();
void solveMultiBodyConstraints(); void solveMultiBodyConstraints();
void solveMultiBodyRelatedConstraints();
void sortConstraints();
}; };
#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H #endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H