internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
_rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
_rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
// 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 = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
K.ez.X = -_rA.Y * iA - _rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
K.ez.Y = _rA.X * iA + _rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - ReferenceAngle;
// Frequency
float omega = 2.0f * MathHelper.Pi * FrequencyHz;
// Damping coefficient
float d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else if (K.ez.Z == 0.0f)
{
K.GetInverse22(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(ref _rB, ref P) + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _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 = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
K.ez.X = -_rA.Y * iA - _rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
K.ez.Y = _rA.X * iA + _rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - ReferenceAngle;
// Frequency
float omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
float d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _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]
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
K.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
K.ez.x = -_rA.y * iA - _rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
K.ez.y = _rA.x * iA + _rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
FP invM = iA + iB;
FP m = invM > 0.0f ? 1.0f / invM : 0.0f;
FP C = aB - aA - ReferenceAngle;
// Frequency
FP omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
FP d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
FP k = m * omega * omega;
// magic formulas
FP h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
FPVector2 P = new FPVector2(_impulse.x, _impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.z);
}
else
{
_impulse = FPVector.zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void InitVelocityConstraints()
{
m_LocalCenterA = _bodyA.centerOfMass;
m_LocalCenterB = _bodyB.centerOfMass;
m_InvMassA = _bodyA._invMass;
m_InvMassB = _bodyB._invMass;
m_InvIA = _bodyA._invI;
m_InvIB = _bodyB._invI;
float aA = _bodyA.rotation * Mathf.Deg2Rad;
Vector2 vA = _bodyA.velocity;
float wA = _bodyA.angularVelocity * Mathf.Deg2Rad;
float aB = _bodyB.rotation * Mathf.Deg2Rad;
Vector2 vB = _bodyB.velocity;
float wB = _bodyB.angularVelocity * Mathf.Deg2Rad;
Rot qA = new Rot(aA), qB = new Rot(aB);
m_RA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA);
m_RB = MathUtils.Mul(qB, _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;
Mat33 K = new Mat33();
K.ex.x = mA + mB + m_RA.y * m_RA.y * iA + m_RB.y * m_RB.y * iB;
K.ey.x = -m_RA.y * m_RA.x * iA - m_RB.y * m_RB.x * iB;
K.ez.x = -m_RA.y * iA - m_RB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + m_RA.x * m_RA.x * iA + m_RB.x * m_RB.x * iB;
K.ez.y = m_RA.x * iA + m_RB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (frequency > 0.0f)
{
K.GetInverse22(ref m_Mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - referenceAngle;
// Frequency
float omega = 2.0f * Mathf.PI * frequency;
// Damping coefficient
float d = 2.0f * m * dampingRatio * omega;
// Spring stiffness
float k = m * omega * omega;
// magic formulas
float h = Time.deltaTime;
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.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref m_Mass);
m_Gamma = 0.0f;
m_Bias = 0.0f;
}
// TODO: Scale impulses to support a variable time step.
// m_Impulse *= data.step.dtRatio;
Vector2 P = new Vector2(m_Impulse.x, m_Impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(m_RA, P) + m_Impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(m_RB, P) + m_Impulse.z);
if (!_bodyA.isKinematic)
{
_bodyA.velocity = vA;
if (!_bodyA.fixedAngle)
{
_bodyA.angularVelocity = wA * Mathf.Rad2Deg;
}
}
if (!_bodyB.isKinematic)
{
_bodyB.velocity = vB;
if (!_bodyB.fixedAngle)
{
_bodyB.angularVelocity = wB * Mathf.Rad2Deg;
}
}
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
GGame.Math.Fix64 aA = data.Positions[_indexA].A;
Vector2 vA = data.Velocities[_indexA].V;
GGame.Math.Fix64 wA = data.Velocities[_indexA].W;
GGame.Math.Fix64 aB = data.Positions[_indexB].A;
Vector2 vB = data.Velocities[_indexB].V;
GGame.Math.Fix64 wB = data.Velocities[_indexB].W;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _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]
GGame.Math.Fix64 mA = _invMassA, mB = _invMassB;
GGame.Math.Fix64 iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
K.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
K.ez.X = -_rA.Y * iA - _rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
K.ez.Y = _rA.X * iA + _rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
GGame.Math.Fix64 invM = iA + iB;
GGame.Math.Fix64 m = invM > 0.0f ? 1.0f / invM : 0.0f;
GGame.Math.Fix64 C = aB - aA - ReferenceAngle;
// Frequency
GGame.Math.Fix64 omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
GGame.Math.Fix64 d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
GGame.Math.Fix64 k = m * omega * omega;
// magic formulas
GGame.Math.Fix64 h = data.Step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else if (K.ez.Z == 0.0f)
{
K.GetInverse22(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.Step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
}
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
data.Velocities[_indexB].V = vB;
data.Velocities[_indexB].W = wB;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
m_indexA = BodyA.IslandIndex;
m_indexB = BodyB.IslandIndex;
m_localCenterA = BodyA.Sweep.LocalCenter;
m_localCenterB = BodyB.Sweep.LocalCenter;
m_invMassA = BodyA.InvMass;
m_invMassB = BodyB.InvMass;
m_invIA = BodyA.InvI;
m_invIB = BodyB.InvI;
//FVector2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
FVector2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
//FVector2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
FVector2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
m_rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA);
m_rB = MathUtils.Mul(qB, 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;
Mat33 K = new Mat33();
K.ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB;
K.ey.X = -m_rA.Y * m_rA.X * iA - m_rB.Y * m_rB.X * iB;
K.ez.X = -m_rA.Y * iA - m_rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB;
K.ez.Y = m_rA.X * iA + m_rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (m_frequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
float invM = iA + iB;
float m = invM > 0.0f ? 1.0f / invM : 0.0f;
float C = aB - aA - ReferenceAngle;
// Frequency
float omega = 2.0f * Settings.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;
_mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
FVector2 P = new FVector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(m_rA, P) + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(m_rB, P) + _impulse.Z);
}
else
{
_impulse = FVector3.Zero;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
/// <seealso cref="Joint.initVelocityConstraints(TimeStep)"></seealso>
public override void InitVelocityConstraints(SolverData data)
{
m_indexA = BodyA.IslandIndex;
m_indexB = BodyB.IslandIndex;
m_localCenterA.Set(BodyA.Sweep.LocalCenter);
m_localCenterB.Set(BodyB.Sweep.LocalCenter);
m_invMassA = BodyA.InvMass;
m_invMassB = BodyB.InvMass;
m_invIA = BodyA.InvI;
m_invIB = BodyB.InvI;
// Vec2 cA = data.positions[m_indexA].c;
float aA = data.Positions[m_indexA].A;
Vec2 vA = data.Velocities[m_indexA].V;
float wA = data.Velocities[m_indexA].W;
// Vec2 cB = data.positions[m_indexB].c;
float aB = data.Positions[m_indexB].A;
Vec2 vB = data.Velocities[m_indexB].V;
float wB = data.Velocities[m_indexB].W;
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
Vec2 temp = Pool.PopVec2();
qA.Set(aA);
qB.Set(aB);
// Compute the effective masses.
Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), m_rA);
Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), m_rB);
// 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;
Mat33 K = Pool.PopMat33();
K.Ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB;
K.Ey.X = (-m_rA.Y) * m_rA.X * iA - m_rB.Y * m_rB.X * iB;
K.Ez.X = (-m_rA.Y) * iA - m_rB.Y * iB;
K.Ex.Y = K.Ey.X;
K.Ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB;
K.Ez.Y = m_rA.X * iA + m_rB.X * iB;
K.Ex.Z = K.Ez.X;
K.Ey.Z = K.Ez.Y;
K.Ez.Z = iA + iB;
if (Frequency > 0.0f)
{
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 * MathUtils.PI * Frequency;
// Damping coefficient
float d = 2.0f * 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.Ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(m_mass);
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (data.Step.WarmStarting)
{
Vec2 P = Pool.PopVec2();
// Scale impulses to support a variable time step.
m_impulse.MulLocal(data.Step.DtRatio);
P.Set(m_impulse.X, m_impulse.Y);
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * (Vec2.Cross(m_rA, P) + m_impulse.Z);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * (Vec2.Cross(m_rB, P) + m_impulse.Z);
Pool.PushVec2(1);
}
else
{
m_impulse.SetZero();
}
data.Velocities[m_indexA].V.Set(vA);
data.Velocities[m_indexA].W = wA;
data.Velocities[m_indexB].V.Set(vB);
data.Velocities[m_indexB].W = wB;
Pool.PushVec2(1);
Pool.PushRot(2);
Pool.PushMat33(1);
}
