dali-physics/third-party/bullet3/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp

   1 #include "btReducedDeformableContactConstraint.h"
   2 #include <iostream>
   3
   4 // ================= static constraints ===================
   5 btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
   6   btReducedDeformableBody* rsb,
   7   btSoftBody::Node* node,
   8         const btVector3& ri,
   9         const btVector3& x0,
  10         const btVector3& dir,
  11   const btContactSolverInfo& infoGlobal,
  12         btScalar dt)
  13   : m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
  14 {
  15         m_erp = 0.2;
  16         m_appliedImpulse = 0;
  17
  18         // get impulse factor
  19   m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index);
  20         m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection);
  21
  22         btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection);
  23         btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection);
  24
  25         m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor;
  26 }
  27
  28 btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
  29 {
  30         // target velocity of fixed constraint is 0
  31         btVector3 deltaVa = getDeltaVa();
  32         btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection);
  33   btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor;
  34         m_appliedImpulse = m_appliedImpulse + deltaImpulse;
  35
  36         btVector3 impulse = deltaImpulse * m_impulseDirection;
  37         applyImpulse(impulse);
  38
  39         // calculate residual
  40         btScalar residualSquare = m_impulseFactor * deltaImpulse;
  41         residualSquare *= residualSquare;
  42
  43         return residualSquare;
  44 }
  45
  46 // this calls reduced deformable body's internalApplyFullSpaceImpulse
  47 void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
  48 {
  49         // apply full space impulse
  50         m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
  51 }
  52
  53 btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const
  54 {
  55         return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
  56 }
  57
  58 // ================= base contact constraints ===================
  59 btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint(
  60   btReducedDeformableBody* rsb,
  61   const btSoftBody::DeformableRigidContact& c,
  62   const btContactSolverInfo& infoGlobal,
  63         btScalar dt)
  64   : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal)
  65 {
  66         m_nodeQueryIndex = 0;
  67         m_appliedNormalImpulse = 0;
  68   m_appliedTangentImpulse = 0;
  69         m_rhs = 0;
  70         m_rhs_tangent = 0;
  71         m_cfm = infoGlobal.m_deformable_cfm;
  72         m_cfm_friction = 0;
  73         m_erp = infoGlobal.m_deformable_erp;
  74         m_erp_friction = infoGlobal.m_deformable_erp;
  75         m_friction = infoGlobal.m_friction;
  76
  77         m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject();
  78         m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK);
  79 }
  80
  81 void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body)
  82 {
  83         if (!m_collideMultibody)
  84         {
  85                 m_solverBodyId = bodyId;
  86                 m_solverBody = &solver_body;
  87                 m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
  88                 btVector3       torqueAxis = -m_relPosA.cross(m_contactNormalA);
  89                 m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
  90
  91                 m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass();
  92                 btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent);
  93                 m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
  94         }
  95 }
  96
  97 btVector3 btReducedDeformableRigidContactConstraint::getVa() const
  98 {
  99         btVector3 Va(0, 0, 0);
 100         if (!m_collideStatic)
 101         {
 102                 Va = btDeformableRigidContactConstraint::getVa();
 103         }
 104         return Va;
 105 }
 106
 107 btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 108 {
 109         // btVector3 Va = getVa();
 110         // btVector3 deltaVa = Va - m_bufferVelocityA;
 111         // if (!m_collideStatic)
 112         // {
 113                 // std::cout << "moving collision!!!\n";
 114                 // std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n";
 115                 // std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t'
 116                 //  << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t'
 117                 //   << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n';
 118         // }
 119         btVector3 deltaVa = getDeltaVa();
 120         btVector3 deltaVb = getDeltaVb();
 121
 122         // if (!m_collideStatic)
 123         // {
 124         //      std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
 125         //      std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
 126         // }
 127
 128         // get delta relative velocity and magnitude (i.e., how much impulse has been applied?)
 129         btVector3 deltaV_rel = deltaVa - deltaVb;
 130         btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
 131
 132         // if (!m_collideStatic)
 133         // {
 134         //      std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n";
 135         //      std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
 136         //      std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n';
 137         // }
 138
 139         // get the normal impulse to be applied
 140         btScalar deltaImpulse = m_rhs - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor;
 141         // if (!m_collideStatic)
 142         // {
 143         //      std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: "  << m_appliedNormalImpulse << "\n";
 144         //      std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n';
 145         // }
 146
 147         {
 148                 // cumulative impulse that has been applied
 149                 btScalar sum = m_appliedNormalImpulse + deltaImpulse;
 150                 // if the cumulative impulse is pushing the object into the rigid body, set it zero
 151                 if (sum < 0)
 152                 {
 153                         deltaImpulse = -m_appliedNormalImpulse;
 154                         m_appliedNormalImpulse = 0;
 155                 }
 156                 else
 157                 {
 158                         m_appliedNormalImpulse = sum;
 159                 }
 160         }
 161
 162         // if (!m_collideStatic)
 163         // {
 164         //      std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
 165         //      std::cout << "deltaImpulse: " << deltaImpulse << '\n';
 166         // }
 167
 168         // residual is the nodal normal velocity change in current iteration
 169         btScalar residualSquare = deltaImpulse * m_normalImpulseFactor; // get residual
 170         residualSquare *= residualSquare;
 171
 172
 173         // apply Coulomb friction (based on delta velocity, |dv_t| = |dv_n * friction|)
 174         btScalar deltaImpulse_tangent = 0;
 175         btScalar deltaImpulse_tangent2 = 0;
 176         {
 177                 // calculate how much impulse is needed
 178                 // btScalar deltaV_rel_tangent = btDot(deltaV_rel, m_contactTangent);
 179                 // btScalar impulse_changed = deltaV_rel_tangent * m_tangentImpulseFactorInv;
 180                 // deltaImpulse_tangent = m_rhs_tangent - impulse_changed;
 181
 182                 // btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent;
 183                 btScalar lower_limit = - m_appliedNormalImpulse * m_friction;
 184                 btScalar upper_limit = m_appliedNormalImpulse * m_friction;
 185                 // if (sum > upper_limit)
 186                 // {
 187                 //      deltaImpulse_tangent = upper_limit - m_appliedTangentImpulse;
 188                 //      m_appliedTangentImpulse = upper_limit;
 189                 // }
 190                 // else if (sum < lower_limit)
 191                 // {
 192                 //      deltaImpulse_tangent = lower_limit - m_appliedTangentImpulse;
 193                 //      m_appliedTangentImpulse = lower_limit;
 194                 // }
 195                 // else
 196                 // {
 197                 //      m_appliedTangentImpulse = sum;
 198                 // }
 199                 //
 200                 calculateTangentialImpulse(deltaImpulse_tangent, m_appliedTangentImpulse, m_rhs_tangent,
 201                                                                                                                          m_tangentImpulseFactorInv, m_contactTangent, lower_limit, upper_limit, deltaV_rel);
 202
 203                 if (m_collideMultibody)
 204                 {
 205                         calculateTangentialImpulse(deltaImpulse_tangent2, m_appliedTangentImpulse2, m_rhs_tangent2,
 206                                                                                                                            m_tangentImpulseFactorInv2, m_contactTangent2, lower_limit, upper_limit, deltaV_rel);
 207                 }
 208
 209
 210                 if (!m_collideStatic)
 211                 {
 212                         // std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t"  << m_contactTangent[1] << "\t"  << m_contactTangent[2] << "\n";
 213                         // std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n';
 214                         // std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n';
 215                         // std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n';
 216                 }
 217         }
 218
 219         // get the total impulse vector
 220         btVector3 impulse_normal = deltaImpulse * m_contactNormalA;
 221         btVector3 impulse_tangent = deltaImpulse_tangent * (-m_contactTangent);
 222         btVector3 impulse_tangent2 = deltaImpulse_tangent2 * (-m_contactTangent2);
 223         btVector3 impulse = impulse_normal + impulse_tangent + impulse_tangent2;
 224
 225         applyImpulse(impulse);
 226
 227         // apply impulse to the rigid/multibodies involved and change their velocities
 228         if (!m_collideStatic)
 229         {
 230                 // std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t'
 231                 //                                                                                                                                      << m_linearComponentNormal[1] << '\t'
 232                 //                                                                                                                                      << m_linearComponentNormal[2] << '\n';
 233                 // std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t'
 234                 //                                                                                                                                      << m_angularComponentNormal[1] << '\t'
 235                 //                                                                                                                                      << m_angularComponentNormal[2] << '\n';
 236
 237                 if (!m_collideMultibody)                // collision with rigid body
 238                 {
 239                         // std::cout << "rigid impulse applied!!\n";
 240                         // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
 241                         // << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
 242                         //      << m_solverBody->getDeltaLinearVelocity()[2] << '\n';
 243                         // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
 244                         // << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
 245                         //      << m_solverBody->getDeltaAngularVelocity()[2] << '\n';
 246
 247                         m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse);
 248                         m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent);
 249
 250                         // std::cout << "after\n";
 251                         // std::cout << "rigid impulse applied!!\n";
 252                         // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
 253                         // << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
 254                         //      << m_solverBody->getDeltaLinearVelocity()[2] << '\n';
 255                         // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
 256                         // << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
 257                         //      << m_solverBody->getDeltaAngularVelocity()[2] << '\n';
 258                 }
 259                 else            // collision with multibody
 260                 {
 261                         btMultiBodyLinkCollider* multibodyLinkCol = 0;
 262                         multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
 263                         if (multibodyLinkCol)
 264                         {
 265                                 const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
 266                                 // apply normal component of the impulse
 267                                 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, -deltaImpulse);
 268
 269                                 // const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
 270                                 // std::cout << "deltaV_normal: ";
 271                                 // for (int i = 0; i < ndof; ++i)
 272                                 // {
 273                                 //      std::cout << i << "\t" << deltaV_normal[i] << '\n';
 274                                 // }
 275
 276                                 if (impulse_tangent.norm() > SIMD_EPSILON)
 277                                 {
 278                                         // apply tangential component of the impulse
 279                                         const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
 280                                         multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, deltaImpulse_tangent);
 281                                         const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
 282                                         multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, deltaImpulse_tangent2);
 283                                 }
 284                         }
 285                 }
 286         }
 287         return residualSquare;
 288 }
 289
 290 void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btScalar& deltaImpulse_tangent,
 291                                                                                                                                                                                                                                                                                                          btScalar& appliedImpulse,
 292                                                                                                                                                                                                                                                                                                          const btScalar rhs_tangent,
 293                                                                                                                                                                                                                                                                                                          const btScalar tangentImpulseFactorInv,
 294                                                                                                                                                                                                                                                                                                          const btVector3& tangent,
 295                                                                                                                                                                                                                                                                                                          const btScalar lower_limit,
 296                                                                                                                                                                                                                                                                                                          const btScalar upper_limit,
 297                                                                                                                                                                                                                                                                                                          const btVector3& deltaV_rel)
 298 {
 299         btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent);
 300         btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv;
 301         deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed;
 302
 303         btScalar sum = appliedImpulse + deltaImpulse_tangent;
 304         if (sum > upper_limit)
 305         {
 306                 deltaImpulse_tangent = upper_limit - appliedImpulse;
 307                 appliedImpulse = upper_limit;
 308         }
 309         else if (sum < lower_limit)
 310         {
 311                 deltaImpulse_tangent = lower_limit - appliedImpulse;
 312                 appliedImpulse = lower_limit;
 313         }
 314         else
 315         {
 316                 appliedImpulse = sum;
 317         }
 318 }
 319
 320 // ================= node vs rigid constraints ===================
 321 btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint(
 322   btReducedDeformableBody* rsb,
 323   const btSoftBody::DeformableNodeRigidContact& contact,
 324   const btContactSolverInfo& infoGlobal,
 325         btScalar dt)
 326   : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
 327 {
 328         m_contactNormalA = contact.m_cti.m_normal;
 329   m_contactNormalB = -contact.m_cti.m_normal;
 330
 331         if (contact.m_node->index < rsb->m_nodes.size())
 332         {
 333                 m_nodeQueryIndex = contact.m_node->index;
 334         }
 335         else
 336         {
 337                 m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset;
 338         }
 339
 340         if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 341         {
 342                 m_relPosA = contact.m_c1;
 343         }
 344         else
 345         {
 346                 m_relPosA = btVector3(0,0,0);
 347         }
 348         m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin();
 349
 350         if (m_collideStatic)            // colliding with static object, only consider reduced deformable body's impulse factor
 351         {
 352                 m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex);
 353         }
 354         else            // colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors
 355         {
 356                 m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0;
 357         }
 358
 359         m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA);
 360         m_tangentImpulseFactor = 0;
 361
 362         warmStarting();
 363 }
 364
 365 void btReducedDeformableNodeRigidContactConstraint::warmStarting()
 366 {
 367         btVector3 va = getVa();
 368         btVector3 vb = getVb();
 369         m_bufferVelocityA = va;
 370         m_bufferVelocityB = vb;
 371
 372         // we define the (+) direction of errors to be the outward surface normal of the rigid object
 373         btVector3 v_rel = vb - va;
 374         // get tangent direction of the relative velocity
 375         btVector3 v_tangent = v_rel - v_rel.dot(m_contactNormalA) * m_contactNormalA;
 376         if (v_tangent.norm() < SIMD_EPSILON)
 377         {
 378                 m_contactTangent = btVector3(0, 0, 0);
 379                 // tangent impulse factor
 380                 m_tangentImpulseFactor = 0;
 381                 m_tangentImpulseFactorInv = 0;
 382         }
 383         else
 384         {
 385                 if (!m_collideMultibody)
 386                 {
 387                         m_contactTangent = v_tangent.normalized();
 388                         m_contactTangent2.setZero();
 389                         // tangent impulse factor 1
 390                         m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
 391                         m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
 392                         // tangent impulse factor 2
 393                         m_tangentImpulseFactor2 = 0;
 394                         m_tangentImpulseFactorInv2 = 0;
 395                 }
 396                 else    // multibody requires 2 contact directions
 397                 {
 398                         m_contactTangent = m_contact->t1;
 399                         m_contactTangent2 = m_contact->t2;
 400
 401                         // tangent impulse factor 1
 402                         m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
 403                         m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
 404                         // tangent impulse factor 2
 405                         m_tangentImpulseFactor2 = (m_impulseFactor * m_contactTangent2).dot(m_contactTangent2);
 406                         m_tangentImpulseFactorInv2 = btScalar(1) / m_tangentImpulseFactor2;
 407                 }
 408         }
 409
 410
 411         // initial guess for normal impulse
 412         {
 413                 btScalar velocity_error = btDot(v_rel, m_contactNormalA);       // magnitude of relative velocity
 414                 btScalar position_error = 0;
 415                 if (m_penetration > 0)
 416                 {
 417                         velocity_error += m_penetration / m_dt;
 418                 }
 419                 else
 420                 {
 421                         // add penetration correction vel
 422                         position_error = m_penetration * m_erp / m_dt;
 423                 }
 424                 // get the initial estimate of impulse magnitude to be applied
 425                 m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor;
 426         }
 427
 428         // initial guess for tangential impulse
 429         {
 430                 btScalar velocity_error = btDot(v_rel, m_contactTangent);
 431                 m_rhs_tangent = velocity_error * m_tangentImpulseFactorInv;
 432
 433                 if (m_collideMultibody)
 434                 {
 435                         btScalar velocity_error2 = btDot(v_rel, m_contactTangent2);
 436                         m_rhs_tangent2 = velocity_error2 * m_tangentImpulseFactorInv2;
 437                 }
 438         }
 439
 440         // warm starting
 441         // applyImpulse(m_rhs * m_contactNormalA);
 442         // if (!m_collideStatic)
 443         // {
 444         //      const btSoftBody::sCti& cti = m_contact->m_cti;
 445         //      if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 446         //      {
 447         //              m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs);
 448         //      }
 449         // }
 450 }
 451
 452 btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
 453 {
 454         return m_node->m_v;
 455 }
 456
 457 btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
 458 {
 459         btVector3 deltaVa(0, 0, 0);
 460         if (!m_collideStatic)
 461         {
 462                 if (!m_collideMultibody)                // for rigid body
 463                 {
 464                         deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA);
 465                 }
 466                 else            // for multibody
 467                 {
 468                         btMultiBodyLinkCollider* multibodyLinkCol = 0;
 469                         multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
 470                         if (multibodyLinkCol)
 471                         {
 472                                 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
 473                                 const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
 474                                 const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
 475                                 const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
 476                                 const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
 477                                 // add in the normal component of the va
 478                                 btScalar vel = 0;
 479                                 for (int k = 0; k < ndof; ++k)
 480                                 {
 481                                         vel += local_dv[k] * J_n[k];
 482                                 }
 483                                 deltaVa = m_contact->m_cti.m_normal * vel;
 484
 485                                 // add in the tangential components of the va
 486                                 vel = 0;
 487                                 for (int k = 0; k < ndof; ++k)
 488                                 {
 489                                         vel += local_dv[k] * J_t1[k];
 490                                 }
 491                                 deltaVa += m_contact->t1 * vel;
 492
 493                                 vel = 0;
 494                                 for (int k = 0; k < ndof; ++k)
 495                                 {
 496                                         vel += local_dv[k] * J_t2[k];
 497                                 }
 498                                 deltaVa += m_contact->t2 * vel;
 499                         }
 500                 }
 501         }
 502         return deltaVa;
 503 }
 504
 505 btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
 506 {
 507         // std::cout << "node: " << m_node->index << '\n';
 508         return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex);
 509 }
 510
 511 btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const
 512 {
 513         return m_node->m_splitv;
 514 }
 515
 516 btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 517 {
 518         return m_total_normal_dv + m_total_tangent_dv;
 519 }
 520
 521 void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
 522 {
 523   m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt);
 524         // m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
 525         // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
 526         // if (!m_collideStatic)
 527         // {
 528         //      // std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
 529         //      // std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n';
 530         //      btVector3 v_after = getDeltaVb() + m_node->m_v;
 531         //      // std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n';
 532         // }
 533         // std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n';
 534 }
 535
 536 // ================= face vs rigid constraints ===================
 537 btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint(
 538   btReducedDeformableBody* rsb,
 539   const btSoftBody::DeformableFaceRigidContact& contact,
 540   const btContactSolverInfo& infoGlobal,
 541         btScalar dt,
 542   bool useStrainLimiting)
 543   : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
 544 {}
 545
 546 btVector3 btReducedDeformableFaceRigidContactConstraint::getVb() const
 547 {
 548         const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 549         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];
 550         return vb;
 551 }
 552
 553 btVector3 btReducedDeformableFaceRigidContactConstraint::getSplitVb() const
 554 {
 555         const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 556         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];
 557         return vb;
 558 }
 559
 560 btVector3 btReducedDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 561 {
 562         btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
 563         const btSoftBody::DeformableFaceRigidContact* contact = getContact();
 564         if (m_face->m_n[0] == node)
 565         {
 566                 return face_dv * contact->m_weights[0];
 567         }
 568         if (m_face->m_n[1] == node)
 569         {
 570                 return face_dv * contact->m_weights[1];
 571         }
 572         btAssert(node == m_face->m_n[2]);
 573         return face_dv * contact->m_weights[2];
 574 }
 575
 576 void btReducedDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
 577 {
 578   //
 579 }