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

switched to deformable rigid contact from Jacobi to Gauss Seidel

Browse Source
This commit is contained in:
Xuchen Han
2019-09-11 17:04:06 -07:00
parent f99cf56149
commit 109d9353af
9 changed files with 312 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

225
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -20,8 +20,7 @@
btScalar btDeformableContactProjection::update()
{
btScalar residualSquare = 0;
// loop through constraints to set constrained values
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
@@ -34,15 +33,14 @@ btScalar btDeformableContactProjection::update()
}
// face constraints
// for (int index = 0; index < m_faceRigidConstraints.size(); ++index)
// {
// btAlignedObjectArray<btDeformableFaceRigidContactConstraint>& constraints = *m_faceRigidConstraints.getAtIndex(index);
// for (int i = 0; i < constraints.size(); ++i)
// {
// btScalar localResidualSquare = constraints[i].solveConstraint();
// residualSquare = btMax(residualSquare, localResidualSquare);
// }
// }
for (int index = 0; index < m_allFaceConstraints.size(); ++index)
{
btDeformableFaceRigidContactConstraint* constraint = m_allFaceConstraints[index];
btScalar localResidualSquare = constraint->solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
// todo xuchenhan@: deformable/deformable constraints
return residualSquare;
}
@@ -68,8 +66,6 @@ void btDeformableContactProjection::setConstraints()
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData_normal;
btMultiBodyJacobianData jacobianData_complementary;
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
{
const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
@@ -99,38 +95,142 @@ void btDeformableContactProjection::setConstraints()
}
}
}
// 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;
}
}
}
// todo xuchenhan@: set Deformable Face vs. Rigid constraint
// todo xuchenhan@: set Deformable Face vs. Deformable Node
}
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
for (int i = 0; i < x.size(); ++i)
// x is set to zero when passed in
// loop through node constraints to add in contributions to dv
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
x[i].setZero();
if (m_staticConstraints.find(i) != NULL)
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints.getAtIndex(index);
// i is node index
int i = m_nodeRigidConstraints.getKeyAtIndex(index).getUid1();
int numConstraints = 1;
// int numConstraints = constraintsList.size();
// if (m_faceRigidConstraints.find(i) != NULL)
// {
// numConstraints += m_faceRigidConstraints[i]->size();
// }
for (int j = 0; j < constraintsList.size(); ++j)
{
// if a node is fixed, dv = 0
continue;
const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
x[i] += constraint.getDv(constraint.getContact()->m_node)/btScalar(numConstraints);
}
if (m_nodeRigidConstraints.find(i) != NULL)
}
// loop through face constraints to add in contributions to dv
// note that for each face constraint is owned by three nodes. Be careful here to only add the dv to the node that owns the constraint
for (int index = 0; index < m_faceRigidConstraints.size(); ++index)
{
btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints.getAtIndex(index);
// i is node index
int i = m_faceRigidConstraints.getKeyAtIndex(index).getUid1();
int numConstraints = 1;
// int numConstraints = constraintsList.size();
// if (m_nodeRigidConstraints.find(i) != NULL)
// {
// numConstraints += m_nodeRigidConstraints[i]->size();
// }
for (int j = 0; j < constraintsList.size(); ++j)
{
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
for (int j = 0; j < constraintsList.size(); ++j)
const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
const btSoftBody::Face* face = constraint->m_face;
btSoftBody::Node* node;
// find the node that owns the constraint
for (int k = 0; k < 3; ++k)
{
const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
x[i] += constraint.getDv(m_nodes->at(i));
if (face->m_n[k]->index == i)
{
node = face->m_n[k];
break;
}
}
x[i] += constraint->getDv(node) / btScalar(numConstraints);
}
}
// todo xuchenhan@: add deformable deformable constraints' contribution to dv
// Finally, loop through static constraints to set dv of static nodes to zero
for (int index = 0; index < m_staticConstraints.size(); ++index)
{
const btDeformableStaticConstraint& constraint = *m_staticConstraints.getAtIndex(index);
int i = m_staticConstraints.getKeyAtIndex(index).getUid1();
x[i] = constraint.getDv(constraint.m_node);
}
//
// for (int i = 0; i < x.size(); ++i)
// {
// x[i].setZero();
// if (m_staticConstraints.find(i) != NULL)
// {
// // if a node is fixed, dv = 0
// continue;
// }
// 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];
//// x[i] += constraint.getDv(m_nodes->at(i));
// x[i] += constraint.getDv(constraint.getContact()->m_node);
// }
// }
// todo xuchenhan@
// if (m_faceRigidConstraints.find(i) != NULL)
// {
//
// }
}
// }
}
void btDeformableContactProjection::project(TVStack& x)
@@ -202,16 +302,27 @@ void btDeformableContactProjection::setProjection()
break;
}
const btVector3& local_normal = constraintsList[k].m_normal;
// add another projection direction if it deviates from the average by more than about 15 degrees
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// todo: xuchenhan@ implement face
// if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
// {
// }
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;
}
}
// build projections
@@ -252,6 +363,48 @@ void btDeformableContactProjection::setProjection()
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;
}
}
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);
}
}
// todo xuchenhan@
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.
// todo xuchenhan@: figure out the index for m_faceRigidConstraints
// f[i] += constraint.getDv(face->m_n[0])* (1./face->m_n[0]->m_im);
// f[i] += constraint.getDv(face->m_n[1])* (1./face->m_n[1]->m_im);
// f[i] += constraint.getDv(face->m_n[2])* (1./face->m_n[2]->m_im);
}
}
}
// for (int index = 0; index < m_constraints.size(); ++index)
// {
// const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
@@ -286,7 +439,13 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
btCGProjection::reinitialize(nodeUpdated);
m_staticConstraints.clear();
m_nodeRigidConstraints.clear();
// m_faceRigidConstraints.clear();
m_faceRigidConstraints.clear();
m_projectionsDict.clear();
for (int i = 0; i < m_allFaceConstraints.size(); ++i)
{
delete m_allFaceConstraints[i];
}
m_allFaceConstraints.clear();
}