dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

   1 /*
   2  Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
   3
   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.
  14  */
  15
  16 #include "btDeformableMultiBodyConstraintSolver.h"
  17 #include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
  18 #include <iostream>
  19
  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)
  22 {
  23         {
  24                 // pair deformable body with solver body
  25                 pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal);
  26
  27                 ///this is a special step to resolve penetrations (just for contacts)
  28                 solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
  29
  30                 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
  31                 for (int iteration = 0; iteration < maxIterations; iteration++)
  32                 {
  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
  34
  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);
  44
  45
  46                         // std::cout << "------------Iteration " << iteration << "------------\n";
  47                         // std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n";
  48
  49                         if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
  50                         {
  51 #ifdef VERBOSE_RESIDUAL_PRINTF
  52                                 if (iteration >= (maxIterations - 1))
  53                                         printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
  54 #endif
  55                                 m_analyticsData.m_numSolverCalls++;
  56                                 m_analyticsData.m_numIterationsUsed = iteration + 1;
  57                                 m_analyticsData.m_islandId = -2;
  58                                 if (numBodies > 0)
  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;
  63
  64                                 m_deformableSolver->deformableBodyInternalWriteBack();
  65                                 // std::cout << "[===================Next Step===================]\n";
  66                                 break;
  67                         }
  68                 }
  69         }
  70         return 0.f;
  71 }
  72
  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)
  74 {
  75         m_tmpMultiBodyConstraints = multiBodyConstraints;
  76         m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
  77
  78         // inherited from MultiBodyConstraintSolver
  79         solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
  80
  81         // overriden
  82         solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
  83
  84         // inherited from MultiBodyConstraintSolver
  85         solveGroupCacheFriendlyFinish(bodies, numBodies, info);
  86
  87         m_tmpMultiBodyConstraints = 0;
  88         m_tmpNumMultiBodyConstraints = 0;
  89 }
  90
  91 void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
  92 {
  93         // reduced soft body solver directly modifies the solver body
  94         if (m_deformableSolver->isReducedSolver())
  95         {
  96                 return;
  97         }
  98
  99         for (int i = 0; i < numBodies; i++)
 100         {
 101                 int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
 102
 103                 btRigidBody* body = btRigidBody::upcast(bodies[i]);
 104                 if (body && body->getInvMass())
 105                 {
 106                         btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
 107                         solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
 108                         solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
 109                 }
 110         }
 111 }
 112
 113 void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
 114 {
 115         // reduced soft body solver directly modifies the solver body
 116         if (m_deformableSolver->isReducedSolver())
 117         {
 118                 return;
 119         }
 120
 121         for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
 122         {
 123                 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
 124                 if (body)
 125                 {
 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);
 128                 }
 129         }
 130 }
 131
 132
 133 void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 134 {
 135         if (!m_deformableSolver->isReducedSolver())
 136         {
 137                 return;
 138         }
 139
 140         btReducedDeformableBodySolver* solver = static_cast<btReducedDeformableBodySolver*>(m_deformableSolver);
 141
 142         for (int i = 0; i < numDeformableBodies; ++i)
 143         {
 144                 for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k)
 145     {
 146       btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k];
 147
 148                         if (!constraint.m_contact->m_cti.m_colObj->isStaticObject())
 149                         {
 150                                 btCollisionObject& col_obj = const_cast<btCollisionObject&>(*constraint.m_contact->m_cti.m_colObj);
 151
 152                                 // object index in the solver body pool
 153                                 int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep);
 154
 155                                 const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]);
 156                                 if (body && body->getInvMass())
 157                                 {
 158                                                 // std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n";
 159                                         btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
 160                                         constraint.setSolverBody(bodyId, solverBody);
 161                                 }
 162                         }
 163     }
 164
 165                 // for (int j = 0; j < numBodies; j++)
 166                 // {
 167                 //      int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep);
 168
 169                 //      btRigidBody* body = btRigidBody::upcast(bodies[j]);
 170                 //      if (body && body->getInvMass())
 171                 //      {
 172                 //              btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
 173                 //              m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody);
 174                 //      }
 175                 // }
 176         }
 177 }
 178
 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)
 180 {
 181         BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
 182         int iteration;
 183         if (infoGlobal.m_splitImpulse)
 184         {
 185                 {
 186                         for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
 187                         {
 188                                 btScalar leastSquaresResidual = 0.f;
 189                                 {
 190                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 191                                         int j;
 192                                         for (j = 0; j < numPoolConstraints; j++)
 193                                         {
 194                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 195
 196                                                 btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 197                                                 leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 198                                         }
 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);
 203                                 }
 204                                 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
 205                                 {
 206 #ifdef VERBOSE_RESIDUAL_PRINTF
 207                                         if (iteration >= (infoGlobal.m_numIterations - 1))
 208                                                 printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
 209 #endif
 210                                         break;
 211                                 }
 212                         }
 213                 }
 214         }
 215 }