dali-physics/third-party/bullet3/src/BulletSoftBody/btPreconditioner.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_PRECONDITIONER_H
  17 #define BT_PRECONDITIONER_H
  18
  19 class Preconditioner
  20 {
  21 public:
  22         typedef btAlignedObjectArray<btVector3> TVStack;
  23         virtual void operator()(const TVStack& x, TVStack& b) = 0;
  24         virtual void reinitialize(bool nodeUpdated) = 0;
  25         virtual ~Preconditioner() {}
  26 };
  27
  28 class DefaultPreconditioner : public Preconditioner
  29 {
  30 public:
  31         virtual void operator()(const TVStack& x, TVStack& b)
  32         {
  33                 btAssert(b.size() == x.size());
  34                 for (int i = 0; i < b.size(); ++i)
  35                         b[i] = x[i];
  36         }
  37         virtual void reinitialize(bool nodeUpdated)
  38         {
  39         }
  40
  41         virtual ~DefaultPreconditioner() {}
  42 };
  43
  44 class MassPreconditioner : public Preconditioner
  45 {
  46         btAlignedObjectArray<btScalar> m_inv_mass;
  47         const btAlignedObjectArray<btSoftBody*>& m_softBodies;
  48
  49 public:
  50         MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies)
  51                 : m_softBodies(softBodies)
  52         {
  53         }
  54
  55         virtual void reinitialize(bool nodeUpdated)
  56         {
  57                 if (nodeUpdated)
  58                 {
  59                         m_inv_mass.clear();
  60                         for (int i = 0; i < m_softBodies.size(); ++i)
  61                         {
  62                                 btSoftBody* psb = m_softBodies[i];
  63                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
  64                                         m_inv_mass.push_back(psb->m_nodes[j].m_im);
  65                         }
  66                 }
  67         }
  68
  69         virtual void operator()(const TVStack& x, TVStack& b)
  70         {
  71                 btAssert(b.size() == x.size());
  72                 btAssert(m_inv_mass.size() <= x.size());
  73                 for (int i = 0; i < m_inv_mass.size(); ++i)
  74                 {
  75                         b[i] = x[i] * m_inv_mass[i];
  76                 }
  77                 for (int i = m_inv_mass.size(); i < b.size(); ++i)
  78                 {
  79                         b[i] = x[i];
  80                 }
  81         }
  82 };
  83
  84 class KKTPreconditioner : public Preconditioner
  85 {
  86         const btAlignedObjectArray<btSoftBody*>& m_softBodies;
  87         const btDeformableContactProjection& m_projections;
  88         const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
  89         TVStack m_inv_A, m_inv_S;
  90         const btScalar& m_dt;
  91         const bool& m_implicit;
  92
  93 public:
  94         KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
  95                 : m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
  96         {
  97         }
  98
  99         virtual void reinitialize(bool nodeUpdated)
 100         {
 101                 if (nodeUpdated)
 102                 {
 103                         int num_nodes = 0;
 104                         for (int i = 0; i < m_softBodies.size(); ++i)
 105                         {
 106                                 btSoftBody* psb = m_softBodies[i];
 107                                 num_nodes += psb->m_nodes.size();
 108                         }
 109                         m_inv_A.resize(num_nodes);
 110                 }
 111                 buildDiagonalA(m_inv_A);
 112                 for (int i = 0; i < m_inv_A.size(); ++i)
 113                 {
 114                         //            printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]);
 115                         for (int d = 0; d < 3; ++d)
 116                         {
 117                                 m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d];
 118                         }
 119                 }
 120                 m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
 121                 //        printf("S.size() = %d \n", m_inv_S.size());
 122                 buildDiagonalS(m_inv_A, m_inv_S);
 123                 for (int i = 0; i < m_inv_S.size(); ++i)
 124                 {
 125                         //            printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]);
 126                         for (int d = 0; d < 3; ++d)
 127                         {
 128                                 m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d];
 129                         }
 130                 }
 131         }
 132
 133         void buildDiagonalA(TVStack& diagA) const
 134         {
 135                 size_t counter = 0;
 136                 for (int i = 0; i < m_softBodies.size(); ++i)
 137                 {
 138                         btSoftBody* psb = m_softBodies[i];
 139                         for (int j = 0; j < psb->m_nodes.size(); ++j)
 140                         {
 141                                 const btSoftBody::Node& node = psb->m_nodes[j];
 142                                 diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im);
 143                                 ++counter;
 144                         }
 145                 }
 146                 if (m_implicit)
 147                 {
 148                         printf("implicit not implemented\n");
 149                         btAssert(false);
 150                 }
 151                 for (int i = 0; i < m_lf.size(); ++i)
 152                 {
 153                         // add damping matrix
 154                         m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA);
 155                 }
 156         }
 157
 158         void buildDiagonalS(const TVStack& inv_A, TVStack& diagS)
 159         {
 160                 for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
 161                 {
 162                         // S[k,k] = e_k^T * C A_d^-1 C^T * e_k
 163                         const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
 164                         btVector3& t = diagS[c];
 165                         t.setZero();
 166                         for (int j = 0; j < lm.m_num_constraints; ++j)
 167                         {
 168                                 for (int i = 0; i < lm.m_num_nodes; ++i)
 169                                 {
 170                                         for (int d = 0; d < 3; ++d)
 171                                         {
 172                                                 t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i];
 173                                         }
 174                                 }
 175                         }
 176                 }
 177         }
 178 //#define USE_FULL_PRECONDITIONER
 179 #ifndef USE_FULL_PRECONDITIONER
 180         virtual void operator()(const TVStack& x, TVStack& b)
 181         {
 182                 btAssert(b.size() == x.size());
 183                 for (int i = 0; i < m_inv_A.size(); ++i)
 184                 {
 185                         b[i] = x[i] * m_inv_A[i];
 186                 }
 187                 int offset = m_inv_A.size();
 188                 for (int i = 0; i < m_inv_S.size(); ++i)
 189                 {
 190                         b[i + offset] = x[i + offset] * m_inv_S[i];
 191                 }
 192         }
 193 #else
 194         virtual void operator()(const TVStack& x, TVStack& b)
 195         {
 196                 btAssert(b.size() == x.size());
 197                 int offset = m_inv_A.size();
 198
 199                 for (int i = 0; i < m_inv_A.size(); ++i)
 200                 {
 201                         b[i] = x[i] * m_inv_A[i];
 202                 }
 203
 204                 for (int i = 0; i < m_inv_S.size(); ++i)
 205                 {
 206                         b[i + offset].setZero();
 207                 }
 208
 209                 for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
 210                 {
 211                         const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
 212                         // C * x
 213                         for (int d = 0; d < lm.m_num_constraints; ++d)
 214                         {
 215                                 for (int i = 0; i < lm.m_num_nodes; ++i)
 216                                 {
 217                                         b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]);
 218                                 }
 219                         }
 220                 }
 221
 222                 for (int i = 0; i < m_inv_S.size(); ++i)
 223                 {
 224                         b[i + offset] = b[i + offset] * m_inv_S[i];
 225                 }
 226
 227                 for (int i = 0; i < m_inv_A.size(); ++i)
 228                 {
 229                         b[i].setZero();
 230                 }
 231
 232                 for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
 233                 {
 234                         // C^T * lambda
 235                         const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
 236                         for (int i = 0; i < lm.m_num_nodes; ++i)
 237                         {
 238                                 for (int j = 0; j < lm.m_num_constraints; ++j)
 239                                 {
 240                                         b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
 241                                 }
 242                         }
 243                 }
 244
 245                 for (int i = 0; i < m_inv_A.size(); ++i)
 246                 {
 247                         b[i] = (x[i] - b[i]) * m_inv_A[i];
 248                 }
 249
 250                 TVStack t;
 251                 t.resize(b.size());
 252                 for (int i = 0; i < m_inv_S.size(); ++i)
 253                 {
 254                         t[i + offset] = x[i + offset] * m_inv_S[i];
 255                 }
 256                 for (int i = 0; i < m_inv_A.size(); ++i)
 257                 {
 258                         t[i].setZero();
 259                 }
 260                 for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
 261                 {
 262                         // C^T * lambda
 263                         const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
 264                         for (int i = 0; i < lm.m_num_nodes; ++i)
 265                         {
 266                                 for (int j = 0; j < lm.m_num_constraints; ++j)
 267                                 {
 268                                         t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
 269                                 }
 270                         }
 271                 }
 272                 for (int i = 0; i < m_inv_A.size(); ++i)
 273                 {
 274                         b[i] += t[i] * m_inv_A[i];
 275                 }
 276
 277                 for (int i = 0; i < m_inv_S.size(); ++i)
 278                 {
 279                         b[i + offset] -= x[i + offset] * m_inv_S[i];
 280                 }
 281         }
 282 #endif
 283 };
 284
 285 #endif /* BT_PRECONDITIONER_H */