From d7442cee218868ebdcfc81904bfdffa48173e634 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 31 Dec 2019 14:04:18 -0800 Subject: [PATCH] add strain rate limiting --- .../btDeformableBackwardEulerObjective.cpp | 4 +- .../btDeformableBackwardEulerObjective.h | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 4 +- src/BulletSoftBody/btDeformableBodySolver.h | 2 +- .../btDeformableContactConstraint.cpp | 87 ++++++++++++++----- .../btDeformableContactConstraint.h | 79 ++++++++--------- .../btDeformableContactProjection.cpp | 12 +-- .../btDeformableContactProjection.h | 2 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 2 +- src/BulletSoftBody/btSoftBodyInternals.h | 1 + 10 files changed, 116 insertions(+), 79 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 1b247641a..eb50f5a56 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -186,9 +186,9 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack } //set constraints as projections -void btDeformableBackwardEulerObjective::setConstraints() +void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal) { - m_projection.setConstraints(); + m_projection.setConstraints(infoGlobal); } void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 05ab42ff0..89cbb38c5 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -79,7 +79,7 @@ public: void updateVelocity(const TVStack& dv); //set constraints as projections - void setConstraints(); + void setConstraints(const btContactSolverInfo& infoGlobal); // update the projections and project the residual void project(TVStack& r) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 7724a8ec6..3972a48b2 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -228,10 +228,10 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArrayreinitialize(nodeUpdated, dt); } -void btDeformableBodySolver::setConstraints() +void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) { BT_PROFILE("setConstraint"); - m_objective->setConstraints(); + m_objective->setConstraints(infoGlobal); } btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies) diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index f78a8f696..bd09eda2f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -77,7 +77,7 @@ public: void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); // set up contact constraints - void setConstraints(); + void setConstraints(const btContactSolverInfo& infoGlobal); // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion virtual void predictMotion(btScalar solverdt); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 96fd8b5ae..c11b84464 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -15,9 +15,9 @@ #include "btDeformableContactConstraint.h" /* ================ Deformable Node Anchor =================== */ -btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a) +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal) : m_anchor(&a) -, btDeformableContactConstraint(a.m_cti.m_normal) +, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal) { } @@ -134,9 +134,9 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) } /* ================ Deformable vs. Rigid =================== */ -btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c) +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal) : m_contact(&c) -, btDeformableContactConstraint(c.m_cti.m_normal) +, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal) { m_total_normal_dv.setZero(); m_total_tangent_dv.setZero(); @@ -321,9 +321,9 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo } /* ================ Node vs. Rigid =================== */ -btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact) +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal) : m_node(contact.m_node) - , btDeformableRigidContactConstraint(contact) + , btDeformableRigidContactConstraint(contact, infoGlobal) { } @@ -359,9 +359,9 @@ void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& }; /* ================ Face vs. Rigid =================== */ -btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact) +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal) : m_face(contact.m_face) -, btDeformableRigidContactConstraint(contact) +, btDeformableRigidContactConstraint(contact, infoGlobal) { } @@ -413,19 +413,60 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul v1 -= dv * contact->m_weights[1]; if (im2 > 0) v2 -= dv * contact->m_weights[2]; - - // apply strain limiting to prevent undamped modes - btScalar m01 = (btScalar(1)/(im0 + im1)); - btScalar m02 = (btScalar(1)/(im0 + im2)); - btScalar m12 = (btScalar(1)/(im1 + im2)); - - btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); - btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); - btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); - - v0 += dv0; - v1 += dv1; - v2 += dv2; + + // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. + btScalar p = 0.01; + btVector3& x0 = face->m_n[0]->m_x; + btVector3& x1 = face->m_n[1]->m_x; + btVector3& x2 = face->m_n[2]->m_x; + const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1}; + const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1}; + btVector3 u[3]; + btScalar x_diff_dot_u, dn[3]; + btScalar dt = m_infoGlobal->m_timeStep; + for (int i = 0; i < 3; ++i) + { + btScalar x_diff_norm = x_diff[i].safeNorm(); + btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); + btScalar strainRate = x_diff_norm_new/x_diff_norm; + u[i] = v_diff[i]; + u[i].safeNormalize(); + if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p)) + { + dn[i] = 0; + continue; + } + x_diff_dot_u = btDot(x_diff[i], u[i]); + btScalar s; + if (1-p > strainRate) + { + s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm)); + } + else + { + s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm)); + } +// x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); +// strainRate = x_diff_norm_new/x_diff_norm; + dn[i] = s - v_diff[i].safeNorm(); + } + + btScalar relaxation = 0.5; + // apply strain limiting to prevent undamped modes + btScalar m01 = (relaxation/(im0 + im1)); + btScalar m02 = (relaxation/(im0 + im2)); + btScalar m12 = (relaxation/(im1 + im2)); + + // btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); + // btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); + // btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); + + btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1])); + btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2])); + btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1])); + v0 += dv0; + v1 += dv1; + v2 += dv2; } void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) @@ -449,11 +490,11 @@ void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& } /* ================ Face vs. Node =================== */ -btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact) +btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal) : m_node(contact.m_node) , m_face(contact.m_face) , m_contact(&contact) -, btDeformableContactConstraint(contact.m_normal) +, btDeformableContactConstraint(contact.m_normal, infoGlobal) { m_total_normal_dv.setZero(); m_total_tangent_dv.setZero(); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 912119e7c..8ede39a19 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -24,26 +24,28 @@ public: // True if the friction is static // False if the friction is dynamic bool m_static; - - // normal of the contact - btVector3 m_normal; - - btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal) - { - } - - btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal) - { - } - - btDeformableContactConstraint(const btDeformableContactConstraint& other) - : m_static(other.m_static) - , m_normal(other.m_normal) - { - - } - btDeformableContactConstraint(){} - + const btContactSolverInfo* m_infoGlobal; + + // normal of the contact + btVector3 m_normal; + + btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal) + { + } + + btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal) + { + } + + btDeformableContactConstraint(){} + + btDeformableContactConstraint(const btDeformableContactConstraint& other) + : m_static(other.m_static) + , m_normal(other.m_normal) + , m_infoGlobal(other.m_infoGlobal) + { + } + virtual ~btDeformableContactConstraint(){} // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence @@ -79,17 +81,14 @@ class btDeformableStaticConstraint : public btDeformableContactConstraint public: const btSoftBody::Node* m_node; - btDeformableStaticConstraint(){} - - btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0)) + btDeformableStaticConstraint(const btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal) { } - + btDeformableStaticConstraint(){} btDeformableStaticConstraint(const btDeformableStaticConstraint& other) : m_node(other.m_node) , btDeformableContactConstraint(other) { - } virtual ~btDeformableStaticConstraint(){} @@ -130,10 +129,10 @@ class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint { public: const btSoftBody::DeformableNodeRigidAnchor* m_anchor; - - btDeformableNodeAnchorConstraint(){} - btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c); + + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal); btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + btDeformableNodeAnchorConstraint(){} virtual ~btDeformableNodeAnchorConstraint() { } @@ -169,10 +168,10 @@ public: btVector3 m_total_tangent_dv; btScalar m_penetration; const btSoftBody::DeformableRigidContact* m_contact; - - btDeformableRigidContactConstraint(){} - btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c); + + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + btDeformableRigidContactConstraint(){} virtual ~btDeformableRigidContactConstraint() { } @@ -197,11 +196,10 @@ class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactCo public: // the deformable node in contact const btSoftBody::Node* m_node; - - btDeformableNodeRigidContactConstraint(){} - btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact); + + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal); btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); - + btDeformableNodeRigidContactConstraint(){} virtual ~btDeformableNodeRigidContactConstraint() { } @@ -228,10 +226,9 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo { public: const btSoftBody::Face* m_face; - btDeformableFaceRigidContactConstraint(){} - btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact); + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal); btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); - + btDeformableFaceRigidContactConstraint(){} virtual ~btDeformableFaceRigidContactConstraint() { } @@ -263,10 +260,8 @@ public: btVector3 m_total_normal_dv; btVector3 m_total_tangent_dv; - btDeformableFaceNodeContactConstraint(){} - - btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact); - + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal); + btDeformableFaceNodeContactConstraint(){} virtual ~btDeformableFaceNodeContactConstraint(){} virtual btScalar solveConstraint(); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 5a4f3241b..e1666fb86 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -108,7 +108,7 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI return residualSquare; } -void btDeformableContactProjection::setConstraints() +void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) { BT_PROFILE("setConstraints"); for (int i = 0; i < m_softBodies.size(); ++i) @@ -124,7 +124,7 @@ void btDeformableContactProjection::setConstraints() { if (psb->m_nodes[j].m_im == 0) { - btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); + btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal); m_staticConstraints[i].push_back(static_constraint); } } @@ -139,7 +139,7 @@ void btDeformableContactProjection::setConstraints() continue; } anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; - btDeformableNodeAnchorConstraint constraint(anchor); + btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal); m_nodeAnchorConstraints[i].push_back(constraint); } @@ -152,7 +152,7 @@ void btDeformableContactProjection::setConstraints() { continue; } - btDeformableNodeRigidContactConstraint constraint(contact); + btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; @@ -173,7 +173,7 @@ void btDeformableContactProjection::setConstraints() { continue; } - btDeformableFaceRigidContactConstraint constraint(contact); + btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; @@ -190,7 +190,7 @@ void btDeformableContactProjection::setConstraints() { const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; - btDeformableFaceNodeContactConstraint constraint(contact); + btDeformableFaceNodeContactConstraint constraint(contact, infoGlobal); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 3c4490765..618eea4c3 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -78,7 +78,7 @@ public: virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. - virtual void setConstraints(); + virtual void setConstraints(const btContactSolverInfo& infoGlobal); // Set up projections for each vertex by adding the projection direction to virtual void setProjection(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 618e5c0d7..d6de78612 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -280,7 +280,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::setupConstraints() { // set up constraints between multibody and deformable bodies - m_deformableBodySolver->setConstraints(); + m_deformableBodySolver->setConstraints(m_solverInfo); // set up constraints among multibodies { diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 894e80f38..055af1652 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1182,6 +1182,7 @@ struct btSoftColliders c.m_face = &f; // friction is handled by the nodes to prevent sticking const btScalar fc = 0; +// const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im;