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

reorganize the contact constraints

Browse Source
This commit is contained in:
Xuchen Han
2019-07-15 14:52:24 -07:00
parent bac7d461c5
commit befab4eab6
3 changed files with 151 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/BulletSoftBody/btCGProjection.h
View File

@@ -14,37 +14,41 @@ class btDeformableRigidDynamicsWorld;
struct Constraint
{
const btSoftBody::RContact* m_contact;
btVector3 m_direction;
btScalar m_value;
btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
btAlignedObjectArray<btVector3> m_direction;
btAlignedObjectArray<btScalar> m_value;
Constraint(const btSoftBody::RContact& rcontact)
: m_contact(&rcontact)
, m_direction(rcontact.m_cti.m_normal)
, m_value(0)
{
m_contact.push_back(&rcontact);
m_direction.push_back(rcontact.m_cti.m_normal);
m_value.push_back(0);
}
Constraint(const btVector3 dir)
: m_contact(nullptr)
, m_direction(dir)
, m_value(0)
{}
{
m_contact.push_back(nullptr);
m_direction.push_back(dir);
m_value.push_back(0);
}
Constraint()
: m_contact(nullptr)
{
}
};
struct Friction
{
btVector3 m_dv;
bool m_static;
btScalar m_value;
btVector3 m_direction;
bool m_static_prev;
btScalar m_value_prev;
btVector3 m_direction_prev;
Friction()
{
m_dv.setZero();
m_direction_prev.setZero();
m_direction.setZero();
}
};

244
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -22,93 +22,97 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
for (int i = 0; i < constraints.size(); ++i)
{
Constraint& constraint = constraints[i];
if (constraint.m_contact == nullptr)
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
// nothing needs to be done for dirichelet constraints
continue;
}
const btSoftBody::RContact* c = constraint.m_contact;
const btSoftBody::sCti& cti = c->m_cti;
btMultiBodyJacobianData jacobianData;
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
btScalar* deltaV;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
if (constraint.m_contact[j] == nullptr)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
// nothing needs to be done for dirichelet constraints
continue;
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
btMultiBodyJacobianData jacobianData;
if (cti.m_colObj->hasContactResponse())
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
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);
deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
btScalar vel = 0.0;
for (int j = 0; j < ndof; ++j)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * m_dt;
}
}
const btVector3 vb = c->m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
btVector3 impulse = c->m_c0 * vr;
const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON)
{
btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3, impulse_tangent.norm());
impulse_tangent_magnitude = 0;
const btVector3 tangent_dir = impulse_tangent.normalized();
impulse_tangent = impulse_tangent_magnitude * tangent_dir;
friction.m_direction = impulse_tangent;
friction.m_dv = -impulse_tangent * c->m_c2/m_dt + (c->m_node->m_v - backupVelocity[m_indices[c->m_node]]).dot(tangent_dir)*tangent_dir;
}
impulse = impulse_normal + impulse_tangent;
// if (1) // in the same CG solve, the set of constraits doesn't change
if (dn <= SIMD_EPSILON)
{
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
// TODO: only contact is considered here, add friction later
// dv = new_impulse + accumulated velocity change in previous CG iterations
// 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]];
btScalar dvn = dv.dot(cti.m_normal);
constraint.m_value = dvn;
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
btScalar* deltaV;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
rigidCol->applyImpulse(impulse_normal, c->m_c1);
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
double multiplier = 0.5;
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
jacobianData.m_jacobians.resize(ndof);
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
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);
deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
btScalar vel = 0.0;
for (int j = 0; j < ndof; ++j)
{
vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
}
va = cti.m_normal * vel * m_dt;
}
}
const btVector3 vb = c->m_node->m_v * m_dt;
const btVector3 vr = vb - va;
const btScalar dn = btDot(vr, cti.m_normal);
btVector3 impulse = c->m_c0 * vr;
const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn);
btVector3 impulse_tangent = impulse - impulse_normal;
if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON)
{
btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3*1000, impulse_tangent.norm());
// impulse_tangent_magnitude = 0;
const btVector3 tangent_dir = impulse_tangent.normalized();
impulse_tangent = impulse_tangent_magnitude * tangent_dir;
friction.m_direction = impulse_tangent;
friction.m_dv = -impulse_tangent * c->m_c2/m_dt + (c->m_node->m_v - backupVelocity[m_indices[c->m_node]]);
}
impulse = impulse_normal + impulse_tangent;
// if (1) // in the same CG solve, the set of constraits doesn't change
if (dn <= SIMD_EPSILON)
{
// c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
// TODO: only contact is considered here, add friction later
// dv = new_impulse + accumulated velocity change in previous CG iterations
// 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]];
btScalar dvn = dv.dot(cti.m_normal);
constraint.m_value[j] = dvn;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
rigidCol->applyImpulse(impulse_normal, 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);
}
}
}
}
@@ -202,78 +206,30 @@ void btContactProjection::setConstraintDirections()
}
else
{
m_constraints[c.m_node].push_back(Constraint(c));
// group colinear constraints into one
const btScalar angle_epsilon = 0.015192247; // less than 10 degree
bool merged = false;
btAlignedObjectArray<Constraint>& constraints = m_constraints[c.m_node];
for (int j = 0; j < constraints.size(); ++j)
{
const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
btScalar dot_prod = dirs[0].dot(cti.m_normal);
if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
{
constraints[j].m_contact.push_back(&c);
constraints[j].m_direction.push_back(cti.m_normal);
constraints[j].m_value.push_back(0);
merged = true;
break;
}
}
const int dim = 3;
// hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim)
constraints.push_back(Constraint(c));
}
continue;
}
}
}
}
// for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions
const int dim = 3;
for (auto& it : m_constraints)
{
btAlignedObjectArray<Constraint>& c = it.second;
if (c.size() > dim)
{
btAlignedObjectArray<Constraint> prunedConstraints;
// always keep the first constrained direction
prunedConstraints.push_back(c[0]);
// find the direction most orthogonal to the first direction and keep it
size_t selected = 1;
btScalar min_dotProductAbs = std::abs(prunedConstraints[0].m_direction.dot(c[selected].m_direction));
for (int j = 2; j < c.size(); ++j)
{
btScalar dotProductAbs =std::abs(prunedConstraints[0].m_direction.dot(c[j].m_direction));
if (dotProductAbs < min_dotProductAbs)
{
selected = j;
min_dotProductAbs = dotProductAbs;
}
}
if (std::abs(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;
}
else
{
// // prune out collinear constraints
// const btVector3& first_dir = c[0].m_direction;
// int i = 1;
// while (i < c.size())
// {
// if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON)
// c.removeAtIndex(i);
// else
// ++i;
// }
// if (c.size() == 3)
// {
// if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON)
// c.removeAtIndex(2);
// }
}
}
}

58
src/BulletSoftBody/btContactProjection.h
View File

@@ -39,21 +39,20 @@ public:
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
if (friction.m_direction.norm() > SIMD_EPSILON)
x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
{
btVector3 dir = friction.m_direction.normalized();
x[i] -= x[i].dot(dir) * dir;
}
}
else if (constraints.size() == 2)
{
// TODO : friction
btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
if (free_dir.norm() < SIMD_EPSILON)
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
else
{
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
}
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
}
else
x[i].setZero();
@@ -73,32 +72,41 @@ public:
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
{
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
btVector3 diff = constraints[0].m_value * constraints[0].m_direction;
x[i] += diff;
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
for (int j = 0; j < constraints[0].m_direction.size(); ++j)
x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
if (friction.m_direction.norm() > SIMD_EPSILON)
{
x[i] -= x[i].dot(friction.m_direction) * friction.m_direction;
btVector3 dir = friction.m_direction.normalized();
x[i] -= x[i].dot(dir) * dir;
x[i] += friction.m_dv;
}
}
else if (constraints.size() == 2)
{
btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction);
if (free_dir.norm() < SIMD_EPSILON)
btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
for (int j = 0; j < constraints.size(); ++j)
{
x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction;
btVector3 diff = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction;
x[i] += diff;
}
else
{
free_dir.normalize();
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;
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
else
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;
{
x[i].setZero();
for (int j = 0; j < constraints.size(); ++j)
{
for (int k = 0; k < constraints[j].m_direction.size(); ++k)
{
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
}
}
}