bullet: Sync with upstream 2.89

This allows distro unbundling again for distros that ship Bullet 2.89+.

1 files changed, 591 insertions, 0 deletions

diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp
new file mode 100644
index 0000000000..e8219dc50e
--- /dev/null
+++ b/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp
@@ -0,0 +1,591 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 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.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "btDeformableContactConstraint.h"
+/* ================   Deformable Node Anchor   =================== */
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
+: m_anchor(&a)
+, btDeformableContactConstraint(a.m_cti.m_normal)
+{
+}
+
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
+: m_anchor(other.m_anchor)
+, btDeformableContactConstraint(other)
+{
+}
+
+btVector3 btDeformableNodeAnchorConstraint::getVa() const
+{
+    const btSoftBody::sCti& cti = m_anchor->m_cti;
+    btVector3 va(0, 0, 0);
+    if (cti.m_colObj->hasContactResponse())
+    {
+        btRigidBody* rigidCol = 0;
+        btMultiBodyLinkCollider* multibodyLinkCol = 0;
+        
+        // grab the velocity of the rigid body
+        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+        {
+            rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+            va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0);
+        }
+        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+        {
+            multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+            if (multibodyLinkCol)
+            {
+                const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0];
+                const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
+                const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
+                const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+                const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+                // add in the normal component of the va
+                btScalar vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_n[k];
+                }
+                va = cti.m_normal * vel;
+                // add in the tangential components of the va
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t1[k];
+                }
+                va += m_anchor->t1 * vel;
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t2[k];
+                }
+                va += m_anchor->t2 * vel;
+            }
+        }
+    }
+    return va;
+}
+
+btScalar btDeformableNodeAnchorConstraint::solveConstraint()
+{
+    const btSoftBody::sCti& cti = m_anchor->m_cti;
+    btVector3 va = getVa();
+    btVector3 vb = getVb();
+    btVector3 vr = (vb - va);
+    // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
+    const btScalar dn = btDot(vr, cti.m_normal);
+    // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+    btScalar residualSquare = dn*dn;
+    btVector3 impulse = m_anchor->m_c0 * vr;
+    // apply impulse to deformable nodes involved and change their velocities
+    applyImpulse(impulse);
+    
+    // apply impulse to the rigid/multibodies involved and change their velocities
+    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+    {
+        btRigidBody* rigidCol = 0;
+        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+        if (rigidCol)
+        {
+            rigidCol->applyImpulse(impulse, m_anchor->m_c1);
+        }
+    }
+    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+    {
+        btMultiBodyLinkCollider* multibodyLinkCol = 0;
+        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+        if (multibodyLinkCol)
+        {
+            const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+            // apply normal component of the impulse
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+            // apply tangential component of the impulse
+            const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
+            const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
+        }
+    }
+    return residualSquare;
+}
+
+btVector3 btDeformableNodeAnchorConstraint::getVb() const
+{
+    return m_anchor->m_node->m_v;
+}
+
+void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
+{
+    btVector3 dv = impulse * m_anchor->m_c2;
+    m_anchor->m_node->m_v -= dv;
+}
+
+/* ================   Deformable vs. Rigid   =================== */
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
+: m_contact(&c)
+, btDeformableContactConstraint(c.m_cti.m_normal)
+{
+    m_total_normal_dv.setZero();
+    m_total_tangent_dv.setZero();
+    // penetration is non-positive. The magnitude of penetration is the depth of penetration.
+    m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
+}
+
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
+: m_contact(other.m_contact)
+, btDeformableContactConstraint(other)
+, m_penetration(other.m_penetration)
+{
+    m_total_normal_dv = other.m_total_normal_dv;
+    m_total_tangent_dv = other.m_total_tangent_dv;
+}
+
+
+btVector3 btDeformableRigidContactConstraint::getVa() const
+{
+    const btSoftBody::sCti& cti = m_contact->m_cti;
+    btVector3 va(0, 0, 0);
+    if (cti.m_colObj->hasContactResponse())
+    {
+        btRigidBody* rigidCol = 0;
+        btMultiBodyLinkCollider* multibodyLinkCol = 0;
+        
+        // grab the velocity of the rigid body
+        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+        {
+            rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+            va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
+        }
+        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+        {
+            multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+            if (multibodyLinkCol)
+            {
+                const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
+                const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
+                const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
+                const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+                const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+                // add in the normal component of the va
+                btScalar vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_n[k];
+                }
+                va = cti.m_normal * vel;
+                // add in the tangential components of the va
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t1[k];
+                }
+                va += m_contact->t1 * vel;
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t2[k];
+                }
+                va += m_contact->t2 * vel;
+            }
+        }
+    }
+    return va;
+}
+
+btScalar btDeformableRigidContactConstraint::solveConstraint()
+{
+    const btSoftBody::sCti& cti = m_contact->m_cti;
+    btVector3 va = getVa();
+    btVector3 vb = getVb();
+    btVector3 vr = vb - va;
+    const btScalar dn = btDot(vr, cti.m_normal);
+    // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+    btScalar residualSquare = dn*dn;
+    btVector3 impulse = m_contact->m_c0 * vr;
+    const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
+    btVector3 impulse_tangent = impulse - impulse_normal;
+    btVector3 old_total_tangent_dv = m_total_tangent_dv;
+    // m_c2 is the inverse mass of the deformable node/face
+    m_total_normal_dv -= impulse_normal * m_contact->m_c2;
+    m_total_tangent_dv -= impulse_tangent * m_contact->m_c2;
+
+    if (m_total_normal_dv.dot(cti.m_normal) < 0)
+    {
+        // separating in the normal direction
+        m_static = false;
+        m_total_tangent_dv = btVector3(0,0,0);
+        impulse_tangent.setZero();
+    }
+    else
+    {
+        if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
+        {
+            // dynamic friction
+            // 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.
+            m_static = false;
+            if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
+            {
+                m_total_tangent_dv = btVector3(0,0,0);
+            }
+            else
+            {
+                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
+            }
+            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
+        }
+        else
+        {
+            // static friction
+            m_static = true;
+        }
+    }
+    impulse = impulse_normal + impulse_tangent;
+    // apply impulse to deformable nodes involved and change their velocities
+    applyImpulse(impulse);
+    // apply impulse to the rigid/multibodies involved and change their velocities
+    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+    {
+        btRigidBody* rigidCol = 0;
+        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+        if (rigidCol)
+        {
+            rigidCol->applyImpulse(impulse, m_contact->m_c1);
+        }
+    }
+    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+    {
+        btMultiBodyLinkCollider* multibodyLinkCol = 0;
+        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+        if (multibodyLinkCol)
+        {
+            const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+            // apply normal component of the impulse
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+            if (impulse_tangent.norm() > SIMD_EPSILON)
+            {
+                // apply tangential component of the impulse
+                const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
+                const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
+            }
+        }
+    }
+    return residualSquare;
+}
+
+btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
+{
+    const btSoftBody::sCti& cti = m_contact->m_cti;
+    const btScalar dn = m_penetration;
+    if (dn != 0)
+    {
+        const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
+        // one iteration of the position impulse corrects all the position error at this timestep
+        m_penetration -= dn;
+        // apply impulse to deformable nodes involved and change their position
+        applySplitImpulse(impulse);
+        // apply impulse to the rigid/multibodies involved and change their position
+        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+        {
+            btRigidBody* rigidCol = 0;
+            rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+            if (rigidCol)
+            {
+                rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
+            }
+        }
+        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+        {
+            // todo xuchenhan@
+        }
+        return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
+    }
+    return 0;
+}
+
+/* ================   Node vs. Rigid   =================== */
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
+    : m_node(contact.m_node)
+    , btDeformableRigidContactConstraint(contact)
+    {
+    }
+
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
+: m_node(other.m_node)
+, btDeformableRigidContactConstraint(other)
+{
+}
+
+btVector3 btDeformableNodeRigidContactConstraint::getVb() const
+{
+    return m_node->m_v;
+}
+
+
+btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+    return m_total_normal_dv + m_total_tangent_dv;
+}
+
+void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+    btVector3 dv = impulse * contact->m_c2;
+    contact->m_node->m_v -= dv;
+}
+
+void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+    btVector3 dv = impulse * contact->m_c2;
+    contact->m_node->m_vsplit -= dv;
+};
+
+/* ================   Face vs. Rigid   =================== */
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
+: m_face(contact.m_face)
+, btDeformableRigidContactConstraint(contact)
+{
+}
+
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
+: m_face(other.m_face)
+, btDeformableRigidContactConstraint(other)
+{
+}
+
+btVector3 btDeformableFaceRigidContactConstraint::getVb() const
+{
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    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];
+    return vb;
+}
+
+
+btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+    btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    if (m_face->m_n[0] == node)
+    {
+        return face_dv * contact->m_weights[0];
+    }
+    if (m_face->m_n[1] == node)
+    {
+        return face_dv * contact->m_weights[1];
+    }
+    btAssert(node == m_face->m_n[2]);
+    return face_dv * contact->m_weights[2];
+}
+
+void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    btVector3 dv = impulse * contact->m_c2;
+    btSoftBody::Face* face = contact->m_face;
+    
+    btVector3& v0 = face->m_n[0]->m_v;
+    btVector3& v1 = face->m_n[1]->m_v;
+    btVector3& v2 = face->m_n[2]->m_v;
+    const btScalar& im0 = face->m_n[0]->m_im;
+    const btScalar& im1 = face->m_n[1]->m_im;
+    const btScalar& im2 = face->m_n[2]->m_im;
+    if (im0 > 0)
+        v0 -= dv * contact->m_weights[0];
+    if (im1 > 0)
+        v1 -= dv * contact->m_weights[1];
+    if (im2 > 0)
+        v2 -= dv * contact->m_weights[2];
+    
+    // apply strain limiting to prevent undamped modes
+    btScalar m01 = (btScalar(1)/(im0 + im1));
+    btScalar m02 = (btScalar(1)/(im0 + im2));
+    btScalar m12 = (btScalar(1)/(im1 + im2));
+    
+    btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
+    btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
+    btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
+    
+    v0 += dv0;
+    v1 += dv1;
+    v2 += dv2;
+}
+
+void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    btVector3 dv = impulse * contact->m_c2;
+    btSoftBody::Face* face = contact->m_face;
+
+    btVector3& v0 = face->m_n[0]->m_vsplit;
+    btVector3& v1 = face->m_n[1]->m_vsplit;
+    btVector3& v2 = face->m_n[2]->m_vsplit;
+    const btScalar& im0 = face->m_n[0]->m_im;
+    const btScalar& im1 = face->m_n[1]->m_im;
+    const btScalar& im2 = face->m_n[2]->m_im;
+    if (im0 > 0)
+        v0 -= dv * contact->m_weights[0];
+    if (im1 > 0)
+        v1 -= dv * contact->m_weights[1];
+    if (im2 > 0)
+        v2 -= dv * contact->m_weights[2];
+}
+
+/* ================   Face vs. Node   =================== */
+btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
+: m_node(contact.m_node)
+, m_face(contact.m_face)
+, m_contact(&contact)
+, btDeformableContactConstraint(contact.m_normal)
+{
+    m_total_normal_dv.setZero();
+    m_total_tangent_dv.setZero();
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getVa() const
+{
+    return m_node->m_v;
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getVb() const
+{
+    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+    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];
+    return vb;
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const
+{
+    btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
+    if (n == m_node)
+        return dv;
+    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+    if (m_face->m_n[0] == n)
+    {
+        return dv * contact->m_weights[0];
+    }
+    if (m_face->m_n[1] == n)
+    {
+        return dv * contact->m_weights[1];
+    }
+    btAssert(n == m_face->m_n[2]);
+    return dv * contact->m_weights[2];
+}
+
+btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
+{
+    btVector3 va = getVa();
+    btVector3 vb = getVb();
+    btVector3 vr = vb - va;
+    const btScalar dn = btDot(vr, m_contact->m_normal);
+    // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+    btScalar residualSquare = dn*dn;
+    btVector3 impulse = m_contact->m_c0 * vr;
+    const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
+    btVector3 impulse_tangent = impulse - impulse_normal;
+    
+    btVector3 old_total_tangent_dv = m_total_tangent_dv;
+    // m_c2 is the inverse mass of the deformable node/face
+    if (m_node->m_im > 0)
+    {
+        m_total_normal_dv -= impulse_normal * m_node->m_im;
+        m_total_tangent_dv -= impulse_tangent * m_node->m_im;
+    }
+    else
+    {
+        m_total_normal_dv -= impulse_normal * m_contact->m_imf;
+        m_total_tangent_dv -= impulse_tangent *  m_contact->m_imf;
+    }
+    
+    if (m_total_normal_dv.dot(m_contact->m_normal) > 0)
+    {
+        // separating in the normal direction
+        m_static = false;
+        m_total_tangent_dv = btVector3(0,0,0);
+        impulse_tangent.setZero();
+    }
+    else
+    {
+        if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
+        {
+            // dynamic friction
+            // 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.
+            m_static = false;
+            if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
+            {
+                m_total_tangent_dv = btVector3(0,0,0);
+            }
+            else
+            {
+                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
+            }
+            impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
+        }
+        else
+        {
+            // static friction
+            m_static = true;
+        }
+    }
+    impulse = impulse_normal + impulse_tangent;
+    // apply impulse to deformable nodes involved and change their velocities
+    applyImpulse(impulse);
+    return residualSquare;
+}
+
+void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+    btVector3 dva = impulse * contact->m_node->m_im;
+    btVector3 dvb = impulse * contact->m_imf;
+    if (contact->m_node->m_im > 0)
+    {
+        contact->m_node->m_v += dva;
+    }
+    
+    btSoftBody::Face* face = contact->m_face;
+    btVector3& v0 = face->m_n[0]->m_v;
+    btVector3& v1 = face->m_n[1]->m_v;
+    btVector3& v2 = face->m_n[2]->m_v;
+    const btScalar& im0 = face->m_n[0]->m_im;
+    const btScalar& im1 = face->m_n[1]->m_im;
+    const btScalar& im2 = face->m_n[2]->m_im;
+    if (im0 > 0)
+    {
+        v0 -= dvb * contact->m_weights[0];
+    }
+    if (im1 > 0)
+    {
+        v1 -= dvb * contact->m_weights[1];
+    }
+    if (im2 > 0)
+    {
+        v2 -= dvb * contact->m_weights[2];
+    }
+    // todo: Face node constraints needs more work
+//    btScalar m01 = (btScalar(1)/(im0 + im1));
+//    btScalar m02 = (btScalar(1)/(im0 + im2));
+//    btScalar m12 = (btScalar(1)/(im1 + im2));
+//
+//    btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
+//    btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
+//    btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
+//    v0 += dv0;
+//    v1 += dv1;
+//    v2 += dv2;
+}