dali-physics/third-party/bullet3/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp

   1 #include "btReducedDeformableBodySolver.h"
   2 #include "btReducedDeformableBody.h"
   3
   4 btReducedDeformableBodySolver::btReducedDeformableBodySolver()
   5 {
   6   m_ascendOrder = true;
   7   m_reducedSolver = true;
   8   m_dampingAlpha = 0;
   9   m_dampingBeta = 0;
  10   m_gravity = btVector3(0, 0, 0);
  11 }
  12
  13 void btReducedDeformableBodySolver::setGravity(const btVector3& gravity)
  14 {
  15   m_gravity = gravity;
  16 }
  17
  18 void btReducedDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt)
  19 {
  20   m_softBodies.copyFromArray(bodies);
  21         bool nodeUpdated = updateNodes();
  22
  23         if (nodeUpdated)
  24         {
  25                 m_dv.resize(m_numNodes, btVector3(0, 0, 0));
  26                 m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
  27                 m_residual.resize(m_numNodes, btVector3(0, 0, 0));
  28                 m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
  29         }
  30
  31         // need to setZero here as resize only set value for newly allocated items
  32         for (int i = 0; i < m_numNodes; ++i)
  33         {
  34                 m_dv[i].setZero();
  35                 m_ddv[i].setZero();
  36                 m_residual[i].setZero();
  37         }
  38
  39         if (dt > 0)
  40         {
  41                 m_dt = dt;
  42         }
  43         m_objective->reinitialize(nodeUpdated, dt);
  44
  45   int N = bodies.size();
  46         if (nodeUpdated)
  47         {
  48                 m_staticConstraints.resize(N);
  49                 m_nodeRigidConstraints.resize(N);
  50                 // m_faceRigidConstraints.resize(N);
  51         }
  52         for (int i = 0; i < N; ++i)
  53         {
  54                 m_staticConstraints[i].clear();
  55                 m_nodeRigidConstraints[i].clear();
  56                 // m_faceRigidConstraints[i].clear();
  57         }
  58
  59   for (int i = 0; i < m_softBodies.size(); ++i)
  60   {
  61     btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
  62     rsb->m_contactNodesList.clear();
  63   }
  64
  65   // set node index offsets
  66   int sum = 0;
  67   for (int i = 0; i < m_softBodies.size(); ++i)
  68   {
  69     btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
  70     rsb->m_nodeIndexOffset = sum;
  71     sum += rsb->m_nodes.size();
  72   }
  73
  74         btDeformableBodySolver::updateSoftBodies();
  75 }
  76
  77 void btReducedDeformableBodySolver::predictMotion(btScalar solverdt)
  78 {
  79   applyExplicitForce(solverdt);
  80
  81   // predict new mesh location
  82   predictReduceDeformableMotion(solverdt);
  83
  84   //TODO: check if there is anything missed from btDeformableBodySolver::predictDeformableMotion
  85 }
  86
  87 void btReducedDeformableBodySolver::predictReduceDeformableMotion(btScalar solverdt)
  88 {
  89   for (int i = 0; i < m_softBodies.size(); ++i)
  90   {
  91     btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
  92     if (!rsb->isActive())
  93     {
  94       continue;
  95     }
  96
  97     // clear contacts variables
  98                 rsb->m_nodeRigidContacts.resize(0);
  99                 rsb->m_faceRigidContacts.resize(0);
 100                 rsb->m_faceNodeContacts.resize(0);
 101
 102     // calculate inverse mass matrix for all nodes
 103     for (int j = 0; j < rsb->m_nodes.size(); ++j)
 104     {
 105       if (rsb->m_nodes[j].m_im > 0)
 106       {
 107         rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse();
 108       }
 109     }
 110
 111     // rigid motion: t, R at time^*
 112     rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
 113
 114     // update reduced dofs at time^*
 115     // rsb->updateReducedDofs(solverdt);
 116
 117     // update local moment arm at time^*
 118     // rsb->updateLocalMomentArm();
 119     // rsb->updateExternalForceProjectMatrix(true);
 120
 121     // predict full space velocity at time^* (needed for constraints)
 122     rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform());
 123
 124     // update full space nodal position at time^*
 125     rsb->mapToFullPosition(rsb->getInterpolationWorldTransform());
 126
 127     // update bounding box
 128     rsb->updateBounds();
 129
 130     // update tree
 131     rsb->updateNodeTree(true, true);
 132     if (!rsb->m_fdbvt.empty())
 133     {
 134       rsb->updateFaceTree(true, true);
 135     }
 136   }
 137 }
 138
 139 void btReducedDeformableBodySolver::applyExplicitForce(btScalar solverdt)
 140 {
 141   for (int i = 0; i < m_softBodies.size(); ++i)
 142   {
 143     btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
 144
 145     // apply gravity to the rigid frame, get m_linearVelocity at time^*
 146     rsb->applyRigidGravity(m_gravity, solverdt);
 147
 148     if (!rsb->isReducedModesOFF())
 149     {
 150       // add internal force (elastic force & damping force)
 151       rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
 152       rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
 153
 154       // get reduced velocity at time^*
 155       rsb->updateReducedVelocity(solverdt);
 156     }
 157
 158     // apply damping (no need at this point)
 159     // rsb->applyDamping(solverdt);
 160   }
 161 }
 162
 163 void btReducedDeformableBodySolver::applyTransforms(btScalar timeStep)
 164 {
 165   for (int i = 0; i < m_softBodies.size(); ++i)
 166   {
 167     btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
 168
 169     // rigid motion
 170     rsb->proceedToTransform(timeStep, true);
 171
 172     if (!rsb->isReducedModesOFF())
 173     {
 174       // update reduced dofs for time^n+1
 175       rsb->updateReducedDofs(timeStep);
 176
 177       // update local moment arm for time^n+1
 178       rsb->updateLocalMomentArm();
 179       rsb->updateExternalForceProjectMatrix(true);
 180     }
 181
 182     // update mesh nodal positions for time^n+1
 183     rsb->mapToFullPosition(rsb->getRigidTransform());
 184
 185     // update mesh nodal velocity
 186     rsb->mapToFullVelocity(rsb->getRigidTransform());
 187
 188     // end of time step clean up and update
 189     rsb->endOfTimeStepZeroing();
 190
 191     // update the rendering mesh
 192     rsb->interpolateRenderMesh();
 193   }
 194 }
 195
 196 void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
 197 {
 198   for (int i = 0; i < m_softBodies.size(); ++i)
 199   {
 200     btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
 201     if (!rsb->isActive())
 202                 {
 203                         continue;
 204                 }
 205
 206     // set fixed constraints
 207     for (int j = 0; j < rsb->m_fixedNodes.size(); ++j)
 208                 {
 209       int i_node = rsb->m_fixedNodes[j];
 210                         if (rsb->m_nodes[i_node].m_im == 0)
 211                         {
 212         for (int k = 0; k < 3; ++k)
 213         {
 214           btVector3 dir(0, 0, 0);
 215           dir[k] = 1;
 216           btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt);
 217           m_staticConstraints[i].push_back(static_constraint);
 218         }
 219                         }
 220                 }
 221     btAssert(rsb->m_fixedNodes.size() * 3 == m_staticConstraints[i].size());
 222
 223     // set Deformable Node vs. Rigid constraint
 224                 for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j)
 225                 {
 226                         const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j];
 227                         // skip fixed points
 228                         if (contact.m_node->m_im == 0)
 229                         {
 230                                 continue;
 231                         }
 232                         btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
 233                         m_nodeRigidConstraints[i].push_back(constraint);
 234       rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset);
 235                 }
 236     // std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n";
 237     // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
 238
 239   }
 240 }
 241
 242 btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 243 {
 244   btScalar residualSquare = 0;
 245
 246   for (int i = 0; i < m_softBodies.size(); ++i)
 247   {
 248     btAlignedObjectArray<int> m_orderNonContactConstraintPool;
 249     btAlignedObjectArray<int> m_orderContactConstraintPool;
 250
 251     btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
 252
 253     // shuffle the order of applying constraint
 254     m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
 255     m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
 256     if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 257     {
 258       // fixed constraint order
 259       for (int j = 0; j < m_staticConstraints[i].size(); ++j)
 260       {
 261         m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j;
 262       }
 263       // contact constraint order
 264       for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 265       {
 266         m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j;
 267       }
 268
 269       m_ascendOrder = m_ascendOrder ? false : true;
 270     }
 271     else
 272     {
 273       for (int j = 0; j < m_staticConstraints[i].size(); ++j)
 274       {
 275         m_orderNonContactConstraintPool[j] = j;
 276       }
 277       // contact constraint order
 278       for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 279       {
 280         m_orderContactConstraintPool[j] = j;
 281       }
 282     }
 283
 284     // handle fixed constraint
 285     for (int k = 0; k < m_staticConstraints[i].size(); ++k)
 286     {
 287       btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]];
 288       btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 289       residualSquare = btMax(residualSquare, localResidualSquare);
 290     }
 291
 292     // handle contact constraint
 293
 294     // node vs rigid contact
 295     // std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
 296     for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
 297     {
 298       btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]];
 299       btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 300       residualSquare = btMax(residualSquare, localResidualSquare);
 301     }
 302
 303     // face vs rigid contact
 304     // for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k)
 305     // {
 306     //  btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k];
 307     //  btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 308     //  residualSquare = btMax(residualSquare, localResidualSquare);
 309     // }
 310   }
 311
 312
 313         return residualSquare;
 314 }
 315
 316 void btReducedDeformableBodySolver::deformableBodyInternalWriteBack()
 317 {
 318   // reduced deformable update
 319   for (int i = 0; i < m_softBodies.size(); ++i)
 320   {
 321     btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
 322     rsb->applyInternalVelocityChanges();
 323   }
 324   m_ascendOrder = true;
 325 }