dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableMassSpringForce.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_MASS_SPRING_H
  17 #define BT_MASS_SPRING_H
  18
  19 #include "btDeformableLagrangianForce.h"
  20
  21 class btDeformableMassSpringForce : public btDeformableLagrangianForce
  22 {
  23         // If true, the damping force will be in the direction of the spring
  24         // If false, the damping force will be in the direction of the velocity
  25         bool m_momentum_conserving;
  26         btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
  27
  28 public:
  29         typedef btAlignedObjectArray<btVector3> TVStack;
  30         btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
  31         {
  32         }
  33         btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k)
  34         {
  35                 if (m_bendingStiffness < btScalar(0))
  36                 {
  37                         m_bendingStiffness = m_elasticStiffness;
  38                 }
  39         }
  40
  41         virtual void addScaledForces(btScalar scale, TVStack& force)
  42         {
  43                 addScaledDampingForce(scale, force);
  44                 addScaledElasticForce(scale, force);
  45         }
  46
  47         virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
  48         {
  49                 addScaledElasticForce(scale, force);
  50         }
  51
  52         virtual void addScaledDampingForce(btScalar scale, TVStack& force)
  53         {
  54                 int numNodes = getNumNodes();
  55                 btAssert(numNodes <= force.size());
  56                 for (int i = 0; i < m_softBodies.size(); ++i)
  57                 {
  58                         const btSoftBody* psb = m_softBodies[i];
  59                         if (!psb->isActive())
  60                         {
  61                                 continue;
  62                         }
  63                         for (int j = 0; j < psb->m_links.size(); ++j)
  64                         {
  65                                 const btSoftBody::Link& link = psb->m_links[j];
  66                                 btSoftBody::Node* node1 = link.m_n[0];
  67                                 btSoftBody::Node* node2 = link.m_n[1];
  68                                 size_t id1 = node1->index;
  69                                 size_t id2 = node2->index;
  70
  71                                 // damping force
  72                                 btVector3 v_diff = (node2->m_v - node1->m_v);
  73                                 btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
  74                                 if (m_momentum_conserving)
  75                                 {
  76                                         if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
  77                                         {
  78                                                 btVector3 dir = (node2->m_x - node1->m_x).normalized();
  79                                                 scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
  80                                         }
  81                                 }
  82                                 force[id1] += scaled_force;
  83                                 force[id2] -= scaled_force;
  84                         }
  85                 }
  86         }
  87
  88         virtual void addScaledElasticForce(btScalar scale, TVStack& force)
  89         {
  90                 int numNodes = getNumNodes();
  91                 btAssert(numNodes <= force.size());
  92                 for (int i = 0; i < m_softBodies.size(); ++i)
  93                 {
  94                         const btSoftBody* psb = m_softBodies[i];
  95                         if (!psb->isActive())
  96                         {
  97                                 continue;
  98                         }
  99                         for (int j = 0; j < psb->m_links.size(); ++j)
 100                         {
 101                                 const btSoftBody::Link& link = psb->m_links[j];
 102                                 btSoftBody::Node* node1 = link.m_n[0];
 103                                 btSoftBody::Node* node2 = link.m_n[1];
 104                                 btScalar r = link.m_rl;
 105                                 size_t id1 = node1->index;
 106                                 size_t id2 = node2->index;
 107
 108                                 // elastic force
 109                                 btVector3 dir = (node2->m_q - node1->m_q);
 110                                 btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
 111                                 btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
 112                                 btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
 113                                 force[id1] += scaled_force;
 114                                 force[id2] -= scaled_force;
 115                         }
 116                 }
 117         }
 118
 119         virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
 120         {
 121                 // implicit damping force differential
 122                 for (int i = 0; i < m_softBodies.size(); ++i)
 123                 {
 124                         btSoftBody* psb = m_softBodies[i];
 125                         if (!psb->isActive())
 126                         {
 127                                 continue;
 128                         }
 129                         btScalar scaled_k_damp = m_dampingStiffness * scale;
 130                         for (int j = 0; j < psb->m_links.size(); ++j)
 131                         {
 132                                 const btSoftBody::Link& link = psb->m_links[j];
 133                                 btSoftBody::Node* node1 = link.m_n[0];
 134                                 btSoftBody::Node* node2 = link.m_n[1];
 135                                 size_t id1 = node1->index;
 136                                 size_t id2 = node2->index;
 137
 138                                 btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
 139                                 if (m_momentum_conserving)
 140                                 {
 141                                         if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
 142                                         {
 143                                                 btVector3 dir = (node2->m_x - node1->m_x).normalized();
 144                                                 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
 145                                         }
 146                                 }
 147                                 df[id1] += local_scaled_df;
 148                                 df[id2] -= local_scaled_df;
 149                         }
 150                 }
 151         }
 152
 153         virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
 154         {
 155                 // implicit damping force differential
 156                 for (int i = 0; i < m_softBodies.size(); ++i)
 157                 {
 158                         btSoftBody* psb = m_softBodies[i];
 159                         if (!psb->isActive())
 160                         {
 161                                 continue;
 162                         }
 163                         btScalar scaled_k_damp = m_dampingStiffness * scale;
 164                         for (int j = 0; j < psb->m_links.size(); ++j)
 165                         {
 166                                 const btSoftBody::Link& link = psb->m_links[j];
 167                                 btSoftBody::Node* node1 = link.m_n[0];
 168                                 btSoftBody::Node* node2 = link.m_n[1];
 169                                 size_t id1 = node1->index;
 170                                 size_t id2 = node2->index;
 171                                 if (m_momentum_conserving)
 172                                 {
 173                                         if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
 174                                         {
 175                                                 btVector3 dir = (node2->m_x - node1->m_x).normalized();
 176                                                 for (int d = 0; d < 3; ++d)
 177                                                 {
 178                                                         if (node1->m_im > 0)
 179                                                                 diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
 180                                                         if (node2->m_im > 0)
 181                                                                 diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
 182                                                 }
 183                                         }
 184                                 }
 185                                 else
 186                                 {
 187                                         for (int d = 0; d < 3; ++d)
 188                                         {
 189                                                 if (node1->m_im > 0)
 190                                                         diagA[id1][d] -= scaled_k_damp;
 191                                                 if (node2->m_im > 0)
 192                                                         diagA[id2][d] -= scaled_k_damp;
 193                                         }
 194                                 }
 195                         }
 196                 }
 197         }
 198
 199         virtual double totalElasticEnergy(btScalar dt)
 200         {
 201                 double energy = 0;
 202                 for (int i = 0; i < m_softBodies.size(); ++i)
 203                 {
 204                         const btSoftBody* psb = m_softBodies[i];
 205                         if (!psb->isActive())
 206                         {
 207                                 continue;
 208                         }
 209                         for (int j = 0; j < psb->m_links.size(); ++j)
 210                         {
 211                                 const btSoftBody::Link& link = psb->m_links[j];
 212                                 btSoftBody::Node* node1 = link.m_n[0];
 213                                 btSoftBody::Node* node2 = link.m_n[1];
 214                                 btScalar r = link.m_rl;
 215
 216                                 // elastic force
 217                                 btVector3 dir = (node2->m_q - node1->m_q);
 218                                 energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r);
 219                         }
 220                 }
 221                 return energy;
 222         }
 223
 224         virtual double totalDampingEnergy(btScalar dt)
 225         {
 226                 double energy = 0;
 227                 int sz = 0;
 228                 for (int i = 0; i < m_softBodies.size(); ++i)
 229                 {
 230                         btSoftBody* psb = m_softBodies[i];
 231                         if (!psb->isActive())
 232                         {
 233                                 continue;
 234                         }
 235                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 236                         {
 237                                 sz = btMax(sz, psb->m_nodes[j].index);
 238                         }
 239                 }
 240                 TVStack dampingForce;
 241                 dampingForce.resize(sz + 1);
 242                 for (int i = 0; i < dampingForce.size(); ++i)
 243                         dampingForce[i].setZero();
 244                 addScaledDampingForce(0.5, dampingForce);
 245                 for (int i = 0; i < m_softBodies.size(); ++i)
 246                 {
 247                         btSoftBody* psb = m_softBodies[i];
 248                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 249                         {
 250                                 const btSoftBody::Node& node = psb->m_nodes[j];
 251                                 energy -= dampingForce[node.index].dot(node.m_v) / dt;
 252                         }
 253                 }
 254                 return energy;
 255         }
 256
 257         virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
 258         {
 259                 // implicit damping force differential
 260                 for (int i = 0; i < m_softBodies.size(); ++i)
 261                 {
 262                         const btSoftBody* psb = m_softBodies[i];
 263                         if (!psb->isActive())
 264                         {
 265                                 continue;
 266                         }
 267                         for (int j = 0; j < psb->m_links.size(); ++j)
 268                         {
 269                                 const btSoftBody::Link& link = psb->m_links[j];
 270                                 btSoftBody::Node* node1 = link.m_n[0];
 271                                 btSoftBody::Node* node2 = link.m_n[1];
 272                                 size_t id1 = node1->index;
 273                                 size_t id2 = node2->index;
 274                                 btScalar r = link.m_rl;
 275
 276                                 btVector3 dir = (node1->m_q - node2->m_q);
 277                                 btScalar dir_norm = dir.norm();
 278                                 btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
 279                                 btVector3 dx_diff = dx[id1] - dx[id2];
 280                                 btVector3 scaled_df = btVector3(0, 0, 0);
 281                                 btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
 282                                 if (dir_norm > SIMD_EPSILON)
 283                                 {
 284                                         scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
 285                                         scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
 286                                         scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff;
 287                                 }
 288
 289                                 df[id1] += scaled_df;
 290                                 df[id2] -= scaled_df;
 291                         }
 292                 }
 293         }
 294
 295         virtual btDeformableLagrangianForceType getForceType()
 296         {
 297                 return BT_MASSSPRING_FORCE;
 298         }
 299 };
 300
 301 #endif /* btMassSpring_h */