dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableBodySolver.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 <stdio.h>
  17 #include <limits>
  18 #include "btDeformableBodySolver.h"
  19 #include "btSoftBodyInternals.h"
  20 #include "LinearMath/btQuickprof.h"
  21 static const int kMaxConjugateGradientIterations = 300;
  22 btDeformableBodySolver::btDeformableBodySolver()
  23         : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
  24 {
  25         m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
  26         m_reducedSolver = false;
  27 }
  28
  29 btDeformableBodySolver::~btDeformableBodySolver()
  30 {
  31         delete m_objective;
  32 }
  33
  34 void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
  35 {
  36         BT_PROFILE("solveDeformableConstraints");
  37         if (!m_implicit)
  38         {
  39                 m_objective->computeResidual(solverdt, m_residual);
  40                 m_objective->applyDynamicFriction(m_residual);
  41                 if (m_useProjection)
  42                 {
  43                         computeStep(m_dv, m_residual);
  44                 }
  45                 else
  46                 {
  47                         TVStack rhs, x;
  48                         m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
  49                         m_objective->addLagrangeMultiplier(m_dv, x);
  50                         m_objective->m_preconditioner->reinitialize(true);
  51                         computeStep(x, rhs);
  52                         for (int i = 0; i < m_dv.size(); ++i)
  53                         {
  54                                 m_dv[i] = x[i];
  55                         }
  56                 }
  57                 updateVelocity();
  58         }
  59         else
  60         {
  61                 for (int i = 0; i < m_maxNewtonIterations; ++i)
  62                 {
  63                         updateState();
  64                         // add the inertia term in the residual
  65                         int counter = 0;
  66                         for (int k = 0; k < m_softBodies.size(); ++k)
  67                         {
  68                                 btSoftBody* psb = m_softBodies[k];
  69                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
  70                                 {
  71                                         if (psb->m_nodes[j].m_im > 0)
  72                                         {
  73                                                 m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter];
  74                                         }
  75                                         ++counter;
  76                                 }
  77                         }
  78
  79                         m_objective->computeResidual(solverdt, m_residual);
  80                         if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
  81                         {
  82                                 break;
  83                         }
  84                         // todo xuchenhan@: this really only needs to be calculated once
  85                         m_objective->applyDynamicFriction(m_residual);
  86                         if (m_lineSearch)
  87                         {
  88                                 btScalar inner_product = computeDescentStep(m_ddv, m_residual);
  89                                 btScalar alpha = 0.01, beta = 0.5;  // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
  90                                 btScalar scale = 2;
  91                                 btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
  92                                 backupDv();
  93                                 do
  94                                 {
  95                                         scale *= beta;
  96                                         if (scale < 1e-8)
  97                                         {
  98                                                 return;
  99                                         }
 100                                         updateEnergy(scale);
 101                                         f1 = m_objective->totalEnergy(solverdt) + kineticEnergy();
 102                                         f2 = f0 - alpha * scale * inner_product;
 103                                 } while (!(f1 < f2 + SIMD_EPSILON));  // if anything here is nan then the search continues
 104                                 revertDv();
 105                                 updateDv(scale);
 106                         }
 107                         else
 108                         {
 109                                 computeStep(m_ddv, m_residual);
 110                                 updateDv();
 111                         }
 112                         for (int j = 0; j < m_numNodes; ++j)
 113                         {
 114                                 m_ddv[j].setZero();
 115                                 m_residual[j].setZero();
 116                         }
 117                 }
 118                 updateVelocity();
 119         }
 120 }
 121
 122 btScalar btDeformableBodySolver::kineticEnergy()
 123 {
 124         btScalar ke = 0;
 125         for (int i = 0; i < m_softBodies.size(); ++i)
 126         {
 127                 btSoftBody* psb = m_softBodies[i];
 128                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 129                 {
 130                         btSoftBody::Node& node = psb->m_nodes[j];
 131                         if (node.m_im > 0)
 132                         {
 133                                 ke += m_dv[node.index].length2() * 0.5 / node.m_im;
 134                         }
 135                 }
 136         }
 137         return ke;
 138 }
 139
 140 void btDeformableBodySolver::backupDv()
 141 {
 142         m_backup_dv.resize(m_dv.size());
 143         for (int i = 0; i < m_backup_dv.size(); ++i)
 144         {
 145                 m_backup_dv[i] = m_dv[i];
 146         }
 147 }
 148
 149 void btDeformableBodySolver::revertDv()
 150 {
 151         for (int i = 0; i < m_backup_dv.size(); ++i)
 152         {
 153                 m_dv[i] = m_backup_dv[i];
 154         }
 155 }
 156
 157 void btDeformableBodySolver::updateEnergy(btScalar scale)
 158 {
 159         for (int i = 0; i < m_dv.size(); ++i)
 160         {
 161                 m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
 162         }
 163         updateState();
 164 }
 165
 166 btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
 167 {
 168         m_cg.solve(*m_objective, ddv, residual, false);
 169         btScalar inner_product = m_cg.dot(residual, m_ddv);
 170         btScalar res_norm = m_objective->computeNorm(residual);
 171         btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
 172         if (inner_product < -tol)
 173         {
 174                 if (verbose)
 175                 {
 176                         std::cout << "Looking backwards!" << std::endl;
 177                 }
 178                 for (int i = 0; i < m_ddv.size(); ++i)
 179                 {
 180                         m_ddv[i] = -m_ddv[i];
 181                 }
 182                 inner_product = -inner_product;
 183         }
 184         else if (std::abs(inner_product) < tol)
 185         {
 186                 if (verbose)
 187                 {
 188                         std::cout << "Gradient Descent!" << std::endl;
 189                 }
 190                 btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
 191                 for (int i = 0; i < m_ddv.size(); ++i)
 192                 {
 193                         m_ddv[i] = scale * residual[i];
 194                 }
 195                 inner_product = scale * res_norm * res_norm;
 196         }
 197         return inner_product;
 198 }
 199
 200 void btDeformableBodySolver::updateState()
 201 {
 202         updateVelocity();
 203         updateTempPosition();
 204 }
 205
 206 void btDeformableBodySolver::updateDv(btScalar scale)
 207 {
 208         for (int i = 0; i < m_numNodes; ++i)
 209         {
 210                 m_dv[i] += scale * m_ddv[i];
 211         }
 212 }
 213
 214 void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
 215 {
 216         if (m_useProjection)
 217                 m_cg.solve(*m_objective, ddv, residual, false);
 218         else
 219                 m_cr.solve(*m_objective, ddv, residual, false);
 220 }
 221
 222 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt)
 223 {
 224         m_softBodies.copyFromArray(softBodies);
 225         bool nodeUpdated = updateNodes();
 226
 227         if (nodeUpdated)
 228         {
 229                 m_dv.resize(m_numNodes, btVector3(0, 0, 0));
 230                 m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
 231                 m_residual.resize(m_numNodes, btVector3(0, 0, 0));
 232                 m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
 233         }
 234
 235         // need to setZero here as resize only set value for newly allocated items
 236         for (int i = 0; i < m_numNodes; ++i)
 237         {
 238                 m_dv[i].setZero();
 239                 m_ddv[i].setZero();
 240                 m_residual[i].setZero();
 241         }
 242
 243         if (dt > 0)
 244         {
 245                 m_dt = dt;
 246         }
 247         m_objective->reinitialize(nodeUpdated, dt);
 248         updateSoftBodies();
 249 }
 250
 251 void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
 252 {
 253         BT_PROFILE("setConstraint");
 254         m_objective->setConstraints(infoGlobal);
 255 }
 256
 257 btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 258 {
 259         BT_PROFILE("solveContactConstraints");
 260         btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal);
 261         return maxSquaredResidual;
 262 }
 263
 264 void btDeformableBodySolver::updateVelocity()
 265 {
 266         int counter = 0;
 267         for (int i = 0; i < m_softBodies.size(); ++i)
 268         {
 269                 btSoftBody* psb = m_softBodies[i];
 270                 psb->m_maxSpeedSquared = 0;
 271                 if (!psb->isActive())
 272                 {
 273                         counter += psb->m_nodes.size();
 274                         continue;
 275                 }
 276                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 277                 {
 278                         // set NaN to zero;
 279                         if (m_dv[counter] != m_dv[counter])
 280                         {
 281                                 m_dv[counter].setZero();
 282                         }
 283                         if (m_implicit)
 284                         {
 285                                 psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
 286                         }
 287                         else
 288                         {
 289                                 psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
 290                         }
 291                         psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
 292                         ++counter;
 293                 }
 294         }
 295 }
 296
 297 void btDeformableBodySolver::updateTempPosition()
 298 {
 299         int counter = 0;
 300         for (int i = 0; i < m_softBodies.size(); ++i)
 301         {
 302                 btSoftBody* psb = m_softBodies[i];
 303                 if (!psb->isActive())
 304                 {
 305                         counter += psb->m_nodes.size();
 306                         continue;
 307                 }
 308                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 309                 {
 310                         psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv);
 311                         ++counter;
 312                 }
 313                 psb->updateDeformation();
 314         }
 315 }
 316
 317 void btDeformableBodySolver::backupVelocity()
 318 {
 319         int counter = 0;
 320         for (int i = 0; i < m_softBodies.size(); ++i)
 321         {
 322                 btSoftBody* psb = m_softBodies[i];
 323                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 324                 {
 325                         m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
 326                 }
 327         }
 328 }
 329
 330 void btDeformableBodySolver::setupDeformableSolve(bool implicit)
 331 {
 332         int counter = 0;
 333         for (int i = 0; i < m_softBodies.size(); ++i)
 334         {
 335                 btSoftBody* psb = m_softBodies[i];
 336                 if (!psb->isActive())
 337                 {
 338                         counter += psb->m_nodes.size();
 339                         continue;
 340                 }
 341                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 342                 {
 343                         if (implicit)
 344                         {
 345                                 // setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint.
 346                                 if (psb->m_nodes[j].m_v == m_backupVelocity[counter])
 347                                         m_dv[counter].setZero();
 348                                 else
 349                                         m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
 350                                 m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
 351                         }
 352                         else
 353                         {
 354                                 m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
 355                         }
 356                         psb->m_nodes[j].m_v = m_backupVelocity[counter];
 357                         ++counter;
 358                 }
 359         }
 360 }
 361
 362 void btDeformableBodySolver::revertVelocity()
 363 {
 364         int counter = 0;
 365         for (int i = 0; i < m_softBodies.size(); ++i)
 366         {
 367                 btSoftBody* psb = m_softBodies[i];
 368                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 369                 {
 370                         psb->m_nodes[j].m_v = m_backupVelocity[counter++];
 371                 }
 372         }
 373 }
 374
 375 bool btDeformableBodySolver::updateNodes()
 376 {
 377         int numNodes = 0;
 378         for (int i = 0; i < m_softBodies.size(); ++i)
 379                 numNodes += m_softBodies[i]->m_nodes.size();
 380         if (numNodes != m_numNodes)
 381         {
 382                 m_numNodes = numNodes;
 383                 return true;
 384         }
 385         return false;
 386 }
 387
 388 void btDeformableBodySolver::predictMotion(btScalar solverdt)
 389 {
 390         // apply explicit forces to velocity
 391         if (m_implicit)
 392         {
 393                 for (int i = 0; i < m_softBodies.size(); ++i)
 394                 {
 395                         btSoftBody* psb = m_softBodies[i];
 396                         if (psb->isActive())
 397                         {
 398                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 399                                 {
 400                                         psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt;
 401                                 }
 402                         }
 403                 }
 404         }
 405         applyExplicitForce();
 406         for (int i = 0; i < m_softBodies.size(); ++i)
 407         {
 408                 btSoftBody* psb = m_softBodies[i];
 409                 if (psb->isActive())
 410                 {
 411                         /* Clear contacts when softbody is active*/
 412                         psb->m_nodeRigidContacts.resize(0);
 413                         psb->m_faceRigidContacts.resize(0);
 414                         psb->m_faceNodeContacts.resize(0);
 415                         psb->m_faceNodeContactsCCD.resize(0);
 416                         // predict motion for collision detection
 417                         predictDeformableMotion(psb, solverdt);
 418                 }
 419         }
 420 }
 421
 422 void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
 423 {
 424         BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
 425         int i, ni;
 426
 427         /* Update                */
 428         if (psb->m_bUpdateRtCst)
 429         {
 430                 psb->m_bUpdateRtCst = false;
 431                 psb->updateConstants();
 432                 psb->m_fdbvt.clear();
 433                 if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
 434                 {
 435                         psb->initializeFaceTree();
 436                 }
 437         }
 438
 439         /* Prepare                */
 440         psb->m_sst.sdt = dt * psb->m_cfg.timescale;
 441         psb->m_sst.isdt = 1 / psb->m_sst.sdt;
 442         psb->m_sst.velmrg = psb->m_sst.sdt * 3;
 443         psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
 444         psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
 445         /* Bounds                */
 446         psb->updateBounds();
 447
 448         /* Integrate            */
 449         // do not allow particles to move more than the bounding box size
 450         btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt;
 451         for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
 452         {
 453                 btSoftBody::Node& n = psb->m_nodes[i];
 454                 // apply drag
 455                 n.m_v *= (1 - psb->m_cfg.drag);
 456                 // scale velocity back
 457                 if (m_implicit)
 458                 {
 459                         n.m_q = n.m_x;
 460                 }
 461                 else
 462                 {
 463                         if (n.m_v.norm() > max_v)
 464                         {
 465                                 n.m_v.safeNormalize();
 466                                 n.m_v *= max_v;
 467                         }
 468                         n.m_q = n.m_x + n.m_v * dt;
 469                 }
 470                 n.m_splitv.setZero();
 471                 n.m_constrained = false;
 472         }
 473
 474         /* Nodes                */
 475         psb->updateNodeTree(true, true);
 476         if (!psb->m_fdbvt.empty())
 477         {
 478                 psb->updateFaceTree(true, true);
 479         }
 480         /* Optimize dbvt's        */
 481         //    psb->m_ndbvt.optimizeIncremental(1);
 482         //    psb->m_fdbvt.optimizeIncremental(1);
 483 }
 484
 485 void btDeformableBodySolver::updateSoftBodies()
 486 {
 487         BT_PROFILE("updateSoftBodies");
 488         for (int i = 0; i < m_softBodies.size(); i++)
 489         {
 490                 btSoftBody* psb = (btSoftBody*)m_softBodies[i];
 491                 if (psb->isActive())
 492                 {
 493                         psb->updateNormals();
 494                 }
 495         }
 496 }
 497
 498 void btDeformableBodySolver::setImplicit(bool implicit)
 499 {
 500         m_implicit = implicit;
 501         m_objective->setImplicit(implicit);
 502 }
 503
 504 void btDeformableBodySolver::setLineSearch(bool lineSearch)
 505 {
 506         m_lineSearch = lineSearch;
 507 }
 508
 509 void btDeformableBodySolver::applyExplicitForce()
 510 {
 511         m_objective->applyExplicitForce(m_residual);
 512 }
 513
 514 void btDeformableBodySolver::applyTransforms(btScalar timeStep)
 515 {
 516         for (int i = 0; i < m_softBodies.size(); ++i)
 517         {
 518                 btSoftBody* psb = m_softBodies[i];
 519                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 520                 {
 521                         btSoftBody::Node& node = psb->m_nodes[j];
 522                         btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
 523                         btScalar clampDeltaV = maxDisplacement / timeStep;
 524                         for (int c = 0; c < 3; c++)
 525                         {
 526                                 if (node.m_v[c] > clampDeltaV)
 527                                 {
 528                                         node.m_v[c] = clampDeltaV;
 529                                 }
 530                                 if (node.m_v[c] < -clampDeltaV)
 531                                 {
 532                                         node.m_v[c] = -clampDeltaV;
 533                                 }
 534                         }
 535                         node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
 536                         node.m_q = node.m_x;
 537                         node.m_vn = node.m_v;
 538                 }
 539                 // enforce anchor constraints
 540                 for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
 541                 {
 542                         btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
 543                         btSoftBody::Node* n = a.m_node;
 544                         n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
 545
 546                         // update multibody anchor info
 547                         if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
 548                         {
 549                                 btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
 550                                 if (multibodyLinkCol)
 551                                 {
 552                                         btVector3 nrm;
 553                                         const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
 554                                         const btTransform& wtr = multibodyLinkCol->getWorldTransform();
 555                                         psb->m_worldInfo->m_sparsesdf.Evaluate(
 556                                                 wtr.invXform(n->m_x),
 557                                                 shp,
 558                                                 nrm,
 559                                                 0);
 560                                         a.m_cti.m_normal = wtr.getBasis() * nrm;
 561                                         btVector3 normal = a.m_cti.m_normal;
 562                                         btVector3 t1 = generateUnitOrthogonalVector(normal);
 563                                         btVector3 t2 = btCross(normal, t1);
 564                                         btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
 565                                         findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
 566                                         findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
 567                                         findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
 568
 569                                         btScalar* J_n = &jacobianData_normal.m_jacobians[0];
 570                                         btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
 571                                         btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
 572
 573                                         btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
 574                                         btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
 575                                         btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
 576
 577                                         btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
 578                                                                         t1.getX(), t1.getY(), t1.getZ(),
 579                                                                         t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
 580                                         const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
 581                                         btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
 582                                         a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
 583                                         a.jacobianData_normal = jacobianData_normal;
 584                                         a.jacobianData_t1 = jacobianData_t1;
 585                                         a.jacobianData_t2 = jacobianData_t2;
 586                                         a.t1 = t1;
 587                                         a.t2 = t2;
 588                                 }
 589                         }
 590                 }
 591                 psb->interpolateRenderMesh();
 592         }
 593 }