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 = 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);
}
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 SolveVelocityConstraints(b2SolverData data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
float mA = InvertedMassA, mB = InvertedMassB;
float iA = InvertedIA, iB = InvertedIB;
// Solve linear motor constraint.
if (m_enableMotor && m_limitState != b2LimitState.e_equalLimits)
{
float Cdot = b2Math.b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
float impulse = m_motorMass * (m_motorSpeed - Cdot);
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorForce;
m_motorImpulse = b2Math.b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
b2Vec2 P = impulse * m_axis;
float LA = impulse * m_a1;
float LB = impulse * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
b2Vec2 Cdot1 = new b2Vec2();
Cdot1.x = b2Math.b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
Cdot2 = b2Math.b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 f1 = m_impulse;
b2Vec3 df = m_K.Solve33(-Cdot);
m_impulse += df;
if (m_limitState == b2LimitState.e_atLowerLimit)
{
m_impulse.z = Math.Max(m_impulse.z, 0.0f);
}
else if (m_limitState == b2LimitState.e_atUpperLimit)
{
m_impulse.z = Math.Min(m_impulse.z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * (new b2Vec2(m_K.ez.x, m_K.ez.y));
b2Vec2 f2r = m_K.Solve22(b) + (new b2Vec2(f1.x, f1.y));
m_impulse.x = f2r.x;
m_impulse.y = f2r.y;
df = m_impulse - f1;
b2Vec2 P = df.x * m_perp + df.z * m_axis;
float LA = df.x * m_s1 + df.y + df.z * m_a1;
float LB = df.x * m_s2 + df.y + df.z * m_a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
b2Vec2 df = m_K.Solve22(-Cdot1);
m_impulse.x += df.x;
m_impulse.y += df.y;
b2Vec2 P = df.x * m_perp;
float LA = df.x * m_s1 + df.y;
float LB = df.x * m_s2 + df.y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
b2Vec2 Cdot10 = Cdot1;
Cdot1.x = b2Math.b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
Cdot1.y = wB - wA;
if (b2Math.b2Abs(Cdot1.x) > 0.01f || b2Math.b2Abs(Cdot1.y) > 0.01f)
{
b2Vec2 test = b2Math.b2Mul22(m_K, df);
Cdot1.x += 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 SolveVelocityConstraints(b2SolverData data)
{
b2Vec2 vA = m_bodyA.InternalVelocity.v;
float wA = m_bodyA.InternalVelocity.w;
b2Vec2 vB = m_bodyB.InternalVelocity.v;
float wB = m_bodyB.InternalVelocity.w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (m_enableMotor && m_limitState != b2LimitState.e_equalLimits && fixedRotation == false)
{
float Cdot = wB - wA - m_motorSpeed;
float impulse = -m_motorMass * Cdot;
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorTorque;
m_motorImpulse = b2Math.b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve limit constraint.
if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit && fixedRotation == false)
{
b2Vec2 Cdot1 = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);
float Cdot2 = wB - wA;
b2Vec3 Cdot = new b2Vec3(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 impulse = -m_mass.Solve33(Cdot);
if (m_limitState == b2LimitState.e_equalLimits)
{
m_impulse += impulse;
}
else if (m_limitState == b2LimitState.e_atLowerLimit)
{
float newImpulse = m_impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
b2Vec2 rhs = -Cdot1 + m_impulse.z * (new b2Vec2(m_mass.ez.x, m_mass.ez.y));
b2Vec2 reduced = m_mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
else
{
m_impulse += impulse;
}
}
else if (m_limitState == b2LimitState.e_atUpperLimit)
{
float newImpulse = m_impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
b2Vec2 rhs = -Cdot1 + m_impulse.z * (new b2Vec2(m_mass.ez.x, m_mass.ez.y));
b2Vec2 reduced = m_mass.Solve22(rhs);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
else
{
m_impulse += impulse;
}
}
b2Vec2 P = new b2Vec2(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (b2Math.b2Cross(m_rA, P) + impulse.z);
vB += mB * P;
wB += iB * (b2Math.b2Cross(m_rB, P) + impulse.z);
}
else
{
// Solve point-to-point constraint
b2Vec2 Cdot = vB + b2Math.b2Cross(wB, ref m_rB) - vA - b2Math.b2Cross(wA, ref m_rA);
b2Vec2 impulse = m_mass.Solve22(-Cdot);
m_impulse.x += impulse.x;
m_impulse.y += impulse.y;
vA -= mA * impulse;
wA -= iA * b2Math.b2Cross(ref m_rA, ref impulse);
vB += mB * impulse;
wB += iB * b2Math.b2Cross(ref m_rB, ref impulse);
}
m_bodyA.InternalVelocity.v = vA;
m_bodyA.InternalVelocity.w = wA;
m_bodyB.InternalVelocity.v = vB;
m_bodyB.InternalVelocity.w = wB;
}
