dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableContactConstraint.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 "btDeformableContactConstraint.h"
  17 /* ================   Deformable Node Anchor   =================== */
  18 btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal)
  19         : m_anchor(&a), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
  20 {
  21 }
  22
  23 btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
  24         : m_anchor(other.m_anchor), btDeformableContactConstraint(other)
  25 {
  26 }
  27
  28 btVector3 btDeformableNodeAnchorConstraint::getVa() const
  29 {
  30         const btSoftBody::sCti& cti = m_anchor->m_cti;
  31         btVector3 va(0, 0, 0);
  32         if (cti.m_colObj->hasContactResponse())
  33         {
  34                 btRigidBody* rigidCol = 0;
  35                 btMultiBodyLinkCollider* multibodyLinkCol = 0;
  36
  37                 // grab the velocity of the rigid body
  38                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
  39                 {
  40                         rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
  41                         va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0);
  42                 }
  43                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
  44                 {
  45                         multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
  46                         if (multibodyLinkCol)
  47                         {
  48                                 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
  49                                 const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0];
  50                                 const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
  51                                 const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
  52                                 const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
  53                                 const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
  54                                 // add in the normal component of the va
  55                                 btScalar vel = 0.0;
  56                                 for (int k = 0; k < ndof; ++k)
  57                                 {
  58                                         vel += (local_v[k] + local_dv[k]) * J_n[k];
  59                                 }
  60                                 va = cti.m_normal * vel;
  61                                 // add in the tangential components of the va
  62                                 vel = 0.0;
  63                                 for (int k = 0; k < ndof; ++k)
  64                                 {
  65                                         vel += (local_v[k] + local_dv[k]) * J_t1[k];
  66                                 }
  67                                 va += m_anchor->t1 * vel;
  68                                 vel = 0.0;
  69                                 for (int k = 0; k < ndof; ++k)
  70                                 {
  71                                         vel += (local_v[k] + local_dv[k]) * J_t2[k];
  72                                 }
  73                                 va += m_anchor->t2 * vel;
  74                         }
  75                 }
  76         }
  77         return va;
  78 }
  79
  80 btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
  81 {
  82         const btSoftBody::sCti& cti = m_anchor->m_cti;
  83         btVector3 va = getVa();
  84         btVector3 vb = getVb();
  85         btVector3 vr = (vb - va);
  86         // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
  87         const btScalar dn = btDot(vr, vr);
  88         // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
  89         btScalar residualSquare = dn * dn;
  90         btVector3 impulse = m_anchor->m_c0 * vr;
  91         // apply impulse to deformable nodes involved and change their velocities
  92         applyImpulse(impulse);
  93
  94         // apply impulse to the rigid/multibodies involved and change their velocities
  95         if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
  96         {
  97                 btRigidBody* rigidCol = 0;
  98                 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
  99                 if (rigidCol)
 100                 {
 101                         rigidCol->applyImpulse(impulse, m_anchor->m_c1);
 102                 }
 103         }
 104         else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
 105         {
 106                 btMultiBodyLinkCollider* multibodyLinkCol = 0;
 107                 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
 108                 if (multibodyLinkCol)
 109                 {
 110                         const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
 111                         // apply normal component of the impulse
 112                         multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
 113                         // apply tangential component of the impulse
 114                         const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
 115                         multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
 116                         const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
 117                         multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
 118                 }
 119         }
 120         return residualSquare;
 121 }
 122
 123 btVector3 btDeformableNodeAnchorConstraint::getVb() const
 124 {
 125         return m_anchor->m_node->m_v;
 126 }
 127
 128 void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
 129 {
 130         btVector3 dv = impulse * m_anchor->m_c2;
 131         m_anchor->m_node->m_v -= dv;
 132 }
 133
 134 /* ================   Deformable vs. Rigid   =================== */
 135 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal)
 136         : m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
 137 {
 138         m_total_normal_dv.setZero();
 139         m_total_tangent_dv.setZero();
 140         // The magnitude of penetration is the depth of penetration.
 141         m_penetration = c.m_cti.m_offset;
 142         m_total_split_impulse = 0;
 143         m_binding = false;
 144 }
 145
 146 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
 147         : m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding)
 148 {
 149         m_total_normal_dv = other.m_total_normal_dv;
 150         m_total_tangent_dv = other.m_total_tangent_dv;
 151 }
 152
 153 btVector3 btDeformableRigidContactConstraint::getVa() const
 154 {
 155         const btSoftBody::sCti& cti = m_contact->m_cti;
 156         btVector3 va(0, 0, 0);
 157         if (cti.m_colObj->hasContactResponse())
 158         {
 159                 btRigidBody* rigidCol = 0;
 160                 btMultiBodyLinkCollider* multibodyLinkCol = 0;
 161
 162                 // grab the velocity of the rigid body
 163                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 164                 {
 165                         rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
 166                         va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
 167                 }
 168                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
 169                 {
 170                         multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
 171                         if (multibodyLinkCol)
 172                         {
 173                                 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
 174                                 const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
 175                                 const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
 176                                 const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
 177                                 const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
 178                                 const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
 179                                 // add in the normal component of the va
 180                                 btScalar vel = 0.0;
 181                                 for (int k = 0; k < ndof; ++k)
 182                                 {
 183                                         vel += (local_v[k] + local_dv[k]) * J_n[k];
 184                                 }
 185                                 va = cti.m_normal * vel;
 186                                 // add in the tangential components of the va
 187                                 vel = 0.0;
 188                                 for (int k = 0; k < ndof; ++k)
 189                                 {
 190                                         vel += (local_v[k] + local_dv[k]) * J_t1[k];
 191                                 }
 192                                 va += m_contact->t1 * vel;
 193                                 vel = 0.0;
 194                                 for (int k = 0; k < ndof; ++k)
 195                                 {
 196                                         vel += (local_v[k] + local_dv[k]) * J_t2[k];
 197                                 }
 198                                 va += m_contact->t2 * vel;
 199                         }
 200                 }
 201         }
 202         return va;
 203 }
 204
 205 btVector3 btDeformableRigidContactConstraint::getSplitVa() const
 206 {
 207         const btSoftBody::sCti& cti = m_contact->m_cti;
 208         btVector3 va(0, 0, 0);
 209         if (cti.m_colObj->hasContactResponse())
 210         {
 211                 btRigidBody* rigidCol = 0;
 212                 btMultiBodyLinkCollider* multibodyLinkCol = 0;
 213
 214                 // grab the velocity of the rigid body
 215                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 216                 {
 217                         rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
 218                         va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
 219                 }
 220                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
 221                 {
 222                         multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
 223                         if (multibodyLinkCol)
 224                         {
 225                                 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
 226                                 const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
 227                                 const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
 228                                 const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
 229                                 const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector();
 230                                 // add in the normal component of the va
 231                                 btScalar vel = 0.0;
 232                                 for (int k = 0; k < ndof; ++k)
 233                                 {
 234                                         vel += local_split_v[k] * J_n[k];
 235                                 }
 236                                 va = cti.m_normal * vel;
 237                                 // add in the tangential components of the va
 238                                 vel = 0.0;
 239                                 for (int k = 0; k < ndof; ++k)
 240                                 {
 241                                         vel += local_split_v[k] * J_t1[k];
 242                                 }
 243                                 va += m_contact->t1 * vel;
 244                                 vel = 0.0;
 245                                 for (int k = 0; k < ndof; ++k)
 246                                 {
 247                                         vel += local_split_v[k] * J_t2[k];
 248                                 }
 249                                 va += m_contact->t2 * vel;
 250                         }
 251                 }
 252         }
 253         return va;
 254 }
 255
 256 btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 257 {
 258         const btSoftBody::sCti& cti = m_contact->m_cti;
 259         btVector3 va = getVa();
 260         btVector3 vb = getVb();
 261         btVector3 vr = vb - va;
 262         btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm;
 263         if (m_penetration > 0)
 264         {
 265                 dn += m_penetration / infoGlobal.m_timeStep;
 266         }
 267         if (!infoGlobal.m_splitImpulse)
 268         {
 269                 dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
 270         }
 271         // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
 272         btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
 273         if (!infoGlobal.m_splitImpulse)
 274         {
 275                 impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
 276         }
 277         btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
 278         btVector3 impulse_tangent = impulse - impulse_normal;
 279         if (dn > 0)
 280         {
 281                 return 0;
 282         }
 283         m_binding = true;
 284         btScalar residualSquare = dn * dn;
 285         btVector3 old_total_tangent_dv = m_total_tangent_dv;
 286         // m_c5 is the inverse mass of the deformable node/face
 287         m_total_normal_dv -= m_contact->m_c5 * impulse_normal;
 288         m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent;
 289
 290         if (m_total_normal_dv.dot(cti.m_normal) < 0)
 291         {
 292                 // separating in the normal direction
 293                 m_binding = false;
 294                 m_static = false;
 295                 impulse_tangent.setZero();
 296         }
 297         else
 298         {
 299                 if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
 300                 {
 301                         // dynamic friction
 302                         // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
 303                         m_static = false;
 304                         if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
 305                         {
 306                                 m_total_tangent_dv = btVector3(0, 0, 0);
 307                         }
 308                         else
 309                         {
 310                                 m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
 311                         }
 312                         //            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
 313                         impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
 314                 }
 315                 else
 316                 {
 317                         // static friction
 318                         m_static = true;
 319                 }
 320         }
 321         impulse = impulse_normal + impulse_tangent;
 322         // apply impulse to deformable nodes involved and change their velocities
 323         applyImpulse(impulse);
 324         // apply impulse to the rigid/multibodies involved and change their velocities
 325         if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 326         {
 327                 btRigidBody* rigidCol = 0;
 328                 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
 329                 if (rigidCol)
 330                 {
 331                         rigidCol->applyImpulse(impulse, m_contact->m_c1);
 332                 }
 333         }
 334         else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
 335         {
 336                 btMultiBodyLinkCollider* multibodyLinkCol = 0;
 337                 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
 338                 if (multibodyLinkCol)
 339                 {
 340                         const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
 341                         // apply normal component of the impulse
 342                         multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
 343                         if (impulse_tangent.norm() > SIMD_EPSILON)
 344                         {
 345                                 // apply tangential component of the impulse
 346                                 const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
 347                                 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
 348                                 const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
 349                                 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
 350                         }
 351                 }
 352         }
 353         return residualSquare;
 354 }
 355
 356 btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
 357 {
 358         btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction;
 359         const btSoftBody::sCti& cti = m_contact->m_cti;
 360         btVector3 vb = getSplitVb();
 361         btVector3 va = getSplitVa();
 362         btScalar p = m_penetration;
 363         if (p > 0)
 364         {
 365                 return 0;
 366         }
 367         btVector3 vr = vb - va;
 368         btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
 369         if (dn > 0)
 370         {
 371                 return 0;
 372         }
 373         if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION)
 374         {
 375                 dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse;
 376         }
 377         if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION)
 378         {
 379                 dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse;
 380         }
 381         m_total_split_impulse += dn;
 382
 383         btScalar residualSquare = dn * dn;
 384         const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn);
 385         applySplitImpulse(impulse);
 386
 387         // apply split impulse to the rigid/multibodies involved and change their velocities
 388         if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 389         {
 390                 btRigidBody* rigidCol = 0;
 391                 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
 392                 if (rigidCol)
 393                 {
 394                         rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
 395                 }
 396         }
 397         else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
 398         {
 399                 btMultiBodyLinkCollider* multibodyLinkCol = 0;
 400                 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
 401                 if (multibodyLinkCol)
 402                 {
 403                         const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
 404                         // apply normal component of the impulse
 405                         multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
 406                 }
 407         }
 408         return residualSquare;
 409 }
 410 /* ================   Node vs. Rigid   =================== */
 411 btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
 412         : m_node(contact.m_node), btDeformableRigidContactConstraint(contact, infoGlobal)
 413 {
 414 }
 415
 416 btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
 417         : m_node(other.m_node), btDeformableRigidContactConstraint(other)
 418 {
 419 }
 420
 421 btVector3 btDeformableNodeRigidContactConstraint::getVb() const
 422 {
 423         return m_node->m_v;
 424 }
 425
 426 btVector3 btDeformableNodeRigidContactConstraint::getSplitVb() const
 427 {
 428         return m_node->m_splitv;
 429 }
 430
 431 btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 432 {
 433         return m_total_normal_dv + m_total_tangent_dv;
 434 }
 435
 436 void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
 437 {
 438         const btSoftBody::DeformableNodeRigidContact* contact = getContact();
 439         btVector3 dv = contact->m_c5 * impulse;
 440         contact->m_node->m_v -= dv;
 441 }
 442
 443 void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
 444 {
 445         const btSoftBody::DeformableNodeRigidContact* contact = getContact();
 446         btVector3 dv = contact->m_c5 * impulse;
 447         contact->m_node->m_splitv -= dv;
 448 }
 449
 450 /* ================   Face vs. Rigid   =================== */
 451 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting)
 452         : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal)
 453 {
 454 }
 455
 456 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
 457         : m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other)
 458 {
 459 }
 460
 461 btVector3 btDeformableFaceRigidContactConstraint::getVb() const
 462 {
 463         const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 464         btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
 465         return vb;
 466 }
 467
 468 btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 469 {
 470         btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
 471         const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 472         if (m_face->m_n[0] == node)
 473         {
 474                 return face_dv * contact->m_weights[0];
 475         }
 476         if (m_face->m_n[1] == node)
 477         {
 478                 return face_dv * contact->m_weights[1];
 479         }
 480         btAssert(node == m_face->m_n[2]);
 481         return face_dv * contact->m_weights[2];
 482 }
 483
 484 void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
 485 {
 486         const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 487         btVector3 dv = impulse * contact->m_c2;
 488         btSoftBody::Face* face = contact->m_face;
 489
 490         btVector3& v0 = face->m_n[0]->m_v;
 491         btVector3& v1 = face->m_n[1]->m_v;
 492         btVector3& v2 = face->m_n[2]->m_v;
 493         const btScalar& im0 = face->m_n[0]->m_im;
 494         const btScalar& im1 = face->m_n[1]->m_im;
 495         const btScalar& im2 = face->m_n[2]->m_im;
 496         if (im0 > 0)
 497                 v0 -= dv * contact->m_weights[0];
 498         if (im1 > 0)
 499                 v1 -= dv * contact->m_weights[1];
 500         if (im2 > 0)
 501                 v2 -= dv * contact->m_weights[2];
 502         if (m_useStrainLimiting)
 503         {
 504                 btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations);
 505                 btScalar m01 = (relaxation / (im0 + im1));
 506                 btScalar m02 = (relaxation / (im0 + im2));
 507                 btScalar m12 = (relaxation / (im1 + im2));
 508 #ifdef USE_STRAIN_RATE_LIMITING
 509                 // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
 510                 btScalar p = 0.01;
 511                 btVector3& x0 = face->m_n[0]->m_x;
 512                 btVector3& x1 = face->m_n[1]->m_x;
 513                 btVector3& x2 = face->m_n[2]->m_x;
 514                 const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1};
 515                 const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1};
 516                 btVector3 u[3];
 517                 btScalar x_diff_dot_u, dn[3];
 518                 btScalar dt = m_infoGlobal->m_timeStep;
 519                 for (int i = 0; i < 3; ++i)
 520                 {
 521                         btScalar x_diff_norm = x_diff[i].safeNorm();
 522                         btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm();
 523                         btScalar strainRate = x_diff_norm_new / x_diff_norm;
 524                         u[i] = v_diff[i];
 525                         u[i].safeNormalize();
 526                         if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p))
 527                         {
 528                                 dn[i] = 0;
 529                                 continue;
 530                         }
 531                         x_diff_dot_u = btDot(x_diff[i], u[i]);
 532                         btScalar s;
 533                         if (1 - p > strainRate)
 534                         {
 535                                 s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm));
 536                         }
 537                         else
 538                         {
 539                                 s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm));
 540                         }
 541                         //              x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm();
 542                         //              strainRate = x_diff_norm_new/x_diff_norm;
 543                         dn[i] = s - v_diff[i].safeNorm();
 544                 }
 545                 btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1]));
 546                 btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2]));
 547                 btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1]));
 548 #else
 549                 // apply strain limiting to prevent undamped modes
 550                 btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0));
 551                 btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1));
 552                 btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2));
 553 #endif
 554                 v0 += dv0;
 555                 v1 += dv1;
 556                 v2 += dv2;
 557         }
 558 }
 559
 560 btVector3 btDeformableFaceRigidContactConstraint::getSplitVb() const
 561 {
 562         const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 563         btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2];
 564         return vb;
 565 }
 566
 567 void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
 568 {
 569         const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 570         btVector3 dv = impulse * contact->m_c2;
 571         btSoftBody::Face* face = contact->m_face;
 572         btVector3& v0 = face->m_n[0]->m_splitv;
 573         btVector3& v1 = face->m_n[1]->m_splitv;
 574         btVector3& v2 = face->m_n[2]->m_splitv;
 575         const btScalar& im0 = face->m_n[0]->m_im;
 576         const btScalar& im1 = face->m_n[1]->m_im;
 577         const btScalar& im2 = face->m_n[2]->m_im;
 578         if (im0 > 0)
 579         {
 580                 v0 -= dv * contact->m_weights[0];
 581         }
 582         if (im1 > 0)
 583         {
 584                 v1 -= dv * contact->m_weights[1];
 585         }
 586         if (im2 > 0)
 587         {
 588                 v2 -= dv * contact->m_weights[2];
 589         }
 590 }
 591
 592 /* ================   Face vs. Node   =================== */
 593 btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
 594         : m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal)
 595 {
 596         m_total_normal_dv.setZero();
 597         m_total_tangent_dv.setZero();
 598 }
 599
 600 btVector3 btDeformableFaceNodeContactConstraint::getVa() const
 601 {
 602         return m_node->m_v;
 603 }
 604
 605 btVector3 btDeformableFaceNodeContactConstraint::getVb() const
 606 {
 607         const btSoftBody::DeformableFaceNodeContact* contact = getContact();
 608         btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
 609         return vb;
 610 }
 611
 612 btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const
 613 {
 614         btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
 615         if (n == m_node)
 616                 return dv;
 617         const btSoftBody::DeformableFaceNodeContact* contact = getContact();
 618         if (m_face->m_n[0] == n)
 619         {
 620                 return dv * contact->m_weights[0];
 621         }
 622         if (m_face->m_n[1] == n)
 623         {
 624                 return dv * contact->m_weights[1];
 625         }
 626         btAssert(n == m_face->m_n[2]);
 627         return dv * contact->m_weights[2];
 628 }
 629
 630 btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 631 {
 632         btVector3 va = getVa();
 633         btVector3 vb = getVb();
 634         btVector3 vr = vb - va;
 635         const btScalar dn = btDot(vr, m_contact->m_normal);
 636         // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
 637         btScalar residualSquare = dn * dn;
 638         btVector3 impulse = m_contact->m_c0 * vr;
 639         const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
 640         btVector3 impulse_tangent = impulse - impulse_normal;
 641
 642         btVector3 old_total_tangent_dv = m_total_tangent_dv;
 643         // m_c2 is the inverse mass of the deformable node/face
 644         if (m_node->m_im > 0)
 645         {
 646                 m_total_normal_dv -= impulse_normal * m_node->m_im;
 647                 m_total_tangent_dv -= impulse_tangent * m_node->m_im;
 648         }
 649         else
 650         {
 651                 m_total_normal_dv -= impulse_normal * m_contact->m_imf;
 652                 m_total_tangent_dv -= impulse_tangent * m_contact->m_imf;
 653         }
 654
 655         if (m_total_normal_dv.dot(m_contact->m_normal) > 0)
 656         {
 657                 // separating in the normal direction
 658                 m_static = false;
 659                 m_total_tangent_dv = btVector3(0, 0, 0);
 660                 impulse_tangent.setZero();
 661         }
 662         else
 663         {
 664                 if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
 665                 {
 666                         // dynamic friction
 667                         // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
 668                         m_static = false;
 669                         if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
 670                         {
 671                                 m_total_tangent_dv = btVector3(0, 0, 0);
 672                         }
 673                         else
 674                         {
 675                                 m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
 676                         }
 677                         impulse_tangent = -btScalar(1) / m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
 678                 }
 679                 else
 680                 {
 681                         // static friction
 682                         m_static = true;
 683                 }
 684         }
 685         impulse = impulse_normal + impulse_tangent;
 686         // apply impulse to deformable nodes involved and change their velocities
 687         applyImpulse(impulse);
 688         return residualSquare;
 689 }
 690
 691 void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse)
 692 {
 693         const btSoftBody::DeformableFaceNodeContact* contact = getContact();
 694         btVector3 dva = impulse * contact->m_node->m_im;
 695         btVector3 dvb = impulse * contact->m_imf;
 696         if (contact->m_node->m_im > 0)
 697         {
 698                 contact->m_node->m_v += dva;
 699         }
 700
 701         btSoftBody::Face* face = contact->m_face;
 702         btVector3& v0 = face->m_n[0]->m_v;
 703         btVector3& v1 = face->m_n[1]->m_v;
 704         btVector3& v2 = face->m_n[2]->m_v;
 705         const btScalar& im0 = face->m_n[0]->m_im;
 706         const btScalar& im1 = face->m_n[1]->m_im;
 707         const btScalar& im2 = face->m_n[2]->m_im;
 708         if (im0 > 0)
 709         {
 710                 v0 -= dvb * contact->m_weights[0];
 711         }
 712         if (im1 > 0)
 713         {
 714                 v1 -= dvb * contact->m_weights[1];
 715         }
 716         if (im2 > 0)
 717         {
 718                 v2 -= dvb * contact->m_weights[2];
 719         }
 720 }