path: root/thirdparty/bullet/BulletSoftBody/btDeformableContactConstraint.cpp

/*
 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;
}