/// <summary>
/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
/// </summary>
public static void GetPointStates(PointState[] /*b2_maxManifoldPoints*/ state1, PointState[] /*b2_maxManifoldPoints*/ state2, Manifold manifold1, Manifold manifold2)
{
for (int i = 0; i < Common.Settings.MaxManifoldPoints; ++i)
{
state1[i] = PointState.NullState;
state2[i] = PointState.NullState;
}
// Detect persists and removes.
for (int i = 0; i < manifold1.PointCount; ++i)
{
ContactID id = manifold1.Points[i].ID;
state1[i] = PointState.RemoveState;
for (int j = 0; j < manifold2.PointCount; ++j)
{
if (manifold2.Points[j].ID.Key == id.Key)
{
state1[i] = PointState.PersistState;
break;
}
}
}
// Detect persists and adds.
for (int i = 0; i < manifold2.PointCount; ++i)
{
ContactID id = manifold2.Points[i].ID;
state2[i] = PointState.AddState;
for (int j = 0; j < manifold1.PointCount; ++j)
{
if (manifold1.Points[j].ID.Key == id.Key)
{
state2[i] = PointState.PersistState;
break;
}
}
}
}
// This implements 2-sided edge vs circle collision.
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, XForm transformA, CircleShape circle, XForm transformB)
{
manifold.PointCount = 0;
Vec2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
Vec2 normal = edge._normal;
Vec2 v1 = edge._v1;
Vec2 v2 = edge._v2;
float radius = edge._radius + circle._radius;
// Barycentric coordinates
float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if (Vec2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v1;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else if (u2 <= 0.0f)
{
// Ahead of v2
if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v2;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else
{
float separation = Vec2.Dot(cLocal - v1, normal);
if (separation < -radius || radius < separation)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal;
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}
/// 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.
public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
{
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
Vector2 pointA = xfA.TransformPoint(manifold.LocalPoint);
Vector2 pointB = xfB.TransformPoint(manifold.Points[0].LocalPoint);
Vector2 normal = new Vector2(1.0f, 0.0f);
if ((pointA - pointB).LengthSquared() > (Box2DNet.Common.Math.Epsilon * Box2DNet.Common.Math.Epsilon))
{
normal = pointB - pointA;
normal.Normalize();
}
Normal = normal;
Vector2 cA = pointA + radiusA * normal;
Vector2 cB = pointB - radiusB * normal;
Points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
Vector2 normal = xfA.TransformDirection(manifold.LocalPlaneNormal);
Vector2 planePoint = xfA.TransformPoint(manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = xfB.TransformPoint(manifold.Points[i].LocalPoint);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
Vector2 normal = xfB.TransformDirection(manifold.LocalPlaneNormal);
Vector2 planePoint = xfB.TransformPoint(manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = -normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = xfA.TransformPoint(manifold.Points[i].LocalPoint);
Vector2 cA = clipPoint - radiusA * normal;
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
}
}
