internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 c1 = b1._sweep.c;
float a1 = b1._sweep.a;
Vector2 c2 = b2._sweep.c;
float a2 = b2._sweep.a;
// Solve linear limit raint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, _localAnchor1 - _localCenter1);
Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenter2);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.b2_linearSlop, 0.0f, Settings.b2_maxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
angularError = Math.Abs(C1.Y);
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.col1 = new Vector3(k11, k12, k13);
_K.col2 = new Vector3(k12, k22, k23);
_K.col3 = new Vector3(k13, k23, k33);
Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C); // negated above
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k22 = i1 + i2;
_K.col1 = new Vector3(k11, k12, 0.0f);
_K.col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.c = c1;
b1._sweep.a = a1;
b2._sweep.c = c2;
b2._sweep.a = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
}
public static Vector2 MultiplyT(ref Mat22 A, Vector2 v)
{
return(new Vector2(Vector2.Dot(v, A.col1), Vector2.Dot(v, A.col2)));
}
public static Vector2 Multiply(ref Mat22 A, Vector2 v)
{
return(new Vector2(A.col1.X * v.X + A.col2.X * v.Y, A.col1.Y * v.X + A.col2.Y * v.Y));
}
/// Initialize using a position vector and a rotation matrix.
public XForm(Vector2 position, ref Mat22 r)
{
Position = position;
R = r;
}
public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R)
{
R = new Mat22(A.col1 + B.col1, A.col2 + B.col2);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = _bodyA;
Body b2 = _bodyB;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit raint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = b2._sweep.a - b1._sweep.a - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.b2_maxAngularCorrection, Settings.b2_maxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.b2_angularSlop, -Settings.b2_maxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.b2_angularSlop, 0.0f, Settings.b2_maxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1._sweep.a -= b1._invI * limitImpulse;
b2._sweep.a += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point raint.
{
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 C = b2._sweep.c + r2 - b1._sweep.c - r1;
positionError = C.Length();
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
float k_allowedStretch = 10.0f * Settings.b2_linearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C; u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.b2_FLT_EPSILON);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
float k_beta = 0.5f;
b1._sweep.c -= k_beta * invMass1 * impulse2;
b2._sweep.c += k_beta * invMass2 * impulse2;
C = b2._sweep.c + r2 - b1._sweep.c - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y), new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y), new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
Mat22 Ka;
Mat22 K;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1._sweep.c -= b1._invMass * impulse;
b1._sweep.a -= b1._invI * MathUtils.Cross(r1, impulse);
b2._sweep.c += b2._invMass * impulse;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, impulse);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
}