btMultiBodyConstraint.cpp

Go to the documentation of this file.

#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btMultiBodyPoint2Point.h"                             //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)



btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
        :m_bodyA(bodyA),
        m_bodyB(bodyB),
        m_linkA(linkA),
        m_linkB(linkB),
        m_numRows(numRows),
        m_jacSizeA(0),
        m_jacSizeBoth(0),
        m_isUnilateral(isUnilateral),
        m_maxAppliedImpulse(100),
        m_numDofsFinalized(-1)
{

}

void btMultiBodyConstraint::updateJacobianSizes()
{
    if(m_bodyA)
        {
                if(m_bodyA->isMultiDof())
                        m_jacSizeA = (6 + m_bodyA->getNumDofs());
                else
                        m_jacSizeA = (6 + m_bodyA->getNumLinks());
        }

        if(m_bodyB)
        {
                if(m_bodyB->isMultiDof())
                        m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
                else
                        m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks();
        }
        else
                m_jacSizeBoth = m_jacSizeA;
}

void btMultiBodyConstraint::allocateJacobiansMultiDof()
{
        updateJacobianSizes();

        m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
        m_data.resize((2 + m_jacSizeBoth) * m_numRows);
}

btMultiBodyConstraint::~btMultiBodyConstraint()
{
}

void    btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{
        for (int i = 0; i < ndof; ++i)
                data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}

btScalar btMultiBodyConstraint::fillMultiBodyConstraint(        btMultiBodySolverConstraint& solverConstraint,
                                                                                                                        btMultiBodyJacobianData& data,
                                                                                                                        btScalar* jacOrgA, btScalar* jacOrgB,
                                                                                                                        const btVector3& contactNormalOnB,
                                                                                                                        const btVector3& posAworld, const btVector3& posBworld,
                                                                                                                        btScalar posError,
                                                                                                                        const btContactSolverInfo& infoGlobal,
                                                                                                                        btScalar lowerLimit, btScalar upperLimit,
                                                                                                                        btScalar relaxation,
                                                                                                                        bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{


        solverConstraint.m_multiBodyA = m_bodyA;
        solverConstraint.m_multiBodyB = m_bodyB;
        solverConstraint.m_linkA = m_linkA;
        solverConstraint.m_linkB = m_linkB;

        btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
        btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;

        btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
        btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);

        btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
        btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;

        btVector3 rel_pos1, rel_pos2;                           //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
        if (bodyA)
                rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
        if (bodyB)
                rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();

        if (multiBodyA)
        {
                const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;

                solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();

                if (solverConstraint.m_deltaVelAindex <0)
                {
                        solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
                        multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
                        data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
                } else
                {
                        btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
                }

                //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
                //resize..
                solverConstraint.m_jacAindex = data.m_jacobians.size();
                data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
                //copy/determine
                if(jacOrgA)
                {
                        for (int i=0;i<ndofA;i++)
                                data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
                }
                else
                {
                        btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
                        if(multiBodyA->isMultiDof())
                                multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
                        else
                                multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
                }

                //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
                //resize..
                data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);               //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
                btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
                btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
                //determine..
                if(multiBodyA->isMultiDof())
                        multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
                else
                        multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
        }
        else if(rb0)
        {
                btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
                solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
                solverConstraint.m_relpos1CrossNormal = torqueAxis0;
                solverConstraint.m_contactNormal1 = contactNormalOnB;
        }

        if (multiBodyB)
        {
                const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;

                solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
                if (solverConstraint.m_deltaVelBindex <0)
                {
                        solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
                        multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
                        data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
                }

                //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
                //resize..
                solverConstraint.m_jacBindex = data.m_jacobians.size();
                data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
                //copy/determine..
                if(jacOrgB)
                {
                        for (int i=0;i<ndofB;i++)
                                data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
                }
                else
                {
                        if(multiBodyB->isMultiDof())
                                multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
                        else
                                multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
                }

                //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
                //resize..
                data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
                btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
                btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
                //determine..
                if(multiBodyB->isMultiDof())
                        multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
                else
                        multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);

        }
        else if(rb1)
        {
                btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
                solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
                solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
                solverConstraint.m_contactNormal2 = -contactNormalOnB;
        }
        {

                btVector3 vec;
                btScalar denom0 = 0.f;
                btScalar denom1 = 0.f;
                btScalar* jacB = 0;
                btScalar* jacA = 0;
                btScalar* deltaVelA = 0;
                btScalar* deltaVelB = 0;
                int ndofA  = 0;
                //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
                if (multiBodyA)
                {
                        ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
                        jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
                        deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
                        for (int i = 0; i < ndofA; ++i)
                        {
                                btScalar j = jacA[i] ;
                                btScalar l = deltaVelA[i];
                                denom0 += j*l;
                        }
                }
                else if(rb0)
                {
                        vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
                        denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
                }
                //
                if (multiBodyB)
                {
                        const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
                        jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
                        deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
                        for (int i = 0; i < ndofB; ++i)
                        {
                                btScalar j = jacB[i] ;
                                btScalar l = deltaVelB[i];
                                denom1 += j*l;
                        }

                }
                else if(rb1)
                {
                        vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
                        denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
                }

                //
                btScalar d = denom0+denom1;
                if (d>SIMD_EPSILON)
                {
                        solverConstraint.m_jacDiagABInv = relaxation/(d);
                }
                else
                {
                //disable the constraint row to handle singularity/redundant constraint
                        solverConstraint.m_jacDiagABInv  = 0.f;
                }
        }


        //compute rhs and remaining solverConstraint fields
        btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;

        btScalar rel_vel = 0.f;
        int ndofA  = 0;
        int ndofB  = 0;
        {
                btVector3 vel1,vel2;
                if (multiBodyA)
                {
                        ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
                        btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
                        for (int i = 0; i < ndofA ; ++i)
                                rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
                }
                else if(rb0)
                {
                        rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
                }
                if (multiBodyB)
                {
                        ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
                        btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
                        for (int i = 0; i < ndofB ; ++i)
                                rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];

                }
                else if(rb1)
                {
                        rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
                }

                solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
        }


        /*
        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
        {
                solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;

                if (solverConstraint.m_appliedImpulse)
                {
                        if (multiBodyA)
                        {
                                btScalar impulse = solverConstraint.m_appliedImpulse;
                                btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
                                multiBodyA->applyDeltaVee(deltaV,impulse);
                                applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
                        } else
                        {
                                if (rb0)
                                        bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
                        }
                        if (multiBodyB)
                        {
                                btScalar impulse = solverConstraint.m_appliedImpulse;
                                btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
                                multiBodyB->applyDeltaVee(deltaV,impulse);
                                applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
                        } else
                        {
                                if (rb1)
                                        bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
                        }
                }
        } else
        */

        solverConstraint.m_appliedImpulse = 0.f;
        solverConstraint.m_appliedPushImpulse = 0.f;

        {

                btScalar positionalError = 0.f;
                btScalar        velocityError = desiredVelocity - rel_vel;// * damping;


                btScalar erp = infoGlobal.m_erp2;
                if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
                {
                        erp = infoGlobal.m_erp;
                }

                positionalError = -penetration * erp/infoGlobal.m_timeStep;

                btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
                btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;

                if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
                {
                        //combine position and velocity into rhs
                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
                        solverConstraint.m_rhsPenetration = 0.f;

                } else
                {
                        //split position and velocity into rhs and m_rhsPenetration
                        solverConstraint.m_rhs = velocityImpulse;
                        solverConstraint.m_rhsPenetration = penetrationImpulse;
                }

                solverConstraint.m_cfm = 0.f;
                solverConstraint.m_lowerLimit = lowerLimit;
                solverConstraint.m_upperLimit = upperLimit;
        }

        return rel_vel;

}