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

finished refactoring; start adding face contact

Browse Source
This commit is contained in:
Xuchen Han
2019-08-30 14:16:56 -07:00
parent f813cb1c88
commit f99cf56149
16 changed files with 937 additions and 355 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -993,7 +993,7 @@ struct btSoftColliders
void DoNode(btSoftBody::Node& n) const
{
const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
btSoftBody::RContact c;
btSoftBody::DeformableNodeRigidContact c;
if (!n.m_battach)
{
@@ -1010,7 +1010,7 @@ struct btSoftColliders
btSoftBody::sCti& cti = c.m_cti;
c.m_node = &n;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
c.m_c2 = ima * psb->m_sst.sdt;
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
@@ -1021,7 +1021,7 @@ struct btSoftColliders
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
if (m_rigidBody)
m_rigidBody->activate();
@@ -1051,8 +1051,7 @@ struct btSoftColliders
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btScalar dt = psb->m_sst.sdt;
btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
@@ -1061,7 +1060,7 @@ struct btSoftColliders
c.t2 = t2;
}
}
psb->m_rcontacts.push_back(c);
psb->m_nodeRigidContacts.push_back(c);
}
}
}
@@ -1072,6 +1071,102 @@ struct btSoftColliders
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideSDF_RDF
//
struct CollideSDF_RDF : btDbvt::ICollide
{
void Process(const btDbvtNode* leaf)
{
btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
DoNode(*face);
}
void DoNode(btSoftBody::Face& f) const
{
btSoftBody::Node* n0 = f.m_n[0];
btSoftBody::Node* n1 = f.m_n[1];
btSoftBody::Node* n2 = f.m_n[2];
const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
btSoftBody::DeformableFaceRigidContact c;
btVector3 contact_point;
btVector3 bary;
if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
{
btScalar ima = n0->m_im + n1->m_im + n2->m_im;
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
const btScalar ms = ima + imb;
if (ms > 0)
{
// resolve contact at x_n
psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
btSoftBody::sCti& cti = c.m_cti;
c.m_contactPoint = contact_point;
c.m_bary = bary;
c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
c.m_face = &f;
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im;
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = contact_point - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
if (m_rigidBody)
m_rigidBody->activate();
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 normal = cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
c.jacobianData_t2 = jacobianData_t2;
c.t1 = t1;
c.t2 = t2;
}
}
psb->m_faceRigidContacts.push_back(c);
}
}
}
btSoftBody* psb;
const btCollisionObjectWrapper* m_colObj1Wrap;
btRigidBody* m_rigidBody;
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideVF_SS
//