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