2 Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
4 Bullet Continuous Collision Detection and Physics Library
5 Copyright (c) 2019 Google Inc. http://bulletphysics.org
6 This software is provided 'as-is', without any express or implied warranty.
7 In no event will the authors be held liable for any damages arising from the use of this software.
8 Permission is granted to anyone to use this software for any purpose,
9 including commercial applications, and to alter it and redistribute it freely,
10 subject to the following restrictions:
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
16 #include "btDeformableMultiBodyConstraintSolver.h"
17 #include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
20 // override the iterations method to include deformable/multibody contact
21 btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
24 // pair deformable body with solver body
25 pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal);
27 ///this is a special step to resolve penetrations (just for contacts)
28 solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
30 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
31 for (int iteration = 0; iteration < maxIterations; iteration++)
33 // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
35 // solve rigid/rigid in solver body
36 m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
37 // solver body velocity -> rigid body velocity
38 solverBodyWriteBack(infoGlobal);
39 btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal);
40 // update rigid body velocity in rigid/deformable contact
41 m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
42 // solver body velocity <- rigid body velocity
43 writeToSolverBody(bodies, numBodies, infoGlobal);
46 // std::cout << "------------Iteration " << iteration << "------------\n";
47 // std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n";
49 if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
51 #ifdef VERBOSE_RESIDUAL_PRINTF
52 if (iteration >= (maxIterations - 1))
53 printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
55 m_analyticsData.m_numSolverCalls++;
56 m_analyticsData.m_numIterationsUsed = iteration + 1;
57 m_analyticsData.m_islandId = -2;
59 m_analyticsData.m_islandId = bodies[0]->getCompanionId();
60 m_analyticsData.m_numBodies = numBodies;
61 m_analyticsData.m_numContactManifolds = numManifolds;
62 m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
64 m_deformableSolver->deformableBodyInternalWriteBack();
65 // std::cout << "[===================Next Step===================]\n";
73 void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
75 m_tmpMultiBodyConstraints = multiBodyConstraints;
76 m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
78 // inherited from MultiBodyConstraintSolver
79 solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
82 solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
84 // inherited from MultiBodyConstraintSolver
85 solveGroupCacheFriendlyFinish(bodies, numBodies, info);
87 m_tmpMultiBodyConstraints = 0;
88 m_tmpNumMultiBodyConstraints = 0;
91 void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
93 // reduced soft body solver directly modifies the solver body
94 if (m_deformableSolver->isReducedSolver())
99 for (int i = 0; i < numBodies; i++)
101 int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
103 btRigidBody* body = btRigidBody::upcast(bodies[i]);
104 if (body && body->getInvMass())
106 btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
107 solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
108 solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
113 void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
115 // reduced soft body solver directly modifies the solver body
116 if (m_deformableSolver->isReducedSolver())
121 for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
123 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
126 m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
127 m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
133 void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
135 if (!m_deformableSolver->isReducedSolver())
140 btReducedDeformableBodySolver* solver = static_cast<btReducedDeformableBodySolver*>(m_deformableSolver);
142 for (int i = 0; i < numDeformableBodies; ++i)
144 for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k)
146 btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k];
148 if (!constraint.m_contact->m_cti.m_colObj->isStaticObject())
150 btCollisionObject& col_obj = const_cast<btCollisionObject&>(*constraint.m_contact->m_cti.m_colObj);
152 // object index in the solver body pool
153 int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep);
155 const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]);
156 if (body && body->getInvMass())
158 // std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n";
159 btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
160 constraint.setSolverBody(bodyId, solverBody);
165 // for (int j = 0; j < numBodies; j++)
167 // int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep);
169 // btRigidBody* body = btRigidBody::upcast(bodies[j]);
170 // if (body && body->getInvMass())
172 // btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
173 // m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody);
179 void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
181 BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
183 if (infoGlobal.m_splitImpulse)
186 for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
188 btScalar leastSquaresResidual = 0.f;
190 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
192 for (j = 0; j < numPoolConstraints; j++)
194 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
196 btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
197 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
199 // solve the position correction between deformable and rigid/multibody
200 // btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
201 btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
202 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
204 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
206 #ifdef VERBOSE_RESIDUAL_PRINTF
207 if (iteration >= (infoGlobal.m_numIterations - 1))
208 printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);