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);
}
}
}
public override void SolveConstraint(float timeStep)
{
Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
Vector3 normal = new Vector3(0, 0, 0);
float tau = 0.3f;
float damping = 1f;
//linear part
if (!_angularOnly)
{
for (int i = 0; i < 3; i++)
{
if (i == 0)
{
normal = new Vector3(1, 0, 0);
}
else if (i == 1)
{
normal = new Vector3(0, 1, 0);
}
else
{
normal = new Vector3(0, 0, 1);
}
float jacDiagABInv = 1f / _jac[i].Diagonal;
Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(normal, vel);
//positional error (zeroth order error)
float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal
float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
AppliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);
RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);
}
}
//solve angular part
// get axes in world space
Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);
Vector3 angVelA = RigidBodyA.AngularVelocity;
Vector3 angVelB = RigidBodyB.AngularVelocity;
Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA);
Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB);
Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA;
Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB;
Vector3 velrelOrthog = angAOrthog - angBOrthog;
//solve angular velocity correction
float relaxation = 1f;
float len = velrelOrthog.Length();
if (len > 0.00001f)
{
Vector3 normal2 = Vector3.Normalize(velrelOrthog);
float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
RigidBodyB.ComputeAngularImpulseDenominator(normal2);
// scale for mass and relaxation
velrelOrthog *= (1f / denom) * 0.9f;
}
//solve angular positional correction
Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);
float len2 = angularError.Length();
if (len2 > 0.00001f)
{
Vector3 normal2 = Vector3.Normalize(angularError);
float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
RigidBodyB.ComputeAngularImpulseDenominator(normal2);
angularError *= (1f / denom2) * relaxation;
}
RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError);
RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError);
//apply motor
if (_enableAngularMotor)
{
//todo: add limits too
Vector3 angularLimit = Vector3.Zero;
Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
float projRelVel = Vector3.Dot(velrel, axisA);
float desiredMotorVel = _motorTargetVelocity;
float motorRelvel = desiredMotorVel - projRelVel;
float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) +
RigidBodyB.ComputeAngularImpulseDenominator(axisA);
float unclippedMotorImpulse = (1f / denom3) * motorRelvel;
//todo: should clip against accumulated impulse
float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse;
clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse;
Vector3 motorImp = clippedMotorImpulse * axisA;
RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit);
RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit);
}
}
public override void BuildJacobian()
{
Vector3 localNormalInA = new Vector3(0, 0, 0);
Vector3 pivotInA = _frameInA.Translation;
Vector3 pivotInB = _frameInB.Translation;
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;
//linear part
for (int i = 0; i < 3; i++)
{
if (IsLimited(i))
{
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);
// Create linear atom
_jacLinear[i] = new JacobianEntry(
MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
normalWorld,
RigidBodyA.InvInertiaDiagLocal,
RigidBodyA.InverseMass,
RigidBodyB.InvInertiaDiagLocal,
RigidBodyB.InverseMass);
//optionally disable warmstarting
_accumulatedImpulse[i] = 0f;
// Apply accumulated impulse
Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld;
RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
}
}
// angular part
for (int i = 0; i < 3; i++)
{
if (IsLimited(i + 3))
{
Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB);
// Create angular atom
_jacAng[i] = new JacobianEntry(axis,
MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
RigidBodyA.InvInertiaDiagLocal,
RigidBodyB.InvInertiaDiagLocal);
_accumulatedImpulse[i + 3] = 0f;
// Apply accumulated impulse
Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis;
RigidBodyA.ApplyTorqueImpulse(impulse_vector);
RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
}
}
}