dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableContactProjection.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 "btDeformableContactProjection.h"
  17 #include "btDeformableMultiBodyDynamicsWorld.h"
  18 #include <algorithm>
  19 #include <cmath>
  20 btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
  21 {
  22         btScalar residualSquare = 0;
  23         for (int i = 0; i < numDeformableBodies; ++i)
  24         {
  25                 for (int j = 0; j < m_softBodies.size(); ++j)
  26                 {
  27                         btCollisionObject* psb = m_softBodies[j];
  28                         if (psb != deformableBodies[i])
  29                         {
  30                                 continue;
  31                         }
  32                         for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
  33                         {
  34                                 btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
  35                                 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
  36                                 residualSquare = btMax(residualSquare, localResidualSquare);
  37                         }
  38                         for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
  39                         {
  40                                 btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
  41                                 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
  42                                 residualSquare = btMax(residualSquare, localResidualSquare);
  43                         }
  44                         for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
  45                         {
  46                                 btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
  47                                 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
  48                                 residualSquare = btMax(residualSquare, localResidualSquare);
  49                         }
  50                         for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
  51                         {
  52                                 btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
  53                                 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
  54                                 residualSquare = btMax(residualSquare, localResidualSquare);
  55                         }
  56                 }
  57         }
  58         return residualSquare;
  59 }
  60
  61 btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
  62 {
  63         btScalar residualSquare = 0;
  64         for (int i = 0; i < numDeformableBodies; ++i)
  65         {
  66                 for (int j = 0; j < m_softBodies.size(); ++j)
  67                 {
  68                         btCollisionObject* psb = m_softBodies[j];
  69                         if (psb != deformableBodies[i])
  70                         {
  71                                 continue;
  72                         }
  73                         for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
  74                         {
  75                                 btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
  76                                 btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
  77                                 residualSquare = btMax(residualSquare, localResidualSquare);
  78                         }
  79                         for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
  80                         {
  81                                 btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
  82                                 btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
  83                                 residualSquare = btMax(residualSquare, localResidualSquare);
  84                         }
  85                 }
  86         }
  87         return residualSquare;
  88 }
  89
  90 void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
  91 {
  92         BT_PROFILE("setConstraints");
  93         for (int i = 0; i < m_softBodies.size(); ++i)
  94         {
  95                 btSoftBody* psb = m_softBodies[i];
  96                 if (!psb->isActive())
  97                 {
  98                         continue;
  99                 }
 100
 101                 // set Dirichlet constraint
 102                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 103                 {
 104                         if (psb->m_nodes[j].m_im == 0)
 105                         {
 106                                 btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
 107                                 m_staticConstraints[i].push_back(static_constraint);
 108                         }
 109                 }
 110
 111                 // set up deformable anchors
 112                 for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
 113                 {
 114                         btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
 115                         // skip fixed points
 116                         if (anchor.m_node->m_im == 0)
 117                         {
 118                                 continue;
 119                         }
 120                         anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
 121                         btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
 122                         m_nodeAnchorConstraints[i].push_back(constraint);
 123                 }
 124
 125                 // set Deformable Node vs. Rigid constraint
 126                 for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
 127                 {
 128                         const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
 129                         // skip fixed points
 130                         if (contact.m_node->m_im == 0)
 131                         {
 132                                 continue;
 133                         }
 134                         btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
 135                         m_nodeRigidConstraints[i].push_back(constraint);
 136                 }
 137
 138                 // set Deformable Face vs. Rigid constraint
 139                 for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
 140                 {
 141                         const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
 142                         // skip fixed faces
 143                         if (contact.m_c2 == 0)
 144                         {
 145                                 continue;
 146                         }
 147                         btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
 148                         m_faceRigidConstraints[i].push_back(constraint);
 149                 }
 150         }
 151 }
 152
 153 void btDeformableContactProjection::project(TVStack& x)
 154 {
 155 #ifndef USE_MGS
 156         const int dim = 3;
 157         for (int index = 0; index < m_projectionsDict.size(); ++index)
 158         {
 159                 btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
 160                 size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
 161                 if (projectionDirs.size() >= dim)
 162                 {
 163                         // static node
 164                         x[i].setZero();
 165                         continue;
 166                 }
 167                 else if (projectionDirs.size() == 2)
 168                 {
 169                         btVector3 dir0 = projectionDirs[0];
 170                         btVector3 dir1 = projectionDirs[1];
 171                         btVector3 free_dir = btCross(dir0, dir1);
 172                         if (free_dir.safeNorm() < SIMD_EPSILON)
 173                         {
 174                                 x[i] -= x[i].dot(dir0) * dir0;
 175                         }
 176                         else
 177                         {
 178                                 free_dir.normalize();
 179                                 x[i] = x[i].dot(free_dir) * free_dir;
 180                         }
 181                 }
 182                 else
 183                 {
 184                         btAssert(projectionDirs.size() == 1);
 185                         btVector3 dir0 = projectionDirs[0];
 186                         x[i] -= x[i].dot(dir0) * dir0;
 187                 }
 188         }
 189 #else
 190         btReducedVector p(x.size());
 191         for (int i = 0; i < m_projections.size(); ++i)
 192         {
 193                 p += (m_projections[i].dot(x) * m_projections[i]);
 194         }
 195         for (int i = 0; i < p.m_indices.size(); ++i)
 196         {
 197                 x[p.m_indices[i]] -= p.m_vecs[i];
 198         }
 199 #endif
 200 }
 201
 202 void btDeformableContactProjection::setProjection()
 203 {
 204 #ifndef USE_MGS
 205         BT_PROFILE("btDeformableContactProjection::setProjection");
 206         btAlignedObjectArray<btVector3> units;
 207         units.push_back(btVector3(1, 0, 0));
 208         units.push_back(btVector3(0, 1, 0));
 209         units.push_back(btVector3(0, 0, 1));
 210         for (int i = 0; i < m_softBodies.size(); ++i)
 211         {
 212                 btSoftBody* psb = m_softBodies[i];
 213                 if (!psb->isActive())
 214                 {
 215                         continue;
 216                 }
 217                 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
 218                 {
 219                         int index = m_staticConstraints[i][j].m_node->index;
 220                         m_staticConstraints[i][j].m_node->m_constrained = true;
 221                         if (m_projectionsDict.find(index) == NULL)
 222                         {
 223                                 m_projectionsDict.insert(index, units);
 224                         }
 225                         else
 226                         {
 227                                 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
 228                                 for (int k = 0; k < 3; ++k)
 229                                 {
 230                                         projections.push_back(units[k]);
 231                                 }
 232                         }
 233                 }
 234                 for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
 235                 {
 236                         int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
 237                         m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
 238                         if (m_projectionsDict.find(index) == NULL)
 239                         {
 240                                 m_projectionsDict.insert(index, units);
 241                         }
 242                         else
 243                         {
 244                                 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
 245                                 for (int k = 0; k < 3; ++k)
 246                                 {
 247                                         projections.push_back(units[k]);
 248                                 }
 249                         }
 250                 }
 251                 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 252                 {
 253                         int index = m_nodeRigidConstraints[i][j].m_node->index;
 254                         m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
 255                         if (m_nodeRigidConstraints[i][j].m_binding)
 256                         {
 257                                 if (m_nodeRigidConstraints[i][j].m_static)
 258                                 {
 259                                         if (m_projectionsDict.find(index) == NULL)
 260                                         {
 261                                                 m_projectionsDict.insert(index, units);
 262                                         }
 263                                         else
 264                                         {
 265                                                 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
 266                                                 for (int k = 0; k < 3; ++k)
 267                                                 {
 268                                                         projections.push_back(units[k]);
 269                                                 }
 270                                         }
 271                                 }
 272                                 else
 273                                 {
 274                                         if (m_projectionsDict.find(index) == NULL)
 275                                         {
 276                                                 btAlignedObjectArray<btVector3> projections;
 277                                                 projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
 278                                                 m_projectionsDict.insert(index, projections);
 279                                         }
 280                                         else
 281                                         {
 282                                                 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
 283                                                 projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
 284                                         }
 285                                 }
 286                         }
 287                 }
 288                 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
 289                 {
 290                         const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
 291                         if (m_faceRigidConstraints[i][j].m_binding)
 292                         {
 293                                 for (int k = 0; k < 3; ++k)
 294                                 {
 295                                         face->m_n[k]->m_constrained = true;
 296                                 }
 297                         }
 298                         for (int k = 0; k < 3; ++k)
 299                         {
 300                                 btSoftBody::Node* node = face->m_n[k];
 301                                 int index = node->index;
 302                                 if (m_faceRigidConstraints[i][j].m_static)
 303                                 {
 304                                         if (m_projectionsDict.find(index) == NULL)
 305                                         {
 306                                                 m_projectionsDict.insert(index, units);
 307                                         }
 308                                         else
 309                                         {
 310                                                 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
 311                                                 for (int l = 0; l < 3; ++l)
 312                                                 {
 313                                                         projections.push_back(units[l]);
 314                                                 }
 315                                         }
 316                                 }
 317                                 else
 318                                 {
 319                                         if (m_projectionsDict.find(index) == NULL)
 320                                         {
 321                                                 btAlignedObjectArray<btVector3> projections;
 322                                                 projections.push_back(m_faceRigidConstraints[i][j].m_normal);
 323                                                 m_projectionsDict.insert(index, projections);
 324                                         }
 325                                         else
 326                                         {
 327                                                 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
 328                                                 projections.push_back(m_faceRigidConstraints[i][j].m_normal);
 329                                         }
 330                                 }
 331                         }
 332                 }
 333         }
 334 #else
 335         int dof = 0;
 336         for (int i = 0; i < m_softBodies.size(); ++i)
 337         {
 338                 dof += m_softBodies[i]->m_nodes.size();
 339         }
 340         for (int i = 0; i < m_softBodies.size(); ++i)
 341         {
 342                 btSoftBody* psb = m_softBodies[i];
 343                 if (!psb->isActive())
 344                 {
 345                         continue;
 346                 }
 347                 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
 348                 {
 349                         int index = m_staticConstraints[i][j].m_node->index;
 350                         m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
 351                         btAlignedObjectArray<int> indices;
 352                         btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
 353                         indices.push_back(index);
 354                         vecs1.push_back(btVector3(1, 0, 0));
 355                         vecs2.push_back(btVector3(0, 1, 0));
 356                         vecs3.push_back(btVector3(0, 0, 1));
 357                         m_projections.push_back(btReducedVector(dof, indices, vecs1));
 358                         m_projections.push_back(btReducedVector(dof, indices, vecs2));
 359                         m_projections.push_back(btReducedVector(dof, indices, vecs3));
 360                 }
 361
 362                 for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
 363                 {
 364                         int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
 365                         m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
 366                         btAlignedObjectArray<int> indices;
 367                         btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
 368                         indices.push_back(index);
 369                         vecs1.push_back(btVector3(1, 0, 0));
 370                         vecs2.push_back(btVector3(0, 1, 0));
 371                         vecs3.push_back(btVector3(0, 0, 1));
 372                         m_projections.push_back(btReducedVector(dof, indices, vecs1));
 373                         m_projections.push_back(btReducedVector(dof, indices, vecs2));
 374                         m_projections.push_back(btReducedVector(dof, indices, vecs3));
 375                 }
 376                 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 377                 {
 378                         int index = m_nodeRigidConstraints[i][j].m_node->index;
 379                         m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
 380                         btAlignedObjectArray<int> indices;
 381                         indices.push_back(index);
 382                         btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
 383                         if (m_nodeRigidConstraints[i][j].m_static)
 384                         {
 385                                 vecs1.push_back(btVector3(1, 0, 0));
 386                                 vecs2.push_back(btVector3(0, 1, 0));
 387                                 vecs3.push_back(btVector3(0, 0, 1));
 388                                 m_projections.push_back(btReducedVector(dof, indices, vecs1));
 389                                 m_projections.push_back(btReducedVector(dof, indices, vecs2));
 390                                 m_projections.push_back(btReducedVector(dof, indices, vecs3));
 391                         }
 392                         else
 393                         {
 394                                 vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
 395                                 m_projections.push_back(btReducedVector(dof, indices, vecs1));
 396                         }
 397                 }
 398                 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
 399                 {
 400                         const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
 401                         btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
 402                         btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
 403                         for (int k = 0; k < 3; ++k)
 404                         {
 405                                 face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
 406                         }
 407                         if (m_faceRigidConstraints[i][j].m_static)
 408                         {
 409                                 for (int l = 0; l < 3; ++l)
 410                                 {
 411                                         btReducedVector rv(dof);
 412                                         for (int k = 0; k < 3; ++k)
 413                                         {
 414                                                 rv.m_indices.push_back(face->m_n[k]->index);
 415                                                 btVector3 v(0, 0, 0);
 416                                                 v[l] = bary[k];
 417                                                 rv.m_vecs.push_back(v);
 418                                                 rv.sort();
 419                                         }
 420                                         m_projections.push_back(rv);
 421                                 }
 422                         }
 423                         else
 424                         {
 425                                 btReducedVector rv(dof);
 426                                 for (int k = 0; k < 3; ++k)
 427                                 {
 428                                         rv.m_indices.push_back(face->m_n[k]->index);
 429                                         rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
 430                                         rv.sort();
 431                                 }
 432                                 m_projections.push_back(rv);
 433                         }
 434                 }
 435         }
 436         btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
 437         mgs.solve();
 438         m_projections = mgs.m_out;
 439 #endif
 440 }
 441
 442 void btDeformableContactProjection::checkConstraints(const TVStack& x)
 443 {
 444         for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
 445         {
 446                 btVector3 d(0, 0, 0);
 447                 const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
 448                 for (int j = 0; j < lm.m_num_constraints; ++j)
 449                 {
 450                         for (int k = 0; k < lm.m_num_nodes; ++k)
 451                         {
 452                                 d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
 453                         }
 454                 }
 455                 //              printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
 456                 //        printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
 457         }
 458 }
 459
 460 void btDeformableContactProjection::setLagrangeMultiplier()
 461 {
 462         for (int i = 0; i < m_softBodies.size(); ++i)
 463         {
 464                 btSoftBody* psb = m_softBodies[i];
 465                 if (!psb->isActive())
 466                 {
 467                         continue;
 468                 }
 469                 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
 470                 {
 471                         int index = m_staticConstraints[i][j].m_node->index;
 472                         m_staticConstraints[i][j].m_node->m_constrained = true;
 473                         LagrangeMultiplier lm;
 474                         lm.m_num_nodes = 1;
 475                         lm.m_indices[0] = index;
 476                         lm.m_weights[0] = 1.0;
 477                         lm.m_num_constraints = 3;
 478                         lm.m_dirs[0] = btVector3(1, 0, 0);
 479                         lm.m_dirs[1] = btVector3(0, 1, 0);
 480                         lm.m_dirs[2] = btVector3(0, 0, 1);
 481                         m_lagrangeMultipliers.push_back(lm);
 482                 }
 483                 for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
 484                 {
 485                         int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
 486                         m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
 487                         LagrangeMultiplier lm;
 488                         lm.m_num_nodes = 1;
 489                         lm.m_indices[0] = index;
 490                         lm.m_weights[0] = 1.0;
 491                         lm.m_num_constraints = 3;
 492                         lm.m_dirs[0] = btVector3(1, 0, 0);
 493                         lm.m_dirs[1] = btVector3(0, 1, 0);
 494                         lm.m_dirs[2] = btVector3(0, 0, 1);
 495                         m_lagrangeMultipliers.push_back(lm);
 496                 }
 497
 498                 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 499                 {
 500                         if (!m_nodeRigidConstraints[i][j].m_binding)
 501                         {
 502                                 continue;
 503                         }
 504                         int index = m_nodeRigidConstraints[i][j].m_node->index;
 505                         m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
 506                         LagrangeMultiplier lm;
 507                         lm.m_num_nodes = 1;
 508                         lm.m_indices[0] = index;
 509                         lm.m_weights[0] = 1.0;
 510                         if (m_nodeRigidConstraints[i][j].m_static)
 511                         {
 512                                 lm.m_num_constraints = 3;
 513                                 lm.m_dirs[0] = btVector3(1, 0, 0);
 514                                 lm.m_dirs[1] = btVector3(0, 1, 0);
 515                                 lm.m_dirs[2] = btVector3(0, 0, 1);
 516                         }
 517                         else
 518                         {
 519                                 lm.m_num_constraints = 1;
 520                                 lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
 521                         }
 522                         m_lagrangeMultipliers.push_back(lm);
 523                 }
 524
 525                 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
 526                 {
 527                         if (!m_faceRigidConstraints[i][j].m_binding)
 528                         {
 529                                 continue;
 530                         }
 531                         btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
 532
 533                         btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
 534                         LagrangeMultiplier lm;
 535                         lm.m_num_nodes = 3;
 536
 537                         for (int k = 0; k < 3; ++k)
 538                         {
 539                                 face->m_n[k]->m_constrained = true;
 540                                 lm.m_indices[k] = face->m_n[k]->index;
 541                                 lm.m_weights[k] = bary[k];
 542                         }
 543                         if (m_faceRigidConstraints[i][j].m_static)
 544                         {
 545                                 face->m_pcontact[3] = 1;
 546                                 lm.m_num_constraints = 3;
 547                                 lm.m_dirs[0] = btVector3(1, 0, 0);
 548                                 lm.m_dirs[1] = btVector3(0, 1, 0);
 549                                 lm.m_dirs[2] = btVector3(0, 0, 1);
 550                         }
 551                         else
 552                         {
 553                                 face->m_pcontact[3] = 0;
 554                                 lm.m_num_constraints = 1;
 555                                 lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
 556                         }
 557                         m_lagrangeMultipliers.push_back(lm);
 558                 }
 559         }
 560 }
 561
 562 //
 563 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 564 {
 565         for (int i = 0; i < m_softBodies.size(); ++i)
 566         {
 567                 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 568                 {
 569                         const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
 570                         const btSoftBody::Node* node = constraint.m_node;
 571                         if (node->m_im != 0)
 572                         {
 573                                 int index = node->index;
 574                                 f[index] += constraint.getDv(node) * (1. / node->m_im);
 575                         }
 576                 }
 577                 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
 578                 {
 579                         const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
 580                         const btSoftBody::Face* face = constraint.getContact()->m_face;
 581                         for (int k = 0; k < 3; ++k)
 582                         {
 583                                 const btSoftBody::Node* node = face->m_n[k];
 584                                 if (node->m_im != 0)
 585                                 {
 586                                         int index = node->index;
 587                                         f[index] += constraint.getDv(node) * (1. / node->m_im);
 588                                 }
 589                         }
 590                 }
 591                 for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
 592                 {
 593                         const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j];
 594                         const btSoftBody::Face* face = constraint.getContact()->m_face;
 595                         const btSoftBody::Node* node = constraint.getContact()->m_node;
 596                         if (node->m_im != 0)
 597                         {
 598                                 int index = node->index;
 599                                 f[index] += constraint.getDv(node) * (1. / node->m_im);
 600                         }
 601                         for (int k = 0; k < 3; ++k)
 602                         {
 603                                 const btSoftBody::Node* node = face->m_n[k];
 604                                 if (node->m_im != 0)
 605                                 {
 606                                         int index = node->index;
 607                                         f[index] += constraint.getDv(node) * (1. / node->m_im);
 608                                 }
 609                         }
 610                 }
 611         }
 612 }
 613
 614 void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 615 {
 616         int N = m_softBodies.size();
 617         if (nodeUpdated)
 618         {
 619                 m_staticConstraints.resize(N);
 620                 m_nodeAnchorConstraints.resize(N);
 621                 m_nodeRigidConstraints.resize(N);
 622                 m_faceRigidConstraints.resize(N);
 623                 m_deformableConstraints.resize(N);
 624         }
 625         for (int i = 0; i < N; ++i)
 626         {
 627                 m_staticConstraints[i].clear();
 628                 m_nodeAnchorConstraints[i].clear();
 629                 m_nodeRigidConstraints[i].clear();
 630                 m_faceRigidConstraints[i].clear();
 631                 m_deformableConstraints[i].clear();
 632         }
 633 #ifndef USE_MGS
 634         m_projectionsDict.clear();
 635 #else
 636         m_projections.clear();
 637 #endif
 638         m_lagrangeMultipliers.clear();
 639 }