b7901c773c
Stop include Bullet headers using `-isystem` for GCC/Clang as it misleads SCons into not properly rebuilding all files when headers change. This means we also need to make sure Bullet builds without warning, and current version fares fairly well, there were just a couple to fix (patch included). Increase minimum version for distro packages to 2.90 (this was never released as the "next" version after 2.89 was 3.05... but that covers it too).
390 lines
12 KiB
C++
390 lines
12 KiB
C++
#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, int type)
|
|
: m_bodyA(bodyA),
|
|
m_bodyB(bodyB),
|
|
m_linkA(linkA),
|
|
m_linkB(linkB),
|
|
m_type(type),
|
|
m_numRows(numRows),
|
|
m_jacSizeA(0),
|
|
m_jacSizeBoth(0),
|
|
m_isUnilateral(isUnilateral),
|
|
m_numDofsFinalized(-1),
|
|
m_maxAppliedImpulse(100)
|
|
{
|
|
}
|
|
|
|
void btMultiBodyConstraint::updateJacobianSizes()
|
|
{
|
|
if (m_bodyA)
|
|
{
|
|
m_jacSizeA = (6 + m_bodyA->getNumDofs());
|
|
}
|
|
|
|
if (m_bodyB)
|
|
{
|
|
m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
|
|
}
|
|
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& constraintNormalAng,
|
|
const btVector3& constraintNormalLin,
|
|
const btVector3& posAworld, const btVector3& posBworld,
|
|
btScalar posError,
|
|
const btContactSolverInfo& infoGlobal,
|
|
btScalar lowerLimit, btScalar upperLimit,
|
|
bool angConstraint,
|
|
btScalar relaxation,
|
|
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip,
|
|
btScalar damping)
|
|
{
|
|
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)
|
|
{
|
|
if (solverConstraint.m_linkA < 0)
|
|
{
|
|
rel_pos1 = posAworld - multiBodyA->getBasePos();
|
|
}
|
|
else
|
|
{
|
|
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
|
|
}
|
|
|
|
const int ndofA = multiBodyA->getNumDofs() + 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];
|
|
//multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
|
|
multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, 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..
|
|
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex], delta, data.scratch_r, data.scratch_v);
|
|
|
|
btVector3 torqueAxis0;
|
|
if (angConstraint)
|
|
{
|
|
torqueAxis0 = constraintNormalAng;
|
|
}
|
|
else
|
|
{
|
|
torqueAxis0 = rel_pos1.cross(constraintNormalLin);
|
|
}
|
|
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
|
solverConstraint.m_contactNormal1 = constraintNormalLin;
|
|
}
|
|
else //if(rb0)
|
|
{
|
|
btVector3 torqueAxis0;
|
|
if (angConstraint)
|
|
{
|
|
torqueAxis0 = constraintNormalAng;
|
|
}
|
|
else
|
|
{
|
|
torqueAxis0 = rel_pos1.cross(constraintNormalLin);
|
|
}
|
|
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
|
|
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
|
solverConstraint.m_contactNormal1 = constraintNormalLin;
|
|
}
|
|
|
|
if (multiBodyB)
|
|
{
|
|
if (solverConstraint.m_linkB < 0)
|
|
{
|
|
rel_pos2 = posBworld - multiBodyB->getBasePos();
|
|
}
|
|
else
|
|
{
|
|
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
|
|
}
|
|
|
|
const int ndofB = multiBodyB->getNumDofs() + 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
|
|
{
|
|
//multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
|
|
multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &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..
|
|
multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex], delta, data.scratch_r, data.scratch_v);
|
|
|
|
btVector3 torqueAxis1;
|
|
if (angConstraint)
|
|
{
|
|
torqueAxis1 = constraintNormalAng;
|
|
}
|
|
else
|
|
{
|
|
torqueAxis1 = rel_pos2.cross(constraintNormalLin);
|
|
}
|
|
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
|
solverConstraint.m_contactNormal2 = -constraintNormalLin;
|
|
}
|
|
else //if(rb1)
|
|
{
|
|
btVector3 torqueAxis1;
|
|
if (angConstraint)
|
|
{
|
|
torqueAxis1 = constraintNormalAng;
|
|
}
|
|
else
|
|
{
|
|
torqueAxis1 = rel_pos2.cross(constraintNormalLin);
|
|
}
|
|
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
|
|
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
|
solverConstraint.m_contactNormal2 = -constraintNormalLin;
|
|
}
|
|
{
|
|
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->getNumDofs() + 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);
|
|
if (angConstraint)
|
|
{
|
|
denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
|
|
}
|
|
else
|
|
{
|
|
denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
|
|
}
|
|
}
|
|
//
|
|
if (multiBodyB)
|
|
{
|
|
const int ndofB = multiBodyB->getNumDofs() + 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);
|
|
if (angConstraint)
|
|
{
|
|
denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
|
|
}
|
|
else
|
|
{
|
|
denom1 = rb1->getInvMass() + constraintNormalLin.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;
|
|
|
|
btScalar rel_vel = 0.f;
|
|
int ndofA = 0;
|
|
int ndofB = 0;
|
|
{
|
|
btVector3 vel1, vel2;
|
|
if (multiBodyA)
|
|
{
|
|
ndofA = multiBodyA->getNumDofs() + 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->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
|
|
rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
|
|
}
|
|
if (multiBodyB)
|
|
{
|
|
ndofB = multiBodyB->getNumDofs() + 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->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
|
|
rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
|
|
}
|
|
|
|
solverConstraint.m_friction = 0.f; //cp.m_combinedFriction;
|
|
}
|
|
|
|
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;
|
|
|
|
//split impulse is not implemented yet for btMultiBody*
|
|
//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;
|
|
|
|
//split impulse is not implemented yet for btMultiBody*
|
|
|
|
// 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;
|
|
}
|