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); }