diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 82f343104..7724a8ec6 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -234,10 +234,10 @@ void btDeformableBodySolver::setConstraints() m_objective->setConstraints(); } -btScalar btDeformableBodySolver::solveContactConstraints() +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies) { BT_PROFILE("solveContactConstraints"); - btScalar maxSquaredResidual = m_objective->m_projection.update(); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies); return maxSquaredResidual; } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 64c7dcfbe..f78a8f696 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -65,7 +65,7 @@ public: virtual void solveDeformableConstraints(btScalar solverdt); // solve the contact between deformable and rigid as well as among deformables - btScalar solveContactConstraints(); + btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies); // solve the position error between deformable and rigid as well as among deformables; btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 5986e2881..5a4f3241b 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,527 +17,492 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include #include -btScalar btDeformableContactProjection::update() +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies) { - btScalar residualSquare = 0; - - // node constraints - for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) - { - btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); - for (int i = 0; i < constraints.size(); ++i) - { - btScalar localResidualSquare = constraints[i].solveConstraint(); - residualSquare = btMax(residualSquare, localResidualSquare); - } - } - - // anchor constraints - for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) - { - btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); - btScalar localResidualSquare = constraint.solveConstraint(); - residualSquare = btMax(residualSquare, localResidualSquare); - } - - // face constraints - for (int index = 0; index < m_allFaceConstraints.size(); ++index) - { - btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; - btScalar localResidualSquare = constraint->solveConstraint(); - residualSquare = btMax(residualSquare, localResidualSquare); - } - - return residualSquare; + btScalar residualSquare = 0; + for (int i = 0; i < numDeformableBodies; ++i) + { + for (int j = 0; j < m_softBodies.size(); ++j) + { + 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.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k) + { + btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + for (int k = 0; k < m_deformableConstraints[j].size(); ++k) + { + btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k]; + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + } + } + return residualSquare; } void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) { - // node constraints - for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) - { - btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); - for (int i = 0; i < constraints.size(); ++i) - { - constraints[i].setPenetrationScale(infoGlobal.m_deformable_erp); - } - } - - // face constraints - for (int index = 0; index < m_allFaceConstraints.size(); ++index) - { - btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; - constraint->setPenetrationScale(infoGlobal.m_deformable_erp); - } + for (int i = 0; i < m_softBodies.size(); ++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) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; + constraint.setPenetrationScale(infoGlobal.m_deformable_erp); + } + } } btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal) { - btScalar residualSquare = 0; - // node constraints - for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) - { - btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); - for (int i = 0; i < constraints.size(); ++i) - { - btScalar localResidualSquare = constraints[i].solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - } - - // anchor constraints - for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) - { - btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); - btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - - // face constraints - for (int index = 0; index < m_allFaceConstraints.size(); ++index) - { - btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; - btScalar localResidualSquare = constraint->solveSplitImpulse(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - return residualSquare; + btScalar residualSquare = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + // node constraints + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + // anchor constraints + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + // face constraints + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + + } + return residualSquare; } void btDeformableContactProjection::setConstraints() { - BT_PROFILE("setConstraints"); - // set Dirichlet constraint - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (psb->m_nodes[j].m_im == 0) - { - btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); - m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint); - } - } - } - - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } + BT_PROFILE("setConstraints"); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } - // set up deformable anchors - for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) - { - btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; - // skip fixed points - if (anchor.m_node->m_im == 0) - { - continue; - } - - if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL) - { - anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; - btDeformableNodeAnchorConstraint constraint(anchor); - m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint); - } - } - - // set Deformable Node vs. Rigid constraint - for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) - { - const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; - // skip fixed points - if (contact.m_node->m_im == 0) - { - continue; - } - btDeformableNodeRigidContactConstraint constraint(contact); - 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) - { - if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL) - { - btAlignedObjectArray constraintsList; - constraintsList.push_back(constraint); - m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList); - } - else - { - btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[contact.m_node->index]; - constraintsList.push_back(constraint); - } - } - } - - // set Deformable Face vs. Rigid constraint - for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) - { - const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j]; - // skip fixed faces - if (contact.m_c2 == 0) - { - continue; - } - btDeformableFaceRigidContactConstraint* constraint = new btDeformableFaceRigidContactConstraint(contact); - 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_allFaceConstraints.push_back(constraint); - // add face constraints to each of the nodes - for (int k = 0; k < 3; ++k) - { - btSoftBody::Node* node = contact.m_face->m_n[k]; - // static node does not need to own face/rigid constraint - if (node->m_im != 0) - { - if (m_faceRigidConstraints.find(node->index) == NULL) - { - btAlignedObjectArray constraintsList; - constraintsList.push_back(constraint); - m_faceRigidConstraints.insert(node->index, constraintsList); - } - else - { - btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[node->index]; - constraintsList.push_back(constraint); - } - } - } - } - else - { - delete constraint; - } - } - - // set Deformable Face vs. Deformable Node constraint - for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) - { - const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; - - btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact); - btVector3 va = constraint->getVa(); - btVector3 vb = constraint->getVb(); - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, contact.m_normal); - if (dn > -SIMD_EPSILON) - { - btSoftBody::Node* node = contact.m_node; - btSoftBody::Face* face = contact.m_face; - m_allFaceConstraints.push_back(constraint); - if (node->m_im != 0) - { - if (m_deformableConstraints.find(node->index) == NULL) - { - btAlignedObjectArray constraintsList; - constraintsList.push_back(constraint); - m_deformableConstraints.insert(node->index, constraintsList); - } - else - { - btAlignedObjectArray& constraintsList = *m_deformableConstraints[node->index]; - constraintsList.push_back(constraint); - } - } - - // add face constraints to each of the nodes - for (int k = 0; k < 3; ++k) - { - btSoftBody::Node* node = face->m_n[k]; - // static node does not need to own face/rigid constraint - if (node->m_im != 0) - { - if (m_deformableConstraints.find(node->index) == NULL) - { - btAlignedObjectArray constraintsList; - constraintsList.push_back(constraint); - m_deformableConstraints.insert(node->index, constraintsList); - } - else - { - btAlignedObjectArray& constraintsList = *m_deformableConstraints[node->index]; - constraintsList.push_back(constraint); - } - } - } - } - else - { - delete constraint; - } - } - } + // set Dirichlet constraint + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im == 0) + { + btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); + m_staticConstraints[i].push_back(static_constraint); + } + } + + // set up deformable anchors + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; + // skip fixed points + if (anchor.m_node->m_im == 0) + { + continue; + } + anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; + btDeformableNodeAnchorConstraint constraint(anchor); + m_nodeAnchorConstraints[i].push_back(constraint); + } + + // set Deformable Node vs. Rigid constraint + for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) + { + const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; + // skip fixed points + if (contact.m_node->m_im == 0) + { + continue; + } + btDeformableNodeRigidContactConstraint constraint(contact); + 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); + } + } + + // set Deformable Face vs. Rigid constraint + for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) + { + const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j]; + // skip fixed faces + if (contact.m_c2 == 0) + { + continue; + } + btDeformableFaceRigidContactConstraint constraint(contact); + 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); + } + } + + // set Deformable Face vs. Deformable Node constraint + for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) + { + const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; + + btDeformableFaceNodeContactConstraint constraint(contact); + btVector3 va = constraint.getVa(); + btVector3 vb = constraint.getVb(); + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, contact.m_normal); + if (dn > -SIMD_EPSILON) + { + m_deformableConstraints[i].push_back(constraint); + } + } + } } void btDeformableContactProjection::project(TVStack& x) { - const int dim = 3; - for (int index = 0; index < m_projectionsDict.size(); ++index) - { - btAlignedObjectArray& 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.norm() < 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& 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; + } + } } void btDeformableContactProjection::setProjection() { - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - int index = psb->m_nodes[j].index; - bool hasConstraint = false; - bool existStaticConstraint = false; - btVector3 averagedNormal(0,0,0); - btAlignedObjectArray normals; - if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL) - { - existStaticConstraint = true; - hasConstraint = true; - } - - // accumulate normals from Deformable Node vs. Rigid constraints - if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL) - { - hasConstraint = true; - btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[index]; - for (int k = 0; k < constraintsList.size(); ++k) - { - if (constraintsList[k].m_static) - { - existStaticConstraint = true; - break; - } - const btVector3& local_normal = constraintsList[k].m_normal; - normals.push_back(local_normal); - averagedNormal += local_normal; - } - } - - // accumulate normals from Deformable Face vs. Rigid constraints - if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL) - { - hasConstraint = true; - btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[index]; - for (int k = 0; k < constraintsList.size(); ++k) - { - if (constraintsList[k]->m_static) - { - existStaticConstraint = true; - break; - } - const btVector3& local_normal = constraintsList[k]->m_normal; - normals.push_back(local_normal); - averagedNormal += local_normal; - } - } - - // accumulate normals from Deformable Node vs. Deformable Face constraints - if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL) - { - hasConstraint = true; - btAlignedObjectArray& constraintsList = *m_deformableConstraints[index]; - for (int k = 0; k < constraintsList.size(); ++k) - { - if (constraintsList[k]->m_static) - { - existStaticConstraint = true; - break; - } - const btVector3& local_normal = constraintsList[k]->m_normal; - normals.push_back(local_normal); - averagedNormal += local_normal; - } - } - - - // build projections - if (!hasConstraint) - { - continue; - } - btAlignedObjectArray projections; - if (existStaticConstraint) - { - projections.push_back(btVector3(1,0,0)); - projections.push_back(btVector3(0,1,0)); - projections.push_back(btVector3(0,0,1)); - } - else - { - bool averageExists = (averagedNormal.length2() > SIMD_EPSILON); - averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0); - if (averageExists) - { - projections.push_back(averagedNormal); - } - for (int k = 0; k < normals.size(); ++k) - { - const btVector3& local_normal = normals[k]; - // add another projection direction if it deviates from the average by more than about 15 degrees - if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25) - { - projections.push_back(local_normal); - } - } - } - m_projectionsDict.insert(index, projections); - } - } + btAlignedObjectArray 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; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& 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; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& 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; + if (m_nodeRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& 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; + for (int k = 0; k < 3; ++k) + { + const 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& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + } + } + } + } + for (int j = 0; j < m_deformableConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face; + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + int index = node->index; + if (m_deformableConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_deformableConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_deformableConstraints[i][j].m_normal); + } + } + } + + const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node; + int index = node->index; + if (m_deformableConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_deformableConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_deformableConstraints[i][j].m_normal); + } + } + } + } } void btDeformableContactProjection::applyDynamicFriction(TVStack& f) { - // loop over constraints - for (int i = 0; i < f.size(); ++i) - { - if (m_projectionsDict.find(i) != NULL) - { - // doesn't need to add friction force for fully constrained vertices - btAlignedObjectArray& projectionDirs = *m_projectionsDict[i]; - if (projectionDirs.size() >= 3) - { - continue; - } - } - - // add friction contribution from Face vs. Node - if (m_nodeRigidConstraints.find(i) != NULL) - { - btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[i]; - for (int j = 0; j < constraintsList.size(); ++j) - { - const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j]; - btSoftBody::Node* node = constraint.getContact()->m_node; - - // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. - f[i] += constraint.getDv(node)* (1./node->m_im); - } - } - - // add friction contribution from Face vs. Rigid - if (m_faceRigidConstraints.find(i) != NULL) - { - btAlignedObjectArray& constraintsList = *m_faceRigidConstraints[i]; - for (int j = 0; j < constraintsList.size(); ++j) - { - const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j]; - btSoftBody::Face* face = constraint->getContact()->m_face; - - // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. - for (int k = 0; k < 3; ++k) - { - if (face->m_n[k]->index == i) - { - if (face->m_n[k]->m_im != 0) - { - f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im); - } - break; - } - } - } - } - - if (m_deformableConstraints.find(i) != NULL) - { - btAlignedObjectArray& constraintsList = *m_deformableConstraints[i]; - for (int j = 0; j < constraintsList.size(); ++j) - { - const btDeformableFaceNodeContactConstraint* constraint = constraintsList[j]; - btSoftBody::Face* face = constraint->getContact()->m_face; - btSoftBody::Node* node = constraint->getContact()->m_node; - - // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway. - if (node->index == i) - { - if (node->m_im != 0) - { - f[i] += constraint->getDv(node)*(1./node->m_im); - } - } - else - { - for (int k = 0; k < 3; ++k) - { - if (face->m_n[k]->index == i) - { - if (face->m_n[k]->m_im != 0) - { - f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im); - } - break; - } - } - } - } - } - } + for (int i = 0; i < m_softBodies.size(); ++i) + { + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; + const btSoftBody::Node* node = constraint.m_node; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node)* (1./node->m_im); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; + const btSoftBody::Face* face = constraint.getContact()->m_face; + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node)* (1./node->m_im); + } + } + } + for (int j = 0; j < m_deformableConstraints[i].size(); ++j) + { + const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j]; + const btSoftBody::Face* face = constraint.getContact()->m_face; + const btSoftBody::Node* node = constraint.getContact()->m_node; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node)* (1./node->m_im); + } + for (int k = 0; k < 3; ++k) + { + const btSoftBody::Node* node = face->m_n[k]; + if (node->m_im != 0) + { + int index = node->index; + f[index] += constraint.getDv(node)* (1./node->m_im); + } + } + } + } } void btDeformableContactProjection::reinitialize(bool nodeUpdated) { - m_staticConstraints.clear(); - m_nodeAnchorConstraints.clear(); - m_nodeRigidConstraints.clear(); - m_faceRigidConstraints.clear(); - m_deformableConstraints.clear(); - m_projectionsDict.clear(); - for (int i = 0; i < m_allFaceConstraints.size(); ++i) - { - delete m_allFaceConstraints[i]; - } - m_allFaceConstraints.clear(); + int N = m_softBodies.size(); + if (nodeUpdated) + { + m_staticConstraints.resize(N); + m_nodeAnchorConstraints.resize(N); + m_nodeRigidConstraints.resize(N); + m_faceRigidConstraints.resize(N); + m_deformableConstraints.resize(N); + + } + for (int i = 0 ; i < N; ++i) + { + m_staticConstraints[i].clear(); + m_nodeAnchorConstraints[i].clear(); + m_nodeRigidConstraints[i].clear(); + m_faceRigidConstraints[i].clear(); + m_deformableConstraints[i].clear(); + } + m_projectionsDict.clear(); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 406eeba3c..3c4490765 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -28,22 +28,33 @@ public: typedef btAlignedObjectArray TVStack; btAlignedObjectArray& m_softBodies; - // map from node index to static constraint - btHashMap m_staticConstraints; - // map from node index to node rigid constraint - btHashMap > m_nodeRigidConstraints; - // map from node index to face rigid constraint - btHashMap > m_faceRigidConstraints; - // map from node index to deformable constraint - btHashMap > m_deformableConstraints; - // map from node index to node anchor constraint - btHashMap m_nodeAnchorConstraints; - +// // map from node index to static constraint +// btHashMap m_staticConstraints; +// // map from node index to node rigid constraint +// btHashMap > m_nodeRigidConstraints; +// // map from node index to face rigid constraint +// btHashMap > m_faceRigidConstraints; +// // map from node index to deformable constraint +// btHashMap > m_deformableConstraints; +// // map from node index to node anchor constraint +// btHashMap m_nodeAnchorConstraints; + // all constraints involving face btAlignedObjectArray m_allFaceConstraints; // map from node index to projection directions btHashMap > m_projectionsDict; + + // map from node index to static constraint + btAlignedObjectArray > m_staticConstraints; + // map from node index to node rigid constraint + btAlignedObjectArray > m_nodeRigidConstraints; + // map from node index to face rigid constraint + btAlignedObjectArray > m_faceRigidConstraints; + // map from node index to deformable constraint + btAlignedObjectArray > m_deformableConstraints; + // map from node index to node anchor constraint + btAlignedObjectArray > m_nodeAnchorConstraints; btDeformableContactProjection(btAlignedObjectArray& softBodies) : m_softBodies(softBodies) @@ -61,7 +72,7 @@ public: virtual void applyDynamicFriction(TVStack& f); // update and solve the constraints - virtual btScalar update(); + virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies); // solve the position error using split impulse virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index d2c810d0b..06f95d69f 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -22,32 +22,8 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b { ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); -// -// int maxMotorIterations = 150; -// for (int iteration = 0; iteration < maxMotorIterations; ++iteration) -// { -// btScalar motorResidual = 0; -// for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) -// { -// int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; -// -// btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; -// -// btScalar residual = resolveSingleConstraintRowGeneric(constraint); -// motorResidual = btMax(motorResidual, residual * residual); -// if (constraint.m_multiBodyA) -// constraint.m_multiBodyA->setPosUpdated(false); -// if (constraint.m_multiBodyB) -// constraint.m_multiBodyB->setPosUpdated(false); -// } -// if (motorResidual < infoGlobal.m_leastSquaresResidualThreshold) -// { -// break; -// } -// } - + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - maxIterations = 500; for (int iteration = 0; iteration < maxIterations; iteration++) { // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body @@ -56,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); // solver body velocity -> rigid body velocity solverBodyWriteBack(infoGlobal); - btScalar deformableResidual = m_deformableSolver->solveContactConstraints(); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies); // update rigid body velocity in rigid/deformable contact m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); // solver body velocity <- rigid body velocity