From 1bfb226be8fb9eb490877f193b8f2f153a1e82bc Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 16 Sep 2019 16:03:36 -0700 Subject: [PATCH] add support for deformable vs. deformable contact --- examples/DeformableDemo/DeformableContact.cpp | 8 ++ examples/DeformableDemo/DeformableContact.h | 13 ++ .../btDeformableBackwardEulerObjective.h | 7 - src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- .../btDeformableContactConstraint.cpp | 129 +++++++++++++++++- .../btDeformableContactConstraint.h | 42 +++++- .../btDeformableContactProjection.cpp | 126 +++++++++-------- .../btDeformableContactProjection.h | 9 +- .../btDeformableMultiBodyConstraintSolver.cpp | 3 +- src/BulletSoftBody/btSoftBody.cpp | 28 +++- src/BulletSoftBody/btSoftBody.h | 17 ++- src/BulletSoftBody/btSoftBodyInternals.h | 118 ++++++++++++++++ 12 files changed, 426 insertions(+), 76 deletions(-) create mode 100644 examples/DeformableDemo/DeformableContact.cpp create mode 100644 examples/DeformableDemo/DeformableContact.h diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp new file mode 100644 index 000000000..b2a0c9efa --- /dev/null +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -0,0 +1,8 @@ +// +// DeformableContact.cpp +// App_BulletExampleBrowser +// +// Created by Xuchen Han on 9/16/19. +// + +#include "DeformableContact.hpp" diff --git a/examples/DeformableDemo/DeformableContact.h b/examples/DeformableDemo/DeformableContact.h new file mode 100644 index 000000000..3183bbedc --- /dev/null +++ b/examples/DeformableDemo/DeformableContact.h @@ -0,0 +1,13 @@ +// +// DeformableContact.hpp +// App_BulletExampleBrowser +// +// Created by Xuchen Han on 9/16/19. +// + +#ifndef DeformableContact_hpp +#define DeformableContact_hpp + +#include + +#endif /* DeformableContact_hpp */ diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 284ec6908..cc6d118b7 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -71,13 +71,6 @@ public: void setDt(btScalar dt); - // enforce constraints in contact solve - void enforceConstraint(TVStack& x) - { - BT_PROFILE("enforceConstraint"); - projection.enforceConstraint(x); - } - void applyDynamicFriction(TVStack& r); // add dv to velocity diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 7342c26a6..0df93534c 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -409,7 +409,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d /* Clear contacts */ psb->m_nodeRigidContacts.resize(0); psb->m_faceRigidContacts.resize(0); - psb->m_scontacts.resize(0); + psb->m_faceNodeContacts.resize(0); /* Optimize dbvt's */ psb->m_ndbvt.optimizeIncremental(1); psb->m_fdbvt.optimizeIncremental(1); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 8ee2806af..17e96b634 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -196,14 +196,12 @@ btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* /* ================ Face vs. Rigid =================== */ btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact) : m_face(contact.m_face) -, m_solved(false) , btDeformableRigidContactConstraint(contact) { } btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other) : m_face(other.m_face) -, m_solved(false) , btDeformableRigidContactConstraint(other) { } @@ -231,3 +229,130 @@ btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* btAssert(node == m_face->m_n[2]); return face_dv * contact->m_weights[2]; } + +/* ================ Face vs. Node =================== */ +btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact) +: m_node(contact.m_node) +, m_face(contact.m_face) +, m_contact(&contact) +, btDeformableContactConstraint(contact.m_normal) +{ + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); +} + +btVector3 btDeformableFaceNodeContactConstraint::getVa() const +{ + return m_node->m_v; +} + +btVector3 btDeformableFaceNodeContactConstraint::getVb() const +{ + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; + return vb; +} + +btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const +{ + btVector3 dv = m_total_normal_dv + m_total_tangent_dv; + if (n == m_node) + return dv; + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + if (m_face->m_n[0] == n) + { + return dv * contact->m_weights[0]; + } + if (m_face->m_n[1] == n) + { + return dv * contact->m_weights[1]; + } + btAssert(n == m_face->m_n[2]); + return dv * contact->m_weights[2]; +} + +btScalar btDeformableFaceNodeContactConstraint::solveConstraint() +{ + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + const btScalar dn = btDot(vr, m_contact->m_normal); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn*dn; + btVector3 impulse = m_contact->m_c0 * vr; + const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + + btVector3 old_total_tangent_dv = m_total_tangent_dv; + // m_c2 is the inverse mass of the deformable node/face + if (m_node->m_im > 0) + { + m_total_normal_dv -= impulse_normal * m_node->m_im; + m_total_tangent_dv -= impulse_tangent * m_node->m_im; + } + else + { + m_total_normal_dv -= impulse_normal * m_contact->m_imf; + m_total_tangent_dv -= impulse_tangent * m_contact->m_imf; + } + + if (m_total_normal_dv.dot(m_contact->m_normal) < 0) + { + // separating in the normal direction + m_static = false; + m_total_tangent_dv = btVector3(0,0,0); + impulse_tangent.setZero(); + } + else + { + if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.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. + m_static = false; + if (m_total_tangent_dv.norm() < SIMD_EPSILON) + { + m_total_tangent_dv = btVector3(0,0,0); + } + else + { + m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_friction; + } + impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + return residualSquare; +} + +void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceNodeContact* contact = getContact(); + btVector3 dva = impulse * contact->m_node->m_im; + btVector3 dvb = impulse * contact->m_imf; + if (contact->m_node->m_im > 0) + { + contact->m_node->m_v += dva; + } + + btSoftBody::Face* face = contact->m_face; + if (face->m_n[0]->m_im > 0) + { + face->m_n[0]->m_v -= dvb * contact->m_weights[0]; + } + if (face->m_n[1]->m_im > 0) + { + face->m_n[1]->m_v -= dvb * contact->m_weights[1]; + } + if (face->m_n[2]->m_im > 0) + { + face->m_n[2]->m_v -= dvb * contact->m_weights[2]; + } +} diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index b88f6e908..3eeef56b4 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -59,6 +59,9 @@ public: // get the velocity change of the soft body node in the constraint virtual btVector3 getDv(const btSoftBody::Node*) const = 0; + + // apply impulse to the soft body node and/or face involved + virtual void applyImpulse(const btVector3& impulse) = 0; }; class btDeformableStaticConstraint : public btDeformableContactConstraint @@ -100,6 +103,8 @@ public: { return btVector3(0,0,0); } + + virtual void applyImpulse(const btVector3& impulse){} }; class btDeformableRigidContactConstraint : public btDeformableContactConstraint @@ -120,8 +125,6 @@ public: virtual btVector3 getVa() const; virtual btScalar solveConstraint(); - - virtual void applyImpulse(const btVector3& impulse) = 0; }; @@ -161,7 +164,6 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo { public: const btSoftBody::Face* m_face; - bool m_solved; btDeformableFaceRigidContactConstraint(){} btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact); btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); @@ -192,4 +194,38 @@ public: face->m_n[2]->m_v -= dv * contact->m_weights[2]; } }; + +class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint +{ +public: + btSoftBody::Node* m_node; + btSoftBody::Face* m_face; + const btSoftBody::DeformableFaceNodeContact* m_contact; + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + + btDeformableFaceNodeContactConstraint(){} + + btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact); + + virtual ~btDeformableFaceNodeContactConstraint(){} + + virtual btScalar solveConstraint(); + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const; + + // 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 + virtual btVector3 getDv(const btSoftBody::Node*) const; + + const btSoftBody::DeformableFaceNodeContact* getContact() const + { + return static_cast(m_contact); + } + + virtual void applyImpulse(const btVector3& impulse); +}; #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index f98c73538..408f73b88 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -35,7 +35,7 @@ btScalar btDeformableContactProjection::update() // face constraints for (int index = 0; index < m_allFaceConstraints.size(); ++index) { - btDeformableFaceRigidContactConstraint* constraint = m_allFaceConstraints[index]; + btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; btScalar localResidualSquare = constraint->solveConstraint(); residualSquare = btMax(residualSquare, localResidualSquare); } @@ -140,66 +140,64 @@ void btDeformableContactProjection::setConstraints() delete constraint; } } - } - // todo xuchenhan@: set Deformable Face vs. Deformable Node -} - -void btDeformableContactProjection::enforceConstraint(TVStack& x) -{ - // x is set to zero when passed in - // loop through node constraints to add in contributions to dv - for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) - { - btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints.getAtIndex(index); - // i is node index - int i = m_nodeRigidConstraints.getKeyAtIndex(index).getUid1(); - int numConstraints = 1; -// int numConstraints = constraintsList.size(); -// if (m_faceRigidConstraints.find(i) != NULL) -// { -// numConstraints += m_faceRigidConstraints[i]->size(); -// } - for (int j = 0; j < constraintsList.size(); ++j) + // set Deformable Face vs. Deformable Node constraint + for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j) { - const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j]; - x[i] += constraint.getDv(constraint.getContact()->m_node)/btScalar(numConstraints); - } - } - - // loop through face constraints to add in contributions to dv - // note that for each face constraint is owned by three nodes. Be careful here to only add the dv to the node that owns the constraint - for (int index = 0; index < m_faceRigidConstraints.size(); ++index) - { - btAlignedObjectArray& constraintsList = *m_faceRigidConstraints.getAtIndex(index); - // i is node index - int i = m_faceRigidConstraints.getKeyAtIndex(index).getUid1(); - for (int j = 0; j < constraintsList.size(); ++j) - { - const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j]; - const btSoftBody::Face* face = constraint->m_face; - btSoftBody::Node* node; - // find the node that owns the constraint - for (int k = 0; k < 3; ++k) + const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j]; + + btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact); + btVector3 va = constraint->getVa(); + btVector3 vb = constraint->getVb(); + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, contact.m_normal); + if (dn > -SIMD_EPSILON) { - if (face->m_n[k]->index == i) + btSoftBody::Node* node = contact.m_node; + btSoftBody::Face* face = contact.m_face; + m_allFaceConstraints.push_back(constraint); + if (node->m_im != 0) { - node = face->m_n[k]; - break; + if (m_deformableConstraints.find(node->index) == NULL) + { + btAlignedObjectArray constraintsList; + constraintsList.push_back(constraint); + m_deformableConstraints.insert(node->index, constraintsList); + } + else + { + btAlignedObjectArray& constraintsList = *m_deformableConstraints[node->index]; + constraintsList.push_back(constraint); + } + } + + // add face constraints to each of the nodes + for (int k = 0; k < 3; ++k) + { + btSoftBody::Node* node = face->m_n[k]; + // static node does not need to own face/rigid constraint + if (node->m_im != 0) + { + if (m_deformableConstraints.find(node->index) == NULL) + { + btAlignedObjectArray constraintsList; + constraintsList.push_back(constraint); + m_deformableConstraints.insert(node->index, constraintsList); + } + else + { + btAlignedObjectArray& constraintsList = *m_deformableConstraints[node->index]; + constraintsList.push_back(constraint); + } + } } } - x[i] += constraint->getDv(node); + else + { + delete constraint; + } } } - // todo xuchenhan@: add deformable deformable constraints' contribution to dv - - // Finally, loop through static constraints to set dv of static nodes to zero - for (int index = 0; index < m_staticConstraints.size(); ++index) - { - const btDeformableStaticConstraint& constraint = *m_staticConstraints.getAtIndex(index); - int i = m_staticConstraints.getKeyAtIndex(index).getUid1(); - x[i] = constraint.getDv(constraint.m_node); - } } void btDeformableContactProjection::project(TVStack& x) @@ -258,7 +256,7 @@ void btDeformableContactProjection::setProjection() hasConstraint = true; } - // accumulate normals + // accumulate normals from Deformable Node vs. Rigid constraints if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL) { hasConstraint = true; @@ -276,6 +274,7 @@ void btDeformableContactProjection::setProjection() } } + // accumulate normals from Deformable Face vs. Rigid constraints if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL) { hasConstraint = true; @@ -293,6 +292,24 @@ void btDeformableContactProjection::setProjection() } } + // accumulate normals from Deformable Node vs. Deformable Face constraints + if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL) + { + hasConstraint = true; + btAlignedObjectArray& constraintsList = *m_deformableConstraints[index]; + for (int k = 0; k < constraintsList.size(); ++k) + { + if (constraintsList[k]->m_static) + { + existStaticConstraint = true; + break; + } + const btVector3& local_normal = constraintsList[k]->m_normal; + normals.push_back(local_normal); + averagedNormal += local_normal; + } + } + // build projections if (!hasConstraint) @@ -388,6 +405,7 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_staticConstraints.clear(); m_nodeRigidConstraints.clear(); m_faceRigidConstraints.clear(); + m_deformableConstraints.clear(); m_projectionsDict.clear(); for (int i = 0; i < m_allFaceConstraints.size(); ++i) { diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index f8e5e9bbf..57a7ecd94 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -31,7 +31,11 @@ public: btHashMap > m_nodeRigidConstraints; // map from node index to face rigid constraint btHashMap > m_faceRigidConstraints; - btAlignedObjectArray m_allFaceConstraints; + // map from node index to deformable constraint + btHashMap > m_deformableConstraints; + + // all constraints involving face + btAlignedObjectArray m_allFaceConstraints; // map from node index to projection directions btHashMap > m_projectionsDict; @@ -51,9 +55,6 @@ public: // add to friction virtual void applyDynamicFriction(TVStack& f); - // apply constraints to x in Ax=b - virtual void enforceConstraint(TVStack& x); - // update the constraints virtual btScalar update(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 80a79c978..7371ea078 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -23,7 +23,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - m_maxOverrideNumSolverIterations = 150; +// m_maxOverrideNumSolverIterations = 150; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { @@ -39,7 +39,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif m_analyticsData.m_numSolverCalls++; - std::cout << "Contact Residual = " << m_leastSquaresResidual << std::endl; m_analyticsData.m_numIterationsUsed = iteration+1; m_analyticsData.m_islandId = -2; if (numBodies>0) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 0c4f50206..6a2f005b1 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -89,6 +89,7 @@ void btSoftBody::initDefaults() m_cfg.diterations = 0; m_cfg.citerations = 4; m_cfg.collisions = fCollision::Default; + m_cfg.collisions |= fCollision::VF_DD; m_pose.m_bvolume = false; m_pose.m_bframe = false; m_pose.m_volume = 0; @@ -1861,8 +1862,7 @@ void btSoftBody::predictMotion(btScalar dt) } } /* Clear contacts */ - m_nodeRigidContacts.resize(0); - m_faceRigidContacts.resize(0); + m_rcontacts.resize(0); m_scontacts.resize(0); /* Optimize dbvt's */ m_ndbvt.optimizeIncremental(1); @@ -3484,6 +3484,30 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } } break; + case fCollision::VF_DD: + { + // self-collision not supported yet + if (this != psb) + { + btSoftColliders::CollideVF_DD docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + } + break; default: { } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index f6be6655e..67c0d3448 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -163,10 +163,11 @@ public: SDF_RD = 0x0003, ///DF based rigid vs deformable SDF_RDF = 0x0004, ///DF based rigid vs deformable faces - SVSmask = 0x0030, ///Rigid versus soft mask + SVSmask = 0x00F0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling CL_SELF = 0x0040, ///Cluster soft body self collision + VF_DD = 0x0050, ///Vertex vs face soft vs soft handling /* presets */ Default = SDF_RS, END @@ -362,6 +363,19 @@ public: btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; }; + struct DeformableFaceNodeContact + { + Node* m_node; // Node + Face* m_face; // Face + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + btVector3 m_normal; // Normal + btScalar m_margin; // Margin + btScalar m_friction; // Friction + btScalar m_imf; // inverse mass of the face at contact point + btScalar m_c0; // scale of the impulse matrix; + }; + /* SContact */ struct SContact { @@ -759,6 +773,7 @@ public: tAnchorArray m_anchors; // Anchors tRContactArray m_rcontacts; // Rigid contacts btAlignedObjectArray m_nodeRigidContacts; + btAlignedObjectArray m_faceNodeContacts; btAlignedObjectArray m_faceRigidContacts; tSContactArray m_scontacts; // Soft contacts tJointArray m_joints; // Joints diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 9c0121d1b..e5b2e4016 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -501,6 +501,77 @@ static inline void ProjectOrigin(const btVector3& a, } } +// +static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t) +{ + btScalar a, f, u, v; + + btVector3 e1 = v1 - v0; + btVector3 e2 = v2 - v0; + btVector3 h = dir.cross(e2); + a = e1.dot(h); + + if (a > -0.00001 && a < 0.00001) + return (false); + + f = btScalar(1) / a; + btVector3 s = origin - v0; + u = f * s.dot(h); + + if (u < 0.0 || u > 1.0) + return (false); + + btVector3 q = s.cross(e1); + v = f * dir.dot(q); + if (v < 0.0 || u + v > 1.0) + return (false); + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * e2.dot(q); + if (t > 0) // ray intersection + return (true); + else // this means that there is a line intersection + // but not a ray intersection + return (false); +} + +static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal) +{ + btVector3 dir = rayEnd - rayStart; + btScalar dir_norm = dir.norm(); + if (dir_norm < SIMD_EPSILON) + return false; + dir.normalize(); + + btScalar t; + + bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); + + if (ret) + { + if (t <= dir_norm) + { + sect = rayStart + dir * t; + } + else + { + ret = false; + } + } + + if (ret) + { + btVector3 n = (p3-p1).cross(p2-p1); + n.safeNormalize(); + if (n.dot(dir) < 0) + normal = n; + else + normal = -n; + } + return ret; +} + + // template static inline T BaryEval(const T& a, @@ -1217,6 +1288,53 @@ struct btSoftColliders btSoftBody* psb[2]; btScalar mrg; }; + + // + // CollideVF_DD + // + struct CollideVF_DD : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 o = node->m_x; + btVector3 p, normal; + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + btVector3 dir = node->m_q - o; + btScalar l = dir.length(); + if (l < SIMD_EPSILON) + return; + btVector3 rayEnd = dir.normalized() * (l + 2*mrg); + 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) + { + p += o; + const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p); + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + const btScalar ms = ma + mb; + if (ms > 0) + { + btSoftBody::DeformableFaceNodeContact c; + c.m_normal = normal; + c.m_margin = mrg; + c.m_node = node; + c.m_face = face; + c.m_bary = w; + c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w; + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + 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); + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + }; }; #endif //_BT_SOFT_BODY_INTERNALS_H