I am having trouble implementing this restricted angle inequality constraint about an arbitrary axis.
I am imposing a revolute joint constraint separately to this, to form a hinge constraint
The procedure I am following is shown in the code below:
1) Convert the restricted axis to local coords
2) Rotate this axis according to the world orientation of each body once at every frame
3) Project the current difference in angle onto this axis and check if it's within the angle limits
4) If it's within the bounds, skip the constraint
When the constraint limits are reached, the joint locks and no force or impulse can bring it back. I would like to keep following the same process, as I have a good visualization of it.
Code: Select all
AMG3DAngleJoint::AMG3DAngleJoint(AMG3DRigidBody *pRigidBody1, AMG3DRigidBody *pRigidBody2, AMG3DVector4 vAxis,
AMG3DScalar sLowerLimit, AMG3DScalar sUpperLimit)
{
m_pRigidBody1 = pRigidBody1;
m_pRigidBody2 = pRigidBody2;
AMG3DMatrix4x4 Rot1, Rot1T, Rot2, Rot2T;
AMG3DQuaternion q1 = m_pRigidBody1->quatOrientation;
AMG3DQuaternion q2 = m_pRigidBody2->quatOrientation;
q1.toMatrix(&Rot1);
Rot1T.transposeOf(Rot1);
q2.toMatrix(&Rot2);
Rot2T.transposeOf(Rot2);
// Convert arbitrary axis to local coords
localAxis1 = Rot1T*vAxis;
localAxis2 = Rot2T*vAxis;
lowerLimit = sLowerLimit;
upperLimit = sUpperLimit;
}
void AMG3DAngleJoint::preStep(AMG3DScalar dt)
{
if(dt<=0.0f)
return;
AMG3DScalar k_biasFactor = (AMG3DPhysicsWorld::g_AMG3D_PositionCorrection) ? 0.2f : 0.0f;
AMG3DMatrix4x4 Rot1, Rot2;
AMG3DQuaternion q1 = m_pRigidBody1->quatOrientation;
AMG3DQuaternion q2 = m_pRigidBody2->quatOrientation;
q1.toMatrix(&Rot1);
q2.toMatrix(&Rot2);
AMG3DVector4 worldAxis1 = Rot1*localAxis1;
AMG3DVector4 worldAxis2 = Rot2*localAxis2;
// The angle joint derivation
// General equation derived for constraints:
// JM^-1JT = -Jvi - beta/h*C
//
// C = ta - tb - ti = 0 where ta, and tb are the initial orientations of each body and ti is the initial difference in angle
//
// dC/dt = wa - wb
// The Jacobian J = [ 0 1 0 -1 ] by inspection
// JM^-1JT = Ia^-1 + Ib^-1
skipConstraint = false;
AMG3DVector4 ta = m_pRigidBody1->orientation;
AMG3DVector4 tb = m_pRigidBody2->orientation;
AMG3DVector4 currAngleDiff = ta - tb;
AMG3DScalar dtdotaxis = currAngleDiff.dot(worldAxis1);
if(dtdotaxis>=lowerLimit && dtdotaxis<=upperLimit) {
skipConstraint = true;
return;
}
// Compute the bias factor to prevent drift
AMG3DVector4 C = ta - tb - worldAxis1*dtdotaxis;
bias = k_biasFactor / dt * C;
AMG3DMatrix3x3 InvIa = m_pRigidBody1->invIWorld; // Ia^-1
AMG3DMatrix3x3 InvIb = m_pRigidBody2->invIWorld; // Ib^-1
// A = JM^-1JT
A = InvIa + InvIb; // A = Ia^-1 + Ib^-1
InvA.inverseOf(A); // A^-1
}
void AMG3DAngleJoint::applyImpulse()
{
if(skipConstraint)
return;
AMG3DVector4 va = m_pRigidBody1->velocity;
AMG3DVector4 wa = m_pRigidBody1->angularVelocity;
AMG3DVector4 vb = m_pRigidBody2->velocity;
AMG3DVector4 wb = m_pRigidBody2->angularVelocity;
// -Jvi = wb - wa
// b = -Jvi - beta/h*C
AMG3DVector4 b = wb - wa - bias;
// x = lambda (corrective impulse)
lambda = b*InvA;
P += lambda; // Accumulate impulse for warm starting (To Do: Add warm starting)
// vfa = via + Ma^-1 * 0 * lambda = via + 0 = via
// wfa = wia + Ia^-1 * 1 * lambda
// vfb = vib + Mb^-1 * 0 * lambda = vib + 0 = vib
// wfb = wib + Ib^-1 * -1 * lambda
// m_pRigidBody1->velocity += 0;
m_pRigidBody1->angularVelocity += m_pRigidBody1->invIWorld*lambda;
// m_pRigidBody2->velocity -= 0;
m_pRigidBody2->angularVelocity -= m_pRigidBody2->invIWorld*lambda;
}