/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out TSVector2 normal, out FixedArray2 <TSVector2> points)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points);
}
public void GetWorldManifold(out TSVector2 normal, out FixedArray2 <TSVector2> points)
{
Body body = this.FixtureA.Body;
Body body2 = this.FixtureB.Body;
Shape shape = this.FixtureA.Shape;
Shape shape2 = this.FixtureB.Shape;
ContactSolver.WorldManifold.Initialize(ref this.Manifold, ref body._xf, shape.Radius, ref body2._xf, shape2.Radius, out normal, out points);
}
public static void Initialize(ref Manifold manifold, ref Transform xfA, FP radiusA, ref Transform xfB, FP radiusB, out TSVector2 normal, out FixedArray2 <TSVector2> points)
{
normal = TSVector2.zero;
points = default(FixedArray2 <TSVector2>);
bool flag = manifold.PointCount == 0;
if (!flag)
{
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new TSVector2(1f, 0f);
TSVector2 tSVector = MathUtils.Mul(ref xfA, manifold.LocalPoint);
TSVector2 tSVector2 = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
bool flag2 = TSVector2.DistanceSquared(tSVector, tSVector2) > Settings.EpsilonSqr;
if (flag2)
{
normal = tSVector2 - tSVector;
normal.Normalize();
}
TSVector2 value = tSVector + radiusA * normal;
TSVector2 value2 = tSVector2 - radiusB * normal;
points[0] = 0.5f * (value + value2);
break;
}
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
TSVector2 value3 = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; i++)
{
TSVector2 value4 = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
TSVector2 value5 = value4 + (radiusA - TSVector2.Dot(value4 - value3, normal)) * normal;
TSVector2 value6 = value4 - radiusB * normal;
points[i] = 0.5f * (value5 + value6);
}
break;
}
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
TSVector2 value7 = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (int j = 0; j < manifold.PointCount; j++)
{
TSVector2 value8 = MathUtils.Mul(ref xfA, manifold.Points[j].LocalPoint);
TSVector2 value9 = value8 + (radiusB - TSVector2.Dot(value8 - value7, normal)) * normal;
TSVector2 value10 = value8 - radiusA * normal;
points[j] = 0.5f * (value10 + value9);
}
normal = -normal;
break;
}
}
}
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="xfA">The transform for A.</param>
/// <param name="radiusA">The radius for A.</param>
/// <param name="xfB">The transform for B.</param>
/// <param name="radiusB">The radius for B.</param>
/// <param name="normal">World vector pointing from A to B</param>
/// <param name="points">Torld contact point (point of intersection).</param>
public static void Initialize(ref Manifold manifold, ref Transform xfA, FP radiusA, ref Transform xfB, FP radiusB, out TSVector2 normal, out FixedArray2 <TSVector2> points)
{
normal = TSVector2.zero;
points = new FixedArray2 <TSVector2>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new TSVector2(1.0f, 0.0f);
TSVector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
TSVector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
if (TSVector2.DistanceSquared(pointA, pointB) > Settings.EpsilonSqr)
{
normal = pointB - pointA;
normal.Normalize();
}
TSVector2 cA = pointA + radiusA * normal;
TSVector2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
TSVector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
TSVector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
TSVector2 cA = clipPoint + (radiusA - TSVector2.Dot(clipPoint - planePoint, normal)) * normal;
TSVector2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
TSVector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
TSVector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
TSVector2 cB = clipPoint + (radiusB - TSVector2.Dot(clipPoint - planePoint, normal)) * normal;
TSVector2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}