public static void CollideCircles(out Manifold manifold,
CircleShape circle1, Transform xf1,
CircleShape circle2, Transform xf2)
{
manifold = new Manifold();
manifold.PointCount = 0;
Vec2 p1 = Math.Mul(xf1, circle1._p);
Vec2 p2 = Math.Mul(xf2, circle2._p);
Vec2 d = p2 - p1;
float distSqr = Vec2.Dot(d, d);
float radius = circle1._radius + circle2._radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalPoint = circle1._p;
manifold.LocalPlaneNormal.SetZero();
manifold.PointCount = 1;
manifold.Points[0].LocalPoint = circle2._p;
manifold.Points[0].ID.Key = 0;
}
public Manifold Clone()
{
Manifold manifold = new Manifold();
manifold.Normal = this.Normal;
manifold.PointCount = this.PointCount;
int num = this.Points.Length;
ManifoldPoint[] array = new ManifoldPoint[num];
for (int i = 0; i < num; i++)
{
array[i] = this.Points[i].Clone();
}
manifold.Points = array;
return manifold;
}
public static void CollideCircles(ref Manifold manifold,
CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
manifold.PointCount = 0;
Vec2 p1 = Common.Math.Mul(xf1, circle1.GetLocalPosition());
Vec2 p2 = Common.Math.Mul(xf2, circle2.GetLocalPosition());
Vec2 d = p2 - p1;
float distSqr = Vec2.Dot(d, d);
float r1 = circle1.GetRadius();
float r2 = circle2.GetRadius();
float radiusSum = r1 + r2;
if (distSqr > radiusSum * radiusSum)
{
return;
}
float separation;
if (distSqr < Common.Settings.FLT_EPSILON)
{
separation = -radiusSum;
manifold.Normal.Set(0.0f, 1.0f);
}
else
{
float dist = Common.Math.Sqrt(distSqr);
separation = dist - radiusSum;
float a = 1.0f / dist;
manifold.Normal.X = a * d.X;
manifold.Normal.Y = a * d.Y;
}
manifold.PointCount = 1;
manifold.Points[0].ID.Key = 0;
manifold.Points[0].Separation = separation;
p1 += r1 * manifold.Normal;
p2 -= r2 * manifold.Normal;
Vec2 p = 0.5f * (p1 + p2);
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, p);
manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, p);
}
/// <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;
}
}
}
}
public static void CollideCircles(ref Manifold manifold, CircleShape circle1, Transform xf1, CircleShape circle2, Transform xf2)
{
manifold.PointCount = 0;
Vector2 p1 = xf1.TransformPoint(circle1._position);
Vector2 p2 = xf2.TransformPoint(circle2._position);
Vector2 d = p2 - p1;
float distSqr = Vector2.Dot(d, d);
float radius = circle1._radius + circle2._radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circle1._position;
manifold.LocalPlaneNormal = Vector2.zero;
manifold.PointCount = 1;
manifold.Points[0].LocalPoint = circle2._position;
manifold.Points[0].ID.Key = 0;
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(out Manifold manifold,
PolygonShape polyA, Transform xfA,
PolygonShape polyB, Transform xfB)
{
manifold = new Manifold();
manifold.PointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = Manifold.ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = Manifold.ManifoldType.FaceA;
flip = 0;
}
ClipVertex[] incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.Vertices;
Vec2 v11 = vertices1[edge1];
Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vec2 localTangent = v12 - v11;
localTangent.Normalize();
Vec2 localNormal = Vec2.Cross(localTangent, 1.0f);
Vec2 planePoint = 0.5f * (v11 + v12);
Vec2 tangent = Math.Mul(xf1.R, localTangent);
Vec2 normal = Vec2.Cross(tangent, 1.0f);
v11 = Math.Mul(xf1, v11);
v12 = Math.Mul(xf1, v12);
// Face offset.
float frontOffset = Vec2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vec2.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Vec2.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.LocalPlaneNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vec2.Dot(normal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = Math.MulT(xf2, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// 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).sqrMagnitude > (Mathf.Epsilon * Mathf.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;
}
}
/// <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;
}
}
}
}
/// 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).sqrMagnitude > (Mathf.Epsilon * Mathf.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;
}
}
/// 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 Manifold.ManifoldType.Circles:
{
Vec2 pointA = Math.Mul(xfA, manifold.LocalPoint);
Vec2 pointB = Math.Mul(xfB, manifold.Points[0].LocalPoint);
Vec2 normal = new Vec2(1.0f, 0.0f);
if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON * Settings.FLT_EPSILON)
{
normal = pointB - pointA;
normal.Normalize();
}
Normal = normal;
Vec2 cA = pointA + radiusA * normal;
Vec2 cB = pointB - radiusB * normal;
Points[0] = 0.5f * (cA + cB);
}
break;
case Manifold.ManifoldType.FaceA:
{
Vec2 normal = Math.Mul(xfA.R, manifold.LocalPlaneNormal);
Vec2 planePoint = Math.Mul(xfA, manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vec2 clipPoint = Math.Mul(xfB, manifold.Points[i].LocalPoint);
Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
Vec2 cB = clipPoint - radiusB * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
case Manifold.ManifoldType.FaceB:
{
Vec2 normal = Math.Mul(xfB.R, manifold.LocalPlaneNormal);
Vec2 planePoint = Math.Mul(xfB, manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = -normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vec2 clipPoint = Math.Mul(xfA, manifold.Points[i].LocalPoint);
Vec2 cA = clipPoint - radiusA * normal;
Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
}
}
public Manifold Clone()
{
Manifold newManifold = new Manifold();
newManifold.LocalPlaneNormal = this.LocalPlaneNormal;
newManifold.LocalPoint = this.LocalPoint;
newManifold.Type = this.Type;
newManifold.PointCount = this.PointCount;
int pointCount = this.Points.Length;
ManifoldPoint[] tmp = new ManifoldPoint[pointCount];
for (int i = 0; i < pointCount; i++)
{
tmp[i] = this.Points[i].Clone();
}
newManifold.Points = tmp;
return newManifold;
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(ref Manifold manifold,
PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB)
{
manifold.PointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = ManifoldType.FaceA;
flip = 0;
}
ClipVertex[] incidentEdge;
Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1._vertexCount;
Vector2[] vertices1 = poly1._vertices;
Vector2 v11 = vertices1[edge1];
Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vector2 dv = v12 - v11;
Vector2 localNormal = dv.CrossScalarPostMultiply(1.0f);
localNormal.Normalize();
Vector2 planePoint = 0.5f * (v11 + v12);
Vector2 sideNormal = xf1.TransformDirection(v12 - v11);
sideNormal.Normalize();
Vector2 frontNormal = sideNormal.CrossScalarPostMultiply(1.0f);
v11 = Common.Math.Mul(xf1, v11);
v12 = Common.Math.Mul(xf1, v12);
float frontOffset = Vector2.Dot(frontNormal, v11);
float sideOffset1 = -Vector2.Dot(sideNormal, v11);
float sideOffset2 = Vector2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
return;
// Now clipPoints2 contains the clipped points.
manifold.LocalPlaneNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = xf2.InverseTransformPoint(clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
// This implements 2-sided edge vs circle collision.
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB)
{
manifold.PointCount = 0;
Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
Vector2 normal = edge._normal;
Vector2 v1 = edge._v1;
Vector2 v2 = edge._v2;
float radius = edge._radius + circle._radius;
// Barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if ((cLocal- v1).sqrMagnitude > 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 ((cLocal- v2).sqrMagnitude > 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 = Vector2.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;
}
}
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vec2 c = Common.Math.Mul(xf2, circle.GetLocalPosition());
Vec2 cLocal = Common.Math.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = circle.GetRadius();
int vertexCount = polygon.VertexCount;
Vec2[] vertices = polygon.GetVertices();
Vec2[] normals = polygon.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vec2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// If the center is inside the polygon ...
if (separation < Common.Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Normal = Common.Math.Mul(xf1.R, normals[normalIndex]);
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
Vec2 position = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position);
manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position);
manifold.Points[0].Separation = separation - radius;
return;
}
// Project the circle center onto the edge segment.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
float length = e.Normalize();
Box2DXDebug.Assert(length > Settings.FLT_EPSILON);
// Project the center onto the edge.
float u = Vec2.Dot(cLocal - vertices[vertIndex1], e);
Vec2 p;
if (u <= 0.0f)
{
p = vertices[vertIndex1];
manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
}
else if (u >= length)
{
p = vertices[vertIndex2];
manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2;
}
else
{
p = vertices[vertIndex1] + u * e;
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature;
}
Vec2 d = cLocal - p;
float dist = d.Normalize();
if (dist > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Normal = Common.Math.Mul(xf1.R, d);
Vec2 position_ = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position_);
manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position_);
manifold.Points[0].Separation = dist - radius;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
}
private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2)
{
Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2);
}
// Callbacks for derived classes.
public override void PreSolve(Contact contact, Manifold oldManifold)
{
Manifold manifold = contact.GetManifold();
if (manifold.PointCount == 0)
{
return;
}
Fixture fixtureA = contact.GetFixtureA();
Fixture fixtureB = contact.GetFixtureB();
PointState[] state1 = new PointState[Box2DX.Common.Settings.MaxManifoldPoints];
PointState[] state2 = new PointState[Box2DX.Common.Settings.MaxManifoldPoints];
Collision.GetPointStates(state1, state2, oldManifold, manifold);
WorldManifold worldManifold;
contact.GetWorldManifold(out worldManifold);
for (int i = 0; i < manifold.PointCount && _pointCount < k_maxContactPoints; ++i)
{
ContactPoint cp = _points[_pointCount];
cp.fixtureA = fixtureA;
cp.fixtureB = fixtureB;
cp.position = worldManifold.Points[i];
cp.normal = worldManifold.Normal;
cp.state = state2[i];
_points[_pointCount] = cp;
++_pointCount;
}
}
// This implements 2-sided edge vs circle collision.
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB)
{
manifold.PointCount = 0;
Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
Vector2 normal = edge._normal;
Vector2 v1 = edge._v1;
Vector2 v2 = edge._v2;
float radius = edge._radius + circle._radius;
// Barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if ((cLocal - v1).LengthSquared > 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 ((cLocal - v2).LengthSquared > 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 = Vector2.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;
}
}
private static void CollideEdgeAndCircle(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2)
{
Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)shape1, xf1, (CircleShape)shape2, xf2);
}
internal void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc)
{
// this is kind of yucky but we do know these were setup before entry to this method
var bodyA = cc.BodyA;
var bodyB = cc.BodyB;
Vector2 vA = bodyA._linearVelocity;
Vector2 vB = bodyB._linearVelocity;
float wA = bodyA._angularVelocity;
float wB = bodyB._angularVelocity;
ContactConstraintPoint[] ccPointsPtr = cc.Points;
for (int j = 0; j < cc.PointCount; ++j)
{
ManifoldPoint cp = manifold.Points[j];
ContactConstraintPoint ccp = ccPointsPtr[j];
ccp.NormalImpulse = cp.NormalImpulse;
ccp.TangentImpulse = cp.TangentImpulse;
ccp.LocalPoint = cp.LocalPoint;
ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;
float rnA = ccp.RA.Cross(cc.Normal);
float rnB = ccp.RB.Cross(cc.Normal);
rnA *= rnA;
rnB *= rnB;
float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;
Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
ccp.NormalMass = 1.0f / kNormal;
float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;
Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
ccp.EqualizedMass = 1.0f / kEqualized;
Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f);
float rtA = ccp.RA.Cross(tangent);
float rtB = ccp.RB.Cross(tangent);
rtA *= rtA;
rtB *= rtB;
float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;
Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
ccp.TangentMass = 1.0f / kTangent;
// Setup a velocity bias for restitution.
ccp.VelocityBias = 0.0f;
float vRel = Vector2.Dot(cc.Normal, vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA));
if (vRel < -Common.Settings.VelocityThreshold)
{
ccp.VelocityBias = -cc.Restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (cc.PointCount == 2)
{
ContactConstraintPoint ccp1 = ccPointsPtr[0];
ContactConstraintPoint ccp2 = ccPointsPtr[1];
float invMassA = bodyA._invMass;
float invIA = bodyA._invI;
float invMassB = bodyB._invMass;
float invIB = bodyB._invI;
float rn1A = ccp1.RA.Cross(cc.Normal);
float rn1B = ccp1.RB.Cross(cc.Normal);
float rn2A = ccp2.RA.Cross(cc.Normal);
float rn2B = ccp2.RB.Cross(cc.Normal);
float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float k_maxConditionNumber = 100.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
cc.K.Col1 = new Vector2(k11, k12);
cc.K.Col2 = new Vector2(k12, k22);
cc.NormalMass = cc.K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
cc.PointCount = 1;
}
}
}
private static bool IsBlocked(ref BoundingBox2D bbox)
{
const int maxShapeCount = 1024;
AABB physBounds;
physBounds.LowerBound = new Vector2( bbox.Min.X, bbox.Min.Y );
physBounds.UpperBound = new Vector2( bbox.Max.X, bbox.Max.Y );
Shape[] tempShapes = new Shape[maxShapeCount];
int numBroadphase = Angel.World.Instance.PhysicsWorld.Query( physBounds, tempShapes, maxShapeCount );
//No bodies here
if( numBroadphase == 0 )
return false;
PolygonDef shapeBoundsDef = new PolygonDef();
shapeBoundsDef.VertexCount = 4;
shapeBoundsDef.Vertices[0] = new Vector2( physBounds.LowerBound.X, physBounds.LowerBound.Y );
shapeBoundsDef.Vertices[1] = new Vector2( physBounds.UpperBound.X, physBounds.LowerBound.Y );
shapeBoundsDef.Vertices[2] = new Vector2( physBounds.UpperBound.X, physBounds.UpperBound.Y );
shapeBoundsDef.Vertices[3] = new Vector2( physBounds.LowerBound.X, physBounds.UpperBound.Y );
BodyDef fakeBodyDef = new BodyDef();
//b2Vec2 center = physBounds.lowerBound + (0.5f * shapeBoundsDef.extents);
fakeBodyDef.Position = Vector2.Zero;
Body fakeBody = new Body( fakeBodyDef, Angel.World.Instance.PhysicsWorld );
PolygonShape shapeBounds = new PolygonShape( shapeBoundsDef );
for( int i = 0; i < numBroadphase; i++ )
{
Shape Sh = tempShapes[i];
if( Sh.Type == ShapeType.PolygonShape )
{
PolygonShape PolyShape = (PolygonShape)Sh;
Manifold m0 = new Manifold();
XForm xf1 = fakeBody.XForm;
XForm xf2 = PolyShape.Body.XForm;
Collision.CollidePolygons(ref m0, shapeBounds, ref xf1, PolyShape, ref xf2);
if( m0.PointCount > 0 )
return true;
}
else if( Sh.Type == ShapeType.CircleShape )
{
CircleShape CircleShape = (CircleShape)Sh;
Manifold m0 = new Manifold();
Collision.CollidePolygonAndCircle( ref m0, shapeBounds, fakeBody.XForm, CircleShape, CircleShape.Body.XForm );
if( m0.PointCount > 0 )
return true;
}
}
return false;
}
private static void Collide(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2) { }
// Polygon versus 2-sided edge.
public static void CollidePolyAndEdge(ref Manifold manifold, PolygonShape polygon, Transform TransformA, EdgeShape edge, Transform TransformB)
{
PolygonShape polygonB = new PolygonShape();
polygonB.SetAsEdge(edge._v1, edge._v2);
CollidePolygons(ref manifold, polygon, TransformA, polygonB, TransformB);
}
private static void CollidePolygons(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
{
Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)shape1, xf1, (PolygonShape)shape2, xf2);
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(ref Manifold manifold,
PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
{
manifold.PointCount = 0;
int edgeA = 0;
float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
return;
int edgeB = 0;
float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > 0.0f)
return;
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
XForm xf1, xf2;
int edge1; // reference edge
byte flip;
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = 0;
}
ClipVertex[] incidentEdge;
Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.GetVertices();
Vec2 v11 = vertices1[edge1];
Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vec2 dv = v12 - v11;
Vec2 sideNormal = Common.Math.Mul(xf1.R, v12 - v11);
sideNormal.Normalize();
Vec2 frontNormal = Vec2.Cross(sideNormal, 1.0f);
v11 = Common.Math.Mul(xf1, v11);
v12 = Common.Math.Mul(xf1, v12);
float frontOffset = Vec2.Dot(frontNormal, v11);
float sideOffset1 = -Vec2.Dot(sideNormal, v11);
float sideOffset2 = Vec2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
return;
// Now clipPoints2 contains the clipped points.
manifold.Normal = flip!=0 ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vec2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= 0.0f)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.Separation = separation;
cp.LocalPoint1 = Box2DX.Common.Math.MulT(xfA, clipPoints2[i].V);
cp.LocalPoint2 = Box2DX.Common.Math.MulT(xfB, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
public Contact(Fixture fixtureA, Fixture fixtureB)
{
Flags = 0;
if (fixtureA.IsSensor || fixtureB.IsSensor)
{
Flags |= ContactFlag.SensorFlag;
}
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
{
Flags |= ContactFlag.ContinuousFlag;
}
else
{
Flags &= ~ContactFlag.ContinuousFlag;
}
_fixtureA = fixtureA;
_fixtureB = fixtureB;
Manifold = new Manifold();
Manifold.PointCount = 0;
Prev = null;
Next = null;
NodeA = new ContactEdge();
NodeA.Contact = null;
NodeA.Prev = null;
NodeA.Next = null;
NodeA.Other = null;
NodeB = new ContactEdge();
NodeB.Contact = null;
NodeB.Prev = null;
NodeB.Next = null;
NodeB.Other = null;
}
public void PreSolve(Contact contact, Manifold oldManifold)
{
//eh
}
public void Update(ContactListener listener)
{
//Note: Manifold is a class, not a struct. It will reference the old manifest, not copy it - DONE
Manifold oldManifold = new Manifold();
oldManifold.LocalPlaneNormal = Manifold.LocalPlaneNormal;
oldManifold.LocalPoint = Manifold.LocalPoint;
oldManifold.PointCount = Manifold.PointCount;
oldManifold.Points = Manifold.Points;
oldManifold.Type = Manifold.Type;
// Re-enable this contact.
Flags &= ~ContactFlag.DisabledFlag;
if (Collision.Collision.TestOverlap(_fixtureA.Aabb, _fixtureB.Aabb))
{
Evaluate();
}
else
{
Manifold.PointCount = 0;
}
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
int oldCount = oldManifold.PointCount;
int newCount = Manifold.PointCount;
if (newCount == 0 && oldCount > 0)
{
bodyA.WakeUp();
bodyB.WakeUp();
}
// Slow contacts don't generate TOI events.
if (bodyA.IsStatic() || bodyA.IsBullet() || bodyB.IsStatic() || bodyB.IsBullet())
{
Flags |= ContactFlag.ContinuousFlag;
}
else
{
Flags &= ~ContactFlag.ContinuousFlag;
}
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < Manifold.PointCount; ++i)
{
ManifoldPoint mp2 = Manifold.Points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.ID;
for (int j = 0; j < oldManifold.PointCount; ++j)
{
ManifoldPoint mp1 = oldManifold.Points[j];
if (mp1.ID.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
break;
}
}
}
if (newCount > 0)
{
Flags |= ContactFlag.TouchingFlag;
}
else
{
Flags &= ~ContactFlag.TouchingFlag;
}
if (oldCount == 0 && newCount > 0)
{
listener.BeginContact(this);
}
if (oldCount > 0 && newCount == 0)
{
listener.EndContact(this);
}
if ((Flags & ContactFlag.SensorFlag) == 0)
{
listener.PreSolve(this, oldManifold);
}
}
/// This is called after a contact is updated. This allows you to inspect a
/// contact before it goes to the solver. If you are careful, you can modify the
/// contact manifold (e.g. disable contact).
/// A copy of the old manifold is provided so that you can detect changes.
/// Note: this is called only for awake bodies.
/// Note: this is called even when the number of contact points is zero.
/// Note: this is not called for sensors.
/// Note: if you set the number of contact points to zero, you will not
/// get an EndContact callback. However, you may get a BeginContact callback
/// the next step.
public virtual void PreSolve(Contact contact, Manifold oldManifold)
{
//B2_NOT_USED(contact);
//B2_NOT_USED(oldManifold);
}
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vec2 c = Common.Math.Mul(xf2, circle._position);
Vec2 cLocal = Common.Math.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = polygon._radius + circle._radius;
int vertexCount = polygon._vertexCount;
Vec2[] vertices = polygon._vertices;
Vec2[] normals = polygon._normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vec2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Common.Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[normalIndex];
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
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)
{
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
{
Vec2 faceCenter = 0.5f * (v1 + v2);
float separation_ = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation_ > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[vertIndex1];
manifold.LocalPoint = faceCenter;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}
private static void CollideCircles(ref Manifold manifold, Shape shape1, XForm xf1, Shape shape2, XForm xf2)
{
Collision.Collision.CollideCircles(ref manifold, (CircleShape)shape1, xf1, (CircleShape)shape2, xf2);
}
/// 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, XForm xfA, float radiusA, XForm xfB, float radiusB)
{
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
Vec2 pointA = Common.Math.Mul(xfA, manifold.LocalPoint);
Vec2 pointB = Common.Math.Mul(xfB, manifold.Points[0].LocalPoint);
Vec2 normal = new Vec2(1.0f, 0.0f);
if (Vec2.DistanceSquared(pointA, pointB) > Common.Settings.FLT_EPSILON_SQUARED)
{
normal = pointB - pointA;
normal.Normalize();
}
Normal = normal;
Vec2 cA = pointA + radiusA * normal;
Vec2 cB = pointB - radiusB * normal;
Points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
Vec2 normal = Common.Math.Mul(xfA.R, manifold.LocalPlaneNormal);
Vec2 planePoint = Common.Math.Mul(xfA, manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vec2 clipPoint = Common.Math.Mul(xfB, manifold.Points[i].LocalPoint);
Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
Vec2 cB = clipPoint - radiusB * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
Vec2 normal = Common.Math.Mul(xfB.R, manifold.LocalPlaneNormal);
Vec2 planePoint = Common.Math.Mul(xfB, manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = -normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vec2 clipPoint = Common.Math.Mul(xfA, manifold.Points[i].LocalPoint);
Vec2 cA = clipPoint - radiusA * normal;
Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
}
}