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

improve dynamic friction

Browse Source
This commit is contained in:
Xuchen Han
2019-08-10 11:11:18 -07:00
parent fb6612c0be
commit fa5741d07e
7 changed files with 245 additions and 378 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -70,78 +70,77 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar timeStep)
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 = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
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)
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)
{
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)
{
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)
{
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;
vel += local_v[k] * J_n[k];
}
}
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;
va = cti.m_normal * vel;
// only perform position correction when penetrating
if (dp < 0)
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 (constraint.m_static[j] == true)
{
if (friction.m_static[j] == true)
{
c->m_node->m_v = va;
}
c->m_node->m_v -= dp * cti.m_normal / timeStep;
c->m_node->m_v = va;
}
c->m_node->m_v -= dp * cti.m_normal / timeStep;
}
}
}