diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 01a71aa3e..9af8e3f77 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -200,7 +200,7 @@ void btDeformableBodySolver::updateDv(btScalar scale) void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - m_cg.solve(*m_objective, ddv, residual, false); + m_cg.solve(*m_objective, ddv, residual); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) @@ -248,6 +248,11 @@ void btDeformableBodySolver::updateVelocity() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + psb->m_maxSpeedSquared = 0; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { // set NaN to zero; @@ -256,6 +261,7 @@ void btDeformableBodySolver::updateVelocity() m_dv[counter].setZero(); } psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; + psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); ++counter; } } @@ -267,6 +273,10 @@ void btDeformableBodySolver::updateTempPosition() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v; @@ -295,6 +305,10 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { if (implicit) diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index e340a3fd5..cf0a5e896 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -51,6 +51,10 @@ void btDeformableContactProjection::setConstraints() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { if (psb->m_nodes[j].m_im == 0) @@ -61,10 +65,15 @@ void btDeformableContactProjection::setConstraints() } } - // set Deformable Node vs. Rigid constraint for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + + // set Deformable Node vs. Rigid constraint for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; @@ -242,6 +251,10 @@ void btDeformableContactProjection::setProjection() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { int index = psb->m_nodes[j].index; diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index abc120252..33e5a8564 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -57,6 +57,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { btSoftBody::Node& n = psb->m_nodes[j]; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 9c21bf9b0..ded9893ce 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -70,18 +70,46 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t ///update vehicle simulation btMultiBodyDynamicsWorld::updateActions(timeStep); - btMultiBodyDynamicsWorld::updateActivationState(timeStep); + updateActivationState(timeStep); // End solver-wise simulation step // /////////////////////////////// } +void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +{ + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + psb->updateDeactivation(timeStep); + if (psb->wantsSleeping()) + { + if (psb->getActivationState() == ACTIVE_TAG) + psb->setActivationState(WANTS_DEACTIVATION); + if (psb->getActivationState() == ISLAND_SLEEPING) + { + psb->setZeroVelocity(); + } + } + else + { + if (psb->getActivationState() != DISABLE_DEACTIVATION) + psb->setActivationState(ACTIVE_TAG); + } + } + btMultiBodyDynamicsWorld::updateActivationState(timeStep); +} + + void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() { m_deformableBodySolver->updateSoftBodies(); for (int i = 0; i < m_softBodies.size(); i++) { - btSoftBody* psb = (btSoftBody*)m_softBodies[i]; - psb->defaultCollisionHandler(psb); + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + psb->defaultCollisionHandler(psb); + } } } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 72cfa4fc5..834c6c8b1 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -62,6 +62,8 @@ protected: void solveConstraints(btScalar timeStep); + void updateActivationState(btScalar timeStep); + public: btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 26f738fda..b113b4546 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -61,6 +61,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_tetras.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; @@ -224,6 +228,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_tetras.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; @@ -262,6 +270,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_tetras.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 843c60e29..b05b28ca4 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -117,7 +117,7 @@ void btSoftBody::initDefaults() m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); m_dampingCoefficient = 1; - m_sleepingThreshold = 0.01; + m_sleepingThreshold = 0.1; m_useFaceContact = false; m_collisionFlags = 0; } @@ -3601,51 +3601,43 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) break; case fCollision::VF_DD: { - if (this != psb) + if (psb->isActive() || this->isActive()) { - 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); - } - else - { - if (psb->useSelfCollision()) + if (this != psb) { - btSoftColliders::CollideFF_DD docollide; - docollide.mrg = getCollisionShape()->getMargin() + - psb->getCollisionShape()->getMargin(); - docollide.psb[0] = this; - docollide.psb[1] = psb; - /* psb0 faces vs psb0 faces */ - btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); - calculateNormalCone(root); - this->m_fdbvt.selfCollideT(root,docollide); - delete root; + 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); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + /* psb0 faces vs psb0 faces */ + btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); + calculateNormalCone(root); + this->m_fdbvt.selfCollideT(root,docollide); + delete root; + } } - -// btSoftColliders::CollideFF_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_fdbvt.m_root, -// docollide.psb[1]->m_fdbvt.m_root, -// docollide); } } break; @@ -4051,3 +4043,47 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ return btSoftBodyDataName; } + +void btSoftBody::updateDeactivation(btScalar timeStep) +{ + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + return; + + if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) + { + m_deactivationTime += timeStep; + } + else + { + m_deactivationTime = btScalar(0.); + setActivationState(0); + } +} + + +void btSoftBody::setZeroVelocity() +{ + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v.setZero(); + } +} + +bool btSoftBody::wantsSleeping() +{ + if (getActivationState() == DISABLE_DEACTIVATION) + return false; + + //disable deactivation + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) + return false; + + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + return true; + + if (m_deactivationTime > gDeactivationTime) + { + return true; + } + return false; +} diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 970290962..195434c06 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1017,8 +1017,11 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); void defaultCollisionHandler(btSoftBody* psb); - void setSelfCollision(bool useSelfCollision); - bool useSelfCollision(); + void setSelfCollision(bool useSelfCollision); + bool useSelfCollision(); + void updateDeactivation(btScalar timeStep); + void setZeroVelocity(); + bool wantsSleeping(); // // Functionality to deal with new accelerated solvers. diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index afe9c7ea8..6e20b3222 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1097,8 +1097,6 @@ struct btSoftColliders 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) { @@ -1200,8 +1198,6 @@ struct btSoftColliders // we do not scale the impulse matrix by dt c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; - if (m_rigidBody) - m_rigidBody->activate(); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) {