Code:

AMG3DDistanceJoint::AMG3DDistanceJoint(AMG3DRigidBody *pRigidBody1, AMG3DRigidBody *pRigidBody2, AMG3DVector4 vAnchorPoint){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);localAnchor1 =  Rot1T*(vAnchorPoint - m_pRigidBody1->position);localAnchor2 =  Rot2T*(vAnchorPoint - m_pRigidBody2->position);initialDistance = (localAnchor1-localAnchor2).magnitude();}void AMG3DDistanceJoint::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 ra = Rot1*localAnchor1;AMG3DVector4 rb = Rot2*localAnchor2;AMG3DMatrix3x3 I;I.identity();// Compute the bias factor to prevent driftAMG3DVector4 dp = m_pRigidBody1->position + ra - m_pRigidBody2->position - rb; // Relative positionAMG3DScalar C = dp.magnitude() - initialDistance;bias = k_biasFactor / dt * C;dp.normalize();n = dp; // n = dp/|dp|InvMa = I*m_pRigidBody1->invMass; // Ma^-1SkewRa = ra.skew(); // [~ra]AMG3DMatrix3x3 InvIa = m_pRigidBody1->invIWorld; // Ia^-1SkewRaT.transposeOf(SkewRa); // [~ra]TInvMb = I*m_pRigidBody2->invMass; // Mb^-1SkewRb = rb.skew(); // [~rb]AMG3DMatrix3x3 InvIb = m_pRigidBody2->invIWorld; // Ib^-1SkewRbT.transposeOf(SkewRb); // [~rb]T// a = JM^-1JT// a = n.(Ma^-1*n + [~ra]T*Ia^-1*n*[~ra] + Mb^-1*n + [~rb]T*Ib^-1*n*[~rb])a = n.dot(InvMa*n + SkewRaT*InvIa*n*SkewRa + InvMb*n + SkewRbT*InvIb*n*SkewRb);}void AMG3DDistanceJoint::applyImpulse(){AMG3DVector4 va = m_pRigidBody1->velocity;AMG3DVector4 wa = m_pRigidBody1->angularVelocity;AMG3DVector4 vb = m_pRigidBody2->velocity;AMG3DVector4 wb = m_pRigidBody2->angularVelocity;// b = -Jvi - beta/h*CAMG3DScalar b = n.reverseOf().dot(va + SkewRaT*wa - vb - SkewRbT*wb) - bias;// x = lambda (corrective impulse)lambda = a>0 ? b/a : 0;P += lambda; // Accumulate impulse for warm starting (To Do: Add warm starting)// vfa = via + Ma^-1*n*1*lambda// wfa = wia + Ia^-1-n*[~ra]*lambda// vfb = vib + Mb^-1*n*-1*lambda// wfb = wib + Ib^-1*n*-[~rb]*lambdam_pRigidBody1->velocity+= InvMa*n*lambda;m_pRigidBody1->angularVelocity+= m_pRigidBody1->invIWorld*n*SkewRa*lambda;m_pRigidBody2->velocity-= InvMb*n*lambda;m_pRigidBody2->angularVelocity-= m_pRigidBody2->invIWorld*n*SkewRb*lambda;}