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