public override Shape Clone()
{
EdgeShape edge = new EdgeShape();
edge.HasVertex0 = HasVertex0;
edge.HasVertex3 = HasVertex3;
edge.Radius = Radius;
edge.Vertex0 = Vertex0;
edge.Vertex1 = Vertex1;
edge.Vertex2 = Vertex2;
edge.Vertex3 = Vertex3;
edge._density = _density;
edge.MassData = MassData;
return edge;
}
/// <summary>
/// Get a child edge.
/// </summary>
/// <param name="edge">The edge.</param>
/// <param name="index">The index.</param>
public void GetChildEdge(ref EdgeShape edge, int index)
{
Debug.Assert(2 <= Vertices.Count);
Debug.Assert(0 <= index && index < Vertices.Count);
edge.ShapeType = ShapeType.Edge;
edge.Radius = Radius;
edge.HasVertex0 = true;
edge.HasVertex3 = true;
int i0 = index - 1 >= 0 ? index - 1 : Vertices.Count - 1;
int i1 = index;
int i2 = index + 1 < Vertices.Count ? index + 1 : 0;
int i3 = index + 2;
while (i3 >= Vertices.Count)
{
i3 -= Vertices.Count;
}
edge.Vertex0 = Vertices[i0];
edge.Vertex1 = Vertices[i1];
edge.Vertex2 = Vertices[i2];
edge.Vertex3 = Vertices[i3];
}
/// <summary>
/// Collides and edge and a polygon, taking into account edge adjacency.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="edgeA">The edge A.</param>
/// <param name="xfA">The xf A.</param>
/// <param name="polygonB">The polygon B.</param>
/// <param name="xfB">The xf B.</param>
public static void CollideEdgeAndPolygon(ref Manifold manifold,
EdgeShape edgeA, ref Transform xfA,
PolygonShape polygonB, ref Transform xfB)
{
MathUtils.MultiplyT(ref xfA, ref xfB, out _xf);
// Edge geometry
_edgeA.V0 = edgeA.Vertex0;
_edgeA.V1 = edgeA.Vertex1;
_edgeA.V2 = edgeA.Vertex2;
_edgeA.V3 = edgeA.Vertex3;
Vector2 e = _edgeA.V2 - _edgeA.V1;
// Normal points outwards in CCW order.
_edgeA.Normal = new Vector2(e.Y, -e.X);
_edgeA.Normal.Normalize();
_edgeA.HasVertex0 = edgeA.HasVertex0;
_edgeA.HasVertex3 = edgeA.HasVertex3;
// Proxy for edge
_proxyA.Vertices[0] = _edgeA.V1;
_proxyA.Vertices[1] = _edgeA.V2;
_proxyA.Normals[0] = _edgeA.Normal;
_proxyA.Normals[1] = -_edgeA.Normal;
_proxyA.Centroid = 0.5f*(_edgeA.V1 + _edgeA.V2);
_proxyA.Count = 2;
// Proxy for polygon
_proxyB.Count = polygonB.Vertices.Count;
_proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid);
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
_proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]);
_proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]);
}
_radius = 2.0f*Settings.PolygonRadius;
_limit11 = Vector2.Zero;
_limit12 = Vector2.Zero;
_limit21 = Vector2.Zero;
_limit22 = Vector2.Zero;
//Collide(ref manifold); inline start
manifold.PointCount = 0;
//ComputeAdjacency(); inline start
Vector2 v0 = _edgeA.V0;
Vector2 v1 = _edgeA.V1;
Vector2 v2 = _edgeA.V2;
Vector2 v3 = _edgeA.V3;
// Determine allowable the normal regions based on adjacency.
// Note: it may be possible that no normal is admissable.
Vector2 centerB = _proxyB.Centroid;
if (_edgeA.HasVertex0)
{
Vector2 e0 = v1 - v0;
Vector2 e1 = v2 - v1;
Vector2 n0 = new Vector2(e0.Y, -e0.X);
Vector2 n1 = new Vector2(e1.Y, -e1.X);
n0.Normalize();
n1.Normalize();
bool convex = MathUtils.Cross(n0, n1) >= 0.0f;
bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f;
bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
if (convex)
{
if (front0 || front1)
{
_limit11 = n1;
_limit12 = n0;
}
else
{
_limit11 = -n1;
_limit12 = -n0;
}
}
else
{
if (front0 && front1)
{
_limit11 = n0;
_limit12 = n1;
}
else
{
_limit11 = -n0;
_limit12 = -n1;
}
}
}
else
{
_limit11 = Vector2.Zero;
_limit12 = Vector2.Zero;
}
if (_edgeA.HasVertex3)
{
Vector2 e1 = v2 - v1;
Vector2 e2 = v3 - v2;
Vector2 n1 = new Vector2(e1.Y, -e1.X);
Vector2 n2 = new Vector2(e2.Y, -e2.X);
n1.Normalize();
n2.Normalize();
bool convex = MathUtils.Cross(n1, n2) >= 0.0f;
bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f;
if (convex)
{
if (front1 || front2)
{
_limit21 = n2;
_limit22 = n1;
}
else
{
_limit21 = -n2;
_limit22 = -n1;
}
}
else
{
if (front1 && front2)
{
_limit21 = n1;
_limit22 = n2;
}
else
{
_limit21 = -n1;
_limit22 = -n2;
}
}
}
else
{
_limit21 = Vector2.Zero;
_limit22 = Vector2.Zero;
}
//ComputeAdjacency(); inline end
//EPAxis edgeAxis = ComputeEdgeSeparation(); inline start
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
// This can happen on the middle edge of a 3-edge zig-zag chain.
if (edgeAxis.Type == EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > _radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation();
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;
}
EPProxy proxy1;
EPProxy proxy2;
FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
if (primaryAxis.Type == EPAxisType.EdgeA)
{
proxy1 = _proxyA;
proxy2 = _proxyB;
manifold.Type = ManifoldType.FaceA;
}
else
{
proxy1 = _proxyB;
proxy2 = _proxyA;
manifold.Type = ManifoldType.FaceB;
}
int edge1 = primaryAxis.Index;
FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2);
int count1 = proxy1.Count;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
Vector2 v11 = proxy1.Vertices[iv1];
Vector2 v12 = proxy1.Vertices[iv2];
Vector2 tangent = v12 - v11;
tangent.Normalize();
Vector2 normal = MathUtils.Cross(tangent, 1.0f);
Vector2 planePoint = 0.5f*(v11 + v12);
// Face offset.
float frontOffset = Vector2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius;
float sideOffset2 = Vector2.Dot(tangent, v12) + _radius;
// Clip incident edge against extruded edge1 side edges.
FixedArray2<ClipVertex> clipPoints1;
FixedArray2<ClipVertex> clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = normal;
manifold.LocalPoint = planePoint;
}
else
{
manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal);
manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint);
}
int pointCount = 0;
for (int i1 = 0; i1 < Settings.MaxManifoldPoints; ++i1)
{
float separation = Vector2.Dot(normal, clipPoints2[i1].V) - frontOffset;
if (separation <= _radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V);
cp.Id = clipPoints2[i1].ID;
}
else
{
cp.LocalPoint = clipPoints2[i1].V;
cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB;
cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA;
cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB;
cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
//Collide(ref manifold); inline end
}
public static Fixture CreateEdge(Vector2 start, Vector2 end, Body body, Object userData)
{
EdgeShape edgeShape = new EdgeShape(start, end);
return body.CreateFixture(edgeShape, userData);
}
/// <summary>
/// Compute contact points for edge versus circle.
/// This accounts for edge connectivity.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="edgeA">The edge A.</param>
/// <param name="transformA">The transform A.</param>
/// <param name="circleB">The circle B.</param>
/// <param name="transformB">The transform B.</param>
public static void CollideEdgeAndCircle(ref Manifold manifold,
EdgeShape edgeA, ref Transform transformA,
CircleShape circleB, ref Transform transformB)
{
manifold.PointCount = 0;
// Compute circle in frame of edge
Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB.Position));
Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
Vector2 e = B - A;
// Barycentric coordinates
float u = Vector2.Dot(e, B - Q);
float v = Vector2.Dot(e, Q - A);
float radius = edgeA.Radius + circleB.Radius;
ContactFeature cf;
cf.IndexB = 0;
cf.TypeB = (byte) ContactFeatureType.Vertex;
Vector2 P, d;
// Region A
if (v <= 0.0f)
{
P = A;
d = Q - P;
float dd;
Vector2.Dot(ref d, ref d, out dd);
if (dd > radius*radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
Vector2 A1 = edgeA.Vertex0;
Vector2 B1 = A;
Vector2 e1 = B1 - A1;
float u1 = Vector2.Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.IndexA = 0;
cf.TypeA = (byte) ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.Zero;
manifold.LocalPoint = P;
ManifoldPoint mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB.Position;
manifold.Points[0] = mp;
return;
}
// Region B
if (u <= 0.0f)
{
P = B;
d = Q - P;
float dd;
Vector2.Dot(ref d, ref d, out dd);
if (dd > radius*radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
Vector2 B2 = edgeA.Vertex3;
Vector2 A2 = B;
Vector2 e2 = B2 - A2;
float v2 = Vector2.Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.IndexA = 1;
cf.TypeA = (byte) ContactFeatureType.Vertex;
manifold.PointCount = 1;
manifold.Type = ManifoldType.Circles;
manifold.LocalNormal = Vector2.Zero;
manifold.LocalPoint = P;
ManifoldPoint mp = new ManifoldPoint();
mp.Id.Key = 0;
mp.Id.Features = cf;
mp.LocalPoint = circleB.Position;
manifold.Points[0] = mp;
return;
}
// Region AB
float den;
Vector2.Dot(ref e, ref e, out den);
Debug.Assert(den > 0.0f);
P = (1.0f/den)*(u*A + v*B);
d = Q - P;
float dd2;
Vector2.Dot(ref d, ref d, out dd2);
if (dd2 > radius*radius)
{
return;
}
Vector2 n = new Vector2(-e.Y, e.X);
if (Vector2.Dot(n, Q - A) < 0.0f)
{
n = new Vector2(-n.X, -n.Y);
}
n.Normalize();
cf.IndexA = 0;
cf.TypeA = (byte) ContactFeatureType.Face;
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalNormal = n;
manifold.LocalPoint = A;
ManifoldPoint mp2 = new ManifoldPoint();
mp2.Id.Key = 0;
mp2.Id.Features = cf;
mp2.LocalPoint = circleB.Position;
manifold.Points[0] = mp2;
}
