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

solve constraints involving deformable objects according to islands

Browse Source
This commit is contained in:
Xuchen Han
2019-12-18 23:11:34 -08:00
parent f65a8b03c0
commit 89553c44e7
5 changed files with 483 additions and 531 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -234,10 +234,10 @@ void btDeformableBodySolver::setConstraints()
m_objective->setConstraints(); m_objective->setConstraints();
} }
btScalar btDeformableBodySolver::solveContactConstraints() btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
{ {
BT_PROFILE("solveContactConstraints"); BT_PROFILE("solveContactConstraints");
btScalar maxSquaredResidual = m_objective->m_projection.update(); btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
return maxSquaredResidual; return maxSquaredResidual;
} }

2
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -65,7 +65,7 @@ public:
virtual void solveDeformableConstraints(btScalar solverdt); virtual void solveDeformableConstraints(btScalar solverdt);
// solve the contact between deformable and rigid as well as among deformables // 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; // solve the position error between deformable and rigid as well as among deformables;
btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);

931
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -17,527 +17,492 @@
#include "btDeformableMultiBodyDynamicsWorld.h" #include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
btScalar btDeformableContactProjection::update() btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
{ {
btScalar residualSquare = 0; btScalar residualSquare = 0;
for (int i = 0; i < numDeformableBodies; ++i)
// node constraints {
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) for (int j = 0; j < m_softBodies.size(); ++j)
{ {
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index); btCollisionObject* psb = m_softBodies[j];
for (int i = 0; i < constraints.size(); ++i) if (psb != deformableBodies[i])
{ {
btScalar localResidualSquare = constraints[i].solveConstraint(); continue;
residualSquare = btMax(residualSquare, localResidualSquare); }
} for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
} {
btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
// anchor constraints btScalar localResidualSquare = constraint.solveConstraint();
for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) residualSquare = btMax(residualSquare, localResidualSquare);
{ }
btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
btScalar localResidualSquare = constraint.solveConstraint(); {
residualSquare = btMax(residualSquare, localResidualSquare); btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
} btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
// face constraints }
for (int index = 0; index < m_allFaceConstraints.size(); ++index) for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
{ {
btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
btScalar localResidualSquare = constraint->solveConstraint(); btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare); residualSquare = btMax(residualSquare, localResidualSquare);
} }
for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
return residualSquare; {
btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
btScalar localResidualSquare = constraint.solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
}
return residualSquare;
} }
void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
{ {
// node constraints for (int i = 0; i < m_softBodies.size(); ++i)
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) {
{ // node constraints
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index); for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
for (int i = 0; i < constraints.size(); ++i) {
{ btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
constraints[i].setPenetrationScale(infoGlobal.m_deformable_erp); constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
} }
} // face constraints
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
// face constraints {
for (int index = 0; index < m_allFaceConstraints.size(); ++index) btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
{ constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; }
constraint->setPenetrationScale(infoGlobal.m_deformable_erp); }
}
} }
btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal) btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
{ {
btScalar residualSquare = 0; btScalar residualSquare = 0;
// node constraints for (int i = 0; i < m_softBodies.size(); ++i)
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) {
{ // node constraints
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index); for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
for (int i = 0; i < constraints.size(); ++i) {
{ btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
btScalar localResidualSquare = constraints[i].solveSplitImpulse(infoGlobal); btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare); 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);
}
// anchor constraints }
for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) return residualSquare;
{
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;
} }
void btDeformableContactProjection::setConstraints() void btDeformableContactProjection::setConstraints()
{ {
BT_PROFILE("setConstraints"); BT_PROFILE("setConstraints");
// set Dirichlet constraint for (int i = 0; i < m_softBodies.size(); ++i)
for (int i = 0; i < m_softBodies.size(); ++i) {
{ btSoftBody* psb = m_softBodies[i];
btSoftBody* psb = m_softBodies[i]; if (!psb->isActive())
if (!psb->isActive()) {
{ continue;
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) // set Dirichlet constraint
{ for (int j = 0; j < psb->m_nodes.size(); ++j)
btSoftBody* psb = m_softBodies[i]; {
if (!psb->isActive()) if (psb->m_nodes[j].m_im == 0)
{ {
continue; btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
} m_staticConstraints[i].push_back(static_constraint);
}
}
// set up deformable anchors // set up deformable anchors
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{ {
btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
// skip fixed points // skip fixed points
if (anchor.m_node->m_im == 0) if (anchor.m_node->m_im == 0)
{ {
continue; continue;
} }
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
btDeformableNodeAnchorConstraint constraint(anchor);
m_nodeAnchorConstraints[i].push_back(constraint);
}
if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL) // set Deformable Node vs. Rigid constraint
{ for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; {
btDeformableNodeAnchorConstraint constraint(anchor); const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint); // 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 Node vs. Rigid constraint // set Deformable Face vs. Rigid constraint
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
{ {
const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
// skip fixed points // skip fixed faces
if (contact.m_node->m_im == 0) if (contact.m_c2 == 0)
{ {
continue; continue;
} }
btDeformableNodeRigidContactConstraint constraint(contact); btDeformableFaceRigidContactConstraint constraint(contact);
btVector3 va = constraint.getVa(); btVector3 va = constraint.getVa();
btVector3 vb = constraint.getVb(); btVector3 vb = constraint.getVb();
const btVector3 vr = vb - va; const btVector3 vr = vb - va;
const btSoftBody::sCti& cti = contact.m_cti; const btSoftBody::sCti& cti = contact.m_cti;
const btScalar dn = btDot(vr, cti.m_normal); const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON) if (dn < SIMD_EPSILON)
{ {
if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL) m_faceRigidConstraints[i].push_back(constraint);
{ }
btAlignedObjectArray<btDeformableNodeRigidContactConstraint> constraintsList; }
constraintsList.push_back(constraint);
m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[contact.m_node->index];
constraintsList.push_back(constraint);
}
}
}
// set Deformable Face vs. Rigid constraint // set Deformable Face vs. Deformable Node constraint
for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
{ {
const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j]; const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[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<btDeformableFaceRigidContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_faceRigidConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[node->index];
constraintsList.push_back(constraint);
}
}
}
}
else
{
delete constraint;
}
}
// set Deformable Face vs. Deformable Node constraint btDeformableFaceNodeContactConstraint constraint(contact);
for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) btVector3 va = constraint.getVa();
{ btVector3 vb = constraint.getVb();
const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, contact.m_normal);
btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact); if (dn > -SIMD_EPSILON)
btVector3 va = constraint->getVa(); {
btVector3 vb = constraint->getVb(); m_deformableConstraints[i].push_back(constraint);
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<btDeformableFaceNodeContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_deformableConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& 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<btDeformableFaceNodeContactConstraint*> constraintsList;
constraintsList.push_back(constraint);
m_deformableConstraints.insert(node->index, constraintsList);
}
else
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
constraintsList.push_back(constraint);
}
}
}
}
else
{
delete constraint;
}
}
}
} }
void btDeformableContactProjection::project(TVStack& x) void btDeformableContactProjection::project(TVStack& x)
{ {
const int dim = 3; const int dim = 3;
for (int index = 0; index < m_projectionsDict.size(); ++index) for (int index = 0; index < m_projectionsDict.size(); ++index)
{ {
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index); btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
if (projectionDirs.size() >= dim) if (projectionDirs.size() >= dim)
{ {
// static node // static node
x[i].setZero(); x[i].setZero();
continue; continue;
} }
else if (projectionDirs.size() == 2) else if (projectionDirs.size() == 2)
{ {
btVector3 dir0 = projectionDirs[0]; btVector3 dir0 = projectionDirs[0];
btVector3 dir1 = projectionDirs[1]; btVector3 dir1 = projectionDirs[1];
btVector3 free_dir = btCross(dir0, dir1); btVector3 free_dir = btCross(dir0, dir1);
if (free_dir.norm() < SIMD_EPSILON) if (free_dir.safeNorm() < SIMD_EPSILON)
{ {
x[i] -= x[i].dot(dir0) * dir0; x[i] -= x[i].dot(dir0) * dir0;
x[i] -= x[i].dot(dir1) * dir1; x[i] -= x[i].dot(dir1) * dir1;
} }
else else
{ {
free_dir.normalize(); free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir; x[i] = x[i].dot(free_dir) * free_dir;
} }
} }
else else
{ {
btAssert(projectionDirs.size() == 1); btAssert(projectionDirs.size() == 1);
btVector3 dir0 = projectionDirs[0]; btVector3 dir0 = projectionDirs[0];
x[i] -= x[i].dot(dir0) * dir0; x[i] -= x[i].dot(dir0) * dir0;
} }
} }
} }
void btDeformableContactProjection::setProjection() void btDeformableContactProjection::setProjection()
{ {
for (int i = 0; i < m_softBodies.size(); ++i) btAlignedObjectArray<btVector3> units;
{ units.push_back(btVector3(1,0,0));
btSoftBody* psb = m_softBodies[i]; units.push_back(btVector3(0,1,0));
if (!psb->isActive()) units.push_back(btVector3(0,0,1));
{ for (int i = 0; i < m_softBodies.size(); ++i)
continue; {
} btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) if (!psb->isActive())
{ {
int index = psb->m_nodes[j].index; continue;
bool hasConstraint = false; }
bool existStaticConstraint = false; for (int j = 0; j < m_staticConstraints[i].size(); ++j)
btVector3 averagedNormal(0,0,0); {
btAlignedObjectArray<btVector3> normals; int index = m_staticConstraints[i][j].m_node->index;
if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL) if (m_projectionsDict.find(index) == NULL)
{ {
existStaticConstraint = true; m_projectionsDict.insert(index, units);
hasConstraint = true; }
} 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;
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;
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;
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<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);
}
}
}
}
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<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_deformableConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_deformableConstraints[i][j].m_normal);
}
}
}
// accumulate normals from Deformable Node vs. Rigid constraints const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node;
if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL) int index = node->index;
{ if (m_deformableConstraints[i][j].m_static)
hasConstraint = true; {
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[index]; if (m_projectionsDict.find(index) == NULL)
for (int k = 0; k < constraintsList.size(); ++k) {
{ m_projectionsDict.insert(index, units);
if (constraintsList[k].m_static) }
{ else
existStaticConstraint = true; {
break; btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
} for (int k = 0; k < 3; ++k)
const btVector3& local_normal = constraintsList[k].m_normal; {
normals.push_back(local_normal); projections.push_back(units[k]);
averagedNormal += local_normal; }
} }
} }
else
// accumulate normals from Deformable Face vs. Rigid constraints {
if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL) if (m_projectionsDict.find(index) == NULL)
{ {
hasConstraint = true; btAlignedObjectArray<btVector3> projections;
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[index]; projections.push_back(m_deformableConstraints[i][j].m_normal);
for (int k = 0; k < constraintsList.size(); ++k) m_projectionsDict.insert(index, projections);
{ }
if (constraintsList[k]->m_static) else
{ {
existStaticConstraint = true; btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
break; projections.push_back(m_deformableConstraints[i][j].m_normal);
} }
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<btDeformableFaceNodeContactConstraint*>& 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<btVector3> 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);
}
}
} }
void btDeformableContactProjection::applyDynamicFriction(TVStack& f) void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{ {
// loop over constraints for (int i = 0; i < m_softBodies.size(); ++i)
for (int i = 0; i < f.size(); ++i) {
{ for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
if (m_projectionsDict.find(i) != NULL) {
{ const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
// doesn't need to add friction force for fully constrained vertices const btSoftBody::Node* node = constraint.m_node;
btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict[i]; if (node->m_im != 0)
if (projectionDirs.size() >= 3) {
{ int index = node->index;
continue; f[index] += constraint.getDv(node)* (1./node->m_im);
} }
} }
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
// add friction contribution from Face vs. Node {
if (m_nodeRigidConstraints.find(i) != NULL) const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
{ const btSoftBody::Face* face = constraint.getContact()->m_face;
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i]; for (int k = 0; k < 3; ++k)
for (int j = 0; j < constraintsList.size(); ++j) {
{ const btSoftBody::Node* node = face->m_n[k];
const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j]; if (node->m_im != 0)
btSoftBody::Node* node = constraint.getContact()->m_node; {
int index = node->index;
// 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[index] += constraint.getDv(node)* (1./node->m_im);
f[i] += constraint.getDv(node)* (1./node->m_im); }
} }
} }
for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
// add friction contribution from Face vs. Rigid {
if (m_faceRigidConstraints.find(i) != NULL) const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j];
{ const btSoftBody::Face* face = constraint.getContact()->m_face;
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[i]; const btSoftBody::Node* node = constraint.getContact()->m_node;
for (int j = 0; j < constraintsList.size(); ++j) if (node->m_im != 0)
{ {
const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j]; int index = node->index;
btSoftBody::Face* face = constraint->getContact()->m_face; f[index] += constraint.getDv(node)* (1./node->m_im);
}
// 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)
for (int k = 0; k < 3; ++k) {
{ const btSoftBody::Node* node = face->m_n[k];
if (face->m_n[k]->index == i) if (node->m_im != 0)
{ {
if (face->m_n[k]->m_im != 0) int index = node->index;
{ f[index] += constraint.getDv(node)* (1./node->m_im);
f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im); }
} }
break; }
} }
}
}
}
if (m_deformableConstraints.find(i) != NULL)
{
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& 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;
}
}
}
}
}
}
} }
void btDeformableContactProjection::reinitialize(bool nodeUpdated) void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{ {
m_staticConstraints.clear(); int N = m_softBodies.size();
m_nodeAnchorConstraints.clear(); if (nodeUpdated)
m_nodeRigidConstraints.clear(); {
m_faceRigidConstraints.clear(); m_staticConstraints.resize(N);
m_deformableConstraints.clear(); m_nodeAnchorConstraints.resize(N);
m_projectionsDict.clear(); m_nodeRigidConstraints.resize(N);
for (int i = 0; i < m_allFaceConstraints.size(); ++i) m_faceRigidConstraints.resize(N);
{ m_deformableConstraints.resize(N);
delete m_allFaceConstraints[i];
} }
m_allFaceConstraints.clear(); 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();
} }

33
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -28,16 +28,16 @@ public:
typedef btAlignedObjectArray<btVector3> TVStack; typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies; btAlignedObjectArray<btSoftBody *>& m_softBodies;
// map from node index to static constraint // // map from node index to static constraint
btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints; // btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// map from node index to node rigid constraint // // map from node index to node rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints; // btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// map from node index to face rigid constraint // // map from node index to face rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints; // btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// map from node index to deformable constraint // // map from node index to deformable constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints; // btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// map from node index to node anchor constraint // // map from node index to node anchor constraint
btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints; // btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// all constraints involving face // all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints; btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
@@ -45,6 +45,17 @@ public:
// map from node index to projection directions // map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict; btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
// map from node index to static constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
// map from node index to node rigid constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// map from node index to face rigid constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
// map from node index to deformable constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableFaceNodeContactConstraint> > m_deformableConstraints;
// map from node index to node anchor constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableNodeAnchorConstraint> > m_nodeAnchorConstraints;
btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies) btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies) : m_softBodies(softBodies)
{ {
@@ -61,7 +72,7 @@ public:
virtual void applyDynamicFriction(TVStack& f); virtual void applyDynamicFriction(TVStack& f);
// update and solve the constraints // update and solve the constraints
virtual btScalar update(); virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies);
// solve the position error using split impulse // solve the position error using split impulse
virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);

26
src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
View File

@@ -22,32 +22,8 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
{ {
///this is a special step to resolve penetrations (just for contacts) ///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); 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; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
maxIterations = 500;
for (int iteration = 0; iteration < maxIterations; iteration++) 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 // 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); m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
// solver body velocity -> rigid body velocity // solver body velocity -> rigid body velocity
solverBodyWriteBack(infoGlobal); solverBodyWriteBack(infoGlobal);
btScalar deformableResidual = m_deformableSolver->solveContactConstraints(); btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies);
// update rigid body velocity in rigid/deformable contact // update rigid body velocity in rigid/deformable contact
m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
// solver body velocity <- rigid body velocity // solver body velocity <- rigid body velocity