From 696c96f392f41ee41045d345082032d6bfcebcd8 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 12 Jul 2019 10:03:38 -0700 Subject: bug fix in projection; start friction --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btCGProjection.h | 29 +++++++------ src/BulletSoftBody/btContactProjection.cpp | 56 ++++++++++++++++++++----- src/BulletSoftBody/btContactProjection.h | 14 ++++++- src/BulletSoftBody/btDeformableBodySolver.cpp | 1 + src/BulletSoftBody/btSoftBodyInternals.h | 3 +- 6 files changed, 75 insertions(+), 30 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index bdb781897..8801e68df 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -8,7 +8,7 @@ #include "btBackwardEulerObjective.h" btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) -: cg(50) +: cg(20) , m_softBodies(softBodies) , precondition(DefaultPreconditioner()) , projection(m_softBodies, m_dt) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index a238e998b..6c104ef2f 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -15,7 +15,7 @@ class btDeformableRigidDynamicsWorld; struct Constraint { const btSoftBody::RContact* m_contact; - const btVector3 m_direction; + btVector3 m_direction; btScalar m_value; Constraint(const btSoftBody::RContact& rcontact) @@ -36,7 +36,17 @@ struct Constraint { } - +}; + +struct Friction +{ + btVector3 m_dv; + btVector3 m_direction; + Friction() + { + m_dv.setZero(); + m_direction.setZero(); + } }; class btCGProjection @@ -49,11 +59,9 @@ public: btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; std::unordered_map m_indices; -// TVArrayStack m_constrainedDirections; -// TArrayStack m_constrainedValues; -// btAlignedObjectArray m_constrainedId; const btScalar& m_dt; std::unordered_map > m_constraints; + std::unordered_map m_frictions; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) @@ -77,17 +85,8 @@ public: { if (nodeUpdated) updateId(); - - // resize and clear the old constraints -// m_constrainedValues.resize(m_indices.size()); -// m_constrainedDirections.resize(m_indices.size()); -// for (int i = 0; i < m_constrainedDirections.size(); ++i) -// { -// m_constrainedDirections[i].clear(); -// m_constrainedValues[i].clear(); -// } -// m_constrainedId.clear(); m_constraints.clear(); + m_frictions.clear(); } void updateId() diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 3016a9e85..b90c749f3 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -7,14 +7,16 @@ #include "btContactProjection.h" #include "btDeformableRigidDynamicsWorld.h" +#include void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) { ///solve rigid body constraints m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values - for (auto it : m_constraints) + for (auto& it : m_constraints) { + Friction& friction = m_frictions[it.first]; btAlignedObjectArray& constraints = it.second; for (int i = 0; i < constraints.size(); ++i) { @@ -66,11 +68,27 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit const btVector3 vb = c->m_node->m_v * m_dt; const btVector3 vr = vb - va; const btScalar dn = btDot(vr, cti.m_normal); - if (1) // in the same CG solve, the set of constraits doesn't change - // if (dn <= SIMD_EPSILON) + 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 - const btVector3 impulse = c->m_c0 *(cti.m_normal * dn); + // TODO: only contact is considered here, add friction later // dv = new_impulse + accumulated velocity change in previous CG iterations @@ -82,7 +100,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { if (rigidCol) - rigidCol->applyImpulse(impulse, c->m_c1); + rigidCol->applyImpulse(impulse_normal, c->m_c1); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -102,7 +120,6 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit void btContactProjection::setConstraintDirections() { // set Dirichlet constraint - size_t counter = 0; for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -116,7 +133,6 @@ void btContactProjection::setConstraintDirections() c.push_back(Constraint(btVector3(0,0,1))); m_constraints[&(psb->m_nodes[j])] = c; } - ++counter; } } @@ -181,6 +197,7 @@ void btContactProjection::setConstraintDirections() btAlignedObjectArray constraints; constraints.push_back(Constraint(c)); m_constraints[c.m_node] = constraints; + m_frictions[c.m_node] = Friction(); } else { @@ -193,11 +210,10 @@ void btContactProjection::setConstraintDirections() } // for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions - counter = 0; const int dim = 3; - for (auto it : m_constraints) + for (auto& it : m_constraints) { - const btAlignedObjectArray& c = it.second; + btAlignedObjectArray& c = it.second; if (c.size() > dim) { btAlignedObjectArray prunedConstraints; @@ -216,7 +232,7 @@ void btContactProjection::setConstraintDirections() min_dotProductAbs = dotProductAbs; } } - if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON) + if (std::abs(std::abs(min_dotProductAbs)-1) < SIMD_EPSILON) { it.second = prunedConstraints; continue; @@ -240,5 +256,23 @@ void btContactProjection::setConstraintDirections() 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 a497c933c..29d0e6632 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -30,18 +30,22 @@ public: virtual void operator()(TVStack& x) { const int dim = 3; - for (auto it : m_constraints) + for (auto& it : m_constraints) { const btAlignedObjectArray& constraints = it.second; size_t i = m_indices[it.first]; + const Friction& friction = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) { x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; + if (friction.m_direction.norm() > SIMD_EPSILON) + x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; } else if (constraints.size() == 2) { + // TODO : friction btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); free_dir.normalize(); x[i] = x[i].dot(free_dir) * free_dir; @@ -55,16 +59,22 @@ public: virtual void enforceConstraint(TVStack& x) { const int dim = 3; - for (auto it : m_constraints) + for (auto& it : m_constraints) { const btAlignedObjectArray& constraints = it.second; size_t i = m_indices[it.first]; + const Friction& friction = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) { x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; x[i] += constraints[0].m_value * constraints[0].m_direction; + if (friction.m_direction.norm() > SIMD_EPSILON) + { + x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; + x[i] += friction.m_dv; + } } else if (constraints.size() == 2) { diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index b457a262f..b7c531385 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -122,6 +122,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) updateVelocity(); } advect(solverdt); +// postStabilize(); } void btDeformableBodySolver::reinitialize(bool nodeUpdated) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 41911b2bc..12d96171f 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -891,7 +891,8 @@ struct btSoftColliders c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); c.m_c1 = ra; c.m_c2 = ima * psb->m_sst.sdt; - c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; +// c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; psb->m_rcontacts.push_back(c); if (m_rigidBody) -- cgit v1.2.1