internal override bool SolvePositionConstraints(SolverData data)
{
var cA = data.Positions[_indexA];
float aA = data.Angles[_indexA];
var cB = data.Positions[_indexB];
float aB = data.Angles[_indexB];
Quaternion2D qA = new(aA), qB = new(aB);
float angularError = 0.0f;
float positionError = 0.0f;
bool fixedRotation = (_invIA + _invIB == 0.0f);
// Solve angular limit constraint
if (EnableLimit && fixedRotation == false)
{
float angle = aB - aA - ReferenceAngle;
float C = 0.0f;
if (Math.Abs(UpperAngle - LowerAngle) < 2.0f * data.AngularSlop)
{
// Prevent large angular corrections
C = Math.Clamp(angle - LowerAngle, -data.MaxAngularCorrection, data.MaxAngularCorrection);
}
else if (angle <= LowerAngle)
{
// Prevent large angular corrections and allow some slop.
C = Math.Clamp(angle - LowerAngle + data.AngularSlop, -data.MaxAngularCorrection, 0.0f);
}
else if (angle >= UpperAngle)
{
// Prevent large angular corrections and allow some slop.
C = Math.Clamp(angle - UpperAngle - data.AngularSlop, 0.0f, data.MaxAngularCorrection);
}
float limitImpulse = -_axialMass * C;
aA -= _invIA * limitImpulse;
aB += _invIB * limitImpulse;
angularError = Math.Abs(C);
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
var rA = Transform.Mul(qA, LocalAnchorA - _localCenterA);
var rB = Transform.Mul(qB, LocalAnchorB - _localCenterB);
var C = cB + rB - cA - rA;
positionError = C.Length;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
var K = new Matrix22(
mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y,
-iA * rA.X * rA.Y - iB * rB.X * rB.Y,
0f,
mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X);
K.EY.X = K.EX.Y;
var impulse = -K.Solve(C);
cA -= impulse * mA;
aA -= iA * Vector2.Cross(rA, impulse);
cB += impulse * mB;
aB += iB * Vector2.Cross(rB, impulse);
}
data.Positions[_indexA] = cA;
data.Angles[_indexA] = aA;
data.Positions[_indexB] = cB;
data.Angles[_indexB] = aB;
return(positionError <= data.LinearSlop && angularError <= data.AngularSlop);
}
internal override void SolveVelocityConstraints(SolverData data)
{
var vA = data.LinearVelocities[_indexA];
float wA = data.AngularVelocities[_indexA];
var vB = data.LinearVelocities[_indexB];
float wB = data.AngularVelocities[_indexB];
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (EnableMotor && !fixedRotation)
{
float Cdot = wB - wA - MotorSpeed;
float impulse = -_axialMass * Cdot;
float oldImpulse = _motorImpulse;
float maxImpulse = data.FrameTime * MaxMotorTorque;
_motorImpulse = Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
if (EnableLimit && fixedRotation == false)
{
// Lower limit
{
float C = _angle - LowerAngle;
float Cdot = wB - wA;
float impulse = -_axialMass * (Cdot + MathF.Max(C, 0.0f) * data.InvDt);
float oldImpulse = _lowerImpulse;
_lowerImpulse = MathF.Max(_lowerImpulse + impulse, 0.0f);
impulse = _lowerImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Upper limit
// Note: signs are flipped to keep C positive when the constraint is satisfied.
// This also keeps the impulse positive when the limit is active.
{
float C = UpperAngle - _angle;
float Cdot = wA - wB;
float impulse = -_axialMass * (Cdot + MathF.Max(C, 0.0f) * data.InvDt);
float oldImpulse = _upperImpulse;
_upperImpulse = MathF.Max(_upperImpulse + impulse, 0.0f);
impulse = _upperImpulse - oldImpulse;
wA += iA * impulse;
wB -= iB * impulse;
}
}
// Solve point-to-point constraint
{
var Cdot = vB + Vector2.Cross(wB, _rB) - vA - Vector2.Cross(wA, _rA);
var impulse = _K.Solve(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
vA -= impulse * mA;
wA -= iA * Vector2.Cross(_rA, impulse);
vB += impulse * mB;
wB += iB * Vector2.Cross(_rB, impulse);
}
data.LinearVelocities[_indexA] = vA;
data.AngularVelocities[_indexA] = wA;
data.LinearVelocities[_indexB] = vB;
data.AngularVelocities[_indexB] = wB;
}