/// Get the symmetric inverse of this matrix as a 3-by-3.
/// Returns the zero matrix if singular.
public b2Mat33 GetSymInverse33(b2Mat33 M)
{
float det = b2Math.b2Dot(ex, b2Math.b2Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
float a11 = ex.x, a12 = ey.x, a13 = ez.x;
float a22 = ey.y, a23 = ez.y;
float a33 = ez.z;
M.ex.x = det * (a22 * a33 - a23 * a23);
M.ex.y = det * (a13 * a23 - a12 * a33);
M.ex.z = det * (a12 * a23 - a13 * a22);
M.ey.x = M.ex.y;
M.ey.y = det * (a11 * a33 - a13 * a13);
M.ey.z = det * (a13 * a12 - a11 * a23);
M.ez.x = M.ex.z;
M.ez.y = M.ey.z;
M.ez.z = det * (a11 * a22 - a12 * a12);
return(M);
}
public static b2Vec2 b2Mul22(b2Mat33 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);
}
/// Get the symmetric inverse of this matrix as a 3-by-3.
/// Returns the zero matrix if singular.
public void GetSymInverse33(b2Mat33 M)
{
float det = b2Math.b2Dot(_ex, b2Math.b2Cross(_ey, _ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
float a11 = _ex.x, a12 = _ey.x, a13 = _ez.x;
float a22 = _ey.y, a23 = _ez.y;
float a33 = _ez.z;
M._ex.x = det * (a22 * a33 - a23 * a23);
M._ex.y = det * (a13 * a23 - a12 * a33);
M._ex.z = det * (a12 * a23 - a13 * a22);
M._ey.x = M._ex.y;
M._ey.y = det * (a11 * a33 - a13 * a13);
M._ey.z = det * (a13 * a12 - a11 * a23);
M._ez.x = M._ex.z;
M._ez.y = M._ey.z;
M._ez.z = det * (a11 * a22 - a12 * a12);
}
public static b2Vec2 b2Mul22(b2Mat33 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);
}
/// Get the inverse of this matrix as a 2-by-2.
/// Returns the zero matrix if singular.
public void GetInverse22(b2Mat33 M)
{
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;
}
M._ex.x = det * d; M._ey.x = -det * b; M._ex.z = 0.0f;
M._ex.y = -det * c; M._ey.y = det * a; M._ey.z = 0.0f;
M._ez.x = 0.0f; M._ez.y = 0.0f; M._ez.z = 0.0f;
}
/// Get the inverse of this matrix as a 2-by-2.
/// Returns the zero matrix if singular.
public void GetInverse22(b2Mat33 M)
{
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;
}
M._ex.x = det * d; M._ey.x = -det * b; M._ex.z = 0.0f;
M._ex.y = -det * c; M._ey.y = det * a; M._ey.z = 0.0f;
M._ez.x = 0.0f; M._ez.y = 0.0f; M._ez.z = 0.0f;
}
/// Get the inverse of this matrix as a 2-by-2.
/// Returns the zero matrix if singular.
public b2Mat33 GetInverse22(b2Mat33 M)
{
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;
}
M.ex.x = det * d; M.ey.x = -det * b; M.ex.z = 0.0f;
M.ex.y = -det * c; M.ey.y = det * a; M.ey.z = 0.0f;
M.ez.x = 0.0f; M.ez.y = 0.0f; M.ez.z = 0.0f;
return(M);
}
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;
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
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 qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
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;
bool fixedRotation = (iA + iB == 0.0f);
b2Vec3 ex = new b2Vec3();
b2Vec3 ey = new b2Vec3();
b2Vec3 ez = new b2Vec3();
ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
ez.x = -m_rA.y * iA - m_rB.y * iB;
ex.y = ey.x;
ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
ez.y = m_rA.x * iA + m_rB.x * iB;
ex.z = ez.x;
ey.z = ez.y;
ez.z = iA + iB;
m_mass = new b2Mat33(ex, ey, ez);
m_motorMass = iA + iB;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
if (m_enableMotor == false || fixedRotation)
{
m_motorImpulse = 0.0f;
}
if (m_enableLimit && fixedRotation == false)
{
float jointAngle = aB - aA - m_referenceAngle;
if (b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2Settings.b2_angularSlop)
{
m_limitState = b2LimitState.e_equalLimits;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != b2LimitState.e_atLowerLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = b2LimitState.e_atLowerLimit;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != b2LimitState.e_atUpperLimit)
{
m_impulse.z = 0.0f;
}
m_limitState = b2LimitState.e_atUpperLimit;
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = b2LimitState.e_inactiveLimit;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_impulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_impulse.x, m_impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
}
else
{
m_impulse.SetZero();
m_motorImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
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;
b2Vec2 cA = m_bodyA.InternalPosition.c;
float aA = m_bodyA.InternalPosition.a;
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
b2Vec2 cB = m_bodyB.InternalPosition.c;
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);
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;
b2Vec3 ex = new b2Vec3();
b2Vec3 ey = new b2Vec3();
b2Vec3 ez = new b2Vec3();
ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
ez.x = -m_rA.y * iA - m_rB.y * iB;
ex.y = ey.x;
ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
ez.y = m_rA.x * iA + m_rB.x * iB;
ex.z = ez.x;
ey.z = ez.y;
ez.z = iA + iB;
b2Mat33 K = new b2Mat33(ex, ey, ez);
if (m_frequencyHz > 0.0f)
{
m_mass = K.GetInverse22(m_mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - m_referenceAngle;
// Frequency
float omega = 2.0f * b2Settings.b2_pi * m_frequencyHz;
// Damping coefficient
float d = 2.0f * m * m_dampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = data.step.dt;
m_gamma = h * (d + h * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
m_bias = C * h * k * m_gamma;
invM += m_gamma;
m_mass.ezz = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
m_mass = K.GetSymInverse33(m_mass);
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_impulse *= data.step.dtRatio;
b2Vec2 P = new b2Vec2(m_impulse.x, m_impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + m_impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + m_impulse.z);
}
else
{
m_impulse.SetZero();
}
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 = m_bodyA.InternalPosition.c;
float aA = m_bodyA.InternalPosition.a;
b2Vec2 cB = m_bodyB.InternalPosition.c;
float aB = m_bodyB.InternalPosition.a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
float positionError, angularError;
b2Vec3 ex = new b2Vec3();
b2Vec3 ey = new b2Vec3();
b2Vec3 ez = new b2Vec3();
ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
ez.x = -rA.y * iA - rB.y * iB;
ex.y = ey.x;
ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
ez.y = rA.x * iA + rB.x * iB;
ex.z = ez.x;
ey.z = ez.y;
ez.z = iA + iB;
b2Mat33 K = new b2Mat33(ex, ey, ez);
if (m_frequencyHz > 0.0f)
{
b2Vec2 C1 = cB + rB - cA - rA;
positionError = C1.Length;
angularError = 0.0f;
b2Vec2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * b2Math.b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Math.b2Cross(rB, P);
}
else
{
b2Vec2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - m_referenceAngle;
positionError = C1.Length;
angularError = b2Math.b2Abs(C2);
b2Vec3 C = new b2Vec3(C1.x, C1.y, C2);
b2Vec3 impulse = -K.Solve33(C);
b2Vec2 P = new b2Vec2(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (b2Math.b2Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (b2Math.b2Cross(rB, P) + impulse.z);
}
m_bodyA.InternalPosition.c = cA;
m_bodyA.InternalPosition.a = aA;
m_bodyB.InternalPosition.c = cB;
m_bodyB.InternalPosition.a = aB;
return positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
}
/// Get the symmetric inverse of this matrix as a 3-by-3.
/// Returns the zero matrix if singular.
public void GetSymInverse33(b2Mat33 M)
{
float det = b2Math.b2Dot(_ex, b2Math.b2Cross(_ey, _ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
float a11 = _ex.x, a12 = _ey.x, a13 = _ez.x;
float a22 = _ey.y, a23 = _ez.y;
float a33 = _ez.z;
M._ex.x = det * (a22 * a33 - a23 * a23);
M._ex.y = det * (a13 * a23 - a12 * a33);
M._ex.z = det * (a12 * a23 - a13 * a22);
M._ey.x = M._ex.y;
M._ey.y = det * (a11 * a33 - a13 * a13);
M._ey.z = det * (a13 * a12 - a11 * a23);
M._ez.x = M._ex.z;
M._ez.y = M._ey.z;
M._ez.z = det * (a11 * a22 - a12 * a12);
}
public static b2Vec3 b2Mul(b2Mat33 A, b2Vec3 v)
{
return(v.x * A.ex + v.y * A.ey + v.z * A.ez);
}
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;
}
/// Multiply a matrix times a vector.
public static b2Vec2 b2Mul22(b2Mat33 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));
}
/// Get the inverse of this matrix as a 2-by-2.
/// Returns the zero matrix if singular.
public b2Mat33 GetInverse22(b2Mat33 M)
{
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;
}
M.ex.x = det * d; M.ey.x = -det * b; M.ex.z = 0.0f;
M.ex.y = -det * c; M.ey.y = det * a; M.ey.z = 0.0f;
M.ez.x = 0.0f; M.ez.y = 0.0f; M.ez.z = 0.0f;
return (M);
}
/// Get the symmetric inverse of this matrix as a 3-by-3.
/// Returns the zero matrix if singular.
public b2Mat33 GetSymInverse33(b2Mat33 M)
{
b2Vec3 cross = b2Math.b2Cross(ey, ez);
float det = b2Math.b2Dot(ex, cross);
if (det != 0.0f)
{
det = 1.0f / det;
}
float a11 = ex.x, a12 = ey.x, a13 = ez.x;
float a22 = ey.y, a23 = ez.y;
float a33 = ez.z;
M.ex.x = det * (a22 * a33 - a23 * a23);
M.ex.y = det * (a13 * a23 - a12 * a33);
M.ex.z = det * (a12 * a23 - a13 * a22);
M.ey.x = M.ex.y;
M.ey.y = det * (a11 * a33 - a13 * a13);
M.ey.z = det * (a13 * a12 - a11 * a23);
M.ez.x = M.ex.z;
M.ez.y = M.ey.z;
M.ez.z = det * (a11 * a22 - a12 * a12);
return (M);
}
