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

add support for anchor constraint between deformable and rigid

Browse Source
This commit is contained in:
Xuchen Han
2019-10-16 19:23:01 -07:00
parent 3d622a3bee
commit 60dfe1fe69
11 changed files with 487 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -15,6 +15,127 @@
#include "btDeformableContactConstraint.h"
/* ================ Deformable Node Anchor =================== */
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
: m_anchor(&a)
, btDeformableContactConstraint(a.m_cti.m_normal)
{
}
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
: m_anchor(other.m_anchor)
, btDeformableContactConstraint(other)
{
}
btVector3 btDeformableNodeAnchorConstraint::getVa() const
{
const btSoftBody::sCti& cti = m_anchor->m_cti;
btVector3 va(0, 0, 0);
if (cti.m_colObj->hasContactResponse())
{
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
// multibody anchor not supported yet
btAssert(false);
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_n[k];
}
va = cti.m_normal * vel;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t1[k];
}
va += m_anchor->t1 * vel;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[k]) * J_t2[k];
}
va += m_anchor->t2 * vel;
}
}
}
return va;
}
btScalar btDeformableNodeAnchorConstraint::solveConstraint()
{
const btSoftBody::sCti& cti = m_anchor->m_cti;
btVector3 va = getVa();
btVector3 vb = getVb();
btVector3 vr = (vb - va);
// + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
const btScalar dn = btDot(vr, cti.m_normal);
// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
btScalar residualSquare = dn*dn;
btVector3 impulse = m_anchor->m_c0 * vr;
// apply impulse to deformable nodes involved and change their velocities
applyImpulse(impulse);
// apply impulse to the rigid/multibodies involved and change their velocities
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btRigidBody* rigidCol = 0;
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
if (rigidCol)
{
rigidCol->applyImpulse(impulse, m_anchor->m_c1);
}
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = 0;
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
}
}
return residualSquare;
}
btVector3 btDeformableNodeAnchorConstraint::getVb() const
{
return m_anchor->m_node->m_v;
}
void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
{
btVector3 dv = impulse * m_anchor->m_c2;
m_anchor->m_node->m_v -= dv;
}
/* ================ Deformable vs. Rigid =================== */
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
: m_contact(&c)

26
src/BulletSoftBody/btDeformableContactConstraint.h
View File

@@ -108,6 +108,32 @@ public:
virtual void applyImpulse(const btVector3& impulse){}
};
//
// Anchor Constraint between rigid and deformable node
class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
{
public:
const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
btDeformableNodeAnchorConstraint(){}
btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
virtual ~btDeformableNodeAnchorConstraint()
{
}
virtual btScalar solveConstraint();
// object A is the rigid/multi body, and object B is the deformable node/face
virtual btVector3 getVa() const;
// get the velocity of the deformable node in contact
virtual btVector3 getVb() const;
virtual btVector3 getDv(const btSoftBody::Node* n) const
{
return btVector3(0,0,0);
}
virtual void applyImpulse(const btVector3& impulse);
};
//
// Constraint between rigid/multi body and deformable objects
class btDeformableRigidContactConstraint : public btDeformableContactConstraint

29
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -32,6 +32,14 @@ btScalar btDeformableContactProjection::update()
}
}
// anchor constraints
for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
{
btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
// face constraints
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
@@ -73,6 +81,24 @@ void btDeformableContactProjection::setConstraints()
continue;
}
// set up deformable anchors
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{
btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
// skip fixed points
if (anchor.m_node->m_im == 0)
{
continue;
}
if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL)
{
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
btDeformableNodeAnchorConstraint constraint(anchor);
m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint);
}
}
// set Deformable Node vs. Rigid constraint
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
{
@@ -262,7 +288,7 @@ void btDeformableContactProjection::setProjection()
bool existStaticConstraint = false;
btVector3 averagedNormal(0,0,0);
btAlignedObjectArray<btVector3> normals;
if (m_staticConstraints.find(index) != NULL)
if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL)
{
existStaticConstraint = true;
hasConstraint = true;
@@ -451,6 +477,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
m_staticConstraints.clear();
m_nodeAnchorConstraints.clear();
m_nodeRigidConstraints.clear();
m_faceRigidConstraints.clear();
m_deformableConstraints.clear();

2
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -36,6 +36,8 @@ public:
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// map from node index to deformable constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// map from node index to node anchor constraint
btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;

7
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@@ -140,6 +140,13 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
// enforce anchor constraints
for (int j = 0; j < psb->m_deformableAnchors.size();++j)
{
btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
btSoftBody::Node* n = a.m_node;
n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
}
psb->interpolateRenderMesh();
}
}

35
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -411,6 +411,41 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca
m_anchors.push_back(a);
}
//
void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
{
DeformableNodeRigidAnchor c;
btSoftBody::Node& n = m_nodes[node];
const btScalar ima = n.m_im;
const btScalar imb = body->getInvMass();
btVector3 nrm;
const btCollisionShape* shp = body->getCollisionShape();
const btTransform& wtr = body->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(m_nodes[node].m_x),
shp,
nrm,
0);
c.m_cti.m_colObj = body;
c.m_cti.m_normal = wtr.getBasis() * nrm;
c.m_cti.m_offset = dst;
c.m_node = &m_nodes[node];
const btScalar fc = m_cfg.kDF * body->getFriction();
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
m_deformableAnchors.push_back(c);
}
//
void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
{

10
src/BulletSoftBody/btSoftBody.h
View File

@@ -354,6 +354,12 @@ public:
Node* m_node; // Owner node
};
class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
{
public:
btVector3 m_local; // Anchor position in body space
};
class DeformableFaceRigidContact : public DeformableRigidContact
{
public:
@@ -774,6 +780,7 @@ public:
btAlignedObjectArray<TetraScratch> m_tetraScratches;
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
tAnchorArray m_anchors; // Anchors
btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
tRContactArray m_rcontacts; // Rigid contacts
btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
@@ -897,7 +904,8 @@ public:
Material* mat = 0);
/* Append anchor */
void appendAnchor(int node,
void appendDeformableAnchor(int node, btRigidBody* body);
void appendAnchor(int node,
btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
/* Append linear joint */