From f99cf56149c105acf9a9c03bdd47c4e76ebbd6be Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 30 Aug 2019 14:16:56 -0700 Subject: [PATCH] finished refactoring; start adding face contact --- src/BulletSoftBody/CMakeLists.txt | 2 + src/BulletSoftBody/btCGProjection.h | 40 +- src/BulletSoftBody/btConjugateGradient.h | 2 +- .../btDeformableBackwardEulerObjective.cpp | 19 +- .../btDeformableBackwardEulerObjective.h | 12 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 55 ++- .../btDeformableContactConstraint.cpp | 190 ++++++++ .../btDeformableContactConstraint.h | 150 +++++++ .../btDeformableContactProjection.cpp | 416 ++++++++---------- .../btDeformableContactProjection.h | 17 +- .../btDeformableCorotatedForce.h | 4 +- .../btDeformableMultiBodyConstraintSolver.cpp | 1 + .../btDeformableMultiBodyDynamicsWorld.cpp | 151 ++++--- src/BulletSoftBody/btSoftBody.cpp | 86 +++- src/BulletSoftBody/btSoftBody.h | 40 ++ src/BulletSoftBody/btSoftBodyInternals.h | 107 ++++- 16 files changed, 937 insertions(+), 355 deletions(-) create mode 100644 src/BulletSoftBody/btDeformableContactConstraint.cpp create mode 100644 src/BulletSoftBody/btDeformableContactConstraint.h diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 8c1e9c022..408d5b5d8 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -22,6 +22,7 @@ SET(BulletSoftBody_SRCS btDeformableMultiBodyConstraintSolver.cpp btDeformableContactProjection.cpp btDeformableMultiBodyDynamicsWorld.cpp + btDeformableContactConstraint.cpp ) @@ -54,6 +55,7 @@ SET(BulletSoftBody_HDRS btDeformableMultiBodyConstraintSolver.h btDeformableContactProjection.h btDeformableMultiBodyDynamicsWorld.h + btDeformableContactConstraint.h btSoftBodySolverVertexBuffer.h ) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 9055ad5ce..20ef961fb 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -20,8 +20,6 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" -//class btDeformableMultiBodyDynamicsWorld; - struct DeformableContactConstraint { const btSoftBody::Node* m_node; @@ -64,6 +62,37 @@ 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 { @@ -73,6 +102,8 @@ public: typedef btAlignedObjectArray > TArrayStack; btAlignedObjectArray& m_softBodies; const btScalar& m_dt; + // map from node indices to node pointers + const btAlignedObjectArray* m_nodes; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) @@ -95,6 +126,11 @@ public: virtual void reinitialize(bool nodeUpdated) { } + + virtual void setIndices(const btAlignedObjectArray* nodes) + { + m_nodes = nodes; + } }; diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 830667448..0236d9941 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -51,7 +51,7 @@ public: A.project(z); btScalar r_dot_z = dot(z,r); btScalar local_tolerance = btMin(relative_tolerance * std::sqrt(r_dot_z), tolerance); - if (std::sqrt(r_dot_z) < local_tolerance) { + if (std::sqrt(r_dot_z) <= local_tolerance) { if (verbose) { std::cout << "Iteration = 0" << std::endl; diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index a83b5d2ad..29801ab1d 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -47,6 +47,7 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar m_lf[i]->reinitialize(nodeUpdated); } projection.reinitialize(nodeUpdated); + projection.setIndices(getIndices()); m_preconditioner->reinitialize(nodeUpdated); } @@ -84,11 +85,21 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { - // only the velocity of the constrained nodes needs to be updated during contact solve - for (int i = 0; i < projection.m_constraints.size(); ++i) +// // only the velocity of the constrained nodes needs to be updated during contact solve +// for (int i = 0; i < projection.m_constraints.size(); ++i) +// { +// int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); +// m_nodes[index]->m_v = m_backupVelocity[index] + dv[index]; +// } + + for (int i = 0; i < m_softBodies.size(); ++i) { - int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); - m_nodes[index]->m_v = m_backupVelocity[index] + dv[index]; + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + node.m_v = m_backupVelocity[node.index] + dv[node.index]; + } } } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index c57f2e097..b61f2235f 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -101,16 +101,22 @@ public: virtual void updateId() { - size_t id = 0; + size_t node_id = 0; + size_t face_id = 0; m_nodes.clear(); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - psb->m_nodes[j].index = id; + psb->m_nodes[j].index = node_id; m_nodes.push_back(&psb->m_nodes[j]); - ++id; + ++node_id; + } + for (int j = 0; j < psb->m_faces.size(); ++j) + { + psb->m_faces[j].m_index = face_id; + ++face_id; } } } diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index b85255220..9be65c031 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -16,6 +16,7 @@ #include #include #include "btDeformableBodySolver.h" +#include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" btDeformableBodySolver::btDeformableBodySolver() @@ -23,11 +24,7 @@ btDeformableBodySolver::btDeformableBodySolver() , m_cg(20) , m_maxNewtonIterations(5) , m_newtonTolerance(1e-4) -//, m_lineSearch(false) -//, m_cg(10) -//, m_maxNewtonIterations(5) -//, m_newtonTolerance(1e-3) -, m_lineSearch(true) +, m_lineSearch(false) { m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity); } @@ -155,7 +152,7 @@ void btDeformableBodySolver::updateEnergy(btScalar scale) btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual) { - btScalar relative_tolerance = btMin(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance))); + btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance))); m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false); btScalar inner_product = m_cg.dot(residual, m_ddv); btScalar tol = 1e-5 * m_objective->computeNorm(residual) * m_objective->computeNorm(m_ddv); @@ -199,7 +196,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(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance))); + btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance))); m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false); } @@ -237,8 +234,18 @@ void btDeformableBodySolver::setConstraints() btScalar btDeformableBodySolver::solveContactConstraints() { BT_PROFILE("setConstraint"); + for (int i = 0; i < m_dv.size();++i) + { + m_dv[i].setZero(); + } btScalar maxSquaredResidual = m_objective->projection.update(); m_objective->enforceConstraint(m_dv); +// std::cout << "=================" << std::endl; +// for (int i = 0; i < m_dv.size(); ++i) +// { +// std::cout << m_dv[i].getX() << " " << m_dv[i].getY() << " " << m_dv[i].getZ() << std::endl; +// } + m_objective->updateVelocity(m_dv); return maxSquaredResidual; } @@ -302,7 +309,7 @@ void btDeformableBodySolver::backupVn() // Here: // dv = 0 for nodes not in constraints // dv = v_{n+1} - v_{n+1}^* for nodes in constraints - if (m_objective->projection.m_constraints.find(psb->m_nodes[j].index)!=NULL) + if (m_objective->projection.m_projectionsDict.find(psb->m_nodes[j].index)!=NULL) { m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn; } @@ -361,6 +368,18 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d { int i, ni; + /* Update */ + if (psb->m_bUpdateRtCst) + { + psb->m_bUpdateRtCst = false; + psb->updateConstants(); + psb->m_fdbvt.clear(); + if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RDF) + { + psb->initializeFaceTree(); + } + } + /* Prepare */ psb->m_sst.sdt = dt * psb->m_cfg.timescale; psb->m_sst.isdt = 1 / psb->m_sst.sdt; @@ -388,11 +407,29 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d psb->m_sst.updmrg); } + if (!psb->m_fdbvt.empty()) + { + for (int i = 0; i < psb->m_faces.size(); ++i) + { + btSoftBody::Face& f = psb->m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, psb->m_sst.radmrg); + psb->m_fdbvt.update(f.m_leaf, + vol, + v * psb->m_sst.velmrg, + psb->m_sst.updmrg); + } + } /* Clear contacts */ - psb->m_rcontacts.resize(0); + psb->m_nodeRigidContacts.resize(0); + psb->m_faceRigidContacts.resize(0); psb->m_scontacts.resize(0); /* Optimize dbvt's */ psb->m_ndbvt.optimizeIncremental(1); + psb->m_fdbvt.optimizeIncremental(1); } void btDeformableBodySolver::updateSoftBodies() diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp new file mode 100644 index 000000000..ce7cd257c --- /dev/null +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -0,0 +1,190 @@ +/* + Written by Xuchen Han + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "btDeformableContactConstraint.h" + +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c) +: m_contact(&c) +, btDeformableContactConstraint(c.m_cti.m_normal) +{ + m_total_normal_dv.setZero(); + m_total_tangent_dv.setZero(); +} + +btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) +: m_contact(other.m_contact) +, btDeformableContactConstraint(other) +{ + m_total_normal_dv = other.m_total_normal_dv; + m_total_tangent_dv = other.m_total_tangent_dv; +} + + +btVector3 btDeformableRigidContactConstraint::getVa() const +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // 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(m_contact->m_c1)) : 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 = &m_contact->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_t1[k]; + } + va += m_contact->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_t2[k]; + } + va += m_contact->t2 * vel; + } + } + } + return va; +} + +btScalar btDeformableRigidContactConstraint::solveConstraint() +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.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 * (cti.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 + m_total_normal_dv -= impulse_normal * m_contact->m_c2; + m_total_tangent_dv -= impulse_tangent * m_contact->m_c2; + + if (m_total_normal_dv.dot(cti.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_c3 < 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_c3; + } + impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); + } + else + { + // static friction + m_static = true; + } + } + impulse = impulse_normal + impulse_tangent; + + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + if (impulse_tangent.norm() > SIMD_EPSILON) + { + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1)); + const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2)); + } + } + } + return residualSquare; +} + +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact) + : m_node(contact.m_node) + , btDeformableRigidContactConstraint(contact) + { + } + +btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other) +: m_node(other.m_node) +, btDeformableRigidContactConstraint(other) +{ +} + +btVector3 btDeformableNodeRigidContactConstraint::getVb() const +{ + return m_node->m_v; +} + + +btVector3 btDeformableNodeRigidContactConstraint::getDv(btSoftBody::Node* node) const +{ + return m_total_normal_dv + m_total_tangent_dv; +} diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h new file mode 100644 index 000000000..4f266f72f --- /dev/null +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -0,0 +1,150 @@ +/* + Written by Xuchen Han + + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef BT_DEFORMABLE_CONTACT_CONSTRAINT_H +#define BT_DEFORMABLE_CONTACT_CONSTRAINT_H +#include "btSoftBody.h" + +class btDeformableContactConstraint +{ +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(){} + + 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. + virtual btScalar solveConstraint() = 0; + + // get the velocity of the object A in the contact + virtual btVector3 getVa() const = 0; + + // get the velocity of the object B in the contact + virtual btVector3 getVb() const = 0; + + // get the velocity change of the soft body node in the constraint + virtual btVector3 getDv(btSoftBody::Node*) const = 0; +}; + +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 btDeformableStaticConstraint& other) + : m_node(other.m_node) + , btDeformableContactConstraint(other) + { + + } + + virtual ~btDeformableStaticConstraint(){} + + virtual btScalar solveConstraint() + { + return 0; + } + + virtual btVector3 getVa() const + { + return btVector3(0,0,0); + } + + virtual btVector3 getVb() const + { + return btVector3(0,0,0); + } + + virtual btVector3 getDv(btSoftBody::Node* n) const + { + return btVector3(0,0,0); + } +}; + +class btDeformableRigidContactConstraint : public btDeformableContactConstraint +{ +public: + btVector3 m_total_normal_dv; + btVector3 m_total_tangent_dv; + const btSoftBody::DeformableRigidContact* m_contact; + + btDeformableRigidContactConstraint(){} + btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c); + btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other); + virtual ~btDeformableRigidContactConstraint() + { + } + + // object A is the rigid/multi body, and object B is the deformable node + virtual btVector3 getVa() const; + + virtual btScalar solveConstraint(); +}; + + + +class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint +{ +public: + const btSoftBody::Node* m_node; + + btDeformableNodeRigidContactConstraint(){} + btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact); + btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other); + + virtual ~btDeformableNodeRigidContactConstraint() + { + + } + + virtual btVector3 getVb() const; + + virtual btVector3 getDv(btSoftBody::Node*) const; + + const btSoftBody::DeformableNodeRigidContact* getContact() const + { + return static_cast(m_contact); + } +}; +#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 404eda494..3f09f58cb 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -20,148 +20,33 @@ btScalar btDeformableContactProjection::update() { btScalar residualSquare = 0; - btScalar max_impulse = 0; // loop through constraints to set constrained values - for (int index = 0; index < m_constraints.size(); ++index) + + // node constraints + for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) { - DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index); - const btSoftBody::Node* node = constraint.m_node; - for (int j = 0; j < constraint.m_contact.size(); ++j) + btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); + for (int i = 0; i < constraints.size(); ++i) { - 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; - - if (cti.m_colObj->hasContactResponse()) - { - 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) - { - 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(); - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - 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]+local_dv[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]+local_dv[k]) * J_t1[k]; - } - va += c->t1 * vel * m_dt; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += (local_v[k]+local_dv[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 - { - 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 - { - // static friction - constraint.m_static[j] = true; - constraint.m_can_be_dynamic[j] = false; - } - } - impulse = impulse_normal + impulse_tangent; - max_impulse = btMax(impulse.length2(), max_impulse); - - // dn is the normal component of velocity diffrerence. Approximates the residual. - residualSquare = btMax(residualSquare, dn*dn); - 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) - { - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); - if (impulse_tangent.norm() > SIMD_EPSILON) - { - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(c->t1)); - const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(c->t2)); - } - } - } - } + btScalar localResidualSquare = constraints[i].solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); } } + + // face constraints +// for (int index = 0; index < m_faceRigidConstraints.size(); ++index) +// { +// btAlignedObjectArray& constraints = *m_faceRigidConstraints.getAtIndex(index); +// for (int i = 0; i < constraints.size(); ++i) +// { +// btScalar localResidualSquare = constraints[i].solveConstraint(); +// residualSquare = btMax(residualSquare, localResidualSquare); +// } +// } return residualSquare; } + void btDeformableContactProjection::setConstraints() { BT_PROFILE("setConstraints"); @@ -173,132 +58,98 @@ void btDeformableContactProjection::setConstraints() { if (psb->m_nodes[j].m_im == 0) { - m_constraints.insert(psb->m_nodes[j].index, DeformableContactConstraint()); + btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]); + m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint); } } } + + // set Deformable Node vs. Rigid constraint for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; btMultiBodyJacobianData jacobianData_normal; btMultiBodyJacobianData jacobianData_complementary; - for (int j = 0; j < psb->m_rcontacts.size(); ++j) + for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { - const btSoftBody::RContact& c = psb->m_rcontacts[j]; - // skip anchor points - if (c.m_node->m_im == 0) + const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; + // skip fixed points + if (contact.m_node->m_im == 0) { continue; } - - const btSoftBody::sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) + btDeformableNodeRigidContactConstraint constraint(contact); + btVector3 va = constraint.getVa(); + btVector3 vb = constraint.getVb(); + const btVector3 vr = vb - va; + const btSoftBody::sCti& cti = contact.m_cti; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn < SIMD_EPSILON) { - btVector3 va(0, 0, 0); - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL) { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0); + btAlignedObjectArray constraintsList; + constraintsList.push_back(constraint); + m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList); } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + else { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - btScalar vel = 0.0; - const btScalar* jac = &c.jacobianData_normal.m_jacobians[0]; - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - for (int j = 0; j < ndof; ++j) - { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; - } - va = cti.m_normal * 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); - if (dn < SIMD_EPSILON) - { - - if (m_constraints.find(c.m_node->index) == NULL) - { - m_constraints.insert(c.m_node->index, DeformableContactConstraint(c)); - } - else - { - DeformableContactConstraint& constraints = *m_constraints[c.m_node->index]; - bool single_contact = true; - if (single_contact) - { - if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset) - { - constraints.replace(c); - } - } - else - { - constraints.append(c); - } - } + btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[contact.m_node->index]; + constraintsList.push_back(constraint); } } } } + + // todo xuchenhan@: set Deformable Face vs. Rigid constraint + + // todo xuchenhan@: set Deformable Face vs. Deformable Node } void btDeformableContactProjection::enforceConstraint(TVStack& x) { - for (int index = 0; index < m_constraints.size(); ++index) + for (int i = 0; i < x.size(); ++i) { - const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index); - size_t i = m_constraints.getKeyAtIndex(index).getUid1(); x[i].setZero(); - for (int j = 0; j < constraints.m_total_normal_dv.size(); ++j) + if (m_staticConstraints.find(i) != NULL) { - x[i] += constraints.m_total_normal_dv[j]; - x[i] += constraints.m_total_tangent_dv[j]; + // if a node is fixed, dv = 0 + continue; } + if (m_nodeRigidConstraints.find(i) != NULL) + { + btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[i]; + for (int j = 0; j < constraintsList.size(); ++j) + { + const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j]; + x[i] += constraint.getDv(m_nodes->at(i)); + } + } + // todo xuchenhan@ +// if (m_faceRigidConstraints.find(i) != NULL) +// { +// +// } } } void btDeformableContactProjection::project(TVStack& x) { const int dim = 3; - for (int index = 0; index < m_constraints.size(); ++index) + for (int index = 0; index < m_projectionsDict.size(); ++index) { - const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index); - size_t i = m_constraints.getKeyAtIndex(index).getUid1(); - if (constraints.m_contact[0] == NULL) + btAlignedObjectArray& projectionDirs = *m_projectionsDict.getAtIndex(index); + size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); + if (projectionDirs.size() >= dim) { // static node x[i].setZero(); continue; } - bool has_static = false; - for (int j = 0; j < constraints.m_static.size(); ++j) + else if (projectionDirs.size() == 2) { - 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 dir0 = projectionDirs[0]; + btVector3 dir1 = projectionDirs[1]; btVector3 free_dir = btCross(dir0, dir1); if (free_dir.norm() < SIMD_EPSILON) { @@ -313,48 +164,129 @@ void btDeformableContactProjection::project(TVStack& x) } else { - 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); + btAssert(projectionDirs.size() == 1); + btVector3 dir0 = projectionDirs[0]; x[i] -= x[i].dot(dir0) * dir0; } } } -void btDeformableContactProjection::applyDynamicFriction(TVStack& f) +void btDeformableContactProjection::setProjection() { - for (int index = 0; index < m_constraints.size(); ++index) + for (int i = 0; i < m_softBodies.size(); ++i) { - 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(); - bool has_static_constraint = false; + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + int index = psb->m_nodes[j].index; + bool hasConstraint = false; + bool existStaticConstraint = false; + btVector3 averagedNormal(0,0,0); + btAlignedObjectArray normals; + if (m_staticConstraints.find(index) != NULL) + { + existStaticConstraint = true; + hasConstraint = true; + } + + // accumulate normals + if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL) + { + hasConstraint = true; + btAlignedObjectArray& constraintsList = *m_nodeRigidConstraints[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; + // add another projection direction if it deviates from the average by more than about 15 degrees + normals.push_back(local_normal); + averagedNormal += local_normal; + } + } - // 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) - { - if (constraint.m_static[j]) + // todo: xuchenhan@ implement face +// if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL) +// { +// } + + + // build projections + if (!hasConstraint) { - has_static_constraint = true; - break; + continue; } - } - 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) + btAlignedObjectArray projections; + if (existStaticConstraint) { - f[i] += friction_force; + projections.push_back(btVector3(1,0,0)); + projections.push_back(btVector3(0,1,0)); + projections.push_back(btVector3(0,0,1)); } + else + { + bool averageExists = (averagedNormal.length2() > SIMD_EPSILON); + averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0); + if (averageExists) + { + projections.push_back(averagedNormal); + } + for (int k = 0; k < normals.size(); ++k) + { + const btVector3& local_normal = normals[k]; + // add another projection direction if it deviates from the average by more than about 15 degrees + if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25) + { + projections.push_back(local_normal); + } + } + } + m_projectionsDict.insert(index, projections); } } } + +void btDeformableContactProjection::applyDynamicFriction(TVStack& f) +{ +// for (int index = 0; index < m_constraints.size(); ++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(); +// bool has_static_constraint = false; +// +// // 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) +// { +// if (constraint.m_static[j]) +// { +// has_static_constraint = true; +// break; +// } +// } +// 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) +// { +// f[i] += friction_force; +// } +// } +// } +} + void btDeformableContactProjection::reinitialize(bool nodeUpdated) { btCGProjection::reinitialize(nodeUpdated); - m_constraints.clear(); + m_staticConstraints.clear(); + m_nodeRigidConstraints.clear(); +// m_faceRigidConstraints.clear(); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 774da1ae3..e62bc3245 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -19,12 +19,20 @@ #include "btSoftBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "btDeformableContactConstraint.h" #include "LinearMath/btHashMap.h" class btDeformableContactProjection : public btCGProjection { public: - // map from node index to constraints - btHashMap m_constraints; + // map from node index to static constraint + btHashMap m_staticConstraints; + // map from node index to node rigid constraint + btHashMap > m_nodeRigidConstraints; +// // map from node index to face rigid constraint +// btHashMap > m_faceRigidConstraints; + + // map from node index to projection directions + btHashMap > m_projectionsDict; btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : btCGProjection(softBodies, dt) @@ -37,6 +45,7 @@ public: // apply the constraints to the rhs virtual void project(TVStack& x); + // add to friction virtual void applyDynamicFriction(TVStack& f); @@ -46,8 +55,12 @@ public: // update the constraints virtual btScalar update(); + // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. virtual void setConstraints(); + // Set up projections for each vertex by adding the projection direction to + virtual void setProjection(); + virtual void reinitialize(bool nodeUpdated); }; #endif /* btDeformableContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableCorotatedForce.h b/src/BulletSoftBody/btDeformableCorotatedForce.h index 13f64ccfa..c2a26338e 100644 --- a/src/BulletSoftBody/btDeformableCorotatedForce.h +++ b/src/BulletSoftBody/btDeformableCorotatedForce.h @@ -19,7 +19,7 @@ #include "btDeformableLagrangianForce.h" #include "LinearMath/btPolarDecomposition.h" -static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) +static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) { static const btPolarDecomposition polar; return polar.decompose(m, q, s); @@ -100,7 +100,7 @@ public: if (J < 1024 * SIMD_EPSILON) R.setIdentity(); else - PolarDecompose(F, R, S); // this QR is not robust, consider using implicit shift svd + PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ P += (F-R) * 2 * m_mu; } diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 65ba43e04..fb69143bd 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -23,6 +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; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index cc54549a1..0eae6a26a 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -70,78 +70,82 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t // /////////////////////////////// } +//void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep) +//{ +// // perform position correction for all constraints +// BT_PROFILE("positionCorrection"); +// for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) +// { +// DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); +// 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) +// { +// 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) +// { +// 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; +// } +// } +// 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) +// { +// c->m_node->m_v -= dp * cti.m_normal / timeStep; +// } +// } +// } +// } +//} + void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep) { - // perform position correction for all constraints - BT_PROFILE("positionCorrection"); - for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) - { - DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); - 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) - { - 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) - { - 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; - } - } - 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) - { - c->m_node->m_v -= dp * cti.m_normal / timeStep; - } - } - } - } } @@ -187,7 +191,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); solveMultiBodyRelatedConstraints(); - + m_deformableBodySolver->m_objective->projection.setProjection(); if (m_implicit) { // at this point dv = v_{n+1} - v_{n+1}^* @@ -321,11 +325,6 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) m_internalTime += timeStep; m_deformableBodySolver->setImplicit(m_implicit); m_deformableBodySolver->reinitialize(m_softBodies, timeStep); -// if (m_implicit) -// { -// // todo: backup v_n velocity somewhere else -// m_deformableBodySolver->backupVelocity(); -// } btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c94ed6fca..c9ab0c66f 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -20,6 +20,9 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include // btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) : m_softBodySolver(0), m_worldInfo(worldInfo) @@ -1858,7 +1861,8 @@ void btSoftBody::predictMotion(btScalar dt) } } /* Clear contacts */ - m_rcontacts.resize(0); + m_nodeRigidContacts.resize(0); + m_faceRigidContacts.resize(0); m_scontacts.resize(0); /* Optimize dbvt's */ m_ndbvt.optimizeIncremental(1); @@ -2289,6 +2293,7 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, } return (false); } + // bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, @@ -2321,6 +2326,63 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr return (false); } +// +// Compute barycentric coordinates (u, v, w) for +// point p with respect to triangle (a, b, c) +static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary) +{ + btVector3 v0 = b - a, v1 = c - a, v2 = p - a; + btScalar d00 = v0.dot(v0); + btScalar d01 = v0.dot(v1); + btScalar d11 = v1.dot(v1); + btScalar d20 = v2.dot(v0); + btScalar d21 = v2.dot(v1); + btScalar denom = d00 * d11 - d01 * d01; + bary.setY((d11 * d20 - d01 * d21) / denom); + bary.setZ((d00 * d21 - d01 * d20) / denom); + bary.setX(btScalar(1) - bary.getY() - bary.getZ()); +} + +// +bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, + const Face& f, + btVector3& contact_point, + btVector3& bary, + btScalar margin, + btSoftBody::sCti& cti, bool predict) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n + btTransform wtr = (predict) ? + (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + : colObjWrap->getWorldTransform(); + + btGjkEpaSolver2::sResults results; + btTransform triangle_transform; + triangle_transform.setIdentity(); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x); + btVector3 guess(0,0,0); + const btConvexShape* csh = static_cast(shp); + btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); + btScalar dst = results.distance; + contact_point = results.witnesses[0]; + getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + if (!predict) + { + cti.m_colObj = colObjWrap->getCollisionObject(); +// cti.m_normal = wtr.getBasis() * results.normal; + cti.m_normal = results.normal; + cti.m_offset = dst; + } + if (dst < 0) + return true; + return (false); +} + // void btSoftBody::updateNormals() { @@ -3339,7 +3401,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap break; case fCollision::SDF_RD: { - btSoftColliders::CollideSDF_RD docollide; + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); btTransform wtr = pcoWrap->getWorldTransform(); @@ -3355,13 +3417,21 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap maxs); volume = btDbvtVolume::FromMM(mins, maxs); volume.Expand(btVector3(basemargin, basemargin, basemargin)); - docollide.psb = this; - docollide.m_colObj1Wrap = pcoWrap; - docollide.m_rigidBody = prb1; + btSoftColliders::CollideSDF_RD docollideNode; + docollideNode.psb = this; + docollideNode.m_colObj1Wrap = pcoWrap; + docollideNode.m_rigidBody = prb1; + docollideNode.dynmargin = basemargin + timemargin; + docollideNode.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); - docollide.dynmargin = basemargin + timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide); + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; + docollideFace.dynmargin = basemargin + timemargin; + docollideFace.stamargin = basemargin; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); } break; } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index b8139c5e9..9d3402834 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -161,6 +161,7 @@ public: SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft SDF_RD = 0x0003, ///DF based rigid vs deformable + SDF_RDF = 0x0004, ///DF based rigid vs deformable faces SVSmask = 0x0030, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling @@ -217,6 +218,7 @@ public: const btCollisionObject* m_colObj; /* Rigid body */ btVector3 m_normal; /* Outward normal */ btScalar m_offset; /* Offset from origin */ + btVector3 m_bary; /* Barycentric weights for faces */ }; /* sMedium */ @@ -283,6 +285,7 @@ public: btVector3 m_normal; // Normal btScalar m_ra; // Rest area btDbvtNode* m_leaf; // Leaf data + int m_index; }; /* Tetra */ struct Tetra : Feature @@ -315,6 +318,40 @@ public: btVector3 t1; btVector3 t2; }; + + class DeformableRigidContact + { + public: + sCti m_cti; // Contact infos + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // inverse mass of node/face + btScalar m_c3; // Friction + btScalar m_c4; // Hardness + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; + }; + + class DeformableNodeRigidContact : public DeformableRigidContact + { + public: + Node* m_node; // Owner node + }; + + class DeformableFaceRigidContact : public DeformableRigidContact + { + public: + Face* m_face; // Owner face + btVector3 m_contactPoint; // Contact point + btVector3 m_bary; // Barycentric weights + btVector3 m_weights; // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v; + }; + /* SContact */ struct SContact { @@ -710,6 +747,8 @@ public: tTetraArray m_tetras; // Tetras tAnchorArray m_anchors; // Anchors tRContactArray m_rcontacts; // Rigid contacts + btAlignedObjectArray m_nodeRigidContacts; + btAlignedObjectArray m_faceRigidContacts; tSContactArray m_scontacts; // Soft contacts tJointArray m_joints; // Joints tMaterialArray m_materials; // Materials @@ -1017,6 +1056,7 @@ public: void initializeFaceTree(); btVector3 evaluateCom() const; bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, const Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 8ce114fa8..54cf354b5 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -993,7 +993,7 @@ struct btSoftColliders void DoNode(btSoftBody::Node& n) const { const btScalar m = n.m_im > 0 ? dynmargin : stamargin; - btSoftBody::RContact c; + btSoftBody::DeformableNodeRigidContact c; if (!n.m_battach) { @@ -1010,7 +1010,7 @@ struct btSoftColliders btSoftBody::sCti& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_c2 = ima * psb->m_sst.sdt; + c.m_c2 = ima; c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; @@ -1021,7 +1021,7 @@ struct btSoftColliders const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; const btVector3 ra = n.m_x - wtr.getOrigin(); - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; if (m_rigidBody) m_rigidBody->activate(); @@ -1051,8 +1051,7 @@ struct btSoftColliders t1.getX(), t1.getY(), t1.getZ(), t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btScalar dt = psb->m_sst.sdt; - btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); c.m_c0 = rot.transpose() * local_impulse_matrix * rot; c.jacobianData_normal = jacobianData_normal; c.jacobianData_t1 = jacobianData_t1; @@ -1061,7 +1060,7 @@ struct btSoftColliders c.t2 = t2; } } - psb->m_rcontacts.push_back(c); + psb->m_nodeRigidContacts.push_back(c); } } } @@ -1072,6 +1071,102 @@ struct btSoftColliders btScalar dynmargin; btScalar stamargin; }; + + // + // CollideSDF_RDF + // + struct CollideSDF_RDF : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Face* face = (btSoftBody::Face*)leaf->data; + DoNode(*face); + } + void DoNode(btSoftBody::Face& f) const + { + btSoftBody::Node* n0 = f.m_n[0]; + btSoftBody::Node* n1 = f.m_n[1]; + btSoftBody::Node* n2 = f.m_n[2]; + + const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin; + btSoftBody::DeformableFaceRigidContact c; + btVector3 contact_point; + btVector3 bary; + if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true)) + { + btScalar ima = n0->m_im + n1->m_im + n2->m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + // resolve contact at x_n + psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false); + btSoftBody::sCti& cti = c.m_cti; + c.m_contactPoint = contact_point; + c.m_bary = bary; + 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(); + 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; + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = contact_point - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + if (m_rigidBody) + m_rigidBody->activate(); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } + psb->m_faceRigidContacts.push_back(c); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; // // CollideVF_SS //