/// <summary>Compute the collision manifold between two circles.</summary>
public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB)
{
manifold.PointCount = 0;
Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position);
Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position);
Vector2 d = pB - pA;
float distSqr = Vector2.Dot(d, d);
float rA = circleA.Radius, rB = circleB.Radius;
float radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circleA.Position;
manifold.LocalNormal = Vector2.Zero;
manifold.PointCount = 1;
ManifoldPoint p0 = manifold.Points[0];
p0.LocalPoint = circleB.Position;
p0.Id.Key = 0;
manifold.Points[0] = p0;
}
public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
manifold.PointCount = 0;
Transform xf = MathUtils.MulT(xfA, xfB);
Vector2 centroidB = MathUtils.Mul(ref xf, polygonB._massData._centroid);
Vector2 v1 = edgeA._vertex1;
Vector2 v2 = edgeA._vertex2;
Vector2 edge1 = v2 - v1;
edge1.Normalize();
// Normal points to the right for a CCW winding
Vector2 normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = MathUtils.Dot(normal1, centroidB - v1);
bool oneSided = edgeA._oneSided;
if (oneSided && offset1 < 0.0f)
{
return;
}
// Get polygonB in frameA
TempPolygon tempPolygonB = new TempPolygon(polygonB._vertices.Count);
for (int i = 0; i < polygonB._vertices.Count; ++i)
{
tempPolygonB.Vertices[i] = MathUtils.Mul(ref xf, polygonB._vertices[i]);
tempPolygonB.Normals[i] = MathUtils.Mul(xf.q, polygonB._normals[i]);
}
float radius = polygonB._radius + edgeA._radius;
EPAxis edgeAxis = ComputeEdgeSeparation(ref tempPolygonB, v1, normal1);
if (edgeAxis.Separation > radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation(ref tempPolygonB, v1, v2);
if (polygonAxis.Separation > radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.Separation - radius > k_relativeTol * (edgeAxis.Separation - radius) + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
if (oneSided)
{
// Smooth collision
// See https://box2d.org/posts/2020/06/ghost-collisions/
Vector2 edge0 = v1 - edgeA._vertex0;
edge0.Normalize();
Vector2 normal0 = new Vector2(edge0.Y, -edge0.X);
bool convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
Vector2 edge2 = edgeA._vertex3 - v2;
edge2.Normalize();
Vector2 normal2 = new Vector2(edge2.Y, -edge2.X);
bool convex2 = MathUtils.Cross(edge1, edge2) >= 0.0f;
const float sinTol = 0.1f;
bool side1 = MathUtils.Dot(primaryAxis.Normal, edge1) <= 0.0f;
// Check Gauss Map
if (side1)
{
if (convex1)
{
if (MathUtils.Cross(primaryAxis.Normal, normal0) > sinTol)
{
// Skip region
return;
}
// Admit region
}
else
{
// Snap region
primaryAxis = edgeAxis;
}
}
else
{
if (convex2)
{
if (MathUtils.Cross(normal2, primaryAxis.Normal) > sinTol)
{
// Skip region
return;
}
// Admit region
}
else
{
// Snap region
primaryAxis = edgeAxis;
}
}
}
FixedArray2 <ClipVertex> clipPoints = new FixedArray2 <ClipVertex>();
ReferenceFace ref1;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = MathUtils.Dot(primaryAxis.Normal, tempPolygonB.Normals[0]);
for (int i = 1; i < tempPolygonB.Count; ++i)
{
float value = MathUtils.Dot(primaryAxis.Normal, tempPolygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < tempPolygonB.Count ? i1 + 1 : 0;
clipPoints.Value0.V = tempPolygonB.Vertices[i1];
clipPoints.Value0.Id.ContactFeature.IndexA = 0;
clipPoints.Value0.Id.ContactFeature.IndexB = (byte)i1;
clipPoints.Value0.Id.ContactFeature.TypeA = ContactFeatureType.Face;
clipPoints.Value0.Id.ContactFeature.TypeB = ContactFeatureType.Vertex;
clipPoints.Value1.V = tempPolygonB.Vertices[i2];
clipPoints.Value1.Id.ContactFeature.IndexA = 0;
clipPoints.Value1.Id.ContactFeature.IndexB = (byte)i2;
clipPoints.Value1.Id.ContactFeature.TypeA = ContactFeatureType.Face;
clipPoints.Value1.Id.ContactFeature.TypeB = ContactFeatureType.Vertex;
ref1.i1 = 0;
ref1.i2 = 1;
ref1.v1 = v1;
ref1.v2 = v2;
ref1.Normal = primaryAxis.Normal;
ref1.SideNormal1 = -edge1;
ref1.SideNormal2 = edge1;
}
else
{
manifold.Type = ManifoldType.FaceB;
clipPoints.Value0.V = v2;
clipPoints.Value0.Id.ContactFeature.IndexA = 1;
clipPoints.Value0.Id.ContactFeature.IndexB = (byte)primaryAxis.Index;
clipPoints.Value0.Id.ContactFeature.TypeA = ContactFeatureType.Vertex;
clipPoints.Value0.Id.ContactFeature.TypeB = ContactFeatureType.Face;
clipPoints.Value1.V = v1;
clipPoints.Value1.Id.ContactFeature.IndexA = 0;
clipPoints.Value1.Id.ContactFeature.IndexB = (byte)primaryAxis.Index;
clipPoints.Value1.Id.ContactFeature.TypeA = ContactFeatureType.Vertex;
clipPoints.Value1.Id.ContactFeature.TypeB = ContactFeatureType.Face;
ref1.i1 = primaryAxis.Index;
ref1.i2 = ref1.i1 + 1 < tempPolygonB.Count ? ref1.i1 + 1 : 0;
ref1.v1 = tempPolygonB.Vertices[ref1.i1];
ref1.v2 = tempPolygonB.Vertices[ref1.i2];
ref1.Normal = tempPolygonB.Normals[ref1.i1];
// CCW winding
ref1.SideNormal1 = new Vector2(ref1.Normal.Y, -ref1.Normal.X);
ref1.SideNormal2 = -ref1.SideNormal1;
}
ref1.SideOffset1 = MathUtils.Dot(ref1.SideNormal1, ref1.v1);
ref1.SideOffset2 = MathUtils.Dot(ref1.SideNormal2, ref1.v2);
// Clip incident edge against reference face side planes
FixedArray2 <ClipVertex> clipPoints1;
FixedArray2 <ClipVertex> clipPoints2;
int np;
// Clip to side 1
np = Collision.ClipSegmentToLine(out clipPoints1, ref clipPoints, ref1.SideNormal1, ref1.SideOffset1, ref1.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to side 2
np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, ref1.SideNormal2, ref1.SideOffset2, ref1.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = ref1.Normal;
manifold.LocalPoint = ref1.v1;
}
else
{
manifold.LocalNormal = polygonB._normals[ref1.i1];
manifold.LocalPoint = polygonB._vertices[ref1.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = MathUtils.Dot(ref1.Normal, clipPoints2[i].V - ref1.v1);
if (separation <= radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].Id;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.ContactFeature.TypeA = clipPoints2[i].Id.ContactFeature.TypeB;
cp.Id.ContactFeature.TypeB = clipPoints2[i].Id.ContactFeature.TypeA;
cp.Id.ContactFeature.IndexA = clipPoints2[i].Id.ContactFeature.IndexB;
cp.Id.ContactFeature.IndexB = clipPoints2[i].Id.ContactFeature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
public static void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
bool front;
Vector2 lowerLimit, upperLimit;
Vector2 normal;
Vector2 normal0 = Vector2.Zero;
Vector2 normal2 = Vector2.Zero;
Transform xf = MathUtils.MulT(xfA, xfB);
Vector2 centroidB = MathUtils.Mul(ref xf, polygonB.MassData.Centroid);
Vector2 v0 = edgeA.Vertex0;
Vector2 v1 = edgeA._vertex1;
Vector2 v2 = edgeA._vertex2;
Vector2 v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
Vector2 edge1 = v2 - v1;
edge1.Normalize();
Vector2 normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = Vector2.Dot(normal1, centroidB - v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vector2 edge0 = v1 - v0;
edge0.Normalize();
normal0 = new Vector2(edge0.Y, -edge0.X);
convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
offset0 = Vector2.Dot(normal0, centroidB - v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vector2 edge2 = v3 - v2;
edge2.Normalize();
normal2 = new Vector2(edge2.Y, -edge2.X);
convex2 = MathUtils.Cross(edge1, edge2) > 0.0f;
offset2 = Vector2.Dot(normal2, centroidB - v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
}
else if (convex1)
{
front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal1;
}
}
else if (convex2)
{
front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal0;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
front = offset0 >= 0.0f || offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
front = offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
}
else
{
front = offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = normal1;
}
}
}
else
{
front = offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
}
// Get polygonB in frameA
Vector2[] normals = new Vector2[Settings.MaxPolygonVertices];
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
int count = polygonB.Vertices.Count;
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref xf, polygonB.Vertices[i]);
normals[i] = MathUtils.Mul(xf.q, polygonB.Normals[i]);
}
float radius = polygonB.Radius + edgeA.Radius;
manifold.PointCount = 0;
//Velcro: ComputeEdgeSeparation() was manually inlined here
EPAxis edgeAxis;
edgeAxis.Type = EPAxisType.EdgeA;
edgeAxis.Index = front ? 0 : 1;
edgeAxis.Separation = MathConstants.MaxFloat;
for (int i = 0; i < count; ++i)
{
float s = Vector2.Dot(normal, vertices[i] - v1);
if (s < edgeAxis.Separation)
{
edgeAxis.Separation = s;
}
}
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > radius)
{
return;
}
//Velcro: ComputePolygonSeparation() was manually inlined here
EPAxis polygonAxis;
polygonAxis.Type = EPAxisType.Unknown;
polygonAxis.Index = -1;
polygonAxis.Separation = -MathConstants.MaxFloat;
Vector2 perp = new Vector2(-normal.Y, normal.X);
for (int i = 0; i < count; ++i)
{
Vector2 n = -normals[i];
float s1 = Vector2.Dot(n, vertices[i] - v1);
float s2 = Vector2.Dot(n, vertices[i] - v2);
float s = Math.Min(s1, s2);
if (s > radius)
{
// No collision
polygonAxis.Type = EPAxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
break;
}
// Adjacency
if (Vector2.Dot(n, perp) >= 0.0f)
{
if (Vector2.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
else
{
if (Vector2.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
if (s > polygonAxis.Separation)
{
polygonAxis.Type = EPAxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
}
}
if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.Type == EPAxisType.Unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
FixedArray2 <ClipVertex> ie = new FixedArray2 <ClipVertex>();
ReferenceFace rf;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vector2.Dot(normal, normals[0]);
for (int i = 1; i < count; ++i)
{
float value = Vector2.Dot(normal, normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < count ? i1 + 1 : 0;
ie.Value0.V = vertices[i1];
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)i1;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
ie.Value1.V = vertices[i2];
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)i2;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
if (front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = v1;
rf.v2 = v2;
rf.Normal = normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = v2;
rf.v2 = v1;
rf.Normal = -normal1;
}
}
else
{
manifold.Type = ManifoldType.FaceB;
ie.Value0.V = v1;
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Face;
ie.Value1.V = v2;
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Face;
rf.i1 = primaryAxis.Index;
rf.i2 = rf.i1 + 1 < count ? rf.i1 + 1 : 0;
rf.v1 = vertices[rf.i1];
rf.v2 = vertices[rf.i2];
rf.Normal = normals[rf.i1];
}
rf.SideNormal1 = new Vector2(rf.Normal.Y, -rf.Normal.X);
rf.SideNormal2 = -rf.SideNormal1;
rf.SideOffset1 = Vector2.Dot(rf.SideNormal1, rf.v1);
rf.SideOffset2 = Vector2.Dot(rf.SideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
// Clip to box side 1
int np = Collision.ClipSegmentToLine(out FixedArray2 <ClipVertex> clipPoints1, ref ie, rf.SideNormal1, rf.SideOffset1, rf.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(out FixedArray2 <ClipVertex> clipPoints2, ref clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = rf.Normal;
manifold.LocalPoint = rf.v1;
}
else
{
manifold.LocalNormal = polygonB.Normals[rf.i1];
manifold.LocalPoint = polygonB.Vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(rf.Normal, clipPoints2[i].V - rf.v1);
if (separation <= radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(ref xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.ContactFeature.TypeA = clipPoints2[i].ID.ContactFeature.TypeB;
cp.Id.ContactFeature.TypeB = clipPoints2[i].ID.ContactFeature.TypeA;
cp.Id.ContactFeature.IndexA = clipPoints2[i].ID.ContactFeature.IndexB;
cp.Id.ContactFeature.IndexB = clipPoints2[i].ID.ContactFeature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// <summary>Compute the collision manifold between two polygons.</summary>
public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform xfA, PolygonShape polyB, ref Transform xfB)
{
// 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
manifold.PointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
float separationA = FindMaxSeparation(out int edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
float separationB = FindMaxSeparation(out int edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
bool flip;
const float k_tol = 0.1f * Settings.LinearSlop;
if (separationB > separationA + k_tol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = ManifoldType.FaceB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = ManifoldType.FaceA;
flip = false;
}
FindIncidentEdge(out FixedArray2 <ClipVertex> incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.Vertices.Count;
Vertices vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
Vector2 v11 = vertices1[iv1];
Vector2 v12 = vertices1[iv2];
Vector2 localTangent = v12 - v11;
localTangent.Normalize();
Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f);
Vector2 planePoint = 0.5f * (v11 + v12);
Vector2 tangent = MathUtils.Mul(ref xf1.q, localTangent);
Vector2 normal = MathUtils.Cross(tangent, 1.0f);
v11 = MathUtils.Mul(ref xf1, v11);
v12 = MathUtils.Mul(ref xf1, v12);
// Face offset.
float frontOffset = Vector2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
// Clip to box side 1
int np = Collision.ClipSegmentToLine(out FixedArray2 <ClipVertex> clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(out FixedArray2 <ClipVertex> clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.LocalNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(normal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = MathUtils.MulT(ref xf2, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
if (flip)
{
// Swap features
ContactFeature cf = cp.Id.ContactFeature;
cp.Id.ContactFeature.IndexA = cf.IndexB;
cp.Id.ContactFeature.IndexB = cf.IndexA;
cp.Id.ContactFeature.TypeA = cf.TypeB;
cp.Id.ContactFeature.TypeB = cf.TypeA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
