public override void SolveConstraint(float timeStep) { float tau = 0.1f; float damping = 1.0f; Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; Vector3 localNormalInA = new Vector3(); // linear for (int i = 0; i < 3; i++) { if (IsLimited(i)) { Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); if (i == 0) { localNormalInA = new Vector3(1, 0, 0); } else if (i == 1) { localNormalInA = new Vector3(0, 1, 0); } else { localNormalInA = new Vector3(0, 0, 1); } Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); float jacDiagABInv = 1f / _jacLinear[i].Diagonal; //velocity error (first order error) float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, RigidBodyB.LinearVelocity, angvelB); //positional error (zeroth order error) float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); float lo = -1e30f; float hi = 1e30f; //handle the limits if (_lowerLimit[i] < _upperLimit[i]) { if (depth > _upperLimit[i]) { depth -= _upperLimit[i]; lo = 0f; } else { if (depth < _lowerLimit[i]) { depth -= _lowerLimit[i]; hi = 0f; } else { continue; } } } float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; float oldNormalImpulse = _accumulatedImpulse[i]; float sum = oldNormalImpulse + normalImpulse; _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; Vector3 impulse_vector = normalWorld * normalImpulse; RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); } } Vector3 axis; float angle; Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); Quaternion diff = new Quaternion(axis, angle); Matrix diffMat = Matrix.CreateFromQuaternion(diff); Vector3 xyz; // this is not perfect, we can first check which axis are limited, and choose a more appropriate order MatrixToEulerXYZ(diffMat, out xyz); // angular for (int i = 0; i < 3; i++) { if (IsLimited(i + 3)) { Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); float jacDiagABInv = 1f / _jacAng[i].Diagonal; //velocity error (first order error) float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, RigidBodyB.LinearVelocity, angvelB); //positional error (zeroth order error) Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); float lo = -1e30f; float hi = 1e30f; //handle the twist limit if (_lowerLimit[i + 3] < _upperLimit[i + 3]) { //clamp the values float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; float projAngle; if (i == 0) { projAngle = -2f * xyz.Z; } else if (i == 1) { projAngle = -2f * xyz.Y; } else { projAngle = -2f * xyz.Z; } if (projAngle < loLimit) { hi = 0f; rel_pos = loLimit - projAngle; } else { if (projAngle > hiLimit) { lo = 0f; rel_pos = (hiLimit - projAngle); } else { continue; } } } //impulse float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; float oldNormalImpulse = _accumulatedImpulse[i + 3]; float sum = oldNormalImpulse + normalImpulse; _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); Vector3 impulse_vector = axis2 * normalImpulse; RigidBodyA.ApplyTorqueImpulse(impulse_vector); RigidBodyB.ApplyTorqueImpulse(-impulse_vector); } } }