diff options
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp | 761 |
1 files changed, 383 insertions, 378 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp index 22ca8bf582..7f67260ce6 100644 --- a/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp +++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include <algorithm> #include <cmath> -btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; for (int i = 0; i < numDeformableBodies; ++i) @@ -58,27 +58,37 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod return residualSquare; } -void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < m_softBodies.size(); ++i) + btScalar residualSquare = 0; + for (int i = 0; i < numDeformableBodies; ++i) { - // node constraints - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); - } - // face constraints - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + for (int j = 0; j < m_softBodies.size(); ++j) { - btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); + btCollisionObject* psb = m_softBodies[j]; + if (psb != deformableBodies[i]) + { + continue; + } + for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } } } + return residualSquare; } void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) -{ +{ BT_PROFILE("setConstraints"); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -97,7 +107,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in m_staticConstraints[i].push_back(static_constraint); } } - + // set up deformable anchors for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) { @@ -111,7 +121,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); m_nodeAnchorConstraints[i].push_back(constraint); } - + // set Deformable Node vs. Rigid constraint for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { @@ -122,17 +132,9 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_nodeRigidConstraints[i].push_back(constraint); - } + m_nodeRigidConstraints[i].push_back(constraint); } - + // set Deformable Face vs. Rigid constraint for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) { @@ -143,15 +145,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_faceRigidConstraints[i].push_back(constraint); - } + m_faceRigidConstraints[i].push_back(constraint); } } } @@ -159,267 +153,269 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in void btDeformableContactProjection::project(TVStack& x) { #ifndef USE_MGS - const int dim = 3; - for (int index = 0; index < m_projectionsDict.size(); ++index) - { - btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); - size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); - if (projectionDirs.size() >= dim) - { - // static node - x[i].setZero(); - continue; - } - else if (projectionDirs.size() == 2) - { - btVector3 dir0 = projectionDirs[0]; - btVector3 dir1 = projectionDirs[1]; - btVector3 free_dir = btCross(dir0, dir1); - if (free_dir.safeNorm() < SIMD_EPSILON) - { - x[i] -= x[i].dot(dir0) * dir0; - x[i] -= x[i].dot(dir1) * dir1; - } - else - { - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; - } - } - else - { - btAssert(projectionDirs.size() == 1); - btVector3 dir0 = projectionDirs[0]; - x[i] -= x[i].dot(dir0) * dir0; - } - } + const int dim = 3; + for (int index = 0; index < m_projectionsDict.size(); ++index) + { + btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); + size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); + if (projectionDirs.size() >= dim) + { + // static node + x[i].setZero(); + continue; + } + else if (projectionDirs.size() == 2) + { + btVector3 dir0 = projectionDirs[0]; + btVector3 dir1 = projectionDirs[1]; + btVector3 free_dir = btCross(dir0, dir1); + if (free_dir.safeNorm() < SIMD_EPSILON) + { + x[i] -= x[i].dot(dir0) * dir0; + } + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + } + else + { + btAssert(projectionDirs.size() == 1); + btVector3 dir0 = projectionDirs[0]; + x[i] -= x[i].dot(dir0) * dir0; + } + } #else - btReducedVector p(x.size()); - for (int i = 0; i < m_projections.size(); ++i) - { - p += (m_projections[i].dot(x) * m_projections[i]); - } - for (int i = 0; i < p.m_indices.size(); ++i) - { - x[p.m_indices[i]] -= p.m_vecs[i]; - } + btReducedVector p(x.size()); + for (int i = 0; i < m_projections.size(); ++i) + { + p += (m_projections[i].dot(x) * m_projections[i]); + } + for (int i = 0; i < p.m_indices.size(); ++i) + { + x[p.m_indices[i]] -= p.m_vecs[i]; + } #endif } void btDeformableContactProjection::setProjection() { #ifndef USE_MGS - BT_PROFILE("btDeformableContactProjection::setProjection"); - btAlignedObjectArray<btVector3> units; - units.push_back(btVector3(1,0,0)); - units.push_back(btVector3(0,1,0)); - units.push_back(btVector3(0,0,1)); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - 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_penetration = SIMD_INFINITY; - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - 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_penetration = SIMD_INFINITY; - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - 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_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - if (m_nodeRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray<btVector3> projections; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - } - } - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; - btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; - for (int k = 0; k < 3; ++k) - { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); - } - for (int k = 0; k < 3; ++k) - { - btSoftBody::Node* node = face->m_n[k]; - node->m_penetration = true; - int index = node->index; - if (m_faceRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray<btVector3> projections; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); - } - } - } - } - } + BT_PROFILE("btDeformableContactProjection::setProjection"); + btAlignedObjectArray<btVector3> units; + units.push_back(btVector3(1, 0, 0)); + units.push_back(btVector3(0, 1, 0)); + units.push_back(btVector3(0, 0, 1)); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + 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; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + 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; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + 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; + if (m_nodeRigidConstraints[i][j].m_binding) + { + if (m_nodeRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray<btVector3> projections; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + } + } + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + if (m_faceRigidConstraints[i][j].m_binding) + { + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_constrained = true; + } + } + for (int k = 0; k < 3; ++k) + { + btSoftBody::Node* node = face->m_n[k]; + int index = node->index; + if (m_faceRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + for (int l = 0; l < 3; ++l) + { + projections.push_back(units[l]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray<btVector3> projections; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index]; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + } + } + } + } + } #else - int dof = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - dof += m_softBodies[i]->m_nodes.size(); - } - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - 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_penetration = SIMD_INFINITY; - btAlignedObjectArray<int> indices; - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - indices.push_back(index); - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - - 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_penetration = SIMD_INFINITY; - btAlignedObjectArray<int> indices; - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - indices.push_back(index); - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - 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_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - btAlignedObjectArray<int> indices; - indices.push_back(index); - btAlignedObjectArray<btVector3> vecs1,vecs2,vecs3; - if (m_nodeRigidConstraints[i][j].m_static) - { - vecs1.push_back(btVector3(1,0,0)); - vecs2.push_back(btVector3(0,1,0)); - vecs3.push_back(btVector3(0,0,1)); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - m_projections.push_back(btReducedVector(dof, indices, vecs2)); - m_projections.push_back(btReducedVector(dof, indices, vecs3)); - } - else - { - vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projections.push_back(btReducedVector(dof, indices, vecs1)); - } - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + int dof = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + dof += m_softBodies[i]->m_nodes.size(); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + 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_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + + 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_penetration = SIMD_INFINITY; + btAlignedObjectArray<int> indices; + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + 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_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; + btAlignedObjectArray<int> indices; + indices.push_back(index); + btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3; + if (m_nodeRigidConstraints[i][j].m_static) + { + vecs1.push_back(btVector3(1, 0, 0)); + vecs2.push_back(btVector3(0, 1, 0)); + vecs3.push_back(btVector3(0, 0, 1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + else + { + vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + 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; - for (int k = 0; k < 3; ++k) - { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); - } + btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + } if (m_faceRigidConstraints[i][j].m_static) { for (int l = 0; l < 3; ++l) { - btReducedVector rv(dof); for (int k = 0; k < 3; ++k) { rv.m_indices.push_back(face->m_n[k]->index); - btVector3 v(0,0,0); + btVector3 v(0, 0, 0); v[l] = bary[k]; rv.m_vecs.push_back(v); - rv.sort(); + rv.sort(); } m_projections.push_back(rv); } @@ -431,121 +427,134 @@ void btDeformableContactProjection::setProjection() { rv.m_indices.push_back(face->m_n[k]->index); rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal); - rv.sort(); + rv.sort(); } m_projections.push_back(rv); } } - } - btModifiedGramSchmidt<btReducedVector> mgs(m_projections); - mgs.solve(); - m_projections = mgs.m_out; + } + btModifiedGramSchmidt<btReducedVector> mgs(m_projections); + mgs.solve(); + m_projections = mgs.m_out; #endif } void btDeformableContactProjection::checkConstraints(const TVStack& x) { - for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) - { - btVector3 d(0,0,0); - const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; - for (int j = 0; j < lm.m_num_constraints; ++j) - { - for (int k = 0; k < lm.m_num_nodes; ++k) - { - d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); - } - } - printf("d = %f, %f, %f\n",d[0],d[1],d[2]); - } + for (int i = 0; i < m_lagrangeMultipliers.size(); ++i) + { + btVector3 d(0, 0, 0); + const LagrangeMultiplier& lm = m_lagrangeMultipliers[i]; + for (int j = 0; j < lm.m_num_constraints; ++j) + { + for (int k = 0; k < lm.m_num_nodes; ++k) + { + d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]); + } + } + // printf("d = %f, %f, %f\n", d[0], d[1], d[2]); + // printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]); + } } void btDeformableContactProjection::setLagrangeMultiplier() { - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - 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_penetration = SIMD_INFINITY; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - m_lagrangeMultipliers.push_back(lm); - } - 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_penetration = SIMD_INFINITY; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - m_lagrangeMultipliers.push_back(lm); - } - 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_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; - LagrangeMultiplier lm; - lm.m_num_nodes = 1; - lm.m_indices[0] = index; - lm.m_weights[0] = 1.0; - if (m_nodeRigidConstraints[i][j].m_static) - { - lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); - } - else - { - lm.m_num_constraints = 1; - lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; - } - m_lagrangeMultipliers.push_back(lm); - } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - 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; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + 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; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + 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; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + m_lagrangeMultipliers.push_back(lm); + } + + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + if (!m_nodeRigidConstraints[i][j].m_binding) + { + continue; + } + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + LagrangeMultiplier lm; + lm.m_num_nodes = 1; + lm.m_indices[0] = index; + lm.m_weights[0] = 1.0; + if (m_nodeRigidConstraints[i][j].m_static) + { + lm.m_num_constraints = 3; + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); + } + else + { + lm.m_num_constraints = 1; + lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; + } + m_lagrangeMultipliers.push_back(lm); + } + + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + if (!m_faceRigidConstraints[i][j].m_binding) + { + continue; + } + btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + + btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; LagrangeMultiplier lm; lm.m_num_nodes = 3; - for (int k = 0; k<3; ++k) + + for (int k = 0; k < 3; ++k) { - face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + face->m_n[k]->m_constrained = true; lm.m_indices[k] = face->m_n[k]->index; lm.m_weights[k] = bary[k]; } - if (m_faceRigidConstraints[i][j].m_static) - { + if (m_faceRigidConstraints[i][j].m_static) + { + face->m_pcontact[3] = 1; lm.m_num_constraints = 3; - lm.m_dirs[0] = btVector3(1,0,0); - lm.m_dirs[1] = btVector3(0,1,0); - lm.m_dirs[2] = btVector3(0,0,1); + lm.m_dirs[0] = btVector3(1, 0, 0); + lm.m_dirs[1] = btVector3(0, 1, 0); + lm.m_dirs[2] = btVector3(0, 0, 1); } else { + face->m_pcontact[3] = 0; lm.m_num_constraints = 1; lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal; } - m_lagrangeMultipliers.push_back(lm); + m_lagrangeMultipliers.push_back(lm); } } } @@ -562,7 +571,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) @@ -575,7 +584,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } } @@ -587,7 +596,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } for (int k = 0; k < 3; ++k) { @@ -595,7 +604,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) if (node->m_im != 0) { int index = node->index; - f[index] += constraint.getDv(node)* (1./node->m_im); + f[index] += constraint.getDv(node) * (1. / node->m_im); } } } @@ -612,9 +621,8 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_nodeRigidConstraints.resize(N); m_faceRigidConstraints.resize(N); m_deformableConstraints.resize(N); - } - for (int i = 0 ; i < N; ++i) + for (int i = 0; i < N; ++i) { m_staticConstraints[i].clear(); m_nodeAnchorConstraints[i].clear(); @@ -623,12 +631,9 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_deformableConstraints[i].clear(); } #ifndef USE_MGS - m_projectionsDict.clear(); + m_projectionsDict.clear(); #else - m_projections.clear(); + m_projections.clear(); #endif - m_lagrangeMultipliers.clear(); + m_lagrangeMultipliers.clear(); } - - - |