internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Vector2 vA = bA.LinearVelocityInternal;
float wA = bA.AngularVelocityInternal;
Vector2 vB = bB.LinearVelocityInternal;
float wB = bB.AngularVelocityInternal;
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// Solve point-to-point constraint
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);
_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);
bA.LinearVelocityInternal = vA;
bA.AngularVelocityInternal = wA;
bB.LinearVelocityInternal = vB;
bB.AngularVelocityInternal = 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;
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
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(ref rA, ref P);
cB += mB * P;
aB += iB * MathUtils.Cross(ref rB, ref 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)
{
Vector2 impulse2 = -K.Solve22(C1);
impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f);
}
else
{
impulse = -K.Solve33(C);
}
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(ref rA, ref P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(ref rB, ref 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(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vec2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
float impulse = _motorMass * (_motorSpeed - Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorForce;
_motorImpulse = Box2DNet.Common.MathB2.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
Vec2 P = impulse * _axis;
float L1 = impulse * _a1;
float L2 = impulse * _a2;
v1 -= _invMass1 * P;
w1 -= _invI1 * L1;
v2 += _invMass2 * P;
w2 += _invI2 * L2;
}
Vec2 Cdot1;
Cdot1.X = Vec2.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.
var Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 f1 = _impulse;
Vec3 df = _K.Solve33(-Cdot);
_impulse += df;
if (_limitState == LimitState.AtLowerLimit)
{
_impulse.Z = Box2DNet.Common.MathB2.Max(_impulse.Z, 0.0f);
}
else if (_limitState == LimitState.AtUpperLimit)
{
_impulse.Z = Box2DNet.Common.MathB2.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)
Vec2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vec2(_K.Col3.X, _K.Col3.Y);
Vec2 f2r = _K.Solve22(b) + new Vec2(f1.X, f1.Y);
_impulse.X = f2r.X;
_impulse.Y = f2r.Y;
df = _impulse - f1;
Vec2 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.
Vec2 df = _K.Solve22(-Cdot1);
_impulse.X += df.X;
_impulse.Y += df.Y;
Vec2 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 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 = MathUtils.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(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
//Solve motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorTorque;
_motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
//Solve limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.EqualLimits)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLowerLimit)
{
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.AtUpperLimit)
{
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 = impulse.ToVector2();
v1 -= m1 * P;
w1 -= i1 * (r1.Cross(P) + impulse.z);
v2 += m2 * P;
w2 += i2 * (r2.Cross(P) + impulse.z);
}
else
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.x += impulse.x;
_impulse.y += impulse.y;
v1 -= m1 * impulse;
w1 -= i1 * r1.Cross(impulse);
v2 += m2 * impulse;
w2 += i2 * r2.Cross(impulse);
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = 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;
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = Complex.Multiply(ref _localXAxis, ref qA);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(ref rB, ref axis);
Vector2 perp = Complex.Multiply(ref _localYAxisA, ref qA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(ref rB, ref perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (_enableLimit)
{
float translation = Vector2.Dot(axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * perp + impulse.Z * axis;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
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 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 bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.Positions[_indexA].C;
GGame.Math.Fix64 aA = data.Positions[_indexA].A;
Vector2 cB = data.Positions[_indexB].C;
GGame.Math.Fix64 aB = data.Positions[_indexB].A;
Rot qA = new Rot(aA), qB = new Rot(aB);
GGame.Math.Fix64 mA = _invMassA, mB = _invMassB;
GGame.Math.Fix64 iA = _invIA, iB = _invIB;
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
GGame.Math.Fix64 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;
GGame.Math.Fix64 C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = GGame.Math.Fix64.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 bool SolvePositionConstraints(ref SolverData data)
{
FVector2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
FVector2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA);
FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_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 (m_frequencyHz > 0.0f)
{
FVector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
FVector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
FVector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
FVector3 C = new FVector3(C1.X, C1.Y, C2);
FVector3 impulse = -K.Solve33(C);
FVector2 P = new FVector2(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[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false)
{
FP Cdot = wB - wA - _motorSpeed;
FP impulse = _motorMass * (-Cdot);
FP oldImpulse = _motorImpulse;
FP 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)
{
FPVector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
FP Cdot2 = wB - wA;
FPVector Cdot = new FPVector(Cdot1.x, Cdot1.y, Cdot2);
FPVector impulse = _mass.Solve33(Cdot) * -1;
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
FP newImpulse = _impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
FPVector2 rhs = -Cdot1 + _impulse.z * new FPVector2(_mass.ez.x, _mass.ez.y);
FPVector2 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)
{
FP newImpulse = _impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
FPVector2 rhs = -Cdot1 + _impulse.z * new FPVector2(_mass.ez.x, _mass.ez.y);
FPVector2 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;
}
}
FPVector2 P = new FPVector2(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
FPVector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA);
FPVector2 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 bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.Positions[_indexA].C;
GGame.Math.Fix64 aA = data.Positions[_indexA].A;
Vector2 cB = data.Positions[_indexB].C;
GGame.Math.Fix64 aB = data.Positions[_indexB].A;
Rot qA = new Rot(aA), qB = new Rot(aB);
GGame.Math.Fix64 mA = _invMassA, mB = _invMassB;
GGame.Math.Fix64 iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
GGame.Math.Fix64 a1 = MathUtils.Cross(d + rA, axis);
GGame.Math.Fix64 a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
GGame.Math.Fix64 s1 = MathUtils.Cross(d + rA, perp);
GGame.Math.Fix64 s2 = MathUtils.Cross(rB, perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
GGame.Math.Fix64 linearError = GGame.Math.Fix64.Abs(C1.X);
GGame.Math.Fix64 angularError = GGame.Math.Fix64.Abs(C1.Y);
bool active = false;
GGame.Math.Fix64 C2 = 0.0f;
if (_enableLimit)
{
GGame.Math.Fix64 translation = Vector2.Dot(axis, d);
if (GGame.Math.Fix64.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Max((float)linearError, (float)GGame.Math.Fix64.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = Math.Max((float)linearError, (float)(_lowerTranslation - translation));
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = Math.Max((float)linearError, (float)(translation - _upperTranslation));
active = true;
}
}
if (active)
{
GGame.Math.Fix64 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
GGame.Math.Fix64 k12 = iA * s1 + iB * s2;
GGame.Math.Fix64 k13 = iA * s1 * a1 + iB * s2 * a2;
GGame.Math.Fix64 k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
GGame.Math.Fix64 k23 = iA * a1 + iB * a2;
GGame.Math.Fix64 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
GGame.Math.Fix64 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
GGame.Math.Fix64 k12 = iA * s1 + iB * s2;
GGame.Math.Fix64 k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * perp + impulse.Z * axis;
GGame.Math.Fix64 LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
GGame.Math.Fix64 LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.Positions[_indexA].C = cA;
data.Positions[_indexA].A = aA;
data.Positions[_indexB].C = cB;
data.Positions[_indexB].A = aB;
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void SolveVelocityConstraints()
{
Vector2 vA = _bodyA.velocity;
float wA = _bodyA.angularVelocity * Mathf.Deg2Rad;
Vector2 vB = _bodyB.velocity;
float wB = _bodyB.angularVelocity * Mathf.Deg2Rad;
float mA = m_InvMassA, mB = m_InvMassB;
float iA = m_InvIA, iB = m_InvIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (m_EnableMotor && limitState != JointLimitState2D.EqualLimits && fixedRotation == false)
{
float Cdot = wB - wA - m_MotorSpeed;
float impulse = m_MotorMass * (-Cdot);
float oldImpulse = m_MotorImpulse;
float maxImpulse = Time.deltaTime * m_MaxMotorTorque;
m_MotorImpulse = Mathf.Clamp(m_MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_MotorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (m_EnableLimit && limitState != JointLimitState2D.Inactive && fixedRotation == false)
{
Vector2 Cdot1 = vB + MathUtils.Cross(wB, m_RB) - vA - MathUtils.Cross(wA, m_RA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 impulse = -m_Mass.Solve33(Cdot);
if (limitState == JointLimitState2D.EqualLimits)
{
m_Impulse += impulse;
}
else if (limitState == JointLimitState2D.LowerLimit)
{
float newImpulse = m_Impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
Vector2 rhs = -Cdot1 + m_Impulse.z * new Vector2(m_Mass.ez.x, m_Mass.ez.y);
Vector2 reduced = m_Mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_Impulse.z;
m_Impulse.x += reduced.x;
m_Impulse.y += reduced.y;
m_Impulse.z = 0.0f;
}
else
{
m_Impulse += impulse;
}
}
else if (limitState == JointLimitState2D.UpperLimit)
{
float newImpulse = m_Impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
Vector2 rhs = -Cdot1 + m_Impulse.z * new Vector2(m_Mass.ez.x, m_Mass.ez.y);
Vector2 reduced = m_Mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_Impulse.z;
m_Impulse.x += reduced.x;
m_Impulse.y += reduced.y;
m_Impulse.z = 0.0f;
}
else
{
m_Impulse += impulse;
}
}
Vector2 P = new Vector2(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(m_RA, P) + impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(m_RB, P) + impulse.z);
}
else
{
// Solve point-to-point constraint
Vector2 Cdot = vB + MathUtils.Cross(wB, m_RB) - vA - MathUtils.Cross(wA, m_RA);
Vector2 impulse = m_Mass.Solve22(-Cdot);
m_Impulse.x += impulse.x;
m_Impulse.y += impulse.y;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(m_RA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(m_RB, impulse);
}
if (!_bodyA.isKinematic)
{
_bodyA.velocity = vA;
if (!_bodyA.fixedAngle)
{
_bodyA.angularVelocity = wA * Mathf.Rad2Deg;
}
}
if (!_bodyB.isKinematic)
{
_bodyB.velocity = vB;
if (!_bodyB.fixedAngle)
{
_bodyB.angularVelocity = wB * Mathf.Rad2Deg;
}
}
}
internal override bool SolvePositionConstraints()
{
Vector2 cA = _bodyA.GetRelativePoint(m_LocalCenterA);
Vector2 cA0 = cA;
float aA = _bodyA.rotation * Mathf.Deg2Rad;
Vector2 cB = _bodyB.GetRelativePoint(m_LocalCenterB);
Vector2 cB0 = cB;
float aB = _bodyB.rotation * Mathf.Deg2Rad;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = m_InvMassA, mB = m_InvMassB;
float iA = m_InvIA, iB = m_InvIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA);
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - m_LocalCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, localXAxisA);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, m_LocalYAxisA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.x = Vector2.Dot(perp, d);
C1.y = aB - aA - referenceAngle;
float linearError = Mathf.Abs(C1.x);
float angularError = Mathf.Abs(C1.y);
bool active = false;
float C2 = 0.0f;
if (m_EnableLimit)
{
float translation = Vector2.Dot(axis, d);
m_Translation = translation;
if (Mathf.Abs(m_UpperTranslation - m_LowerTranslation) < 2.0f * Constants.linearSlop)
{
// Prevent large angular corrections
C2 = Mathf.Clamp(translation, -Constants.maxLinearCorrection, Constants.maxLinearCorrection);
linearError = Mathf.Max(linearError, Mathf.Abs(translation));
active = true;
}
else if (translation <= m_LowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Mathf.Clamp(translation - m_LowerTranslation + Constants.linearSlop,
-Constants.maxLinearCorrection, 0.0f);
linearError = Mathf.Max(linearError, m_LowerTranslation - translation);
active = true;
}
else if (translation >= m_UpperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Mathf.Clamp(translation - m_UpperTranslation - Constants.linearSlop, 0.0f,
Constants.maxLinearCorrection);
linearError = Mathf.Max(linearError, translation - m_UpperTranslation);
active = true;
}
}
if (active)
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
Vector2 P = impulse.x * perp + impulse.z * axis;
float LA = impulse.x * s1 + impulse.y + impulse.z * a1;
float LB = impulse.x * s2 + impulse.y + impulse.z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
if (!_bodyA.isKinematic)
{
var dcA = cA0 - cA;
_bodyA.position -= dcA;
if (!_bodyA.fixedAngle)
{
_bodyA.rotation = aA * Mathf.Rad2Deg;
}
}
if (!_bodyB.isKinematic)
{
var dcB = cB0 - cB;
_bodyB.position -= dcB;
if (!_bodyB.fixedAngle)
{
_bodyB.rotation = aB * Mathf.Rad2Deg;
}
}
return(linearError < Constants.linearSlop && angularError < Constants.angularSlop);
}
internal override void SolveVelocityConstraints()
{
Vector2 vA = _bodyA.velocity;
float wA = _bodyA.angularVelocity * Mathf.Deg2Rad;
Vector2 vB = _bodyB.velocity;
float wB = _bodyB.angularVelocity * Mathf.Deg2Rad;
float mA = m_InvMassA, mB = m_InvMassB;
float iA = m_InvIA, iB = m_InvIB;
// Solve linear motor constraint.
if (m_EnableMotor && m_LimitState != JointLimitState2D.EqualLimits)
{
float Cdot = Vector2.Dot(m_Axis, vB - vA) + m_A2 * wB - m_A1 * wA;
float impulse = m_MotorMass * (m_MotorSpeed - Cdot);
float oldImpulse = m_MotorImpulse;
float maxImpulse = Time.deltaTime * m_MaxMotorForce;
m_MotorImpulse = Mathf.Clamp(m_MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_MotorImpulse - oldImpulse;
Vector2 P = impulse * m_Axis;
float LA = impulse * m_A1;
float LB = impulse * m_A2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
Vector2 Cdot1 = new Vector2();
Cdot1.x = Vector2.Dot(m_Perp, vB - vA) + m_S2 * wB - m_S1 * wA;
Cdot1.y = wB - wA;
if (m_EnableLimit && m_LimitState != JointLimitState2D.Inactive)
{
// Solve slider and limit constraint in block form.
float Cdot2;
Cdot2 = Vector2.Dot(m_Axis, vB - vA) + m_A2 * wB - m_A1 * wA;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 f1 = m_Impulse;
Vector3 df = m_K.Solve33(-Cdot);
m_Impulse += df;
if (m_LimitState == JointLimitState2D.LowerLimit)
{
m_Impulse.z = Mathf.Max(m_Impulse.z, 0.0f);
}
else if (m_LimitState == JointLimitState2D.UpperLimit)
{
m_Impulse.z = Mathf.Min(m_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 - (m_Impulse.z - f1.z) * new Vector2(m_K.ez.x, m_K.ez.y);
Vector2 f2r = m_K.Solve22(b) + new Vector2(f1.x, f1.y);
m_Impulse.x = f2r.x;
m_Impulse.y = f2r.y;
df = m_Impulse - f1;
Vector2 P = df.x * m_Perp + df.z * m_Axis;
float LA = df.x * m_S1 + df.y + df.z * m_A1;
float LB = df.x * m_S2 + df.y + df.z * m_A2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the slider constraint in block form.
Vector2 df = m_K.Solve22(-Cdot1);
m_Impulse.x += df.x;
m_Impulse.y += df.y;
Vector2 P = df.x * m_Perp;
float LA = df.x * m_S1 + df.y;
float LB = df.x * m_S2 + df.y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
//FVector2 Cdot10 = Cdot1;
Cdot1.x = Vector2.Dot(m_Perp, vB - vA) + m_S2 * wB - m_S1 * wA;
Cdot1.y = wB - wA;
if (Mathf.Abs(Cdot1.x) > 0.01f || Mathf.Abs(Cdot1.y) > 0.01f)
{
//FVector2 test = MathUtils.Mul22(m_K, df);
Cdot1.x += 0.0f;
}
}
if (!_bodyA.isKinematic)
{
_bodyA.velocity = vA;
if (!_bodyA.fixedAngle)
{
_bodyA.angularVelocity = wA * Mathf.Rad2Deg;
}
}
if (!_bodyB.isKinematic)
{
_bodyB.velocity = vB;
if (!_bodyB.fixedAngle)
{
_bodyB.angularVelocity = wB * Mathf.Rad2Deg;
}
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vec2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
//Solve motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorTorque;
_motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
//Solve limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vec2 Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
float Cdot2 = w2 - w1;
Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.EqualLimits)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
Vec2 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.AtUpperLimit)
{
float newImpulse = _impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vec2 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;
}
}
Vec2 P = new Vec2(impulse.X, impulse.Y);
v1 -= m1 * P;
w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z);
v2 += m2 * P;
w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z);
}
else
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vec2 Cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
Vec2 impulse = _mass.Solve22(-Cdot);
_impulse.X += impulse.X;
_impulse.Y += impulse.Y;
v1 -= m1 * impulse;
w1 -= i1 * Vec2.Cross(r1, impulse);
v2 += m2 * impulse;
w2 += i2 * Vec2.Cross(r2, impulse);
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
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 bool SolvePositionConstraints()
{
Vector2 cA = _bodyA.GetRelativePoint(m_LocalCenterA);
Vector2 cA0 = cA;
float aA = _bodyA.rotation * Mathf.Deg2Rad;
Vector2 cB = _bodyB.GetRelativePoint(m_LocalCenterB);
Vector2 cB0 = cB;
float aB = _bodyB.rotation * Mathf.Deg2Rad;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = m_InvMassA, mB = m_InvMassB;
float iA = m_InvIA, iB = m_InvIB;
Vector2 rA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA);
Vector2 rB = MathUtils.Mul(qB, _localAnchorB - m_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 (frequency > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.magnitude;
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.magnitude;
angularError = Mathf.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);
}
if (!_bodyA.isKinematic)
{
var dcA = cA0 - cA;
_bodyA.position -= dcA;
if (!_bodyA.fixedAngle)
{
_bodyA.rotation = aA * Mathf.Rad2Deg;
}
}
if (!_bodyB.isKinematic)
{
var dcB = cB0 - cB;
_bodyB.position -= dcB;
if (!_bodyB.fixedAngle)
{
_bodyB.rotation = aB * Mathf.Rad2Deg;
}
}
return(positionError <= Constants.linearSlop && angularError <= Constants.angularSlop);
}
/// <summary>
/// Solves the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = Body1;
Body b2 = Body2;
Vec2 v1 = b1.LinearVelocity;
float w1 = b1.AngularVelocity;
Vec2 v2 = b2.LinearVelocity;
float w2 = b2.AngularVelocity;
// Solve linear motor constraint.
if (IsMotorEnabled && LimitState != LimitState.EqualLimits)
{
float cdot = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1;
float impulse = MotorMass * (motorSpeedx - cdot);
float oldImpulse = MotorForce;
float maxImpulse = step.Dt * MaxMotorForce;
MotorForce = Box2DXMath.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse);
impulse = MotorForce - oldImpulse;
Vec2 p = impulse * Axis;
float l1 = impulse * a1;
float l2 = impulse * A2;
v1 -= InvMass1 * p;
w1 -= InvI1 * l1;
v2 += InvMass2 * p;
w2 += InvI2 * l2;
}
Vec2 cdot1;
cdot1.X = Vec2.Dot(Perp, v2 - v1) + s2 * w2 - s1 * w1;
cdot1.Y = w2 - w1;
if (IsLimitEnabled && LimitState != LimitState.InactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float cdot2;
cdot2 = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1;
Vec3 cdot = new Vec3(cdot1.X, cdot1.Y, cdot2);
Vec3 f1 = Impulse;
Vec3 df = K.Solve33(-cdot);
Impulse += df;
if (LimitState == LimitState.AtLowerLimit)
{
Impulse.Z = Box2DXMath.Max(Impulse.Z, 0.0f);
}
else if (LimitState == LimitState.AtUpperLimit)
{
Impulse.Z = Box2DXMath.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)
Vec2 b = -cdot1 - (Impulse.Z - f1.Z) * new Vec2(K.Col3.X, K.Col3.Y);
Vec2 f2R = K.Solve22(b) + new Vec2(f1.X, f1.Y);
Impulse.X = f2R.X;
Impulse.Y = f2R.Y;
df = Impulse - f1;
Vec2 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.
Vec2 df = K.Solve22(-cdot1);
Impulse.X += df.X;
Impulse.Y += df.Y;
Vec2 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)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 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;
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 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)
{
FPVector2 C1 = cB + rB - cA - rA;
positionError = C1.magnitude;
angularError = 0.0f;
FPVector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
FPVector2 C1 = cB + rB - cA - rA;
FP C2 = aB - aA - ReferenceAngle;
positionError = C1.magnitude;
angularError = FP.Abs(C2);
FPVector C = new FPVector(C1.x, C1.y, C2);
FPVector impulse = K.Solve33(C) * -1;
FPVector2 P = new FPVector2(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;
// 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;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (_enableLimit)
{
float translation = Vector2.Dot(axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * perp + impulse.Z * axis;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b2 = BodyB;
Vector2 v1 = Vector2.Zero;
float w1 = 0.0f;
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 L2 = df.X * _s2 + df.Y + df.Z * _a2;
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 L2 = df.X * _s2 + df.Y;
v2 += InvMassB * P;
w2 += InvIB * L2;
}
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;
}
