internal override bool SolvePositionConstraints(ref SolverData data)
{
TSVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
TSVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// Get the pulley axes.
TSVector2 uA = cA + rA - WorldAnchorA;
TSVector2 uB = cB + rB - WorldAnchorB;
FP lengthA = uA.magnitude;
FP lengthB = uB.magnitude;
if (lengthA > 10.0f * Settings.LinearSlop)
{
uA *= 1.0f / lengthA;
}
else
{
uA = TSVector2.zero;
}
if (lengthB > 10.0f * Settings.LinearSlop)
{
uB *= 1.0f / lengthB;
}
else
{
uB = TSVector2.zero;
}
// Compute effective mass.
FP ruA = MathUtils.Cross(rA, uA);
FP ruB = MathUtils.Cross(rB, uB);
FP mA = _invMassA + _invIA * ruA * ruA;
FP mB = _invMassB + _invIB * ruB * ruB;
FP mass = mA + Ratio * Ratio * mB;
if (mass > 0.0f)
{
mass = 1.0f / mass;
}
FP C = Constant - lengthA - Ratio * lengthB;
FP linearError = FP.Abs(C);
FP impulse = -mass * C;
TSVector2 PA = -impulse * uA;
TSVector2 PB = -Ratio * impulse * uB;
cA += _invMassA * PA;
aA += _invIA * MathUtils.Cross(rA, PA);
cB += _invMassB * PB;
aB += _invIB * MathUtils.Cross(rB, PB);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(linearError < Settings.LinearSlop);
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
TSVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
TSVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
TSVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
TSVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// Get the pulley axes.
_uA = cA + _rA - WorldAnchorA;
_uB = cB + _rB - WorldAnchorB;
FP lengthA = _uA.magnitude;
FP lengthB = _uB.magnitude;
if (lengthA > 10.0f * Settings.LinearSlop)
{
_uA *= 1.0f / lengthA;
}
else
{
_uA = TSVector2.zero;
}
if (lengthB > 10.0f * Settings.LinearSlop)
{
_uB *= 1.0f / lengthB;
}
else
{
_uB = TSVector2.zero;
}
// Compute effective mass.
FP ruA = MathUtils.Cross(_rA, _uA);
FP ruB = MathUtils.Cross(_rB, _uB);
FP mA = _invMassA + _invIA * ruA * ruA;
FP mB = _invMassB + _invIB * ruB * ruB;
_mass = mA + Ratio * Ratio * mB;
if (_mass > 0.0f)
{
_mass = 1.0f / _mass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support variable time steps.
_impulse *= data.step.dtRatio;
// Warm starting.
TSVector2 PA = -(_impulse) * _uA;
TSVector2 PB = (-Ratio * _impulse) * _uB;
vA += _invMassA * PA;
wA += _invIA * MathUtils.Cross(_rA, PA);
vB += _invMassB * PB;
wB += _invIB * MathUtils.Cross(_rB, PB);
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}