dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableNeoHookeanForce.h

   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 #ifndef BT_NEOHOOKEAN_H
  17 #define BT_NEOHOOKEAN_H
  18
  19 #include "btDeformableLagrangianForce.h"
  20 #include "LinearMath/btQuickprof.h"
  21 #include "LinearMath/btImplicitQRSVD.h"
  22 // This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
  23 class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
  24 {
  25 public:
  26         typedef btAlignedObjectArray<btVector3> TVStack;
  27         btScalar m_mu, m_lambda;  // Lame Parameters
  28         btScalar m_E, m_nu;       // Young's modulus and Poisson ratio
  29         btScalar m_mu_damp, m_lambda_damp;
  30         btDeformableNeoHookeanForce() : m_mu(1), m_lambda(1)
  31         {
  32                 btScalar damping = 0.05;
  33                 m_mu_damp = damping * m_mu;
  34                 m_lambda_damp = damping * m_lambda;
  35                 updateYoungsModulusAndPoissonRatio();
  36         }
  37
  38         btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
  39         {
  40                 m_mu_damp = damping * m_mu;
  41                 m_lambda_damp = damping * m_lambda;
  42                 updateYoungsModulusAndPoissonRatio();
  43         }
  44
  45         void updateYoungsModulusAndPoissonRatio()
  46         {
  47                 // conversion from Lame Parameters to Young's modulus and Poisson ratio
  48                 // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
  49                 m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
  50                 m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
  51         }
  52
  53         void updateLameParameters()
  54         {
  55                 // conversion from Young's modulus and Poisson ratio to Lame Parameters
  56                 // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
  57                 m_mu = m_E * 0.5 / (1 + m_nu);
  58                 m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
  59         }
  60
  61         void setYoungsModulus(btScalar E)
  62         {
  63                 m_E = E;
  64                 updateLameParameters();
  65         }
  66
  67         void setPoissonRatio(btScalar nu)
  68         {
  69                 m_nu = nu;
  70                 updateLameParameters();
  71         }
  72
  73         void setDamping(btScalar damping)
  74         {
  75                 m_mu_damp = damping * m_mu;
  76                 m_lambda_damp = damping * m_lambda;
  77         }
  78
  79         void setLameParameters(btScalar mu, btScalar lambda)
  80         {
  81                 m_mu = mu;
  82                 m_lambda = lambda;
  83                 updateYoungsModulusAndPoissonRatio();
  84         }
  85
  86         virtual void addScaledForces(btScalar scale, TVStack& force)
  87         {
  88                 addScaledDampingForce(scale, force);
  89                 addScaledElasticForce(scale, force);
  90         }
  91
  92         virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
  93         {
  94                 addScaledElasticForce(scale, force);
  95         }
  96
  97         // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
  98         virtual void addScaledDampingForce(btScalar scale, TVStack& force)
  99         {
 100                 if (m_mu_damp == 0 && m_lambda_damp == 0)
 101                         return;
 102                 int numNodes = getNumNodes();
 103                 btAssert(numNodes <= force.size());
 104                 btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
 105                 for (int i = 0; i < m_softBodies.size(); ++i)
 106                 {
 107                         btSoftBody* psb = m_softBodies[i];
 108                         if (!psb->isActive())
 109                         {
 110                                 continue;
 111                         }
 112                         for (int j = 0; j < psb->m_tetras.size(); ++j)
 113                         {
 114                                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
 115                                 btSoftBody::Node* node0 = tetra.m_n[0];
 116                                 btSoftBody::Node* node1 = tetra.m_n[1];
 117                                 btSoftBody::Node* node2 = tetra.m_n[2];
 118                                 btSoftBody::Node* node3 = tetra.m_n[3];
 119                                 size_t id0 = node0->index;
 120                                 size_t id1 = node1->index;
 121                                 size_t id2 = node2->index;
 122                                 size_t id3 = node3->index;
 123                                 btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
 124                                 btMatrix3x3 I;
 125                                 I.setIdentity();
 126                                 btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
 127                                 //                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
 128                                 btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col);
 129                                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
 130
 131                                 // damping force differential
 132                                 btScalar scale1 = scale * tetra.m_element_measure;
 133                                 force[id0] -= scale1 * df_on_node0;
 134                                 force[id1] -= scale1 * df_on_node123.getColumn(0);
 135                                 force[id2] -= scale1 * df_on_node123.getColumn(1);
 136                                 force[id3] -= scale1 * df_on_node123.getColumn(2);
 137                         }
 138                 }
 139         }
 140
 141         virtual double totalElasticEnergy(btScalar dt)
 142         {
 143                 double energy = 0;
 144                 for (int i = 0; i < m_softBodies.size(); ++i)
 145                 {
 146                         btSoftBody* psb = m_softBodies[i];
 147                         if (!psb->isActive())
 148                         {
 149                                 continue;
 150                         }
 151                         for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
 152                         {
 153                                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
 154                                 btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
 155                                 energy += tetra.m_element_measure * elasticEnergyDensity(s);
 156                         }
 157                 }
 158                 return energy;
 159         }
 160
 161         // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
 162         virtual double totalDampingEnergy(btScalar dt)
 163         {
 164                 double energy = 0;
 165                 int sz = 0;
 166                 for (int i = 0; i < m_softBodies.size(); ++i)
 167                 {
 168                         btSoftBody* psb = m_softBodies[i];
 169                         if (!psb->isActive())
 170                         {
 171                                 continue;
 172                         }
 173                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 174                         {
 175                                 sz = btMax(sz, psb->m_nodes[j].index);
 176                         }
 177                 }
 178                 TVStack dampingForce;
 179                 dampingForce.resize(sz + 1);
 180                 for (int i = 0; i < dampingForce.size(); ++i)
 181                         dampingForce[i].setZero();
 182                 addScaledDampingForce(0.5, dampingForce);
 183                 for (int i = 0; i < m_softBodies.size(); ++i)
 184                 {
 185                         btSoftBody* psb = m_softBodies[i];
 186                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 187                         {
 188                                 const btSoftBody::Node& node = psb->m_nodes[j];
 189                                 energy -= dampingForce[node.index].dot(node.m_v) / dt;
 190                         }
 191                 }
 192                 return energy;
 193         }
 194
 195         double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
 196         {
 197                 double density = 0;
 198                 density += m_mu * 0.5 * (s.m_trace - 3.);
 199                 density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda);
 200                 density -= m_mu * 0.5 * log(s.m_trace + 1);
 201                 return density;
 202         }
 203
 204         virtual void addScaledElasticForce(btScalar scale, TVStack& force)
 205         {
 206                 int numNodes = getNumNodes();
 207                 btAssert(numNodes <= force.size());
 208                 btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
 209                 for (int i = 0; i < m_softBodies.size(); ++i)
 210                 {
 211                         btSoftBody* psb = m_softBodies[i];
 212                         if (!psb->isActive())
 213                         {
 214                                 continue;
 215                         }
 216                         btScalar max_p = psb->m_cfg.m_maxStress;
 217                         for (int j = 0; j < psb->m_tetras.size(); ++j)
 218                         {
 219                                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
 220                                 btMatrix3x3 P;
 221                                 firstPiola(psb->m_tetraScratches[j], P);
 222 #ifdef USE_SVD
 223                                 if (max_p > 0)
 224                                 {
 225                                         // since we want to clamp the principal stress to max_p, we only need to
 226                                         // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
 227                                         btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
 228                                         if (trPTP > max_p * max_p)
 229                                         {
 230                                                 btMatrix3x3 U, V;
 231                                                 btVector3 sigma;
 232                                                 singularValueDecomposition(P, U, sigma, V);
 233                                                 sigma[0] = btMin(sigma[0], max_p);
 234                                                 sigma[1] = btMin(sigma[1], max_p);
 235                                                 sigma[2] = btMin(sigma[2], max_p);
 236                                                 sigma[0] = btMax(sigma[0], -max_p);
 237                                                 sigma[1] = btMax(sigma[1], -max_p);
 238                                                 sigma[2] = btMax(sigma[2], -max_p);
 239                                                 btMatrix3x3 Sigma;
 240                                                 Sigma.setIdentity();
 241                                                 Sigma[0][0] = sigma[0];
 242                                                 Sigma[1][1] = sigma[1];
 243                                                 Sigma[2][2] = sigma[2];
 244                                                 P = U * Sigma * V.transpose();
 245                                         }
 246                                 }
 247 #endif
 248                                 //                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
 249                                 btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
 250                                 btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
 251
 252                                 btSoftBody::Node* node0 = tetra.m_n[0];
 253                                 btSoftBody::Node* node1 = tetra.m_n[1];
 254                                 btSoftBody::Node* node2 = tetra.m_n[2];
 255                                 btSoftBody::Node* node3 = tetra.m_n[3];
 256                                 size_t id0 = node0->index;
 257                                 size_t id1 = node1->index;
 258                                 size_t id2 = node2->index;
 259                                 size_t id3 = node3->index;
 260
 261                                 // elastic force
 262                                 btScalar scale1 = scale * tetra.m_element_measure;
 263                                 force[id0] -= scale1 * force_on_node0;
 264                                 force[id1] -= scale1 * force_on_node123.getColumn(0);
 265                                 force[id2] -= scale1 * force_on_node123.getColumn(1);
 266                                 force[id3] -= scale1 * force_on_node123.getColumn(2);
 267                         }
 268                 }
 269         }
 270
 271         // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
 272         virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
 273         {
 274                 if (m_mu_damp == 0 && m_lambda_damp == 0)
 275                         return;
 276                 int numNodes = getNumNodes();
 277                 btAssert(numNodes <= df.size());
 278                 btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
 279                 for (int i = 0; i < m_softBodies.size(); ++i)
 280                 {
 281                         btSoftBody* psb = m_softBodies[i];
 282                         if (!psb->isActive())
 283                         {
 284                                 continue;
 285                         }
 286                         for (int j = 0; j < psb->m_tetras.size(); ++j)
 287                         {
 288                                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
 289                                 btSoftBody::Node* node0 = tetra.m_n[0];
 290                                 btSoftBody::Node* node1 = tetra.m_n[1];
 291                                 btSoftBody::Node* node2 = tetra.m_n[2];
 292                                 btSoftBody::Node* node3 = tetra.m_n[3];
 293                                 size_t id0 = node0->index;
 294                                 size_t id1 = node1->index;
 295                                 size_t id2 = node2->index;
 296                                 size_t id3 = node3->index;
 297                                 btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
 298                                 btMatrix3x3 I;
 299                                 I.setIdentity();
 300                                 btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
 301                                 //                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
 302                                 //                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
 303                                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
 304                                 btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
 305
 306                                 // damping force differential
 307                                 btScalar scale1 = scale * tetra.m_element_measure;
 308                                 df[id0] -= scale1 * df_on_node0;
 309                                 df[id1] -= scale1 * df_on_node123.getColumn(0);
 310                                 df[id2] -= scale1 * df_on_node123.getColumn(1);
 311                                 df[id3] -= scale1 * df_on_node123.getColumn(2);
 312                         }
 313                 }
 314         }
 315
 316         virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {}
 317
 318         virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
 319         {
 320                 int numNodes = getNumNodes();
 321                 btAssert(numNodes <= df.size());
 322                 btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
 323                 for (int i = 0; i < m_softBodies.size(); ++i)
 324                 {
 325                         btSoftBody* psb = m_softBodies[i];
 326                         if (!psb->isActive())
 327                         {
 328                                 continue;
 329                         }
 330                         for (int j = 0; j < psb->m_tetras.size(); ++j)
 331                         {
 332                                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
 333                                 btSoftBody::Node* node0 = tetra.m_n[0];
 334                                 btSoftBody::Node* node1 = tetra.m_n[1];
 335                                 btSoftBody::Node* node2 = tetra.m_n[2];
 336                                 btSoftBody::Node* node3 = tetra.m_n[3];
 337                                 size_t id0 = node0->index;
 338                                 size_t id1 = node1->index;
 339                                 size_t id2 = node2->index;
 340                                 size_t id3 = node3->index;
 341                                 btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
 342                                 btMatrix3x3 dP;
 343                                 firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
 344                                 //                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
 345                                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
 346                                 btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
 347
 348                                 // elastic force differential
 349                                 btScalar scale1 = scale * tetra.m_element_measure;
 350                                 df[id0] -= scale1 * df_on_node0;
 351                                 df[id1] -= scale1 * df_on_node123.getColumn(0);
 352                                 df[id2] -= scale1 * df_on_node123.getColumn(1);
 353                                 df[id3] -= scale1 * df_on_node123.getColumn(2);
 354                         }
 355                 }
 356         }
 357
 358         void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
 359         {
 360                 btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.)));
 361                 btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
 362                 P = s.m_F * c1 + s.m_cofF * c2;
 363         }
 364
 365         // Let P be the first piola stress.
 366         // This function calculates the dP = dP/dF * dF
 367         void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
 368         {
 369                 btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.));
 370                 btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)));
 371                 btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
 372                 dP = dF * c1 + s.m_F * c2;
 373                 addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP);
 374                 dP += s.m_cofF * c3;
 375         }
 376
 377         // Let Q be the damping stress.
 378         // This function calculates the dP = dQ/dF * dF
 379         void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
 380         {
 381                 btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.)));
 382                 btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))));
 383                 btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
 384                 dP = dF * c1 + s.m_F * c2;
 385                 addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP);
 386                 dP += s.m_cofF * c3;
 387         }
 388
 389         btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
 390         {
 391                 btScalar ans = 0;
 392                 for (int i = 0; i < 3; ++i)
 393                 {
 394                         ans += A[i].dot(B[i]);
 395                 }
 396                 return ans;
 397         }
 398
 399         // Let C(A) be the cofactor of the matrix A
 400         // Let H = the derivative of C(A) with respect to A evaluated at F = A
 401         // This function calculates H*dF
 402         void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
 403         {
 404                 M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
 405                 M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
 406                 M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
 407                 M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
 408                 M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
 409                 M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
 410                 M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
 411                 M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
 412                 M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
 413         }
 414
 415         virtual btDeformableLagrangianForceType getForceType()
 416         {
 417                 return BT_NEOHOOKEAN_FORCE;
 418         }
 419 };
 420 #endif /* BT_NEOHOOKEAN_H */