btPoint2PointConstraint makes use of three orthogonal (world) axes to construct 3 Jacobian rows:
Code: Select all
btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
<snip>
btVector3 normal(0,0,0);
for (int i=0;i<3;i++)
{
normal[i] = 1;
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
normal[i] = 0;
}
Code: Select all
void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
<snip>
b2Body* b1 = m_body1;
b2Body* b2 = m_body2;
m_u = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
// Handle singularity.
float32 length = m_u.Length();
if (length > b2_linearSlop)
{
m_u *= 1.0f / length;
}
else
{
m_u.Set(0.0f, 0.0f);
}
<snip>
// Cdot = dot(u, v + cross(w, r))
b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
float32 Cdot = b2Dot(m_u, v2 - v1);
float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
Thanks for any help.