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

reformulate how constraints are managed in the projection class

Browse Source
This commit is contained in:
Xuchen Han
2019-07-11 11:26:30 -07:00
parent b28f1fdac3
commit 4e5f4b9fe9
5 changed files with 173 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/BulletSoftBody/btBackwardEulerObjective.cpp
View File

@@ -70,19 +70,27 @@ void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual,
void btBackwardEulerObjective::updateVelocity(const TVStack& dv) void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
{ {
for (int i = 0; i < m_softBodies.size(); ++i) // only the velocity of the constrained nodes needs to be updated during CG solve
for (auto it : projection.m_constraints)
{ {
int counter = 0; int i = projection.m_indices[it.first];
for (int i = 0; i < m_softBodies.size(); ++i) it.first->m_v = m_backupVelocity[i] + dv[i];
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
// only the velocity of the constrained nodes needs to be updated during CG solve
if (projection.m_constrainedDirections.size() > 0)
psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
++counter;
}
}
} }
} }
// for (int i = 0; i < m_softBodies.size(); ++i)
// {
// int counter = 0;
// for (int i = 0; i < m_softBodies.size(); ++i)
// {
// btSoftBody* psb = m_softBodies[i];
// for (int j = 0; j < psb->m_nodes.size(); ++j)
// {
// // only the velocity of the constrained nodes needs to be updated during CG solve
// if (projection.m_constraints[&(psb->m_nodes[j])].size() > 0)
// psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
// ++counter;
// }
// }
// }

53
src/BulletSoftBody/btCGProjection.h
View File

@@ -11,6 +11,34 @@
#include <unordered_map> #include <unordered_map>
class btDeformableRigidDynamicsWorld; class btDeformableRigidDynamicsWorld;
struct Constraint
{
const btSoftBody::RContact* m_contact;
const btVector3 m_direction;
btScalar m_value;
Constraint(const btSoftBody::RContact& rcontact)
: m_contact(&rcontact)
, m_direction(rcontact.m_cti.m_normal)
, m_value(0)
{
}
Constraint(const btVector3 dir)
: m_contact(nullptr)
, m_direction(dir)
, m_value(0)
{}
Constraint()
: m_contact(nullptr)
{
}
};
class btCGProjection class btCGProjection
{ {
public: public:
@@ -21,11 +49,11 @@ public:
btAlignedObjectArray<btSoftBody *> m_softBodies; btAlignedObjectArray<btSoftBody *> m_softBodies;
btDeformableRigidDynamicsWorld* m_world; btDeformableRigidDynamicsWorld* m_world;
std::unordered_map<btSoftBody::Node *, size_t> m_indices; std::unordered_map<btSoftBody::Node *, size_t> m_indices;
TVArrayStack m_constrainedDirections; // TVArrayStack m_constrainedDirections;
TArrayStack m_constrainedValues; // TArrayStack m_constrainedValues;
btAlignedObjectArray<int> m_constrainedId; // btAlignedObjectArray<int> m_constrainedId;
const btScalar& m_dt; const btScalar& m_dt;
std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Constraint> > m_constraints;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
: m_softBodies(softBodies) : m_softBodies(softBodies)
@@ -51,14 +79,15 @@ public:
updateId(); updateId();
// resize and clear the old constraints // resize and clear the old constraints
m_constrainedValues.resize(m_indices.size()); // m_constrainedValues.resize(m_indices.size());
m_constrainedDirections.resize(m_indices.size()); // m_constrainedDirections.resize(m_indices.size());
for (int i = 0; i < m_constrainedDirections.size(); ++i) // for (int i = 0; i < m_constrainedDirections.size(); ++i)
{ // {
m_constrainedDirections[i].clear(); // m_constrainedDirections[i].clear();
m_constrainedValues[i].clear(); // m_constrainedValues[i].clear();
} // }
m_constrainedId.clear(); // m_constrainedId.clear();
m_constraints.clear();
} }
void updateId() void updateId()

153
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -12,20 +12,21 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
///solve rigid body constraints ///solve rigid body constraints
m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
// loop through contacts to create contact constraints // loop through constraints to set constrained values
for (int i = 0; i < m_softBodies.size(); ++i) for (auto it : m_constraints)
{ {
btSoftBody* psb = m_softBodies[i]; btAlignedObjectArray<Constraint>& constraints = it.second;
btMultiBodyJacobianData jacobianData; for (int i = 0; i < constraints.size(); ++i)
for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
{ {
const btSoftBody::RContact& c = psb->m_rcontacts[i]; Constraint& constraint = constraints[i];
if (constraint.m_contact == nullptr)
// skip anchor points {
if (c.m_node->m_im == 0) // nothing needs to be done for dirichelet constraints
continue; continue;
}
const btSoftBody::sCti& cti = c.m_cti; const btSoftBody::RContact* c = constraint.m_contact;
const btSoftBody::sCti& cti = c->m_cti;
btMultiBodyJacobianData jacobianData;
if (cti.m_colObj->hasContactResponse()) if (cti.m_colObj->hasContactResponse())
{ {
btVector3 va(0, 0, 0); btVector3 va(0, 0, 0);
@@ -37,7 +38,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{ {
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0); va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
} }
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{ {
@@ -49,7 +50,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
btScalar* jac = &jacobianData.m_jacobians[0]; btScalar* jac = &jacobianData.m_jacobians[0];
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c->m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0]; deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v); multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
@@ -62,26 +63,26 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
} }
} }
const btVector3 vb = c.m_node->m_v * m_dt; const btVector3 vb = c->m_node->m_v * m_dt;
const btVector3 vr = vb - va; const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal); const btScalar dn = btDot(vr, cti.m_normal);
if (1) // in the same CG solve, the set of constraits doesn't change if (1) // in the same CG solve, the set of constraits doesn't change
// if (dn <= SIMD_EPSILON) // if (dn <= SIMD_EPSILON)
{ {
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
const btVector3 impulse = c.m_c0 *(cti.m_normal * dn); const btVector3 impulse = c->m_c0 *(cti.m_normal * dn);
// TODO: only contact is considered here, add friction later // TODO: only contact is considered here, add friction later
// dv = new_impulse + accumulated velocity change in previous CG iterations // dv = new_impulse + accumulated velocity change in previous CG iterations
// so we have the invariant node->m_v = backupVelocity + dv; // so we have the invariant node->m_v = backupVelocity + dv;
btVector3 dv = -impulse * c.m_c2/m_dt + c.m_node->m_v - backupVelocity[m_indices[c.m_node]]; btVector3 dv = -impulse * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
btScalar dvn = dv.dot(cti.m_normal); btScalar dvn = dv.dot(cti.m_normal);
m_constrainedValues[m_indices[c.m_node]][0]=(dvn); constraint.m_value = dvn;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{ {
if (rigidCol) if (rigidCol)
rigidCol->applyImpulse(impulse, c.m_c1); rigidCol->applyImpulse(impulse, c->m_c1);
} }
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{ {
@@ -97,24 +98,23 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
} }
} }
void btContactProjection::setConstraintDirections() void btContactProjection::setConstraintDirections()
{ {
// set Dirichlet constraint // set Dirichlet constraint
size_t counter = 0; size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i) for (int i = 0; i < m_softBodies.size(); ++i)
{ {
const btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j) for (int j = 0; j < psb->m_nodes.size(); ++j)
{ {
if (psb->m_nodes[j].m_im == 0) if (psb->m_nodes[j].m_im == 0)
{ {
m_constrainedDirections[counter].push_back(btVector3(1,0,0)); btAlignedObjectArray<Constraint> c;
m_constrainedDirections[counter].push_back(btVector3(0,1,0)); c.push_back(Constraint(btVector3(1,0,0)));
m_constrainedDirections[counter].push_back(btVector3(0,0,1)); c.push_back(Constraint(btVector3(0,1,0)));
m_constrainedValues[counter].push_back(0); c.push_back(Constraint(btVector3(0,0,1)));
m_constrainedValues[counter].push_back(0); m_constraints[&(psb->m_nodes[j])] = c;
m_constrainedValues[counter].push_back(0);
m_constrainedId.push_back(counter);
} }
++counter; ++counter;
} }
@@ -125,14 +125,12 @@ void btContactProjection::setConstraintDirections()
btSoftBody* psb = m_softBodies[i]; btSoftBody* psb = m_softBodies[i];
btMultiBodyJacobianData jacobianData; btMultiBodyJacobianData jacobianData;
int j = 0; for (int j = 0; j < psb->m_rcontacts.size(); ++j)
while (j < psb->m_rcontacts.size())
{ {
const btSoftBody::RContact& c = psb->m_rcontacts[j]; const btSoftBody::RContact& c = psb->m_rcontacts[j];
// skip anchor points // skip anchor points
if (c.m_node->m_im == 0) if (c.m_node->m_im == 0)
{ {
psb->m_rcontacts.removeAtIndex(j);
continue; continue;
} }
@@ -178,68 +176,69 @@ void btContactProjection::setConstraintDirections()
const btScalar dn = btDot(vr, cti.m_normal); const btScalar dn = btDot(vr, cti.m_normal);
if (dn < SIMD_EPSILON) if (dn < SIMD_EPSILON)
{ {
++j; if (m_constraints.find(c.m_node) == m_constraints.end())
m_constrainedDirections[m_indices[c.m_node]].push_back(cti.m_normal); {
m_constrainedValues[m_indices[c.m_node]].resize(m_constrainedValues[m_indices[c.m_node]].size()+1); btAlignedObjectArray<Constraint> constraints;
m_constrainedId.push_back(m_indices[c.m_node]); constraints.push_back(Constraint(c));
m_constraints[c.m_node] = constraints;
}
else
{
m_constraints[c.m_node].push_back(Constraint(c));
}
continue; continue;
} }
} }
psb->m_rcontacts.removeAtIndex(j);
} }
} }
// for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions // for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions
counter = 0; counter = 0;
const int dim = 3; const int dim = 3;
for (int i = 0; i < m_softBodies.size(); ++i) for (auto it : m_constraints)
{ {
const btSoftBody* psb = m_softBodies[i]; const btAlignedObjectArray<Constraint>& c = it.second;
for (int j = 0; j < psb->m_nodes.size(); ++j) if (c.size() > dim)
{ {
if (m_constrainedDirections[counter].size() > dim) btAlignedObjectArray<Constraint> prunedConstraints;
{ // always keep the first constrained direction
btAlignedObjectArray<btVector3> prunedConstraints; prunedConstraints.push_back(c[0]);
// always keep the first constrained direction
prunedConstraints.push_back(m_constrainedDirections[counter][0]);
// find the direction most orthogonal to the first direction and keep it
size_t selected = 1;
btScalar min_dotProductAbs = std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][selected]));
for (int j = 2; j < m_constrainedDirections[counter].size(); ++j)
{
btScalar dotProductAbs =std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][j]));
if (dotProductAbs < min_dotProductAbs)
{
selected = j;
min_dotProductAbs = dotProductAbs;
}
}
if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON)
{
m_constrainedDirections[counter++] = prunedConstraints;
continue;
}
prunedConstraints.push_back(m_constrainedDirections[counter][selected]);
// find the direction most orthogonal to the previous two directions and keep it // find the direction most orthogonal to the first direction and keep it
size_t selected2 = (selected == 1) ? 2 : 1; size_t selected = 1;
btVector3 normal = btCross(prunedConstraints[0], prunedConstraints[1]); btScalar min_dotProductAbs = std::abs(prunedConstraints[0].m_direction.dot(c[selected].m_direction));
normal.normalize(); for (int j = 2; j < c.size(); ++j)
btScalar max_dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][selected2])); {
for (int j = 3; j < m_constrainedDirections[counter].size(); ++j) btScalar dotProductAbs =std::abs(prunedConstraints[0].m_direction.dot(c[j].m_direction));
if (dotProductAbs < min_dotProductAbs)
{ {
btScalar dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][j])); selected = j;
if (dotProductAbs > min_dotProductAbs) min_dotProductAbs = dotProductAbs;
{
selected2 = j;
max_dotProductAbs = dotProductAbs;
}
} }
prunedConstraints.push_back(m_constrainedDirections[counter][selected2]);
m_constrainedDirections[counter] = prunedConstraints;
m_constrainedValues[counter].resize(dim);
} }
++counter; if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON)
{
it.second = prunedConstraints;
continue;
}
prunedConstraints.push_back(c[selected]);
// find the direction most orthogonal to the previous two directions and keep it
size_t selected2 = (selected == 1) ? 2 : 1;
btVector3 normal = btCross(prunedConstraints[0].m_direction, prunedConstraints[1].m_direction);
normal.normalize();
btScalar max_dotProductAbs = std::abs(normal.dot(c[selected2].m_direction));
for (int j = 3; j < c.size(); ++j)
{
btScalar dotProductAbs = std::abs(normal.dot(c[j].m_direction));
if (dotProductAbs > min_dotProductAbs)
{
selected2 = j;
max_dotProductAbs = dotProductAbs;
}
}
prunedConstraints.push_back(c[selected2]);
it.second = prunedConstraints;
} }
} }
} }

45
src/BulletSoftBody/btContactProjection.h
View File

@@ -30,20 +30,19 @@ public:
virtual void operator()(TVStack& x) virtual void operator()(TVStack& x)
{ {
const int dim = 3; const int dim = 3;
for (int j = 0; j < m_constrainedId.size(); ++j) for (auto it : m_constraints)
{ {
int i = m_constrainedId[j]; const btAlignedObjectArray<Constraint>& constraints = it.second;
btAssert(m_constrainedDirections[i].size() <= dim); size_t i = m_indices[it.first];
if (m_constrainedDirections[i].size() <= 1) btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{ {
for (int j = 0; j < m_constrainedDirections[i].size(); ++j) x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
{
x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j];
}
} }
else if (m_constrainedDirections[i].size() == 2) else if (constraints.size() == 2)
{ {
btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]); btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
free_dir.normalize(); free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir; x[i] = x[i].dot(free_dir) * free_dir;
} }
@@ -52,29 +51,29 @@ public:
} }
} }
virtual void enforceConstraint(TVStack& x) virtual void enforceConstraint(TVStack& x)
{ {
const int dim = 3; const int dim = 3;
for (int j = 0; j < m_constrainedId.size(); ++j) for (auto it : m_constraints)
{ {
int i = m_constrainedId[j]; const btAlignedObjectArray<Constraint>& constraints = it.second;
btAssert(m_constrainedDirections[i].size() <= dim); size_t i = m_indices[it.first];
if (m_constrainedDirections[i].size() <= 1) btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{ {
for (int j = 0; j < m_constrainedDirections[i].size(); ++j) x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
{ x[i] += constraints[0].m_value * constraints[0].m_direction;
x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j];
x[i] += m_constrainedValues[i][j] * m_constrainedDirections[i][j];
}
} }
else if (m_constrainedDirections[i].size() == 2) else if (constraints.size() == 2)
{ {
btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]); btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
free_dir.normalize(); free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir + m_constrainedDirections[i][0] * m_constrainedValues[i][0] + m_constrainedDirections[i][1] * m_constrainedValues[i][1]; x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value;
} }
else else
x[i] = m_constrainedDirections[i][0] * m_constrainedValues[i][0] + m_constrainedDirections[i][1] * m_constrainedValues[i][1] + m_constrainedDirections[i][2] * m_constrainedValues[i][2]; x[i] = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction + constraints[2].m_value * constraints[2].m_direction;
} }
} }

26
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -89,20 +89,20 @@ void btDeformableBodySolver::postStabilize()
c.m_node->m_x -= dp * cti.m_normal * c.m_c4; c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
//// ////
// if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) // if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
// { // {
// if (rigidCol) // if (rigidCol)
// rigidCol->applyImpulse(impulse, c.m_c1); // rigidCol->applyImpulse(impulse, c.m_c1);
// } // }
// else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
// {
// if (multibodyLinkCol)
// {
// double multiplier = 0.5;
// multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
// }
// }
} }
// else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
// {
// if (multibodyLinkCol)
// {
// double multiplier = 0.5;
// multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
// }
// }
} }
} }
} }