// A^T * B
public static b2Mat22 b2MulT(b2Mat22 A, b2Mat22 B)
{
b2Vec2 c1 = new b2Vec2(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
b2Vec2 c2 = new b2Vec2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
return(new b2Mat22(c1, c2));
}
public static b2Vec2 b2MulT(ref b2Mat22 A, ref b2Vec2 v)
{
b2Vec2 b = b2Vec2.Zero;
b.Set(b2Dot(v, A.ex), b2Dot(v, A.ey));
return(b);
}
public static b2Vec2 b2Mul(b2Mat22 A, b2Vec2 v)
{
b2Vec2 b = b2Vec2.Zero;
b.Set(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
return(b);
}
public static b2Vec2 b2Mul(ref b2Mat22 A, ref b2Vec2 v)
{
b2Vec2 b = b2Vec2.Zero;
b.m_x = A.ex.x * v.m_x + A.ey.x * v.m_y;
b.m_y = A.ex.y * v.m_x + A.ey.y * v.m_y;
return b;
}
public static b2Vec2 b2Mul(b2Mat22 A, b2Vec2 v)
{
b2Vec2 b;
b.x = A.ex.x * v.x + A.ey.x * v.y;
b.y = A.ex.y * v.x + A.ey.y * v.y;
return(b);
}
public static b2Vec2 b2MulT(ref b2Mat22 A, ref b2Vec2 v)
{
b2Vec2 b;
b.x = b2Dot(ref v, ref A.ex);
b.y = b2Dot(ref v, ref A.ey);
return(b);
}
public static b2Mat22 b2MulT(b2Mat22 A, b2Mat22 B)
{
b2Vec2 c1 = b2Vec2.Zero;
b2Vec2 c2 = b2Vec2.Zero;
c1.Set(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
c2.Set(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
return new b2Mat22(c1, c2);
}
public static b2Mat22 b2MulT(b2Mat22 A, b2Mat22 B)
{
b2Vec2 c1;
b2Vec2 c2;
c1.x = b2Dot(ref A.ex, ref B.ex);
c1.y = b2Dot(ref A.ey, ref B.ex);
c2.x = b2Dot(ref A.ex, ref B.ey);
c2.y = b2Dot(ref A.ey, ref B.ey);
return(new b2Mat22(c1, c2));
}
public void GetInverse(out b2Mat22 matrix)
{
float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
matrix.ex.x = det * d; matrix.ey.x = -det * b;
matrix.ex.y = -det * c; matrix.ey.y = det * a;
}
public b2Mat22 GetInverse()
{
float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
b2Mat22 B = new b2Mat22();
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;
}
public b2Mat22 GetInverse()
{
float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
b2Mat22 B = new b2Mat22();
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);
}
public void GetInverse(out b2Mat22 matrix)
{
float a = ex.x, b = ey.x, c = ex.y, d = ey.y;
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
matrix.ex.x = det * d; matrix.ey.x = -det * b;
matrix.ex.y = -det * c; matrix.ey.y = det * a;
}
public static b2Mat22 b2Abs(b2Mat22 A)
{
return(new b2Mat22(b2Abs(A.ex), b2Abs(A.ey)));
}
/// Multiply a matrix times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another.
public static b2Vec2 b2Mul(b2Mat22 A, b2Vec2 v)
{
return(new b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y));
}
public static b2Mat22 b2Mul(b2Mat22 A, b2Mat22 B)
{
return(new b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)));
}
/// 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 b2Vec2 b2MulT(b2Mat22 A, b2Vec2 v)
{
return(new b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)));
}
public override bool SolvePositionConstraints(b2SolverData data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
float mA = InvertedMassA, mB = InvertedMassB;
float iA = InvertedIA, iB = InvertedIB;
// Compute fresh Jacobians
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = cB + rB - cA - rA;
b2Vec2 axis = b2Math.b2Mul(qA, m_localXAxisA);
float a1 = b2Math.b2Cross(d + rA, axis);
float a2 = b2Math.b2Cross(rB, axis);
b2Vec2 perp = b2Math.b2Mul(qA, m_localYAxisA);
float s1 = b2Math.b2Cross(d + rA, perp);
float s2 = b2Math.b2Cross(rB, perp);
b2Vec3 impulse;
b2Vec2 C1 = new b2Vec2();
C1.x = b2Math.b2Dot(perp, d);
C1.y = aB - aA - m_referenceAngle;
float linearError = b2Math.b2Abs(C1.x);
float angularError = b2Math.b2Abs(C1.y);
bool active = false;
float C2 = 0.0f;
if (m_enableLimit)
{
float translation = b2Math.b2Dot(axis, d);
if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop)
{
// Prevent large angular corrections
C2 = b2Math.b2Clamp(translation, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection);
linearError = Math.Max(linearError, b2Math.b2Abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = b2Math.b2Clamp(translation - m_lowerTranslation + b2Settings.b2_linearSlop, -b2Settings.b2_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 = b2Math.b2Clamp(translation - m_upperTranslation - b2Settings.b2_linearSlop, 0.0f, b2Settings.b2_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;
b2Mat33 K = new b2Mat33(
new b2Vec3(k11, k12, k13),
new b2Vec3(k12, k22, k23),
new b2Vec3(k13, k23, k33));
b2Vec3 C = new b2Vec3(C1.x, C1.y, 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;
}
b2Mat22 K = new b2Mat22();
K.ex.Set(k11, k12);
K.ey.Set(k12, k22);
b2Vec2 impulse1 = K.Solve(-C1);
impulse = new b2Vec3();
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
b2Vec2 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 <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexA = m_bodyA.IslandIndex;
m_indexB = m_bodyB.IslandIndex;
m_localCenterA = m_bodyA.Sweep.localCenter;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassA = m_bodyA.InvertedMass;
m_invMassB = m_bodyB.InvertedMass;
m_invIA = m_bodyA.InvertedI;
m_invIB = m_bodyB.InvertedI;
float aA = m_bodyA.InternalPosition.a;
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
float aB = m_bodyB.InternalPosition.a;
b2Vec2 vB = m_bodyB.InternalVelocity.v;
float wB = m_bodyB.InternalVelocity.w;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
// Compute the effective mass matrix.
m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
// 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 = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
b2Mat22 K = new b2Mat22();
K.exx = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
K.exy = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
K.eyx = K.ex.y;
K.eyy = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= data.step.dtRatio;
m_angularImpulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
}
m_bodyA.InternalVelocity.v = vA;
m_bodyA.InternalVelocity.w = wA;
m_bodyB.InternalVelocity.v = vB;
m_bodyB.InternalVelocity.w = wB;
}
public override bool SolvePositionConstraints(b2SolverData data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
float angularError = 0.0f;
float positionError = 0.0f;
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit && fixedRotation == false)
{
float angle = aB - aA - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState == b2LimitState.e_equalLimits)
{
// Prevent large angular corrections
float C = b2Math.b2Clamp(angle - m_lowerAngle, -b2Settings.b2_maxAngularCorrection, b2Settings.b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = b2Math.b2Abs(C);
}
else if (m_limitState == b2LimitState.e_atLowerLimit)
{
float C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = b2Math.b2Clamp(C + b2Settings.b2_angularSlop, -b2Settings.b2_maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == b2LimitState.e_atUpperLimit)
{
float C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = b2Math.b2Clamp(C - b2Settings.b2_angularSlop, 0.0f, b2Settings.b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
aA -= m_invIA * limitImpulse;
aB += m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 C = cB + rB - cA - rA;
positionError = C.Length();
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
b2Mat22 K = new b2Mat22();
K.exx = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
K.exy = -iA * rA.x * rA.y - iB * rB.x * rB.y;
K.eyx = K.ex.y;
K.eyy = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
b2Vec2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * b2Math.b2Cross(rA, impulse);
cB += mB * impulse;
aB += iB * b2Math.b2Cross(rB, impulse);
}
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 <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
}
public override void InitVelocityConstraints(b2SolverData data)
{
m_indexB = m_bodyB.IslandIndex;
m_localCenterB = m_bodyB.Sweep.localCenter;
m_invMassB = m_bodyB.InvertedMass;
m_invIB = m_bodyB.InvertedI;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
b2Rot qB = new b2Rot(aB);
float mass = m_bodyB.Mass;
// Frequency
float omega = 2.0f * b2Settings.b2_pi * m_frequencyHz;
// Damping coefficient
float d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.step.dt;
Debug.Assert(d + h * k > b2Settings.b2_epsilon);
m_gamma = h * (d + h * k);
if (m_gamma != 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_beta = h * k * m_gamma;
// Compute the effective mass matrix.
m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
// 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]
b2Mat22 K = new b2Mat22();
K.exx = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
K.exy = -m_invIB * m_rB.x * m_rB.y;
K.eyx = K.ex.y;
K.eyy = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
m_mass = K.GetInverse();
m_C = cB + m_rB - m_targetA;
m_C *= m_beta;
// Cheat with some damping
wB *= 0.98f;
if (data.step.warmStarting)
{
m_impulse *= data.step.dtRatio;
vB += m_invMassB * m_impulse;
wB += m_invIB * b2Math.b2Cross(m_rB, m_impulse);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
