diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp index b8c9ce201..306f98f96 100644 --- a/examples/DeformableDemo/SplitImpulse.cpp +++ b/examples/DeformableDemo/SplitImpulse.cpp @@ -165,7 +165,7 @@ void SplitImpulse::initPhysics() psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 0.1; + psb->m_cfg.kDF = 1; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; getDeformableDynamicsWorld()->addSoftBody(psb); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 132699c54..736fe4695 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -262,11 +262,6 @@ btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** def return maxSquaredResidual; } -void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal) -{ - m_objective->m_projection.splitImpulseSetup(infoGlobal); -} - void btDeformableBodySolver::updateVelocity() { int counter = 0; @@ -286,7 +281,7 @@ void btDeformableBodySolver::updateVelocity() { m_dv[counter].setZero(); } - psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); ++counter; } @@ -349,7 +344,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) } else { - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; + m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter]; } psb->m_nodes[j].m_v = m_backupVelocity[counter]; ++counter; @@ -441,6 +436,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d n.m_v *= max_v; } n.m_q = n.m_x + n.m_v * dt; + n.m_splitv.setZero(); n.m_penetration = 0; } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index d4e5f4c60..9d1ca9c43 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -67,9 +67,6 @@ public: // solve the momentum equation virtual void solveDeformableConstraints(btScalar solverdt); - - // set up the position error in split impulse - void splitImpulseSetup(const btContactSolverInfo& infoGlobal); // resize/clear data structures void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 2864446de..e4a2001db 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -213,10 +213,18 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btVector3 va = getVa(); btVector3 vb = getVb(); btVector3 vr = vb - va; - btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + btScalar dn = btDot(vr, cti.m_normal); + if (!infoGlobal.m_splitImpulse) + { + dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + } // 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 + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ; + btVector3 impulse = m_contact->m_c0 * vr; + if (!infoGlobal.m_splitImpulse) + { + impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); + } 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; @@ -288,13 +296,22 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv } } } -// va = getVa(); -// vb = getVb(); -// vr = vb - va; -// btScalar dn1 = btDot(vr, cti.m_normal) / 150; -// m_penetration += dn1; return residualSquare; } + +btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + btVector3 vb = getSplitVb(); + btScalar p = m_penetration; + btScalar dn = btDot(vb, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + if (dn > 0) + return 0; + btScalar residualSquare = dn*dn; + const btVector3 impulse = 1.0/m_contact->m_c2 * (cti.m_normal * dn); + applySplitImpulse(impulse); + return residualSquare; +} /* ================ Node vs. Rigid =================== */ btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal) : m_node(contact.m_node) @@ -313,6 +330,10 @@ btVector3 btDeformableNodeRigidContactConstraint::getVb() const return m_node->m_v; } +btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const +{ + return m_node->m_splitv; +} btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const { @@ -326,6 +347,13 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul contact->m_node->m_v -= dv; } +void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + contact->m_node->m_splitv -= dv; +} + /* ================ Face vs. Rigid =================== */ btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting) : m_face(contact.m_face) @@ -441,6 +469,40 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul } } + +btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv)* contact->m_bary[2]; + return vb; +} + +void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + btVector3& v0 = face->m_n[0]->m_splitv; + btVector3& v1 = face->m_n[1]->m_splitv; + btVector3& v2 = face->m_n[2]->m_splitv; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + { + v0 -= dv * contact->m_weights[0]; + } + if (im1 > 0) + { + v1 -= dv * contact->m_weights[1]; + } + if (im2 > 0) + { + v2 -= dv * contact->m_weights[2]; + } +} + + /* ================ Face vs. Node =================== */ btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal) : m_node(contact.m_node) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 9f9d5bf0a..8e73e4da4 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -160,12 +160,19 @@ public: // object A is the rigid/multi body, and object B is the deformable node/face virtual btVector3 getVa() const; + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const = 0; + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual void setPenetrationScale(btScalar scale) { m_penetration *= scale; } + + btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); + + virtual void applySplitImpulse(const btVector3& impulse) = 0; }; // @@ -186,6 +193,9 @@ public: // get the velocity of the deformable node in contact virtual btVector3 getVb() const; + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + // get the velocity change of the input soft body node in the constraint virtual btVector3 getDv(const btSoftBody::Node*) const; @@ -196,6 +206,8 @@ public: } virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -215,9 +227,12 @@ public: // get the velocity of the deformable face at the contact point virtual btVector3 getVb() const; + // get the split impulse velocity of the deformable face at the contact point + virtual btVector3 getSplitVb() const; + // get the velocity change of the input soft body node in the constraint virtual btVector3 getDv(const btSoftBody::Node*) const; - + // cast the contact to the desired type const btSoftBody::DeformableFaceRigidContact* getContact() const { @@ -225,6 +240,8 @@ public: } virtual void applyImpulse(const btVector3& impulse); + + virtual void applySplitImpulse(const btVector3& impulse); }; // diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 22ca8bf58..20a720273 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -58,23 +58,27 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod return residualSquare; } -void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) { - for (int i = 0; i < m_softBodies.size(); ++i) - { - // node constraints - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); - } - // face constraints - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j]; - constraint.setPenetrationScale(infoGlobal.m_deformable_erp); - } - } + btScalar residualSquare = 0; + for (int i = 0; i < numDeformableBodies; ++i) + { + for (int j = 0; j < m_softBodies.size(); ++j) + { + btCollisionObject* psb = m_softBodies[j]; + if (psb != deformableBodies[i]) + { + continue; + } + for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) + { + btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + } + } + return residualSquare; } void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 8d7e94d4f..554c693ba 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -91,7 +91,7 @@ public: virtual void reinitialize(bool nodeUpdated); - virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal); + btScalar solveSplitImpulse(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); virtual void setLagrangeMultiplier(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index c8cc47923..6a3f677f6 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -21,7 +21,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b { { ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) @@ -105,14 +105,13 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS } } -void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) +void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies,int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); int iteration; if (infoGlobal.m_splitImpulse) { { -// m_deformableSolver->splitImpulseSetup(infoGlobal); for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { btScalar leastSquaresResidual = 0.f; @@ -128,7 +127,8 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI } // solve the position correction between deformable and rigid/multibody // btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal); -// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); + btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 0c7cc26a8..984a026b6 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -44,7 +44,7 @@ protected: // write the velocity of the underlying rigid body to the the the solver body void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies,int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); public: diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 6b742978e..46004bd37 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -315,7 +315,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) node.m_v[c] = -clampDeltaV; } } - node.m_x = node.m_x + timeStep * node.m_v; + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); node.m_q = node.m_x; node.m_vn = node.m_v; } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 6a55eccbd..954adf4d1 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -271,7 +271,8 @@ public: btDbvtNode* m_leaf; // Leaf data btScalar m_penetration; // depth of penetration int m_battach : 1; // Attached - int index; + int index; + btVector3 m_splitv; // velocity associated with split impulse }; /* Link */ ATTRIBUTE_ALIGNED16(struct) @@ -296,7 +297,7 @@ public: btDbvtNode* m_leaf; // Leaf data btVector4 m_pcontact; // barycentric weights of the persistent contact btVector3 m_n0, m_n1, m_vn; - int m_index; + int m_index; }; /* Tetra */ struct Tetra : Feature