From fa5741d07edee8ca9466dc86c2ab030d2debb782 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 10 Aug 2019 11:11:18 -0700 Subject: [PATCH] improve dynamic friction --- src/BulletSoftBody/btCGProjection.h | 31 +- .../btDeformableBackwardEulerObjective.cpp | 4 +- .../btDeformableBackwardEulerObjective.h | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 8 +- .../btDeformableContactProjection.cpp | 456 +++++++----------- .../btDeformableContactProjection.h | 5 +- .../btDeformableRigidDynamicsWorld.cpp | 117 +++-- 7 files changed, 245 insertions(+), 378 deletions(-) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 0a4db2b49..ef902c70d 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -22,39 +22,30 @@ class btDeformableRigidDynamicsWorld; struct DeformableContactConstraint { + const btSoftBody::Node* m_node; btAlignedObjectArray m_contact; - btAlignedObjectArray m_direction; - btAlignedObjectArray m_value; - // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve - btAlignedObjectArray m_accumulated_normal_impulse; + btAlignedObjectArray m_total_normal_dv; + btAlignedObjectArray m_total_tangent_dv; + btAlignedObjectArray m_static; + btAlignedObjectArray m_can_be_dynamic; - DeformableContactConstraint(const btSoftBody::RContact& rcontact) + DeformableContactConstraint(const btSoftBody::RContact& rcontact): m_node(rcontact.m_node) { append(rcontact); } - DeformableContactConstraint(const btVector3& dir) + DeformableContactConstraint(): m_node(NULL) { m_contact.push_back(NULL); - m_direction.push_back(dir); - m_value.push_back(0); - m_accumulated_normal_impulse.push_back(0); - } - - DeformableContactConstraint() - { - m_contact.push_back(NULL); - m_direction.push_back(btVector3(0,0,0)); - m_value.push_back(0); - m_accumulated_normal_impulse.push_back(0); } void append(const btSoftBody::RContact& rcontact) { m_contact.push_back(&rcontact); - m_direction.push_back(rcontact.m_cti.m_normal); - m_value.push_back(0); - m_accumulated_normal_impulse.push_back(0); + m_total_normal_dv.push_back(btVector3(0,0,0)); + m_total_tangent_dv.push_back(btVector3(0,0,0)); + m_static.push_back(false); + m_can_be_dynamic.push_back(true); } ~DeformableContactConstraint() diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 09bb9ee2d..f4a3f55e5 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -146,7 +146,7 @@ void btDeformableBackwardEulerObjective::setConstraints() projection.setConstraints(); } -void btDeformableBackwardEulerObjective::projectFriction(TVStack& r) +void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) { - projection.projectFriction(r); + projection.applyDynamicFriction(r); } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 5de89849a..3863970c6 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -75,7 +75,7 @@ public: projection.enforceConstraint(x); } - void projectFriction(TVStack& r); + void applyDynamicFriction(TVStack& r); // add dv to velocity void updateVelocity(const TVStack& dv); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index daa85849b..be0262842 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -36,17 +36,17 @@ void btDeformableBodySolver::solveConstraints(float solverdt) // add constraints to the solver setConstraints(); - m_objective->computeResidual(solverdt, m_residual); - m_objective->projectFriction(m_residual); + m_objective->applyDynamicFriction(m_residual); computeStep(m_dv, m_residual); + updateVelocity(); } void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual) { - btScalar tolerance = std::numeric_limits::epsilon()* 1024 * m_objective->computeNorm(residual); + btScalar tolerance = std::numeric_limits::epsilon() * m_objective->computeNorm(residual); m_cg.solve(*m_objective, dv, residual, tolerance); } @@ -76,7 +76,7 @@ void btDeformableBodySolver::setConstraints() { BT_PROFILE("setConstraint"); m_objective->setConstraints(); - for (int i = 0; i < 1; ++i) + for (int i = 0; i < 10; ++i) { m_objective->projection.update(); m_objective->enforceConstraint(m_dv); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index ea88bee49..9bdd8323e 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -57,164 +57,129 @@ void btDeformableContactProjection::update() // loop through constraints to set constrained values for (int index = 0; index < m_constraints.size(); ++index) { - btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; - btAlignedObjectArray& 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 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 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 constraints; - constraints.push_back(DeformableContactConstraint(c)); - m_constraints.insert(c.m_node->index,constraints); - btAlignedObjectArray 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& constraints = *m_constraints[c.m_node->index]; - btAlignedObjectArray& frictions = *m_frictions[c.m_node->index]; - for (int j = 0; j < constraints.size(); ++j) - { - const btAlignedObjectArray& 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& constraints = *m_constraints.getAtIndex(index); + const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index); size_t i = m_constraints.getKeyAtIndex(index).getUid1(); - const btAlignedObjectArray& 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& constraints = *m_constraints.getAtIndex(index); - btAlignedObjectArray& 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& 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& 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(); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 0827554bb..331e1df6d 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -22,8 +22,7 @@ class btDeformableContactProjection : public btCGProjection { public: // map from node index to constraints - btHashMap > m_constraints; - btHashMap >m_frictions; + btHashMap m_constraints; btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : btCGProjection(softBodies, dt) @@ -37,7 +36,7 @@ public: // apply the constraints to the rhs virtual void project(TVStack& x); // add to friction - virtual void projectFriction(TVStack& x); + virtual void applyDynamicFriction(TVStack& f); // apply constraints to x in Ax=b virtual void enforceConstraint(TVStack& x); diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 80c77ada1..900ca9d8c 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -70,78 +70,77 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar timeStep) BT_PROFILE("positionCorrection"); for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) { - btAlignedObjectArray& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)]; - btAlignedObjectArray& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); - for (int i = 0; i < constraints.size(); ++i) + DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); + 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) + const btSoftBody::RContact* c = constraint.m_contact[j]; + // skip anchor points + if (c == NULL || c->m_node->m_im == 0) + continue; + const btSoftBody::sCti& cti = c->m_cti; + btVector3 va(0, 0, 0); + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - const btSoftBody::RContact* c = constraint.m_contact[j]; - // skip anchor points - if (c == NULL || c->m_node->m_im == 0) - continue; - const btSoftBody::sCti& cti = c->m_cti; - btVector3 va(0, 0, 0); - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) { - btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* 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(); + // 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(); - // 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; - - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += local_v[k] * J_t1[k]; - } - va += c->t1 * vel; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += local_v[k] * J_t2[k]; - } - va += c->t2 * vel; + vel += local_v[k] * J_n[k]; } - } - else - { - // The object interacting with deformable node is not supported for position correction - btAssert(false); - } - - if (cti.m_colObj->hasContactResponse()) - { - btScalar dp = cti.m_offset; + va = cti.m_normal * vel; - // only perform position correction when penetrating - if (dp < 0) + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_t1[k]; + } + va += c->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_t2[k]; + } + va += c->t2 * vel; + } + } + else + { + // The object interacting with deformable node is not supported for position correction + btAssert(false); + } + + if (cti.m_colObj->hasContactResponse()) + { + btScalar dp = cti.m_offset; + + // only perform position correction when penetrating + if (dp < 0) + { + if (constraint.m_static[j] == true) { if (friction.m_static[j] == true) { c->m_node->m_v = va; } c->m_node->m_v -= dp * cti.m_normal / timeStep; + c->m_node->m_v = va; } + c->m_node->m_v -= dp * cti.m_normal / timeStep; } } }