Page MenuHome
Differential D1739 Diff 5899 extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

#include "btMultiBodyConstraint.h" #include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.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) btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA), :m_bodyA(bodyA),
m_bodyB(bodyB), m_bodyB(bodyB),
m_linkA(linkA), m_linkA(linkA),
m_linkB(linkB), m_linkB(linkB),
m_num_rows(numRows), m_numRows(numRows),
m_jacSizeA(0),
m_jacSizeBoth(0),
m_isUnilateral(isUnilateral), m_isUnilateral(isUnilateral),
m_numDofsFinalized(-1),
m_maxAppliedImpulse(100) m_maxAppliedImpulse(100)
{ {
m_jac_size_A = (6 + bodyA->getNumLinks());
m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
m_data.resize((2 + m_jac_size_both) * m_num_rows);
}
btMultiBodyConstraint::~btMultiBodyConstraint()
{
} }
void btMultiBodyConstraint::updateJacobianSizes()
btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit)
{ {
if(m_bodyA)
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
if (multiBodyA)
{ {
m_jacSizeA = (6 + m_bodyA->getNumDofs());
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];
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
} }
if (multiBodyB) if(m_bodyB)
{ {
const int ndofB = multiBodyB->getNumLinks() + 6; m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
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);
} }
else
constraintRow.m_jacBindex = data.m_jacobians.size(); m_jacSizeBoth = m_jacSizeA;
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; void btMultiBodyConstraint::allocateJacobiansMultiDof()
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; updateJacobianSizes();
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar 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)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
denom1 += j*l;
}
m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
m_data.resize((2 + m_jacSizeBoth) * m_numRows);
} }
if (multiBodyA && (multiBodyA==multiBodyB)) btMultiBodyConstraint::~btMultiBodyConstraint()
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar 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 = 0.f;
constraintRow.m_appliedImpulse = 0.f;
constraintRow.m_appliedPushImpulse = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse)
{
//combine position and velocity into rhs
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{ {
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
}
constraintRow.m_cfm = 0.f;
constraintRow.m_lowerLimit = lowerLimit;
constraintRow.m_upperLimit = upperLimit;
}
return rel_vel;
} }
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{ {
for (int i = 0; i < ndof; ++i) for (int i = 0; i < ndof; ++i)
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
} }
btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data, btMultiBodyJacobianData& data,
btScalar* jacOrgA, btScalar* jacOrgB,
const btVector3& contactNormalOnB, const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld, const btVector3& posAworld, const btVector3& posBworld,
btScalar position, btScalar posError,
const btContactSolverInfo& infoGlobal, const btContactSolverInfo& infoGlobal,
btScalar& relaxation, btScalar lowerLimit, btScalar upperLimit,
btScalar relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{ {
btVector3 rel_pos1 = posAworld;
btVector3 rel_pos2 = posBworld;
solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB; solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA; solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB; solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
const btVector3& pos1 = posAworld;
const btVector3& pos2 = posBworld;
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->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) if (bodyA)
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
if (bodyB) if (bodyB)
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f;
if (multiBodyA) if (multiBodyA)
{ {
const int ndofA = multiBodyA->getNumLinks() + 6; 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(); solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0) if (solverConstraint.m_deltaVelAindex <0)
{ {
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else } else
{ {
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); 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(); solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA); data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //copy/determine
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); 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]; btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); multiBodyA->fillContactJacobianMultiDof(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]; btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); //determine..
} else multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormalOnB;
}
else //if(rb0)
{ {
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormalOnB; solverConstraint.m_contactNormal1 = contactNormalOnB;
} }
if (multiBodyB) if (multiBodyB)
{ {
const int ndofB = multiBodyB->getNumLinks() + 6; 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(); solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0) if (solverConstraint.m_deltaVelBindex <0)
{ {
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); 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(); solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB); 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, -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); data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); 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);
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
} else solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
else //if(rb1)
{ {
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB; solverConstraint.m_contactNormal2 = -contactNormalOnB;
} }
{ {
btVector3 vec; btVector3 vec;
btScalar denom0 = 0.f; btScalar denom0 = 0.f;
btScalar denom1 = 0.f; btScalar denom1 = 0.f;
btScalar* jacB = 0; btScalar* jacB = 0;
btScalar* jacA = 0; btScalar* jacA = 0;
btScalar* lambdaA =0; btScalar* deltaVelA = 0;
btScalar* lambdaB =0; btScalar* deltaVelB = 0;
int ndofA = 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) if (multiBodyA)
{ {
ndofA = multiBodyA->getNumLinks() + 6; ndofA = multiBodyA->getNumDofs() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i) for (int i = 0; i < ndofA; ++i)
{ {
btScalar j = jacA[i] ; btScalar j = jacA[i] ;
btScalar l =lambdaA[i]; btScalar l = deltaVelA[i];
denom0 += j*l; denom0 += j*l;
} }
} else }
{ else if(rb0)
if (rb0)
{ {
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
} }
} //
if (multiBodyB) if (multiBodyB)
{ {
const int ndofB = multiBodyB->getNumLinks() + 6; const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i) for (int i = 0; i < ndofB; ++i)
{ {
btScalar j = jacB[i] ; btScalar j = jacB[i] ;
btScalar l =lambdaB[i]; btScalar l = deltaVelB[i];
denom1 += j*l; denom1 += j*l;
} }
} else }
{ else if(rb1)
if (rb1)
{ {
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
} }
}
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];
}
}
//
btScalar d = denom0+denom1; btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON) if (d>SIMD_EPSILON)
{ {
solverConstraint.m_jacDiagABInv = relaxation/(d); solverConstraint.m_jacDiagABInv = relaxation/(d);
} else }
else
{ {
solverConstraint.m_jacDiagABInv = 1.f; //disable the constraint row to handle singularity/redundant constraint
solverConstraint.m_jacDiagABInv = 0.f;
} }
} }
//compute rhs and remaining solverConstraint fields //compute rhs and remaining solverConstraint fields
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f; btScalar rel_vel = 0.f;
int ndofA = 0; int ndofA = 0;
int ndofB = 0; int ndofB = 0;
{ {
btVector3 vel1,vel2; btVector3 vel1,vel2;
if (multiBodyA) if (multiBodyA)
{ {
ndofA = multiBodyA->getNumLinks() + 6; ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i) for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
} else }
{ else if(rb0)
if (rb0)
{ {
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
} }
}
if (multiBodyB) if (multiBodyB)
{ {
ndofB = multiBodyB->getNumLinks() + 6; ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i) for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
} else }
{ else if(rb1)
if (rb1)
{ {
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
} }
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
} }
///warm starting (or zero if disabled) ///warm starting (or zero if disabled)
/* /*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{ {
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
Show All 20 Lines if (solverConstraint.m_appliedImpulse)
} else } else
{ {
if (rb1) if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
} }
} }
} else } else
*/ */
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f;
{ {
btScalar positionalError = 0.f; btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping; btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2; btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{ {
erp = infoGlobal.m_erp; erp = infoGlobal.m_erp;
} }
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep; positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{ {
//combine position and velocity into rhs //combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_rhsPenetration = 0.f;
} else } else
{ {
//split position and velocity into rhs and m_rhsPenetration //split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse;
} }
solverConstraint.m_cfm = 0.f; solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; solverConstraint.m_lowerLimit = lowerLimit;
solverConstraint.m_upperLimit = m_maxAppliedImpulse; solverConstraint.m_upperLimit = upperLimit;
} }
return rel_vel;
} }