diff options
Diffstat (limited to 'src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp')
| -rw-r--r-- | src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp | 71 |
1 files changed, 71 insertions, 0 deletions
diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 631fd5fbe..2a0e5ec34 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -14,11 +14,16 @@ */ #include "btDeformableMultiBodyConstraintSolver.h" +#include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h" #include <iostream> +#include <thread> // override the iterations method to include deformable/multibody contact btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { { + // pair deformable body with solver body + pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal); + ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); @@ -37,6 +42,10 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b // solver body velocity <- rigid body velocity writeToSolverBody(bodies, numBodies, infoGlobal); + + // std::cout << "------------Iteration " << iteration << "------------\n"; + // std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n"; + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) { #ifdef VERBOSE_RESIDUAL_PRINTF @@ -51,6 +60,9 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b m_analyticsData.m_numBodies = numBodies; m_analyticsData.m_numContactManifolds = numManifolds; m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; + + m_deformableSolver->deformableBodyInternalWriteBack(); + // std::cout << "[===================Next Step===================]\n"; break; } } @@ -78,6 +90,12 @@ void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollision void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { + // reduced soft body solver directly modifies the solver body + if (m_deformableSolver->isReducedSolver()) + { + return; + } + for (int i = 0; i < numBodies; i++) { int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); @@ -94,6 +112,12 @@ void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject* void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) { + // reduced soft body solver directly modifies the solver body + if (m_deformableSolver->isReducedSolver()) + { + return; + } + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) { btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; @@ -105,6 +129,53 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS } } + +void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) +{ + if (!m_deformableSolver->isReducedSolver()) + { + return; + } + + btReducedDeformableBodySolver* solver = static_cast<btReducedDeformableBodySolver*>(m_deformableSolver); + + for (int i = 0; i < numDeformableBodies; ++i) + { + for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k) + { + btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k]; + + if (!constraint.m_contact->m_cti.m_colObj->isStaticObject()) + { + btCollisionObject& col_obj = const_cast<btCollisionObject&>(*constraint.m_contact->m_cti.m_colObj); + + // object index in the solver body pool + int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep); + + const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]); + if (body && body->getInvMass()) + { + // std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n"; + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + constraint.setSolverBody(bodyId, solverBody); + } + } + } + + // for (int j = 0; j < numBodies; j++) + // { + // int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep); + + // btRigidBody* body = btRigidBody::upcast(bodies[j]); + // if (body && body->getInvMass()) + // { + // btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + // m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody); + // } + // } + } +} + 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"); |