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

add option for deformable rigid split impulse

Browse Source
This commit is contained in:
Xuchen Han
2019-11-05 18:07:32 -08:00
parent fb85b2e05f
commit 13314360a8
15 changed files with 571 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -332,6 +332,48 @@ public:
}
}
}
void applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos)
{
if (m_inverseMass != btScalar(0.))
{
applyCentralPushImpulse(impulse);
if (m_angularFactor)
{
applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor));
}
}
}
btVector3 getPushVelocity()
{
return m_pushVelocity;
}
btVector3 getTurnVelocity()
{
return m_turnVelocity;
}
void setPushVelocity(const btVector3& v)
{
m_pushVelocity = v;
}
void setTurnVelocity(const btVector3& v)
{
m_turnVelocity = v;
}
void applyCentralPushImpulse(const btVector3& impulse)
{
m_pushVelocity += impulse * m_linearFactor * m_inverseMass;
}
void applyTorqueTurnImpulse(const btVector3& torque)
{
m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
}
void clearForces()
{

14
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -36,7 +36,7 @@ btDeformableBodySolver::~btDeformableBodySolver()
void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
BT_PROFILE("solveConstraints");
BT_PROFILE("solveDeformableConstraints");
if (!m_implicit)
{
m_objective->computeResidual(solverdt, m_residual);
@@ -241,6 +241,16 @@ btScalar btDeformableBodySolver::solveContactConstraints()
return maxSquaredResidual;
}
btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveSplitImpulse");
return m_objective->m_projection.solveSplitImpulse(infoGlobal);
}
void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
{
m_objective->m_projection.splitImpulseSetup(infoGlobal);
}
void btDeformableBodySolver::updateVelocity()
{
@@ -321,7 +331,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
}
else
m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter];
psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit;
++counter;
}
}

6
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -66,6 +66,12 @@ public:
// solve the contact between deformable and rigid as well as among deformables
btScalar solveContactConstraints();
// solve the position error between deformable and rigid as well as among deformables;
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
// set up the position error in split impulse
void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
// resize/clear data structures
void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);

60
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -140,11 +140,14 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS
{
m_total_normal_dv.setZero();
m_total_tangent_dv.setZero();
// penetration is non-positive. The magnitude of penetration is the depth of penetration.
m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
}
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
: m_contact(other.m_contact)
, btDeformableContactConstraint(other)
, m_penetration(other.m_penetration)
{
m_total_normal_dv = other.m_total_normal_dv;
m_total_tangent_dv = other.m_total_tangent_dv;
@@ -285,6 +288,36 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
return residualSquare;
}
btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
const btSoftBody::sCti& cti = m_contact->m_cti;
const btScalar dn = m_penetration;
if (dn != 0)
{
const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
// one iteration of the position impulse corrects all the position error at this timestep
m_penetration -= dn;
// apply impulse to deformable nodes involved and change their position
applySplitImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their position
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
// todo xuchenhan@
}
return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
}
return 0;
}
/* ================ Node vs. Rigid =================== */
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
: m_node(contact.m_node)
@@ -316,6 +349,13 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul
contact->m_node->m_v -= dv;
}
void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
contact->m_node->m_vsplit -= dv;
};
/* ================ Face vs. Rigid =================== */
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
: m_face(contact.m_face)
@@ -386,6 +426,26 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
v2 += dv2;
}
void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableFaceRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btSoftBody::Face* face = contact->m_face;
btVector3& v0 = face->m_n[0]->m_vsplit;
btVector3& v1 = face->m_n[1]->m_vsplit;
btVector3& v2 = face->m_n[2]->m_vsplit;
const btScalar& im0 = face->m_n[0]->m_im;
const btScalar& im1 = face->m_n[1]->m_im;
const btScalar& im2 = face->m_n[2]->m_im;
if (im0 > 0)
v0 -= dv * contact->m_weights[0];
if (im1 > 0)
v1 -= dv * contact->m_weights[1];
if (im2 > 0)
v2 -= dv * contact->m_weights[2];
}
/* ================ Face vs. Node =================== */
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
: m_node(contact.m_node)

47
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@@ -50,6 +50,9 @@ public:
// the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
virtual btScalar solveConstraint() = 0;
// solve the position error by applying an inelastic impulse that changes only the position (not velocity)
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
// get the velocity of the object A in the contact
virtual btVector3 getVa() const = 0;
@@ -61,6 +64,12 @@ public:
// apply impulse to the soft body node and/or face involved
virtual void applyImpulse(const btVector3& impulse) = 0;
// apply position based impulse to the soft body node and/or face involved
virtual void applySplitImpulse(const btVector3& impulse) = 0;
// scale the penetration depth by erp
virtual void setPenetrationScale(btScalar scale) = 0;
};
//
@@ -90,6 +99,11 @@ public:
return 0;
}
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
return 0;
}
virtual btVector3 getVa() const
{
return btVector3(0,0,0);
@@ -106,6 +120,8 @@ public:
}
virtual void applyImpulse(const btVector3& impulse){}
virtual void applySplitImpulse(const btVector3& impulse){}
virtual void setPenetrationScale(btScalar scale){}
};
//
@@ -122,6 +138,11 @@ public:
{
}
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
// todo xuchenhan@
return 0;
}
// object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const;
// get the velocity of the deformable node in contact
@@ -131,6 +152,11 @@ public:
return btVector3(0,0,0);
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse)
{
// todo xuchenhan@
};
virtual void setPenetrationScale(btScalar scale){}
};
@@ -141,6 +167,7 @@ class btDeformableRigidContactConstraint : public btDeformableContactConstraint
public:
btVector3 m_total_normal_dv;
btVector3 m_total_tangent_dv;
btScalar m_penetration;
const btSoftBody::DeformableRigidContact* m_contact;
btDeformableRigidContactConstraint(){}
@@ -154,6 +181,13 @@ public:
virtual btVector3 getVa() const;
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
virtual void setPenetrationScale(btScalar scale)
{
m_penetration *= scale;
}
};
//
@@ -185,6 +219,7 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
};
//
@@ -214,6 +249,7 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse);
};
//
@@ -235,6 +271,12 @@ public:
virtual btScalar solveConstraint();
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
// todo: xuchenhan@
return 0;
}
// get the velocity of the object A in the contact
virtual btVector3 getVa() const;
@@ -251,5 +293,10 @@ public:
}
virtual void applyImpulse(const btVector3& impulse);
virtual void applySplitImpulse(const btVector3& impulse)
{
// todo xuchenhan@
}
virtual void setPenetrationScale(btScalar scale){}
};
#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

51
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -51,6 +51,57 @@ btScalar btDeformableContactProjection::update()
return residualSquare;
}
void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
{
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
constraints[i].setPenetrationScale(infoGlobal.m_erp);
}
}
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
constraint->setPenetrationScale(infoGlobal.m_erp);
}
}
btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{
btScalar residualSquare = 0;
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
btScalar localResidualSquare = constraints[i].solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
// anchor constraints
for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
{
btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
btScalar localResidualSquare = constraint->solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare);
}
return residualSquare;
}
void btDeformableContactProjection::setConstraints()
{

7
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -60,9 +60,12 @@ public:
// add friction force to the rhs of the linear solve
virtual void applyDynamicFriction(TVStack& f);
// update the constraints
// update and solve the constraints
virtual btScalar update();
// solve the position error using split impulse
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
// Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
virtual void setConstraints();
@@ -70,5 +73,7 @@ public:
virtual void setProjection();
virtual void reinitialize(bool nodeUpdated);
virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal);
};
#endif /* btDeformableContactProjection_h */

37
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -104,3 +104,40 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
}
}
}
void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
int iteration;
if (infoGlobal.m_splitImpulse)
{
{
m_deformableSolver->splitImpulseSetup(infoGlobal);
for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
{
btScalar leastSquaresResidual = 0.f;
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j = 0; j < numPoolConstraints; j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
// solve the position correction between deformable and rigid/multibody
btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
#endif
break;
}
}
}
}
}

2
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h
View File

@@ -44,6 +44,8 @@ protected:
// write the velocity of the underlying rigid body to the the the solver body
void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();

25
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -118,9 +118,32 @@ void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
}
}
void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
{
// correct the position of rigid bodies with temporary velocity generated from split impulse
btContactSolverInfo infoGlobal;
btVector3 zero(0,0,0);
for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
{
btRigidBody* rb = m_nonStaticRigidBodies[i];
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btVector3 pushVelocity = rb->getPushVelocity();
btVector3 turnVelocity = rb->getTurnVelocity();
if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
{
btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
rb->setWorldTransform(newTransform);
rb->setPushVelocity(zero);
rb->setTurnVelocity(zero);
}
}
}
void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
positionCorrection(timeStep);
btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
for (int i = 0; i < m_softBodies.size(); ++i)
{
@@ -142,6 +165,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
node.m_x = node.m_x + timeStep * node.m_v;
node.m_v -= node.m_vsplit;
node.m_vsplit.setZero();
node.m_q = node.m_x;
node.m_vn = node.m_v;
}

1
src/BulletSoftBody/btSoftBody.h
View File

@@ -257,6 +257,7 @@ public:
btVector3 m_x; // Position
btVector3 m_q; // Previous step position/Test position
btVector3 m_v; // Velocity
btVector3 m_vsplit; // Temporary Velocity in addintion to velocity used in split impulse
btVector3 m_vn; // Previous step velocity
btVector3 m_f; // Force accumulator
btVector3 m_n; // Normal