public bool SolvePositionConstraints(){
float minSeparation = 0.0f;
for (int i = 0; i < m_contacts.Count(); ++i) {
ContactPositionConstraint pc = m_positionConstraints[i];
int indexA = pc.indexA;
int indexB = pc.indexB;
Vec2 localCenterA = pc.localCenterA;
float mA = pc.invMassA;
float iA = pc.invIA;
Vec2 localCenterB = pc.localCenterB;
float mB = pc.invMassB;
float iB = pc.invIB;
int pointCount = pc.pointCount;
Vec2 cA = m_positions[indexA].c;
float aA = m_positions[indexA].a;
Vec2 cB = m_positions[indexB].c;
float aB = m_positions[indexB].a;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j) {
Transform xfA = new Transform();
Transform xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - Utilities.Mul(xfA.q, localCenterA);
xfB.p = cB - Utilities.Mul(xfB.q, localCenterB);
PositionSolverManifold psm = new PositionSolverManifold();
psm.Initialize(pc, xfA, xfB, j);
Vec2 normal = psm.normal;
Vec2 point = psm.point;
float separation = psm.separation;
Vec2 rA = point - cA;
Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Utilities.Clamp(Settings._baumgarte * (separation + Settings._linearSlop), -Settings._maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = Utilities.Cross(rA, normal);
float rnB = Utilities.Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * Utilities.Cross(rA, P);
cB += mB * P;
aB += iB * Utilities.Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -_linearSlop because we don't
// push the separation above -_linearSlop.
return minSeparation >= -3.0f * Settings._linearSlop;
}
public bool SolvePositionConstraints(float baumgarte)
{
float minSeparation = 0.0f;
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = _constraints[i];
Body bodyA = c.bodyA;
Body bodyB = c.bodyB;
float invMassA = bodyA._mass * bodyA._invMass;
float invIA = bodyA._mass * bodyA._invI;
float invMassB = bodyB._mass * bodyB._invMass;
float invIB = bodyB._mass * bodyB._invI;
// Solve normal constraints
for (int j = 0; j < c.pointCount; ++j)
{
PositionSolverManifold psm = new PositionSolverManifold(ref c, j);
Vector2 normal = psm._normal;
Vector2 point = psm._point;
float separation = psm._separation;
Vector2 rA = point - bodyA._sweep.c;
Vector2 rB = point - bodyB._sweep.c;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = MathUtils.Clamp(baumgarte * (separation + Settings.b2_linearSlop), -Settings.b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = MathUtils.Cross(rA, normal);
float rnB = MathUtils.Cross(rB, normal);
float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
#if MATH_OVERLOADS
Vector2 P = impulse * normal;
bodyA._sweep.c -= invMassA * P;
bodyA._sweep.a -= invIA * MathUtils.Cross(rA, P);
bodyB._sweep.c += invMassB * P;
bodyB._sweep.a += invIB * MathUtils.Cross(rB, P);
#else
Vector2 P = new Vector2(impulse * normal.x, impulse * normal.y);
bodyA._sweep.c.x -= invMassA * P.x;
bodyA._sweep.c.y -= invMassA * P.y;
bodyA._sweep.a -= invIA * (rA.x * P.y - rA.y * P.x);
bodyB._sweep.c.x += invMassB * P.x;
bodyB._sweep.c.y += invMassB * P.y;
bodyB._sweep.a += invIB * (rB.x * P.y - rB.y * P.x);
#endif
bodyA.SynchronizeTransform();
bodyB.SynchronizeTransform();
}
}
// We can't expect minSpeparation >= -Settings.b2_linearSlop because we don't
// push the separation above -Settings.b2_linearSlop.
return(minSeparation >= -1.5f * Settings.b2_linearSlop);
}
public bool SolvePositionConstraints()
{
float minSeparation = 0.0f;
for (int i = 0; i < m_contacts.Count(); ++i)
{
ContactPositionConstraint pc = m_positionConstraints[i];
int indexA = pc.indexA;
int indexB = pc.indexB;
Vec2 localCenterA = pc.localCenterA;
float mA = pc.invMassA;
float iA = pc.invIA;
Vec2 localCenterB = pc.localCenterB;
float mB = pc.invMassB;
float iB = pc.invIB;
int pointCount = pc.pointCount;
Vec2 cA = m_positions[indexA].c;
float aA = m_positions[indexA].a;
Vec2 cB = m_positions[indexB].c;
float aB = m_positions[indexB].a;
// Solve normal constraints
for (int j = 0; j < pointCount; ++j)
{
Transform xfA = new Transform();
Transform xfB = new Transform();
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - Utilities.Mul(xfA.q, localCenterA);
xfB.p = cB - Utilities.Mul(xfB.q, localCenterB);
PositionSolverManifold psm = new PositionSolverManifold();
psm.Initialize(pc, xfA, xfB, j);
Vec2 normal = psm.normal;
Vec2 point = psm.point;
float separation = psm.separation;
Vec2 rA = point - cA;
Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = Math.Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float C = Utilities.Clamp(Settings._baumgarte * (separation + Settings._linearSlop), -Settings._maxLinearCorrection, 0.0f);
// Compute the effective mass.
float rnA = Utilities.Cross(rA, normal);
float rnB = Utilities.Cross(rB, normal);
float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float impulse = K > 0.0f ? -C / K : 0.0f;
Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * Utilities.Cross(rA, P);
cB += mB * P;
aB += iB * Utilities.Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -_linearSlop because we don't
// push the separation above -_linearSlop.
return(minSeparation >= -3.0f * Settings._linearSlop);
}