diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 908785895..a1fd7a5c5 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -361,12 +361,32 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul const btSoftBody::DeformableFaceRigidContact* contact = getContact(); btVector3 dv = impulse * contact->m_c2; btSoftBody::Face* face = contact->m_face; - if (face->m_n[0]->m_im > 0) - face->m_n[0]->m_v -= dv * contact->m_weights[0]; - if (face->m_n[1]->m_im > 0) - face->m_n[1]->m_v -= dv * contact->m_weights[1]; - if (face->m_n[2]->m_im > 0) - face->m_n[2]->m_v -= dv * contact->m_weights[2]; + + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + 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]; + + // apply strain limiting to prevent undamped modes + btScalar m01 = (btScalar(1)/(im0 + im1)); + btScalar m02 = (btScalar(1)/(im0 + im2)); + btScalar m12 = (btScalar(1)/(im1 + im2)); + + btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); + btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); + btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); + + v0 += dv0; + v1 += dv1; + v2 += dv2; } /* ================ Face vs. Node =================== */ @@ -482,16 +502,33 @@ void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impuls } btSoftBody::Face* face = contact->m_face; - if (face->m_n[0]->m_im > 0) + btVector3& v0 = face->m_n[0]->m_v; + btVector3& v1 = face->m_n[1]->m_v; + btVector3& v2 = face->m_n[2]->m_v; + 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) { - face->m_n[0]->m_v -= dvb * contact->m_weights[0]; + v0 -= dvb * contact->m_weights[0]; } - if (face->m_n[1]->m_im > 0) + if (im1 > 0) { - face->m_n[1]->m_v -= dvb * contact->m_weights[1]; + v1 -= dvb * contact->m_weights[1]; } - if (face->m_n[2]->m_im > 0) + if (im2 > 0) { - face->m_n[2]->m_v -= dvb * contact->m_weights[2]; + v2 -= dvb * contact->m_weights[2]; } + // todo: Face node constraints needs more work +// btScalar m01 = (btScalar(1)/(im0 + im1)); +// btScalar m02 = (btScalar(1)/(im0 + im2)); +// btScalar m12 = (btScalar(1)/(im1 + im2)); +// +// btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); +// btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); +// btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); +// v0 += dv0; +// v1 += dv1; +// v2 += dv2; }