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 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);
}
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);
}
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 bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit
float oldLimitImpulse;
float C;
b2Mat22 tMat;
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
float angularError = 0.0f;
float positionError = 0.0f;
float tX;
float impulseX;
float impulseY;
// Solve angular limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
float angle = bB.m_sweep.a - bA.m_sweep.a - m_referenceAngle;
float limitImpulse = 0.0f;
if (m_limitState == e_equalLimits)
{
// Prevent large angular corrections
C = b2Math.Clamp(angle - m_lowerAngle, -b2Settings.b2_maxAngularCorrection, b2Settings.b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = b2Math.Abs(C);
}
else if (m_limitState == e_atLowerLimit)
{
C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = b2Math.Clamp(C + b2Settings.b2_angularSlop, -b2Settings.b2_maxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (m_limitState == e_atUpperLimit)
{
C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = b2Math.Clamp(C - b2Settings.b2_angularSlop, 0.0f, b2Settings.b2_maxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
bA.m_sweep.a -= bA.m_invI * limitImpulse;
bB.m_sweep.a += bB.m_invI * limitImpulse;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
}
// Solve point-to-point constraint
{
//b2Vec2 r1 = b2Mul(bA->m_xf.R, m_localAnchor1 - bA->GetLocalCenter());
tMat = bA.m_xf.R;
float r1X = m_localAnchor1.x - bA.m_sweep.localCenter.x;
float r1Y = m_localAnchor1.y - bA.m_sweep.localCenter.y;
tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
r1X = tX;
//b2Vec2 r2 = b2Mul(bB->m_xf.R, m_localAnchor2 - bB->GetLocalCenter());
tMat = bB.m_xf.R;
float r2X = m_localAnchor2.x - bB.m_sweep.localCenter.x;
float r2Y = m_localAnchor2.y - bB.m_sweep.localCenter.y;
tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
r2X = tX;
//b2Vec2 C = bB->m_sweep.c + r2 - bA->m_sweep.c - r1;
float CX = bB.m_sweep.c.x + r2X - bA.m_sweep.c.x - r1X;
float CY = bB.m_sweep.c.y + r2Y - bA.m_sweep.c.y - r1Y;
float CLengthSquared = CX * CX + CY * CY;
float CLength = Mathf.Sqrt(CLengthSquared);
positionError = CLength;
float invMass1 = bA.m_invMass;
float invMass2 = bB.m_invMass;
float invI1 = bA.m_invI;
float invI2 = bB.m_invI;
//Handle large detachment.
const float k_allowedStretch = 10.0f * b2Settings.b2_linearSlop;
if (CLengthSquared > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation)
//b2Vec2 u = C; u.Normalize();
float uX = CX / CLength;
float uY = CY / CLength;
float k = invMass1 + invMass2;
//b2Settings.b2Assert(k>Number.MIN_VALUE)
float m = 1.0f / k;
impulseX = m * (-CX);
impulseY = m * (-CY);
const float k_beta = 0.5f;
bA.m_sweep.c.x -= k_beta * invMass1 * impulseX;
bA.m_sweep.c.y -= k_beta * invMass1 * impulseY;
bB.m_sweep.c.x += k_beta * invMass2 * impulseX;
bB.m_sweep.c.y += k_beta * invMass2 * impulseY;
//C = bB->m_sweep.c + r2 - bA->m_sweep.c - r1;
CX = bB.m_sweep.c.x + r2X - bA.m_sweep.c.x - r1X;
CY = bB.m_sweep.c.y + r2Y - bA.m_sweep.c.y - r1Y;
}
//b2Mat22 K1;
K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2;
//b2Mat22 K2;
K2.col1.x = invI1 * r1Y * r1Y; K2.col2.x = -invI1 * r1X * r1Y;
K2.col1.y = -invI1 * r1X * r1Y; K2.col2.y = invI1 * r1X * r1X;
//b2Mat22 K3;
K3.col1.x = invI2 * r2Y * r2Y; K3.col2.x = -invI2 * r2X * r2Y;
K3.col1.y = -invI2 * r2X * r2Y; K3.col2.y = invI2 * r2X * r2X;
//b2Mat22 K = K1 + K2 + K3;
K.SetM(K1);
K.AddM(K2);
K.AddM(K3);
//b2Vec2 impulse = K.Solve(-C);
K.Solve(tImpulse, -CX, -CY);
impulseX = tImpulse.x;
impulseY = tImpulse.y;
//bA.m_sweep.c -= bA.m_invMass * impulse;
bA.m_sweep.c.x -= bA.m_invMass * impulseX;
bA.m_sweep.c.y -= bA.m_invMass * impulseY;
//bA.m_sweep.a -= bA.m_invI * b2Cross(r1, impulse);
bA.m_sweep.a -= bA.m_invI * (r1X * impulseY - r1Y * impulseX);
//bB.m_sweep.c += bB.m_invMass * impulse;
bB.m_sweep.c.x += bB.m_invMass * impulseX;
bB.m_sweep.c.y += bB.m_invMass * impulseY;
//bB.m_sweep.a += bB.m_invI * b2Cross(r2, impulse);
bB.m_sweep.a += bB.m_invI * (r2X * impulseY - r2Y * impulseX);
bA.SynchronizeTransform();
bB.SynchronizeTransform();
}
return(positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop);
}
