public Mat22 GetInverse()
{
float a = ex.X, b = ey.X, c = ex.Y, d = ey.Y;
Mat22 B = new Mat22();
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
B.ex.X = det * d; B.ey.X = -det * b;
B.ex.Y = -det * c; B.ey.Y = det * a;
return B;
}
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 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.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 constraint.
{
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(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.magnitude;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.b2_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.b2_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 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);
}
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_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, _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);
}
// A * B
public static Mat22 Mul(Mat22 A, Mat22 B)
{
return new Mat22(Mul(A, B.ex), Mul(A, B.ey));
}
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 constraint.
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 - _localCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenterB);
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);
Vector2 impulse;
float C1;
C1 = Vector2.Dot(_perp, d);
linearError = Math.Max(linearError, Math.Abs(C1));
angularError = 0.0f;
if (active)
{
float m1 = _invMassA, m2 = _invMassB;
float i1 = _invIA, i2 = _invIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.col1 = new Vector2(k11, k12);
_K.col2 = new Vector2(k12, k22);
Vector2 C = new Vector2(-C1, -C2);
impulse = _K.Solve(C); //note i inverted above
}
else
{
float m1 = _invMassA, m2 = _invMassB;
float i1 = _invIA, i2 = _invIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1;
if (k11 != 0.0f)
{
impulse1 = -C1 / k11;
}
else
{
impulse1 = 0.0f;
}
impulse.x = impulse1;
impulse.y = 0.0f;
}
Vector2 P = impulse.x * _perp + impulse.y * _axis;
float L1 = impulse.x * _s1 + impulse.y * _a1;
float L2 = impulse.x * _s2 + impulse.y * _a2;
c1 -= _invMassA * P;
a1 -= _invIA * L1;
c2 += _invMassB * P;
a2 += _invIB * 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);
}
/// Multiply a matrix times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another.
public static Vec2 Mul(Mat22 A, Vec2 v)
{
return new Vec2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y);
}
/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another (inverse transform).
public static Vec2 MulT(Mat22 A, Vec2 v)
{
return new Vec2(Dot(v, A.ex), Dot(v, A.ey));
}
public static Mat22 Abs(Mat22 A)
{
return new Mat22(Abs(A.ex), Abs(A.ey));
}
// A^T * B
public static Mat22 MulT(Mat22 A, Mat22 B)
{
Vec2 c1 = new Vec2(Dot(A.ex, B.ex), Dot(A.ey, B.ex));
Vec2 c2 = new Vec2(Dot(A.ex, B.ey), Dot(A.ey, B.ey));
return new Mat22(c1, c2);
}
internal override bool SolvePositionConstraints(SolverData data) {
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 d = cB + rB - cA - rA;
Vec2 axis = Utilities.Mul(qA, m_localXAxisA);
float a1 = Utilities.Cross(d + rA, axis);
float a2 = Utilities.Cross(rB, axis);
Vec2 perp = Utilities.Mul(qA, m_localYAxisA);
float s1 = Utilities.Cross(d + rA, perp);
float s2 = Utilities.Cross(rB, perp);
Vec3 impulse;
Vec2 C1;
C1.X = Utilities.Dot(perp, d);
C1.Y = aB - aA - m_referenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (m_enableLimit)
{
float translation = Utilities.Dot(axis, d);
if (Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f *Settings._linearSlop)
{
// Prevent large angular corrections
C2 = Utilities.Clamp(translation, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Utilities.Clamp(translation - m_lowerTranslation + Settings._linearSlop, -Settings._maxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, m_lowerTranslation - translation);
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Utilities.Clamp(translation - m_upperTranslation - Settings._linearSlop, 0.0f, Settings._maxLinearCorrection);
linearError = Math.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.Set(k11, k12, k13);
K.ey.Set(k12, k22, k23);
K.ez.Set(k13, k23, k33);
Vec3 C = new Vec3();
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.Set(k11, k12);
K.ey.Set(k12, k22);
Vec2 impulse1 = K.Solve(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vec2 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[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return linearError <= Settings._linearSlop && angularError <= Settings._angularSlop;
}
internal override bool SolvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 d = cB + rB - cA - rA;
Vec2 axis = Utilities.Mul(qA, m_localXAxisA);
float a1 = Utilities.Cross(d + rA, axis);
float a2 = Utilities.Cross(rB, axis);
Vec2 perp = Utilities.Mul(qA, m_localYAxisA);
float s1 = Utilities.Cross(d + rA, perp);
float s2 = Utilities.Cross(rB, perp);
Vec3 impulse;
Vec2 C1;
C1.X = Utilities.Dot(perp, d);
C1.Y = aB - aA - m_referenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (m_enableLimit)
{
float translation = Utilities.Dot(axis, d);
if (Math.Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings._linearSlop)
{
// Prevent large angular corrections
C2 = Utilities.Clamp(translation, -Settings._maxLinearCorrection, Settings._maxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Utilities.Clamp(translation - m_lowerTranslation + Settings._linearSlop, -Settings._maxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, m_lowerTranslation - translation);
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Utilities.Clamp(translation - m_upperTranslation - Settings._linearSlop, 0.0f, Settings._maxLinearCorrection);
linearError = Math.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.Set(k11, k12, k13);
K.ey.Set(k12, k22, k23);
K.ez.Set(k13, k23, k33);
Vec3 C = new Vec3();
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.Set(k11, k12);
K.ey.Set(k12, k22);
Vec2 impulse1 = K.Solve(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vec2 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[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(linearError <= 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, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// 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.GetInverse();
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);
bA._linearVelocity -= mA * P;
bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
bB._linearVelocity += mB * P;
bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.zero;
_angularImpulse = 0.0f;
}
}
