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

2
src/BulletSoftBody/btDeformableBodySolver.h
View File

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

945
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -17,527 +17,492 @@
#include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm>
#include <cmath>
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<btDeformableNodeRigidContactConstraint>& 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<btDeformableNodeRigidContactConstraint>& 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<btDeformableNodeRigidContactConstraint>& 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<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
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<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
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;
}
}
}
// 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<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
if (projectionDirs.size() >= dim)
{
// static node
x[i].setZero();
continue;
}
else if (projectionDirs.size() == 2)
{
btVector3 dir0 = projectionDirs[0];
btVector3 dir1 = projectionDirs[1];
btVector3 free_dir = btCross(dir0, dir1);
if (free_dir.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<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
if (projectionDirs.size() >= dim)
{
// static node
x[i].setZero();
continue;
}
else if (projectionDirs.size() == 2)
{
btVector3 dir0 = projectionDirs[0];
btVector3 dir1 = projectionDirs[1];
btVector3 free_dir = btCross(dir0, dir1);
if (free_dir.safeNorm() < SIMD_EPSILON)
{
x[i] -= x[i].dot(dir0) * dir0;
x[i] -= x[i].dot(dir1) * dir1;
}
else
{
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
}
}
else
{
btAssert(projectionDirs.size() == 1);
btVector3 dir0 = projectionDirs[0];
x[i] -= x[i].dot(dir0) * dir0;
}
}
}
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<btVector3> 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<btDeformableNodeRigidContactConstraint>& 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<btDeformableFaceRigidContactConstraint*>& 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<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);
}
}
btAlignedObjectArray<btVector3> units;
units.push_back(btVector3(1,0,0));
units.push_back(btVector3(0,1,0));
units.push_back(btVector3(0,0,1));
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
{
int index = m_staticConstraints[i][j].m_node->index;
if (m_projectionsDict.find(index) == NULL)
{
m_projectionsDict.insert(index, units);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
{
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
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);
}
}
}
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)
{
// 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<btVector3>& projectionDirs = *m_projectionsDict[i];
if (projectionDirs.size() >= 3)
{
continue;
}
}
// add friction contribution from Face vs. Node
if (m_nodeRigidConstraints.find(i) != NULL)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& 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<btDeformableFaceRigidContactConstraint*>& 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<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;
}
}
}
}
}
}
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();
}

35
src/BulletSoftBody/btDeformableContactProjection.h
View File

@@ -28,22 +28,33 @@ public:
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
// map from node index to static constraint
btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// map from node index to node rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// map from node index to face rigid constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// map from node index to deformable constraint
btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// map from node index to node anchor constraint
btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// // map from node index to static constraint
// btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// // map from node index to node rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// // map from node index to face rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// // map from node index to deformable constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// // map from node index to node anchor constraint
// btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
// map from node index to projection directions
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)
: 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);

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