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

improve dynamic friction

Browse Source
This commit is contained in:
Xuchen Han
2019-08-10 11:11:18 -07:00
parent fb6612c0be
commit fa5741d07e
7 changed files with 245 additions and 378 deletions
Download Patch File Download Diff File Expand all files Collapse all files

456
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -57,164 +57,129 @@ void btDeformableContactProjection::update()
// loop through constraints to set constrained values
for (int index = 0; index < m_constraints.size(); ++index)
{
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
const btSoftBody::Node* node = constraint.m_node;
for (int j = 0; j < constraint.m_contact.size(); ++j)
{
DeformableContactConstraint& constraint = constraints[i];
DeformableFrictionConstraint& friction = frictions[i];
for (int j = 0; j < constraint.m_contact.size(); ++j)
if (constraint.m_contact[j] == NULL)
{
if (constraint.m_contact[j] == NULL)
{
// nothing needs to be done for dirichelet constraints
continue;
}
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
// nothing needs to be done for dirichelet constraints
continue;
}
const btSoftBody::RContact* c = constraint.m_contact[j];
const btSoftBody::sCti& cti = c->m_cti;
if (cti.m_colObj->hasContactResponse())
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
const btScalar* deltaV_normal;
if (cti.m_colObj->hasContactResponse())
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
const btScalar* deltaV_normal;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
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)
{
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)
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
// add in the normal component of the va
btScalar vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_n[k];
}
va = cti.m_normal * vel * m_dt;
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t1[k];
}
va += c->t1 * vel * m_dt;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t2[k];
}
va += c->t2 * vel * m_dt;
vel += local_v[k] * J_n[k];
}
}
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);
const btVector3 impulse_tangent = impulse - impulse_normal;
// start friction handling
// copy old data
friction.m_impulse_prev[j] = friction.m_impulse[j];
friction.m_dv_prev[j] = friction.m_dv[j];
friction.m_static_prev[j] = friction.m_static[j];
friction.m_direction_prev[j] = friction.m_direction[j];
// get the current tangent direction
btScalar local_tangent_norm = impulse_tangent.norm();
btVector3 local_tangent_dir = -friction.m_direction[j];
if (local_tangent_norm > SIMD_EPSILON)
local_tangent_dir = impulse_tangent.normalized();
va = cti.m_normal * vel * m_dt;
// accumulated impulse on the rb in this and all prev cg iterations
constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j];
// the total tangential impulse required to stop sliding
btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent;
btScalar tangent_norm = tangent.norm();
if (accumulated_normal < 0)
{
friction.m_direction[j] = -local_tangent_dir;
// do not allow switching from static friction to dynamic friction
// it causes cg to explode
if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
// add in the tangential components of the va
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
friction.m_static[j] = false;
friction.m_impulse[j] = -accumulated_normal*c->m_c3;
vel += local_v[k] * J_t1[k];
}
va += c->t1 * vel * m_dt;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += local_v[k] * J_t2[k];
}
va += c->t2 * 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;
btVector3 old_total_tangent_dv = constraint.m_total_tangent_dv[j];
constraint.m_total_normal_dv[j] -= impulse_normal * node->m_im;
constraint.m_total_tangent_dv[j] -= impulse_tangent * node->m_im;
if (constraint.m_total_normal_dv[j].dot(cti.m_normal) < 0)
{
// separating in the normal direction
constraint.m_static[j] = false;
constraint.m_can_be_dynamic[j] = false;
constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
impulse_tangent.setZero();
}
else
{
if (constraint.m_can_be_dynamic[j] && constraint.m_total_normal_dv[j].norm() * c->m_c3 < constraint.m_total_tangent_dv[j].norm())
{
// dynamic friction
// with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
constraint.m_static[j] = false;
constraint.m_can_be_dynamic[j] = true;
if (constraint.m_total_tangent_dv[j].norm() < SIMD_EPSILON)
{
constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
}
else
{
friction.m_static[j] = true;
friction.m_impulse[j] = tangent_norm;
constraint.m_total_tangent_dv[j] = constraint.m_total_tangent_dv[j].normalized() * constraint.m_total_normal_dv[j].norm() * c->m_c3;
}
impulse_tangent = -btScalar(1)/node->m_im * (constraint.m_total_tangent_dv[j] - old_total_tangent_dv);
}
else
{
friction.m_released[j] = true;
friction.m_static[j] = false;
friction.m_impulse[j] = 0;
friction.m_direction[j] = btVector3(0,0,0);
// static friction
constraint.m_static[j] = true;
constraint.m_can_be_dynamic[j] = false;
}
friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt;
friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j];
// the incremental impulse applied to rb in the tangential direction
btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
// dv = new_impulse + accumulated velocity change in previous CG iterations
// so we have the invariant node->m_v = backupVelocity + dv;
btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
// the following is equivalent
/*
btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)];
btScalar dvn = dv.dot(cti.m_normal);
*/
constraint.m_value[j] = dvn;
// the incremental impulse:
// in the normal direction it's the normal component of "impulse"
// in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
impulse = impulse_normal + incremental_tangent;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
}
impulse = impulse_normal + impulse_tangent;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
rigidCol->applyImpulse(impulse, c->m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
if (multibodyLinkCol)
{
if (rigidCol)
rigidCol->applyImpulse(impulse, c->m_c1);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
if (multibodyLinkCol)
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
if (impulse_tangent.norm() > SIMD_EPSILON)
{
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
if (incremental_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2));
}
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2));
}
}
}
@@ -234,19 +199,7 @@ void btDeformableContactProjection::setConstraints()
{
if (psb->m_nodes[j].m_im == 0)
{
btAlignedObjectArray<DeformableContactConstraint> c;
c.reserve(3);
c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
m_constraints.insert(psb->m_nodes[j].index, c);
btAlignedObjectArray<DeformableFrictionConstraint> f;
f.reserve(3);
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
f.push_back(DeformableFrictionConstraint());
m_frictions.insert(psb->m_nodes[j].index, f);
m_constraints.insert(psb->m_nodes[j].index, DeformableContactConstraint());
}
}
}
@@ -301,41 +254,12 @@ void btDeformableContactProjection::setConstraints()
if (m_constraints.find(c.m_node->index) == NULL)
{
btAlignedObjectArray<DeformableContactConstraint> constraints;
constraints.push_back(DeformableContactConstraint(c));
m_constraints.insert(c.m_node->index,constraints);
btAlignedObjectArray<DeformableFrictionConstraint> frictions;
frictions.push_back(DeformableFrictionConstraint());
m_frictions.insert(c.m_node->index,frictions);
m_constraints.insert(c.m_node->index, DeformableContactConstraint(c));
}
else
{
// group colinear constraints into one
const btScalar angle_epsilon = 0.015192247; // less than 10 degree
bool merged = false;
btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints[c.m_node->index];
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[c.m_node->index];
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)
{
// group the constraints
constraints[j].append(c);
// push in an empty friction
frictions[j].append();
merged = true;
break;
}
}
const int dim = 3;
// hard coded no more than 3 constraint directions
if (!merged && constraints.size() < dim)
{
constraints.push_back(DeformableContactConstraint(c));
frictions.push_back(DeformableFrictionConstraint());
}
DeformableContactConstraint& constraints = *m_constraints[c.m_node->index];
constraints.append(c);
}
}
}
@@ -345,64 +269,15 @@ void btDeformableContactProjection::setConstraints()
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
const int dim = 3;
for (int index = 0; index < m_constraints.size(); ++index)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
x[i].setZero();
for (int j = 0; j < constraints.m_total_normal_dv.size(); ++j)
{
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];
}
else if (constraints.size() == 2)
{
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)
{
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].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];
}
}
}
// apply friction if the node is not constrained in all directions
if (constraints.size() < 3)
{
for (int f = 0; f < frictions.size(); ++f)
{
const DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// add the friction constraint
if (friction.m_static[j] == true)
{
// if (friction.m_static_prev[j] == true)
// x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
// x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
x[i] += friction.m_direction[j] * friction.m_dv[j];
}
}
}
x[i] += constraints.m_total_normal_dv[j];
x[i] += constraints.m_total_tangent_dv[j];
}
}
}
@@ -412,76 +287,80 @@ void btDeformableContactProjection::project(TVStack& x)
const int dim = 3;
for (int index = 0; index < m_constraints.size(); ++index)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
if (constraints.size() == 1)
if (constraints.m_contact[0] == NULL)
{
x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
// static node
x[i].setZero();
continue;
}
else if (constraints.size() == 2)
bool has_static = false;
for (int j = 0; j < constraints.m_static.size(); ++j)
{
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;
has_static = has_static || constraints.m_static[j];
}
// static friction => fully constrained
if (has_static)
{
x[i].setZero();
}
else if (constraints.m_total_normal_dv.size() >= dim)
{
x[i].setZero();
}
else if (constraints.m_total_normal_dv.size() == 2)
{
btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
btVector3 dir1 = (constraints.m_total_normal_dv[1].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[1].normalized() : btVector3(0,0,0);
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
x[i].setZero();
// apply the friction constraint
if (constraints.size() < 3)
{
for (int f = 0; f < frictions.size(); ++f)
{
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
if (friction.m_static[j] == true)
{
x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
}
}
}
btAssert(constraints.m_total_normal_dv.size() == 1);
btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
x[i] -= x[i].dot(dir0) * dir0;
}
}
}
void btDeformableContactProjection::projectFriction(TVStack& x)
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{
const int dim = 3;
for (int index = 0; index < m_constraints.size(); ++index)
{
const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
const btSoftBody::Node* node = constraint.m_node;
if (node == NULL)
continue;
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
btAssert(constraints.size() <= dim);
btAssert(constraints.size() > 0);
bool has_static_constraint = false;
// apply friction if the node is not constrained in all directions
if (constraints.size() < 3)
// apply dynamic friction force (scaled by dt) if the node does not have static friction constraint
for (int j = 0; j < constraint.m_static.size(); ++j)
{
bool has_static_constraint = false;
for (int f = 0; f < frictions.size(); ++f)
if (constraint.m_static[j])
{
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
has_static_constraint = true;
break;
}
for (int f = 0; f < frictions.size(); ++f)
}
for (int j = 0; j < constraint.m_total_tangent_dv.size(); ++j)
{
btVector3 friction_force = constraint.m_total_tangent_dv[j] * (1./node->m_im);
if (!has_static_constraint)
{
DeformableFrictionConstraint& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// only add to the rhs if there is no static friction constraint on the node
if (friction.m_static[j] == false)
{
if (!has_static_constraint)
x[i] += friction.m_direction[j] * friction.m_impulse[j];
}
}
f[i] += friction_force;
}
}
}
@@ -491,7 +370,6 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
btCGProjection::reinitialize(nodeUpdated);
m_constraints.clear();
m_frictions.clear();
}