diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index d3899289b..c3a7051e5 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -431,7 +431,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d n.m_v *= max_v; } n.m_q = n.m_x + n.m_v * dt; - n.m_constrained = false; + n.m_penetration = 0; } /* Nodes */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 19991d6b8..0eab2d1e0 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -224,7 +224,7 @@ void btDeformableContactProjection::setProjection() for (int j = 0; j < m_staticConstraints[i].size(); ++j) { int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_constrained = true; + m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; btAlignedObjectArray indices; btAlignedObjectArray vecs1,vecs2,vecs3; indices.push_back(index); @@ -239,7 +239,7 @@ void btDeformableContactProjection::setProjection() for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) { int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; btAlignedObjectArray indices; btAlignedObjectArray vecs1,vecs2,vecs3; indices.push_back(index); @@ -253,7 +253,7 @@ void btDeformableContactProjection::setProjection() for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) { int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; btAlignedObjectArray indices; indices.push_back(index); btAlignedObjectArray vecs1,vecs2,vecs3; @@ -276,11 +276,12 @@ void btDeformableContactProjection::setProjection() { const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; + btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; if (m_faceRigidConstraints[i][j].m_static) { for (int l = 0; l < 3; ++l) { - face->m_n[l]->m_constrained = true; + face->m_n[l]->m_penetration = penetration; btReducedVector rv(dof); for (int k = 0; k < 3; ++k) { @@ -340,7 +341,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() for (int j = 0; j < m_staticConstraints[i].size(); ++j) { int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_constrained = true; + m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; LagrangeMultiplier lm; lm.m_num_nodes = 1; lm.m_indices[0] = index; @@ -354,7 +355,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) { int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; LagrangeMultiplier lm; lm.m_num_nodes = 1; lm.m_indices[0] = index; @@ -368,7 +369,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) { int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; LagrangeMultiplier lm; lm.m_num_nodes = 1; lm.m_indices[0] = index; @@ -392,11 +393,12 @@ void btDeformableContactProjection::setLagrangeMultiplier() const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; + btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; LagrangeMultiplier lm; lm.m_num_nodes = 3; for (int k = 0; k<3; ++k) { - face->m_n[k]->m_constrained = true; + face->m_n[k]->m_penetration = penetration; lm.m_indices[k] = face->m_n[k]->index; lm.m_weights[k] = bary[k]; } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 54e63d2a0..834ae880a 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -261,10 +261,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim { for (int j = 0; j < psb->m_nodes.size(); ++j) { - if (!psb->m_nodes[j].m_constrained) - { - psb->m_nodes[j].m_v *= psb->m_dampingCoefficient; - } + psb->m_nodes[j].m_v *= psb->m_dampingCoefficient; } } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 166ec68e7..11055fc24 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -268,7 +268,7 @@ public: btScalar m_im; // 1/mass btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data - bool m_constrained; // constrained node + btScalar m_penetration; // depth of penetration int m_battach : 1; // Attached int index; }; @@ -1303,20 +1303,20 @@ public: I = -btMin(repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); if (vn < 0) I += 0.5 * mass * vn; - bool face_constrained = false, node_constrained = node->m_constrained; + btScalar face_penetration = 0, node_penetration = node->m_penetration; for (int i = 0; i < 3; ++i) - face_constrained |= face->m_n[i]->m_constrained; + face_penetration = btMax(face_penetration, face->m_n[i]->m_penetration); btScalar I_tilde = .5 *I /(1.0+w.length2()); // double the impulse if node or face is constrained. - if (face_constrained || node_constrained) - I_tilde *= 2.0; - if (!face_constrained) +// if (face_penetration > 0 || node_penetration > 0) +// I_tilde *= 2.0; + if (face_penetration <= node_penetration) { for (int j = 0; j < 3; ++j) face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im; } - if (!node_constrained) + if (face_penetration >= node_penetration) { node->m_v -= I_tilde*node->m_im*n; } @@ -1332,14 +1332,14 @@ public: vt.safeNormalize(); I_tilde = .5 *I /(1.0+w.length2()); // double the impulse if node or face is constrained. - if (face_constrained || node_constrained) - I_tilde *= 2.0; - if (!face_constrained) +// if (face_penetration > 0 || node_penetration > 0) +// I_tilde *= 2.0; + if (face_penetration <= node_penetration) { for (int j = 0; j < 3; ++j) face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im; } - if (!node_constrained) + if (face_penetration >= node_penetration) { node->m_v -= I_tilde * node->m_im * vt; } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 7f5f0db29..b9ebc95b6 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1664,7 +1664,6 @@ struct btSoftColliders const btScalar ms = ima + imb; if (ms > 0) { - n.m_constrained = true; // resolve contact at x_n psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); btSoftBody::sCti& cti = c.m_cti; @@ -1745,8 +1744,6 @@ struct btSoftColliders btSoftBody::Node* n0 = f.m_n[0]; btSoftBody::Node* n1 = f.m_n[1]; btSoftBody::Node* n2 = f.m_n[2]; - if (n0->m_constrained && n1->m_constrained && n2->m_constrained) - return; const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; btSoftBody::DeformableFaceRigidContact c; btVector3 contact_point;