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