internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = _bodyB;
float mass = b.GetMass();
// Frequency
float omega = 2.0f * Settings.b2_pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * mass * _dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Debug.Assert(d + step.dt * k > Settings.b2_FLT_EPSILON);
_gamma = 1.0f / (step.dt * (d + step.dt * k));
_beta = step.dt * k * _gamma;
// Compute the effective mass matrix.
XForm xf1;
b.GetXForm(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
float invMass = b._invMass;
float invI = b._invI;
Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));
Mat22 K;
Mat22.Add(ref K1, ref K2, out K);
K.col1.X += _gamma;
K.col2.Y += _gamma;
_mass = K.GetInverse();
_C = b._sweep.c + r - _target;
// Cheat with some damping
b._angularVelocity *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b._linearVelocity += invMass * _impulse;
b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
}
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);
}