640 lines
19 KiB
C++
640 lines
19 KiB
C++
/*
|
|
Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
|
|
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2019 Google Inc. http://bulletphysics.org
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "btDeformableContactProjection.h"
|
|
#include "btDeformableMultiBodyDynamicsWorld.h"
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
|
{
|
|
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(infoGlobal);
|
|
residualSquare = btMax(residualSquare, localResidualSquare);
|
|
}
|
|
for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
|
|
{
|
|
btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
|
|
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
|
residualSquare = btMax(residualSquare, localResidualSquare);
|
|
}
|
|
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
|
|
{
|
|
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
|
|
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
|
residualSquare = btMax(residualSquare, localResidualSquare);
|
|
}
|
|
for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
|
|
{
|
|
btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
|
|
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
|
residualSquare = btMax(residualSquare, localResidualSquare);
|
|
}
|
|
}
|
|
}
|
|
return residualSquare;
|
|
}
|
|
|
|
btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
|
{
|
|
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.solveSplitImpulse(infoGlobal);
|
|
residualSquare = btMax(residualSquare, localResidualSquare);
|
|
}
|
|
for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
|
|
{
|
|
btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
|
|
btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
|
|
residualSquare = btMax(residualSquare, localResidualSquare);
|
|
}
|
|
}
|
|
}
|
|
return residualSquare;
|
|
}
|
|
|
|
void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
|
|
{
|
|
BT_PROFILE("setConstraints");
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
|
{
|
|
btSoftBody* psb = m_softBodies[i];
|
|
if (!psb->isActive())
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// 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], infoGlobal);
|
|
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, infoGlobal);
|
|
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, infoGlobal);
|
|
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, infoGlobal, m_useStrainLimiting);
|
|
m_faceRigidConstraints[i].push_back(constraint);
|
|
}
|
|
}
|
|
}
|
|
|
|
void btDeformableContactProjection::project(TVStack& x)
|
|
{
|
|
#ifndef USE_MGS
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
#else
|
|
btReducedVector p(x.size());
|
|
for (int i = 0; i < m_projections.size(); ++i)
|
|
{
|
|
p += (m_projections[i].dot(x) * m_projections[i]);
|
|
}
|
|
for (int i = 0; i < p.m_indices.size(); ++i)
|
|
{
|
|
x[p.m_indices[i]] -= p.m_vecs[i];
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void btDeformableContactProjection::setProjection()
|
|
{
|
|
#ifndef USE_MGS
|
|
BT_PROFILE("btDeformableContactProjection::setProjection");
|
|
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;
|
|
m_staticConstraints[i][j].m_node->m_constrained = true;
|
|
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;
|
|
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
|
|
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;
|
|
m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
|
|
if (m_nodeRigidConstraints[i][j].m_binding)
|
|
{
|
|
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;
|
|
if (m_faceRigidConstraints[i][j].m_binding)
|
|
{
|
|
for (int k = 0; k < 3; ++k)
|
|
{
|
|
face->m_n[k]->m_constrained = true;
|
|
}
|
|
}
|
|
for (int k = 0; k < 3; ++k)
|
|
{
|
|
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 l = 0; l < 3; ++l)
|
|
{
|
|
projections.push_back(units[l]);
|
|
}
|
|
}
|
|
}
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#else
|
|
int dof = 0;
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
|
{
|
|
dof += m_softBodies[i]->m_nodes.size();
|
|
}
|
|
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;
|
|
m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
|
|
btAlignedObjectArray<int> indices;
|
|
btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
|
|
indices.push_back(index);
|
|
vecs1.push_back(btVector3(1, 0, 0));
|
|
vecs2.push_back(btVector3(0, 1, 0));
|
|
vecs3.push_back(btVector3(0, 0, 1));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
|
}
|
|
|
|
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
|
|
{
|
|
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
|
|
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
|
|
btAlignedObjectArray<int> indices;
|
|
btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
|
|
indices.push_back(index);
|
|
vecs1.push_back(btVector3(1, 0, 0));
|
|
vecs2.push_back(btVector3(0, 1, 0));
|
|
vecs3.push_back(btVector3(0, 0, 1));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
|
}
|
|
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
|
{
|
|
int index = m_nodeRigidConstraints[i][j].m_node->index;
|
|
m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
|
btAlignedObjectArray<int> indices;
|
|
indices.push_back(index);
|
|
btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
|
|
if (m_nodeRigidConstraints[i][j].m_static)
|
|
{
|
|
vecs1.push_back(btVector3(1, 0, 0));
|
|
vecs2.push_back(btVector3(0, 1, 0));
|
|
vecs3.push_back(btVector3(0, 0, 1));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs2));
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs3));
|
|
}
|
|
else
|
|
{
|
|
vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
|
|
m_projections.push_back(btReducedVector(dof, indices, vecs1));
|
|
}
|
|
}
|
|
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
|
{
|
|
const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
|
|
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
|
|
btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
|
|
for (int k = 0; k < 3; ++k)
|
|
{
|
|
face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
|
|
}
|
|
if (m_faceRigidConstraints[i][j].m_static)
|
|
{
|
|
for (int l = 0; l < 3; ++l)
|
|
{
|
|
btReducedVector rv(dof);
|
|
for (int k = 0; k < 3; ++k)
|
|
{
|
|
rv.m_indices.push_back(face->m_n[k]->index);
|
|
btVector3 v(0, 0, 0);
|
|
v[l] = bary[k];
|
|
rv.m_vecs.push_back(v);
|
|
rv.sort();
|
|
}
|
|
m_projections.push_back(rv);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
btReducedVector rv(dof);
|
|
for (int k = 0; k < 3; ++k)
|
|
{
|
|
rv.m_indices.push_back(face->m_n[k]->index);
|
|
rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
|
|
rv.sort();
|
|
}
|
|
m_projections.push_back(rv);
|
|
}
|
|
}
|
|
}
|
|
btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
|
|
mgs.solve();
|
|
m_projections = mgs.m_out;
|
|
#endif
|
|
}
|
|
|
|
void btDeformableContactProjection::checkConstraints(const TVStack& x)
|
|
{
|
|
for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
|
|
{
|
|
btVector3 d(0, 0, 0);
|
|
const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
|
|
for (int j = 0; j < lm.m_num_constraints; ++j)
|
|
{
|
|
for (int k = 0; k < lm.m_num_nodes; ++k)
|
|
{
|
|
d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
|
|
}
|
|
}
|
|
// printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
|
|
// printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
|
|
}
|
|
}
|
|
|
|
void btDeformableContactProjection::setLagrangeMultiplier()
|
|
{
|
|
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;
|
|
m_staticConstraints[i][j].m_node->m_constrained = true;
|
|
LagrangeMultiplier lm;
|
|
lm.m_num_nodes = 1;
|
|
lm.m_indices[0] = index;
|
|
lm.m_weights[0] = 1.0;
|
|
lm.m_num_constraints = 3;
|
|
lm.m_dirs[0] = btVector3(1, 0, 0);
|
|
lm.m_dirs[1] = btVector3(0, 1, 0);
|
|
lm.m_dirs[2] = btVector3(0, 0, 1);
|
|
m_lagrangeMultipliers.push_back(lm);
|
|
}
|
|
for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
|
|
{
|
|
int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
|
|
m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
|
|
LagrangeMultiplier lm;
|
|
lm.m_num_nodes = 1;
|
|
lm.m_indices[0] = index;
|
|
lm.m_weights[0] = 1.0;
|
|
lm.m_num_constraints = 3;
|
|
lm.m_dirs[0] = btVector3(1, 0, 0);
|
|
lm.m_dirs[1] = btVector3(0, 1, 0);
|
|
lm.m_dirs[2] = btVector3(0, 0, 1);
|
|
m_lagrangeMultipliers.push_back(lm);
|
|
}
|
|
|
|
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
|
{
|
|
if (!m_nodeRigidConstraints[i][j].m_binding)
|
|
{
|
|
continue;
|
|
}
|
|
int index = m_nodeRigidConstraints[i][j].m_node->index;
|
|
m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
|
|
LagrangeMultiplier lm;
|
|
lm.m_num_nodes = 1;
|
|
lm.m_indices[0] = index;
|
|
lm.m_weights[0] = 1.0;
|
|
if (m_nodeRigidConstraints[i][j].m_static)
|
|
{
|
|
lm.m_num_constraints = 3;
|
|
lm.m_dirs[0] = btVector3(1, 0, 0);
|
|
lm.m_dirs[1] = btVector3(0, 1, 0);
|
|
lm.m_dirs[2] = btVector3(0, 0, 1);
|
|
}
|
|
else
|
|
{
|
|
lm.m_num_constraints = 1;
|
|
lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
|
|
}
|
|
m_lagrangeMultipliers.push_back(lm);
|
|
}
|
|
|
|
for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
|
|
{
|
|
if (!m_faceRigidConstraints[i][j].m_binding)
|
|
{
|
|
continue;
|
|
}
|
|
btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
|
|
|
|
btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
|
|
LagrangeMultiplier lm;
|
|
lm.m_num_nodes = 3;
|
|
|
|
for (int k = 0; k < 3; ++k)
|
|
{
|
|
face->m_n[k]->m_constrained = true;
|
|
lm.m_indices[k] = face->m_n[k]->index;
|
|
lm.m_weights[k] = bary[k];
|
|
}
|
|
if (m_faceRigidConstraints[i][j].m_static)
|
|
{
|
|
face->m_pcontact[3] = 1;
|
|
lm.m_num_constraints = 3;
|
|
lm.m_dirs[0] = btVector3(1, 0, 0);
|
|
lm.m_dirs[1] = btVector3(0, 1, 0);
|
|
lm.m_dirs[2] = btVector3(0, 0, 1);
|
|
}
|
|
else
|
|
{
|
|
face->m_pcontact[3] = 0;
|
|
lm.m_num_constraints = 1;
|
|
lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
|
|
}
|
|
m_lagrangeMultipliers.push_back(lm);
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
|
|
{
|
|
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)
|
|
{
|
|
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();
|
|
}
|
|
#ifndef USE_MGS
|
|
m_projectionsDict.clear();
|
|
#else
|
|
m_projections.clear();
|
|
#endif
|
|
m_lagrangeMultipliers.clear();
|
|
}
|