298 lines
8.6 KiB
C++
298 lines
8.6 KiB
C++
|
|
#include "btMultiBodyJointLimitConstraint.h"
|
|
#include "btMultiBody.h"
|
|
#include "btMultiBodyLinkCollider.h"
|
|
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
|
|
|
|
|
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
|
|
:btMultiBodyConstraint(body,body,link,link,2,true),
|
|
m_lowerBound(lower),
|
|
m_upperBound(upper)
|
|
{
|
|
// the data.m_jacobians never change, so may as well
|
|
// initialize them here
|
|
|
|
// note: we rely on the fact that data.m_jacobians are
|
|
// always initialized to zero by the Constraint ctor
|
|
|
|
// row 0: the lower bound
|
|
jacobianA(0)[6 + link] = 1;
|
|
|
|
// row 1: the upper bound
|
|
jacobianB(1)[6 + link] = -1;
|
|
}
|
|
btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
|
|
{
|
|
}
|
|
|
|
int btMultiBodyJointLimitConstraint::getIslandIdA() const
|
|
{
|
|
return m_bodyA->getLinkCollider(0)->getIslandTag();
|
|
}
|
|
|
|
int btMultiBodyJointLimitConstraint::getIslandIdB() const
|
|
{
|
|
return m_bodyB->getLinkCollider(0)->getIslandTag();
|
|
}
|
|
|
|
|
|
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
|
btMultiBodyJacobianData& data,
|
|
const btContactSolverInfo& infoGlobal)
|
|
{
|
|
// only positions need to be updated -- data.m_jacobians and force
|
|
// directions were set in the ctor and never change.
|
|
|
|
// row 0: the lower bound
|
|
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound);
|
|
|
|
// row 1: the upper bound
|
|
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
|
|
|
|
for (int row=0;row<getNumRows();row++)
|
|
{
|
|
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
|
constraintRow.m_multiBodyA = m_bodyA;
|
|
constraintRow.m_multiBodyB = m_bodyB;
|
|
btScalar penetration = getPosition(row);
|
|
fillConstraintRow(constraintRow,data,jacobianA(row),jacobianB(row),penetration,0,0,infoGlobal);
|
|
}
|
|
|
|
}
|
|
|
|
void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow,
|
|
btMultiBodyJacobianData& data,
|
|
btScalar* jacOrgA,btScalar* jacOrgB,
|
|
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
|
|
const btContactSolverInfo& infoGlobal)
|
|
|
|
{
|
|
|
|
|
|
|
|
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
|
|
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
|
|
|
|
if (multiBodyA)
|
|
{
|
|
|
|
const int ndofA = multiBodyA->getNumLinks() + 6;
|
|
|
|
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
|
|
|
|
if (constraintRow.m_deltaVelAindex <0)
|
|
{
|
|
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
|
|
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
|
|
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
|
|
} else
|
|
{
|
|
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
|
|
}
|
|
|
|
constraintRow.m_jacAindex = data.m_jacobians.size();
|
|
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
|
|
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
|
|
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
|
for (int i=0;i<ndofA;i++)
|
|
data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
|
|
|
|
float* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
|
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
|
}
|
|
|
|
if (multiBodyB)
|
|
{
|
|
const int ndofB = multiBodyB->getNumLinks() + 6;
|
|
|
|
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
|
|
if (constraintRow.m_deltaVelBindex <0)
|
|
{
|
|
constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
|
|
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
|
|
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
|
|
}
|
|
|
|
constraintRow.m_jacBindex = data.m_jacobians.size();
|
|
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
|
|
|
|
for (int i=0;i<ndofB;i++)
|
|
data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
|
|
|
|
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
|
|
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
|
|
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
|
|
}
|
|
{
|
|
|
|
btVector3 vec;
|
|
btScalar denom0 = 0.f;
|
|
btScalar denom1 = 0.f;
|
|
btScalar* jacB = 0;
|
|
btScalar* jacA = 0;
|
|
btScalar* lambdaA =0;
|
|
btScalar* lambdaB =0;
|
|
int ndofA = 0;
|
|
if (multiBodyA)
|
|
{
|
|
ndofA = multiBodyA->getNumLinks() + 6;
|
|
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
|
|
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
|
for (int i = 0; i < ndofA; ++i)
|
|
{
|
|
float j = jacA[i] ;
|
|
float l =lambdaA[i];
|
|
denom0 += j*l;
|
|
}
|
|
}
|
|
if (multiBodyB)
|
|
{
|
|
const int ndofB = multiBodyB->getNumLinks() + 6;
|
|
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
|
|
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
|
|
for (int i = 0; i < ndofB; ++i)
|
|
{
|
|
float j = jacB[i] ;
|
|
float l =lambdaB[i];
|
|
denom1 += j*l;
|
|
}
|
|
|
|
}
|
|
|
|
if (multiBodyA && (multiBodyA==multiBodyB))
|
|
{
|
|
// ndof1 == ndof2 in this case
|
|
for (int i = 0; i < ndofA; ++i)
|
|
{
|
|
denom1 += jacB[i] * lambdaA[i];
|
|
denom1 += jacA[i] * lambdaB[i];
|
|
}
|
|
}
|
|
|
|
float d = denom0+denom1;
|
|
if (btFabs(d)>SIMD_EPSILON)
|
|
{
|
|
|
|
constraintRow.m_jacDiagABInv = 1.f/(d);
|
|
} else
|
|
{
|
|
constraintRow.m_jacDiagABInv = 1.f;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
//compute rhs and remaining constraintRow fields
|
|
|
|
|
|
|
|
|
|
btScalar rel_vel = 0.f;
|
|
int ndofA = 0;
|
|
int ndofB = 0;
|
|
{
|
|
|
|
btVector3 vel1,vel2;
|
|
if (multiBodyA)
|
|
{
|
|
ndofA = multiBodyA->getNumLinks() + 6;
|
|
btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
|
|
for (int i = 0; i < ndofA ; ++i)
|
|
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
|
|
}
|
|
if (multiBodyB)
|
|
{
|
|
ndofB = multiBodyB->getNumLinks() + 6;
|
|
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
|
|
for (int i = 0; i < ndofB ; ++i)
|
|
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
|
|
|
|
}
|
|
|
|
constraintRow.m_friction = combinedFrictionCoeff;
|
|
|
|
|
|
|
|
}
|
|
|
|
/*
|
|
///warm starting (or zero if disabled)
|
|
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
|
{
|
|
constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
|
|
|
if (constraintRow.m_appliedImpulse)
|
|
{
|
|
if (multiBodyA)
|
|
{
|
|
btScalar impulse = constraintRow.m_appliedImpulse;
|
|
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
|
|
multiBodyA->applyDeltaVee(deltaV,impulse);
|
|
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA);
|
|
}
|
|
if (multiBodyB)
|
|
{
|
|
btScalar impulse = constraintRow.m_appliedImpulse;
|
|
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
|
|
multiBodyB->applyDeltaVee(deltaV,impulse);
|
|
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
*/
|
|
{
|
|
constraintRow.m_appliedImpulse = 0.f;
|
|
}
|
|
|
|
constraintRow.m_appliedPushImpulse = 0.f;
|
|
|
|
{
|
|
float desiredVelocity = -0.3;
|
|
|
|
btScalar positionalError = 0.f;
|
|
btScalar velocityError = - rel_vel;// * damping;
|
|
|
|
|
|
btScalar erp = infoGlobal.m_erp2;
|
|
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
|
{
|
|
erp = infoGlobal.m_erp;
|
|
}
|
|
|
|
if (penetration>0)
|
|
{
|
|
positionalError = 0;
|
|
velocityError = -penetration / infoGlobal.m_timeStep;
|
|
} else
|
|
{
|
|
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
|
}
|
|
|
|
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
|
|
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
|
|
|
|
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
|
{
|
|
//combine position and velocity into rhs
|
|
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
|
|
constraintRow.m_rhsPenetration = 0.f;
|
|
|
|
} else
|
|
{
|
|
//split position and velocity into rhs and m_rhsPenetration
|
|
constraintRow.m_rhs = velocityImpulse;
|
|
constraintRow.m_rhsPenetration = penetrationImpulse;
|
|
}
|
|
|
|
|
|
|
|
|
|
constraintRow.m_cfm = 0.f;
|
|
constraintRow.m_lowerLimit = 0;
|
|
constraintRow.m_upperLimit = 1e10f;
|
|
}
|
|
|
|
}
|