/// <summary>
/// Computes the polygon separation using the specified polygon b
/// </summary>
/// <param name="polygonB">The polygon</param>
/// <param name="v1">The </param>
/// <param name="v2">The </param>
/// <returns>The axis</returns>
private static EpAxis ComputePolygonSeparation(ref TempPolygon polygonB, Vector2 v1, Vector2 v2)
{
EpAxis axis;
axis.Type = EpAxisType.Unknown;
axis.Index = -1;
axis.Separation = -MathConstants.MaxFloat;
axis.Normal = Vector2.Zero;
for (int i = 0; i < polygonB.Count; ++i)
{
Vector2 n = -polygonB.Normals[i];
float s1 = MathUtils.Dot(n, polygonB.Vertices[i] - v1);
float s2 = MathUtils.Dot(n, polygonB.Vertices[i] - v2);
float s = MathUtils.Min(s1, s2);
if (s > axis.Separation)
{
axis.Type = EpAxisType.EdgeB;
axis.Index = i;
axis.Separation = s;
axis.Normal = n;
}
}
return(axis);
}
/// <summary>
/// Computes the edge separation using the specified polygon b
/// </summary>
/// <param name="polygonB">The polygon</param>
/// <param name="v1">The </param>
/// <param name="normal1">The normal</param>
/// <returns>The axis</returns>
private static EpAxis ComputeEdgeSeparation(ref TempPolygon polygonB, Vector2 v1, Vector2 normal1)
{
EpAxis axis;
axis.Type = EpAxisType.EdgeA;
axis.Index = -1;
axis.Separation = -MathConstants.MaxFloat;
axis.Normal = Vector2.Zero;
Vector2[] axes = { normal1, -normal1 };
// Find axis with least overlap (min-max problem)
for (int j = 0; j < 2; ++j)
{
float sj = MathConstants.MaxFloat;
// Find deepest polygon vertex along axis j
for (int i = 0; i < polygonB.Count; ++i)
{
float si = MathUtils.Dot(axes[j], polygonB.Vertices[i] - v1);
if (si < sj)
{
sj = si;
}
}
if (sj > axis.Separation)
{
axis.Index = j;
axis.Separation = sj;
axis.Normal = axes[j];
}
}
return(axis);
}
/// <summary>
/// Collides the edge and polygon using the specified manifold
/// </summary>
/// <param name="manifold">The manifold</param>
/// <param name="edgeA">The edge</param>
/// <param name="xfA">The xf</param>
/// <param name="polygonB">The polygon</param>
/// <param name="xfB">The xf</param>
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.MassDataPrivate.Centroid);
Vector2 v1 = edgeA.Vertex1;
Vector2 v2 = edgeA.Vertex2;
Vector2 edge1 = v2 - v1;
edge1 = Vector2.Normalize(edge1);
// 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.VerticesPrivate.Count);
for (int i = 0; i < polygonB.VerticesPrivate.Count; ++i)
{
tempPolygonB.Vertices[i] = MathUtils.Mul(ref xf, polygonB.VerticesPrivate[i]);
tempPolygonB.Normals[i] = MathUtils.Mul(xf.Q, polygonB.NormalsPrivate[i]);
}
float radius = polygonB.RadiusPrivate + edgeA.RadiusPrivate;
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 kRelativeTol = 0.98f;
const float kAbsoluteTol = 0.001f;
EpAxis primaryAxis;
if (polygonAxis.Separation - radius > kRelativeTol * (edgeAxis.Separation - radius) + kAbsoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
if (oneSided)
{
// Smooth collision
// See https://box2d.org/posts/2020/06/ghost-collisions/
Vector2 edge0 = v1 - edgeA.Vertex0;
edge0 = Vector2.Normalize(edge0);
Vector2 normal0 = new Vector2(edge0.Y, -edge0.X);
bool convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
Vector2 edge2 = edgeA.Vertex3 - v2;
edge2 = Vector2.Normalize(edge2);
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.NormalsPrivate[ref1.I1];
manifold.LocalPoint = polygonB.VerticesPrivate[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;
}
