public TOISolverManifold(ref TOIConstraint cc, int index) { Debug.Assert(cc.PointCount > 0); switch (cc.Type) { case ManifoldType.Circles: { Vector2 pointA = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); Vector2 pointB = cc.BodyB.GetWorldPoint(cc.LocalPoints[0]); if ((pointA - pointB).LengthSquared() > Settings.Epsilon * Settings.Epsilon) { Normal = pointB - pointA; Normal.Normalize(); } else { Normal = new Vector2(1.0f, 0.0f); } Point = 0.5f * (pointA + pointB); Separation = Vector2.Dot(pointB - pointA, Normal) - cc.Radius; } break; case ManifoldType.FaceA: { Normal = cc.BodyA.GetWorldVector(ref cc.LocalNormal); Vector2 planePoint = cc.BodyA.GetWorldPoint(ref cc.LocalPoint); Vector2 clipPoint = cc.BodyB.GetWorldPoint(cc.LocalPoints[index]); Separation = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Point = clipPoint; } break; case ManifoldType.FaceB: { Normal = cc.BodyB.GetWorldVector(ref cc.LocalNormal); Vector2 planePoint = cc.BodyB.GetWorldPoint(ref cc.LocalPoint); Vector2 clipPoint = cc.BodyA.GetWorldPoint(cc.LocalPoints[index]); Separation = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius; Point = clipPoint; // Ensure normal points from A to B Normal = -Normal; } break; default: Normal = Vector2.UnitY; Point = Vector2.Zero; Separation = 0.0f; break; } }
public void Initialize(Contact[] contacts, int count, Body toiBody) { _count = count; _toiBody = toiBody; if (_constraints.Length < _count) { _constraints = new TOIConstraint[Math.Max(_constraints.Length * 2, _count)]; } for (int i = 0; i < _count; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold; contact.GetManifold(out manifold); Debug.Assert(manifold.PointCount > 0); TOIConstraint constraint = _constraints[i]; constraint.BodyA = bodyA; constraint.BodyB = bodyB; constraint.LocalNormal = manifold.LocalNormal; constraint.LocalPoint = manifold.LocalPoint; constraint.Type = manifold.Type; constraint.PointCount = manifold.PointCount; constraint.Radius = radiusA + radiusB; for (int j = 0; j < constraint.PointCount; ++j) { constraint.LocalPoints[j] = manifold.Points[j].LocalPoint; } _constraints[i] = constraint; } }
/// <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); }