internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.Velocities[_indexA].V;
float wA = data.Velocities[_indexA].W;
Vector2 vB = data.Velocities[_indexB].V;
float wB = data.Velocities[_indexB].W;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = MotorImpulse;
float maxImpulse = data.Step.dt * _maxMotorForce;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float LA = impulse * _a1;
float LB = impulse * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
Vector2 Cdot1 = new Vector2();
Cdot1.X = Vector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA;
Cdot1.Y = wB - wA;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.ez.X, _K.ez.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float LA = df.X * _s1 + df.Y + df.Z * _a1;
float LB = df.X * _s2 + df.Y + df.Z * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float LA = df.X * _s1 + df.Y;
float LB = df.X * _s2 + df.Y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
data.Velocities[_indexB].V = vB;
data.Velocities[_indexB].W = wB;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
float positionError, angularError;
Mat33 K = new Mat33();
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.ez.X = -rA.Y * iA - rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.ez.Y = rA.X * iA + rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
Vector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse = -K.Solve33(C);
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false)
{
float Cdot = wB - wA - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = data.step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = -_mass.Solve33(Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y);
Vector2 reduced = _mass.Solve22(rhs);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
else
{
_impulse += impulse;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y);
Vector2 reduced = _mass.Solve22(rhs);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
else
{
_impulse += impulse;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z);
}
else
{
// Solve point-to-point constraint
Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(_rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(_rB, impulse);
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorForce;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1);
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.Z = Math.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.Z = Math.Min(_impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.Col3.X, _K.Col3.Y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vector2 P = df.X * _perp + df.Z * _axis;
float L1 = df.X * _s1 + df.Y + df.Z * _a1;
float L2 = df.X * _s2 + df.Y + df.Z * _a2;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vector2 P = df.X * _perp;
float L1 = df.X * _s1 + df.Y;
float L2 = df.X * _s2 + df.Y;
v1 -= InvMassA * P;
w1 -= InvIA * L1;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.Positions[_indexA].C;
float aA = data.Positions[_indexA].A;
Vector2 cB = data.Positions[_indexB].C;
float aB = data.Positions[_indexB].A;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
float positionError, angularError;
Mat33 K = new Mat33();
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.ez.X = -rA.Y * iA - rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.ez.Y = rA.X * iA + rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
Vector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse;
if (K.ez.Z > 0.0f)
{
impulse = -K.Solve33(C);
}
else
{
Vector2 impulse2 = -K.Solve22(C1);
impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f);
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
}
data.Positions[_indexA].C = cA;
data.Positions[_indexA].A = aA;
data.Positions[_indexB].C = cB;
data.Positions[_indexB].A = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = Vector2.Zero;
const float w2 = 0;
float m1 = b1.InvMass;
float i1 = b1.InvI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathHelper.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
v1 -= m1 * P;
w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z);
}
else
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
// Solve point-to-point constraint
Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
w1 -= i1 * MathUtils.Cross(r1, impulse);
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 v1 = b1.LinearVelocityInternal;
float w1 = b1.AngularVelocityInternal;
Vector2 v2 = b2.LinearVelocityInternal;
float w2 = b2.AngularVelocityInternal;
float m1 = b1.InvMass, m2 = b2.InvMass;
float i1 = b1.InvI, i2 = b2.InvI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
/*Transform xf1, xf2;
* b1.GetTransform(out xf1);
* b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
// Solve point-to-point constraint
MathUtils.Cross(w2, ref r2, out _tmpVector2);
MathUtils.Cross(w1, ref r1, out _tmpVector1);
Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.X = reduced.X;
impulse.Y = reduced.Y;
impulse.Z = -_impulse.Z;
_impulse.X += reduced.X;
_impulse.Y += reduced.Y;
_impulse.Z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
v1 -= m1 * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
w1 -= i1 * (/* r1 x P */ _tmpFloat1 + impulse.Z);
v2 += m2 * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
w2 += i2 * (/* r2 x P */ _tmpFloat1 + impulse.Z);
}
else
{
/*Transform xf1, xf2;
* b1.GetTransform(out xf1);
* b2.GetTransform(out xf2);*/
_tmpVector1 = LocalAnchorA - b1.LocalCenter;
_tmpVector2 = LocalAnchorB - b2.LocalCenter;
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2);
// Solve point-to-point constraint
MathUtils.Cross(w2, ref r2, out _tmpVector2);
MathUtils.Cross(w1, ref r1, out _tmpVector1);
Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1;
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
w1 -= i1 * /* r1 x impulse */ _tmpFloat1;
v2 += m2 * impulse;
MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
w2 += i2 * /* r2 x impulse */ _tmpFloat1;
}
b1.LinearVelocityInternal = v1;
b1.AngularVelocityInternal = w1;
b2.LinearVelocityInternal = v2;
b2.AngularVelocityInternal = w2;
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorForce;
_motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vector2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
Vector2 Cdot1;
Cdot1.x = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
Cdot1.y = w2 - w1;
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 f1 = _impulse;
Vector3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLowerLimit)
{
_impulse.z = Mathf.Max(_impulse.z, 0.0f);
}
else if (_limitState == LimitState.AtUpperLimit)
{
_impulse.z = Mathf.Min(_impulse.z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
Vector2 b = -Cdot1 - (_impulse.z - f1.z) * new Vector2(_K.Col3.x, _K.Col3.y);
Vector2 f2r = _K.Solve22(b) + new Vector2(f1.x, f1.y);
_impulse.x = f2r.x;
_impulse.y = f2r.y;
df = _impulse - f1;
Vector2 P = df.x * _perp + df.z * _axis;
float L1 = df.x * _s1 + df.y + df.z * _a1;
float L2 = df.x * _s2 + df.y + df.z * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vector2 df = _K.Solve22(-Cdot1);
_impulse.x += df.x;
_impulse.y += df.y;
Vector2 P = df.x * _perp;
float L1 = df.x * _s1 + df.y;
float L2 = df.x * _s2 + df.y;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
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);
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FP positionError, angularError;
Mat33 K = new Mat33();
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
K.ez.x = -rA.y * iA - rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
K.ez.y = rA.x * iA + rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
TSVector2 C1 = cB + rB - cA - rA;
positionError = C1.magnitude;
angularError = 0.0f;
TSVector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
TSVector2 C1 = cB + rB - cA - rA;
FP C2 = aB - aA - ReferenceAngle;
positionError = C1.magnitude;
angularError = FP.Abs(C2);
TSVector C = new TSVector(C1.x, C1.y, C2);
TSVector impulse = K.Solve33(C) * -1;
TSVector2 P = new TSVector2(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.z);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
