internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
Body b2 = BodyB;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
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.MaxAngularCorrection,
Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = (float)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.AngularSlop, -Settings.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.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b2.Sweep.A += b2.InvI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
/*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);
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.
const float k_allowedStretch = 10.0f * Settings.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.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const 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.Add(ref K1, ref K2, out Ka);
Mat22 K;
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1.Sweep.C -= b1.InvMass * impulse;
MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1;
b2.Sweep.C += b2.InvMass * impulse;
MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Mat22 K1 = new Mat22();
K1.Col1.X = mA + mB;
K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f;
K1.Col2.Y = mA + mB;
Mat22 K2 = new Mat22();
K2.Col1.X = iA * rA.Y * rA.Y;
K2.Col2.X = -iA * rA.X * rA.Y;
K2.Col1.Y = -iA * rA.X * rA.Y;
K2.Col2.Y = iA * rA.X * rA.X;
Mat22 K3 = new Mat22();
K3.Col1.X = iB * rB.Y * rB.Y;
K3.Col2.X = -iB * rB.X * rB.Y;
K3.Col1.Y = -iB * rB.X * rB.Y;
K3.Col2.Y = iB * rB.X * rB.X;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
Mat22 K;
Mat22.Add(ref K12, ref K3, out K);
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
bB.LinearVelocityInternal += mB * P;
bB.AngularVelocityInternal += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
}
internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = 0 - b1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
Settings.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.AngularSlop, -Settings.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.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b1.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
Vector2 C = Vector2.zero + r2 - b1.Sweep.C - r1;
positionError = C.magnitude;
float invMass1 = b1.InvMass;
const float invMass2 = 0;
float invI1 = b1.InvI;
const float invI2 = 0;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C;
u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const float k_beta = 0.5f;
b1.Sweep.C -= k_beta * invMass1 * impulse2;
C = Vector2.zero + 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.Add(ref K1, ref K2, out Ka);
Mat22 K;
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);
b1.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = BodyA;
float mass = b.Mass;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// 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.Epsilon);
_gamma = step.dt * (d + step.dt * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = step.dt * k * _gamma;
// Compute the effective mass matrix.
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
// 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.Inverse;
_C = b.Sweep.C + r - _worldAnchor;
// Cheat with some damping
b.AngularVelocityInternal *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b.LinearVelocityInternal += invMass * _impulse;
b.AngularVelocityInternal += invI * MathUtils.Cross(r, _impulse);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Transform xfA;
bA.GetTransform(out xfA);
// Compute the effective mass Matrix4x4.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA.InvMass;
float iA = bA.InvI;
Mat22 K1 = new Mat22();
K1.Col1.x = mA;
K1.Col2.x = 0.0f;
K1.Col1.y = 0.0f;
K1.Col2.y = mA;
Mat22 K2 = new Mat22();
K2.Col1.x = iA * rA.y * rA.y;
K2.Col2.x = -iA * rA.x * rA.y;
K2.Col1.y = -iA * rA.x * rA.y;
K2.Col2.y = iA * rA.x * rA.x;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
_linearMass = K12.Inverse;
_angularMass = iA;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.zero;
_angularImpulse = 0.0f;
}
}
