diff options
author | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-07-15 14:52:24 -0700 |
---|---|---|
committer | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-08-02 13:12:51 -0700 |
commit | befab4eab6ac8ef8aea141d8e89a463648e9de41 (patch) | |
tree | d1ed2abb3955abee728c6f245e5ceca80a5350d2 | |
parent | bac7d461c5a9d318a04b9741cccf7f1759132bf3 (diff) | |
download | bullet3-befab4eab6ac8ef8aea141d8e89a463648e9de41.tar.gz |
reorganize the contact constraints
-rw-r--r-- | src/BulletSoftBody/btCGProjection.h | 32 | ||||
-rw-r--r-- | src/BulletSoftBody/btContactProjection.cpp | 240 | ||||
-rw-r--r-- | src/BulletSoftBody/btContactProjection.h | 56 |
3 files changed, 148 insertions, 180 deletions
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 6c104ef2f..91621e49b 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -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(); } }; diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 6082f4dad..9388ac7e4 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -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) + 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) { - 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) + 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) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + 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; } - 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 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; - 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; - - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON) { - if (rigidCol) - rigidCol->applyImpulse(impulse_normal, c->m_c1); + 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]]); } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + impulse = impulse_normal + impulse_tangent; + + // if (1) // in the same CG solve, the set of constraits doesn't change + if (dn <= SIMD_EPSILON) { - if (multibodyLinkCol) + // 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) { - double multiplier = 0.5; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + 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); -// } - } - } } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 8ce3cb578..1e5c52a60 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -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; + for (int k = 0; k < constraints[j].m_direction.size(); ++k) + { + x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; + } } - else + } + else + { + x[i].setZero(); + for (int j = 0; j < constraints.size(); ++j) { - 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; } } |