/// <summary> /// Perform one solver iteration. Returns true if converged. /// </summary> /// <param name="baumgarte">The baumgarte value.</param> /// <returns></returns> public bool Solve(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < _count; ++i) { TOIConstraint c = _constraints[i]; Body bodyA = c.BodyA; Body bodyB = c.BodyB; float massA = bodyA.Mass; float massB = bodyB.Mass; // Only the TOI body should move. if (bodyA == _toiBody) { massB = 0.0f; } else { massA = 0.0f; } float invMassA = massA * bodyA.InvMass; float invIA = massA * bodyA.InvI; float invMassB = massB * bodyB.InvMass; float invIB = massB * bodyB.InvI; // Solve normal constraints for (int j = 0; j < c.PointCount; ++j) { TOISolverManifold psm = new TOISolverManifold(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.LinearSlop), -Settings.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; Vector2 P = impulse * normal; bodyA.Sweep.c -= invMassA * P; bodyA.Sweep.a -= invIA * MathUtils.Cross(rA, P); bodyA.SynchronizeTransform(); bodyB.Sweep.c += invMassB * P; bodyB.Sweep.a += invIB * MathUtils.Cross(rB, P); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }
/// <summary> /// Perform one solver iteration. Returns true if converged. /// </summary> /// <param name="baumgarte">The baumgarte value.</param> /// <returns></returns> public bool Solve(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < _count; ++i) { TOIConstraint c = _constraints[i]; Body bodyA = c.BodyA; Body bodyB = c.BodyB; float massA = bodyA.Mass; float massB = bodyB.Mass; // Only the TOI body should move. if (bodyA == _toiBody) { massB = 0.0f; } else { massA = 0.0f; } float invMassA = massA * bodyA.InvMass; float invIA = massA * bodyA.InvI; float invMassB = massB * bodyB.InvMass; float invIB = massB * bodyB.InvI; // Solve normal constraints for (int j = 0; j < c.PointCount; ++j) { TOISolverManifold psm = new TOISolverManifold(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.LinearSlop), -Settings.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; Vector2 P = impulse * normal; bodyA.Sweep.c -= invMassA * P; bodyA.Sweep.a -= invIA * MathUtils.Cross(rA, P); bodyA.SynchronizeTransform(); bodyB.Sweep.c += invMassB * P; bodyB.Sweep.a += invIB * MathUtils.Cross(rB, P); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return minSeparation >= -1.5f * Settings.LinearSlop; }