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 angularError = 0.0f;
float positionError = 0.0f;
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != LimitState.e_inactiveLimit && fixedRotation == false)
{
float angle = aB - aA - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState == LimitState.e_equalLimits)
{
// Prevent large angular corrections
float C = Utilities.Clamp(angle - m_lowerAngle, -Settings._maxAngularCorrection, Settings._maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = Math.Abs(C);
}
else if (m_limitState == LimitState.e_atLowerLimit)
{
float C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Utilities.Clamp(C + Settings._angularSlop, -Settings._maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == LimitState.e_atUpperLimit)
{
float C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Utilities.Clamp(C - Settings._angularSlop, 0.0f, Settings._maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
aA -= m_invIA * limitImpulse;
aB += m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 C = cB + rB - cA - rA;
positionError = C.Length();
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Mat22 K;
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
Vec2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * Utilities.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * Utilities.Cross(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 <= 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 angularError = 0.0f;
float positionError = 0.0f;
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != LimitState.e_inactiveLimit && fixedRotation == false)
{
float angle = aB - aA - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState ==LimitState.e_equalLimits)
{
// Prevent large angular corrections
float C = Utilities.Clamp(angle - m_lowerAngle, -Settings._maxAngularCorrection, Settings._maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = Math.Abs(C);
}
else if (m_limitState == LimitState.e_atLowerLimit)
{
float C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Utilities.Clamp(C + Settings._angularSlop, -Settings._maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == LimitState.e_atUpperLimit)
{
float C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Utilities.Clamp(C - Settings._angularSlop, 0.0f, Settings._maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
aA -= m_invIA * limitImpulse;
aB += m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA);
Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB);
Vec2 C = cB + rB - cA - rA;
positionError = C.Length();
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Mat22 K;
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
Vec2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * Utilities.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * Utilities.Cross(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 <=Settings._linearSlop && angularError <= Settings._angularSlop;
}
/// Set this based on the position and angle.
public void Set(Vec2 position, float angle)
{
p = position;
q.Set(angle);
}
