diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 20ef961fb..d047e6d3d 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -62,37 +62,6 @@ struct DeformableContactConstraint { } }; -// -// -//struct DeformableFaceContactConstraint -//{ -// const btSoftBody::Face* m_face; -// const btSoftBody::FaceRContact* m_contact; -// btVector3 m_total_normal_dv; -// btVector3 m_total_tangent_dv; -// bool m_static; -// bool m_can_be_dynamic; -// -// DeformableFaceContactConstraint(const btSoftBody::FaceRContact& rcontact) -// : m_face(rcontact.m_face), -// m_contact(&rcontact), -// m_total_normal_dv(0,0,0), -// m_total_tangent_dv(0,0,0), -// m_static(false), -// m_can_be_dynamic(true) -// { -// } -// -// void replace(const btSoftBody::FaceRContact& rcontact) -// { -// m_contact = &rcontact; -// m_face = rcontact.m_face; -// } -// -// ~DeformableFaceContactConstraint() -// { -// } -//}; class btCGProjection { diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 67d04eb4b..38158dc09 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -37,7 +37,7 @@ public: virtual ~btConjugateGradient(){} // return the number of iterations taken - int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar relative_tolerance, bool verbose = false) + int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) { BT_PROFILE("CGSolve"); btAssert(x.size() == b.size()); @@ -50,7 +50,6 @@ public: A.precondition(r, z); A.project(z); btScalar r_dot_z = dot(z,r); -// btScalar local_tolerance = btMin(relative_tolerance * std::sqrt(r_dot_z), tolerance); btScalar local_tolerance = tolerance; if (std::sqrt(r_dot_z) <= local_tolerance) { if (verbose) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index d2add96db..fac85fbc1 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -76,7 +76,6 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) btScalar inner_product = computeDescentStep(m_ddv,m_residual); btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8 btScalar scale = 2; - // todo xuchenhan@: add damping energy to f0 and f1 btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2; backupDv(); do { @@ -152,9 +151,7 @@ void btDeformableBodySolver::updateEnergy(btScalar scale) btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual) { -// btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance))); - btScalar relative_tolerance = 0.5; - m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false); + m_cg.solve(*m_objective, ddv, residual, false); btScalar inner_product = m_cg.dot(residual, m_ddv); btScalar res_norm = m_objective->computeNorm(residual); btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv); @@ -197,10 +194,7 @@ void btDeformableBodySolver::updateDv(btScalar scale) void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - //btScalar tolerance = std::numeric_limits::epsilon() * m_objective->computeNorm(residual); -// btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance))); - btScalar relative_tolerance = 0.5; - m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false); + m_cg.solve(*m_objective, ddv, residual, false); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 17e96b634..ce5018f2f 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -193,6 +193,13 @@ btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* return m_total_normal_dv + m_total_tangent_dv; } +void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + contact->m_node->m_v -= dv; +} + /* ================ Face vs. Rigid =================== */ btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact) : m_face(contact.m_face) @@ -230,6 +237,19 @@ btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* return face_dv * contact->m_weights[2]; } +void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + if (face->m_n[0]->m_im > 0) + face->m_n[0]->m_v -= dv * contact->m_weights[0]; + if (face->m_n[1]->m_im > 0) + face->m_n[1]->m_v -= dv * contact->m_weights[1]; + if (face->m_n[2]->m_im > 0) + face->m_n[2]->m_v -= dv * contact->m_weights[2]; +} + /* ================ Face vs. Node =================== */ btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact) : m_node(contact.m_node) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 3eeef56b4..f79bc6065 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -17,6 +17,7 @@ #define BT_DEFORMABLE_CONTACT_CONSTRAINT_H #include "btSoftBody.h" +// btDeformableContactConstraint is an abstract class specifying the method that each type of contact constraint needs to implement class btDeformableContactConstraint { public: @@ -45,10 +46,8 @@ public: virtual ~btDeformableContactConstraint(){} - // solve the constraint with inelastic impulse and return the error, which is the square of normal component of dt scaled velocity diffrerence - // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse. - // if the object is rigid/multibody apply the impulse to change the velocity, - // if the object is deformable node, change the according dv. + // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence + // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact virtual btScalar solveConstraint() = 0; // get the velocity of the object A in the contact @@ -64,6 +63,8 @@ public: virtual void applyImpulse(const btVector3& impulse) = 0; }; +// +// Constraint that a certain node in the deformable objects cannot move class btDeformableStaticConstraint : public btDeformableContactConstraint { public: @@ -107,6 +108,8 @@ public: virtual void applyImpulse(const btVector3& impulse){} }; +// +// Constraint between rigid/multi body and deformable objects class btDeformableRigidContactConstraint : public btDeformableContactConstraint { public: @@ -121,17 +124,18 @@ public: { } - // object A is the rigid/multi body, and object B is the deformable node + // object A is the rigid/multi body, and object B is the deformable node/face virtual btVector3 getVa() const; virtual btScalar solveConstraint(); }; - - +// +// Constraint between rigid/multi body and deformable objects nodes class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint { public: + // the deformable node in contact const btSoftBody::Node* m_node; btDeformableNodeRigidContactConstraint(){} @@ -142,24 +146,23 @@ public: { } + // get the velocity of the deformable node in contact virtual btVector3 getVb() const; + // get the velocity change of the input soft body node in the constraint virtual btVector3 getDv(const btSoftBody::Node*) const; + // cast the contact to the desired type const btSoftBody::DeformableNodeRigidContact* getContact() const { return static_cast(m_contact); } - virtual void applyImpulse(const btVector3& impulse) - { - const btSoftBody::DeformableNodeRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - contact->m_node->m_v -= dv; - } + virtual void applyImpulse(const btVector3& impulse); }; - +// +// Constraint between rigid/multi body and deformable objects faces class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint { public: @@ -172,29 +175,23 @@ public: { } + // get the velocity of the deformable face at the contact point virtual btVector3 getVb() const; + // get the velocity change of the input soft body node in the constraint virtual btVector3 getDv(const btSoftBody::Node*) const; + // cast the contact to the desired type const btSoftBody::DeformableFaceRigidContact* getContact() const { return static_cast(m_contact); } - virtual void applyImpulse(const btVector3& impulse) - { - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; - btSoftBody::Face* face = contact->m_face; - if (face->m_n[0]->m_im > 0) - face->m_n[0]->m_v -= dv * contact->m_weights[0]; - if (face->m_n[1]->m_im > 0) - face->m_n[1]->m_v -= dv * contact->m_weights[1]; - if (face->m_n[2]->m_im > 0) - face->m_n[2]->m_v -= dv * contact->m_weights[2]; - } + virtual void applyImpulse(const btVector3& impulse); }; +// +// Constraint between deformable objects faces and deformable objects nodes class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint { public: @@ -218,9 +215,10 @@ public: // get the velocity of the object B in the contact virtual btVector3 getVb() const; - // get the velocity change of the soft body node in the constraint + // get the velocity change of the input soft body node in the constraint virtual btVector3 getDv(const btSoftBody::Node*) const; + // cast the contact to the desired type const btSoftBody::DeformableFaceNodeContact* getContact() const { return static_cast(m_contact); diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index cc935abeb..d9bef8b07 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -73,6 +73,7 @@ public: return BT_GRAVITY_FORCE; } + // the gravitational potential energy virtual double totalEnergy(btScalar dt) { double e = 0; diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index 53f06e4ea..f65569451 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -77,6 +77,7 @@ public: return numNodes; } + // add a soft body to be affected by the particular lagrangian force virtual void addSoftBody(btSoftBody* psb) { m_softBodies.push_back(psb); @@ -87,6 +88,7 @@ public: m_nodes = nodes; } + // Calculate the incremental deformable generated from the input dx virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) { btVector3 c1 = dx[id1] - dx[id0]; @@ -98,6 +100,7 @@ public: return dF; } + // Calculate the incremental deformable generated from the current velocity virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) { btVector3 c1 = n1->m_v - n0->m_v; diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 7371ea078..f201a4d24 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -27,10 +27,17 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { - m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body + // solve rigid/rigid in solver body + m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + // solver body velocity -> rigid body velocity solverBodyWriteBack(infoGlobal); + + // update rigid body velocity in rigid/deformable contact m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints()); + + // solver body velocity <- rigid body velocity writeToSolverBody(bodies, numBodies, infoGlobal); if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 80aafe03f..d28aa204e 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -31,6 +31,7 @@ protected: // override the iterations method to include deformable/multibody contact virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + // write the velocity of the the solver body to the underlying rigid body void solverBodyWriteBack(const btContactSolverInfo& infoGlobal) { for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index f4191bf76..8b6d80814 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -55,7 +55,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t beforeSolverCallbacks(timeStep); - ///solve deformable bodies constraints + ///solve contact constraints and then deformable bodies momemtum equation solveConstraints(timeStep); afterSolverCallbacks(timeStep); @@ -193,7 +193,11 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); - solveMultiBodyRelatedConstraints(); + + // solve contact constraints + solveContactConstraints(); + + // set up the directions in which the velocity does not change in the momentum solve m_deformableBodySolver->m_objective->m_projection.setProjection(); // for explicit scheme, m_backupVelocity = v_{n+1}^* @@ -202,6 +206,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) m_deformableBodySolver->setupDeformableSolve(m_implicit); // At this point, dv should be golden for nodes in contact + // proceed to solve deformable momentum equation m_deformableBodySolver->solveDeformableConstraints(timeStep); } @@ -217,7 +222,6 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints() btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; m_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); // build islands m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); @@ -243,7 +247,7 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints() } -void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints() +void btDeformableMultiBodyDynamicsWorld::solveContactConstraints() { // process constraints on each island m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 4cd1eaebc..b9059f0c6 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -152,7 +152,7 @@ public: void solveMultiBodyConstraints(); - void solveMultiBodyRelatedConstraints(); + void solveContactConstraints(); void sortConstraints(); diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 842855644..5957c77aa 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -18,6 +18,7 @@ subject to the following restrictions: #include "btDeformableLagrangianForce.h" +// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { public: @@ -48,6 +49,7 @@ public: addScaledElasticForce(scale, force); } + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search virtual void addScaledDampingForce(btScalar scale, TVStack& force) { if (m_mu_damp == 0 && m_lambda_damp == 0) @@ -101,6 +103,7 @@ public: return energy; } + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search virtual double totalDampingEnergy(btScalar dt) { double energy = 0; @@ -174,6 +177,7 @@ public: } } + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { if (m_mu_damp == 0 && m_lambda_damp == 0) @@ -251,6 +255,8 @@ public: P = s.m_F * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_cofF * (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); } + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { dP = dF * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); @@ -259,6 +265,8 @@ public: dP += s.m_cofF * m_lambda * DotProduct(s.m_cofF, dF); } + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { dP = dF * m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu_damp) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); @@ -277,6 +285,9 @@ public: return ans; } + // Let C(A) be the cofactor of the matrix A + // Let H = the derivative of C(A) with respect to A evaluated at F = A + // This function calculates H*dF void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) { M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 5e58fd89e..0c0bc4f64 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1457,8 +1457,7 @@ void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* p fs_write.close(); } - - +// Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary) { btVector3 vap = p - a; @@ -1513,6 +1512,8 @@ void btSoftBodyHelpers::readRenderMeshFromObj(const char* file, btSoftBody* psb) fs.close(); } +// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights +// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) { psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size()); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index e5b2e4016..1db6ab8ff 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -29,6 +29,8 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include //for memset #include + +// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, @@ -1068,7 +1070,7 @@ struct btSoftColliders if (!n.m_battach) { - // check for collision at x_{n+1}^* + // check for collision at x_{n+1}^* as well at x_n if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) { const btScalar ima = n.m_im; @@ -1175,11 +1177,14 @@ struct btSoftColliders btSoftBody::sCti& cti = c.m_cti; c.m_contactPoint = contact_point; c.m_bary = bary; - // todo xuchenhan@: check m_c2 and m_weights + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary; c.m_face = &f; 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; + c.m_c2 = ima; c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; @@ -1190,6 +1195,7 @@ struct btSoftColliders const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; const btVector3 ra = contact_point - wtr.getOrigin(); + // we do not scale the impulse matrix by dt c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; if (m_rigidBody) @@ -1307,6 +1313,7 @@ struct btSoftColliders if (l < SIMD_EPSILON) return; btVector3 rayEnd = dir.normalized() * (l + 2*mrg); + // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, face->m_n[0]->m_x-o, face->m_n[1]->m_x-o, face->m_n[2]->m_x-o, p, normal); if (intersect) @@ -1324,8 +1331,10 @@ struct btSoftColliders c.m_node = node; c.m_face = face; c.m_bary = w; + // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w; c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im; c.m_c0 = btScalar(1)/(ma + c.m_imf); psb[0]->m_faceNodeContacts.push_back(c);