btReducedDeformableContactConstraint.cpp

Go to the documentation of this file.

#include "btReducedDeformableContactConstraint.h"
#include <iostream>
 
// ================= static constraints ===================
btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
  btReducedDeformableBody* rsb, 
  btSoftBody::Node* node,
        const btVector3& ri,
        const btVector3& x0,
        const btVector3& dir,
  const btContactSolverInfo& infoGlobal,
        btScalar dt)
  : m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
{
        m_erp = 0.2;
        m_appliedImpulse = 0;
 
        // get impulse factor
  m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index);
        m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection);
 
        btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection);
        btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection);
 
        m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor;
}
 
btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
        // target velocity of fixed constraint is 0
        btVector3 deltaVa = getDeltaVa();
        btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection);
  btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor;
        m_appliedImpulse = m_appliedImpulse + deltaImpulse;
 
        btVector3 impulse = deltaImpulse * m_impulseDirection;
        applyImpulse(impulse);
 
        // calculate residual
        btScalar residualSquare = m_impulseFactor * deltaImpulse;
        residualSquare *= residualSquare;
 
        return residualSquare;
}
  
// this calls reduced deformable body's internalApplyFullSpaceImpulse
void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
{
        // apply full space impulse
        m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
}
 
btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const
{
        return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
}
 
// ================= base contact constraints ===================
btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint(
  btReducedDeformableBody* rsb, 
  const btSoftBody::DeformableRigidContact& c, 
  const btContactSolverInfo& infoGlobal,
        btScalar dt)
  : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal)
{
        m_nodeQueryIndex = 0;
        m_appliedNormalImpulse = 0;
  m_appliedTangentImpulse = 0;
        m_rhs = 0;
        m_rhs_tangent = 0;
        m_cfm = infoGlobal.m_deformable_cfm;
        m_cfm_friction = 0;
        m_erp = infoGlobal.m_deformable_erp;
        m_erp_friction = infoGlobal.m_deformable_erp;
        m_friction = infoGlobal.m_friction;
 
        m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject();
        m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK);
}
 
void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body)
{
        if (!m_collideMultibody)
        {
                m_solverBodyId = bodyId;
                m_solverBody = &solver_body;
                m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
                btVector3       torqueAxis = -m_relPosA.cross(m_contactNormalA);
                m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
                
                m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass();
                btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent);
                m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
        }
}
 
btVector3 btReducedDeformableRigidContactConstraint::getVa() const
{
        btVector3 Va(0, 0, 0);
        if (!m_collideStatic)
        {
                Va = btDeformableRigidContactConstraint::getVa();
        }
        return Va;
}
 
btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
{
        // btVector3 Va = getVa();
        // btVector3 deltaVa = Va - m_bufferVelocityA;
        // if (!m_collideStatic)
        // {
                // std::cout << "moving collision!!!\n";
                // std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n";
                // std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t'
                //  << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t'
                //   << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n';
        // }
        btVector3 deltaVa = getDeltaVa();
        btVector3 deltaVb = getDeltaVb();
 
        // if (!m_collideStatic)
        // {
        //      std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
        //      std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
        // }
 
        // get delta relative velocity and magnitude (i.e., how much impulse has been applied?)
        btVector3 deltaV_rel = deltaVa - deltaVb;
        btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
 
        // if (!m_collideStatic)
        // {
        //      std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n";
        //      std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
        //      std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n';
        // }
        
        // get the normal impulse to be applied
        btScalar deltaImpulse = m_rhs - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor;
        // if (!m_collideStatic)
        // {
        //      std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: "  << m_appliedNormalImpulse << "\n";
        //      std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n';
        // }
 
        {
                // cumulative impulse that has been applied
                btScalar sum = m_appliedNormalImpulse + deltaImpulse;
                // if the cumulative impulse is pushing the object into the rigid body, set it zero
                if (sum < 0)
                {
                        deltaImpulse = -m_appliedNormalImpulse;
                        m_appliedNormalImpulse = 0;
                }
                else
                {
                        m_appliedNormalImpulse = sum;
                }       
        }
 
        // if (!m_collideStatic)
        // {
        //      std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
        //      std::cout << "deltaImpulse: " << deltaImpulse << '\n';
        // }
 
        // residual is the nodal normal velocity change in current iteration
        btScalar residualSquare = deltaImpulse * m_normalImpulseFactor; // get residual
        residualSquare *= residualSquare;
 
        
        // apply Coulomb friction (based on delta velocity, |dv_t| = |dv_n * friction|)
        btScalar deltaImpulse_tangent = 0;
        btScalar deltaImpulse_tangent2 = 0;
        {
                // calculate how much impulse is needed
                // btScalar deltaV_rel_tangent = btDot(deltaV_rel, m_contactTangent);
                // btScalar impulse_changed = deltaV_rel_tangent * m_tangentImpulseFactorInv;
                // deltaImpulse_tangent = m_rhs_tangent - impulse_changed;
 
                // btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent;
                btScalar lower_limit = - m_appliedNormalImpulse * m_friction;
                btScalar upper_limit = m_appliedNormalImpulse * m_friction;
                // if (sum > upper_limit)
                // {
                //      deltaImpulse_tangent = upper_limit - m_appliedTangentImpulse;
                //      m_appliedTangentImpulse = upper_limit;
                // }
                // else if (sum < lower_limit)
                // {
                //      deltaImpulse_tangent = lower_limit - m_appliedTangentImpulse;
                //      m_appliedTangentImpulse = lower_limit;
                // }
                // else
                // {
                //      m_appliedTangentImpulse = sum;
                // }
                // 
                calculateTangentialImpulse(deltaImpulse_tangent, m_appliedTangentImpulse, m_rhs_tangent,
                                                                                                                         m_tangentImpulseFactorInv, m_contactTangent, lower_limit, upper_limit, deltaV_rel);
                
                if (m_collideMultibody)
                {
                        calculateTangentialImpulse(deltaImpulse_tangent2, m_appliedTangentImpulse2, m_rhs_tangent2,
                                                                                                                           m_tangentImpulseFactorInv2, m_contactTangent2, lower_limit, upper_limit, deltaV_rel);
                }
                                                                                                                         
 
                if (!m_collideStatic)
                {
                        // std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t"  << m_contactTangent[1] << "\t"  << m_contactTangent[2] << "\n";
                        // std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n';
                        // std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n';
                        // std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n';
                }
        }
 
        // get the total impulse vector
        btVector3 impulse_normal = deltaImpulse * m_contactNormalA;
        btVector3 impulse_tangent = deltaImpulse_tangent * (-m_contactTangent);
        btVector3 impulse_tangent2 = deltaImpulse_tangent2 * (-m_contactTangent2);
        btVector3 impulse = impulse_normal + impulse_tangent + impulse_tangent2;
 
        applyImpulse(impulse);
        
        // apply impulse to the rigid/multibodies involved and change their velocities
        if (!m_collideStatic)
        {
                // std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t'
                //                                                                                                                                      << m_linearComponentNormal[1] << '\t'
                //                                                                                                                                      << m_linearComponentNormal[2] << '\n';
                // std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t'
                //                                                                                                                                      << m_angularComponentNormal[1] << '\t'
                //                                                                                                                                      << m_angularComponentNormal[2] << '\n';
 
                if (!m_collideMultibody)                // collision with rigid body
                {
                        // std::cout << "rigid impulse applied!!\n";
                        // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
                        // << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
                        //      << m_solverBody->getDeltaLinearVelocity()[2] << '\n';
                        // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
                        // << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
                        //      << m_solverBody->getDeltaAngularVelocity()[2] << '\n';
 
                        m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse);
                        m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent);
 
                        // std::cout << "after\n";
                        // std::cout << "rigid impulse applied!!\n";
                        // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
                        // << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
                        //      << m_solverBody->getDeltaLinearVelocity()[2] << '\n';
                        // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
                        // << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
                        //      << m_solverBody->getDeltaAngularVelocity()[2] << '\n';
                }
                else            // collision with multibody
                {
                        btMultiBodyLinkCollider* multibodyLinkCol = 0;
                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_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, -deltaImpulse);
                                
                                // const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
                                // std::cout << "deltaV_normal: ";
                                // for (int i = 0; i < ndof; ++i)
                                // {
                                //      std::cout << i << "\t" << deltaV_normal[i] << '\n';
                                // }
 
                                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, deltaImpulse_tangent);
                                        const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
                                        multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, deltaImpulse_tangent2);
                                }
                        }
                }
        }
        return residualSquare;
}
 
void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btScalar& deltaImpulse_tangent, 
                                                                                                                                                                                                                                                                                                         btScalar& appliedImpulse, 
                                                                                                                                                                                                                                                                                                         const btScalar rhs_tangent,
                                                                                                                                                                                                                                                                                                         const btScalar tangentImpulseFactorInv,
                                                                                                                                                                                                                                                                                                         const btVector3& tangent,
                                                                                                                                                                                                                                                                                                         const btScalar lower_limit,
                                                                                                                                                                                                                                                                                                         const btScalar upper_limit,
                                                                                                                                                                                                                                                                                                         const btVector3& deltaV_rel)
{
        btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent);
        btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv;
        deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed;
 
        btScalar sum = appliedImpulse + deltaImpulse_tangent;
        if (sum > upper_limit)
        {
                deltaImpulse_tangent = upper_limit - appliedImpulse;
                appliedImpulse = upper_limit;
        }
        else if (sum < lower_limit)
        {
                deltaImpulse_tangent = lower_limit - appliedImpulse;
                appliedImpulse = lower_limit;
        }
        else
        {
                appliedImpulse = sum;
        }
}
 
// ================= node vs rigid constraints ===================
btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint(
  btReducedDeformableBody* rsb, 
  const btSoftBody::DeformableNodeRigidContact& contact, 
  const btContactSolverInfo& infoGlobal,
        btScalar dt)
  : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
{
        m_contactNormalA = contact.m_cti.m_normal;
  m_contactNormalB = -contact.m_cti.m_normal;
 
        if (contact.m_node->index < rsb->m_nodes.size())
        {
                m_nodeQueryIndex = contact.m_node->index;
        }
        else
        {
                m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset;
        }
 
        if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
        {
                m_relPosA = contact.m_c1;
        }
        else
        {
                m_relPosA = btVector3(0,0,0);
        }
        m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin();
 
        if (m_collideStatic)            // colliding with static object, only consider reduced deformable body's impulse factor
        {
                m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex);
        }
        else            // colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors
        {
                m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0;
        }
 
        m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA);
        m_tangentImpulseFactor = 0;
 
        warmStarting();
}
 
void btReducedDeformableNodeRigidContactConstraint::warmStarting()
{
        btVector3 va = getVa();
        btVector3 vb = getVb();
        m_bufferVelocityA = va;
        m_bufferVelocityB = vb;
 
        // we define the (+) direction of errors to be the outward surface normal of the rigid object
        btVector3 v_rel = vb - va;
        // get tangent direction of the relative velocity
        btVector3 v_tangent = v_rel - v_rel.dot(m_contactNormalA) * m_contactNormalA;
        if (v_tangent.norm() < SIMD_EPSILON)
        {
                m_contactTangent = btVector3(0, 0, 0);
                // tangent impulse factor
                m_tangentImpulseFactor = 0;
                m_tangentImpulseFactorInv = 0;
        }
        else
        {
                if (!m_collideMultibody)
                {
                        m_contactTangent = v_tangent.normalized();
                        m_contactTangent2.setZero();
                        // tangent impulse factor 1
                        m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
                        m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
                        // tangent impulse factor 2
                        m_tangentImpulseFactor2 = 0;
                        m_tangentImpulseFactorInv2 = 0;
                }
                else    // multibody requires 2 contact directions
                {
                        m_contactTangent = m_contact->t1;
                        m_contactTangent2 = m_contact->t2;
 
                        // tangent impulse factor 1
                        m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
                        m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
                        // tangent impulse factor 2
                        m_tangentImpulseFactor2 = (m_impulseFactor * m_contactTangent2).dot(m_contactTangent2);
                        m_tangentImpulseFactorInv2 = btScalar(1) / m_tangentImpulseFactor2;
                }
        }
 
 
        // initial guess for normal impulse
        {
                btScalar velocity_error = btDot(v_rel, m_contactNormalA);       // magnitude of relative velocity
                btScalar position_error = 0;
                if (m_penetration > 0)
                {
                        velocity_error += m_penetration / m_dt;
                }
                else
                {
                        // add penetration correction vel
                        position_error = m_penetration * m_erp / m_dt;
                }
                // get the initial estimate of impulse magnitude to be applied
                m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor;
        }
 
        // initial guess for tangential impulse
        {
                btScalar velocity_error = btDot(v_rel, m_contactTangent);
                m_rhs_tangent = velocity_error * m_tangentImpulseFactorInv;
 
                if (m_collideMultibody)
                {
                        btScalar velocity_error2 = btDot(v_rel, m_contactTangent2);
                        m_rhs_tangent2 = velocity_error2 * m_tangentImpulseFactorInv2;
                }
        }
 
        // warm starting
        // applyImpulse(m_rhs * m_contactNormalA);
        // if (!m_collideStatic)
        // {
        //      const btSoftBody::sCti& cti = m_contact->m_cti;
        //      if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
        //      {
        //              m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs);
        //      }
        // }
}
 
btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
{
        return m_node->m_v;
}
 
btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
{
        btVector3 deltaVa(0, 0, 0);
        if (!m_collideStatic)
        {
                if (!m_collideMultibody)                // for rigid body
                {
                        deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA);
                }
                else            // for multibody
                {
                        btMultiBodyLinkCollider* multibodyLinkCol = 0;
                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_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_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
                                // add in the normal component of the va
                                btScalar vel = 0;
                                for (int k = 0; k < ndof; ++k)
                                {
                                        vel += local_dv[k] * J_n[k];
                                }
                                deltaVa = m_contact->m_cti.m_normal * vel;
                                
                                // add in the tangential components of the va
                                vel = 0;
                                for (int k = 0; k < ndof; ++k)
                                {
                                        vel += local_dv[k] * J_t1[k];
                                }
                                deltaVa += m_contact->t1 * vel;
 
                                vel = 0;
                                for (int k = 0; k < ndof; ++k)
                                {
                                        vel += local_dv[k] * J_t2[k];
                                }
                                deltaVa += m_contact->t2 * vel;
                        }
                }
        }
        return deltaVa;
}
 
btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
{       
        // std::cout << "node: " << m_node->index << '\n';
        return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex);
}
 
btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const
{
        return m_node->m_splitv;
}
 
btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
{
        return m_total_normal_dv + m_total_tangent_dv;
}
 
void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
{
  m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt);
        // m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
        // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
        // if (!m_collideStatic)
        // {
        //      // std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
        //      // 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';
        //      btVector3 v_after = getDeltaVb() + m_node->m_v;
        //      // std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n';
        // }
        // 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';
}
 
// ================= face vs rigid constraints ===================
btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint(
  btReducedDeformableBody* rsb, 
  const btSoftBody::DeformableFaceRigidContact& contact, 
  const btContactSolverInfo& infoGlobal,
        btScalar dt, 
  bool useStrainLimiting)
  : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
{}
 
btVector3 btReducedDeformableFaceRigidContactConstraint::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 btReducedDeformableFaceRigidContactConstraint::getSplitVb() const
{
        const btSoftBody::DeformableFaceRigidContact* contact = getContact();
        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];
        return vb;
}
 
btVector3 btReducedDeformableFaceRigidContactConstraint::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 btReducedDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
{
  //
}