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

finished refactoring; start adding face contact

Browse Source
This commit is contained in:
Xuchen Han
2019-08-30 14:16:56 -07:00
parent f813cb1c88
commit f99cf56149
16 changed files with 937 additions and 355 deletions
Download Patch File Download Diff File Expand all files Collapse all files

416
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -20,148 +20,33 @@
btScalar btDeformableContactProjection::update()
{
btScalar residualSquare = 0;
btScalar max_impulse = 0;
// loop through constraints to set constrained values
for (int index = 0; index < m_constraints.size(); ++index)
// node constraints
for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
{
DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
const btSoftBody::Node* node = constraint.m_node;
for (int j = 0; j < constraint.m_contact.size(); ++j)
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
for (int i = 0; i < constraints.size(); ++i)
{
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;
if (cti.m_colObj->hasContactResponse())
{
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)
{
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();
const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
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]+local_dv[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]+local_dv[k]) * J_t1[k];
}
va += c->t1 * vel * m_dt;
vel = 0.0;
for (int k = 0; k < ndof; ++k)
{
vel += (local_v[k]+local_dv[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
{
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
{
// static friction
constraint.m_static[j] = true;
constraint.m_can_be_dynamic[j] = false;
}
}
impulse = impulse_normal + impulse_tangent;
max_impulse = btMax(impulse.length2(), max_impulse);
// dn is the normal component of velocity diffrerence. Approximates the residual.
residualSquare = btMax(residualSquare, dn*dn);
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)
{
// apply normal component of the impulse
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
if (impulse_tangent.norm() > SIMD_EPSILON)
{
// apply tangential component of the impulse
const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(c->t1));
const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(c->t2));
}
}
}
}
btScalar localResidualSquare = constraints[i].solveConstraint();
residualSquare = btMax(residualSquare, localResidualSquare);
}
}
// 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);
// }
// }
return residualSquare;
}
void btDeformableContactProjection::setConstraints()
{
BT_PROFILE("setConstraints");
@@ -173,132 +58,98 @@ void btDeformableContactProjection::setConstraints()
{
if (psb->m_nodes[j].m_im == 0)
{
m_constraints.insert(psb->m_nodes[j].index, DeformableContactConstraint());
btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint);
}
}
}
// set Deformable Node vs. Rigid constraint
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_rcontacts.size(); ++j)
for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
{
const btSoftBody::RContact& c = psb->m_rcontacts[j];
// skip anchor points
if (c.m_node->m_im == 0)
const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
// skip fixed points
if (contact.m_node->m_im == 0)
{
continue;
}
const btSoftBody::sCti& cti = c.m_cti;
if (cti.m_colObj->hasContactResponse())
btDeformableNodeRigidContactConstraint constraint(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)
{
btVector3 va(0, 0, 0);
btRigidBody* rigidCol = 0;
btMultiBodyLinkCollider* multibodyLinkCol = 0;
// grab the velocity of the rigid body
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
btAlignedObjectArray<btDeformableNodeRigidContactConstraint> constraintsList;
constraintsList.push_back(constraint);
m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
else
{
multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
if (multibodyLinkCol)
{
btScalar vel = 0.0;
const btScalar* jac = &c.jacobianData_normal.m_jacobians[0];
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
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);
if (dn < SIMD_EPSILON)
{
if (m_constraints.find(c.m_node->index) == NULL)
{
m_constraints.insert(c.m_node->index, DeformableContactConstraint(c));
}
else
{
DeformableContactConstraint& constraints = *m_constraints[c.m_node->index];
bool single_contact = true;
if (single_contact)
{
if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset)
{
constraints.replace(c);
}
}
else
{
constraints.append(c);
}
}
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[contact.m_node->index];
constraintsList.push_back(constraint);
}
}
}
}
// todo xuchenhan@: set Deformable Face vs. Rigid constraint
// todo xuchenhan@: set Deformable Face vs. Deformable Node
}
void btDeformableContactProjection::enforceConstraint(TVStack& x)
{
for (int index = 0; index < m_constraints.size(); ++index)
for (int i = 0; i < x.size(); ++i)
{
const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
x[i].setZero();
for (int j = 0; j < constraints.m_total_normal_dv.size(); ++j)
if (m_staticConstraints.find(i) != NULL)
{
x[i] += constraints.m_total_normal_dv[j];
x[i] += constraints.m_total_tangent_dv[j];
// 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));
}
}
// todo xuchenhan@
// if (m_faceRigidConstraints.find(i) != NULL)
// {
//
// }
}
}
void btDeformableContactProjection::project(TVStack& x)
{
const int dim = 3;
for (int index = 0; index < m_constraints.size(); ++index)
for (int index = 0; index < m_projectionsDict.size(); ++index)
{
const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
size_t i = m_constraints.getKeyAtIndex(index).getUid1();
if (constraints.m_contact[0] == NULL)
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;
}
bool has_static = false;
for (int j = 0; j < constraints.m_static.size(); ++j)
else if (projectionDirs.size() == 2)
{
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 dir0 = projectionDirs[0];
btVector3 dir1 = projectionDirs[1];
btVector3 free_dir = btCross(dir0, dir1);
if (free_dir.norm() < SIMD_EPSILON)
{
@@ -313,48 +164,129 @@ void btDeformableContactProjection::project(TVStack& x)
}
else
{
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);
btAssert(projectionDirs.size() == 1);
btVector3 dir0 = projectionDirs[0];
x[i] -= x[i].dot(dir0) * dir0;
}
}
}
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
void btDeformableContactProjection::setProjection()
{
for (int index = 0; index < m_constraints.size(); ++index)
for (int i = 0; i < m_softBodies.size(); ++i)
{
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();
bool has_static_constraint = false;
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
int index = psb->m_nodes[j].index;
bool hasConstraint = false;
bool existStaticConstraint = false;
btVector3 averagedNormal(0,0,0);
btAlignedObjectArray<btVector3> normals;
if (m_staticConstraints.find(index) != NULL)
{
existStaticConstraint = true;
hasConstraint = true;
}
// accumulate normals
if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL)
{
hasConstraint = true;
btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[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;
// add another projection direction if it deviates from the average by more than about 15 degrees
normals.push_back(local_normal);
averagedNormal += local_normal;
}
}
// 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)
{
if (constraint.m_static[j])
// todo: xuchenhan@ implement face
// if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
// {
// }
// build projections
if (!hasConstraint)
{
has_static_constraint = true;
break;
continue;
}
}
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)
btAlignedObjectArray<btVector3> projections;
if (existStaticConstraint)
{
f[i] += friction_force;
projections.push_back(btVector3(1,0,0));
projections.push_back(btVector3(0,1,0));
projections.push_back(btVector3(0,0,1));
}
else
{
bool averageExists = (averagedNormal.length2() > SIMD_EPSILON);
averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0);
if (averageExists)
{
projections.push_back(averagedNormal);
}
for (int k = 0; k < normals.size(); ++k)
{
const btVector3& local_normal = normals[k];
// add another projection direction if it deviates from the average by more than about 15 degrees
if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25)
{
projections.push_back(local_normal);
}
}
}
m_projectionsDict.insert(index, projections);
}
}
}
void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
{
// for (int index = 0; index < m_constraints.size(); ++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();
// bool has_static_constraint = false;
//
// // 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)
// {
// if (constraint.m_static[j])
// {
// has_static_constraint = true;
// break;
// }
// }
// 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)
// {
// f[i] += friction_force;
// }
// }
// }
}
void btDeformableContactProjection::reinitialize(bool nodeUpdated)
{
btCGProjection::reinitialize(nodeUpdated);
m_constraints.clear();
m_staticConstraints.clear();
m_nodeRigidConstraints.clear();
// m_faceRigidConstraints.clear();
}