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);

945
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)
// anchor constraints {
for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j];
{ btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); residualSquare = btMax(residualSquare, localResidualSquare);
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); }
residualSquare = btMax(residualSquare, localResidualSquare); // face constraints
} for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{
// face constraints btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
for (int index = 0; index < m_allFaceConstraints.size(); ++index) btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
{ residualSquare = btMax(residualSquare, localResidualSquare);
btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; }
btScalar localResidualSquare = constraint->solveSplitImpulse(infoGlobal);
residualSquare = btMax(residualSquare, localResidualSquare); }
} return residualSquare;
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)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
// set up deformable anchors // set Dirichlet constraint
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; if (psb->m_nodes[j].m_im == 0)
// skip fixed points {
if (anchor.m_node->m_im == 0) btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
{ m_staticConstraints[i].push_back(static_constraint);
continue; }
} }
if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL) // set up deformable anchors
{ for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; {
btDeformableNodeAnchorConstraint constraint(anchor); btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint); // skip fixed points
} if (anchor.m_node->m_im == 0)
} {
continue;
// 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[i].push_back(constraint);
// skip fixed points }
if (contact.m_node->m_im == 0)
{ // set Deformable Node vs. Rigid constraint
continue; for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
} {
btDeformableNodeRigidContactConstraint constraint(contact); const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
btVector3 va = constraint.getVa(); // skip fixed points
btVector3 vb = constraint.getVb(); if (contact.m_node->m_im == 0)
const btVector3 vr = vb - va; {
const btSoftBody::sCti& cti = contact.m_cti; continue;
const btScalar dn = btDot(vr, cti.m_normal); }
if (dn < SIMD_EPSILON) btDeformableNodeRigidContactConstraint constraint(contact);
{ btVector3 va = constraint.getVa();
if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL) btVector3 vb = constraint.getVb();
{ const btVector3 vr = vb - va;
btAlignedObjectArray<btDeformableNodeRigidContactConstraint> constraintsList; const btSoftBody::sCti& cti = contact.m_cti;
constraintsList.push_back(constraint); const btScalar dn = btDot(vr, cti.m_normal);
m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList); if (dn < SIMD_EPSILON)
} {
else m_nodeRigidConstraints[i].push_back(constraint);
{ }
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& 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];
// set Deformable Face vs. Rigid constraint // skip fixed faces
for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j) if (contact.m_c2 == 0)
{ {
const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j]; continue;
// skip fixed faces }
if (contact.m_c2 == 0) btDeformableFaceRigidContactConstraint constraint(contact);
{ btVector3 va = constraint.getVa();
continue; btVector3 vb = constraint.getVb();
} const btVector3 vr = vb - va;
btDeformableFaceRigidContactConstraint* constraint = new btDeformableFaceRigidContactConstraint(contact); const btSoftBody::sCti& cti = contact.m_cti;
btVector3 va = constraint->getVa(); const btScalar dn = btDot(vr, cti.m_normal);
btVector3 vb = constraint->getVb(); if (dn < SIMD_EPSILON)
const btVector3 vr = vb - va; {
const btSoftBody::sCti& cti = contact.m_cti; m_faceRigidConstraints[i].push_back(constraint);
const btScalar dn = btDot(vr, cti.m_normal); }
if (dn < SIMD_EPSILON) }
{
m_allFaceConstraints.push_back(constraint); // set Deformable Face vs. Deformable Node constraint
// add face constraints to each of the nodes for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
for (int k = 0; k < 3; ++k) {
{ const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
btSoftBody::Node* node = contact.m_face->m_n[k];
// static node does not need to own face/rigid constraint btDeformableFaceNodeContactConstraint constraint(contact);
if (node->m_im != 0) btVector3 va = constraint.getVa();
{ btVector3 vb = constraint.getVb();
if (m_faceRigidConstraints.find(node->index) == NULL) const btVector3 vr = vb - va;
{ const btScalar dn = btDot(vr, contact.m_normal);
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> constraintsList; if (dn > -SIMD_EPSILON)
constraintsList.push_back(constraint); {
m_faceRigidConstraints.insert(node->index, constraintsList); m_deformableConstraints[i].push_back(constraint);
} }
else }
{ }
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& 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<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
{
// accumulate normals from Deformable Node vs. Rigid constraints btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL) for (int k = 0; k < 3; ++k)
{ {
hasConstraint = true; projections.push_back(units[k]);
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[index]; }
for (int k = 0; k < constraintsList.size(); ++k) }
{ }
if (constraintsList[k].m_static) for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{ {
existStaticConstraint = true; int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
break; if (m_projectionsDict.find(index) == NULL)
} {
const btVector3& local_normal = constraintsList[k].m_normal; m_projectionsDict.insert(index, units);
normals.push_back(local_normal); }
averagedNormal += local_normal; else
} {
} btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
// accumulate normals from Deformable Face vs. Rigid constraints {
if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL) projections.push_back(units[k]);
{ }
hasConstraint = true; }
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[index]; }
for (int k = 0; k < constraintsList.size(); ++k) for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
{ {
if (constraintsList[k]->m_static) int index = m_nodeRigidConstraints[i][j].m_node->index;
{ if (m_nodeRigidConstraints[i][j].m_static)
existStaticConstraint = true; {
break; if (m_projectionsDict.find(index) == NULL)
} {
const btVector3& local_normal = constraintsList[k]->m_normal; m_projectionsDict.insert(index, units);
normals.push_back(local_normal); }
averagedNormal += local_normal; else
} {
} btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
// accumulate normals from Deformable Node vs. Deformable Face constraints {
if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL) projections.push_back(units[k]);
{ }
hasConstraint = true; }
btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[index]; }
for (int k = 0; k < constraintsList.size(); ++k) else
{ {
if (constraintsList[k]->m_static) if (m_projectionsDict.find(index) == NULL)
{ {
existStaticConstraint = true; btAlignedObjectArray<btVector3> projections;
break; projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
} m_projectionsDict.insert(index, projections);
const btVector3& local_normal = constraintsList[k]->m_normal; }
normals.push_back(local_normal); else
averagedNormal += local_normal; {
} btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
} projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
}
}
// build projections }
if (!hasConstraint) for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
{ {
continue; const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
} for (int k = 0; k < 3; ++k)
btAlignedObjectArray<btVector3> projections; {
if (existStaticConstraint) const btSoftBody::Node* node = face->m_n[k];
{ int index = node->index;
projections.push_back(btVector3(1,0,0)); if (m_faceRigidConstraints[i][j].m_static)
projections.push_back(btVector3(0,1,0)); {
projections.push_back(btVector3(0,0,1)); if (m_projectionsDict.find(index) == NULL)
} {
else m_projectionsDict.insert(index, units);
{ }
bool averageExists = (averagedNormal.length2() > SIMD_EPSILON); else
averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0); {
if (averageExists) btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
{ for (int k = 0; k < 3; ++k)
projections.push_back(averagedNormal); {
} projections.push_back(units[k]);
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 else
if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25) {
{ if (m_projectionsDict.find(index) == NULL)
projections.push_back(local_normal); {
} btAlignedObjectArray<btVector3> projections;
} projections.push_back(m_faceRigidConstraints[i][j].m_normal);
} m_projectionsDict.insert(index, projections);
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);
}
}
}
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<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);
}
}
}
}
} }
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();
} }

35
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -28,22 +28,33 @@ 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;
// 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);

28
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