From d6d8c3ceacedad756872030ff5e9aedd3c8a612e Mon Sep 17 00:00:00 2001 From: jyc-n Date: Thu, 12 Aug 2021 22:36:39 -0400 Subject: [PATCH] reworking static constraint WIP --- .../btReducedDeformableContactConstraint.cpp | 26 +++++++++++++++++-- .../btReducedDeformableContactConstraint.h | 9 ++++--- .../btReducedSoftBody.cpp | 7 +++++ .../BulletReducedSoftBody/btReducedSoftBody.h | 3 +++ .../btReducedSoftBodySolver.cpp | 17 +++++++++++- 5 files changed, 56 insertions(+), 6 deletions(-) diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp index c72223f7c..210557d01 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp @@ -5,10 +5,32 @@ btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint( btReducedSoftBody* rsb, btSoftBody::Node* node, + const btVector3& ri, const btContactSolverInfo& infoGlobal, btScalar dt) - : m_rsb(rsb), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal) -{} + : m_rsb(rsb), m_ri(ri), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal) +{ + // get impulse + m_impulseFactor = rsb->getImpulseFactor(m_node->index); +} + +btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) +{ + // target velocity of fixed constraint is 0 + btVector3 impulse = -(m_impulseFactor.inverse() * m_node->m_v); + + // apply full space impulse + applyImpulse(impulse); + + return 0; +} + +// this calls reduced deformable body's applyFullSpaceImpulse +void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse) +{ + m_rsb->applyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt); +} + // ================= base contact constraints =================== btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint( diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h index 654520a7f..920e36544 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h @@ -7,19 +7,22 @@ class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint public: btReducedSoftBody* m_rsb; btScalar m_dt; + btMatrix3x3 m_impulseFactor; + btVector3 m_ri; btReducedDeformableStaticConstraint(btReducedSoftBody* rsb, btSoftBody::Node* node, + const btVector3& ri, const btContactSolverInfo& infoGlobal, btScalar dt); // btReducedDeformableStaticConstraint(const btReducedDeformableStaticConstraint& other); btReducedDeformableStaticConstraint() {} virtual ~btReducedDeformableStaticConstraint() {} - // virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - // // this calls reduced deformable body's applyFullSpaceImpulse - // virtual void applyImpulse(const btVector3& impulse); + // this calls reduced deformable body's applyFullSpaceImpulse + virtual void applyImpulse(const btVector3& impulse); // virtual void applySplitImpulse(const btVector3& impulse) {} }; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index fe58a237e..db944d2e5 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -359,6 +359,13 @@ void btReducedSoftBody::applyRigidImpulse(const btVector3& impulse, const btVect } } +btVector3 btReducedSoftBody::getRelativePos(int n_node) +{ + btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis(); + btVector3 ri = rotation * m_localMomentArm[n_node]; + return ri; +} + btMatrix3x3 btReducedSoftBody::getImpulseFactor(int n_node) { // relative position diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 9f317d029..681754920 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -174,6 +174,9 @@ class btReducedSoftBody : public btSoftBody // calculate the impulse factor virtual btMatrix3x3 getImpulseFactor(int n_node); + // get relative position from a node to the CoM of the rigid frame + btVector3 getRelativePos(int n_node); + // apply velocity constraint void applyVelocityConstraint(const btVector3& target_vel, int n_node, btScalar dt); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 5c186b0a2..aa21d4a47 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -200,7 +200,7 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob { if (rsb->m_nodes[j].m_im == 0) { - btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[rsb->m_fixedNodes[j]], infoGlobal, m_dt); + btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[rsb->m_fixedNodes[j]], rsb->getRelativePos(rsb->m_fixedNodes[j]), infoGlobal, m_dt); m_staticConstraints[i].push_back(static_constraint); } } @@ -240,6 +240,19 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; + + // handle fixed constraint + for (int i = 0; i < m_softBodies.size(); ++i) + { + for (int k = 0; k < m_staticConstraints[i].size(); ++k) + { + btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][k]; + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + } + + // handle contact constraint for (int i = 0; i < numDeformableBodies; ++i) { for (int j = 0; j < m_softBodies.size(); ++j) @@ -249,6 +262,8 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de { continue; } + + // node vs rigid contact for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) { btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];