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);
}
}
}